| 6 |
|
|
| 7 |
|
RigidBody::RigidBody() : StuntDouble() { |
| 8 |
|
objType = OT_RIGIDBODY; |
| 9 |
< |
com_good = false; |
| 10 |
< |
precalc_done = false; |
| 9 |
> |
is_linear = false; |
| 10 |
> |
linear_axis = -1; |
| 11 |
|
} |
| 12 |
|
|
| 13 |
|
RigidBody::~RigidBody() { |
| 97 |
|
trq[i] = 0.0; |
| 98 |
|
} |
| 99 |
|
|
| 100 |
– |
forces_good = false; |
| 101 |
– |
|
| 100 |
|
} |
| 101 |
|
|
| 102 |
< |
void RigidBody::setEulerAngles( double phi, double theta, double psi ){ |
| 102 |
> |
void RigidBody::setEuler( double phi, double theta, double psi ){ |
| 103 |
|
|
| 104 |
|
A[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi)); |
| 105 |
|
A[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi)); |
| 189 |
|
|
| 190 |
|
for (int i = 0; i < 3; i++) |
| 191 |
|
for (int j = 0; j < 3; j++) |
| 192 |
< |
the_A[i][j] = the_A[i][j]; |
| 192 |
> |
the_A[i][j] = A[i][j]; |
| 193 |
|
|
| 194 |
|
} |
| 195 |
|
|
| 226 |
|
} |
| 227 |
|
|
| 228 |
|
void RigidBody::getI( double the_I[3][3] ){ |
| 231 |
– |
|
| 232 |
– |
if (precalc_done) { |
| 229 |
|
|
| 230 |
|
for (int i = 0; i < 3; i++) |
| 231 |
|
for (int j = 0; j < 3; j++) |
| 232 |
|
the_I[i][j] = I[i][j]; |
| 233 |
|
|
| 238 |
– |
} else { |
| 239 |
– |
|
| 240 |
– |
} |
| 234 |
|
} |
| 235 |
|
|
| 236 |
|
void RigidBody::lab2Body( double r[3] ){ |
| 263 |
|
|
| 264 |
|
void RigidBody::calcRefCoords( ) { |
| 265 |
|
|
| 266 |
< |
int i,j,k; |
| 266 |
> |
int i,j,k, it, n_linear_coords; |
| 267 |
|
double mtmp; |
| 268 |
|
vec3 apos; |
| 269 |
|
double refCOM[3]; |
| 270 |
+ |
vec3 ptmp; |
| 271 |
+ |
double Itmp[3][3]; |
| 272 |
+ |
double evals[3]; |
| 273 |
+ |
double evects[3][3]; |
| 274 |
+ |
double r, r2, len; |
| 275 |
|
|
| 276 |
+ |
// First, find the center of mass: |
| 277 |
+ |
|
| 278 |
|
mass = 0.0; |
| 279 |
|
for (j=0; j<3; j++) |
| 280 |
|
refCOM[j] = 0.0; |
| 293 |
|
for(j = 0; j < 3; j++) |
| 294 |
|
refCOM[j] /= mass; |
| 295 |
|
|
| 296 |
+ |
// Next, move the origin of the reference coordinate system to the COM: |
| 297 |
+ |
|
| 298 |
|
for (i = 0; i < myAtoms.size(); i++) { |
| 299 |
|
apos = refCoords[i]; |
| 300 |
|
for (j=0; j < 3; j++) { |
| 303 |
|
refCoords[i] = apos; |
| 304 |
|
} |
| 305 |
|
|
| 306 |
+ |
// Moment of Inertia calculation |
| 307 |
+ |
|
| 308 |
+ |
for (i = 0; i < 3; i++) |
| 309 |
+ |
for (j = 0; j < 3; j++) |
| 310 |
+ |
Itmp[i][j] = 0.0; |
| 311 |
+ |
|
| 312 |
+ |
for (it = 0; it < myAtoms.size(); it++) { |
| 313 |
+ |
|
| 314 |
+ |
mtmp = myAtoms[it]->getMass(); |
| 315 |
+ |
ptmp = refCoords[it]; |
| 316 |
+ |
r= norm3(ptmp.vec); |
| 317 |
+ |
r2 = r*r; |
| 318 |
+ |
|
| 319 |
+ |
for (i = 0; i < 3; i++) { |
| 320 |
+ |
for (j = 0; j < 3; j++) { |
| 321 |
+ |
|
| 322 |
+ |
if (i==j) Itmp[i][j] += mtmp * r2; |
| 323 |
+ |
|
| 324 |
+ |
Itmp[i][j] -= mtmp * ptmp.vec[i]*ptmp.vec[j]; |
| 325 |
+ |
} |
| 326 |
+ |
} |
| 327 |
+ |
} |
| 328 |
+ |
|
| 329 |
+ |
diagonalize3x3(Itmp, evals, sU); |
| 330 |
+ |
|
| 331 |
+ |
// zero out I and then fill the diagonals with the moments of inertia: |
| 332 |
+ |
|
| 333 |
+ |
n_linear_coords = 0; |
| 334 |
+ |
|
| 335 |
+ |
for (i = 0; i < 3; i++) { |
| 336 |
+ |
for (j = 0; j < 3; j++) { |
| 337 |
+ |
I[i][j] = 0.0; |
| 338 |
+ |
} |
| 339 |
+ |
I[i][i] = evals[i]; |
| 340 |
+ |
|
| 341 |
+ |
if (fabs(evals[i]) < momIntTol) { |
| 342 |
+ |
is_linear = true; |
| 343 |
+ |
n_linear_coords++; |
| 344 |
+ |
linear_axis = i; |
| 345 |
+ |
} |
| 346 |
+ |
} |
| 347 |
+ |
|
| 348 |
+ |
if (n_linear_coords > 1) { |
| 349 |
+ |
sprintf( painCave.errMsg, |
| 350 |
+ |
"RigidBody error.\n" |
| 351 |
+ |
"\tOOPSE found more than one axis in this rigid body with a vanishing \n" |
| 352 |
+ |
"\tmoment of inertia. This can happen in one of three ways:\n" |
| 353 |
+ |
"\t 1) Only one atom was specified, or \n" |
| 354 |
+ |
"\t 2) All atoms were specified at the same location, or\n" |
| 355 |
+ |
"\t 3) The programmers did something stupid.\n" |
| 356 |
+ |
"\tIt is silly to use a rigid body to describe this situation. Be smarter.\n" |
| 357 |
+ |
); |
| 358 |
+ |
painCave.isFatal = 1; |
| 359 |
+ |
simError(); |
| 360 |
+ |
} |
| 361 |
+ |
|
| 362 |
+ |
// renormalize column vectors: |
| 363 |
+ |
|
| 364 |
+ |
for (i=0; i < 3; i++) { |
| 365 |
+ |
len = 0.0; |
| 366 |
+ |
for (j = 0; j < 3; j++) { |
| 367 |
+ |
len += sU[i][j]*sU[i][j]; |
| 368 |
+ |
} |
| 369 |
+ |
len = sqrt(len); |
| 370 |
+ |
for (j = 0; j < 3; j++) { |
| 371 |
+ |
sU[i][j] /= len; |
| 372 |
+ |
} |
| 373 |
+ |
} |
| 374 |
|
} |
| 375 |
|
|
| 376 |
|
void RigidBody::doEulerToRotMat(vec3 &euler, mat3x3 &myA ){ |
| 435 |
|
// Convert Torque to Body-fixed coordinates: |
| 436 |
|
// (Actually, on second thought, don't. Integrator does this now.) |
| 437 |
|
// lab2Body(trq); |
| 368 |
– |
|
| 369 |
– |
forces_good = true; |
| 438 |
|
|
| 439 |
|
} |
| 440 |
|
|
| 624 |
|
vel[j] /= mass; |
| 625 |
|
} |
| 626 |
|
|
| 559 |
– |
com_good = true; |
| 627 |
|
} |
| 561 |
– |
|
| 562 |
– |
void RigidBody::findOrient() { |
| 563 |
– |
|
| 564 |
– |
size_t it; |
| 565 |
– |
int i, j; |
| 566 |
– |
double ptmp[3]; |
| 567 |
– |
double Itmp[3][3]; |
| 568 |
– |
double evals[3]; |
| 569 |
– |
double evects[3][3]; |
| 570 |
– |
double r2, mtmp, len; |
| 571 |
– |
|
| 572 |
– |
if (!com_good) findCOM(); |
| 628 |
|
|
| 629 |
< |
// Calculate inertial tensor matrix elements: |
| 630 |
< |
|
| 631 |
< |
for (i = 0; i < 3; i++) |
| 577 |
< |
for (j = 0; j < 3; j++) |
| 578 |
< |
Itmp[i][j] = 0.0; |
| 579 |
< |
|
| 580 |
< |
for (it = 0; it < myAtoms.size(); it++) { |
| 581 |
< |
|
| 582 |
< |
mtmp = myAtoms[it]->getMass(); |
| 583 |
< |
myAtoms[it]->getPos(ptmp); |
| 584 |
< |
|
| 585 |
< |
for (j = 0; j < 3; j++) |
| 586 |
< |
ptmp[j] = pos[j] - ptmp[j]; |
| 587 |
< |
|
| 588 |
< |
r2 = norm3(ptmp); |
| 589 |
< |
|
| 590 |
< |
for (i = 0; i < 3; i++) { |
| 591 |
< |
for (j = 0; j < 3; j++) { |
| 592 |
< |
|
| 593 |
< |
if (i==j) Itmp[i][j] = mtmp * r2; |
| 629 |
> |
void RigidBody::accept(BaseVisitor* v){ |
| 630 |
> |
vector<Atom*>::iterator atomIter; |
| 631 |
> |
v->visit(this); |
| 632 |
|
|
| 633 |
< |
Itmp[i][j] -= mtmp * ptmp[i]*ptmp[j]; |
| 634 |
< |
} |
| 635 |
< |
} |
| 598 |
< |
} |
| 599 |
< |
|
| 600 |
< |
diagonalize3x3(Itmp, evals, sU); |
| 601 |
< |
|
| 602 |
< |
// zero out I and then fill the diagonals with the moments of inertia: |
| 603 |
< |
|
| 604 |
< |
for (i = 0; i < 3; i++) { |
| 605 |
< |
for (j = 0; j < 3; j++) { |
| 606 |
< |
I[i][j] = 0.0; |
| 607 |
< |
} |
| 608 |
< |
I[i][i] = evals[i]; |
| 609 |
< |
} |
| 610 |
< |
|
| 611 |
< |
// renormalize column vectors: |
| 612 |
< |
|
| 613 |
< |
for (i=0; i < 3; i++) { |
| 614 |
< |
len = 0.0; |
| 615 |
< |
for (j = 0; j < 3; j++) { |
| 616 |
< |
len += sU[i][j]*sU[i][j]; |
| 617 |
< |
} |
| 618 |
< |
len = sqrt(len); |
| 619 |
< |
for (j = 0; j < 3; j++) { |
| 620 |
< |
sU[i][j] /= len; |
| 621 |
< |
} |
| 622 |
< |
} |
| 623 |
< |
|
| 624 |
< |
// sU now contains the coordinates of the 'special' frame; |
| 625 |
< |
|
| 626 |
< |
orient_good = true; |
| 627 |
< |
|
| 628 |
< |
} |
| 633 |
> |
for(atomIter = myAtoms.begin(); atomIter != myAtoms.end(); ++atomIter) |
| 634 |
> |
(*atomIter)->accept(v); |
| 635 |
> |
} |
