--- trunk/OOPSE/libmdtools/RigidBody.cpp	2004/04/19 20:54:58	1120
+++ trunk/OOPSE/libmdtools/RigidBody.cpp	2004/08/23 15:11:36	1452
@@ -8,6 +8,7 @@ RigidBody::RigidBody() : StuntDouble() {
   objType = OT_RIGIDBODY;
   is_linear = false;
   linear_axis =  -1;
+  momIntTol = 1e-6;
 }
 
 RigidBody::~RigidBody() {
@@ -85,6 +86,11 @@ void RigidBody::getFrc(double theF[3]){
     theF[i] = frc[i];
 }	
 
+void RigidBody::setFrc(double theF[3]){
+  for (int i = 0; i < 3 ; i++) 
+    frc[i] = theF[i];
+} 
+
 void RigidBody::addFrc(double theF[3]){
   for (int i = 0; i < 3 ; i++) 
     frc[i] += theF[i];
@@ -182,7 +188,6 @@ void RigidBody::setQ( double the_q[4] ){
   A[2][0] = 2.0 * ( the_q[1] * the_q[3] + the_q[0] * the_q[2] );
   A[2][1] = 2.0 * ( the_q[2] * the_q[3] - the_q[0] * the_q[1] );
   A[2][2] = q0Sqr - q1Sqr -q2Sqr +q3Sqr;   
-
 }
 
 void RigidBody::getA( double the_A[3][3] ){
@@ -218,6 +223,11 @@ void RigidBody::getTrq(double theT[3]){
 void RigidBody::getTrq(double theT[3]){
   for (int i = 0; i < 3 ; i++) 
     theT[i] = trq[i];
+}	
+
+void RigidBody::setTrq(double theT[3]){
+  for (int i = 0; i < 3 ; i++) 
+    trq[i] = theT[i];
 }	
 
 void RigidBody::addTrq(double theT[3]){
@@ -258,7 +268,19 @@ void RigidBody::body2Lab( double r[3] ){
   r[0] = (A[0][0] * rb[0]) + (A[1][0] * rb[1]) + (A[2][0] * rb[2]);
   r[1] = (A[0][1] * rb[0]) + (A[1][1] * rb[1]) + (A[2][1] * rb[2]);
   r[2] = (A[0][2] * rb[0]) + (A[1][2] * rb[1]) + (A[2][2] * rb[2]);
+
+}
+
+double RigidBody::getZangle( ){
+    return zAngle;
+}
+
+void RigidBody::setZangle( double zAng ){
+    zAngle = zAng;
+}
 
+void RigidBody::addZangle( double zAng ){
+    zAngle += zAng;
 }
 
 void RigidBody::calcRefCoords( ) {
@@ -632,4 +654,67 @@ void RigidBody::accept(BaseVisitor* v){
 
   //for(atomIter = myAtoms.begin(); atomIter != myAtoms.end(); ++atomIter)
   //  (*atomIter)->accept(v);
-}
\ No newline at end of file
+}
+void RigidBody::getAtomRefCoor(double pos[3], int index){
+  vec3 ref;
+
+  ref = refCoords[index];
+  pos[0] = ref[0];
+  pos[1] = ref[1];
+  pos[2] = ref[2];
+  
+}
+
+
+void RigidBody::getAtomPos(double theP[3], int index){
+  vec3 ref;
+
+  if (index >= myAtoms.size())
+    cerr << index << " is an invalid index, current rigid body contains " << myAtoms.size() << "atoms" << endl;
+
+  ref = refCoords[index];
+  body2Lab(ref.vec);
+  
+  theP[0] = pos[0] + ref[0];
+  theP[1] = pos[1] + ref[1];
+  theP[2] = pos[2] + ref[2];
+}
+
+
+void RigidBody::getAtomVel(double theV[3], int index){
+  vec3 ref;
+  double velRot[3];
+  double skewMat[3][3];
+  double aSkewMat[3][3];
+  double aSkewTransMat[3][3];
+  
+  //velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
+
+  if (index >= myAtoms.size())
+    cerr << index << " is an invalid index, current rigid body contains " << myAtoms.size() << "atoms" << endl;
+
+  ref = refCoords[index];
+
+  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;
+  
+  matMul3(A, skewMat, aSkewMat);
+
+  transposeMat3(aSkewMat, aSkewTransMat);
+
+  matVecMul3(aSkewTransMat, ref.vec, velRot);
+  theV[0] = vel[0] + velRot[0];
+  theV[1] = vel[1] + velRot[1];
+  theV[2] = vel[2] + velRot[2];
+}
+
+