ViewVC Help
View File | Revision Log | Show Annotations | View Changeset | Root Listing
root/group/trunk/OOPSE/libmdtools/RigidBody.cpp
(Generate patch)

Comparing trunk/OOPSE/libmdtools/RigidBody.cpp (file contents):
Revision 1100 by gezelter, Mon Apr 12 21:02:01 2004 UTC vs.
Revision 1452 by tim, Mon Aug 23 15:11:36 2004 UTC

# Line 6 | Line 6 | RigidBody::RigidBody() : StuntDouble() {
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 >  momIntTol = 1e-6;
12   }
13  
14   RigidBody::~RigidBody() {
# Line 85 | Line 86 | void RigidBody::getFrc(double theF[3]){
86      theF[i] = frc[i];
87   }      
88  
89 + void RigidBody::setFrc(double theF[3]){
90 +  for (int i = 0; i < 3 ; i++)
91 +    frc[i] = theF[i];
92 + }
93 +
94   void RigidBody::addFrc(double theF[3]){
95    for (int i = 0; i < 3 ; i++)
96      frc[i] += theF[i];
# Line 97 | Line 103 | void RigidBody::zeroForces() {
103      trq[i] = 0.0;
104    }
105  
100 –  forces_good = false;
101 –
106   }
107  
108 < void RigidBody::setEulerAngles( double phi, double theta, double psi ){
108 > void RigidBody::setEuler( double phi, double theta, double psi ){
109    
110      A[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi));
111      A[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi));
# Line 184 | Line 188 | void RigidBody::setQ( double the_q[4] ){
188    A[2][0] = 2.0 * ( the_q[1] * the_q[3] + the_q[0] * the_q[2] );
189    A[2][1] = 2.0 * ( the_q[2] * the_q[3] - the_q[0] * the_q[1] );
190    A[2][2] = q0Sqr - q1Sqr -q2Sqr +q3Sqr;  
187 –
191   }
192  
193   void RigidBody::getA( double the_A[3][3] ){
194    
195    for (int i = 0; i < 3; i++)
196      for (int j = 0; j < 3; j++)
197 <      the_A[i][j] = the_A[i][j];
197 >      the_A[i][j] = A[i][j];
198  
199   }
200  
# Line 222 | Line 225 | void RigidBody::getTrq(double theT[3]){
225      theT[i] = trq[i];
226   }      
227  
228 + void RigidBody::setTrq(double theT[3]){
229 +  for (int i = 0; i < 3 ; i++)
230 +    trq[i] = theT[i];
231 + }      
232 +
233   void RigidBody::addTrq(double theT[3]){
234    for (int i = 0; i < 3 ; i++)
235      trq[i] += theT[i];
# Line 229 | Line 237 | void RigidBody::getI( double the_I[3][3] ){  
237  
238   void RigidBody::getI( double the_I[3][3] ){  
239  
232 –  if (precalc_done) {
233 –
240      for (int i = 0; i < 3; i++)
241        for (int j = 0; j < 3; j++)
242          the_I[i][j] = I[i][j];
243  
238 –  } else {
239 –
240 –  }
244   }
245  
246   void RigidBody::lab2Body( double r[3] ){
# Line 265 | Line 268 | void RigidBody::body2Lab( double r[3] ){
268    r[0] = (A[0][0] * rb[0]) + (A[1][0] * rb[1]) + (A[2][0] * rb[2]);
269    r[1] = (A[0][1] * rb[0]) + (A[1][1] * rb[1]) + (A[2][1] * rb[2]);
270    r[2] = (A[0][2] * rb[0]) + (A[1][2] * rb[1]) + (A[2][2] * rb[2]);
271 +
272 + }
273 +
274 + double RigidBody::getZangle( ){
275 +    return zAngle;
276 + }
277  
278 + void RigidBody::setZangle( double zAng ){
279 +    zAngle = zAng;
280   }
281  
282 + void RigidBody::addZangle( double zAng ){
283 +    zAngle += zAng;
284 + }
285 +
286   void RigidBody::calcRefCoords( ) {
287  
288 <  int i,j,k;
288 >  int i,j,k, it, n_linear_coords;
289    double mtmp;
290    vec3 apos;
291    double refCOM[3];
292 +  vec3 ptmp;
293 +  double Itmp[3][3];
294 +  double evals[3];
295 +  double evects[3][3];
296 +  double r, r2, len;
297  
298 +  // First, find the center of mass:
299 +  
300    mass = 0.0;
301    for (j=0; j<3; j++)
302      refCOM[j] = 0.0;
# Line 293 | Line 315 | void RigidBody::calcRefCoords( ) {
315    for(j = 0; j < 3; j++)
316      refCOM[j] /= mass;
317  
318 + // Next, move the origin of the reference coordinate system to the COM:
319 +
320    for (i = 0; i < myAtoms.size(); i++) {
321      apos = refCoords[i];
322      for (j=0; j < 3; j++) {
# Line 301 | Line 325 | void RigidBody::calcRefCoords( ) {
325      refCoords[i] = apos;
326    }
327  
328 + // Moment of Inertia calculation
329 +
330 +  for (i = 0; i < 3; i++)
331 +    for (j = 0; j < 3; j++)
332 +      Itmp[i][j] = 0.0;  
333 +  
334 +  for (it = 0; it < myAtoms.size(); it++) {
335 +
336 +    mtmp = myAtoms[it]->getMass();
337 +    ptmp = refCoords[it];
338 +    r= norm3(ptmp.vec);
339 +    r2 = r*r;
340 +    
341 +    for (i = 0; i < 3; i++) {
342 +      for (j = 0; j < 3; j++) {
343 +        
344 +        if (i==j) Itmp[i][j] += mtmp * r2;
345 +
346 +        Itmp[i][j] -= mtmp * ptmp.vec[i]*ptmp.vec[j];
347 +      }
348 +    }
349 +  }
350 +  
351 +  diagonalize3x3(Itmp, evals, sU);
352 +  
353 +  // zero out I and then fill the diagonals with the moments of inertia:
354 +
355 +  n_linear_coords = 0;
356 +
357 +  for (i = 0; i < 3; i++) {
358 +    for (j = 0; j < 3; j++) {
359 +      I[i][j] = 0.0;  
360 +    }
361 +    I[i][i] = evals[i];
362 +
363 +    if (fabs(evals[i]) < momIntTol) {
364 +      is_linear = true;
365 +      n_linear_coords++;
366 +      linear_axis = i;
367 +    }
368 +  }
369 +
370 +  if (n_linear_coords > 1) {
371 +          sprintf( painCave.errMsg,
372 +               "RigidBody error.\n"
373 +               "\tOOPSE found more than one axis in this rigid body with a vanishing \n"
374 +               "\tmoment of inertia.  This can happen in one of three ways:\n"
375 +               "\t 1) Only one atom was specified, or \n"
376 +               "\t 2) All atoms were specified at the same location, or\n"
377 +               "\t 3) The programmers did something stupid.\n"
378 +               "\tIt is silly to use a rigid body to describe this situation.  Be smarter.\n"
379 +               );
380 +      painCave.isFatal = 1;
381 +      simError();
382 +  }
383 +  
384 +  // renormalize column vectors:
385 +  
386 +  for (i=0; i < 3; i++) {
387 +    len = 0.0;
388 +    for (j = 0; j < 3; j++) {
389 +      len += sU[i][j]*sU[i][j];
390 +    }
391 +    len = sqrt(len);
392 +    for (j = 0; j < 3; j++) {
393 +      sU[i][j] /= len;
394 +    }
395 +  }
396   }
397  
398   void RigidBody::doEulerToRotMat(vec3 &euler, mat3x3 &myA ){
# Line 366 | Line 458 | void RigidBody::calcForcesAndTorques() {
458    // (Actually, on second thought, don't.  Integrator does this now.)
459    // lab2Body(trq);
460  
369 –  forces_good = true;
370 –
461   }
462  
463   void RigidBody::updateAtoms() {
# Line 556 | Line 646 | void RigidBody::findCOM() {
646      vel[j] /= mass;
647    }
648  
559 –  com_good = true;
649   }
650 <  
651 < void RigidBody::findOrient() {
650 >
651 > void RigidBody::accept(BaseVisitor* v){
652 >  vector<Atom*>::iterator atomIter;
653 >  v->visit(this);
654 >
655 >  //for(atomIter = myAtoms.begin(); atomIter != myAtoms.end(); ++atomIter)
656 >  //  (*atomIter)->accept(v);
657 > }
658 > void RigidBody::getAtomRefCoor(double pos[3], int index){
659 >  vec3 ref;
660 >
661 >  ref = refCoords[index];
662 >  pos[0] = ref[0];
663 >  pos[1] = ref[1];
664 >  pos[2] = ref[2];
665    
666 <  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;
666 > }
667  
572 –  if (!com_good) findCOM();
668  
669 <  // Calculate inertial tensor matrix elements:
669 > void RigidBody::getAtomPos(double theP[3], int index){
670 >  vec3 ref;
671  
672 <  for (i = 0; i < 3; i++)
673 <    for (j = 0; j < 3; j++)
674 <      Itmp[i][j] = 0.0;  
672 >  if (index >= myAtoms.size())
673 >    cerr << index << " is an invalid index, current rigid body contains " << myAtoms.size() << "atoms" << endl;
674 >
675 >  ref = refCoords[index];
676 >  body2Lab(ref.vec);
677    
678 <  for (it = 0; it < myAtoms.size(); it++) {
679 <    
680 <    mtmp = myAtoms[it]->getMass();    
681 <    myAtoms[it]->getPos(ptmp);
678 >  theP[0] = pos[0] + ref[0];
679 >  theP[1] = pos[1] + ref[1];
680 >  theP[2] = pos[2] + ref[2];
681 > }
682  
585 –    for (j = 0; j < 3; j++)
586 –      ptmp[j] = pos[j] - ptmp[j];
683  
684 <    r2 = norm3(ptmp);
685 <    
686 <    for (i = 0; i < 3; i++) {
687 <      for (j = 0; j < 3; j++) {
688 <        
689 <        if (i==j) Itmp[i][j] = mtmp * r2;
594 <
595 <        Itmp[i][j] -= mtmp * ptmp[i]*ptmp[j];
596 <      }
597 <    }
598 <  }
684 > void RigidBody::getAtomVel(double theV[3], int index){
685 >  vec3 ref;
686 >  double velRot[3];
687 >  double skewMat[3][3];
688 >  double aSkewMat[3][3];
689 >  double aSkewTransMat[3][3];
690    
691 <  diagonalize3x3(Itmp, evals, sU);
601 <  
602 <  // zero out I and then fill the diagonals with the moments of inertia:
691 >  //velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
692  
693 <  for (i = 0; i < 3; i++) {
694 <    for (j = 0; j < 3; j++) {
695 <      I[i][j] = 0.0;  
696 <    }
697 <    I[i][i] = evals[i];
698 <  }
693 >  if (index >= myAtoms.size())
694 >    cerr << index << " is an invalid index, current rigid body contains " << myAtoms.size() << "atoms" << endl;
695 >
696 >  ref = refCoords[index];
697 >
698 >  skewMat[0][0] =0;
699 >  skewMat[0][1] = ji[2] /I[2][2];
700 >  skewMat[0][2] = -ji[1] /I[1][1];
701 >
702 >  skewMat[1][0] = -ji[2] /I[2][2];
703 >  skewMat[1][1] = 0;
704 >  skewMat[1][2] = ji[0]/I[0][0];
705 >
706 >  skewMat[2][0] =ji[1] /I[1][1];
707 >  skewMat[2][1] = -ji[0]/I[0][0];
708 >  skewMat[2][2] = 0;
709    
710 <  // 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;
710 >  matMul3(A, skewMat, aSkewMat);
711  
712 +  transposeMat3(aSkewMat, aSkewTransMat);
713 +
714 +  matVecMul3(aSkewTransMat, ref.vec, velRot);
715 +  theV[0] = vel[0] + velRot[0];
716 +  theV[1] = vel[1] + velRot[1];
717 +  theV[2] = vel[2] + velRot[2];
718   }
719 +
720 +

Diff Legend

– Removed lines
+ Added lines
< Changed lines
> Changed lines