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 (19 years, 10 months ago) by tim
File size:	6791 byte(s)
Log Message:	* empty log message *
#	Content
1	#include <iostream>
2	#include "ConstraintElement.hpp"
3	#include "GenericData.hpp"
4	#include "DirectionalAtom.hpp"
5
6	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
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	}
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	atom->getFrc(oldConsAtomInfo->frc);
163	//don't forget torque
164
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
198
199	}
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	double frc[3];
208	double trq[3];
209
210	refSd->getFrc(frc);
211	oldRb->setFrc(frc);
212
213	refSd->getTrq(trq);
214	oldRb->setTrq(trq);
215
216	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	//refSd->getQ(q);
229	//oldRb->setQ(q);
230
231	}
232
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	}