OOPSE/libmdtools/ConstraintElement.cpp

#include <iostream>
#include "ConstraintElement.hpp"
#include "GenericData.hpp"
#include "DirectionalAtom.hpp"

using namespace std;

////////////////////////////////////////////////////////////////////////////////
//Implementation of ConstraintElement
////////////////////////////////////////////////////////////////////////////////
ConstraintElement::ConstraintElement(StuntDouble* rhs) : refSd(rhs){
  GenericData* gdata;

  gdata = refSd->getProperty("Moved");
  if (gdata == NULL){
    movedData = new BoolData();
    if(movedData == NULL)
      cerr << "Memory Allocation Error in ConstraintElement::ConstraintElement()" << endl;
    
    movedData->setID("Moved");
    refSd->addProperty(movedData);
  }
  else{
    movedData = dynamic_cast<BoolData*>(gdata);
    if (movedData == NULL)
      cerr << "Dynamic casting to movedData Error in ConstraintElement::ConstraintElement()"<< endl;
  }
  
  gdata = refSd->getProperty("Moving");
  if (gdata == NULL){
    movingData = new BoolData();
    if(movingData == NULL)
      cerr << "Memory Allocation Error in ConstraintElement::ConstraintElement()" << endl;
    
    movingData->setID("Moving");
    refSd->addProperty(movingData);
  }
  else{
    movingData = dynamic_cast<BoolData*>(gdata);
    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;
  }  
}


////////////////////////////////////////////////////////////////////////////////
//Implementation of ConstraintAtom
////////////////////////////////////////////////////////////////////////////////
ConstraintAtom::ConstraintAtom(Atom* rhs) : ConstraintElement(rhs){
  GenericData* gdata;
  ConsAtomData* atomData;
  
  gdata = refSd->getProperty("OldState");
  
  if(gdata ==NULL){
    oldConsAtomInfo = new ConsAtomInfo;
    atomData = new ConsAtomData();
    atomData->setData (oldConsAtomInfo);
    atomData->setID("OldState");

    rhs->addProperty(atomData);
  }
  else{
    atomData = dynamic_cast<ConsAtomData*>(gdata);
    if(atomData == NULL)
      cerr <<  "dynamic_cast to ConsAtomData Error in ConstraintRigidBody::ConstraintRigidBody()" << endl;
    else
      oldConsAtomInfo = atomData->getData();
  }
  
}

void ConstraintAtom::getOldPos(double pos[3]){
  pos[0] = oldConsAtomInfo->pos[0];
  pos[1] = oldConsAtomInfo->pos[1];
  pos[2] = oldConsAtomInfo->pos[2];
}

void ConstraintAtom::getOldVel(double vel[3]){
  vel[0] = oldConsAtomInfo->vel[0];
  vel[1] = oldConsAtomInfo->vel[1];
  vel[2] = oldConsAtomInfo->vel[2];
}

void ConstraintAtom::getOldJ(double j[3]){
  if (refSd->isDirectional()){
    j[0] = oldConsAtomInfo->j[0];
    j[1] = oldConsAtomInfo->j[1];
    j[2] = oldConsAtomInfo->j[2];
  }
  else
    cerr << "Regular Atom don't have angular velocity" << endl;
}

void ConstraintAtom::getOldA(double a[3][3]){
  if (refSd->isDirectional()){
    a[0][0] = oldConsAtomInfo->a[0][0];
    a[0][1] = oldConsAtomInfo->a[0][1];
    a[0][2] = oldConsAtomInfo->a[0][2];

    a[1][0] = oldConsAtomInfo->a[1][0];
    a[1][1] = oldConsAtomInfo->a[1][1];
    a[1][2] = oldConsAtomInfo->a[1][2];

    a[2][0] = oldConsAtomInfo->a[2][0];
    a[2][1] = oldConsAtomInfo->a[2][1];
    a[2][2] = oldConsAtomInfo->a[2][2];
  }
  else
    cerr << "Regular Atom don't have rotation matrix" << endl;
}

