--- trunk/src/primitives/RigidBody.cpp	2005/01/25 17:45:23	273
+++ trunk/src/primitives/RigidBody.cpp	2005/02/22 18:56:25	374
@@ -42,6 +42,7 @@
 #include <math.h>
 #include "primitives/RigidBody.hpp"
 #include "utils/simError.h"
+#include "utils/NumericConstant.hpp"
 namespace oopse {
 
 RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData), inertiaTensor_(0.0){
@@ -274,13 +275,99 @@ void  RigidBody::updateAtoms() {
           dAtom = (DirectionalAtom *) atoms_[i];
           dAtom->setA(a * refOrients_[i]);
           //dAtom->rotateBy( A );      
+        }
+
+    }
+  
+}
+
+
+void  RigidBody::updateAtoms(int frame) {
+    unsigned int i;
+    Vector3d ref;
+    Vector3d apos;
+    DirectionalAtom* dAtom;
+    Vector3d pos = getPos(frame);
+    RotMat3x3d a = getA(frame);
+    
+    for (i = 0; i < atoms_.size(); i++) {
+     
+        ref = body2Lab(refCoords_[i], frame);
+
+        apos = pos + ref;
+
+        atoms_[i]->setPos(apos, frame);
+
+        if (atoms_[i]->isDirectional()) {
+          
+          dAtom = (DirectionalAtom *) atoms_[i];
+          dAtom->setA(a * refOrients_[i], frame);
         }
 
     }
   
 }
 
+void RigidBody::updateAtomVel() {
+    Mat3x3d skewMat;;
 
+    Vector3d ji = getJ();
+    Mat3x3d I =  getI();
+
+    skewMat(0, 0) =0;
+    skewMat(0, 1) = ji[2] /I(2, 2);
+    skewMat(0, 2) = -ji[1] /I(1, 1);
+
+    skewMat(1, 0) = -ji[2] /I(2, 2);
+    skewMat(1, 1) = 0;
+    skewMat(1, 2) = ji[0]/I(0, 0);
+
+    skewMat(2, 0) =ji[1] /I(1, 1);
+    skewMat(2, 1) = -ji[0]/I(0, 0);
+    skewMat(2, 2) = 0;
+
+    Mat3x3d mat = (getA() * skewMat).transpose();
+    Vector3d rbVel = getVel();
+
+
+    Vector3d velRot;        
+    for (int i =0 ; i < refCoords_.size(); ++i) {
+        atoms_[i]->setVel(rbVel + mat * refCoords_[i]);
+    }
+
+}
+
+void RigidBody::updateAtomVel(int frame) {
+    Mat3x3d skewMat;;
+
+    Vector3d ji = getJ(frame);
+    Mat3x3d I =  getI();
+
+    skewMat(0, 0) =0;
+    skewMat(0, 1) = ji[2] /I(2, 2);
+    skewMat(0, 2) = -ji[1] /I(1, 1);
+
+    skewMat(1, 0) = -ji[2] /I(2, 2);
+    skewMat(1, 1) = 0;
+    skewMat(1, 2) = ji[0]/I(0, 0);
+
+    skewMat(2, 0) =ji[1] /I(1, 1);
+    skewMat(2, 1) = -ji[0]/I(0, 0);
+    skewMat(2, 2) = 0;
+
+    Mat3x3d mat = (getA(frame) * skewMat).transpose();
+    Vector3d rbVel = getVel(frame);
+
+
+    Vector3d velRot;        
+    for (int i =0 ; i < refCoords_.size(); ++i) {
+        atoms_[i]->setVel(rbVel + mat * refCoords_[i], frame);
+    }
+
+}
+
+        
+
 bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
     if (index < atoms_.size()) {
 
@@ -424,9 +511,9 @@ void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
       simError();
     }    
     
-    euler[0] = ats->getEulerPhi();
-    euler[1] = ats->getEulerTheta();
-    euler[2] = ats->getEulerPsi();
+    euler[0] = ats->getEulerPhi() * NumericConstant::PI /180.0;
+    euler[1] = ats->getEulerTheta() * NumericConstant::PI /180.0;
+    euler[2] = ats->getEulerPsi() * NumericConstant::PI /180.0;
 
     RotMat3x3d Atmp(euler);
     refOrients_.push_back(Atmp);