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;
    
    movedData = (BoolData*) gdata;
  }
  
  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;
  }
  
}


////////////////////////////////////////////////////////////////////////////////
//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);

  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];
  
  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);
  refSd->setQ(q);
  
}
Revision:	1232
Committed:	Thu Jun 3 21:51:55 2004 UTC (21 years, 5 months ago) by tim
File size:	5169 byte(s)
Log Message:	new implementation of constraint
#	User	Rev	Content
1	tim	1232	#include <iostream>
2			#include "ConstraintElement.hpp"
3			#include "GenericData.hpp"
4			#include "DirectionalAtom.hpp"
5			using namespace std;
6
7			////////////////////////////////////////////////////////////////////////////////
8			//Implementation of ConstraintElement
9			////////////////////////////////////////////////////////////////////////////////
10			ConstraintElement::ConstraintElement(StuntDouble* rhs) : refSd(rhs){
11			GenericData* gdata;
12
13			gdata = refSd->getProperty("Moved");
14			if (gdata == NULL){
15			movedData = new BoolData();
16			if(movedData == NULL)
17			cerr << "Memory Allocation Error in ConstraintElement::ConstraintElement()" << endl;
18
19			movedData->setID("Moved");
20			refSd->addProperty(movedData);
21			}
22			else{
23			movedData = dynamic_cast<BoolData*>(gdata);
24			if (movedData == NULL)
25			cerr << "Dynamic casting to movedData Error in ConstraintElement::ConstraintElement()"<< endl;
26
27			movedData = (BoolData*) gdata;
28			}
29
30			gdata = refSd->getProperty("Moving");
31			if (gdata == NULL){
32			movingData = new BoolData();
33			if(movingData == NULL)
34			cerr << "Memory Allocation Error in ConstraintElement::ConstraintElement()" << endl;
35
36			movingData->setID("Moving");
37			refSd->addProperty(movingData);
38			}
39			else{
40			movingData = dynamic_cast<BoolData*>(gdata);
41			if (movingData == NULL)
42			cerr << "Dynamic casting to movingData Error in ConstraintElement::ConstraintElement()"<< endl;
43			}
44
45			}
46
47
48			////////////////////////////////////////////////////////////////////////////////
49			//Implementation of ConstraintAtom
50			////////////////////////////////////////////////////////////////////////////////
51			ConstraintAtom::ConstraintAtom(Atom* rhs) : ConstraintElement(rhs){
52			GenericData* gdata;
53			ConsAtomData* atomData;
54
55			gdata = refSd->getProperty("OldState");
56
57			if(gdata ==NULL){
58			oldConsAtomInfo = new ConsAtomInfo;
59			atomData = new ConsAtomData();
60			atomData->setData (oldConsAtomInfo);
61			atomData->setID("OldState");
62
63			rhs->addProperty(atomData);
64			}
65			else{
66			atomData = dynamic_cast<ConsAtomData*>(gdata);
67			if(atomData == NULL)
68			cerr << "dynamic_cast to ConsAtomData Error in ConstraintRigidBody::ConstraintRigidBody()" << endl;
69			else
70			oldConsAtomInfo = atomData->getData();
71			}
72
73			}
74
75			void ConstraintAtom::getOldPos(double pos[3]){
76			pos[0] = oldConsAtomInfo->pos[0];
77			pos[1] = oldConsAtomInfo->pos[1];
78			pos[2] = oldConsAtomInfo->pos[2];
79			}
80
81			void ConstraintAtom::getOldVel(double vel[3]){
82			vel[0] = oldConsAtomInfo->vel[0];
83			vel[1] = oldConsAtomInfo->vel[1];
84			vel[2] = oldConsAtomInfo->vel[2];
85			}
86
87			void ConstraintAtom::getOldJ(double j[3]){
88			if (refSd->isDirectional()){
89			j[0] = oldConsAtomInfo->j[0];
90			j[1] = oldConsAtomInfo->j[1];
91			j[2] = oldConsAtomInfo->j[2];
92			}
93			else
94			cerr << "Regular Atom don't have angular velocity" << endl;
95			}
96
97			void ConstraintAtom::getOldA(double a[3][3]){
98			if (refSd->isDirectional()){
99			a[0][0] = oldConsAtomInfo->a[0][0];
100			a[0][1] = oldConsAtomInfo->a[0][1];
101			a[0][2] = oldConsAtomInfo->a[0][2];
102
103			a[1][0] = oldConsAtomInfo->a[1][0];
104			a[1][1] = oldConsAtomInfo->a[1][1];
105			a[1][2] = oldConsAtomInfo->a[1][2];
106
107			a[2][0] = oldConsAtomInfo->a[2][0];
108			a[2][1] = oldConsAtomInfo->a[2][1];
109			a[2][2] = oldConsAtomInfo->a[2][2];
110			}
111			else
112			cerr << "Regular Atom don't have rotation matrix" << endl;
113			}
114
115			void ConstraintAtom::getOldQ(double q[4]){
116			if (refSd->isDirectional()){
117			q[0] = oldConsAtomInfo->q[0];
118			q[1] = oldConsAtomInfo->q[1];
119			q[2] = oldConsAtomInfo->q[2];
120			q[3] = oldConsAtomInfo->q[3];
121			}
122			else
123			cerr << "Regular Atom don't have quaternion" << endl;
124
125			}
126
127			void ConstraintAtom::saveOldState(){
128			Atom* atom;
129			DirectionalAtom* datom;
130			atom = (Atom*)refSd;
131			atom->getPos(oldConsAtomInfo->pos);
132			atom->getVel(oldConsAtomInfo->vel);
133
134			if(atom->isDirectional()){
135			datom = (DirectionalAtom*) atom;
136			datom->getA(oldConsAtomInfo->a);
137			datom->getJ(oldConsAtomInfo->j);
138			datom->getQ(oldConsAtomInfo->q);
139			}
140			}
141
142			////////////////////////////////////////////////////////////////////////////////
143			//Implementation of ConstraintRigidBody
144			////////////////////////////////////////////////////////////////////////////////
145			ConstraintRigidBody::ConstraintRigidBody(RigidBody* rhs, int index)
146			: ConstraintElement(rhs), consAtomIndex(index){
147			GenericData* gdata;
148			ConsRbData* rbData;
149
150			gdata = refSd->getProperty("OldState");
151			if(gdata == NULL){
152			rbData = new ConsRbData();
153			oldRb = new RigidBody(*rhs);
154
155			rbData->setID("OldState");
156			rbData->setData(oldRb);
157			rhs->addProperty(rbData);
158			}
159			else{
160			rbData = dynamic_cast<ConsRbData*>(gdata);
161			if(rbData == NULL)
162			cerr << "dynamic_cast to ConsRbData Error in ConstraintRigidBody::ConstraintRigidBody()" << endl;
163			else
164			oldRb = rbData->getData();
165			}
166			}
167
168			void ConstraintRigidBody::saveOldState(){
169			double pos[3];
170			double vel[3];
171			double a[3][3];
172			double j[3];
173			double q[3];
174
175			refSd->getPos(pos);
176			oldRb->setPos(pos);
177
178			refSd->getVel(vel);
179			oldRb->setVel(vel);
180
181			refSd->getA(a);
182			oldRb->setA(a);
183
184			refSd->getJ(j);
185			oldRb->setJ(j);
186
187			refSd->getQ(q);
188			refSd->setQ(q);
189
190			}