Demos/DynamicControlDemo/MotorDemo.cpp

   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
  17 #include "btBulletDynamicsCommon.h"
  18 #include "GlutStuff.h"
  19 #include "GL_ShapeDrawer.h"
  20
  21 #include "LinearMath/btIDebugDraw.h"
  22
  23 #include "GLDebugDrawer.h"
  24 #include "MotorDemo.h"
  25
  26
  27 #ifndef M_PI
  28 #define M_PI       3.14159265358979323846
  29 #endif
  30
  31 #ifndef M_PI_2
  32 #define M_PI_2     1.57079632679489661923
  33 #endif
  34
  35 #ifndef M_PI_4
  36 #define M_PI_4     0.785398163397448309616
  37 #endif
  38
  39 #ifndef M_PI_8
  40 #define M_PI_8     0.5 * M_PI_4
  41 #endif
  42
  43
  44 // LOCAL FUNCTIONS
  45
  46 void vertex(btVector3 &v)
  47 {
  48         glVertex3d(v.getX(), v.getY(), v.getZ());
  49 }
  50
  51 void drawFrame(btTransform &tr)
  52 {
  53         const float fSize = 1.f;
  54
  55         glBegin(GL_LINES);
  56
  57         // x
  58         glColor3f(255.f,0,0);
  59         btVector3 vX = tr*btVector3(fSize,0,0);
  60         vertex(tr.getOrigin()); vertex(vX);
  61
  62         // y
  63         glColor3f(0,255.f,0);
  64         btVector3 vY = tr*btVector3(0,fSize,0);
  65         vertex(tr.getOrigin()); vertex(vY);
  66
  67         // z
  68         glColor3f(0,0,255.f);
  69         btVector3 vZ = tr*btVector3(0,0,fSize);
  70         vertex(tr.getOrigin()); vertex(vZ);
  71
  72         glEnd();
  73 }
  74
  75 // /LOCAL FUNCTIONS
  76
  77
  78
  79 #define NUM_LEGS 6
  80 #define BODYPART_COUNT 2 * NUM_LEGS + 1
  81 #define JOINT_COUNT BODYPART_COUNT - 1
  82
  83 class TestRig
  84 {
  85         btDynamicsWorld*        m_ownerWorld;
  86         btCollisionShape*       m_shapes[BODYPART_COUNT];
  87         btRigidBody*            m_bodies[BODYPART_COUNT];
  88         btTypedConstraint*      m_joints[JOINT_COUNT];
  89
  90         btRigidBody* localCreateRigidBody (btScalar mass, const btTransform& startTransform, btCollisionShape* shape)
  91         {
  92                 bool isDynamic = (mass != 0.f);
  93
  94                 btVector3 localInertia(0,0,0);
  95                 if (isDynamic)
  96                         shape->calculateLocalInertia(mass,localInertia);
  97
  98                 btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
  99                 btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia);
 100                 btRigidBody* body = new btRigidBody(rbInfo);
 101
 102                 m_ownerWorld->addRigidBody(body);
 103
 104                 return body;
 105         }
 106
 107
 108 public:
 109         TestRig (btDynamicsWorld* ownerWorld, const btVector3& positionOffset, bool bFixed)
 110                 : m_ownerWorld (ownerWorld)
 111         {
 112                 btVector3 vUp(0, 1, 0);
 113
 114                 //
 115                 // Setup geometry
 116                 //
 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));
 121                 int i;
 122                 for ( i=0; i<NUM_LEGS; i++)
 123                 {
 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));
 126                 }
 127
 128                 //
 129                 // Setup rigid bodies
 130                 //
 131                 float fHeight = 0.5;
 132                 btTransform offset; offset.setIdentity();
 133                 offset.setOrigin(positionOffset);
 134
 135                 // root
 136                 btVector3 vRoot = btVector3(btScalar(0.), btScalar(fHeight), btScalar(0.));
 137                 btTransform transform;
 138                 transform.setIdentity();
 139                 transform.setOrigin(vRoot);
 140                 if (bFixed)
 141                 {
 142                         m_bodies[0] = localCreateRigidBody(btScalar(0.), offset*transform, m_shapes[0]);
 143                 } else
 144                 {
 145                         m_bodies[0] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[0]);
 146                 }
 147                 // legs
 148                 for ( i=0; i<NUM_LEGS; i++)
 149                 {
 150                         float fAngle = 2 * M_PI * i / NUM_LEGS;
 151                         float fSin = sin(fAngle);
 152                         float fCos = cos(fAngle);
 153
 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);
 157
 158                         // thigh
 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]);
 163
 164                         // shin
 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]);
 168                 }
 169
 170                 // Setup some damping on the m_bodies
 171                 for (i = 0; i < BODYPART_COUNT; ++i)
 172                 {
 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);
 177                 }
 178
 179
 180                 //
 181                 // Setup the constraints
 182                 //
 183                 btHingeConstraint* hingeC;
 184                 //btConeTwistConstraint* coneC;
 185
 186                 btTransform localA, localB, localC;
 187
 188                 for ( i=0; i<NUM_LEGS; i++)
 189                 {
 190                         float fAngle = 2 * M_PI * i / NUM_LEGS;
 191                         float fSin = sin(fAngle);
 192                         float fCos = cos(fAngle);
 193
 194                         // hip joints
 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);
 203
 204                         // knee joints
 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);
 214                 }
 215         }
 216
 217         virtual ~TestRig ()
 218         {
 219                 int i;
 220
 221                 // Remove all constraints
 222                 for ( i = 0; i < JOINT_COUNT; ++i)
 223                 {
 224                         m_ownerWorld->removeConstraint(m_joints[i]);
 225                         delete m_joints[i]; m_joints[i] = 0;
 226                 }
 227
 228                 // Remove all bodies and shapes
 229                 for ( i = 0; i < BODYPART_COUNT; ++i)
 230                 {
 231                         m_ownerWorld->removeRigidBody(m_bodies[i]);
 232
 233                         delete m_bodies[i]->getMotionState();
 234
 235                         delete m_bodies[i]; m_bodies[i] = 0;
 236                         delete m_shapes[i]; m_shapes[i] = 0;
 237                 }
 238         }
 239
 240         btTypedConstraint** GetJoints() {return &m_joints[0];}
 241
 242 };
 243
 244
 245
 246 void motorPreTickCallback (btDynamicsWorld *world, btScalar timeStep)
 247 {
 248         MotorDemo* motorDemo = (MotorDemo*)world->getWorldUserInfo();
 249
 250         motorDemo->setMotorTargets(timeStep);
 251
 252 }
 253
 254
 255
 256 void MotorDemo::initPhysics()
 257 {
 258         setTexturing(true);
 259         setShadows(true);
 260
 261         // Setup the basic world
 262
 263         m_Time = 0;
 264         m_fCyclePeriod = 2000.f; // in milliseconds
 265
 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;
 271
 272         setCameraDistance(btScalar(5.));
 273
 274         m_collisionConfiguration = new btDefaultCollisionConfiguration();
 275
 276         m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
 277
 278         btVector3 worldAabbMin(-10000,-10000,-10000);
 279         btVector3 worldAabbMax(10000,10000,10000);
 280         m_broadphase = new btAxisSweep3 (worldAabbMin, worldAabbMax);
 281
 282         m_solver = new btSequentialImpulseConstraintSolver;
 283
 284         m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
 285
 286         m_dynamicsWorld->setInternalTickCallback(motorPreTickCallback,this,true);
 287
 288
 289         // Setup a big ground box
 290         {
 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);
 297         }
 298
 299         // Spawn one ragdoll
 300         btVector3 startOffset(1,0.5,0);
 301         spawnTestRig(startOffset, false);
 302         startOffset.setValue(-2,0.5,0);
 303         spawnTestRig(startOffset, true);
 304
 305         clientResetScene();
 306 }
 307
 308
 309 void MotorDemo::spawnTestRig(const btVector3& startOffset, bool bFixed)
 310 {
 311         TestRig* rig = new TestRig(m_dynamicsWorld, startOffset, bFixed);
 312         m_rigs.push_back(rig);
 313 }
 314
 315 void    PreStep()
 316 {
 317
 318 }
 319
 320
 321
 322
 323 void MotorDemo::setMotorTargets(btScalar deltaTime)
 324 {
 325
 326         float ms = deltaTime*1000000.;
 327         float minFPS = 1000000.f/60.f;
 328         if (ms > minFPS)
 329                 ms = minFPS;
 330
 331         m_Time += ms;
 332
 333         //
 334         // set per-frame sinusoidal position targets using angular motor (hacky?)
 335         //
 336         for (int r=0; r<m_rigs.size(); r++)
 337         {
 338                 for (int i=0; i<2*NUM_LEGS; i++)
 339                 {
 340                         btHingeConstraint* hingeC = static_cast<btHingeConstraint*>(m_rigs[r]->GetJoints()[i]);
 341                         btScalar fCurAngle      = hingeC->getHingeAngle();
 342
 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);
 349                 }
 350         }
 351
 352
 353 }
 354
 355 void MotorDemo::clientMoveAndDisplay()
 356 {
 357         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 358
 359         //simple dynamics world doesn't handle fixed-time-stepping
 360         float deltaTime = getDeltaTimeMicroseconds()/1000000.f;
 361
 362
 363         if (m_dynamicsWorld)
 364         {
 365                 m_dynamicsWorld->stepSimulation(deltaTime);
 366                 m_dynamicsWorld->debugDrawWorld();
 367         }
 368
 369         renderme();
 370
 371         for (int i=2; i>=0 ;i--)
 372         {
 373                 btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
 374                 btRigidBody* body = btRigidBody::upcast(obj);
 375                 drawFrame(body->getWorldTransform());
 376         }
 377
 378         glFlush();
 379
 380         glutSwapBuffers();
 381 }
 382
 383 void MotorDemo::displayCallback()
 384 {
 385         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 386
 387         if (m_dynamicsWorld)
 388                 m_dynamicsWorld->debugDrawWorld();
 389
 390         renderme();
 391
 392         glFlush();
 393         glutSwapBuffers();
 394 }
 395
 396 void MotorDemo::keyboardCallback(unsigned char key, int x, int y)
 397 {
 398         switch (key)
 399         {
 400         case '+': case '=':
 401                 m_fCyclePeriod /= 1.1f;
 402                 if (m_fCyclePeriod < 1.f)
 403                         m_fCyclePeriod = 1.f;
 404                 break;
 405         case '-': case '_':
 406                 m_fCyclePeriod *= 1.1f;
 407                 break;
 408         case '[':
 409                 m_fMuscleStrength /= 1.1f;
 410                 break;
 411         case ']':
 412                 m_fMuscleStrength *= 1.1f;
 413                 break;
 414         default:
 415                 DemoApplication::keyboardCallback(key, x, y);
 416         }
 417 }
 418
 419
 420
 421 void MotorDemo::exitPhysics()
 422 {
 423
 424         int i;
 425
 426         for (i=0;i<m_rigs.size();i++)
 427         {
 428                 TestRig* rig = m_rigs[i];
 429                 delete rig;
 430         }
 431
 432         //cleanup in the reverse order of creation/initialization
 433
 434         //remove the rigidbodies from the dynamics world and delete them
 435
 436         for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
 437         {
 438                 btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
 439                 btRigidBody* body = btRigidBody::upcast(obj);
 440                 if (body && body->getMotionState())
 441                 {
 442                         delete body->getMotionState();
 443                 }
 444                 m_dynamicsWorld->removeCollisionObject( obj );
 445                 delete obj;
 446         }
 447
 448         //delete collision shapes
 449         for (int j=0;j<m_collisionShapes.size();j++)
 450         {
 451                 btCollisionShape* shape = m_collisionShapes[j];
 452                 delete shape;
 453         }
 454
 455         //delete dynamics world
 456         delete m_dynamicsWorld;
 457
 458         //delete solver
 459         delete m_solver;
 460
 461         //delete broadphase
 462         delete m_broadphase;
 463
 464         //delete dispatcher
 465         delete m_dispatcher;
 466
 467         delete m_collisionConfiguration;
 468 }