| 2 |
|
#include "ConstraintElement.hpp" |
| 3 |
|
#include "GenericData.hpp" |
| 4 |
|
#include "DirectionalAtom.hpp" |
| 5 |
+ |
|
| 6 |
|
using namespace std; |
| 7 |
|
|
| 8 |
|
//////////////////////////////////////////////////////////////////////////////// |
| 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"); |
| 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 |
|
|
| 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; |
| 194 |
|
else |
| 195 |
|
oldRb = rbData->getData(); |
| 196 |
|
} |
| 197 |
+ |
|
| 198 |
+ |
|
| 199 |
|
} |
| 200 |
|
|
| 201 |
|
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 |
|
|
| 225 |
|
refSd->getJ(j); |
| 226 |
|
oldRb->setJ(j); |
| 227 |
|
|
| 228 |
< |
refSd->getQ(q); |
| 229 |
< |
refSd->setQ(q); |
| 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 |
+ |
} |