void ConstraintAtom::getOldQ(double q[4]){
  if (refSd->isDirectional()){
    q[0] = oldConsAtomInfo->q[0];
    q[1] = oldConsAtomInfo->q[1];
    q[2] = oldConsAtomInfo->q[2];
    q[3] = oldConsAtomInfo->q[3];
  }
  else
    cerr << "Regular Atom don't have quaternion" << endl;

}

void ConstraintAtom::saveOldState(){
  Atom* atom;
  DirectionalAtom* datom;
  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;
    datom->getA(oldConsAtomInfo->a);
    datom->getJ(oldConsAtomInfo->j);
    datom->getQ(oldConsAtomInfo->q);
  }
}

////////////////////////////////////////////////////////////////////////////////
//Implementation of ConstraintRigidBody
////////////////////////////////////////////////////////////////////////////////
ConstraintRigidBody::ConstraintRigidBody(RigidBody* rhs, int index) 
                                 : ConstraintElement(rhs), consAtomIndex(index){
  GenericData* gdata;
  ConsRbData* rbData;
  
  gdata = refSd->getProperty("OldState");
  if(gdata == NULL){
    rbData = new ConsRbData();
    oldRb = new RigidBody(*rhs);
    
    rbData->setID("OldState");
    rbData->setData(oldRb);
    rhs->addProperty(rbData);
  }
  else{
    rbData = dynamic_cast<ConsRbData*>(gdata);
    if(rbData == NULL)
      cerr << "dynamic_cast to ConsRbData Error in ConstraintRigidBody::ConstraintRigidBody()" << endl;
    else
      oldRb = rbData->getData();
  }

  
}

