source/gameengine/Physics/Bullet/CcdPhysicsController.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 #include "CcdPhysicsController.h"
  17 #include "btBulletDynamicsCommon.h"
  18
  19 #include "PHY_IMotionState.h"
  20 #include "CcdPhysicsEnvironment.h"
  21
  22
  23 class BP_Proxy;
  24
  25 ///todo: fill all the empty CcdPhysicsController methods, hook them up to the btRigidBody class
  26
  27 //'temporarily' global variables
  28 //float gDeactivationTime = 2.f;
  29 //bool  gDisableDeactivation = false;
  30 extern float gDeactivationTime;
  31 extern bool gDisableDeactivation;
  32
  33
  34 float gLinearSleepingTreshold = 0.8f;
  35 float gAngularSleepingTreshold = 1.0f;
  36
  37
  38 btVector3 startVel(0,0,0);//-10000);
  39 CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
  40 :m_cci(ci)
  41 {
  42         m_collisionDelay = 0;
  43         m_newClientInfo = 0;
  44
  45         m_MotionState = ci.m_MotionState;
  46         m_bulletMotionState = 0;
  47
  48
  49         CreateRigidbody();
  50
  51
  52
  53         #ifdef WIN32
  54         if (m_body->getInvMass())
  55                 m_body->setLinearVelocity(startVel);
  56         #endif
  57
  58 }
  59
  60 btTransform     CcdPhysicsController::GetTransformFromMotionState(PHY_IMotionState* motionState)
  61 {
  62         btTransform trans;
  63         float tmp[3];
  64         motionState->getWorldPosition(tmp[0],tmp[1],tmp[2]);
  65         trans.setOrigin(btVector3(tmp[0],tmp[1],tmp[2]));
  66
  67         btQuaternion orn;
  68         motionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
  69         trans.setRotation(orn);
  70         return trans;
  71
  72 }
  73
  74 class   BlenderBulletMotionState : public btMotionState
  75 {
  76         PHY_IMotionState*       m_blenderMotionState;
  77
  78 public:
  79
  80         BlenderBulletMotionState(PHY_IMotionState* bms)
  81                 :m_blenderMotionState(bms)
  82         {
  83
  84         }
  85
  86         virtual void    getWorldTransform(btTransform& worldTrans ) const
  87         {
  88                 float pos[3];
  89                 float quatOrn[4];
  90
  91                 m_blenderMotionState->getWorldPosition(pos[0],pos[1],pos[2]);
  92                 m_blenderMotionState->getWorldOrientation(quatOrn[0],quatOrn[1],quatOrn[2],quatOrn[3]);
  93                 worldTrans.setOrigin(btVector3(pos[0],pos[1],pos[2]));
  94                 worldTrans.setBasis(btMatrix3x3(btQuaternion(quatOrn[0],quatOrn[1],quatOrn[2],quatOrn[3])));
  95         }
  96
  97         virtual void    setWorldTransform(const btTransform& worldTrans)
  98         {
  99                 m_blenderMotionState->setWorldPosition(worldTrans.getOrigin().getX(),worldTrans.getOrigin().getY(),worldTrans.getOrigin().getZ());
 100                 btQuaternion rotQuat = worldTrans.getRotation();
 101                 m_blenderMotionState->setWorldOrientation(rotQuat[0],rotQuat[1],rotQuat[2],rotQuat[3]);
 102                 m_blenderMotionState->calculateWorldTransformations();
 103         }
 104
 105 };
 106
 107 void CcdPhysicsController::CreateRigidbody()
 108 {
 109
 110         btTransform trans = GetTransformFromMotionState(m_cci.m_MotionState);
 111
 112         m_bulletMotionState = new BlenderBulletMotionState(m_cci.m_MotionState);
 113
 114         m_body = new btRigidBody(m_cci.m_mass,
 115                 m_bulletMotionState,
 116                 m_cci.m_collisionShape,
 117                 m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor,
 118                 m_cci.m_linearDamping,m_cci.m_angularDamping,
 119                 m_cci.m_friction,m_cci.m_restitution);
 120
 121
 122
 123         //
 124         // init the rigidbody properly
 125         //
 126
 127         //setMassProps this also sets collisionFlags
 128         //convert collision flags!
 129
 130         m_body->setCollisionFlags(m_body->getCollisionFlags() | m_cci.m_collisionFlags);
 131
 132         m_body->setGravity( m_cci.m_gravity);
 133         m_body->setDamping(m_cci.m_linearDamping, m_cci.m_angularDamping);
 134
 135 }
 136
 137 CcdPhysicsController::~CcdPhysicsController()
 138 {
 139         //will be reference counted, due to sharing
 140         if (m_cci.m_physicsEnv)
 141                 m_cci.m_physicsEnv->removeCcdPhysicsController(this);
 142
 143         delete m_MotionState;
 144         if (m_bulletMotionState)
 145                 delete m_bulletMotionState;
 146         delete m_body;
 147 }
 148
 149                 /**
 150                         SynchronizeMotionStates ynchronizes dynas, kinematic and deformable entities (and do 'late binding')
 151                 */
 152 bool            CcdPhysicsController::SynchronizeMotionStates(float time)
 153 {
 154         //sync non-static to motionstate, and static from motionstate (todo: add kinematic etc.)
 155
 156         if (!m_body->isStaticObject())
 157         {
 158                 const btVector3& worldPos = m_body->getCenterOfMassPosition();
 159                 m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
 160
 161                 const btQuaternion& worldquat = m_body->getOrientation();
 162                 m_MotionState->setWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
 163
 164                 m_MotionState->calculateWorldTransformations();
 165
 166                 float scale[3];
 167                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
 168                 btVector3 scaling(scale[0],scale[1],scale[2]);
 169                 GetCollisionShape()->setLocalScaling(scaling);
 170         } else
 171         {
 172                 btVector3 worldPos;
 173                 btQuaternion worldquat;
 174
 175 /*              m_MotionState->getWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
 176                 m_MotionState->getWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
 177                 btTransform oldTrans = m_body->getCenterOfMassTransform();
 178                 btTransform newTrans(worldquat,worldPos);
 179
 180                 m_body->setCenterOfMassTransform(newTrans);
 181                 //need to keep track of previous position for friction effects...
 182
 183                 m_MotionState->calculateWorldTransformations();
 184 */
 185                 float scale[3];
 186                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
 187                 btVector3 scaling(scale[0],scale[1],scale[2]);
 188                 GetCollisionShape()->setLocalScaling(scaling);
 189         }
 190         return true;
 191
 192 }
 193
 194                 /**
 195                         WriteMotionStateToDynamics synchronizes dynas, kinematic and deformable entities (and do 'late binding')
 196                 */
 197
 198 void            CcdPhysicsController::WriteMotionStateToDynamics(bool nondynaonly)
 199 {
 200
 201 }
 202 void            CcdPhysicsController::WriteDynamicsToMotionState()
 203 {
 204 }
 205                 // controller replication
 206 void            CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl)
 207 {
 208         m_MotionState = motionstate;
 209
 210
 211
 212         m_body = 0;
 213         CreateRigidbody();
 214
 215         m_cci.m_physicsEnv->addCcdPhysicsController(this);
 216
 217
 218 /*      SM_Object* dynaparent=0;
 219         SumoPhysicsController* sumoparentctrl = (SumoPhysicsController* )parentctrl;
 220
 221         if (sumoparentctrl)
 222         {
 223                 dynaparent = sumoparentctrl->GetSumoObject();
 224         }
 225
 226         SM_Object* orgsumoobject = m_sumoObj;
 227
 228
 229         m_sumoObj       =       new SM_Object(
 230                 orgsumoobject->getShapeHandle(),
 231                 orgsumoobject->getMaterialProps(),
 232                 orgsumoobject->getShapeProps(),
 233                 dynaparent);
 234
 235         m_sumoObj->setRigidBody(orgsumoobject->isRigidBody());
 236
 237         m_sumoObj->setMargin(orgsumoobject->getMargin());
 238         m_sumoObj->setPosition(orgsumoobject->getPosition());
 239         m_sumoObj->setOrientation(orgsumoobject->getOrientation());
 240         //if it is a dyna, register for a callback
 241         m_sumoObj->registerCallback(*this);
 242
 243         m_sumoScene->add(* (m_sumoObj));
 244         */
 245
 246
 247
 248 }
 249
 250                 // kinematic methods
 251 void            CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
 252 {
 253         if (m_body)
 254         {
 255                 m_body->activate(true);
 256                 if (m_body->isStaticObject())
 257                 {
 258                         m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
 259                 }
 260
 261
 262                 btVector3 dloc(dlocX,dlocY,dlocZ);
 263                 btTransform xform = m_body->getCenterOfMassTransform();
 264
 265                 if (local)
 266                 {
 267                         dloc = xform.getBasis()*dloc;
 268                 }
 269
 270                 xform.setOrigin(xform.getOrigin() + dloc);
 271                 m_body->setCenterOfMassTransform(xform);
 272         }
 273
 274 }
 275
 276 void            CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
 277 {
 278         if (m_body )
 279         {
 280                 m_body->activate(true);
 281                 if (m_body->isStaticObject())
 282                 {
 283                         m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
 284                 }
 285
 286                 btMatrix3x3 drotmat(    rotval[0],rotval[1],rotval[2],
 287                                                                 rotval[4],rotval[5],rotval[6],
 288                                                                 rotval[8],rotval[9],rotval[10]);
 289
 290
 291                 btMatrix3x3 currentOrn;
 292                 GetWorldOrientation(currentOrn);
 293
 294                 btTransform xform = m_body->getCenterOfMassTransform();
 295
 296                 xform.setBasis(xform.getBasis()*(local ?
 297                 drotmat : (currentOrn.inverse() * drotmat * currentOrn)));
 298
 299                 m_body->setCenterOfMassTransform(xform);
 300         }
 301
 302 }
 303
 304 void CcdPhysicsController::GetWorldOrientation(btMatrix3x3& mat)
 305 {
 306         float orn[4];
 307         m_MotionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
 308         btQuaternion quat(orn[0],orn[1],orn[2],orn[3]);
 309         mat.setRotation(quat);
 310 }
 311
 312 void            CcdPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
 313 {
 314         btQuaternion q = m_body->getCenterOfMassTransform().getRotation();
 315         quatImag0 = q[0];
 316         quatImag1 = q[1];
 317         quatImag2 = q[2];
 318         quatReal = q[3];
 319 }
 320 void            CcdPhysicsController::setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal)
 321 {
 322         m_body->activate(true);
 323         if (m_body->isStaticObject())
 324         {
 325                 m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
 326         }
 327
 328         m_MotionState->setWorldOrientation(quatImag0,quatImag1,quatImag2,quatReal);
 329         btTransform xform  = m_body->getCenterOfMassTransform();
 330         xform.setRotation(btQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
 331         m_body->setCenterOfMassTransform(xform);
 332
 333 }
 334
 335 void            CcdPhysicsController::setPosition(float posX,float posY,float posZ)
 336 {
 337         m_body->activate(true);
 338         if (m_body->isStaticObject())
 339         {
 340                 m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
 341         }
 342         m_MotionState->setWorldPosition(posX,posY,posZ);
 343         btTransform xform  = m_body->getCenterOfMassTransform();
 344         xform.setOrigin(btVector3(posX,posY,posZ));
 345         m_body->setCenterOfMassTransform(xform);
 346
 347 }
 348 void            CcdPhysicsController::resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ)
 349 {
 350 }
 351
 352 void            CcdPhysicsController::getPosition(PHY__Vector3& pos) const
 353 {
 354         const btTransform& xform = m_body->getCenterOfMassTransform();
 355         pos[0] = xform.getOrigin().x();
 356         pos[1] = xform.getOrigin().y();
 357         pos[2] = xform.getOrigin().z();
 358 }
 359
 360 void            CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
 361 {
 362         if (!btFuzzyZero(m_cci.m_scaling.x()-scaleX) ||
 363                 !btFuzzyZero(m_cci.m_scaling.y()-scaleY) ||
 364                 !btFuzzyZero(m_cci.m_scaling.z()-scaleZ))
 365         {
 366                 m_cci.m_scaling = btVector3(scaleX,scaleY,scaleZ);
 367
 368                 if (m_body && m_body->getCollisionShape())
 369                 {
 370                         m_body->getCollisionShape()->setLocalScaling(m_cci.m_scaling);
 371                         m_body->getCollisionShape()->calculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor);
 372                         m_body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
 373                 }
 374         }
 375 }
 376
 377                 // physics methods
 378 void            CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)
 379 {
 380         btVector3 torque(torqueX,torqueY,torqueZ);
 381         btTransform xform = m_body->getCenterOfMassTransform();
 382         if (torque.length2() > (SIMD_EPSILON*SIMD_EPSILON))
 383         {
 384                 m_body->activate();
 385         }
 386         if (local)
 387         {
 388                 torque  = xform.getBasis()*torque;
 389         }
 390         m_body->applyTorque(torque);
 391 }
 392
 393 void            CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local)
 394 {
 395         btVector3 force(forceX,forceY,forceZ);
 396
 397         if (force.length2() > (SIMD_EPSILON*SIMD_EPSILON))
 398         {
 399                 m_body->activate();
 400         }
 401
 402
 403         btTransform xform = m_body->getCenterOfMassTransform();
 404         if (local)
 405         {
 406                 force   = xform.getBasis()*force;
 407         }
 408         m_body->applyCentralForce(force);
 409 }
 410 void            CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
 411 {
 412         btVector3 angvel(ang_velX,ang_velY,ang_velZ);
 413         if (angvel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
 414         {
 415                 m_body->activate(true);
 416         }
 417
 418         {
 419                 btTransform xform = m_body->getCenterOfMassTransform();
 420                 if (local)
 421                 {
 422                         angvel  = xform.getBasis()*angvel;
 423                 }
 424
 425                 m_body->setAngularVelocity(angvel);
 426         }
 427
 428 }
 429 void            CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)
 430 {
 431
 432         btVector3 linVel(lin_velX,lin_velY,lin_velZ);
 433         if (linVel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
 434         {
 435                 m_body->activate(true);
 436         }
 437
 438         {
 439                 btTransform xform = m_body->getCenterOfMassTransform();
 440                 if (local)
 441                 {
 442                         linVel  = xform.getBasis()*linVel;
 443                 }
 444                 m_body->setLinearVelocity(linVel);
 445         }
 446 }
 447 void            CcdPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)
 448 {
 449         btVector3 impulse(impulseX,impulseY,impulseZ);
 450
 451         if (impulse.length2() > (SIMD_EPSILON*SIMD_EPSILON))
 452         {
 453                 m_body->activate();
 454
 455                 btVector3 pos(attachX,attachY,attachZ);
 456
 457                 m_body->activate();
 458
 459                 m_body->applyImpulse(impulse,pos);
 460         }
 461
 462 }
 463 void            CcdPhysicsController::SetActive(bool active)
 464 {
 465 }
 466                 // reading out information from physics
 467 void            CcdPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& linvZ)
 468 {
 469         const btVector3& linvel = this->m_body->getLinearVelocity();
 470         linvX = linvel.x();
 471         linvY = linvel.y();
 472         linvZ = linvel.z();
 473
 474 }
 475
 476 void            CcdPhysicsController::GetAngularVelocity(float& angVelX,float& angVelY,float& angVelZ)
 477 {
 478         const btVector3& angvel= m_body->getAngularVelocity();
 479         angVelX = angvel.x();
 480         angVelY = angvel.y();
 481         angVelZ = angvel.z();
 482 }
 483
 484 void            CcdPhysicsController::GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ)
 485 {
 486         btVector3 pos(posX,posY,posZ);
 487         btVector3 rel_pos = pos-m_body->getCenterOfMassPosition();
 488         btVector3 linvel = m_body->getVelocityInLocalPoint(rel_pos);
 489         linvX = linvel.x();
 490         linvY = linvel.y();
 491         linvZ = linvel.z();
 492 }
 493 void            CcdPhysicsController::getReactionForce(float& forceX,float& forceY,float& forceZ)
 494 {
 495 }
 496
 497                 // dyna's that are rigidbody are free in orientation, dyna's with non-rigidbody are restricted
 498 void            CcdPhysicsController::setRigidBody(bool rigid)
 499 {
 500         if (!rigid)
 501         {
 502                 //fake it for now
 503                 btVector3 inertia = m_body->getInvInertiaDiagLocal();
 504                 inertia[1] = 0.f;
 505                 m_body->setInvInertiaDiagLocal(inertia);
 506                 m_body->updateInertiaTensor();
 507         }
 508 }
 509
 510                 // clientinfo for raycasts for example
 511 void*           CcdPhysicsController::getNewClientInfo()
 512 {
 513         return m_newClientInfo;
 514 }
 515 void            CcdPhysicsController::setNewClientInfo(void* clientinfo)
 516 {
 517         m_newClientInfo = clientinfo;
 518 }
 519
 520
 521 void    CcdPhysicsController::UpdateDeactivation(float timeStep)
 522 {
 523         m_body->updateDeactivation( timeStep);
 524 }
 525
 526 bool CcdPhysicsController::wantsSleeping()
 527 {
 528
 529         return m_body->wantsSleeping();
 530 }
 531
 532 PHY_IPhysicsController* CcdPhysicsController::GetReplica()
 533 {
 534         //very experimental, shape sharing is not implemented yet.
 535         //just support btSphereShape/ConeShape for now
 536
 537         CcdConstructionInfo cinfo = m_cci;
 538         if (cinfo.m_collisionShape)
 539         {
 540                 switch (cinfo.m_collisionShape->getShapeType())
 541                 {
 542                 case SPHERE_SHAPE_PROXYTYPE:
 543                         {
 544                                 btSphereShape* orgShape = (btSphereShape*)cinfo.m_collisionShape;
 545                                 cinfo.m_collisionShape = new btSphereShape(*orgShape);
 546                                 break;
 547                         }
 548
 549                         case CONE_SHAPE_PROXYTYPE:
 550                         {
 551                                 btConeShape* orgShape = (btConeShape*)cinfo.m_collisionShape;
 552                                 cinfo.m_collisionShape = new btConeShape(*orgShape);
 553                                 break;
 554                         }
 555
 556
 557                 default:
 558                         {
 559                                 return 0;
 560                         }
 561                 }
 562         }
 563
 564         cinfo.m_MotionState = new DefaultMotionState();
 565
 566         CcdPhysicsController* replica = new CcdPhysicsController(cinfo);
 567         return replica;
 568 }
 569
 570 ///////////////////////////////////////////////////////////
 571 ///A small utility class, DefaultMotionState
 572 ///
 573 ///////////////////////////////////////////////////////////
 574
 575 DefaultMotionState::DefaultMotionState()
 576 {
 577         m_worldTransform.setIdentity();
 578         m_localScaling.setValue(1.f,1.f,1.f);
 579 }
 580
 581
 582 DefaultMotionState::~DefaultMotionState()
 583 {
 584
 585 }
 586
 587 void    DefaultMotionState::getWorldPosition(float& posX,float& posY,float& posZ)
 588 {
 589         posX = m_worldTransform.getOrigin().x();
 590         posY = m_worldTransform.getOrigin().y();
 591         posZ = m_worldTransform.getOrigin().z();
 592 }
 593
 594 void    DefaultMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ)
 595 {
 596         scaleX = m_localScaling.getX();
 597         scaleY = m_localScaling.getY();
 598         scaleZ = m_localScaling.getZ();
 599 }
 600
 601 void    DefaultMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal)
 602 {
 603         quatIma0 = m_worldTransform.getRotation().x();
 604         quatIma1 = m_worldTransform.getRotation().y();
 605         quatIma2 = m_worldTransform.getRotation().z();
 606         quatReal = m_worldTransform.getRotation()[3];
 607 }
 608
 609 void    DefaultMotionState::setWorldPosition(float posX,float posY,float posZ)
 610 {
 611         btPoint3 pos(posX,posY,posZ);
 612         m_worldTransform.setOrigin( pos );
 613 }
 614
 615 void    DefaultMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal)
 616 {
 617         btQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal);
 618         m_worldTransform.setRotation( orn );
 619 }
 620
 621 void    DefaultMotionState::calculateWorldTransformations()
 622 {
 623
 624 }
 625