6 |
|
|
7 |
|
RigidBody::RigidBody() : StuntDouble() { |
8 |
|
objType = OT_RIGIDBODY; |
9 |
– |
com_good = false; |
10 |
– |
precalc_done = false; |
9 |
|
} |
10 |
|
|
11 |
|
RigidBody::~RigidBody() { |
95 |
|
trq[i] = 0.0; |
96 |
|
} |
97 |
|
|
100 |
– |
forces_good = false; |
101 |
– |
|
98 |
|
} |
99 |
|
|
100 |
< |
void RigidBody::setEulerAngles( double phi, double theta, double psi ){ |
100 |
> |
void RigidBody::setEuler( double phi, double theta, double psi ){ |
101 |
|
|
102 |
|
A[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi)); |
103 |
|
A[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi)); |
187 |
|
|
188 |
|
for (int i = 0; i < 3; i++) |
189 |
|
for (int j = 0; j < 3; j++) |
190 |
< |
the_A[i][j] = the_A[i][j]; |
190 |
> |
the_A[i][j] = A[i][j]; |
191 |
|
|
192 |
|
} |
193 |
|
|
225 |
|
|
226 |
|
void RigidBody::getI( double the_I[3][3] ){ |
227 |
|
|
232 |
– |
if (precalc_done) { |
233 |
– |
|
228 |
|
for (int i = 0; i < 3; i++) |
229 |
|
for (int j = 0; j < 3; j++) |
230 |
|
the_I[i][j] = I[i][j]; |
231 |
|
|
238 |
– |
} else { |
239 |
– |
|
240 |
– |
} |
232 |
|
} |
233 |
|
|
234 |
|
void RigidBody::lab2Body( double r[3] ){ |
261 |
|
|
262 |
|
void RigidBody::calcRefCoords( ) { |
263 |
|
|
264 |
< |
int i,j,k; |
264 |
> |
int i,j,k, it; |
265 |
|
double mtmp; |
266 |
|
vec3 apos; |
267 |
|
double refCOM[3]; |
268 |
+ |
vec3 ptmp; |
269 |
+ |
double Itmp[3][3]; |
270 |
+ |
double evals[3]; |
271 |
+ |
double evects[3][3]; |
272 |
+ |
double r, r2, len; |
273 |
|
|
274 |
+ |
// First, find the center of mass: |
275 |
+ |
|
276 |
|
mass = 0.0; |
277 |
|
for (j=0; j<3; j++) |
278 |
|
refCOM[j] = 0.0; |
291 |
|
for(j = 0; j < 3; j++) |
292 |
|
refCOM[j] /= mass; |
293 |
|
|
294 |
+ |
// Next, move the origin of the reference coordinate system to the COM: |
295 |
+ |
|
296 |
|
for (i = 0; i < myAtoms.size(); i++) { |
297 |
|
apos = refCoords[i]; |
298 |
|
for (j=0; j < 3; j++) { |
299 |
|
apos[j] = apos[j] - refCOM[j]; |
300 |
|
} |
301 |
|
refCoords[i] = apos; |
302 |
+ |
} |
303 |
+ |
|
304 |
+ |
// Moment of Inertia calculation |
305 |
+ |
|
306 |
+ |
for (i = 0; i < 3; i++) |
307 |
+ |
for (j = 0; j < 3; j++) |
308 |
+ |
Itmp[i][j] = 0.0; |
309 |
+ |
|
310 |
+ |
for (it = 0; it < myAtoms.size(); it++) { |
311 |
+ |
|
312 |
+ |
mtmp = myAtoms[it]->getMass(); |
313 |
+ |
ptmp = refCoords[it]; |
314 |
+ |
r= norm3(ptmp.vec); |
315 |
+ |
r2 = r*r; |
316 |
+ |
|
317 |
+ |
for (i = 0; i < 3; i++) { |
318 |
+ |
for (j = 0; j < 3; j++) { |
319 |
+ |
|
320 |
+ |
if (i==j) Itmp[i][j] += mtmp * r2; |
321 |
+ |
|
322 |
+ |
Itmp[i][j] -= mtmp * ptmp.vec[i]*ptmp.vec[j]; |
323 |
+ |
} |
324 |
+ |
} |
325 |
|
} |
326 |
+ |
|
327 |
+ |
diagonalize3x3(Itmp, evals, sU); |
328 |
+ |
|
329 |
+ |
// zero out I and then fill the diagonals with the moments of inertia: |
330 |
|
|
331 |
+ |
for (i = 0; i < 3; i++) { |
332 |
+ |
for (j = 0; j < 3; j++) { |
333 |
+ |
I[i][j] = 0.0; |
334 |
+ |
} |
335 |
+ |
I[i][i] = evals[i]; |
336 |
+ |
} |
337 |
+ |
|
338 |
+ |
// renormalize column vectors: |
339 |
+ |
|
340 |
+ |
for (i=0; i < 3; i++) { |
341 |
+ |
len = 0.0; |
342 |
+ |
for (j = 0; j < 3; j++) { |
343 |
+ |
len += sU[i][j]*sU[i][j]; |
344 |
+ |
} |
345 |
+ |
len = sqrt(len); |
346 |
+ |
for (j = 0; j < 3; j++) { |
347 |
+ |
sU[i][j] /= len; |
348 |
+ |
} |
349 |
+ |
} |
350 |
|
} |
351 |
|
|
352 |
|
void RigidBody::doEulerToRotMat(vec3 &euler, mat3x3 &myA ){ |
412 |
|
// (Actually, on second thought, don't. Integrator does this now.) |
413 |
|
// lab2Body(trq); |
414 |
|
|
369 |
– |
forces_good = true; |
370 |
– |
|
415 |
|
} |
416 |
|
|
417 |
|
void RigidBody::updateAtoms() { |
600 |
|
vel[j] /= mass; |
601 |
|
} |
602 |
|
|
559 |
– |
com_good = true; |
603 |
|
} |
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(); |
573 |
– |
|
574 |
– |
// Calculate inertial tensor matrix elements: |
575 |
– |
|
576 |
– |
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; |
594 |
– |
|
595 |
– |
Itmp[i][j] -= mtmp * ptmp[i]*ptmp[j]; |
596 |
– |
} |
597 |
– |
} |
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 |
– |
} |