void ConstraintRigidBody::saveOldState(){
  double pos[3];
  double vel[3];
  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);

  refSd->getVel(vel);
  oldRb->setVel(vel);

  refSd->getA(a);
  oldRb->setA(a);

  refSd->getJ(j);
  oldRb->setJ(j);

  //refSd->getQ(q);
  //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);
}
Revision:	1452
Committed:	Mon Aug 23 15:11:36 2004 UTC (20 years ago) by tim
File size:	6791 byte(s)
Log Message:	* empty log message *
#	User	Rev	Content
1	tim	1232	#include <iostream>
2			#include "ConstraintElement.hpp"
3			#include "GenericData.hpp"
4			#include "DirectionalAtom.hpp"
5	tim	1452
6	tim	1232	using namespace std;
7
8			////////////////////////////////////////////////////////////////////////////////
9			//Implementation of ConstraintElement
10			////////////////////////////////////////////////////////////////////////////////
11			ConstraintElement::ConstraintElement(StuntDouble* rhs) : refSd(rhs){
12			GenericData* gdata;
13
14			gdata = refSd->getProperty("Moved");
15			if (gdata == NULL){
16			movedData = new BoolData();
17			if(movedData == NULL)
18			cerr << "Memory Allocation Error in ConstraintElement::ConstraintElement()" << endl;
19
20			movedData->setID("Moved");
21			refSd->addProperty(movedData);
22			}
23			else{
24			movedData = dynamic_cast<BoolData*>(gdata);
25			if (movedData == NULL)
26			cerr << "Dynamic casting to movedData Error in ConstraintElement::ConstraintElement()"<< endl;
27			}
28
29			gdata = refSd->getProperty("Moving");
30			if (gdata == NULL){
31			movingData = new BoolData();
32			if(movingData == NULL)
33			cerr << "Memory Allocation Error in ConstraintElement::ConstraintElement()" << endl;
34
35			movingData->setID("Moving");
36			refSd->addProperty(movingData);
37			}
38			else{
39			movingData = dynamic_cast<BoolData*>(gdata);
40			if (movingData == NULL)
41			cerr << "Dynamic casting to movingData Error in ConstraintElement::ConstraintElement()"<< endl;
42			}
43	tim	1452
44
45			gdata = refSd->getProperty("ConsForce");
46			if (gdata == NULL){
47			consForceData= new Vector3dData();
48			if(consForceData == NULL)
49			cerr << "Memory Allocation Error in ConstraintElement::ConstraintElement()" << endl;
50
51			consForceData->setID("ConsForce");
52			refSd->addProperty(consForceData);
53			}
54			else{
55			consForceData = dynamic_cast<Vector3dData*>(gdata);
56			if (consForceData == NULL)
57			cerr << "Dynamic casting to ConsForceData Error in ConstraintElement::ConstraintElement()"<< endl;
58			}
59
60			gdata = refSd->getProperty("ConsTorque");
61			if (gdata == NULL){
62			consTorqueData= new Vector3dData();
63			if(consTorqueData == NULL)
64			cerr << "Memory Allocation Error in ConstraintElement::ConstraintElement()" << endl;
65
66			consTorqueData->setID("ConsForce");
67			refSd->addProperty(consTorqueData);
68			}
69			else{
70			consTorqueData = dynamic_cast<Vector3dData*>(gdata);
71			if (consTorqueData == NULL)
72			cerr << "Dynamic casting to ConsTorqueData Error in ConstraintElement::ConstraintElement()"<< endl;
73			}
74	tim	1232	}
75
76
77			////////////////////////////////////////////////////////////////////////////////
78			//Implementation of ConstraintAtom
79			////////////////////////////////////////////////////////////////////////////////
80			ConstraintAtom::ConstraintAtom(Atom* rhs) : ConstraintElement(rhs){
81			GenericData* gdata;
82			ConsAtomData* atomData;
83
84			gdata = refSd->getProperty("OldState");
85
86			if(gdata ==NULL){
87			oldConsAtomInfo = new ConsAtomInfo;
88			atomData = new ConsAtomData();
89			atomData->setData (oldConsAtomInfo);
90			atomData->setID("OldState");
91
92			rhs->addProperty(atomData);
93			}
94			else{
95			atomData = dynamic_cast<ConsAtomData*>(gdata);
96			if(atomData == NULL)
97			cerr << "dynamic_cast to ConsAtomData Error in ConstraintRigidBody::ConstraintRigidBody()" << endl;
98			else
99			oldConsAtomInfo = atomData->getData();
100			}
101
102			}
103
104			void ConstraintAtom::getOldPos(double pos[3]){
105			pos[0] = oldConsAtomInfo->pos[0];
106			pos[1] = oldConsAtomInfo->pos[1];
107			pos[2] = oldConsAtomInfo->pos[2];
108			}
109
110			void ConstraintAtom::getOldVel(double vel[3]){
111			vel[0] = oldConsAtomInfo->vel[0];
112			vel[1] = oldConsAtomInfo->vel[1];
113			vel[2] = oldConsAtomInfo->vel[2];
114			}
115
116			void ConstraintAtom::getOldJ(double j[3]){
117			if (refSd->isDirectional()){
118			j[0] = oldConsAtomInfo->j[0];
119			j[1] = oldConsAtomInfo->j[1];
120			j[2] = oldConsAtomInfo->j[2];
121			}
122			else
123			cerr << "Regular Atom don't have angular velocity" << endl;
124			}
125
126			void ConstraintAtom::getOldA(double a[3][3]){
127			if (refSd->isDirectional()){
128			a[0][0] = oldConsAtomInfo->a[0][0];
129			a[0][1] = oldConsAtomInfo->a[0][1];
130			a[0][2] = oldConsAtomInfo->a[0][2];
131
132			a[1][0] = oldConsAtomInfo->a[1][0];
133			a[1][1] = oldConsAtomInfo->a[1][1];
134			a[1][2] = oldConsAtomInfo->a[1][2];
135
136			a[2][0] = oldConsAtomInfo->a[2][0];
137			a[2][1] = oldConsAtomInfo->a[2][1];
138			a[2][2] = oldConsAtomInfo->a[2][2];
139			}
140			else
141			cerr << "Regular Atom don't have rotation matrix" << endl;
142			}
143
144			void ConstraintAtom::getOldQ(double q[4]){
145			if (refSd->isDirectional()){
146			q[0] = oldConsAtomInfo->q[0];
147			q[1] = oldConsAtomInfo->q[1];
148			q[2] = oldConsAtomInfo->q[2];
149			q[3] = oldConsAtomInfo->q[3];
150			}
151			else
152			cerr << "Regular Atom don't have quaternion" << endl;
153
154			}
155
156			void ConstraintAtom::saveOldState(){
157			Atom* atom;
158			DirectionalAtom* datom;
159			atom = (Atom*)refSd;
160			atom->getPos(oldConsAtomInfo->pos);
161			atom->getVel(oldConsAtomInfo->vel);
162	tim	1452	atom->getFrc(oldConsAtomInfo->frc);
163			//don't forget torque
164	tim	1232
165			if(atom->isDirectional()){
166			datom = (DirectionalAtom*) atom;
167			datom->getA(oldConsAtomInfo->a);
168			datom->getJ(oldConsAtomInfo->j);
169			datom->getQ(oldConsAtomInfo->q);
170			}
171			}
172
173			////////////////////////////////////////////////////////////////////////////////
174			//Implementation of ConstraintRigidBody
175			////////////////////////////////////////////////////////////////////////////////
176			ConstraintRigidBody::ConstraintRigidBody(RigidBody* rhs, int index)
177			: ConstraintElement(rhs), consAtomIndex(index){
178			GenericData* gdata;
179			ConsRbData* rbData;
180
181			gdata = refSd->getProperty("OldState");
182			if(gdata == NULL){
183			rbData = new ConsRbData();
184			oldRb = new RigidBody(*rhs);
185
186			rbData->setID("OldState");
187			rbData->setData(oldRb);
188			rhs->addProperty(rbData);
189			}
190			else{
191			rbData = dynamic_cast<ConsRbData*>(gdata);
192			if(rbData == NULL)
193			cerr << "dynamic_cast to ConsRbData Error in ConstraintRigidBody::ConstraintRigidBody()" << endl;
194			else
195			oldRb = rbData->getData();
196			}
197	tim	1452
198
199	tim	1232	}
200
201			void ConstraintRigidBody::saveOldState(){
202			double pos[3];
203			double vel[3];
204			double a[3][3];
205			double j[3];
206			double q[3];
207	tim	1452	double frc[3];
208			double trq[3];
209	tim	1232
210	tim	1452	refSd->getFrc(frc);
211			oldRb->setFrc(frc);
212
213			refSd->getTrq(trq);
214			oldRb->setTrq(trq);
215
216	tim	1232	refSd->getPos(pos);
217			oldRb->setPos(pos);
218
219			refSd->getVel(vel);
220			oldRb->setVel(vel);
221
222			refSd->getA(a);
223			oldRb->setA(a);
224
225			refSd->getJ(j);
226			oldRb->setJ(j);
227
228	tim	1268	//refSd->getQ(q);
229			//oldRb->setQ(q);
230	tim	1232
231			}
232	tim	1452
233			void ConstraintRigidBody::restoreUnconsStatus(){
234			double pos[3];
235			double vel[3];
236			double a[3][3];
237			double j[3];
238			double q[3];
239			double frc[3];
240			double trq[3];
241
242			oldRb->getFrc(frc);
243			refSd->setFrc(frc);
244
245			oldRb->getTrq(trq);
246			refSd->setTrq(trq);
247
248			oldRb->getPos(pos);
249			refSd->setPos(pos);
250
251			oldRb->getVel(vel);
252			refSd->setVel(vel);
253
254			oldRb->getA(a);
255			refSd->setA(a);
256
257			oldRb->getJ(j);
258			refSd->setJ(j);
259			}