AnimatLab  2
Test
BlSimulator.cpp
1 // BlSimulator.cpp: implementation of the BlSimulator class.
2 //
4 
5 #include "StdAfx.h"
6 #include <stdarg.h>
7 #include "BlOsgGeometry.h"
8 #include "BlJoint.h"
9 #include "BlMotorizedJoint.h"
10 #include "BlRigidBody.h"
11 #include "BlClassFactory.h"
12 #include "BlSimulator.h"
13 
14 namespace BulletAnimatSim
15 {
16 
18 // Construction/Destruction
20 
21 
22 BlSimulator::BlSimulator()
23 {
24  m_lpCollisionConfiguration = NULL;
25  m_lpDispatcher = NULL;
26  m_lpSolver = NULL;
27  m_lpBroadPhase = NULL;
28  m_lpDynamicsWorld = NULL;
29 
30  m_grpScene = NULL;
31  m_vsWinMgr = NULL;
32  m_vsWinMgr = new OsgSimulationWindowMgr;
33  m_lpWinMgr = m_vsWinMgr;
34  m_lpWinMgr->SetSystemPointers(this, NULL, NULL, NULL, true);
35  m_dblTotalStepTime = 0;
36  m_lStepTimeCount = 0;
37  m_dblTotalStepTime= 0;
38  m_lStepTimeCount = 0;
39  m_dblTotalVortexStepTime = 0;
40  m_lStepVortexTimeCount = 0;
41  m_lpMeshMgr = NULL;
42  m_osgAlphafunc = NULL;
43  m_iSubstepCallbackCount = 0;
44 
45  //Setup the global matrix util.
46  m_lpMatrixUtil = new OsgMatrixUtil;
47  //m_lpMatrixUtil = new BlMatrixUtil;
48  SetMatrixUtil(m_lpMatrixUtil);
49 
50  if(!m_lpAnimatClassFactory)
51  m_lpAnimatClassFactory = new BlClassFactory;
52 
53  m_bDrawDebug = false;
54 }
55 
56 BlSimulator::~BlSimulator()
57 {
58 
59 try
60 {
61  if(m_lpMeshMgr)
62  {
63  delete m_lpMeshMgr;
64  m_lpMeshMgr = NULL;
65  }
66 
67  m_bShuttingDown = true;
68 
69  Reset();
70 }
71 catch(...)
72 {Std_TraceMsg(0, "Caught Error in desctructor of Simulator\r\n", "", -1, false, true);}
73 }
74 
75 #pragma region MutatorOverrides
76 
77 void BlSimulator::StabilityScale(float fltVal)
78 {
79  OsgSimulator::StabilityScale(fltVal);
80  SetSimulationStabilityParams();
81 }
82 
83 void BlSimulator::LinearCompliance(float fltVal, bool bUseScaling)
84 {
85  OsgSimulator::LinearCompliance(fltVal, bUseScaling);
86  SetSimulationStabilityParams();
87 }
88 
89 void BlSimulator::AngularCompliance(float fltVal, bool bUseScaling)
90 {
91  OsgSimulator::AngularCompliance(fltVal, bUseScaling);
92  SetSimulationStabilityParams();
93 }
94 
95 void BlSimulator::LinearDamping(float fltVal, bool bUseScaling)
96 {
97  OsgSimulator::LinearDamping(fltVal, bUseScaling);
98  SetSimulationStabilityParams();
99 }
100 
101 void BlSimulator::AngularDamping(float fltVal, bool bUseScaling)
102 {
103  OsgSimulator::AngularDamping(fltVal, bUseScaling);
104  SetSimulationStabilityParams();
105 }
106 
107 void BlSimulator::LinearKineticLoss(float fltVal, bool bUseScaling)
108 {
109  OsgSimulator::LinearKineticLoss(fltVal, bUseScaling);
110  SetSimulationStabilityParams();
111 }
112 
113 void BlSimulator::AngularKineticLoss(float fltVal, bool bUseScaling)
114 {
115  OsgSimulator::AngularKineticLoss(fltVal, bUseScaling);
116  SetSimulationStabilityParams();
117 }
118 
119 void BlSimulator::Gravity(float fltVal, bool bUseScaling)
120 {
121  OsgSimulator::Gravity(fltVal, bUseScaling);
122 
123  if(m_lpDynamicsWorld)
124  m_lpDynamicsWorld->setGravity( btVector3( 0, m_fltGravity, 0 ) );
125 }
126 
127 #pragma endregion
128 
129 
130 SimulationRecorder *BlSimulator::CreateSimulationRecorder()
131 {
132 //NEED_TO_FIX
133  return NULL; //new VsSimulationRecorder;
134 }
135 
136 void BlSimulator::Reset()
137 {
138  OsgSimulator::Reset();
139 
140  if(m_lpDynamicsWorld)
141  {
142  delete m_lpDynamicsWorld;
143  m_lpDynamicsWorld = NULL;
144  }
145 
146  if(m_lpSolver)
147  {
148  delete m_lpSolver;
149  m_lpSolver = NULL;
150  }
151 
152  if(m_lpBroadPhase)
153  {
154  delete m_lpBroadPhase;
155  m_lpBroadPhase = NULL;
156  }
157 
158  if(m_lpDispatcher)
159  {
160  delete m_lpDispatcher;
161  m_lpDispatcher = NULL;
162  }
163 
164  if(m_lpCollisionConfiguration)
165  {
166  delete m_lpCollisionConfiguration;
167  m_lpCollisionConfiguration = NULL;
168  }
169 
170  if(!m_lpAnimatClassFactory)
171  m_lpAnimatClassFactory = new BlClassFactory;
172 
173  m_aryFluidPlanes.clear();
174 }
175 
176 void BlSimulator::ResetSimulation()
177 {
178  OsgSimulator::ResetSimulation();
179 
180  m_bSimRunning = false;
181 }
182 
184 //bool AnimatContactCallback (
185 // btManifoldPoint& cp,
186 // const btCollisionObject* colObj0,
187 // int partId0,
188 // int index0,
189 // const btCollisionObject* colObj1,
190 // int partId1,
191 // int index1)
192 //{
193 //
194 // //if (colObj0->getRootCollisionShape()->getShapeType()==COMPOUND_SHAPE_PROXYTYPE)
195 // //{
196 // // btCompoundShape* compound = (btCompoundShape*)colObj0->getRootCollisionShape();
197 // // btCollisionShape* childShape;
198 // // childShape = compound->getChildShape(index0);
199 // //}
200 //
201 // //if (colObj1->getRootCollisionShape()->getShapeType()==COMPOUND_SHAPE_PROXYTYPE)
202 // //{
203 // // btCompoundShape* compound = (btCompoundShape*)colObj1->getRootCollisionShape();
204 // // btCollisionShape* childShape;
205 // // childShape = compound->getChildShape(index1);
206 // //}
207 //
208 // return true;
209 //}
210 
211 bool AnimatContactCallback(btManifoldPoint& cp, void* body0, void* body1)
212 {
213  btCollisionObject *lpBtBody1 = (btCollisionObject *) body0;
214  btCollisionObject *lpBtBody2 = (btCollisionObject *) body1;
215 
216  //Only process these if they both exist and one of them has custom material callback flag set.
217  if( lpBtBody1 && lpBtBody2 &&
218  ( (lpBtBody1->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) ||
219  (lpBtBody2->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)) )
220  {
221  BlBulletData *lpData1 = (BlBulletData *) lpBtBody1->getUserPointer();
222  BlBulletData *lpData2 = (BlBulletData *) lpBtBody2->getUserPointer();
223 
224  if(lpData1 && lpData1->m_lpBody && lpData2 && lpData2->m_lpBody)
225  {
226  if(lpBtBody1->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)
227  lpData1->m_lpBody->m_aryContactPoints.Add(new BlContactPoint(&cp, lpData2->m_lpBody, true));
228 
229  if(lpBtBody2->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK)
230  lpData2->m_lpBody->m_aryContactPoints.Add(new BlContactPoint(&cp, lpData1->m_lpBody, false));
231  }
232  }
233 
234  return true;
235 }
236 
237 void BlSimulator::InitializeBulletViewer(int argc, const char **argv)
238 {
239  //osg::ArgumentParser arguments(&argc, (char **) argv);
240 
241  osg::setNotifyLevel(osg::NotifySeverity::NOTICE); //ConvertTraceLevelToOSG());
242  //osg::setNotifyLevel(osg::NotifySeverity::DEBUG_INFO); //ConvertTraceLevelToOSG());
243 
244  //osg::notify(osg::NOTICE) << "Setting OSG notice level to '" << Std_GetTraceLevel() << std::endl
245 
246  // set up the usage document, in case we need to print out how to use this program.
247  //arguments.getApplicationUsage()->setApplicationName(arguments.getApplicationName());
248  //arguments.getApplicationUsage()->setDescription(arguments.getApplicationName()+" is the standard OpenSceneGraph example which loads and visualises 3d models.");
249  //arguments.getApplicationUsage()->setCommandLineUsage(arguments.getApplicationName()+" [options] filename ...");
250  //arguments.getApplicationUsage()->addCommandLineOption("-h or --help", "Display this information");
251 
252  //Ensure that our exe path is the only place it will attempt to find library files.
253  osgDB::FilePathList aryList = osgDB::getLibraryFilePathList();
254  std::string strPath = m_strExecutablePath.substr(0, m_strExecutablePath.length()-1);
255  aryList.clear();
256  aryList.push_front(strPath);
257  osgDB::setLibraryFilePathList(aryList);
258 
259  // add resource search paths
260  osgDB::getDataFilePathList().push_front("./Resources");
261  osgDB::getDataFilePathList().push_front("../Resources");
262  osgDB::getDataFilePathList().push_front("../../Resources");
263 
264 #ifndef WIN32
265  osgDB::getDataFilePathList().push_front("/usr/share/fonts/truetype");
266 #endif
267 
268  std::string strFile = osgDB::findLibraryFile("osgdb_freetype.dll");
269 
270  //This is the root of the scenegraph. Which will corrospond
271  //to the root of the simulation
272  m_grpScene = new osg::MatrixTransform;
273  m_grpScene->setMatrix(osg::Matrix::identity());
274  m_grpScene->setName("World");
275 
276  //Add the mouse spring lines to the scene
277  m_grpScene->addChild(m_lpMouseSpring->GetNode());
278 
279  //Create the windows, cameras, and Huds
280  m_vsWinMgr->Initialize();
281 
282  //Create the command manager if needed.
283  if(!m_osgCmdMgr.valid())
284  m_osgCmdMgr = new osgManipulator::CommandManager;
285 
286  osg::StateSet* rootStateSet = m_grpScene->getOrCreateStateSet();
287  rootStateSet->setMode( GL_LIGHTING, osg::StateAttribute::ON );
288  rootStateSet->setMode( GL_LIGHT0, osg::StateAttribute::ON );
289  //rootStateSet->setMode( GL_LIGHT1, osg::StateAttribute::ON );
290 
291  // set up an alphafunc by default to speed up blending operations.
292  m_osgAlphafunc = new osg::AlphaFunc;
293  m_osgAlphafunc->setFunction(osg::AlphaFunc::GEQUAL, m_fltAlphaThreshold);
294  rootStateSet->setAttributeAndModes(m_osgAlphafunc, osg::StateAttribute::ON);
295 
296  m_oLightMgr.Initialize();
297 }
298 
299 void BlSimulator::SetSimulationStabilityParams()
300 {
301 
302 }
303 
304 void BlSimulator::BulletStepFinished(btScalar timeStep)
305 {
306  m_iSubstepCallbackCount++;
307  if(m_iPhysicsSubsteps == m_iSubstepCallbackCount)
308  {
309  AfterStepSimulation();
310  m_iSubstepCallbackCount = 0;
311  }
312 }
313 
314 void ProcessTickCallback(btDynamicsWorld *world, btScalar timeStep)
315 {
316  BlSimulator *w = static_cast<BlSimulator *>(world->getWorldUserInfo());
317  w->BulletStepFinished(timeStep);
318 }
319 
320 //This function initializes the Vortex related
321 //classes and the vortex viewer.
322 void BlSimulator::InitializeBullet(int argc, const char **argv)
323 {
324  InitializeBulletViewer(argc, argv);
325 
326  int iObjectCount = 100 + m_iPhysicsBodyCount;
327  int iCollisionCount = iObjectCount*40;
328 
329  m_lpCollisionConfiguration = new btDefaultCollisionConfiguration();
330  m_lpDispatcher = new BlAnimatCollisionDispatcher(m_lpCollisionConfiguration, this);
331 
332  m_lpSolver = new btSequentialImpulseConstraintSolver;
333  //btDantzigSolver* mlcp = new btDantzigSolver();
334  //m_lpSolver = new btMLCPSolver(mlcp);
335 
336  btVector3 worldAabbMin( -10000, -10000, -10000 );
337  btVector3 worldAabbMax( 10000, 10000, 10000 );
338  m_lpBroadPhase = new btAxisSweep3( worldAabbMin, worldAabbMax, 1000 );
339 
340  m_lpDynamicsWorld = new btDiscreteDynamicsWorld( m_lpDispatcher, m_lpBroadPhase, m_lpSolver, m_lpCollisionConfiguration );
341  m_lpDynamicsWorld->setGravity( btVector3( 0, m_fltGravity, 0 ) );
342 
343  m_lpDynamicsWorld->setInternalTickCallback(ProcessTickCallback, static_cast<void *>(this));
344 
345  if(m_bDrawDebug)
346  {
347  this->OSGRoot()->addChild(m_dbgDraw.getSceneGraph());
348  m_lpDynamicsWorld->setDebugDrawer( &m_dbgDraw );
349  }
350 
351  //gContactAddedCallback = &AnimatContactCallback;
352  gContactProcessedCallback = &AnimatContactCallback;
353 }
354 
370 void BlSimulator::GenerateCollisionMeshFile(std::string strOriginalMeshFile, std::string strCollisionMeshFile, float fltScaleX, float fltScaleY, float fltScaleZ)
371 {
372  //First load the original mesh in.
373  std::string strPath = this->ProjectPath();
374  std::string strOrigFile = AnimatSim::GetFilePath(strPath, strOriginalMeshFile);
375  std::string strNewFile = AnimatSim::GetFilePath(strPath, strCollisionMeshFile);
376 
377  osg::ref_ptr<osg::Node> osgNode = MeshMgr()->LoadMesh(strOrigFile); //osgDB::readNodeFile(strOrigFile.c_str());
378 
379  //Make sure the mesh loaded is valid.
380  if(!osgNode.valid())
381  THROW_PARAM_ERROR(Bl_Err_lErrorLoadingMesh, Bl_Err_strErrorLoadingMesh, "Original Mesh file", strOriginalMeshFile);
382 
383  //Now create a convex mesh with the physics engine using the loaded mesh.
384  btConvexHullShape *btHull = OsgConvexShrunkenHullCollisionShape(osgNode.get());
385 
386  if(!btHull)
387  THROW_PARAM_ERROR(Bl_Err_lConvertingMeshToConvexHull, Bl_Err_strConvertingMeshToConvexHull, "Original Mesh file", strOriginalMeshFile);
388 
389  //Now use that convexmesh geometry to create a new osg node.
390  osg::ref_ptr<osg::Node> osgNewNode = osgbCollision::osgNodeFromBtCollisionShape( btHull );
391 
392  osg::Matrix osgScale = osg::Matrix::scale(fltScaleX, fltScaleY, fltScaleZ);
393  osg::ref_ptr<osg::MatrixTransform> osgScaleMT = new osg::MatrixTransform(osgScale);
394  osgScaleMT->addChild(osgNewNode.get());
395  osgScaleMT->setDataVariance(osg::Object::STATIC);
396 
397  delete btHull;
398  btHull = NULL;
399 
400  // Now do some OSG voodoo, which should spread the transform downward
401  // through the loaded model, and delete the transform.
402  osgUtil::Optimizer optimizer;
403  optimizer.optimize(osgScaleMT.get(), osgUtil::Optimizer::FLATTEN_STATIC_TRANSFORMS);
404 
405  //Now save out the new collision mesh.
406  osgDB::writeNodeFile(*osgNewNode, strNewFile.c_str());
407 
408  //Make sure we stamp the new file tim on the file. For some reason osgDB is not settign that correctly.
409  //If we do not do this then the mesh mgr will not recognize that it has changed, and will not load it.
410  Std_SetFileTime(strNewFile);
411 }
412 
413 void BlSimulator::ConvertV1MeshFile(std::string strOriginalMeshFile, std::string strNewMeshFile, std::string strTexture)
414 {
415  //First load the original mesh in.
416  std::string strPath = this->ProjectPath();
417  std::string strOrigFile = AnimatSim::GetFilePath(strPath, strOriginalMeshFile);
418  std::string strNewFile = AnimatSim::GetFilePath(strPath, strNewMeshFile);
419  std::string strTextFile = "";
420 
421  if(!Std_IsBlank(strTexture))
422  strTextFile = AnimatSim::GetFilePath(strPath, strTexture);
423 
424  osg::ref_ptr<osg::Node> osgNode = osgDB::readNodeFile(strOrigFile.c_str());
425 
426  //Make sure the mesh loaded is valid.
427  if(!osgNode.valid())
428  THROW_PARAM_ERROR(Bl_Err_lErrorLoadingMesh, Bl_Err_strErrorLoadingMesh, "Original Mesh file", strOriginalMeshFile);
429 
430  CStdFPoint vPos(0, 0, 0), vRot( -(osg::PI/2), 0, 0);
431  ApplyVertexTransform(osgNode.get(), SetupMatrix(vPos, vRot));
432  AddNodeTexture(osgNode.get(), strTextFile, GL_TEXTURE_2D);
433 
434  //Now save out the new collision mesh.
435  osgDB::writeNodeFile(*osgNode, strNewFile.c_str());
436 }
437 
438 
439 void BlSimulator::GetPositionAndRotationFromD3DMatrix(float (&aryTransform)[4][4], CStdFPoint &vPos, CStdFPoint &vRot)
440 {
441  osg::Matrix osgMT(aryTransform[0][0], aryTransform[0][1], aryTransform[0][2], aryTransform[0][3],
442  aryTransform[1][0], aryTransform[1][1], aryTransform[1][2], aryTransform[1][3],
443  aryTransform[2][0], aryTransform[2][1], aryTransform[2][2], aryTransform[2][3],
444  aryTransform[3][0], aryTransform[3][1], aryTransform[3][2], aryTransform[3][3]);
445 
446  osg::Matrix osgFinal;
447 
448  osgFinal = osgMT;
449 
450  //Lets get the current world coordinates for this body part and then recalculate the
451  //new local position for the part and then finally reset its new local position.
452  osg::Vec3 vL = osgFinal.getTrans();
453  vPos.Set(vL.x(), vL.y(), vL.z());
454  vPos.ClearNearZero();
455 
456  vRot = EulerRotationFromMatrix(osgFinal);
457 }
458 
459 void BlSimulator::Initialize(int argc, const char **argv)
460 {
461  InitializeBullet(argc, argv);
462 
463  OsgSimulator::Initialize(argc, argv);
464 }
465 
466 void BlSimulator::StepSimulation()
467 {
469  //if(m_lTimeSlice > 10 && m_lTimeSlice < 5000 && !m_timePeriod.TimerStarted())
470  // m_timePeriod.StartTimer();
471 
472 
473  try
474  {
475  //step the frame and update the windows
476  if (!m_bPaused)
477  {
478  OsgSimulator::StepSimulation();
479 
480  unsigned long long lStart = GetTimerTick();
481 
482  if( m_bDrawDebug )
483  m_dbgDraw.BeginDraw();
484 
485  m_lpDynamicsWorld->stepSimulation(m_fltPhysicsTimeStep, m_iPhysicsSubsteps, m_fltPhysicsSubstepTime);
486 
487  if( m_bDrawDebug )
488  {
489  m_lpDynamicsWorld->debugDrawWorld();
490  m_dbgDraw.EndDraw();
491  }
492 
493  double dblVal = TimerDiff_s(lStart, GetTimerTick());
494  m_fltPhysicsStepTime += dblVal;
495 
496  if(m_lTimeSlice > 10 && m_lTimeSlice < 5000)
497  {
498  m_dblTotalVortexStepTime += dblVal;
499  m_lStepVortexTimeCount++;
500  }
501  else if(m_lTimeSlice == 5000)
502  {
503  double dblAvgStepTime = m_dblTotalVortexStepTime/m_lStepVortexTimeCount;
504  osg::notify(osg::NOTICE) << "Average step time: " << dblAvgStepTime << std::endl;
505  osg::notify(osg::NOTICE) << "Total vortex step time: " << m_dblTotalVortexStepTime << std::endl;
506  osg::notify(osg::NOTICE) << "Slice Time: " << m_lTimeSlice << std::endl;
507  osg::notify(osg::NOTICE) << "Sim Time: " << Time() << std::endl;
508  }
509  }
510 
511  //PauseSimulation();
512 
513  }
514  catch(CStdErrorInfo oError)
515  {
516  std::string strError = "An error occurred while step the simulation.\nError: " + oError.m_strError;
517  HandleNonCriticalError(strError);
518  }
519 
520 
521  //double dblVal2 = m_timeSimulationStep.StopTimer();
522  //if(m_lTimeSlice > 10 && m_lTimeSlice < 5000)
523  //{
524  // m_dblTotalStepTime += dblVal2;
525  // m_lStepTimeCount++;
526  //}
527  //else if(m_lTimeSlice == 5000)
528  //{
529  // double dblAvgStepTime = m_dblTotalStepTime/m_lStepTimeCount;
530  // cout << "Average step time: " << dblAvgStepTime << std::endl;
531  // cout << "Total step time: " << m_dblTotalStepTime << ", " << m_lStepTimeCount << std::endl;
532  // cout << "Period time: " << m_timePeriod.StopTimer() << std::endl;
533  // cout << "Slice Time: " << m_lTimeSlice << std::endl;
534  // cout << "Sim Time: " << Time() << std::endl;
535  //}
536 
537 }
538 
539 
540 void BlSimulator::SimulateEnd()
541 {
542  Reset();
543 }
544 
545 bool BlSimulator::HasFluidPlane(FluidPlane *lpPlane)
546 {
547  std::list<FluidPlane *>::iterator it;
548  for (it=m_aryFluidPlanes.begin(); it!=m_aryFluidPlanes.end(); ++it)
549  if(*it == lpPlane)
550  return true;
551 
552  return false;
553 }
554 
555 bool compare_fluid_planes (FluidPlane *first, FluidPlane *second)
556 {
557  if(first && second)
558  return (first->Height() < second->Height());
559 
560  return false;
561 }
562 
563 void BlSimulator::AddFluidPlane(FluidPlane *lpPlane)
564 {
565  if(!HasFluidPlane(lpPlane))
566  {
567  m_aryFluidPlanes.push_back(lpPlane);
568  m_aryFluidPlanes.sort(compare_fluid_planes);
569  }
570 }
571 
572 void BlSimulator::RemoveFluidPlane(FluidPlane *lpPlane)
573 {
574  if(HasFluidPlane(lpPlane))
575  {
576  m_aryFluidPlanes.remove(lpPlane);
577  m_aryFluidPlanes.sort(compare_fluid_planes);
578  }
579 }
580 
581 void BlSimulator::SortFluidPlanes()
582 {
583  m_aryFluidPlanes.sort(compare_fluid_planes);
584 }
585 
599 FluidPlane *BlSimulator::FindFluidPlaneForDepth(float fltDepth)
600 {
601  std::list<FluidPlane *>::iterator it;
602  for (it=m_aryFluidPlanes.begin(); it!=m_aryFluidPlanes.end(); ++it)
603  if(fltDepth <= (*it)->Height())
604  return (*it);
605 
606  return NULL;
607 }
608 
609 } //BulletAnimatSim
virtual void GenerateCollisionMeshFile(std::string strOriginalMeshFile, std::string strCollisionMeshFile, float fltScaleX, float fltScaleY, float fltScaleZ)
Generates a collision mesh file.
Classes for implementing the cm-labs vortex physics engine for AnimatLab.
virtual FluidPlane * FindFluidPlaneForDepth(float fltDepth)
Searches the fluid planes starting at the lowest height and working its way up to find the first one ...
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.
bool Std_IsBlank(std::string strVal)
Trims a string and tests if a string is blank.