| 6 |
|
|
| 7 |
|
RigidBody::RigidBody() : StuntDouble() { |
| 8 |
|
objType = OT_RIGIDBODY; |
| 9 |
+ |
is_linear = false; |
| 10 |
+ |
linear_axis = -1; |
| 11 |
+ |
momIntTol = 1e-6; |
| 12 |
|
} |
| 13 |
|
|
| 14 |
|
RigidBody::~RigidBody() { |
| 262 |
|
|
| 263 |
|
} |
| 264 |
|
|
| 265 |
+ |
double RigidBody::getZangle( ){ |
| 266 |
+ |
return zAngle; |
| 267 |
+ |
} |
| 268 |
+ |
|
| 269 |
+ |
void RigidBody::setZangle( double zAng ){ |
| 270 |
+ |
zAngle = zAng; |
| 271 |
+ |
} |
| 272 |
+ |
|
| 273 |
+ |
void RigidBody::addZangle( double zAng ){ |
| 274 |
+ |
zAngle += zAng; |
| 275 |
+ |
} |
| 276 |
+ |
|
| 277 |
|
void RigidBody::calcRefCoords( ) { |
| 278 |
|
|
| 279 |
< |
int i,j,k, it; |
| 279 |
> |
int i,j,k, it, n_linear_coords; |
| 280 |
|
double mtmp; |
| 281 |
|
vec3 apos; |
| 282 |
|
double refCOM[3]; |
| 343 |
|
|
| 344 |
|
// zero out I and then fill the diagonals with the moments of inertia: |
| 345 |
|
|
| 346 |
+ |
n_linear_coords = 0; |
| 347 |
+ |
|
| 348 |
|
for (i = 0; i < 3; i++) { |
| 349 |
|
for (j = 0; j < 3; j++) { |
| 350 |
|
I[i][j] = 0.0; |
| 351 |
|
} |
| 352 |
|
I[i][i] = evals[i]; |
| 353 |
+ |
|
| 354 |
+ |
if (fabs(evals[i]) < momIntTol) { |
| 355 |
+ |
is_linear = true; |
| 356 |
+ |
n_linear_coords++; |
| 357 |
+ |
linear_axis = i; |
| 358 |
+ |
} |
| 359 |
|
} |
| 360 |
+ |
|
| 361 |
+ |
if (n_linear_coords > 1) { |
| 362 |
+ |
sprintf( painCave.errMsg, |
| 363 |
+ |
"RigidBody error.\n" |
| 364 |
+ |
"\tOOPSE found more than one axis in this rigid body with a vanishing \n" |
| 365 |
+ |
"\tmoment of inertia. This can happen in one of three ways:\n" |
| 366 |
+ |
"\t 1) Only one atom was specified, or \n" |
| 367 |
+ |
"\t 2) All atoms were specified at the same location, or\n" |
| 368 |
+ |
"\t 3) The programmers did something stupid.\n" |
| 369 |
+ |
"\tIt is silly to use a rigid body to describe this situation. Be smarter.\n" |
| 370 |
+ |
); |
| 371 |
+ |
painCave.isFatal = 1; |
| 372 |
+ |
simError(); |
| 373 |
+ |
} |
| 374 |
|
|
| 375 |
|
// renormalize column vectors: |
| 376 |
|
|
| 636 |
|
pos[j] /= mass; |
| 637 |
|
vel[j] /= mass; |
| 638 |
|
} |
| 639 |
+ |
|
| 640 |
+ |
} |
| 641 |
+ |
|
| 642 |
+ |
void RigidBody::accept(BaseVisitor* v){ |
| 643 |
+ |
vector<Atom*>::iterator atomIter; |
| 644 |
+ |
v->visit(this); |
| 645 |
+ |
|
| 646 |
+ |
//for(atomIter = myAtoms.begin(); atomIter != myAtoms.end(); ++atomIter) |
| 647 |
+ |
// (*atomIter)->accept(v); |
| 648 |
+ |
} |
| 649 |
+ |
void RigidBody::getAtomRefCoor(double pos[3], int index){ |
| 650 |
+ |
vec3 ref; |
| 651 |
+ |
|
| 652 |
+ |
ref = refCoords[index]; |
| 653 |
+ |
pos[0] = ref[0]; |
| 654 |
+ |
pos[1] = ref[1]; |
| 655 |
+ |
pos[2] = ref[2]; |
| 656 |
+ |
|
| 657 |
+ |
} |
| 658 |
|
|
| 659 |
+ |
|
| 660 |
+ |
void RigidBody::getAtomPos(double theP[3], int index){ |
| 661 |
+ |
vec3 ref; |
| 662 |
+ |
|
| 663 |
+ |
if (index >= myAtoms.size()) |
| 664 |
+ |
cerr << index << " is an invalid index, current rigid body contains " << myAtoms.size() << "atoms" << endl; |
| 665 |
+ |
|
| 666 |
+ |
ref = refCoords[index]; |
| 667 |
+ |
body2Lab(ref.vec); |
| 668 |
+ |
|
| 669 |
+ |
theP[0] = pos[0] + ref[0]; |
| 670 |
+ |
theP[1] = pos[1] + ref[1]; |
| 671 |
+ |
theP[2] = pos[2] + ref[2]; |
| 672 |
|
} |
| 673 |
+ |
|
| 674 |
+ |
|
| 675 |
+ |
void RigidBody::getAtomVel(double theV[3], int index){ |
| 676 |
+ |
vec3 ref; |
| 677 |
+ |
double velRot[3]; |
| 678 |
+ |
double skewMat[3][3]; |
| 679 |
+ |
double aSkewMat[3][3]; |
| 680 |
+ |
double aSkewTransMat[3][3]; |
| 681 |
+ |
|
| 682 |
+ |
//velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$ |
| 683 |
+ |
|
| 684 |
+ |
if (index >= myAtoms.size()) |
| 685 |
+ |
cerr << index << " is an invalid index, current rigid body contains " << myAtoms.size() << "atoms" << endl; |
| 686 |
+ |
|
| 687 |
+ |
ref = refCoords[index]; |
| 688 |
+ |
|
| 689 |
+ |
skewMat[0][0] =0; |
| 690 |
+ |
skewMat[0][1] = ji[2] /I[2][2]; |
| 691 |
+ |
skewMat[0][2] = -ji[1] /I[1][1]; |
| 692 |
+ |
|
| 693 |
+ |
skewMat[1][0] = -ji[2] /I[2][2]; |
| 694 |
+ |
skewMat[1][1] = 0; |
| 695 |
+ |
skewMat[1][2] = ji[0]/I[0][0]; |
| 696 |
+ |
|
| 697 |
+ |
skewMat[2][0] =ji[1] /I[1][1]; |
| 698 |
+ |
skewMat[2][1] = -ji[0]/I[0][0]; |
| 699 |
+ |
skewMat[2][2] = 0; |
| 700 |
+ |
|
| 701 |
+ |
matMul3(A, skewMat, aSkewMat); |
| 702 |
+ |
|
| 703 |
+ |
transposeMat3(aSkewMat, aSkewTransMat); |
| 704 |
+ |
|
| 705 |
+ |
matVecMul3(aSkewTransMat, ref.vec, velRot); |
| 706 |
+ |
theV[0] = vel[0] + velRot[0]; |
| 707 |
+ |
theV[1] = vel[1] + velRot[1]; |
| 708 |
+ |
theV[2] = vel[2] + velRot[2]; |
| 709 |
+ |
} |
| 710 |
+ |
|
| 711 |
+ |
|
