AnimatLab  2
Test
VsRigidBody.cpp
1 // VsRigidBody.cpp: implementation of the VsRigidBody class.
2 //
4 
5 #include "StdAfx.h"
6 #include "VsMovableItem.h"
7 #include "VsBody.h"
8 #include "VsJoint.h"
9 #include "VsRigidBody.h"
10 #include "VsStructure.h"
11 #include "VsSimulator.h"
12 #include "VsOsgUserData.h"
13 #include "VsOsgUserDataVisitor.h"
14 #include "VsDragger.h"
15 
16 namespace VortexAnimatSim
17 {
18  namespace Environment
19  {
20 
22 // Construction/Destruction
24 
25 VsRigidBody::VsRigidBody()
26 {
27  m_vxSensor = NULL;
28  m_vxPart = NULL;
29  m_vxGeometry = NULL;
30  m_vxCollisionGeometry = NULL;
31  m_fltBlank = 0;
32 
33  m_fltEstimatedMass = 0;
34  m_fltEstimatedVolume = 0;
35 
36  for(int i=0; i<3; i++)
37  {
38  m_vTorque[i] = 0;
39  m_vForce[i] = 0;
40  m_vLinearVelocity[i] = 0;
41  m_vAngularVelocity[i] = 0;
42  m_vLinearAcceleration[i] = 0;
43  m_vAngularAcceleration[i] = 0;
44  }
45 
46 }
47 
48 VsRigidBody::~VsRigidBody()
49 {
50 
51 try
52 {
53  //int i= 5;
54 }
55 catch(...)
56 {Std_TraceMsg(0, "Caught Error in desctructor of VsRigidBody\r\n", "", -1, false, true);}
57 }
58 
59 void VsRigidBody::SetThisPointers()
60 {
61  VsBody::SetThisPointers();
62  m_lpThisRB = dynamic_cast<RigidBody *>(this);
63  if(!m_lpThisRB)
64  THROW_TEXT_ERROR(Vs_Err_lThisPointerNotDefined, Vs_Err_strThisPointerNotDefined, "m_lpThisRB, " + m_lpThisAB->Name());
65 
66  m_lpThisRB->PhysicsBody(this);
67 }
68 
69 Vx::VxCollisionSensor* VsRigidBody::Sensor()
70 {
71  return m_vxSensor;
72 }
73 
74 Vx::VxPart* VsRigidBody::Part()
75 {
76  return m_vxPart;
77 }
78 
79 Vx::VxNode VsRigidBody::GraphicsNode()
80 {
81  return m_vxGraphicNode;
82 }
83 
84 Vx::VxCollisionGeometry *VsRigidBody::CollisionGeometry()
85 {
86  return m_vxCollisionGeometry;
87 }
88 
89 void VsRigidBody::CollisionGeometry(Vx::VxCollisionGeometry *vxGeometry)
90 {
91  m_vxCollisionGeometry = vxGeometry;
92  Physics_FluidDataChanged();
93 }
94 
95 CStdFPoint VsRigidBody::Physics_GetCurrentPosition()
96 {
97  if(m_vxSensor && m_lpThisMI)
98  {
99  Vx::VxReal3Ptr vPos = NULL;
100  m_vxSensor->getPosition(vPos);
101  m_lpThisMI->AbsolutePosition(vPos[0], vPos[1], vPos[2]);
102  return m_lpThisMI->AbsolutePosition();
103  }
104  else
105  {
106  CStdFPoint vPos;
107  return vPos;
108  }
109 }
110 
111 void VsRigidBody::Physics_UpdateMatrix()
112 {
113  VsBody::Physics_UpdateMatrix();
114 
115  if(m_lpThisRB)
116  m_lpThisRB->UpdatePhysicsPosFromGraphics();
117 }
118 
119 void VsRigidBody::UpdatePositionAndRotationFromMatrix()
120 {
121  VsBody::UpdatePositionAndRotationFromMatrix();
122 
123  if(m_lpThisRB)
124  m_lpThisRB->UpdatePhysicsPosFromGraphics();
125 }
126 
127 void VsRigidBody::Physics_UpdateNode()
128 {
129  if(m_lpThisRB)
130  {
131  if(m_lpThisRB->IsContactSensor())
132  ResetSensorCollisionGeom();
133  else if(m_lpThisRB->HasStaticJoint())
134  ResetStaticCollisionGeom(); //If this body uses a static joint then we need to reset the offest matrix for its collision geometry.
135  else if(m_vxSensor)
136  m_vxSensor->updateFromNode();
137  }
138 
139  Physics_UpdateAbsolutePosition();
140 }
141 
142 void VsRigidBody::Physics_SetColor()
143 {
144  if(m_lpThisRB)
145  SetColor(*m_lpThisRB->Ambient(), *m_lpThisRB->Diffuse(), *m_lpThisRB->Specular(), m_lpThisRB->Shininess());
146 }
147 
148 void VsRigidBody::Physics_TextureChanged()
149 {
150  if(m_lpThisRB)
151  SetTexture(m_lpThisRB->Texture());
152 }
153 
154 void VsRigidBody::Physics_SetFreeze(bool bVal)
155 {
156  if(m_vxSensor)
157  m_vxSensor->freeze(bVal);
158 }
159 
160 void VsRigidBody::Physics_SetDensity(float fltVal)
161 {
162  if(m_vxCollisionGeometry)
163  m_vxCollisionGeometry->setRelativeDensity(fltVal);
164 
165  //If this is a static joint then resize the parent so it gets the
166  //volume and mass reclaculated correctly.
167  if(m_lpThisRB && m_lpThisRB->HasStaticJoint())
168  {
169  VsRigidBody *lpVsParent = dynamic_cast<VsRigidBody *>(m_lpThisRB->Parent());
170  if(lpVsParent)
171  lpVsParent->GetBaseValues();
172  }
173 }
174 
175 void VsRigidBody::Physics_SetMaterialID(std::string strID)
176 {
177  if(m_vxCollisionGeometry && m_lpThisAB)
178  {
179  int iMaterialID = m_lpThisAB->GetSimulator()->GetMaterialID(strID);
180 
181  if(iMaterialID >= 0)
182  m_vxCollisionGeometry->setMaterialID(iMaterialID);
183  }
184 }
185 
186 void VsRigidBody::Physics_SetVelocityDamping(float fltLinear, float fltAngular)
187 {
188  if(m_vxPart && fltLinear > 0)
189  m_vxPart->setLinearVelocityDamping(fltLinear);
190 
191  if(m_vxPart && fltAngular > 0)
192  m_vxPart->setAngularVelocityDamping(fltAngular);
193 }
194 
195 void VsRigidBody::Physics_SetCenterOfMass(float fltTx, float fltTy, float fltTz)
196 {
197  if(m_vxPart)
198  {
199  if(fltTx != 0 || fltTy != 0 || fltTz != 0)
200  m_vxPart->setCOMOffset(fltTx, fltTy, fltTz);
201  }
202 }
203 
204 void VsRigidBody::Physics_Resize()
205 {
206  //First lets get rid of the current current geometry and then put new geometry in place.
207  if(m_osgNode.valid())
208  {
209  osg::Geode *osgGroup = NULL;
210  if(m_osgGeometryRotationMT.valid())
211  osgGroup = dynamic_cast<osg::Geode *>(m_osgGeometryRotationMT->getChild(0));
212  else
213  osgGroup = dynamic_cast<osg::Geode *>(m_osgNode.get());
214 
215  if(!osgGroup)
216  THROW_TEXT_ERROR(Vs_Err_lNodeNotGeode, Vs_Err_strNodeNotGeode, m_lpThisAB->Name());
217 
218  if(osgGroup && osgGroup->containsDrawable(m_osgGeometry.get()))
219  osgGroup->removeDrawable(m_osgGeometry.get());
220 
221  m_osgGeometry.release();
222 
223  //Create a new box geometry with the new sizes.
224  CreateGraphicsGeometry();
225  if(m_lpThisAB)
226  m_osgGeometry->setName(m_lpThisAB->Name() + "_Geometry");
227 
228  //Add it to the geode.
229  osgGroup->addDrawable(m_osgGeometry.get());
230 
231  //Now lets re-adjust the gripper size.
232  if(m_osgDragger.valid())
233  m_osgDragger->SetupMatrix();
234 
235  //Reset the user data for the new parts.
236  if(m_osgNodeGroup.valid())
237  {
238  osg::ref_ptr<VsOsgUserDataVisitor> osgVisitor = new VsOsgUserDataVisitor(this);
239  osgVisitor->traverse(*m_osgNodeGroup);
240  }
241  }
242 
243  if(m_vxGeometry)
244  {
245  ResizePhysicsGeometry();
246 
247  //We need to reset the density in order for it to recompute the mass and volume.
248  if(m_lpThisRB)
249  Physics_SetDensity(m_lpThisRB->Density());
250 
251  //Now get base values, including mass and volume
252  GetBaseValues();
253  }
254 }
255 
256 void VsRigidBody::Physics_SelectedVertex(float fltXPos, float fltYPos, float fltZPos)
257 {
258  if(m_lpThisRB->IsCollisionObject() && m_osgSelVertexMT.valid())
259  {
260  osg::Matrix osgMT;
261  osgMT.makeIdentity();
262  osgMT = osgMT.translate(fltXPos, fltYPos, fltZPos);
263 
264  m_osgSelVertexMT->setMatrix(osgMT);
265  }
266 }
267 
268 
269 void VsRigidBody::ShowSelectedVertex()
270 {
271  if(m_lpThisRB && m_lpThisAB && m_lpThisRB->IsCollisionObject() && m_lpThisAB->Selected() && m_osgMT.valid() && m_osgSelVertexMT.valid())
272  {
273  if(!m_osgMT->containsNode(m_osgSelVertexMT.get()))
274  m_osgMT->addChild(m_osgSelVertexMT.get());
275  }
276 }
277 
278 void VsRigidBody::HideSelectedVertex()
279 {
280  if(m_lpThisRB->IsCollisionObject() && m_osgMT.valid() && m_osgSelVertexMT.valid())
281  {
282  if(m_osgMT->containsNode(m_osgSelVertexMT.get()))
283  m_osgMT->removeChild(m_osgSelVertexMT.get());
284  }
285 }
286 
287 void VsRigidBody::Physics_ResizeSelectedReceptiveFieldVertex()
288 {
289  DeleteSelectedVertex();
290  CreateSelectedVertex(m_lpThisAB->Name());
291 }
292 
293 void VsRigidBody::Physics_FluidDataChanged()
294 {
295  if(m_vxCollisionGeometry && m_lpThisRB)
296  {
297  CStdFPoint vpCenter = m_lpThisRB->BuoyancyCenter();
298  Vx::VxReal3 vCenter = {vpCenter.x, vpCenter.y, vpCenter.z};
299 
300  CStdFPoint vpDrag = m_lpThisRB->LinearDrag();
301  Vx::VxReal3 vDrag = {vpDrag.x, vpDrag.y, vpDrag.z};
302 
303  float fltScale = m_lpThisRB->BuoyancyScale();
304  float fltMagnus = m_lpThisRB->Magnus();
305  bool bEnabled = m_lpThisRB->EnableFluids();
306 
307  m_vxCollisionGeometry->setFluidInteractionData(vCenter, vDrag, fltScale, fltMagnus);
308 
309  if(bEnabled)
310  m_vxCollisionGeometry->setDefaultFluidInteractionForceFn();
311  else
312  m_vxCollisionGeometry->setFluidInteractionForceFn(NULL);
313  }
314 }
315 
316 void VsRigidBody::GetBaseValues()
317 {
318  if(m_vxPart && m_lpThisRB)
319  {
320  //Fluid Density is in the units being used. So if the user set 1 g/cm^3
321  //and the units were grams and decimeters then density would be 1000 g/dm^3
322 
323  //Recalculate the mass and volume
324  m_lpThisRB->GetMass();
325  m_lpThisRB->GetVolume();
326 
327  //m_fltBuoyancy = -(m_lpThisAB->GetSimulator()->FluidDensity() * fltVolume * m_lpThisAB->GetSimulator()->Gravity());
328  //m_fltReportBuoyancy = m_fltBuoyancy * m_lpThisAB->GetSimulator()->MassUnits() * m_lpThisAB->GetSimulator()->DistanceUnits();
329  }
330 }
331 
332 void VsRigidBody::Initialize()
333 {
334  //GetBaseValues();
335 }
336 
346 void VsRigidBody::BuildLocalMatrix()
347 {
348  //build the local matrix
349  if(m_lpThisRB && m_lpThisMI && m_lpThisAB)
350  {
351  if(m_lpThisRB->IsRoot())
352  VsBody::BuildLocalMatrix(m_lpThisAB->GetStructure()->AbsolutePosition(), m_lpThisMI->Rotation(), m_lpThisAB->Name());
353  else
354  VsBody::BuildLocalMatrix(m_lpThisMI->Position(), m_lpThisMI->Rotation(), m_lpThisAB->Name());
355  }
356 }
357 
370 osg::Group *VsRigidBody::ParentOSG()
371 {
372  VsMovableItem *lpItem = NULL;
373 
374  if(!m_lpThisRB->IsRoot() && m_lpParentVsMI)
375  return m_lpParentVsMI->GetMatrixTransform();
376  else
377  return GetVsSimulator()->OSGRoot();
378 }
379 
380 void VsRigidBody::SetupPhysics()
381 {
382  //If no geometry is defined then this part does not have a physics representation.
383  //it is purely an osg node attached to other parts. An example of this is an attachment point or a sensor.
384  if(m_vxGeometry && m_lpThisRB)
385  {
386  //If the parent is not null and the joint is null then that means we need to statically link this part to
387  //its parent. So we do not create a physics part, we just get a link to its parents part.
388  if(m_lpThisRB->IsContactSensor())
389  CreateSensorPart();
390  else if(m_lpThisRB->HasStaticJoint())
391  CreateStaticPart();
392  else
393  CreateDynamicPart();
394  }
395 }
396 
397 void VsRigidBody::CreateSensorPart()
398 {
399  if(m_lpThisRB && m_lpThisAB)
400  {
401  VsRigidBody *lpVsParent = dynamic_cast<VsRigidBody *>(m_lpThisRB->Parent());
402 
403  m_vxSensor = new VxCollisionSensor;
404  m_vxPart = NULL;
405  m_vxSensor->setUserData((void*) m_lpThisRB);
406 
407  m_vxSensor->setName(m_lpThisAB->ID().c_str()); // Give it a name.
408  CollisionGeometry(m_vxSensor->addGeometry(m_vxGeometry, 0));
409  std::string strName = m_lpThisAB->ID() + "_CollisionGeometry";
410  m_vxCollisionGeometry->setName(strName.c_str());
411 
412  m_vxSensor->setControl(VxEntity::kControlNode);
413 
414  //For some reason attempting to disable collisions between a sensor and its parent part does not work.
415  //I put some code in the checking to prevent this from being counted.
416 
417  if(lpVsParent)
418  SetFollowEntity(lpVsParent);
419  else
420  m_vxSensor->freeze(m_lpThisRB->Freeze());
421  }
422 }
423 
424 void VsRigidBody::SetFollowEntity(VsRigidBody *lpEntity)
425 {
426  if(m_vxSensor)
427  {
428  m_vxSensor->setFastMoving(true);
429  Vx::VxReal44 vOffset;
430  VxOSG::copyOsgMatrix_to_VxReal44(m_osgMT->getMatrix(), vOffset);
431  Vx::VxTransform vTM(vOffset);
432 
433  Vx::VxCollisionSensor *vxSensor = lpEntity->Sensor();
434  if(vxSensor)
435  m_vxSensor->followEntity(vxSensor, true, &vTM);
436  }
437 }
438 
439 void VsRigidBody::ResetSensorCollisionGeom()
440 {
441  VsRigidBody *lpVsParent = dynamic_cast<VsRigidBody *>(m_lpThisRB->Parent());
442 
443  if(lpVsParent)
444  SetFollowEntity(lpVsParent);
445 }
446 
447 
448 void VsRigidBody::CreateDynamicPart()
449 {
450  if(m_lpThisRB && m_lpThisAB)
451  {
452  // Create the physics object.
453  m_vxPart = new VxPart;
454  m_vxSensor = m_vxPart;
455  m_vxSensor->setUserData((void*) m_lpThisRB);
456  int iMaterialID = m_lpThisAB->GetSimulator()->GetMaterialID(m_lpThisRB->MaterialID());
457 
458  m_vxSensor->setName(m_lpThisAB->ID().c_str()); // Give it a name.
459  m_vxSensor->setControl(m_eControlType); // Set it to dynamic.
460  CollisionGeometry(m_vxSensor->addGeometry(m_vxGeometry, iMaterialID, 0, m_lpThisRB->Density()));
461 
462  GetBaseValues();
463 
464  std::string strName = m_lpThisAB->ID() + "_CollisionGeometry";
465  m_vxCollisionGeometry->setName(strName.c_str());
466  m_vxSensor->setFastMoving(true);
467 
468  //if this body is frozen; freeze it
469  m_vxSensor->freeze(m_lpThisRB->Freeze());
470 
471  //if the center of mass isn't the graphical center then set the offset relative to position and orientation
472  if(m_vxPart)
473  {
474  CStdFPoint vCOM = m_lpThisRB->CenterOfMass();
475  if(vCOM.x != 0 || vCOM.y != 0 || vCOM.z != 0)
476  m_vxPart->setCOMOffset(vCOM.x, vCOM.y, vCOM.z);
477 
478  if(m_lpThisRB->LinearVelocityDamping() > 0)
479  m_vxPart->setLinearVelocityDamping(m_lpThisRB->LinearVelocityDamping());
480 
481  if(m_lpThisRB->AngularVelocityDamping() > 0)
482  m_vxPart->setAngularVelocityDamping(m_lpThisRB->AngularVelocityDamping());
483  }
484  }
485 }
486 
487 void VsRigidBody::CreateStaticPart()
488 {
489  if(m_lpThisRB && m_lpThisAB)
490  {
491  VsRigidBody *lpVsParent = dynamic_cast<VsRigidBody *>(m_lpThisRB->Parent());
492 
493  Vx::VxReal44 vOffset;
494  VxOSG::copyOsgMatrix_to_VxReal44(m_osgMT->getMatrix(), vOffset);
495  int iMaterialID = m_lpThisAB->GetSimulator()->GetMaterialID(m_lpThisRB->MaterialID());
496 
497  if(lpVsParent)
498  {
499  Vx::VxCollisionSensor *vxSensor = lpVsParent->Sensor();
500  if(vxSensor)
501  {
502  CollisionGeometry(vxSensor->addGeometry(m_vxGeometry, iMaterialID, vOffset, m_lpThisRB->Density()));
503  std::string strName = m_lpThisAB->ID() + "_CollisionGeometry";
504  m_vxCollisionGeometry->setName(strName.c_str());
505  }
506  }
507  }
508 }
509 
510 void VsRigidBody::RemoveStaticPart()
511 {
512  VsRigidBody *lpVsParent = dynamic_cast<VsRigidBody *>(m_lpThisRB->Parent());
513 
514  if(lpVsParent)
515  {
516  Vx::VxCollisionSensor *vxSensor = lpVsParent->Sensor();
517  if(vxSensor && m_vxCollisionGeometry)
518  vxSensor->removeCollisionGeometry(m_vxCollisionGeometry);
519  }
520 }
521 
522 void VsRigidBody::ResetStaticCollisionGeom()
523 {
524  if(m_osgMT.valid() && m_lpThisRB && m_lpThisRB->Parent())
525  {
526  VsRigidBody *lpVsParent = dynamic_cast<VsRigidBody *>(m_lpThisRB->Parent());
527 
528  if(lpVsParent)
529  {
530  Vx::VxReal44 vOffset;
531  VxOSG::copyOsgMatrix_to_VxReal44(m_osgMT->getMatrix(), vOffset);
532 
533  Vx::VxCollisionSensor *vxSensor = lpVsParent->Sensor();
534  if(vxSensor && m_vxCollisionGeometry)
535  m_vxCollisionGeometry->setRelativeTransform(vOffset);
536  }
537  }
538 }
539 
540 void VsRigidBody::DeletePhysics()
541 {
542  if(m_vxSensor)
543  {
544  if(GetVsSimulator() && GetVsSimulator()->Universe())
545  {
546  GetVsSimulator()->Universe()->removeEntity(m_vxSensor);
547  delete m_vxSensor;
548  }
549 
550  m_vxSensor = NULL;
551  m_vxPart = NULL;
552  m_vxGeometry = NULL;
553  }
554  else if(m_lpThisRB->HasStaticJoint())
555  RemoveStaticPart();
556 }
557 
558 void VsRigidBody::SetBody()
559 {
560  if(m_vxSensor && m_lpThisAB)
561  {
562  VsSimulator *lpVsSim = dynamic_cast<VsSimulator *>(m_lpThisAB->GetSimulator());
563 
564  osg::MatrixTransform *lpMT = dynamic_cast<osg::MatrixTransform *>(m_osgMT.get());
565  m_vxSensor->setNode(lpMT); // Connect to the node.
566 
567  // Add the part to the universe.
568  if(lpVsSim && lpVsSim->Universe())
569  lpVsSim->Universe()->addEntity(m_vxSensor);
570  }
571 }
572 
573 int VsRigidBody::GetPartIndex(VxPart *vxP0, VxPart *vxP1)
574 {
575  if(m_vxPart == vxP1)
576  return 1;
577  else
578  return 0;
579 }
580 
581 void VsRigidBody::ProcessContacts()
582 {
583  ContactSensor *lpSensor = m_lpThisRB->GetContactSensor();
584  float fDisUnits = m_lpThisAB->GetSimulator()->DistanceUnits();
585  float fMassUnits = m_lpThisAB->GetSimulator()->MassUnits();
586 
587  if(m_vxPart && lpSensor)
588  {
589  lpSensor->ClearCurrents();
590 
591  Vx::VxPart::DynamicsContactIterator itd = m_vxPart->dynamicsContactBegin();
592  VxPart *p[2];
593  int iPartIdx=0;
594  VxReal3 vWorldPos;
595  StdVector3 vBodyPos;
596  VxReal3 vForce;
597  float fltForceMag = 0;
598 
600  //if(m_lpThisRB->GetSimulator()->Time() >= 6.758)
601  // fltForceMag = 0;
602 
603  int iCount=0;
604  for(; itd != m_vxPart->dynamicsContactEnd(); ++itd, iCount++)
605  {
606  VxDynamicsContact *vxDyn = *itd;
607  vxDyn->getPartPair(p, p+1);
608 
609  vxDyn->getPosition(vWorldPos);
610  WorldToBodyCoords(vWorldPos, vBodyPos);
611 
612  iPartIdx = GetPartIndex(p[0], p[1]);
613  vxDyn->getForce(iPartIdx, vForce);
614  fltForceMag = V3_MAG(vForce) * (fMassUnits * fDisUnits);
615 
616  if(fltForceMag > 0)
617  lpSensor->ProcessContact(vBodyPos, fltForceMag);
618  }
619  }
620 }
621 
622 void VsRigidBody::Physics_CollectData()
623 {
624  Vx::VxReal3 vData;
625 
626  if(m_vxSensor)
627  {
628  UpdateWorldMatrix();
629 
630  //Update the world matrix for this part
631  Vx::VxReal44 vxMT;
632  m_vxSensor->getTransform(vxMT);
633  VxOSG::copyVxReal44_to_OsgMatrix(m_osgWorldMatrix, vxMT);
634 
635  //Then update the absolute position and rotation.
636  m_vxSensor->getPosition(vData);
637  m_lpThisMI->AbsolutePosition(vData[0], vData[1], vData[2]);
638 
639  m_vxSensor->getOrientationEulerAngles(vData);
640  m_lpThisMI->ReportRotation(vData[0], vData[1], vData[2]);
641  }
642  else
643  {
644  //If we are here then we did not have a physics component, just and OSG one.
645  Physics_UpdateAbsolutePosition();
646 
647  //TODO: Get Rotation
648  //m_lpThis->ReportRotation(QuaterionToEuler(m_osgLocalMatrix.getRotate());
649  }
650 
651 
652  if(m_lpThisRB->GetContactSensor())
653  ProcessContacts();
654 }
655 
656 void VsRigidBody::Physics_CollectExtraData()
657 {
658  float fDisUnits = m_lpThisAB->GetSimulator()->DistanceUnits();
659  float fMassUnits = m_lpThisAB->GetSimulator()->MassUnits();
660  Vx::VxReal3 vData;
661 
662  if(m_vxPart)
663  {
664  m_vxPart->getLinearVelocity(vData);
665  m_vLinearVelocity[0] = vData[0] * fDisUnits;
666  m_vLinearVelocity[1] = vData[1] * fDisUnits;
667  m_vLinearVelocity[2] = vData[2] * fDisUnits;
668 
669  m_vxPart->getAngularVelocity(vData);
670  m_vAngularVelocity[0] = vData[0];
671  m_vAngularVelocity[1] = vData[1];
672  m_vAngularVelocity[2] = vData[2];
673 
674  Vx::VxReal3 vData2;
675  m_vxPart->getLastForceAndTorque(vData, vData2);
676  float fltRatio = fMassUnits * fDisUnits;
677  m_vForce[0] = vData[0] * fltRatio;
678  m_vForce[1] = vData[1] * fltRatio;
679  m_vForce[2] = vData[2] * fltRatio;
680 
681  fltRatio = fMassUnits * fDisUnits * fDisUnits;
682  m_vTorque[0] = vData2[0] * fltRatio;
683  m_vTorque[1] = vData2[1] * fltRatio;
684  m_vTorque[2] = vData2[2] * fltRatio;
685 
686  Vx::VxReal3 vAccel;
687  m_vxPart->getLinearAcceleration(vAccel);
688  m_vLinearAcceleration[0] = vAccel[0] * fDisUnits;
689  m_vLinearAcceleration[1] = vAccel[1] * fDisUnits;
690  m_vLinearAcceleration[2] = vAccel[2] * fDisUnits;
691 
692  m_vxPart->getAngularAcceleration(vAccel);
693  m_vAngularAcceleration[0] = vAccel[0];
694  m_vAngularAcceleration[1] = vAccel[1];
695  m_vAngularAcceleration[2] = vAccel[2];
696  }
697 
698 }
699 
700 
701 void VsRigidBody::Physics_ResetSimulation()
702 {
703  VsBody::Physics_ResetSimulation();
704 
705  if(m_vxSensor)
706  {
707  //Reset the dynamics of the part and make it match the new scenegraph position
708  m_vxSensor->updateFromNode();
709 
710  if(m_vxPart)
711  {
712  m_vxPart->resetDynamics();
713  m_vxPart->wakeDynamics();
714  }
715  }
716 
717  for(int i=0; i<3; i++)
718  {
719  m_vTorque[i] = 0;
720  m_vForce[i] = 0;
721  m_vLinearVelocity[i] = 0;
722  m_vAngularVelocity[i] = 0;
723  m_vLinearAcceleration[i] = 0;
724  m_vAngularAcceleration[i] = 0;
725  }
726 }
727 
728 void VsRigidBody::Physics_EnableCollision(RigidBody *lpBody)
729 {
730  if(!lpBody)
731  THROW_ERROR(Al_Err_lBodyNotDefined, Al_Err_strBodyNotDefined);
732 
733  VsRigidBody *lpVsBody = dynamic_cast<VsRigidBody *>(lpBody);
734 
735  if(!lpVsBody)
736  THROW_PARAM_ERROR(Al_Err_lUnableToCastBodyToDesiredType, Al_Err_strUnableToCastBodyToDesiredType, "Type", "VsRigidBody");
737 
738  VsSimulator *lpVsSim = dynamic_cast<VsSimulator *>(m_lpThisAB->GetSimulator());
739 
740  if(!lpVsSim)
741  THROW_ERROR(Al_Err_lSimulationNotDefined, Al_Err_strSimulationNotDefined);
742 
743  //If collisions between the two objects is enabled then disable it.
744  if(lpVsSim->Universe()->getPairIntersectEnabled(m_vxCollisionGeometry, lpVsBody->CollisionGeometry()) == false)
745  lpVsSim->Universe()->enablePairIntersect(m_vxPart, lpVsBody->Sensor());
746 }
747 
748 void VsRigidBody::Physics_DisableCollision(RigidBody *lpBody)
749 {
750  if(!lpBody)
751  THROW_ERROR(Al_Err_lBodyNotDefined, Al_Err_strBodyNotDefined);
752 
753  VsRigidBody *lpVsBody = dynamic_cast<VsRigidBody *>(lpBody);
754 
755  if(!lpVsBody)
756  THROW_PARAM_ERROR(Al_Err_lUnableToCastBodyToDesiredType, Al_Err_strUnableToCastBodyToDesiredType, "Type", "VsRigidBody");
757 
758  VsSimulator *lpVsSim = dynamic_cast<VsSimulator *>(m_lpThisAB->GetSimulator());
759 
760  if(!lpVsSim)
761  THROW_ERROR(Al_Err_lSimulationNotDefined, Al_Err_strSimulationNotDefined);
762 
763  //If collisions between the two objects is enabled then disable it.
764  Vx::VxUniverse *lpUniv = lpVsSim->Universe();
765  if(!m_vxCollisionGeometry || !lpVsBody->CollisionGeometry())
766  THROW_PARAM_ERROR(Vs_Err_lCollisionGeomNotDefined, Vs_Err_strCollisionGeomNotDefined, "ID", m_lpThisAB->ID());
767 
768  if(lpUniv->getPairIntersectEnabled(m_vxCollisionGeometry, lpVsBody->CollisionGeometry()) == true)
769  lpUniv->disablePairIntersect(m_vxSensor, lpVsBody->Sensor());
770 }
771 
772 float *VsRigidBody::Physics_GetDataPointer(const std::string &strDataType)
773 {
774  std::string strType = Std_CheckString(strDataType);
775  RigidBody *lpBody = dynamic_cast<RigidBody *>(this);
776 
777  if(strType == "BODYBUOYANCY")
778  return (&m_fltBlank);
779 
780  if(strType == "BODYDRAGFORCEX")
781  return (&m_fltBlank);
782 
783  if(strType == "BODYDRAGFORCEY")
784  return (&m_fltBlank);
785 
786  if(strType == "BODYDRAGFORCEZ")
787  return (&m_fltBlank);
788 
789  if(strType == "BODYDRAGTORQUEX")
790  return (&m_fltBlank);
791 
792  if(strType == "BODYDRAGTORQUEY")
793  return (&m_fltBlank);
794 
795  if(strType == "BODYDRAGTORQUEZ")
796  return (&m_fltBlank);
797 
798  if(strType == "BODYTORQUEX")
799  {
800  GetVsSimulator()->AddToExtractExtraData(m_lpThisRB);
801  return (&m_vTorque[0]);
802  }
803 
804  if(strType == "BODYTORQUEY")
805  {
806  GetVsSimulator()->AddToExtractExtraData(m_lpThisRB);
807  return (&m_vTorque[1]);
808  }
809 
810  if(strType == "BODYTORQUEZ")
811  {
812  GetVsSimulator()->AddToExtractExtraData(m_lpThisRB);
813  return (&m_vTorque[2]);
814  }
815 
816  if(strType == "BODYFORCEX")
817  {
818  GetVsSimulator()->AddToExtractExtraData(m_lpThisRB);
819  return (&m_vForce[0]);
820  }
821 
822  if(strType == "BODYFORCEY")
823  {
824  GetVsSimulator()->AddToExtractExtraData(m_lpThisRB);
825  return (&m_vForce[1]);
826  }
827 
828  if(strType == "BODYFORCEZ")
829  {
830  GetVsSimulator()->AddToExtractExtraData(m_lpThisRB);
831  return (&m_vForce[2]);
832  }
833 
834  if(strType == "BODYLINEARVELOCITYX")
835  {
836  GetVsSimulator()->AddToExtractExtraData(m_lpThisRB);
837  return (&m_vLinearVelocity[0]);
838  }
839 
840  if(strType == "BODYLINEARVELOCITYY")
841  {
842  GetVsSimulator()->AddToExtractExtraData(m_lpThisRB);
843  return (&m_vLinearVelocity[1]);
844  }
845 
846  if(strType == "BODYLINEARVELOCITYZ")
847  {
848  GetVsSimulator()->AddToExtractExtraData(m_lpThisRB);
849  return (&m_vLinearVelocity[2]);
850  }
851 
852  if(strType == "BODYANGULARVELOCITYX")
853  {
854  GetVsSimulator()->AddToExtractExtraData(m_lpThisRB);
855  return (&m_vAngularVelocity[0]);
856  }
857 
858  if(strType == "BODYANGULARVELOCITYY")
859  {
860  GetVsSimulator()->AddToExtractExtraData(m_lpThisRB);
861  return (&m_vAngularVelocity[1]);
862  }
863 
864  if(strType == "BODYANGULARVELOCITYZ")
865  {
866  GetVsSimulator()->AddToExtractExtraData(m_lpThisRB);
867  return (&m_vAngularVelocity[2]);
868  }
869 
870  //if(strType == "BODYBUOYANCY")
871  // {m_bCollectExtraData = true; return (&m_fltReportBuoyancy);}
872 
873  if(strType == "BODYLINEARACCELERATIONX")
874  {
875  GetVsSimulator()->AddToExtractExtraData(m_lpThisRB);
876  return (&m_vLinearAcceleration[0]);
877  }
878 
879  if(strType == "BODYLINEARACCELERATIONY")
880  {
881  GetVsSimulator()->AddToExtractExtraData(m_lpThisRB);
882  return (&m_vLinearAcceleration[1]);
883  }
884 
885  if(strType == "BODYLINEARACCELERATIONZ")
886  {
887  GetVsSimulator()->AddToExtractExtraData(m_lpThisRB);
888  return (&m_vLinearAcceleration[2]);
889  }
890 
891  if(strType == "BODYANGULARACCELERATIONX")
892  {
893  GetVsSimulator()->AddToExtractExtraData(m_lpThisRB);
894  return (&m_vAngularAcceleration[0]);
895  }
896 
897  if(strType == "BODYANGULARACCELERATIONY")
898  {
899  GetVsSimulator()->AddToExtractExtraData(m_lpThisRB);
900  return (&m_vAngularAcceleration[1]);
901  }
902 
903  if(strType == "BODYANGULARACCELERATIONZ")
904  {
905  GetVsSimulator()->AddToExtractExtraData(m_lpThisRB);
906  return (&m_vAngularAcceleration[2]);
907  }
908 
909  if(strType == "ESTIMATEDMASS")
910  return &m_fltEstimatedMass;
911 
912  if(strType == "ESTIMATEDVOLUME")
913  return &m_fltEstimatedVolume;
914 
915  return NULL;
916 }
917 
918 void VsRigidBody::Physics_AddBodyForceAtLocalPos(float fltPx, float fltPy, float fltPz, float fltFx, float fltFy, float fltFz, bool bScaleUnits)
919 {
920  if(m_vxPart && (fltFx || fltFy || fltFz) && !m_lpThisRB->Freeze())
921  {
922  VxReal3 fltF, fltP;
923  if(bScaleUnits)
924  {
925  fltF[0] = fltFx * (m_lpThisAB->GetSimulator()->InverseMassUnits() * m_lpThisAB->GetSimulator()->InverseDistanceUnits());
926  fltF[1] = fltFy * (m_lpThisAB->GetSimulator()->InverseMassUnits() * m_lpThisAB->GetSimulator()->InverseDistanceUnits());
927  fltF[2] = fltFz * (m_lpThisAB->GetSimulator()->InverseMassUnits() * m_lpThisAB->GetSimulator()->InverseDistanceUnits());
928  }
929  else
930  {
931  fltF[0] = fltFx;
932  fltF[1] = fltFy;
933  fltF[2] = fltFz;
934  }
935 
936  fltP[0] = fltPx;
937  fltP[1] = fltPy;
938  fltP[2] = fltPz;
939 
940  m_vxPart->addForceAtPosition(fltF, fltP);
941  }
942 }
943 
944 void VsRigidBody::Physics_AddBodyForceAtWorldPos(float fltPx, float fltPy, float fltPz, float fltFx, float fltFy, float fltFz, bool bScaleUnits)
945 {
946  if(m_vxPart && (fltFx || fltFy || fltFz) && !m_lpThisRB->Freeze())
947  {
948  VxReal3 fltF, fltP;
949  if(bScaleUnits)
950  {
951  fltF[0] = fltFx * (m_lpThisAB->GetSimulator()->InverseMassUnits() * m_lpThisAB->GetSimulator()->InverseDistanceUnits());
952  fltF[1] = fltFy * (m_lpThisAB->GetSimulator()->InverseMassUnits() * m_lpThisAB->GetSimulator()->InverseDistanceUnits());
953  fltF[2] = fltFz * (m_lpThisAB->GetSimulator()->InverseMassUnits() * m_lpThisAB->GetSimulator()->InverseDistanceUnits());
954  }
955  else
956  {
957  fltF[0] = fltFx;
958  fltF[1] = fltFy;
959  fltF[2] = fltFz;
960  }
961 
962  fltP[0] = fltPx;
963  fltP[1] = fltPy;
964  fltP[2] = fltPz;
965 
966  m_vxPart->addForceAtPosition(fltF, fltP);
967  }
968 }
969 
970 void VsRigidBody::Physics_AddBodyTorque(float fltTx, float fltTy, float fltTz, bool bScaleUnits)
971 {
972  if(m_vxPart && (fltTx || fltTy || fltTz))
973  {
974  VxReal3 fltT;
975  if(bScaleUnits)
976  {
977  fltT[0] = fltTx * (m_lpThisAB->GetSimulator()->InverseMassUnits() * m_lpThisAB->GetSimulator()->InverseDistanceUnits() * m_lpThisAB->GetSimulator()->InverseDistanceUnits());
978  fltT[1] = fltTy * (m_lpThisAB->GetSimulator()->InverseMassUnits() * m_lpThisAB->GetSimulator()->InverseDistanceUnits() * m_lpThisAB->GetSimulator()->InverseDistanceUnits());
979  fltT[2] = fltTz * (m_lpThisAB->GetSimulator()->InverseMassUnits() * m_lpThisAB->GetSimulator()->InverseDistanceUnits() * m_lpThisAB->GetSimulator()->InverseDistanceUnits());
980  }
981  else
982  {
983  fltT[0] = fltTx;
984  fltT[1] = fltTy;
985  fltT[2] = fltTz;
986  }
987 
988  m_vxPart->addTorque(fltT);
989  }
990 }
991 
992 CStdFPoint VsRigidBody::Physics_GetVelocityAtPoint(float x, float y, float z)
993 {
994  CStdFPoint linVel(0,0,0);
995  Vx::VxReal3 vxLinVel = {0,0,0};
996  Vx::VxReal3 vxPoint = {x,y,z};
997 
998  //if this is a contact sensor then return nothing.
999  if(m_vxPart)
1000  {
1001  m_vxPart->getVelocityAtPoint(vxPoint, vxLinVel);
1002  linVel.Set(vxLinVel[0], vxLinVel[1], vxLinVel[2]);
1003  }
1004 
1005  return linVel;
1006 }
1007 
1008 float VsRigidBody::Physics_GetMass()
1009 {
1010  float fltMass = 0;
1011 
1012  //If thi spart is frozen then we will get mass values of 0. So we need to
1013  //temporarilly unfreeze it to get the mass and volume, then refreeze it.
1014  if(m_lpThisRB && m_vxSensor && m_lpThisRB->Freeze())
1015  m_vxSensor->freeze(false);
1016 
1017  if(m_vxPart)
1018  fltMass = m_vxPart->getMass();
1019 
1020  if(m_lpThisRB && m_vxSensor && m_lpThisRB->Freeze())
1021  m_vxSensor->freeze(true);
1022 
1023  return fltMass;
1024 }
1025 
1026 float VsRigidBody::Physics_GetDensity()
1027 {
1028  if(m_lpThisRB)
1029  return m_lpThisRB->Density();
1030  else
1031  return 0;
1032 }
1033 
1034 bool VsRigidBody::Physics_HasCollisionGeometry()
1035 {
1036  if(m_vxSensor)
1037  return true;
1038  else
1039  return false;
1040 }
1041 
1042 bool VsRigidBody::Physics_IsDefined()
1043 {
1044  if(m_vxPart && m_vxSensor)
1045  return true;
1046  else
1047  return false;
1048 }
1049 
1050 bool VsRigidBody::Physics_IsGeometryDefined()
1051 {
1052  if(m_vxGeometry)
1053  return true;
1054  else
1055  return false;
1056 }
1057 
1058 void VsRigidBody::Physics_WakeDynamics()
1059 {
1060  if(m_vxPart)
1061  m_vxPart->wakeDynamics();
1062 }
1063 
1064 
1065  } // Environment
1066 } //VortexAnimatSim
VortexAnimatSim::Environment::VsRigidBody::ParentOSG
virtual osg::Group * ParentOSG()
Gets the parent osg node.
Definition: VsRigidBody.cpp:370
VortexAnimatSim::Environment::VsRigidBody
A common class for all rigid body data specific to vortex.
Definition: VsRigidBody.h:55
VortexAnimatSim::Environment::VsRigidBody::m_fltEstimatedVolume
float m_fltEstimatedVolume
The estimated volume. See m_fltEstimatedMass desciption.
Definition: VsRigidBody.h:92
VortexAnimatSim::Environment::VsRigidBody::m_fltBlank
float m_fltBlank
Definition: VsRigidBody.h:96
VortexAnimatSim::Environment::VsMovableItem::m_osgGeometryRotationMT
osg::ref_ptr< osg::MatrixTransform > m_osgGeometryRotationMT
Definition: VsMovableItem.h:46
VortexAnimatSim
Classes for implementing the cm-labs vortex physics engine for AnimatLab.
Definition: MeshMinVertexDistanceVisitor.cpp:5
StdUtils::Std_TraceMsg
void Std_TraceMsg(const int iLevel, std::string strMessage, std::string strSourceFile, int iSourceLine, bool bLogToFile, bool bPrintHeader)
Traces a message to the debugger window.
Definition: StdUtilFunctions.cpp:3137
VortexAnimatSim::Environment::VsRigidBody::BuildLocalMatrix
virtual void BuildLocalMatrix()
Builds the local matrix.
Definition: VsRigidBody.cpp:346
VortexAnimatSim::Environment::VsRigidBody::m_fltEstimatedMass
float m_fltEstimatedMass
Definition: VsRigidBody.h:89
StdUtils::Std_CheckString
std::string Std_CheckString(std::string strVal)
Converts a string to upper case and trims it.
Definition: StdUtilFunctions.cpp:1793
VsStructure.h
Declares the vortex structure class.