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);
  //oldRb->setQ(q);
  
}
Revision:	1268
Committed:	Fri Jun 11 17:16:21 2004 UTC (21 years, 4 months ago) by tim
File size:	5173 byte(s)
Log Message:	roll in progress
#	Content
1	#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	//oldRb->setQ(q);
189
190	}