NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
tgWorldBulletPhysicsImpl.cpp
Go to the documentation of this file.
1 /*
2  * Copyright © 2012, United States Government, as represented by the
3  * Administrator of the National Aeronautics and Space Administration.
4  * All rights reserved.
5  *
6  * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed
7  * under the Apache License, Version 2.0 (the "License");
8  * you may not use this file except in compliance with the License.
9  * You may obtain a copy of the License at
10  * http://www.apache.org/licenses/LICENSE-2.0.
11  *
12  * Unless required by applicable law or agreed to in writing,
13  * software distributed under the License is distributed on an
14  * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,
15  * either express or implied. See the License for the specific language
16  * governing permissions and limitations under the License.
17  */
18 
19 
27 // This module
29 // This application
30 #include "tgWorld.h"
31 #include "tgCast.h"
32 #include "terrain/tgBulletGround.h"
33 #include "terrain/tgEmptyGround.h"
34 // The Bullet Physics library
35 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
36 #include "BulletCollision/BroadphaseCollision/btDbvtBroadphase.h"
37 #include "BulletCollision/BroadphaseCollision/btAxisSweep3.h" // New broadphase
38 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
39 #include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h"
40 #include "BulletCollision/CollisionShapes/btBoxShape.h"
41 #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
42 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
43 #include "BulletDynamics/Dynamics/btRigidBody.h"
44 #include "BulletDynamics/Dynamics/btDynamicsWorld.h"
45 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
46 #include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
47 #include "BulletSoftBody/btSoftRigidDynamicsWorld.h"
48 #include "LinearMath/btDefaultMotionState.h"
49 #include "LinearMath/btScalar.h"
50 #include "LinearMath/btTransform.h"
51 #include "LinearMath/btVector3.h"
52 #include "LinearMath/btQuickprof.h"
53 
54 // Ghost objects
55 #include "BulletCollision/CollisionDispatch/btGhostObject.h"
56 #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
57 
58 #define MLCP_SOLVER
59 
60 #ifdef MLCP_SOLVER
61 
62 #include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
63 #include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
64 #include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
65 
66 #endif //MLCP_SOLVER
67 
73 {
74  public:
75  IntermediateBuildProducts(double worldSize) :
76  corner1 (-worldSize,-worldSize, -worldSize),
77  corner2 (worldSize, worldSize, worldSize),
78  dispatcher(&collisionConfiguration),
79  ghostCallback(),
80 #ifndef MLCP_SOLVER
81  #if (1) // More acc broadphase - remeber the comma (consider doing ifndef)
82  broadphase(corner1, corner2, 16384)
83  #endif // Broadphase
84 #else
85  broadphase(corner1, corner2, 16384),
86  solver(&mlcp)
87 #endif //MLCP_SOLVER
88 
89  {
90  broadphase.getOverlappingPairCache()->setInternalGhostPairCallback(&ghostCallback);
91  }
92  const btVector3 corner1;
93  const btVector3 corner2;
94  btSoftBodyRigidBodyCollisionConfiguration collisionConfiguration;
95  btCollisionDispatcher dispatcher;
96  btGhostPairCallback ghostCallback;
97 #if (0) // Default broadphase
98  btDbvtBroadphase broadphase;
99 #else
100  // More accurate broadphase:
101  btAxisSweep3 broadphase;
102 #endif
103 
104 #ifdef MLCP_SOLVER
105  btDantzigSolver mlcp;
106  //btSolveProjectedGaussSeidel mlcp;
107  btMLCPSolver solver;
108 #else
109  btSequentialImpulseConstraintSolver solver;
110 #endif
111 
112 };
113 
115  tgBulletGround* ground) :
116  tgWorldImpl(config, ground),
117  m_pIntermediateBuildProducts(new IntermediateBuildProducts(config.worldSize)),
118  m_pDynamicsWorld(createDynamicsWorld())
119 {
120 
121  // Gravitational acceleration is down on the Y axis
122  const btVector3 gravityVector(0, -config.gravity, 0);
123  m_pDynamicsWorld->setGravity(gravityVector);
124 
125  if (!tgCast::cast<tgBulletGround, tgEmptyGround>(ground) && ground != NULL)
126  {
127  m_pDynamicsWorld->addRigidBody(ground->getGroundRigidBody());
128  }
129 
130  /*
131  * These are lines from the old BasicLearningApp.cpp that we aren't using.
132  * http://bulletphysics.org/mediawiki-1.5.8/index.php/BtContactSolverInfo
133  */
134  #if (0)
135  // Split impulse is on by default
136  m_pDynamicsWorld->getSolverInfo().m_splitImpulse = true;
137  m_pDynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold = -0.02;
138 
139  // Default is 10 - increases runtime but decreases odds of penetration
140  // Makes tetraspine sine waves more accurate and static test less accurate
141  m_pDynamicsWorld->getSolverInfo().m_numIterations = 20;
142 
143  // Ground contact params:
144  m_pDynamicsWorld->getSolverInfo().m_erp = 0.8;
145 
146  #endif
147 
148 
149 
150  // Postcondition
151  assert(invariant());
152 }
153 
155 {
156  // Delete all the collision objects. The dynamics world must exist.
157  // Delete in reverse order of creation.
158  const size_t nco = m_pDynamicsWorld->getNumCollisionObjects();
159  btCollisionObjectArray& oa = m_pDynamicsWorld->getCollisionObjectArray();
160  for (int i = nco - 1; i >= 0; --i)
161  {
162  btCollisionObject * const pCollisionObject = oa[i];
163 
164  // If the collision object is a rigid body, delete its motion state
165  const btRigidBody* const pRigidBody =
166  btRigidBody::upcast(pCollisionObject);
167  if (pRigidBody)
168  {
169  delete pRigidBody->getMotionState();
170  }
171 
172  // Remove the collision object from the dynamics world
173  m_pDynamicsWorld->removeCollisionObject(pCollisionObject);
174  // Delete the collision object
175  delete pCollisionObject;
176  }
177  // All collision objects have been removed and deleted
178  assert(m_pDynamicsWorld->getNumCollisionObjects() == 0);
179 
180  // Delete all the collision shapes. This can be done at any time.
181  const size_t ncs = m_collisionShapes.size();
182 
183  for (size_t i = 0; i < ncs; ++i) { delete m_collisionShapes[i]; }
184 
185  delete m_pDynamicsWorld;
186 
187  // Delete the intermediate build products, which are now orphaned
188  delete m_pIntermediateBuildProducts;
189 }
190 
195 btDynamicsWorld* tgWorldBulletPhysicsImpl::createDynamicsWorld() const
196 {
197 
198  btSoftRigidDynamicsWorld* const result =
199  new btSoftRigidDynamicsWorld(&m_pIntermediateBuildProducts->dispatcher,
200  &m_pIntermediateBuildProducts->broadphase,
201  &m_pIntermediateBuildProducts->solver,
202  &m_pIntermediateBuildProducts->collisionConfiguration);
203 #ifdef MLCPSOLVER
204  result ->getSolverInfo().m_minimumSolverBatchSize = 1;//for direct solver it is better to have a small A matrix
205 #endif
206  return result;
207 }
208 
210 {
211  // Precondition
212  assert(dt > 0.0);
213 
214  const btScalar timeStep = dt;
215  const int maxSubSteps = 1;
216  const btScalar fixedTimeStep = dt;
217  m_pDynamicsWorld->stepSimulation(timeStep, maxSubSteps, fixedTimeStep);
218 
219  // Postcondition
220  assert(invariant());
221 }
222 
223 void tgWorldBulletPhysicsImpl::addCollisionShape(btCollisionShape* pShape)
224 {
225 #ifndef BT_NO_PROFILE
226  BT_PROFILE("addCollisionShape");
227 #endif //BT_NO_PROFILE
228 
229  if (pShape)
230  {
231  m_collisionShapes.push_back(pShape);
232  }
233 
234  // Postcondition
235  assert(invariant());
236 }
237 
239 {
240 #ifndef BT_NO_PROFILE
241  BT_PROFILE("deleteCollisionShape");
242 #endif //BT_NO_PROFILE
243 
244  if (pShape)
245  {
246  btCompoundShape* cShape = tgCast::cast<btCollisionShape, btCompoundShape>(pShape);
247  if (cShape)
248  {
249  std::size_t n = cShape->getNumChildShapes();
250  for( std::size_t i = 0; i < n; i++)
251  {
252  deleteCollisionShape(cShape->getChildShape(i));
253  }
254  }
255  m_collisionShapes.remove(pShape);
256  delete pShape;
257  }
258 
259  // Postcondition
260  assert(invariant());
261 }
262 
263 bool tgWorldBulletPhysicsImpl::invariant() const
264 {
265  return (m_pDynamicsWorld != 0);
266 }
267 
Contains the definition of class tgBulletGround.
Contains the definition of class tgWorldBulletPhysicsImpl.
Utility class for class casting and filtering collections by type.
tgWorldBulletPhysicsImpl(const tgWorld::Config &config, tgBulletGround *ground)
double gravity
Definition: tgWorld.h:53
virtual btRigidBody * getGroundRigidBody() const =0
Contains the definition of class tgWorld $Id$.
void addCollisionShape(btCollisionShape *pShape)
void deleteCollisionShape(btCollisionShape *pShape)
Contains the definition of class tgEmptyGround.