# | Line 2 | Line 2 | |
---|---|---|
2 | #include "ConstraintElement.hpp" | |
3 | #include "GenericData.hpp" | |
4 | #include "DirectionalAtom.hpp" | |
5 | + | |
6 | using namespace std; | |
7 | ||
8 | //////////////////////////////////////////////////////////////////////////////// | |
# | Line 23 | Line 24 | ConstraintElement::ConstraintElement(StuntDouble* rhs) | |
24 | movedData = dynamic_cast<BoolData*>(gdata); | |
25 | if (movedData == NULL) | |
26 | cerr << "Dynamic casting to movedData Error in ConstraintElement::ConstraintElement()"<< endl; | |
26 | – | |
27 | – | movedData = (BoolData*) gdata; |
27 | } | |
28 | ||
29 | gdata = refSd->getProperty("Moving"); | |
# | Line 41 | Line 40 | ConstraintElement::ConstraintElement(StuntDouble* rhs) | |
40 | if (movingData == NULL) | |
41 | cerr << "Dynamic casting to movingData Error in ConstraintElement::ConstraintElement()"<< endl; | |
42 | } | |
43 | < | |
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 | ||
# | Line 130 | Line 159 | void ConstraintAtom::saveOldState(){ | |
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; | |
# | Line 163 | Line 194 | ConstraintRigidBody::ConstraintRigidBody(RigidBody* rh | |
194 | else | |
195 | oldRb = rbData->getData(); | |
196 | } | |
197 | + | |
198 | + | |
199 | } | |
200 | ||
201 | void ConstraintRigidBody::saveOldState(){ | |
# | Line 171 | Line 204 | void ConstraintRigidBody::saveOldState(){ | |
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 | ||
# | Line 188 | Line 229 | void ConstraintRigidBody::saveOldState(){ | |
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 | + | } |
– | Removed lines |
+ | Added lines |
< | Changed lines |
> | Changed lines |