1 /*
2 Bullet Continuous Collision Detection and Physics Library Copyright (c) 2007 Erwin Coumans
3 Motor Demo
4
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:
10
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.
14 */
15
16 #include "btBulletDynamicsCommon.h"
17
18 #include "LinearMath/btIDebugDraw.h"
19 #include "MotorDemo.h"
20
21 #include <cmath>
22
23 #include "LinearMath/btAlignedObjectArray.h"
24 class btBroadphaseInterface;
25 class btCollisionShape;
26 class btOverlappingPairCache;
27 class btCollisionDispatcher;
28 class btConstraintSolver;
29 struct btCollisionAlgorithmCreateFunc;
30 class btDefaultCollisionConfiguration;
31
32 #include "../CommonInterfaces/CommonRigidBodyBase.h"
33
34 class MotorDemo : public CommonRigidBodyBase
35 {
36 float m_Time;
37 float m_fCyclePeriod; // in milliseconds
38 float m_fMuscleStrength;
39
40 btAlignedObjectArray<class TestRig*> m_rigs;
41
42 public:
MotorDemo(struct GUIHelperInterface * helper)43 MotorDemo(struct GUIHelperInterface* helper)
44 : CommonRigidBodyBase(helper)
45 {
46 }
47
48 void initPhysics();
49
50 void exitPhysics();
51
~MotorDemo()52 virtual ~MotorDemo()
53 {
54 }
55
56 void spawnTestRig(const btVector3& startOffset, bool bFixed);
57
58 // virtual void keyboardCallback(unsigned char key, int x, int y);
59
60 void setMotorTargets(btScalar deltaTime);
61
resetCamera()62 void resetCamera()
63 {
64 float dist = 11;
65 float pitch = -35;
66 float yaw = 52;
67 float targetPos[3] = {0, 0.46, 0};
68 m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
69 }
70 };
71
72 #ifndef M_PI
73 #define M_PI 3.14159265358979323846
74 #endif
75
76 #ifndef M_PI_2
77 #define M_PI_2 1.57079632679489661923
78 #endif
79
80 #ifndef M_PI_4
81 #define M_PI_4 0.785398163397448309616
82 #endif
83
84 #ifndef M_PI_8
85 #define M_PI_8 0.5 * M_PI_4
86 #endif
87
88 // /LOCAL FUNCTIONS
89
90 #define NUM_LEGS 6
91 #define BODYPART_COUNT 2 * NUM_LEGS + 1
92 #define JOINT_COUNT BODYPART_COUNT - 1
93
94 class TestRig
95 {
96 btDynamicsWorld* m_ownerWorld;
97 btCollisionShape* m_shapes[BODYPART_COUNT];
98 btRigidBody* m_bodies[BODYPART_COUNT];
99 btTypedConstraint* m_joints[JOINT_COUNT];
100
localCreateRigidBody(btScalar mass,const btTransform & startTransform,btCollisionShape * shape)101 btRigidBody* localCreateRigidBody(btScalar mass, const btTransform& startTransform, btCollisionShape* shape)
102 {
103 bool isDynamic = (mass != 0.f);
104
105 btVector3 localInertia(0, 0, 0);
106 if (isDynamic)
107 shape->calculateLocalInertia(mass, localInertia);
108
109 btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
110 btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, shape, localInertia);
111 btRigidBody* body = new btRigidBody(rbInfo);
112
113 m_ownerWorld->addRigidBody(body);
114
115 return body;
116 }
117
118 public:
TestRig(btDynamicsWorld * ownerWorld,const btVector3 & positionOffset,bool bFixed)119 TestRig(btDynamicsWorld* ownerWorld, const btVector3& positionOffset, bool bFixed)
120 : m_ownerWorld(ownerWorld)
121 {
122 btVector3 vUp(0, 1, 0);
123
124 //
125 // Setup geometry
126 //
127 float fBodySize = 0.25f;
128 float fLegLength = 0.45f;
129 float fForeLegLength = 0.75f;
130 m_shapes[0] = new btCapsuleShape(btScalar(fBodySize), btScalar(0.10));
131 int i;
132 for (i = 0; i < NUM_LEGS; i++)
133 {
134 m_shapes[1 + 2 * i] = new btCapsuleShape(btScalar(0.10), btScalar(fLegLength));
135 m_shapes[2 + 2 * i] = new btCapsuleShape(btScalar(0.08), btScalar(fForeLegLength));
136 }
137
138 //
139 // Setup rigid bodies
140 //
141 float fHeight = 0.5;
142 btTransform offset;
143 offset.setIdentity();
144 offset.setOrigin(positionOffset);
145
146 // root
147 btVector3 vRoot = btVector3(btScalar(0.), btScalar(fHeight), btScalar(0.));
148 btTransform transform;
149 transform.setIdentity();
150 transform.setOrigin(vRoot);
151 if (bFixed)
152 {
153 m_bodies[0] = localCreateRigidBody(btScalar(0.), offset * transform, m_shapes[0]);
154 }
155 else
156 {
157 m_bodies[0] = localCreateRigidBody(btScalar(1.), offset * transform, m_shapes[0]);
158 }
159 // legs
160 for (i = 0; i < NUM_LEGS; i++)
161 {
162 float fAngle = 2 * M_PI * i / NUM_LEGS;
163 float fSin = std::sin(fAngle);
164 float fCos = std::cos(fAngle);
165
166 transform.setIdentity();
167 btVector3 vBoneOrigin = btVector3(btScalar(fCos * (fBodySize + 0.5 * fLegLength)), btScalar(fHeight), btScalar(fSin * (fBodySize + 0.5 * fLegLength)));
168 transform.setOrigin(vBoneOrigin);
169
170 // thigh
171 btVector3 vToBone = (vBoneOrigin - vRoot).normalize();
172 btVector3 vAxis = vToBone.cross(vUp);
173 transform.setRotation(btQuaternion(vAxis, M_PI_2));
174 m_bodies[1 + 2 * i] = localCreateRigidBody(btScalar(1.), offset * transform, m_shapes[1 + 2 * i]);
175
176 // shin
177 transform.setIdentity();
178 transform.setOrigin(btVector3(btScalar(fCos * (fBodySize + fLegLength)), btScalar(fHeight - 0.5 * fForeLegLength), btScalar(fSin * (fBodySize + fLegLength))));
179 m_bodies[2 + 2 * i] = localCreateRigidBody(btScalar(1.), offset * transform, m_shapes[2 + 2 * i]);
180 }
181
182 // Setup some damping on the m_bodies
183 for (i = 0; i < BODYPART_COUNT; ++i)
184 {
185 m_bodies[i]->setDamping(0.05, 0.85);
186 m_bodies[i]->setDeactivationTime(0.8);
187 //m_bodies[i]->setSleepingThresholds(1.6, 2.5);
188 m_bodies[i]->setSleepingThresholds(0.5f, 0.5f);
189 }
190
191 //
192 // Setup the constraints
193 //
194 btHingeConstraint* hingeC;
195 //btConeTwistConstraint* coneC;
196
197 btTransform localA, localB, localC;
198
199 for (i = 0; i < NUM_LEGS; i++)
200 {
201 float fAngle = 2 * M_PI * i / NUM_LEGS;
202 float fSin = std::sin(fAngle);
203 float fCos = std::cos(fAngle);
204
205 // hip joints
206 localA.setIdentity();
207 localB.setIdentity();
208 localA.getBasis().setEulerZYX(0, -fAngle, 0);
209 localA.setOrigin(btVector3(btScalar(fCos * fBodySize), btScalar(0.), btScalar(fSin * fBodySize)));
210 localB = m_bodies[1 + 2 * i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA;
211 hingeC = new btHingeConstraint(*m_bodies[0], *m_bodies[1 + 2 * i], localA, localB);
212 hingeC->setLimit(btScalar(-0.75 * M_PI_4), btScalar(M_PI_8));
213 //hingeC->setLimit(btScalar(-0.1), btScalar(0.1));
214 m_joints[2 * i] = hingeC;
215 m_ownerWorld->addConstraint(m_joints[2 * i], true);
216
217 // knee joints
218 localA.setIdentity();
219 localB.setIdentity();
220 localC.setIdentity();
221 localA.getBasis().setEulerZYX(0, -fAngle, 0);
222 localA.setOrigin(btVector3(btScalar(fCos * (fBodySize + fLegLength)), btScalar(0.), btScalar(fSin * (fBodySize + fLegLength))));
223 localB = m_bodies[1 + 2 * i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA;
224 localC = m_bodies[2 + 2 * i]->getWorldTransform().inverse() * m_bodies[0]->getWorldTransform() * localA;
225 hingeC = new btHingeConstraint(*m_bodies[1 + 2 * i], *m_bodies[2 + 2 * i], localB, localC);
226 //hingeC->setLimit(btScalar(-0.01), btScalar(0.01));
227 hingeC->setLimit(btScalar(-M_PI_8), btScalar(0.2));
228 m_joints[1 + 2 * i] = hingeC;
229 m_ownerWorld->addConstraint(m_joints[1 + 2 * i], true);
230 }
231 }
232
~TestRig()233 virtual ~TestRig()
234 {
235 int i;
236
237 // Remove all constraints
238 for (i = 0; i < JOINT_COUNT; ++i)
239 {
240 m_ownerWorld->removeConstraint(m_joints[i]);
241 delete m_joints[i];
242 m_joints[i] = 0;
243 }
244
245 // Remove all bodies and shapes
246 for (i = 0; i < BODYPART_COUNT; ++i)
247 {
248 m_ownerWorld->removeRigidBody(m_bodies[i]);
249
250 delete m_bodies[i]->getMotionState();
251
252 delete m_bodies[i];
253 m_bodies[i] = 0;
254 delete m_shapes[i];
255 m_shapes[i] = 0;
256 }
257 }
258
GetJoints()259 btTypedConstraint** GetJoints() { return &m_joints[0]; }
260 };
261
motorPreTickCallback(btDynamicsWorld * world,btScalar timeStep)262 void motorPreTickCallback(btDynamicsWorld* world, btScalar timeStep)
263 {
264 MotorDemo* motorDemo = (MotorDemo*)world->getWorldUserInfo();
265
266 motorDemo->setMotorTargets(timeStep);
267 }
268
initPhysics()269 void MotorDemo::initPhysics()
270 {
271 m_guiHelper->setUpAxis(1);
272
273 // Setup the basic world
274
275 m_Time = 0;
276 m_fCyclePeriod = 2000.f; // in milliseconds
277
278 // m_fMuscleStrength = 0.05f;
279 // new SIMD solver for joints clips accumulated impulse, so the new limits for the motor
280 // should be (numberOfsolverIterations * oldLimits)
281 // currently solver uses 10 iterations, so:
282 m_fMuscleStrength = 0.5f;
283
284 m_collisionConfiguration = new btDefaultCollisionConfiguration();
285
286 m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
287
288 btVector3 worldAabbMin(-10000, -10000, -10000);
289 btVector3 worldAabbMax(10000, 10000, 10000);
290 m_broadphase = new btAxisSweep3(worldAabbMin, worldAabbMax);
291
292 m_solver = new btSequentialImpulseConstraintSolver;
293
294 m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
295
296 m_dynamicsWorld->setInternalTickCallback(motorPreTickCallback, this, true);
297 m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
298
299 // Setup a big ground box
300 {
301 btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(200.), btScalar(10.), btScalar(200.)));
302 m_collisionShapes.push_back(groundShape);
303 btTransform groundTransform;
304 groundTransform.setIdentity();
305 groundTransform.setOrigin(btVector3(0, -10, 0));
306 createRigidBody(btScalar(0.), groundTransform, groundShape);
307 }
308
309 // Spawn one ragdoll
310 btVector3 startOffset(1, 0.5, 0);
311 spawnTestRig(startOffset, false);
312 startOffset.setValue(-2, 0.5, 0);
313 spawnTestRig(startOffset, true);
314
315 m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
316 }
317
spawnTestRig(const btVector3 & startOffset,bool bFixed)318 void MotorDemo::spawnTestRig(const btVector3& startOffset, bool bFixed)
319 {
320 TestRig* rig = new TestRig(m_dynamicsWorld, startOffset, bFixed);
321 m_rigs.push_back(rig);
322 }
323
PreStep()324 void PreStep()
325 {
326 }
327
setMotorTargets(btScalar deltaTime)328 void MotorDemo::setMotorTargets(btScalar deltaTime)
329 {
330 float ms = deltaTime * 1000000.;
331 float minFPS = 1000000.f / 60.f;
332 if (ms > minFPS)
333 ms = minFPS;
334
335 m_Time += ms;
336
337 //
338 // set per-frame sinusoidal position targets using angular motor (hacky?)
339 //
340 for (int r = 0; r < m_rigs.size(); r++)
341 {
342 for (int i = 0; i < 2 * NUM_LEGS; i++)
343 {
344 btHingeConstraint* hingeC = static_cast<btHingeConstraint*>(m_rigs[r]->GetJoints()[i]);
345 btScalar fCurAngle = hingeC->getHingeAngle();
346
347 btScalar fTargetPercent = (int(m_Time / 1000) % int(m_fCyclePeriod)) / m_fCyclePeriod;
348 btScalar fTargetAngle = 0.5 * (1 + sin(2 * M_PI * fTargetPercent));
349 btScalar fTargetLimitAngle = hingeC->getLowerLimit() + fTargetAngle * (hingeC->getUpperLimit() - hingeC->getLowerLimit());
350 btScalar fAngleError = fTargetLimitAngle - fCurAngle;
351 btScalar fDesiredAngularVel = 1000000.f * fAngleError / ms;
352 hingeC->enableAngularMotor(true, fDesiredAngularVel, m_fMuscleStrength);
353 }
354 }
355 }
356
357 #if 0
358 void MotorDemo::keyboardCallback(unsigned char key, int x, int y)
359 {
360 switch (key)
361 {
362 case '+': case '=':
363 m_fCyclePeriod /= 1.1f;
364 if (m_fCyclePeriod < 1.f)
365 m_fCyclePeriod = 1.f;
366 break;
367 case '-': case '_':
368 m_fCyclePeriod *= 1.1f;
369 break;
370 case '[':
371 m_fMuscleStrength /= 1.1f;
372 break;
373 case ']':
374 m_fMuscleStrength *= 1.1f;
375 break;
376 default:
377 DemoApplication::keyboardCallback(key, x, y);
378 }
379 }
380 #endif
381
exitPhysics()382 void MotorDemo::exitPhysics()
383 {
384 int i;
385
386 for (i = 0; i < m_rigs.size(); i++)
387 {
388 TestRig* rig = m_rigs[i];
389 delete rig;
390 }
391
392 //cleanup in the reverse order of creation/initialization
393
394 //remove the rigidbodies from the dynamics world and delete them
395
396 for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
397 {
398 btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
399 btRigidBody* body = btRigidBody::upcast(obj);
400 if (body && body->getMotionState())
401 {
402 delete body->getMotionState();
403 }
404 m_dynamicsWorld->removeCollisionObject(obj);
405 delete obj;
406 }
407
408 //delete collision shapes
409 for (int j = 0; j < m_collisionShapes.size(); j++)
410 {
411 btCollisionShape* shape = m_collisionShapes[j];
412 delete shape;
413 }
414
415 //delete dynamics world
416 delete m_dynamicsWorld;
417
418 //delete solver
419 delete m_solver;
420
421 //delete broadphase
422 delete m_broadphase;
423
424 //delete dispatcher
425 delete m_dispatcher;
426
427 delete m_collisionConfiguration;
428 }
429
MotorControlCreateFunc(struct CommonExampleOptions & options)430 class CommonExampleInterface* MotorControlCreateFunc(struct CommonExampleOptions& options)
431 {
432 return new MotorDemo(options.m_guiHelper);
433 }
434