Demos/SliderConstraintDemo/SliderConstraintDemo.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
   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 Added by Roman Ponomarev (rponom@gmail.com)
  18 April 04, 2008
  19
  20 Added support for ODE sover
  21 April 24, 2008
  22 */
  23
  24
  25
  26
  27 #include "btBulletDynamicsCommon.h"
  28 #include "LinearMath/btIDebugDraw.h"
  29
  30 #include "GLDebugDrawer.h"
  31
  32
  33 #include <stdio.h> //printf debugging
  34
  35 #include "SliderConstraintDemo.h"
  36 #include "GL_ShapeDrawer.h"
  37 #include "GlutStuff.h"
  38
  39
  40
  41 #define SLIDER_DEMO_USE_ODE_SOLVER 0
  42 #define SLIDER_DEMO_USE_6DOF 0
  43
  44 #define CUBE_HALF_EXTENTS 1.f
  45
  46 #define SLIDER_ENABLE_ALL_DEMOS 1
  47
  48
  49
  50 // A couple of sliders
  51 #if SLIDER_DEMO_USE_6DOF
  52         static btGeneric6DofConstraint *spSlider1, *spSlider2;
  53 #else
  54         static btSliderConstraint *spSlider1, *spSlider2;
  55 #endif
  56
  57 static btPoint2PointConstraint* spP2PConst;
  58 static btHingeConstraint* spHingeConst;
  59
  60
  61
  62 static void draw_axes(const btRigidBody& rb, const btTransform& frame)
  63 {
  64         glBegin(GL_LINES);
  65         // draw world transform
  66         btVector3 from = rb.getWorldTransform().getOrigin();
  67         btVector3 to = from + rb.getWorldTransform().getBasis() * btVector3(5,0,0);
  68         // X in red
  69         glColor3f(255.0F, 0.0F, 0.0F);
  70         glVertex3d(from.getX(), from.getY(), from.getZ());
  71         glVertex3d(to.getX(), to.getY(), to.getZ());
  72         to = from + rb.getWorldTransform().getBasis() * btVector3(0,5,0);
  73         // Y in green
  74         glColor3f(0.0F, 255.0F, 0.0F);
  75         glVertex3d(from.getX(), from.getY(), from.getZ());
  76         glVertex3d(to.getX(), to.getY(), to.getZ());
  77         to = from + rb.getWorldTransform().getBasis() * btVector3(0,0,5);
  78         // Z in blue
  79         glColor3f(0.0F, 0.0F, 255.0F);
  80         glVertex3d(from.getX(), from.getY(), from.getZ());
  81         glVertex3d(to.getX(), to.getY(), to.getZ());
  82         // draw slider frame
  83         btTransform calc_frame = rb.getWorldTransform() * frame;
  84         from = calc_frame.getOrigin();
  85         to = from + calc_frame.getBasis() * btVector3(10,0,0);
  86         // X in red
  87         glColor3f(255.0F, 0.0F, 0.0F);
  88         glVertex3d(from.getX(), from.getY(), from.getZ());
  89         glVertex3d(to.getX(), to.getY(), to.getZ());
  90         to = from + calc_frame.getBasis() * btVector3(0,10,0);
  91         // Y in green
  92         glColor3f(0.0F, 255.0F, 0.0F);
  93         glVertex3d(from.getX(), from.getY(), from.getZ());
  94         glVertex3d(to.getX(), to.getY(), to.getZ());
  95         to = from + calc_frame.getBasis() * btVector3(0,0,10);
  96         // Z in blue
  97         glColor3f(0.0F, 0.0F, 255.0F);
  98         glVertex3d(from.getX(), from.getY(), from.getZ());
  99         glVertex3d(to.getX(), to.getY(), to.getZ());
 100         glEnd();
 101 } // draw_axes()
 102
 103
 104
 105 #if SLIDER_DEMO_USE_6DOF
 106 static void     drawSlider(btGeneric6DofConstraint* pSlider)
 107 {
 108         draw_axes(pSlider->getRigidBodyA(), pSlider->getFrameOffsetA());
 109         draw_axes(pSlider->getRigidBodyB(), pSlider->getFrameOffsetB());
 110 } // drawSlider()
 111 #else
 112 static void     drawSlider(btSliderConstraint* pSlider)
 113 {
 114         draw_axes(pSlider->getRigidBodyA(), pSlider->getFrameOffsetA());
 115         draw_axes(pSlider->getRigidBodyB(), pSlider->getFrameOffsetB());
 116         // draw limits in white
 117         btVector3 from(pSlider->getLowerLinLimit(), 0, 0);
 118         btVector3 to(pSlider->getUpperLinLimit(), 0, 0);
 119         btTransform trans;
 120         if(pSlider->getUseLinearReferenceFrameA())
 121         {
 122                 trans = pSlider->getRigidBodyA().getWorldTransform() * pSlider->getFrameOffsetA();
 123         }
 124         else
 125         {
 126                 trans = pSlider->getRigidBodyB().getWorldTransform() * pSlider->getFrameOffsetB();
 127         }
 128         from = trans * from;
 129         to = trans * to;
 130         glBegin(GL_LINES);
 131         glColor3f(255.0F, 255.0F, 255.0F);
 132         glVertex3d(from.getX(), from.getY(), from.getZ());
 133         glVertex3d(to.getX(), to.getY(), to.getZ());
 134         glEnd();
 135 } // drawSlider()
 136 #endif
 137
 138
 139
 140 void SliderConstraintDemo::initPhysics()
 141 {
 142         setTexturing(true);
 143         setShadows(true);
 144
 145         setCameraDistance(26.f);
 146
 147         // init world
 148         m_collisionConfiguration = new btDefaultCollisionConfiguration();
 149         m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
 150         btVector3 worldMin(-1000,-1000,-1000);
 151         btVector3 worldMax(1000,1000,1000);
 152         m_overlappingPairCache = new btAxisSweep3(worldMin,worldMax);
 153
 154 #if SLIDER_DEMO_USE_ODE_SOLVER
 155         m_constraintSolver = new btOdeQuickstepConstraintSolver();
 156 #else
 157         m_constraintSolver = new btSequentialImpulseConstraintSolver();
 158 #endif
 159
 160         btDiscreteDynamicsWorld* wp = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_constraintSolver,m_collisionConfiguration);
 161         //      wp->getSolverInfo().m_numIterations = 20; // default is 10
 162         m_dynamicsWorld = wp;
 163 //      wp->getSolverInfo().m_erp = 0.8;
 164
 165         // add floor
 166         //btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
 167         btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
 168         m_collisionShapes.push_back(groundShape);
 169         btTransform groundTransform;
 170         groundTransform.setIdentity();
 171         groundTransform.setOrigin(btVector3(0,-76,0));
 172         btRigidBody* groundBody;
 173         groundBody = localCreateRigidBody(0, groundTransform, groundShape);
 174
 175         // add box shape (will be reused for all bodies)
 176         btCollisionShape* shape = new btBoxShape(btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS));
 177         m_collisionShapes.push_back(shape);
 178         // mass of dymamic bodies
 179         btScalar mass = btScalar(1.);
 180
 181         // add dynamic rigid body A1
 182         btTransform trans;
 183         trans.setIdentity();
 184         btVector3 worldPos(-20,0,0);
 185         trans.setOrigin(worldPos);
 186         btTransform frameInA, frameInB;
 187         frameInA = btTransform::getIdentity();
 188         frameInB = btTransform::getIdentity();
 189
 190 #if SLIDER_ENABLE_ALL_DEMOS
 191         btRigidBody* pRbA1 = localCreateRigidBody(mass, trans, shape);
 192 //      btRigidBody* pRbA1 = localCreateRigidBody(0.f, trans, shape);
 193         pRbA1->setActivationState(DISABLE_DEACTIVATION);
 194
 195         // add dynamic rigid body B1
 196         worldPos.setValue(-30,0,0);
 197         trans.setOrigin(worldPos);
 198         btRigidBody* pRbB1 = localCreateRigidBody(mass, trans, shape);
 199 //      btRigidBody* pRbB1 = localCreateRigidBody(0.f, trans, shape);
 200         pRbB1->setActivationState(DISABLE_DEACTIVATION);
 201
 202         // create slider constraint between A1 and B1 and add it to world
 203
 204 #if SLIDER_DEMO_USE_6DOF
 205         spSlider1 = new btGeneric6DofConstraint(*pRbA1, *pRbB1, frameInA, frameInB, true);
 206         btVector3 lowerSliderLimit = btVector3(-20,0,0);
 207         btVector3 hiSliderLimit = btVector3(-10,0,0);
 208 //      btVector3 lowerSliderLimit = btVector3(-20,-5,-5);
 209 //      btVector3 hiSliderLimit = btVector3(-10,5,5);
 210         spSlider1->setLinearLowerLimit(lowerSliderLimit);
 211         spSlider1->setLinearUpperLimit(hiSliderLimit);
 212         spSlider1->setAngularLowerLimit(btVector3(0,0,0));
 213         spSlider1->setAngularUpperLimit(btVector3(0,0,0));
 214 #else
 215         spSlider1 = new btSliderConstraint(*pRbA1, *pRbB1, frameInA, frameInB, true);
 216 //      spSlider1 = new btSliderConstraint(*pRbA1, *pRbB1, frameInA, frameInB, false);
 217         spSlider1->setLowerLinLimit(-15.0F);
 218         spSlider1->setUpperLinLimit(-5.0F);
 219 //      spSlider1->setLowerLinLimit(5.0F);
 220 //      spSlider1->setUpperLinLimit(15.0F);
 221 //      spSlider1->setLowerLinLimit(-10.0F);
 222 //      spSlider1->setUpperLinLimit(-10.0F);
 223
 224         spSlider1->setLowerAngLimit(-SIMD_PI / 3.0F);
 225         spSlider1->setUpperAngLimit( SIMD_PI / 3.0F);
 226 #endif
 227
 228         m_dynamicsWorld->addConstraint(spSlider1, true);
 229         spSlider1->setDbgDrawSize(btScalar(5.f));
 230 #endif
 231
 232 #if SLIDER_ENABLE_ALL_DEMOS
 233         // add kinematic rigid body A2
 234 //      worldPos.setValue(20,4,0);
 235         worldPos.setValue(5,-20,0);
 236         trans.setOrigin(worldPos);
 237         btRigidBody* pRbA2 = localCreateRigidBody(0., trans, shape);
 238 //      btRigidBody* pRbA2 = localCreateRigidBody(mass, trans, shape);
 239 //      btRigidBody* pRbA2 = localCreateRigidBody(mass * 10000, trans, shape);
 240         pRbA2->setActivationState(DISABLE_DEACTIVATION);
 241
 242         // add dynamic rigid body B2
 243 //      worldPos.setValue(-20,4,0);
 244         worldPos.setValue(-5,-20,0);
 245         trans.setOrigin(worldPos);
 246 //      btRigidBody* pRbB2 = localCreateRigidBody(0., trans, shape);
 247         btRigidBody* pRbB2 = localCreateRigidBody(mass, trans, shape);
 248 //      btRigidBody* pRbB2 = localCreateRigidBody(mass * 10000, trans, shape);
 249         pRbB2->setActivationState(DISABLE_DEACTIVATION);
 250
 251 //      frameInA.getBasis().setEulerZYX(1.f, 1.f, 1.f);
 252 //      frameInB.getBasis().setEulerZYX(1.f, 1.f, 1.f);
 253 //      frameInA.getBasis().setEulerZYX(1.f, 1.f, 1.f);
 254 //      frameInB.getBasis().setEulerZYX(1.f, 1.f, 1.f);
 255
 256
 257 //      frameInA.setOrigin(btVector3(-20., 5., 0));
 258 //      frameInB.setOrigin(btVector3( 20., 5., 0));
 259         frameInA.setOrigin(btVector3(-5., 20., 0));
 260         frameInB.setOrigin(btVector3( 5., 20., 0));
 261
 262
 263         // create slider constraint between A2 and B2 and add it to world
 264 #if SLIDER_DEMO_USE_6DOF
 265         spSlider2 = new btGeneric6DofConstraint(*pRbA2, *pRbB2, frameInA, frameInB, true);
 266         spSlider2->setLinearLowerLimit(lowerSliderLimit);
 267         spSlider2->setLinearUpperLimit(hiSliderLimit);
 268         spSlider2->setAngularLowerLimit(btVector3(0,0,0));
 269         spSlider2->setAngularUpperLimit(btVector3(0,0,0));
 270 #else
 271         spSlider2 = new btSliderConstraint(*pRbA2, *pRbB2, frameInA, frameInB, true);
 272 //      spSlider2 = new btSliderConstraint(*pRbA2, *pRbB2, frameInA, frameInB, false);
 273 //      spSlider2->setLowerLinLimit(0.0F);
 274 //      spSlider2->setUpperLinLimit(0.0F);
 275         spSlider2->setLowerLinLimit(-2.0F);
 276         spSlider2->setUpperLinLimit(2.0F);
 277 //      spSlider2->setLowerLinLimit(5.0F);
 278 //      spSlider2->setUpperLinLimit(25.0F);
 279 //      spSlider2->setUpperLinLimit(-5.0F);
 280 //      spSlider2->setUpperLinLimit(-9.99F);
 281
 282
 283 //      spSlider2->setLowerAngLimit(SIMD_PI / 2.0F);
 284 //      spSlider2->setUpperAngLimit(-SIMD_PI / 2.0F);
 285
 286         //      spSlider2->setLowerAngLimit(-SIMD_PI / 2.0F);
 287 //      spSlider2->setUpperAngLimit(SIMD_PI / 2.0F);
 288
 289 //      spSlider2->setLowerAngLimit(-SIMD_PI);
 290 //      spSlider2->setUpperAngLimit(SIMD_PI *0.8F);
 291
 292
 293 //      spSlider2->setLowerAngLimit(-0.01F);
 294 //      spSlider2->setUpperAngLimit(0.01F);
 295         spSlider2->setLowerAngLimit(-1.570796326F * 0.5f);
 296         spSlider2->setUpperAngLimit(1.570796326F * 0.5f);
 297 //      spSlider2->setLowerAngLimit(1.F);
 298 //      spSlider2->setUpperAngLimit(-1.F);
 299
 300
 301 //      spSlider2->setDampingLimLin(0.5f);
 302
 303 #if 0
 304         // add motors
 305         spSlider2->setPoweredLinMotor(true);
 306         spSlider2->setMaxLinMotorForce(0.1);
 307         spSlider2->setTargetLinMotorVelocity(-5.0);
 308
 309         spSlider2->setPoweredAngMotor(true);
 310 //      spSlider2->setMaxAngMotorForce(0.01);
 311         spSlider2->setMaxAngMotorForce(10.0);
 312         spSlider2->setTargetAngMotorVelocity(1.0);
 313
 314         // change default damping and restitution
 315
 316         spSlider2->setDampingDirLin(0.005F);
 317         spSlider2->setRestitutionLimLin(1.1F);
 318 #endif
 319
 320         // various ODE tests
 321 //      spSlider2->setDampingLimLin(0.1F); // linear bounce factor for ODE == 1.0 - DampingLimLin;
 322 //      spSlider2->setDampingLimAng(0.1F); // angular bounce factor for ODE == 1.0 - DampingLimAng;
 323 //      spSlider2->setSoftnessOrthoAng(0.1);
 324 //      spSlider2->setSoftnessOrthoLin(0.1);
 325 //      spSlider2->setSoftnessLimLin(0.1);
 326 //      spSlider2->setSoftnessLimAng(0.1);
 327 #endif
 328         m_dynamicsWorld->addConstraint(spSlider2, true);
 329         spSlider2->setDbgDrawSize(btScalar(5.f));
 330 #endif
 331
 332 #if SLIDER_ENABLE_ALL_DEMOS
 333 {
 334         // add dynamic rigid body A1
 335         trans.setIdentity();
 336         worldPos.setValue(20,0,0);
 337         trans.setOrigin(worldPos);
 338         btRigidBody* pRbA3 = localCreateRigidBody(0.0F, trans, shape);
 339         pRbA3->setActivationState(DISABLE_DEACTIVATION);
 340
 341         // add dynamic rigid body B1
 342         worldPos.setValue(25,0,0);
 343         trans.setOrigin(worldPos);
 344         btRigidBody* pRbB3 = localCreateRigidBody(mass, trans, shape);
 345         pRbB3->setActivationState(DISABLE_DEACTIVATION);
 346
 347         btVector3 pivA( 2.5, 0., 0.);
 348         btVector3 pivB(-2.5, 0., 0.);
 349         spP2PConst = new btPoint2PointConstraint(*pRbA3, *pRbB3, pivA, pivB);
 350         m_dynamicsWorld->addConstraint(spP2PConst, true);
 351         spP2PConst->setDbgDrawSize(btScalar(5.f));
 352
 353 }
 354 #endif
 355
 356 #if 0 // SLIDER_ENABLE_ALL_DEMOS
 357         // add dynamic rigid body A4
 358         trans.setIdentity();
 359         worldPos.setValue(20,10,0);
 360         trans.setOrigin(worldPos);
 361         btRigidBody* pRbA4 = localCreateRigidBody(0.0F, trans, shape);
 362         pRbA4->setActivationState(DISABLE_DEACTIVATION);
 363
 364         // add dynamic rigid body B1
 365         worldPos.setValue(27,10,0);
 366         trans.setOrigin(worldPos);
 367         btRigidBody* pRbB4 = localCreateRigidBody(mass, trans, shape);
 368         pRbB1->setActivationState(DISABLE_DEACTIVATION);
 369
 370
 371         btVector3 pivA( 2., 0., 0.);
 372         btVector3 pivB(-5., 0., 0.);
 373         btVector3 axisA(0., 0., 1.);
 374         btVector3 axisB(0., 0., 1.);
 375
 376         spHingeConst = new btHingeConstraint(*pRbA4, *pRbB4, pivA, pivB, axisA, axisB);
 377 //      spHingeConst->setLimit(-1.57, 1.57);
 378         spHingeConst->setLimit(1.57, -1.57);
 379         spHingeConst->enableAngularMotor(true, 10.0, 0.19);
 380
 381         m_dynamicsWorld->addConstraint(spHingeConst, true);
 382         spHingeConst->setDbgDrawSize(btScalar(5.f));
 383
 384 #endif
 385
 386 } // SliderConstraintDemo::initPhysics()
 387
 388
 389
 390 SliderConstraintDemo::~SliderConstraintDemo()
 391 {
 392         //cleanup in the reverse order of creation/initialization
 393         int i;
 394         //removed/delete constraints
 395         for (i=m_dynamicsWorld->getNumConstraints()-1; i>=0 ;i--)
 396         {
 397                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
 398                 m_dynamicsWorld->removeConstraint(constraint);
 399                 delete constraint;
 400         }
 401         //remove the rigidbodies from the dynamics world and delete them
 402         for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
 403         {
 404                 btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
 405                 btRigidBody* body = btRigidBody::upcast(obj);
 406                 if (body && body->getMotionState())
 407                 {
 408                         delete body->getMotionState();
 409                 }
 410                 m_dynamicsWorld->removeCollisionObject( obj );
 411                 delete obj;
 412         }
 413         //delete collision shapes
 414         for (int j=0;j<m_collisionShapes.size();j++)
 415         {
 416                 btCollisionShape* shape = m_collisionShapes[j];
 417                 delete shape;
 418         }
 419         //delete dynamics world
 420         delete m_dynamicsWorld;
 421         //delete solver
 422         delete m_constraintSolver;
 423         //delete broadphase
 424         delete m_overlappingPairCache;
 425         //delete dispatcher
 426         delete m_dispatcher;
 427         delete m_collisionConfiguration;
 428 } // SliderConstraintDemo::~SliderConstraintDemo()
 429
 430
 431
 432 void SliderConstraintDemo::clientMoveAndDisplay()
 433 {
 434         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 435         float dt = float(getDeltaTimeMicroseconds()) * 0.000001f;
 436         //during idle mode, just run 1 simulation step maximum
 437         int maxSimSubSteps = m_idle ? 1 : 1;
 438         if(m_idle)
 439         {
 440                 dt = 1.0/420.f;
 441         }
 442         int numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps);
 443         //optional but useful: debug drawing
 444         m_dynamicsWorld->debugDrawWorld();
 445         bool verbose = false;
 446         if (verbose)
 447         {
 448                 if (!numSimSteps)
 449                         printf("Interpolated transforms\n");
 450                 else
 451                 {
 452                         if (numSimSteps > maxSimSubSteps)
 453                         {
 454                                 //detect dropping frames
 455                                 printf("Dropped (%i) simulation steps out of %i\n",numSimSteps - maxSimSubSteps,numSimSteps);
 456                         } else
 457                         {
 458                                 printf("Simulated (%i) steps\n",numSimSteps);
 459                         }
 460                 }
 461         }
 462         renderme();
 463 //      drawSlider(spSlider1);
 464 //      drawSlider(spSlider2);
 465     glFlush();
 466     glutSwapBuffers();
 467 } // SliderConstraintDemo::clientMoveAndDisplay()
 468
 469
 470
 471 void SliderConstraintDemo::displayCallback(void)
 472 {
 473     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 474         if(m_dynamicsWorld)
 475         {
 476                 m_dynamicsWorld->debugDrawWorld();
 477         }
 478 //      drawSlider(spSlider1);
 479 //      drawSlider(spSlider2);
 480         renderme();
 481     glFlush();
 482     glutSwapBuffers();
 483 } // SliderConstraintDemo::displayCallback()
 484
 485
 486 void SliderConstraintDemo::keyboardCallback(unsigned char key, int x, int y)
 487 {
 488         (void)x;
 489         (void)y;
 490         switch (key)
 491         {
 492                 case 'O' :
 493                         {
 494                                 bool offectOnOff;
 495                                 offectOnOff = spSlider1->getUseFrameOffset();
 496                                 offectOnOff = !offectOnOff;
 497                                 spSlider1->setUseFrameOffset(offectOnOff);
 498                                 printf("Slider1 %s frame offset\n", offectOnOff ? "uses" : "does not use");
 499                                 offectOnOff = spSlider2->getUseFrameOffset();
 500                                 offectOnOff = !offectOnOff;
 501                                 spSlider2->setUseFrameOffset(offectOnOff);
 502                                 printf("Slider2 %s frame offset\n", offectOnOff ? "uses" : "does not use");
 503                         }
 504                         break;
 505                 default :
 506                         {
 507                                 DemoApplication::keyboardCallback(key, x, y);
 508                         }
 509                         break;
 510         }
 511 }
 512