--- trunk/OOPSE/libmdtools/ConstraintElement.cpp	2004/06/11 17:16:21	1268
+++ trunk/OOPSE/libmdtools/ConstraintElement.cpp	2004/08/23 15:11:36	1452
@@ -2,6 +2,7 @@ using namespace std;
 #include "ConstraintElement.hpp"
 #include "GenericData.hpp"
 #include "DirectionalAtom.hpp"
+
 using namespace std;
 
 ////////////////////////////////////////////////////////////////////////////////
\ No newline at end of file
@@ -23,8 +24,6 @@ ConstraintElement::ConstraintElement(StuntDouble* rhs)
     movedData = dynamic_cast<BoolData*>(gdata);
     if (movedData == NULL)
       cerr << "Dynamic casting to movedData Error in ConstraintElement::ConstraintElement()"<< endl;
-    
-    movedData = (BoolData*) gdata;
   }
   
   gdata = refSd->getProperty("Moving");
\ No newline at end of file
@@ -41,7 +40,37 @@ ConstraintElement::ConstraintElement(StuntDouble* rhs)
     if (movingData == NULL)
       cerr << "Dynamic casting to movingData Error in ConstraintElement::ConstraintElement()"<< endl;
   }
-  
+
+
+  gdata = refSd->getProperty("ConsForce");
+  if (gdata == NULL){
+    consForceData= new Vector3dData();
+    if(consForceData == NULL)
+      cerr << "Memory Allocation Error in ConstraintElement::ConstraintElement()" << endl;
+    
+    consForceData->setID("ConsForce");
+    refSd->addProperty(consForceData);
+  }
+  else{
+    consForceData = dynamic_cast<Vector3dData*>(gdata);
+    if (consForceData == NULL)
+      cerr << "Dynamic casting to ConsForceData Error in ConstraintElement::ConstraintElement()"<< endl;
+  }
+
+  gdata = refSd->getProperty("ConsTorque");
+  if (gdata == NULL){
+    consTorqueData= new Vector3dData();
+    if(consTorqueData == NULL)
+      cerr << "Memory Allocation Error in ConstraintElement::ConstraintElement()" << endl;
+    
+    consTorqueData->setID("ConsForce");
+    refSd->addProperty(consTorqueData);
+  }
+  else{
+    consTorqueData = dynamic_cast<Vector3dData*>(gdata);
+    if (consTorqueData == NULL)
+      cerr << "Dynamic casting to ConsTorqueData Error in ConstraintElement::ConstraintElement()"<< endl;
+  }  
 }
 
 
\ No newline at end of file
@@ -130,6 +159,8 @@ void ConstraintAtom::saveOldState(){
   atom = (Atom*)refSd;
   atom->getPos(oldConsAtomInfo->pos);
   atom->getVel(oldConsAtomInfo->vel);
+  atom->getFrc(oldConsAtomInfo->frc);
+  //don't forget torque
 
   if(atom->isDirectional()){
     datom = (DirectionalAtom*) atom;
\ No newline at end of file
@@ -163,6 +194,8 @@ ConstraintRigidBody::ConstraintRigidBody(RigidBody* rh
     else
       oldRb = rbData->getData();
   }
+
+  
 }
 
 void ConstraintRigidBody::saveOldState(){
\ No newline at end of file
@@ -171,7 +204,15 @@ void ConstraintRigidBody::saveOldState(){
   double a[3][3];
   double j[3];
   double q[3];
+  double frc[3];
+  double trq[3];
   
+  refSd->getFrc(frc);
+  oldRb->setFrc(frc);
+
+  refSd->getTrq(trq);
+  oldRb->setTrq(trq);
+  
   refSd->getPos(pos);
   oldRb->setPos(pos);
 
\ No newline at end of file
@@ -188,3 +229,31 @@ void ConstraintRigidBody::saveOldState(){
   //oldRb->setQ(q);
   
 }
+
+void ConstraintRigidBody::restoreUnconsStatus(){
+  double pos[3];
+  double vel[3];
+  double a[3][3];
+  double j[3];
+  double q[3];
+  double frc[3];
+  double trq[3];
+
+  oldRb->getFrc(frc);
+  refSd->setFrc(frc);
+
+  oldRb->getTrq(trq);
+  refSd->setTrq(trq);
+  
+  oldRb->getPos(pos);
+  refSd->setPos(pos);
+
+  oldRb->getVel(vel);
+  refSd->setVel(vel);
+
+  oldRb->getA(a);
+  refSd->setA(a);
+
+  oldRb->getJ(j);
+  refSd->setJ(j);
+}
\ No newline at end of file