2 Bullet Continuous Collision Detection and Physics Library Copyright (c) 2007 Erwin Coumans
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
17 #include "btBulletDynamicsCommon.h"
18 #include "GlutStuff.h"
19 #include "GL_ShapeDrawer.h"
21 #include "LinearMath/btIDebugDraw.h"
23 #include "GLDebugDrawer.h"
24 #include "MotorDemo.h"
28 #define M_PI 3.14159265358979323846
32 #define M_PI_2 1.57079632679489661923
36 #define M_PI_4 0.785398163397448309616
40 #define M_PI_8 0.5 * M_PI_4
46 void vertex(btVector3 &v)
48 glVertex3d(v.getX(), v.getY(), v.getZ());
51 void drawFrame(btTransform &tr)
53 const float fSize = 1.f;
59 btVector3 vX = tr*btVector3(fSize,0,0);
60 vertex(tr.getOrigin()); vertex(vX);
64 btVector3 vY = tr*btVector3(0,fSize,0);
65 vertex(tr.getOrigin()); vertex(vY);
69 btVector3 vZ = tr*btVector3(0,0,fSize);
70 vertex(tr.getOrigin()); vertex(vZ);
80 #define BODYPART_COUNT 2 * NUM_LEGS + 1
81 #define JOINT_COUNT BODYPART_COUNT - 1
85 btDynamicsWorld* m_ownerWorld;
86 btCollisionShape* m_shapes[BODYPART_COUNT];
87 btRigidBody* m_bodies[BODYPART_COUNT];
88 btTypedConstraint* m_joints[JOINT_COUNT];
90 btRigidBody* localCreateRigidBody (btScalar mass, const btTransform& startTransform, btCollisionShape* shape)
92 bool isDynamic = (mass != 0.f);
94 btVector3 localInertia(0,0,0);
96 shape->calculateLocalInertia(mass,localInertia);
98 btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
99 btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia);
100 btRigidBody* body = new btRigidBody(rbInfo);
102 m_ownerWorld->addRigidBody(body);
109 TestRig (btDynamicsWorld* ownerWorld, const btVector3& positionOffset, bool bFixed)
110 : m_ownerWorld (ownerWorld)
112 btVector3 vUp(0, 1, 0);
117 float fBodySize = 0.25f;
118 float fLegLength = 0.45f;
119 float fForeLegLength = 0.75f;
120 m_shapes[0] = new btCapsuleShape(btScalar(fBodySize), btScalar(0.10));
122 for ( i=0; i<NUM_LEGS; i++)
124 m_shapes[1 + 2*i] = new btCapsuleShape(btScalar(0.10), btScalar(fLegLength));
125 m_shapes[2 + 2*i] = new btCapsuleShape(btScalar(0.08), btScalar(fForeLegLength));
129 // Setup rigid bodies
132 btTransform offset; offset.setIdentity();
133 offset.setOrigin(positionOffset);
136 btVector3 vRoot = btVector3(btScalar(0.), btScalar(fHeight), btScalar(0.));
137 btTransform transform;
138 transform.setIdentity();
139 transform.setOrigin(vRoot);
142 m_bodies[0] = localCreateRigidBody(btScalar(0.), offset*transform, m_shapes[0]);
145 m_bodies[0] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[0]);
148 for ( i=0; i<NUM_LEGS; i++)
150 float fAngle = 2 * M_PI * i / NUM_LEGS;
151 float fSin = sin(fAngle);
152 float fCos = cos(fAngle);
154 transform.setIdentity();
155 btVector3 vBoneOrigin = btVector3(btScalar(fCos*(fBodySize+0.5*fLegLength)), btScalar(fHeight), btScalar(fSin*(fBodySize+0.5*fLegLength)));
156 transform.setOrigin(vBoneOrigin);
159 btVector3 vToBone = (vBoneOrigin - vRoot).normalize();
160 btVector3 vAxis = vToBone.cross(vUp);
161 transform.setRotation(btQuaternion(vAxis, M_PI_2));
162 m_bodies[1+2*i] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[1+2*i]);
165 transform.setIdentity();
166 transform.setOrigin(btVector3(btScalar(fCos*(fBodySize+fLegLength)), btScalar(fHeight-0.5*fForeLegLength), btScalar(fSin*(fBodySize+fLegLength))));
167 m_bodies[2+2*i] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[2+2*i]);
170 // Setup some damping on the m_bodies
171 for (i = 0; i < BODYPART_COUNT; ++i)
173 m_bodies[i]->setDamping(0.05, 0.85);
174 m_bodies[i]->setDeactivationTime(0.8);
175 //m_bodies[i]->setSleepingThresholds(1.6, 2.5);
176 m_bodies[i]->setSleepingThresholds(0.5f, 0.5f);
181 // Setup the constraints
183 btHingeConstraint* hingeC;
184 //btConeTwistConstraint* coneC;
186 btTransform localA, localB, localC;
188 for ( i=0; i<NUM_LEGS; i++)
190 float fAngle = 2 * M_PI * i / NUM_LEGS;
191 float fSin = sin(fAngle);
192 float fCos = cos(fAngle);
195 localA.setIdentity(); localB.setIdentity();
196 localA.getBasis().setEulerZYX(0,-fAngle,0); localA.setOrigin(btVector3(btScalar(fCos*fBodySize), btScalar(0.), btScalar(fSin*fBodySize)));
197 localB = m_bodies[1+2*i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA;
198 hingeC = new btHingeConstraint(*m_bodies[0], *m_bodies[1+2*i], localA, localB);
199 hingeC->setLimit(btScalar(-0.75 * M_PI_4), btScalar(M_PI_8));
200 //hingeC->setLimit(btScalar(-0.1), btScalar(0.1));
201 m_joints[2*i] = hingeC;
202 m_ownerWorld->addConstraint(m_joints[2*i], true);
205 localA.setIdentity(); localB.setIdentity(); localC.setIdentity();
206 localA.getBasis().setEulerZYX(0,-fAngle,0); localA.setOrigin(btVector3(btScalar(fCos*(fBodySize+fLegLength)), btScalar(0.), btScalar(fSin*(fBodySize+fLegLength))));
207 localB = m_bodies[1+2*i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA;
208 localC = m_bodies[2+2*i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA;
209 hingeC = new btHingeConstraint(*m_bodies[1+2*i], *m_bodies[2+2*i], localB, localC);
210 //hingeC->setLimit(btScalar(-0.01), btScalar(0.01));
211 hingeC->setLimit(btScalar(-M_PI_8), btScalar(0.2));
212 m_joints[1+2*i] = hingeC;
213 m_ownerWorld->addConstraint(m_joints[1+2*i], true);
221 // Remove all constraints
222 for ( i = 0; i < JOINT_COUNT; ++i)
224 m_ownerWorld->removeConstraint(m_joints[i]);
225 delete m_joints[i]; m_joints[i] = 0;
228 // Remove all bodies and shapes
229 for ( i = 0; i < BODYPART_COUNT; ++i)
231 m_ownerWorld->removeRigidBody(m_bodies[i]);
233 delete m_bodies[i]->getMotionState();
235 delete m_bodies[i]; m_bodies[i] = 0;
236 delete m_shapes[i]; m_shapes[i] = 0;
240 btTypedConstraint** GetJoints() {return &m_joints[0];}
246 void motorPreTickCallback (btDynamicsWorld *world, btScalar timeStep)
248 MotorDemo* motorDemo = (MotorDemo*)world->getWorldUserInfo();
250 motorDemo->setMotorTargets(timeStep);
256 void MotorDemo::initPhysics()
261 // Setup the basic world
264 m_fCyclePeriod = 2000.f; // in milliseconds
266 // m_fMuscleStrength = 0.05f;
267 // new SIMD solver for joints clips accumulated impulse, so the new limits for the motor
268 // should be (numberOfsolverIterations * oldLimits)
269 // currently solver uses 10 iterations, so:
270 m_fMuscleStrength = 0.5f;
272 setCameraDistance(btScalar(5.));
274 m_collisionConfiguration = new btDefaultCollisionConfiguration();
276 m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
278 btVector3 worldAabbMin(-10000,-10000,-10000);
279 btVector3 worldAabbMax(10000,10000,10000);
280 m_broadphase = new btAxisSweep3 (worldAabbMin, worldAabbMax);
282 m_solver = new btSequentialImpulseConstraintSolver;
284 m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
286 m_dynamicsWorld->setInternalTickCallback(motorPreTickCallback,this,true);
289 // Setup a big ground box
291 btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(200.),btScalar(10.),btScalar(200.)));
292 m_collisionShapes.push_back(groundShape);
293 btTransform groundTransform;
294 groundTransform.setIdentity();
295 groundTransform.setOrigin(btVector3(0,-10,0));
296 localCreateRigidBody(btScalar(0.),groundTransform,groundShape);
300 btVector3 startOffset(1,0.5,0);
301 spawnTestRig(startOffset, false);
302 startOffset.setValue(-2,0.5,0);
303 spawnTestRig(startOffset, true);
309 void MotorDemo::spawnTestRig(const btVector3& startOffset, bool bFixed)
311 TestRig* rig = new TestRig(m_dynamicsWorld, startOffset, bFixed);
312 m_rigs.push_back(rig);
323 void MotorDemo::setMotorTargets(btScalar deltaTime)
326 float ms = deltaTime*1000000.;
327 float minFPS = 1000000.f/60.f;
334 // set per-frame sinusoidal position targets using angular motor (hacky?)
336 for (int r=0; r<m_rigs.size(); r++)
338 for (int i=0; i<2*NUM_LEGS; i++)
340 btHingeConstraint* hingeC = static_cast<btHingeConstraint*>(m_rigs[r]->GetJoints()[i]);
341 btScalar fCurAngle = hingeC->getHingeAngle();
343 btScalar fTargetPercent = (int(m_Time / 1000) % int(m_fCyclePeriod)) / m_fCyclePeriod;
344 btScalar fTargetAngle = 0.5 * (1 + sin(2 * M_PI * fTargetPercent));
345 btScalar fTargetLimitAngle = hingeC->getLowerLimit() + fTargetAngle * (hingeC->getUpperLimit() - hingeC->getLowerLimit());
346 btScalar fAngleError = fTargetLimitAngle - fCurAngle;
347 btScalar fDesiredAngularVel = 1000000.f * fAngleError/ms;
348 hingeC->enableAngularMotor(true, fDesiredAngularVel, m_fMuscleStrength);
355 void MotorDemo::clientMoveAndDisplay()
357 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
359 //simple dynamics world doesn't handle fixed-time-stepping
360 float deltaTime = getDeltaTimeMicroseconds()/1000000.f;
365 m_dynamicsWorld->stepSimulation(deltaTime);
366 m_dynamicsWorld->debugDrawWorld();
371 for (int i=2; i>=0 ;i--)
373 btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
374 btRigidBody* body = btRigidBody::upcast(obj);
375 drawFrame(body->getWorldTransform());
383 void MotorDemo::displayCallback()
385 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
388 m_dynamicsWorld->debugDrawWorld();
396 void MotorDemo::keyboardCallback(unsigned char key, int x, int y)
401 m_fCyclePeriod /= 1.1f;
402 if (m_fCyclePeriod < 1.f)
403 m_fCyclePeriod = 1.f;
406 m_fCyclePeriod *= 1.1f;
409 m_fMuscleStrength /= 1.1f;
412 m_fMuscleStrength *= 1.1f;
415 DemoApplication::keyboardCallback(key, x, y);
421 void MotorDemo::exitPhysics()
426 for (i=0;i<m_rigs.size();i++)
428 TestRig* rig = m_rigs[i];
432 //cleanup in the reverse order of creation/initialization
434 //remove the rigidbodies from the dynamics world and delete them
436 for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
438 btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
439 btRigidBody* body = btRigidBody::upcast(obj);
440 if (body && body->getMotionState())
442 delete body->getMotionState();
444 m_dynamicsWorld->removeCollisionObject( obj );
448 //delete collision shapes
449 for (int j=0;j<m_collisionShapes.size();j++)
451 btCollisionShape* shape = m_collisionShapes[j];
455 //delete dynamics world
456 delete m_dynamicsWorld;
467 delete m_collisionConfiguration;