source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.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
  18
  19 #include "CcdPhysicsEnvironment.h"
  20 #include "CcdPhysicsController.h"
  21 #include "CcdGraphicController.h"
  22
  23 #include <algorithm>
  24 #include "btBulletDynamicsCommon.h"
  25 #include "LinearMath/btIDebugDraw.h"
  26 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
  27 #include "BulletSoftBody/btSoftRigidDynamicsWorld.h"
  28 #include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
  29 #include "BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h"
  30
  31 //profiling/timings
  32 #include "LinearMath/btQuickprof.h"
  33
  34
  35 #include "PHY_IMotionState.h"
  36
  37 #define CCD_CONSTRAINT_DISABLE_LINKED_COLLISION 0x80
  38
  39 bool useIslands = true;
  40
  41 #ifdef NEW_BULLET_VEHICLE_SUPPORT
  42 #include "BulletDynamics/Vehicle/btRaycastVehicle.h"
  43 #include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
  44 #include "BulletDynamics/Vehicle/btWheelInfo.h"
  45 #include "PHY_IVehicle.h"
  46 btRaycastVehicle::btVehicleTuning       gTuning;
  47
  48 #endif //NEW_BULLET_VEHICLE_SUPPORT
  49 #include "LinearMath/btAabbUtil2.h"
  50
  51
  52 #ifdef WIN32
  53 void DrawRasterizerLine(const float* from,const float* to,int color);
  54 #endif
  55
  56
  57 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
  58
  59
  60 #include <stdio.h>
  61 #include <string.h>             // for memset
  62
  63 #ifdef NEW_BULLET_VEHICLE_SUPPORT
  64 class WrapperVehicle : public PHY_IVehicle
  65 {
  66
  67         btRaycastVehicle*       m_vehicle;
  68         PHY_IPhysicsController* m_chassis;
  69
  70 public:
  71
  72         WrapperVehicle(btRaycastVehicle* vehicle,PHY_IPhysicsController* chassis)
  73                 :m_vehicle(vehicle),
  74                 m_chassis(chassis)
  75         {
  76         }
  77
  78         btRaycastVehicle*       GetVehicle()
  79         {
  80                 return m_vehicle;
  81         }
  82
  83         PHY_IPhysicsController* GetChassis()
  84         {
  85                 return m_chassis;
  86         }
  87
  88         virtual void    AddWheel(
  89                 PHY_IMotionState*       motionState,
  90                 PHY__Vector3    connectionPoint,
  91                 PHY__Vector3    downDirection,
  92                 PHY__Vector3    axleDirection,
  93                 float   suspensionRestLength,
  94                 float wheelRadius,
  95                 bool hasSteering
  96                 )
  97         {
  98                 btVector3 connectionPointCS0(connectionPoint[0],connectionPoint[1],connectionPoint[2]);
  99                 btVector3 wheelDirectionCS0(downDirection[0],downDirection[1],downDirection[2]);
 100                 btVector3 wheelAxle(axleDirection[0],axleDirection[1],axleDirection[2]);
 101
 102
 103                 btWheelInfo& info = m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxle,
 104                         suspensionRestLength,wheelRadius,gTuning,hasSteering);
 105                 info.m_clientInfo = motionState;
 106
 107         }
 108
 109         void    SyncWheels()
 110         {
 111                 int numWheels = GetNumWheels();
 112                 int i;
 113                 for (i=0;i<numWheels;i++)
 114                 {
 115                         btWheelInfo& info = m_vehicle->getWheelInfo(i);
 116                         PHY_IMotionState* motionState = (PHY_IMotionState*)info.m_clientInfo ;
 117         //              m_vehicle->updateWheelTransformsWS(info,false);
 118                         m_vehicle->updateWheelTransform(i,false);
 119                         btTransform trans = m_vehicle->getWheelInfo(i).m_worldTransform;
 120                         btQuaternion orn = trans.getRotation();
 121                         const btVector3& pos = trans.getOrigin();
 122                         motionState->setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]);
 123                         motionState->setWorldPosition(pos.x(),pos.y(),pos.z());
 124
 125                 }
 126         }
 127
 128         virtual int             GetNumWheels() const
 129         {
 130                 return m_vehicle->getNumWheels();
 131         }
 132
 133         virtual void    GetWheelPosition(int wheelIndex,float& posX,float& posY,float& posZ) const
 134         {
 135                 btTransform     trans = m_vehicle->getWheelTransformWS(wheelIndex);
 136                 posX = trans.getOrigin().x();
 137                 posY = trans.getOrigin().y();
 138                 posZ = trans.getOrigin().z();
 139         }
 140         virtual void    GetWheelOrientationQuaternion(int wheelIndex,float& quatX,float& quatY,float& quatZ,float& quatW) const
 141         {
 142                 btTransform     trans = m_vehicle->getWheelTransformWS(wheelIndex);
 143                 btQuaternion quat = trans.getRotation();
 144                 btMatrix3x3 orn2(quat);
 145
 146                 quatX = trans.getRotation().x();
 147                 quatY = trans.getRotation().y();
 148                 quatZ = trans.getRotation().z();
 149                 quatW = trans.getRotation()[3];
 150
 151
 152                 //printf("test");
 153
 154
 155         }
 156
 157         virtual float   GetWheelRotation(int wheelIndex) const
 158         {
 159                 float rotation = 0.f;
 160
 161                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
 162                 {
 163                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
 164                         rotation = info.m_rotation;
 165                 }
 166                 return rotation;
 167
 168         }
 169
 170
 171
 172         virtual int     GetUserConstraintId() const
 173         {
 174                 return m_vehicle->getUserConstraintId();
 175         }
 176
 177         virtual int     GetUserConstraintType() const
 178         {
 179                 return m_vehicle->getUserConstraintType();
 180         }
 181
 182         virtual void    SetSteeringValue(float steering,int wheelIndex)
 183         {
 184                 m_vehicle->setSteeringValue(steering,wheelIndex);
 185         }
 186
 187         virtual void    ApplyEngineForce(float force,int wheelIndex)
 188         {
 189                 m_vehicle->applyEngineForce(force,wheelIndex);
 190         }
 191
 192         virtual void    ApplyBraking(float braking,int wheelIndex)
 193         {
 194                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
 195                 {
 196                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
 197                         info.m_brake = braking;
 198                 }
 199         }
 200
 201         virtual void    SetWheelFriction(float friction,int wheelIndex)
 202         {
 203                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
 204                 {
 205                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
 206                         info.m_frictionSlip = friction;
 207                 }
 208
 209         }
 210
 211         virtual void    SetSuspensionStiffness(float suspensionStiffness,int wheelIndex)
 212         {
 213                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
 214                 {
 215                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
 216                         info.m_suspensionStiffness = suspensionStiffness;
 217
 218                 }
 219         }
 220
 221         virtual void    SetSuspensionDamping(float suspensionDamping,int wheelIndex)
 222         {
 223                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
 224                 {
 225                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
 226                         info.m_wheelsDampingRelaxation = suspensionDamping;
 227                 }
 228         }
 229
 230         virtual void    SetSuspensionCompression(float suspensionCompression,int wheelIndex)
 231         {
 232                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
 233                 {
 234                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
 235                         info.m_wheelsDampingCompression = suspensionCompression;
 236                 }
 237         }
 238
 239
 240
 241         virtual void    SetRollInfluence(float rollInfluence,int wheelIndex)
 242         {
 243                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
 244                 {
 245                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
 246                         info.m_rollInfluence = rollInfluence;
 247                 }
 248         }
 249
 250         virtual void    SetCoordinateSystem(int rightIndex,int upIndex,int forwardIndex)
 251         {
 252                 m_vehicle->setCoordinateSystem(rightIndex,upIndex,forwardIndex);
 253         }
 254
 255
 256
 257 };
 258 #endif //NEW_BULLET_VEHICLE_SUPPORT
 259
 260 class CcdOverlapFilterCallBack : public btOverlapFilterCallback
 261 {
 262 private:
 263         class CcdPhysicsEnvironment* m_physEnv;
 264 public:
 265         CcdOverlapFilterCallBack(CcdPhysicsEnvironment* env) :
 266                 m_physEnv(env)
 267         {
 268         }
 269         virtual ~CcdOverlapFilterCallBack()
 270         {
 271         }
 272         // return true when pairs need collision
 273         virtual bool    needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const;
 274 };
 275
 276
 277 void CcdPhysicsEnvironment::setDebugDrawer(btIDebugDraw* debugDrawer)
 278 {
 279         if (debugDrawer && m_dynamicsWorld)
 280                 m_dynamicsWorld->setDebugDrawer(debugDrawer);
 281         m_debugDrawer = debugDrawer;
 282 }
 283
 284 static void DrawAabb(btIDebugDraw* debugDrawer,const btVector3& from,const btVector3& to,const btVector3& color)
 285 {
 286         btVector3 halfExtents = (to-from)* 0.5f;
 287         btVector3 center = (to+from) *0.5f;
 288         int i,j;
 289
 290         btVector3 edgecoord(1.f,1.f,1.f),pa,pb;
 291         for (i=0;i<4;i++)
 292         {
 293                 for (j=0;j<3;j++)
 294                 {
 295                         pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],
 296                                 edgecoord[2]*halfExtents[2]);
 297                         pa+=center;
 298
 299                         int othercoord = j%3;
 300                         edgecoord[othercoord]*=-1.f;
 301                         pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],
 302                                 edgecoord[2]*halfExtents[2]);
 303                         pb+=center;
 304
 305                         debugDrawer->drawLine(pa,pb,color);
 306                 }
 307                 edgecoord = btVector3(-1.f,-1.f,-1.f);
 308                 if (i<3)
 309                         edgecoord[i]*=-1.f;
 310         }
 311
 312
 313 }
 314
 315
 316
 317
 318
 319
 320 CcdPhysicsEnvironment::CcdPhysicsEnvironment(bool useDbvtCulling,btDispatcher* dispatcher,btOverlappingPairCache* pairCache)
 321 :m_cullingCache(NULL),
 322 m_cullingTree(NULL),
 323 m_numIterations(10),
 324 m_scalingPropagated(false),
 325 m_numTimeSubSteps(1),
 326 m_ccdMode(0),
 327 m_solverType(-1),
 328 m_profileTimings(0),
 329 m_enableSatCollisionDetection(false),
 330 m_solver(NULL),
 331 m_ownPairCache(NULL),
 332 m_filterCallback(NULL),
 333 m_ownDispatcher(NULL)
 334 {
 335
 336         for (int i=0;i<PHY_NUM_RESPONSE;i++)
 337         {
 338                 m_triggerCallbacks[i] = 0;
 339         }
 340
 341 //      m_collisionConfiguration = new btDefaultCollisionConfiguration();
 342         m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
 343         //m_collisionConfiguration->setConvexConvexMultipointIterations();
 344
 345         if (!dispatcher)
 346         {
 347                 btCollisionDispatcher* disp = new btCollisionDispatcher(m_collisionConfiguration);
 348                 dispatcher = disp;
 349                 btGImpactCollisionAlgorithm::registerAlgorithm(disp);
 350                 m_ownDispatcher = dispatcher;
 351         }
 352
 353         //m_broadphase = new btAxisSweep3(btVector3(-1000,-1000,-1000),btVector3(1000,1000,1000));
 354         //m_broadphase = new btSimpleBroadphase();
 355         m_broadphase = new btDbvtBroadphase();
 356         // avoid any collision in the culling tree
 357         if (useDbvtCulling) {
 358                 m_cullingCache = new btNullPairCache();
 359                 m_cullingTree = new btDbvtBroadphase(m_cullingCache);
 360         }
 361
 362         m_filterCallback = new CcdOverlapFilterCallBack(this);
 363         m_broadphase->getOverlappingPairCache()->setOverlapFilterCallback(m_filterCallback);
 364
 365         setSolverType(1);//issues with quickstep and memory allocations
 366 //      m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
 367         m_dynamicsWorld = new btSoftRigidDynamicsWorld(dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
 368         //m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.01f;
 369         //m_dynamicsWorld->getSolverInfo().m_solverMode=        SOLVER_USE_WARMSTARTING +       SOLVER_USE_2_FRICTION_DIRECTIONS +      SOLVER_RANDMIZE_ORDER + SOLVER_USE_FRICTION_WARMSTARTING;
 370
 371         m_debugDrawer = 0;
 372         m_gravity = btVector3(0.f,-10.f,0.f);
 373         m_dynamicsWorld->setGravity(m_gravity);
 374
 375 }
 376
 377 void    CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
 378 {
 379         btRigidBody* body = ctrl->GetRigidBody();
 380         btCollisionObject* obj = ctrl->GetCollisionObject();
 381
 382         //this m_userPointer is just used for triggers, see CallbackTriggers
 383         obj->setUserPointer(ctrl);
 384         if (body)
 385                 body->setGravity( m_gravity );
 386
 387         m_controllers.insert(ctrl);
 388
 389         if (body)
 390         {
 391                 //use explicit group/filter for finer control over collision in bullet => near/radar sensor
 392                 m_dynamicsWorld->addRigidBody(body, ctrl->GetCollisionFilterGroup(), ctrl->GetCollisionFilterMask());
 393         } else
 394         {
 395                 if (ctrl->GetSoftBody())
 396                 {
 397                         btSoftBody* softBody = ctrl->GetSoftBody();
 398                         m_dynamicsWorld->addSoftBody(softBody);
 399                 } else
 400                 {
 401                         if (obj->getCollisionShape())
 402                         {
 403                                 m_dynamicsWorld->addCollisionObject(obj,ctrl->GetCollisionFilterGroup(), ctrl->GetCollisionFilterMask());
 404                         }
 405                 }
 406         }
 407         if (obj->isStaticOrKinematicObject())
 408         {
 409                 obj->setActivationState(ISLAND_SLEEPING);
 410         }
 411
 412
 413         //CollisionObject(body,ctrl->GetCollisionFilterGroup(),ctrl->GetCollisionFilterMask());
 414
 415         assert(obj->getBroadphaseHandle());
 416
 417         btBroadphaseInterface* scene =  getBroadphase();
 418
 419
 420         btCollisionShape* shapeinterface = ctrl->GetCollisionShape();
 421
 422         assert(shapeinterface);
 423
 424         const btTransform& t = ctrl->GetCollisionObject()->getWorldTransform();
 425
 426
 427         btVector3 minAabb,maxAabb;
 428
 429         shapeinterface->getAabb(t,minAabb,maxAabb);
 430
 431         float timeStep = 0.02f;
 432
 433
 434         //extent it with the motion
 435
 436         if (body)
 437         {
 438                 btVector3 linMotion = body->getLinearVelocity()*timeStep;
 439
 440                 float maxAabbx = maxAabb.getX();
 441                 float maxAabby = maxAabb.getY();
 442                 float maxAabbz = maxAabb.getZ();
 443                 float minAabbx = minAabb.getX();
 444                 float minAabby = minAabb.getY();
 445                 float minAabbz = minAabb.getZ();
 446
 447                 if (linMotion.x() > 0.f)
 448                         maxAabbx += linMotion.x();
 449                 else
 450                         minAabbx += linMotion.x();
 451                 if (linMotion.y() > 0.f)
 452                         maxAabby += linMotion.y();
 453                 else
 454                         minAabby += linMotion.y();
 455                 if (linMotion.z() > 0.f)
 456                         maxAabbz += linMotion.z();
 457                 else
 458                         minAabbz += linMotion.z();
 459
 460
 461                 minAabb = btVector3(minAabbx,minAabby,minAabbz);
 462                 maxAabb = btVector3(maxAabbx,maxAabby,maxAabbz);
 463         }
 464
 465
 466
 467 }
 468
 469
 470
 471 void    CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctrl)
 472 {
 473         //also remove constraint
 474         btRigidBody* body = ctrl->GetRigidBody();
 475         if (body)
 476         {
 477                 m_dynamicsWorld->removeRigidBody(ctrl->GetRigidBody());
 478         } else
 479         {
 480                 //if a softbody
 481                 if (ctrl->GetSoftBody())
 482                 {
 483                         m_dynamicsWorld->removeSoftBody(ctrl->GetSoftBody());
 484                 } else
 485                 {
 486                         m_dynamicsWorld->removeCollisionObject(ctrl->GetCollisionObject());
 487                 }
 488         }
 489         m_controllers.erase(ctrl);
 490
 491         if (ctrl->m_registerCount != 0)
 492                 printf("Warning: removing controller with non-zero m_registerCount: %d\n", ctrl->m_registerCount);
 493
 494         //remove it from the triggers
 495         m_triggerControllers.erase(ctrl);
 496 }
 497
 498 void    CcdPhysicsEnvironment::updateCcdPhysicsController(CcdPhysicsController* ctrl, btScalar newMass, int newCollisionFlags, short int newCollisionGroup, short int newCollisionMask)
 499 {
 500         // this function is used when the collisionning group of a controller is changed
 501         // remove and add the collistioning object
 502         btRigidBody* body = ctrl->GetRigidBody();
 503         btCollisionObject* obj = ctrl->GetCollisionObject();
 504         if (obj)
 505         {
 506                 btVector3 inertia(0.0,0.0,0.0);
 507                 m_dynamicsWorld->removeCollisionObject(obj);
 508                 obj->setCollisionFlags(newCollisionFlags);
 509                 if (body)
 510                 {
 511                         if (newMass)
 512                                 body->getCollisionShape()->calculateLocalInertia(newMass, inertia);
 513                         body->setMassProps(newMass, inertia);
 514                 }
 515                 m_dynamicsWorld->addCollisionObject(obj, newCollisionGroup, newCollisionMask);
 516         }
 517         // to avoid nasty interaction, we must update the property of the controller as well
 518         ctrl->m_cci.m_mass = newMass;
 519         ctrl->m_cci.m_collisionFilterGroup = newCollisionGroup;
 520         ctrl->m_cci.m_collisionFilterMask = newCollisionMask;
 521         ctrl->m_cci.m_collisionFlags = newCollisionFlags;
 522 }
 523
 524 void CcdPhysicsEnvironment::enableCcdPhysicsController(CcdPhysicsController* ctrl)
 525 {
 526         if (m_controllers.insert(ctrl).second)
 527         {
 528                 btCollisionObject* obj = ctrl->GetCollisionObject();
 529                 obj->setUserPointer(ctrl);
 530                 m_dynamicsWorld->addCollisionObject(obj,
 531                         ctrl->GetCollisionFilterGroup(), ctrl->GetCollisionFilterMask());
 532         }
 533 }
 534
 535 void CcdPhysicsEnvironment::disableCcdPhysicsController(CcdPhysicsController* ctrl)
 536 {
 537         if (m_controllers.erase(ctrl))
 538         {
 539                 btRigidBody* body = ctrl->GetRigidBody();
 540                 if (body)
 541                 {
 542                         m_dynamicsWorld->removeRigidBody(body);
 543                 } else
 544                 {
 545                         if (ctrl->GetSoftBody())
 546                         {
 547                         } else
 548                         {
 549                                 m_dynamicsWorld->removeCollisionObject(ctrl->GetCollisionObject());
 550                         }
 551                 }
 552         }
 553 }
 554
 555 void CcdPhysicsEnvironment::refreshCcdPhysicsController(CcdPhysicsController* ctrl)
 556 {
 557         btCollisionObject* obj = ctrl->GetCollisionObject();
 558         if (obj)
 559         {
 560                 btBroadphaseProxy* proxy = obj->getBroadphaseHandle();
 561                 if (proxy)
 562                 {
 563                         m_dynamicsWorld->getPairCache()->cleanProxyFromPairs(proxy,m_dynamicsWorld->getDispatcher());
 564                 }
 565         }
 566 }
 567
 568 void CcdPhysicsEnvironment::addCcdGraphicController(CcdGraphicController* ctrl)
 569 {
 570         if (m_cullingTree)
 571         {
 572                 btVector3       minAabb;
 573                 btVector3       maxAabb;
 574                 ctrl->getAabb(minAabb, maxAabb);
 575
 576                 ctrl->setBroadphaseHandle(m_cullingTree->createProxy(
 577                                 minAabb,
 578                                 maxAabb,
 579                                 INVALID_SHAPE_PROXYTYPE,        // this parameter is not used
 580                                 ctrl,
 581                                 0,                                                      // this object does not collision with anything
 582                                 0,
 583                                 NULL,                                           // dispatcher => this parameter is not used
 584                                 0));
 585
 586                 assert(ctrl->getBroadphaseHandle());
 587         }
 588 }
 589
 590 void CcdPhysicsEnvironment::removeCcdGraphicController(CcdGraphicController* ctrl)
 591 {
 592         if (m_cullingTree)
 593         {
 594                 btBroadphaseProxy* bp = ctrl->getBroadphaseHandle();
 595                 if (bp)
 596                 {
 597                         m_cullingTree->destroyProxy(bp,NULL);
 598                         ctrl->setBroadphaseHandle(0);
 599                 }
 600         }
 601 }
 602
 603 void    CcdPhysicsEnvironment::beginFrame()
 604 {
 605
 606 }
 607
 608 void CcdPhysicsEnvironment::debugDrawWorld()
 609 {
 610         if (m_dynamicsWorld->getDebugDrawer() &&  m_dynamicsWorld->getDebugDrawer()->getDebugMode() >0)
 611                         m_dynamicsWorld->debugDrawWorld();
 612 }
 613
 614 bool    CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
 615 {
 616         std::set<CcdPhysicsController*>::iterator it;
 617         int i;
 618
 619         for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
 620         {
 621                 (*it)->SynchronizeMotionStates(timeStep);
 622         }
 623
 624         processFhSprings(curTime,timeStep);
 625
 626         float subStep = timeStep / float(m_numTimeSubSteps);
 627         for (i=0;i<m_numTimeSubSteps;i++)
 628         {
 629 //                      m_dynamicsWorld->stepSimulation(subStep,20,1./240.);//perform always a full simulation step
 630                         m_dynamicsWorld->stepSimulation(subStep,0);//perform always a full simulation step
 631         }
 632
 633         for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
 634         {
 635                 (*it)->SynchronizeMotionStates(timeStep);
 636         }
 637
 638         //for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
 639         //{
 640         //      (*it)->SynchronizeMotionStates(timeStep);
 641         //}
 642
 643         for (i=0;i<m_wrapperVehicles.size();i++)
 644         {
 645                 WrapperVehicle* veh = m_wrapperVehicles[i];
 646                 veh->SyncWheels();
 647         }
 648
 649
 650         CallbackTriggers();
 651
 652         return true;
 653 }
 654
 655 class ClosestRayResultCallbackNotMe : public btCollisionWorld::ClosestRayResultCallback
 656 {
 657         btCollisionObject* m_owner;
 658         btCollisionObject* m_parent;
 659
 660 public:
 661         ClosestRayResultCallbackNotMe(const btVector3& rayFromWorld,const btVector3& rayToWorld,btCollisionObject* owner,btCollisionObject* parent)
 662                 :btCollisionWorld::ClosestRayResultCallback(rayFromWorld,rayToWorld),
 663                 m_owner(owner),
 664                 m_parent(parent)
 665         {
 666
 667         }
 668
 669         virtual bool needsCollision(btBroadphaseProxy* proxy0) const
 670         {
 671                 //don't collide with self
 672                 if (proxy0->m_clientObject == m_owner)
 673                         return false;
 674
 675                 if (proxy0->m_clientObject == m_parent)
 676                         return false;
 677
 678                 return btCollisionWorld::ClosestRayResultCallback::needsCollision(proxy0);
 679         }
 680
 681 };
 682
 683 void    CcdPhysicsEnvironment::processFhSprings(double curTime,float timeStep)
 684 {
 685         std::set<CcdPhysicsController*>::iterator it;
 686
 687         for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
 688         {
 689                 CcdPhysicsController* ctrl = (*it);
 690                 btRigidBody* body = ctrl->GetRigidBody();
 691
 692                 if (body && (ctrl->getConstructionInfo().m_do_fh || ctrl->getConstructionInfo().m_do_rot_fh))
 693                 {
 694                         //printf("has Fh or RotFh\n");
 695                         //re-implement SM_FhObject.cpp using btCollisionWorld::rayTest and info from ctrl->getConstructionInfo()
 696                         //send a ray from {0.0, 0.0, 0.0} towards {0.0, 0.0, -10.0}, in local coordinates
 697
 698
 699                         CcdPhysicsController* parentCtrl = ctrl->getParentCtrl();
 700                         btRigidBody* parentBody = parentCtrl?parentCtrl->GetRigidBody() : 0;
 701                         btRigidBody* cl_object = parentBody ? parentBody : body;
 702
 703                         if (body->isStaticOrKinematicObject())
 704                                 continue;
 705
 706                         btVector3 rayDirLocal(0,0,-10);
 707
 708                         //m_dynamicsWorld
 709                         //ctrl->GetRigidBody();
 710                         btVector3 rayFromWorld = body->getCenterOfMassPosition();
 711                         //btVector3     rayToWorld = rayFromWorld + body->getCenterOfMassTransform().getBasis() * rayDirLocal;
 712                         //ray always points down the z axis in world space...
 713                         btVector3       rayToWorld = rayFromWorld + rayDirLocal;
 714
 715                         ClosestRayResultCallbackNotMe   resultCallback(rayFromWorld,rayToWorld,body,parentBody);
 716
 717                         m_dynamicsWorld->rayTest(rayFromWorld,rayToWorld,resultCallback);
 718                         if (resultCallback.hasHit())
 719                         {
 720                                 //we hit this one: resultCallback.m_collisionObject;
 721                                 CcdPhysicsController* controller = static_cast<CcdPhysicsController*>(resultCallback.m_collisionObject->getUserPointer());
 722
 723                                 if (controller)
 724                                 {
 725                                         if (controller->getConstructionInfo().m_fh_distance < SIMD_EPSILON)
 726                                                 continue;
 727
 728                                         btRigidBody* hit_object = controller->GetRigidBody();
 729                                         if (!hit_object)
 730                                                 continue;
 731
 732                                         CcdConstructionInfo& hitObjShapeProps = controller->getConstructionInfo();
 733
 734                                         float distance = resultCallback.m_closestHitFraction*rayDirLocal.length()-ctrl->getConstructionInfo().m_radius;
 735                                         if (distance >= hitObjShapeProps.m_fh_distance)
 736                                                 continue;
 737
 738
 739
 740                                         //btVector3 ray_dir = cl_object->getCenterOfMassTransform().getBasis()* rayDirLocal.normalized();
 741                                         btVector3 ray_dir = rayDirLocal.normalized();
 742                                         btVector3 normal = resultCallback.m_hitNormalWorld;
 743                                         normal.normalize();
 744
 745
 746                                         if (ctrl->getConstructionInfo().m_do_fh)
 747                                         {
 748                                                 btVector3 lspot = cl_object->getCenterOfMassPosition()
 749                                                         + rayDirLocal * resultCallback.m_closestHitFraction;
 750
 751
 752
 753
 754                                                 lspot -= hit_object->getCenterOfMassPosition();
 755                                                 btVector3 rel_vel = cl_object->getLinearVelocity() - hit_object->getVelocityInLocalPoint(lspot);
 756                                                 btScalar rel_vel_ray = ray_dir.dot(rel_vel);
 757                                                 btScalar spring_extent = 1.0 - distance / hitObjShapeProps.m_fh_distance;
 758
 759                                                 btScalar i_spring = spring_extent * hitObjShapeProps.m_fh_spring;
 760                                                 btScalar i_damp =   rel_vel_ray * hitObjShapeProps.m_fh_damping;
 761
 762                                                 cl_object->setLinearVelocity(cl_object->getLinearVelocity() + (-(i_spring + i_damp) * ray_dir));
 763                                                 if (hitObjShapeProps.m_fh_normal)
 764                                                 {
 765                                                         cl_object->setLinearVelocity(cl_object->getLinearVelocity()+(i_spring + i_damp) *(normal - normal.dot(ray_dir) * ray_dir));
 766                                                 }
 767
 768                                                 btVector3 lateral = rel_vel - rel_vel_ray * ray_dir;
 769
 770
 771                                                 if (ctrl->getConstructionInfo().m_do_anisotropic) {
 772                                                         //Bullet basis contains no scaling/shear etc.
 773                                                         const btMatrix3x3& lcs = cl_object->getCenterOfMassTransform().getBasis();
 774                                                         btVector3 loc_lateral = lateral * lcs;
 775                                                         const btVector3& friction_scaling = cl_object->getAnisotropicFriction();
 776                                                         loc_lateral *= friction_scaling;
 777                                                         lateral = lcs * loc_lateral;
 778                                                 }
 779
 780                                                 btScalar rel_vel_lateral = lateral.length();
 781
 782                                                 if (rel_vel_lateral > SIMD_EPSILON) {
 783                                                         btScalar friction_factor = hit_object->getFriction();//cl_object->getFriction();
 784
 785                                                         btScalar max_friction = friction_factor * btMax(btScalar(0.0), i_spring);
 786
 787                                                         btScalar rel_mom_lateral = rel_vel_lateral / cl_object->getInvMass();
 788
 789                                                         btVector3 friction = (rel_mom_lateral > max_friction) ?
 790                                                                 -lateral * (max_friction / rel_vel_lateral) :
 791                                                                 -lateral;
 792
 793                                                                 cl_object->applyCentralImpulse(friction);
 794                                                 }
 795                                         }
 796
 797
 798                                         if (ctrl->getConstructionInfo().m_do_rot_fh) {
 799                                                 btVector3 up2 = cl_object->getWorldTransform().getBasis().getColumn(2);
 800
 801                                                 btVector3 t_spring = up2.cross(normal) * hitObjShapeProps.m_fh_spring;
 802                                                 btVector3 ang_vel = cl_object->getAngularVelocity();
 803
 804                                                 // only rotations that tilt relative to the normal are damped
 805                                                 ang_vel -= ang_vel.dot(normal) * normal;
 806
 807                                                 btVector3 t_damp = ang_vel * hitObjShapeProps.m_fh_damping;
 808
 809                                                 cl_object->setAngularVelocity(cl_object->getAngularVelocity() + (t_spring - t_damp));
 810                                         }
 811
 812                                 }
 813
 814
 815                         }
 816
 817
 818                 }
 819         }
 820
 821 }
 822
 823 void            CcdPhysicsEnvironment::setDebugMode(int debugMode)
 824 {
 825         if (m_debugDrawer){
 826                 m_debugDrawer->setDebugMode(debugMode);
 827         }
 828 }
 829
 830 void            CcdPhysicsEnvironment::setNumIterations(int numIter)
 831 {
 832         m_numIterations = numIter;
 833 }
 834 void            CcdPhysicsEnvironment::setDeactivationTime(float dTime)
 835 {
 836         gDeactivationTime = dTime;
 837 }
 838 void            CcdPhysicsEnvironment::setDeactivationLinearTreshold(float linTresh)
 839 {
 840         gLinearSleepingTreshold = linTresh;
 841 }
 842 void            CcdPhysicsEnvironment::setDeactivationAngularTreshold(float angTresh)
 843 {
 844         gAngularSleepingTreshold = angTresh;
 845 }
 846
 847 void            CcdPhysicsEnvironment::setContactBreakingTreshold(float contactBreakingTreshold)
 848 {
 849         gContactBreakingThreshold = contactBreakingTreshold;
 850
 851 }
 852
 853
 854 void            CcdPhysicsEnvironment::setCcdMode(int ccdMode)
 855 {
 856         m_ccdMode = ccdMode;
 857 }
 858
 859
 860 void            CcdPhysicsEnvironment::setSolverSorConstant(float sor)
 861 {
 862         m_solverInfo.m_sor = sor;
 863 }
 864
 865 void            CcdPhysicsEnvironment::setSolverTau(float tau)
 866 {
 867         m_solverInfo.m_tau = tau;
 868 }
 869 void            CcdPhysicsEnvironment::setSolverDamping(float damping)
 870 {
 871         m_solverInfo.m_damping = damping;
 872 }
 873
 874
 875 void            CcdPhysicsEnvironment::setLinearAirDamping(float damping)
 876 {
 877         //gLinearAirDamping = damping;
 878 }
 879
 880 void            CcdPhysicsEnvironment::setUseEpa(bool epa)
 881 {
 882         //gUseEpa = epa;
 883 }
 884
 885 void            CcdPhysicsEnvironment::setSolverType(int solverType)
 886 {
 887
 888         switch (solverType)
 889         {
 890         case 1:
 891                 {
 892                         if (m_solverType != solverType)
 893                         {
 894
 895                                 m_solver = new btSequentialImpulseConstraintSolver();
 896
 897
 898                                 break;
 899                         }
 900                 }
 901
 902         case 0:
 903         default:
 904                 if (m_solverType != solverType)
 905                 {
 906 //                      m_solver = new OdeConstraintSolver();
 907
 908                         break;
 909                 }
 910
 911         };
 912
 913         m_solverType = solverType ;
 914 }
 915
 916
 917
 918 void            CcdPhysicsEnvironment::getGravity(PHY__Vector3& grav)
 919 {
 920                 const btVector3& gravity = m_dynamicsWorld->getGravity();
 921                 grav[0] = gravity.getX();
 922                 grav[1] = gravity.getY();
 923                 grav[2] = gravity.getZ();
 924 }
 925
 926
 927 void            CcdPhysicsEnvironment::setGravity(float x,float y,float z)
 928 {
 929         m_gravity = btVector3(x,y,z);
 930         m_dynamicsWorld->setGravity(m_gravity);
 931
 932 }
 933
 934
 935
 936
 937 static int gConstraintUid = 1;
 938
 939 //Following the COLLADA physics specification for constraints
 940 int                     CcdPhysicsEnvironment::createUniversalD6Constraint(
 941                                                 class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
 942                                                 btTransform& frameInA,
 943                                                 btTransform& frameInB,
 944                                                 const btVector3& linearMinLimits,
 945                                                 const btVector3& linearMaxLimits,
 946                                                 const btVector3& angularMinLimits,
 947                                                 const btVector3& angularMaxLimits,int flags
 948 )
 949 {
 950
 951         bool disableCollisionBetweenLinkedBodies = (0!=(flags & CCD_CONSTRAINT_DISABLE_LINKED_COLLISION));
 952
 953         //we could either add some logic to recognize ball-socket and hinge, or let that up to the user
 954         //perhaps some warning or hint that hinge/ball-socket is more efficient?
 955
 956
 957         btGeneric6DofConstraint* genericConstraint = 0;
 958         CcdPhysicsController* ctrl0 = (CcdPhysicsController*) ctrlRef;
 959         CcdPhysicsController* ctrl1 = (CcdPhysicsController*) ctrlOther;
 960
 961         btRigidBody* rb0 = ctrl0->GetRigidBody();
 962         btRigidBody* rb1 = ctrl1->GetRigidBody();
 963
 964         if (rb1)
 965         {
 966
 967
 968                 bool useReferenceFrameA = true;
 969                 genericConstraint = new btGeneric6DofConstraint(
 970                         *rb0,*rb1,
 971                         frameInA,frameInB,useReferenceFrameA);
 972                 genericConstraint->setLinearLowerLimit(linearMinLimits);
 973                 genericConstraint->setLinearUpperLimit(linearMaxLimits);
 974                 genericConstraint->setAngularLowerLimit(angularMinLimits);
 975                 genericConstraint->setAngularUpperLimit(angularMaxLimits);
 976         } else
 977         {
 978                 // TODO: Implement single body case...
 979                 //No, we can use a fixed rigidbody in above code, rather then unnecessary duplation of code
 980
 981         }
 982
 983         if (genericConstraint)
 984         {
 985         //      m_constraints.push_back(genericConstraint);
 986                 m_dynamicsWorld->addConstraint(genericConstraint,disableCollisionBetweenLinkedBodies);
 987
 988                 genericConstraint->setUserConstraintId(gConstraintUid++);
 989                 genericConstraint->setUserConstraintType(PHY_GENERIC_6DOF_CONSTRAINT);
 990                 //64 bit systems can't cast pointer to int. could use size_t instead.
 991                 return genericConstraint->getUserConstraintId();
 992         }
 993         return 0;
 994 }
 995
 996
 997
 998 void            CcdPhysicsEnvironment::removeConstraint(int     constraintId)
 999 {
1000
1001         int i;
1002         int numConstraints = m_dynamicsWorld->getNumConstraints();
1003         for (i=0;i<numConstraints;i++)
1004         {
1005                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
1006                 if (constraint->getUserConstraintId() == constraintId)
1007                 {
1008                         constraint->getRigidBodyA().activate();
1009                         constraint->getRigidBodyB().activate();
1010                         m_dynamicsWorld->removeConstraint(constraint);
1011                         break;
1012                 }
1013         }
1014 }
1015
1016
1017 struct  FilterClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
1018 {
1019         PHY_IRayCastFilterCallback&     m_phyRayFilter;
1020         const btCollisionShape*         m_hitTriangleShape;
1021         int                                                     m_hitTriangleIndex;
1022
1023         FilterClosestRayResultCallback (PHY_IRayCastFilterCallback& phyRayFilter,const btVector3& rayFrom,const btVector3& rayTo)
1024                 : btCollisionWorld::ClosestRayResultCallback(rayFrom,rayTo),
1025                 m_phyRayFilter(phyRayFilter),
1026                 m_hitTriangleShape(NULL),
1027                 m_hitTriangleIndex(0)
1028         {
1029         }
1030
1031         virtual ~FilterClosestRayResultCallback()
1032         {
1033         }
1034
1035         virtual bool needsCollision(btBroadphaseProxy* proxy0) const
1036         {
1037                 if (!(proxy0->m_collisionFilterGroup & m_collisionFilterMask))
1038                         return false;
1039                 if (!(m_collisionFilterGroup & proxy0->m_collisionFilterMask))
1040                         return false;
1041                 btCollisionObject* object = (btCollisionObject*)proxy0->m_clientObject;
1042                 CcdPhysicsController* phyCtrl = static_cast<CcdPhysicsController*>(object->getUserPointer());
1043                 if (phyCtrl == m_phyRayFilter.m_ignoreController)
1044                         return false;
1045                 return m_phyRayFilter.needBroadphaseRayCast(phyCtrl);
1046         }
1047
1048         virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace)
1049         {
1050                 CcdPhysicsController* curHit = static_cast<CcdPhysicsController*>(rayResult.m_collisionObject->getUserPointer());
1051                 // save shape information as ClosestRayResultCallback::AddSingleResult() does not do it
1052                 if (rayResult.m_localShapeInfo)
1053                 {
1054                         m_hitTriangleShape = rayResult.m_collisionObject->getCollisionShape();
1055                         m_hitTriangleIndex = rayResult.m_localShapeInfo->m_triangleIndex;
1056                 } else
1057                 {
1058                         m_hitTriangleShape = NULL;
1059                         m_hitTriangleIndex = 0;
1060                 }
1061                 return ClosestRayResultCallback::addSingleResult(rayResult,normalInWorldSpace);
1062         }
1063
1064 };
1065
1066 PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IRayCastFilterCallback &filterCallback, float fromX,float fromY,float fromZ, float toX,float toY,float toZ)
1067 {
1068
1069
1070         float minFraction = 1.f;
1071
1072         btVector3 rayFrom(fromX,fromY,fromZ);
1073         btVector3 rayTo(toX,toY,toZ);
1074
1075         btVector3       hitPointWorld,normalWorld;
1076
1077         //Either Ray Cast with or without filtering
1078
1079         //btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo);
1080         FilterClosestRayResultCallback   rayCallback(filterCallback,rayFrom,rayTo);
1081
1082
1083         PHY_RayCastResult result;
1084         memset(&result, 0, sizeof(result));
1085
1086         // don't collision with sensor object
1087         rayCallback.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
1088         //, ,filterCallback.m_faceNormal);
1089
1090         m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback);
1091         if (rayCallback.hasHit())
1092         {
1093                 CcdPhysicsController* controller = static_cast<CcdPhysicsController*>(rayCallback.m_collisionObject->getUserPointer());
1094                 result.m_controller = controller;
1095                 result.m_hitPoint[0] = rayCallback.m_hitPointWorld.getX();
1096                 result.m_hitPoint[1] = rayCallback.m_hitPointWorld.getY();
1097                 result.m_hitPoint[2] = rayCallback.m_hitPointWorld.getZ();
1098
1099                 if (rayCallback.m_hitTriangleShape != NULL)
1100                 {
1101                         // identify the mesh polygon
1102                         CcdShapeConstructionInfo* shapeInfo = controller->m_shapeInfo;
1103                         if (shapeInfo)
1104                         {
1105                                 btCollisionShape* shape = controller->GetCollisionObject()->getCollisionShape();
1106                                 if (shape->isCompound())
1107                                 {
1108                                         btCompoundShape* compoundShape = (btCompoundShape*)shape;
1109                                         CcdShapeConstructionInfo* compoundShapeInfo = shapeInfo;
1110                                         // need to search which sub-shape has been hit
1111                                         for (int i=0; i<compoundShape->getNumChildShapes(); i++)
1112                                         {
1113                                                 shapeInfo = compoundShapeInfo->GetChildShape(i);
1114                                                 shape=compoundShape->getChildShape(i);
1115                                                 if (shape == rayCallback.m_hitTriangleShape)
1116                                                         break;
1117                                         }
1118                                 }
1119                                 if (shape == rayCallback.m_hitTriangleShape &&
1120                                         rayCallback.m_hitTriangleIndex < shapeInfo->m_polygonIndexArray.size())
1121                                 {
1122                                         result.m_meshObject = shapeInfo->GetMesh();
1123                                         result.m_polygon = shapeInfo->m_polygonIndexArray.at(rayCallback.m_hitTriangleIndex);
1124
1125                                         // Bullet returns the normal from "outside".
1126                                         // If the user requests the real normal, compute it now
1127                     if (filterCallback.m_faceNormal)
1128                                         {
1129                                                 // mesh shapes are shared and stored in the shapeInfo
1130                                                 btTriangleMeshShape* triangleShape = shapeInfo->GetMeshShape();
1131                                                 if (triangleShape)
1132                                                 {
1133                                                         // this code is copied from Bullet
1134                                                         btVector3 triangle[3];
1135                                                         const unsigned char *vertexbase;
1136                                                         int numverts;
1137                                                         PHY_ScalarType type;
1138                                                         int stride;
1139                                                         const unsigned char *indexbase;
1140                                                         int indexstride;
1141                                                         int numfaces;
1142                                                         PHY_ScalarType indicestype;
1143                                                         btStridingMeshInterface* meshInterface = triangleShape->getMeshInterface();
1144
1145                                                         meshInterface->getLockedReadOnlyVertexIndexBase(
1146                                                                 &vertexbase,
1147                                                                 numverts,
1148                                                                 type,
1149                                                                 stride,
1150                                                                 &indexbase,
1151                                                                 indexstride,
1152                                                                 numfaces,
1153                                                                 indicestype,
1154                                                                 0);
1155
1156                                                         unsigned int* gfxbase = (unsigned int*)(indexbase+rayCallback.m_hitTriangleIndex*indexstride);
1157                                                         const btVector3& meshScaling = shape->getLocalScaling();
1158                                                         for (int j=2;j>=0;j--)
1159                                                         {
1160                                                                 int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
1161
1162                                                                 btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride);
1163
1164                                                                 triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());
1165                                                         }
1166                                                         meshInterface->unLockReadOnlyVertexBase(0);
1167                                                         btVector3 triangleNormal;
1168                                                         triangleNormal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]);
1169                                                         rayCallback.m_hitNormalWorld = rayCallback.m_collisionObject->getWorldTransform().getBasis()*triangleNormal;
1170                                                 }
1171                                         }
1172                                 }
1173                         }
1174                 }
1175                 if (rayCallback.m_hitNormalWorld.length2() > (SIMD_EPSILON*SIMD_EPSILON))
1176                 {
1177                         rayCallback.m_hitNormalWorld.normalize();
1178                 } else
1179                 {
1180                         rayCallback.m_hitNormalWorld.setValue(1,0,0);
1181                 }
1182                 result.m_hitNormal[0] = rayCallback.m_hitNormalWorld.getX();
1183                 result.m_hitNormal[1] = rayCallback.m_hitNormalWorld.getY();
1184                 result.m_hitNormal[2] = rayCallback.m_hitNormalWorld.getZ();
1185                 filterCallback.reportHit(&result);
1186         }
1187
1188
1189         return result.m_controller;
1190 }
1191
1192 struct  DbvtCullingCallback : btDbvt::ICollide
1193 {
1194         PHY_CullingCallback m_clientCallback;
1195         void* m_userData;
1196
1197         DbvtCullingCallback(PHY_CullingCallback clientCallback, void* userData)
1198         {
1199                 m_clientCallback = clientCallback;
1200                 m_userData = userData;
1201         }
1202
1203         void Process(const btDbvtNode* node,btScalar depth)
1204         {
1205                 Process(node);
1206         }
1207         void Process(const btDbvtNode* leaf)
1208         {
1209                 btBroadphaseProxy*      proxy=(btBroadphaseProxy*)leaf->data;
1210                 // the client object is a graphic controller
1211                 CcdGraphicController* ctrl = static_cast<CcdGraphicController*>(proxy->m_clientObject);
1212                 KX_ClientObjectInfo* info = (KX_ClientObjectInfo*)ctrl->getNewClientInfo();
1213                 if (info)
1214                         (*m_clientCallback)(info, m_userData);
1215         }
1216 };
1217
1218 bool CcdPhysicsEnvironment::cullingTest(PHY_CullingCallback callback, void* userData, PHY__Vector4 *planes, int nplanes)
1219 {
1220         if (!m_cullingTree)
1221                 return false;
1222         DbvtCullingCallback dispatcher(callback, userData);
1223         btVector3 planes_n[5];
1224         btScalar planes_o[5];
1225         if (nplanes > 5)
1226                 nplanes = 5;
1227         for (int i=0; i<nplanes; i++)
1228         {
1229                 planes_n[i].setValue(planes[i][0], planes[i][1], planes[i][2]);
1230                 planes_o[i] = planes[i][3];
1231         }
1232         btDbvt::collideKDOP(m_cullingTree->m_sets[1].m_root,planes_n,planes_o,nplanes,dispatcher);
1233         btDbvt::collideKDOP(m_cullingTree->m_sets[0].m_root,planes_n,planes_o,nplanes,dispatcher);
1234         return true;
1235 }
1236
1237
1238 int     CcdPhysicsEnvironment::getNumContactPoints()
1239 {
1240         return 0;
1241 }
1242
1243 void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
1244 {
1245
1246 }
1247
1248
1249
1250
1251 btBroadphaseInterface*  CcdPhysicsEnvironment::getBroadphase()
1252 {
1253         return m_dynamicsWorld->getBroadphase();
1254 }
1255
1256 btDispatcher*   CcdPhysicsEnvironment::getDispatcher()
1257 {
1258         return m_dynamicsWorld->getDispatcher();
1259 }
1260
1261
1262
1263
1264
1265
1266 CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
1267 {
1268
1269 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1270         m_wrapperVehicles.clear();
1271 #endif //NEW_BULLET_VEHICLE_SUPPORT
1272
1273         //m_broadphase->DestroyScene();
1274         //delete broadphase ? release reference on broadphase ?
1275
1276         //first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher
1277         //delete m_dispatcher;
1278         delete m_dynamicsWorld;
1279
1280
1281         if (NULL != m_ownPairCache)
1282                 delete m_ownPairCache;
1283
1284         if (NULL != m_ownDispatcher)
1285                 delete m_ownDispatcher;
1286
1287         if (NULL != m_solver)
1288                 delete m_solver;
1289
1290         if (NULL != m_debugDrawer)
1291                 delete m_debugDrawer;
1292
1293         if (NULL != m_filterCallback)
1294                 delete m_filterCallback;
1295
1296         if (NULL != m_collisionConfiguration)
1297                 delete m_collisionConfiguration;
1298
1299         if (NULL != m_broadphase)
1300                 delete m_broadphase;
1301
1302         if (NULL != m_cullingTree)
1303                 delete m_cullingTree;
1304
1305         if (NULL != m_cullingCache)
1306                 delete m_cullingCache;
1307
1308 }
1309
1310
1311 void    CcdPhysicsEnvironment::setConstraintParam(int constraintId,int param,float value0,float value1)
1312 {
1313         btTypedConstraint* typedConstraint = getConstraintById(constraintId);
1314         switch (typedConstraint->getUserConstraintType())
1315         {
1316         case PHY_GENERIC_6DOF_CONSTRAINT:
1317                 {
1318                         //param = 1..12, min0,max0,min1,max1...min6,max6
1319                         btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
1320                         genCons->setLimit(param,value0,value1);
1321                         break;
1322                 };
1323         default:
1324                 {
1325                 };
1326         };
1327 }
1328
1329 btTypedConstraint*      CcdPhysicsEnvironment::getConstraintById(int constraintId)
1330 {
1331
1332         int numConstraints = m_dynamicsWorld->getNumConstraints();
1333         int i;
1334         for (i=0;i<numConstraints;i++)
1335         {
1336                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
1337                 if (constraint->getUserConstraintId()==constraintId)
1338                 {
1339                         return constraint;
1340                 }
1341         }
1342         return 0;
1343 }
1344
1345
1346 void CcdPhysicsEnvironment::addSensor(PHY_IPhysicsController* ctrl)
1347 {
1348
1349         CcdPhysicsController* ctrl1 = (CcdPhysicsController* )ctrl;
1350         // addSensor() is a "light" function for bullet because it is used
1351         // dynamically when the sensor is activated. Use enableCcdPhysicsController() instead
1352         //if (m_controllers.insert(ctrl1).second)
1353         //{
1354         //      addCcdPhysicsController(ctrl1);
1355         //}
1356         enableCcdPhysicsController(ctrl1);
1357
1358         //Collision filter/mask is now set at the time of the creation of the controller
1359         //force collision detection with everything, including static objects (might hurt performance!)
1360         //ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterMask = btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::SensorTrigger;
1361         //ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterGroup = btBroadphaseProxy::SensorTrigger;
1362         //todo: make this 'sensor'!
1363
1364         requestCollisionCallback(ctrl);
1365         //printf("addSensor\n");
1366 }
1367
1368 void CcdPhysicsEnvironment::removeCollisionCallback(PHY_IPhysicsController* ctrl)
1369 {
1370         CcdPhysicsController* ccdCtrl = (CcdPhysicsController*)ctrl;
1371         if (ccdCtrl->Unregister())
1372                 m_triggerControllers.erase(ccdCtrl);
1373 }
1374
1375
1376 void CcdPhysicsEnvironment::removeSensor(PHY_IPhysicsController* ctrl)
1377 {
1378         removeCollisionCallback(ctrl);
1379
1380         disableCcdPhysicsController((CcdPhysicsController*)ctrl);
1381 }
1382
1383 void CcdPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user)
1384 {
1385         /*      printf("addTouchCallback\n(response class = %i)\n",response_class);
1386
1387         //map PHY_ convention into SM_ convention
1388         switch (response_class)
1389         {
1390         case    PHY_FH_RESPONSE:
1391         printf("PHY_FH_RESPONSE\n");
1392         break;
1393         case PHY_SENSOR_RESPONSE:
1394         printf("PHY_SENSOR_RESPONSE\n");
1395         break;
1396         case PHY_CAMERA_RESPONSE:
1397         printf("PHY_CAMERA_RESPONSE\n");
1398         break;
1399         case PHY_OBJECT_RESPONSE:
1400         printf("PHY_OBJECT_RESPONSE\n");
1401         break;
1402         case PHY_STATIC_RESPONSE:
1403         printf("PHY_STATIC_RESPONSE\n");
1404         break;
1405         default:
1406         assert(0);
1407         return;
1408         }
1409         */
1410
1411         m_triggerCallbacks[response_class] = callback;
1412         m_triggerCallbacksUserPtrs[response_class] = user;
1413
1414 }
1415 void CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctrl)
1416 {
1417         CcdPhysicsController* ccdCtrl = static_cast<CcdPhysicsController*>(ctrl);
1418
1419         if (ccdCtrl->Register())
1420                 m_triggerControllers.insert(ccdCtrl);
1421 }
1422
1423 void    CcdPhysicsEnvironment::CallbackTriggers()
1424 {
1425
1426         CcdPhysicsController* ctrl0=0,*ctrl1=0;
1427
1428         if (m_triggerCallbacks[PHY_OBJECT_RESPONSE] || (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)))
1429         {
1430                 //walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback
1431                 btDispatcher* dispatcher = m_dynamicsWorld->getDispatcher();
1432                 int numManifolds = dispatcher->getNumManifolds();
1433                 for (int i=0;i<numManifolds;i++)
1434                 {
1435                         btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
1436                         int numContacts = manifold->getNumContacts();
1437                         if (numContacts)
1438                         {
1439                                 btRigidBody* rb0 = static_cast<btRigidBody*>(manifold->getBody0());
1440                                 btRigidBody* rb1 = static_cast<btRigidBody*>(manifold->getBody1());
1441                                 if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints))
1442                                 {
1443                                         for (int j=0;j<numContacts;j++)
1444                                         {
1445                                                 btVector3 color(1,0,0);
1446                                                 const btManifoldPoint& cp = manifold->getContactPoint(j);
1447                                                 if (m_debugDrawer)
1448                                                         m_debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
1449                                         }
1450                                 }
1451                                 btRigidBody* obj0 = rb0;
1452                                 btRigidBody* obj1 = rb1;
1453
1454                                 //m_internalOwner is set in 'addPhysicsController'
1455                                 CcdPhysicsController* ctrl0 = static_cast<CcdPhysicsController*>(obj0->getUserPointer());
1456                                 CcdPhysicsController* ctrl1 = static_cast<CcdPhysicsController*>(obj1->getUserPointer());
1457
1458                                 std::set<CcdPhysicsController*>::const_iterator i = m_triggerControllers.find(ctrl0);
1459                                 if (i == m_triggerControllers.end())
1460                                 {
1461                                         i = m_triggerControllers.find(ctrl1);
1462                                 }
1463
1464                                 if (!(i == m_triggerControllers.end()))
1465                                 {
1466                                         m_triggerCallbacks[PHY_OBJECT_RESPONSE](m_triggerCallbacksUserPtrs[PHY_OBJECT_RESPONSE],
1467                                                 ctrl0,ctrl1,0);
1468                                 }
1469                                 // Bullet does not refresh the manifold contact point for object without contact response
1470                                 // may need to remove this when a newer Bullet version is integrated
1471                                 if (!dispatcher->needsResponse(rb0, rb1))
1472                                 {
1473                                         // Refresh algorithm fails sometimes when there is penetration
1474                                         // (usuall the case with ghost and sensor objects)
1475                                         // Let's just clear the manifold, in any case, it is recomputed on each frame.
1476                                         manifold->clearManifold(); //refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
1477                                 }
1478                         }
1479                 }
1480
1481
1482
1483         }
1484
1485
1486 }
1487
1488 // This call back is called before a pair is added in the cache
1489 // Handy to remove objects that must be ignored by sensors
1490 bool CcdOverlapFilterCallBack::needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
1491 {
1492         btCollisionObject *colObj0, *colObj1;
1493         CcdPhysicsController *sensorCtrl, *objCtrl;
1494         bool collides;
1495         // first check the filters
1496         collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
1497         collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
1498         if (!collides)
1499                 return false;
1500
1501         // additional check for sensor object
1502         if (proxy0->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger)
1503         {
1504                 // this is a sensor object, the other one can't be a sensor object because
1505                 // they exclude each other in the above test
1506                 assert(!(proxy1->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger));
1507                 colObj0 = (btCollisionObject*)proxy0->m_clientObject;
1508                 colObj1 = (btCollisionObject*)proxy1->m_clientObject;
1509         }
1510         else if (proxy1->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger)
1511         {
1512                 colObj0 = (btCollisionObject*)proxy1->m_clientObject;
1513                 colObj1 = (btCollisionObject*)proxy0->m_clientObject;
1514         }
1515         else
1516         {
1517                 return true;
1518         }
1519         if (!colObj0 || !colObj1)
1520                 return false;
1521         sensorCtrl = static_cast<CcdPhysicsController*>(colObj0->getUserPointer());
1522         objCtrl = static_cast<CcdPhysicsController*>(colObj1->getUserPointer());
1523         if (m_physEnv->m_triggerCallbacks[PHY_BROADPH_RESPONSE])
1524         {
1525                 return m_physEnv->m_triggerCallbacks[PHY_BROADPH_RESPONSE](m_physEnv->m_triggerCallbacksUserPtrs[PHY_BROADPH_RESPONSE], sensorCtrl, objCtrl, 0);
1526         }
1527         return true;
1528 }
1529
1530
1531 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1532
1533 //complex constraint for vehicles
1534 PHY_IVehicle*   CcdPhysicsEnvironment::getVehicleConstraint(int constraintId)
1535 {
1536         int i;
1537
1538         int numVehicles = m_wrapperVehicles.size();
1539         for (i=0;i<numVehicles;i++)
1540         {
1541                 WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
1542                 if (wrapperVehicle->GetVehicle()->getUserConstraintId() == constraintId)
1543                         return wrapperVehicle;
1544         }
1545
1546         return 0;
1547 }
1548
1549 #endif //NEW_BULLET_VEHICLE_SUPPORT
1550
1551
1552 int currentController = 0;
1553 int numController = 0;
1554
1555
1556
1557
1558 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateSphereController(float radius,const PHY__Vector3& position)
1559 {
1560
1561         CcdConstructionInfo     cinfo;
1562         memset(&cinfo, 0, sizeof(cinfo)); /* avoid uninitialized values */
1563         cinfo.m_collisionShape = new btSphereShape(radius); // memory leak! The shape is not deleted by Bullet and we cannot add it to the KX_Scene.m_shapes list
1564         cinfo.m_MotionState = 0;
1565         cinfo.m_physicsEnv = this;
1566         // declare this object as Dyamic rather then static!!
1567         // The reason as it is designed to detect all type of object, including static object
1568         // It would cause static-static message to be printed on the console otherwise
1569         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE/* | btCollisionObject::CF_KINEMATIC_OBJECT*/;
1570         DefaultMotionState* motionState = new DefaultMotionState();
1571         cinfo.m_MotionState = motionState;
1572         // we will add later the possibility to select the filter from option
1573         cinfo.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
1574         cinfo.m_collisionFilterGroup = CcdConstructionInfo::SensorFilter;
1575         motionState->m_worldTransform.setIdentity();
1576         motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
1577
1578         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
1579
1580         return sphereController;
1581 }
1582
1583 int findClosestNode(btSoftBody* sb,const btVector3& worldPoint);
1584 int findClosestNode(btSoftBody* sb,const btVector3& worldPoint)
1585 {
1586         int node = -1;
1587
1588         btSoftBody::tNodeArray&   nodes(sb->m_nodes);
1589         float maxDistSqr = 1e30f;
1590
1591         for (int n=0;n<nodes.size();n++)
1592         {
1593                 btScalar distSqr = (nodes[n].m_x - worldPoint).length2();
1594                 if (distSqr<maxDistSqr)
1595                 {
1596                         maxDistSqr = distSqr;
1597                         node = n;
1598                 }
1599         }
1600         return node;
1601 }
1602
1603 int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
1604                                                                                                         float pivotX,float pivotY,float pivotZ,
1605                                                                                                         float axisX,float axisY,float axisZ,
1606                                                                                                         float axis1X,float axis1Y,float axis1Z,
1607                                                                                                         float axis2X,float axis2Y,float axis2Z,int flags
1608                                                                                                         )
1609 {
1610
1611         bool disableCollisionBetweenLinkedBodies = (0!=(flags & CCD_CONSTRAINT_DISABLE_LINKED_COLLISION));
1612
1613
1614
1615         CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
1616         CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
1617
1618         btRigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
1619         btRigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
1620
1621
1622
1623
1624         bool rb0static = rb0 ? rb0->isStaticOrKinematicObject() : true;
1625         bool rb1static = rb1 ? rb1->isStaticOrKinematicObject() : true;
1626
1627         btCollisionObject* colObj0 = c0->GetCollisionObject();
1628         if (!colObj0)
1629         {
1630                 return 0;
1631         }
1632
1633         btVector3 pivotInA(pivotX,pivotY,pivotZ);
1634
1635
1636
1637         //it might be a soft body, let's try
1638         btSoftBody* sb0 = c0 ? c0->GetSoftBody() : 0;
1639         btSoftBody* sb1 = c1 ? c1->GetSoftBody() : 0;
1640         if (sb0 && sb1)
1641         {
1642                 //not between two soft bodies?
1643                 return 0;
1644         }
1645
1646         if (sb0)
1647         {
1648                 //either cluster or node attach, let's find closest node first
1649                 //the soft body doesn't have a 'real' world transform, so get its initial world transform for now
1650                 btVector3 pivotPointSoftWorld = sb0->m_initialWorldTransform(pivotInA);
1651                 int node=findClosestNode(sb0,pivotPointSoftWorld);
1652                 if (node >=0)
1653                 {
1654                         bool clusterconstaint = false;
1655 /*
1656                         switch (type)
1657                         {
1658                         case PHY_LINEHINGE_CONSTRAINT:
1659                                 {
1660                                         if (sb0->clusterCount() && rb1)
1661                                         {
1662                                                 btSoftBody::LJoint::Specs       ls;
1663                                                 ls.erp=0.5f;
1664                                                 ls.position=sb0->clusterCom(0);
1665                                                 sb0->appendLinearJoint(ls,rb1);
1666                                                 clusterconstaint = true;
1667                                                 break;
1668                                         }
1669                                 }
1670                         case PHY_GENERIC_6DOF_CONSTRAINT:
1671                                 {
1672                                         if (sb0->clusterCount() && rb1)
1673                                         {
1674                                                 btSoftBody::AJoint::Specs as;
1675                                                 as.erp = 1;
1676                                                 as.cfm = 1;
1677                                                 as.axis.setValue(axisX,axisY,axisZ);
1678                                                 sb0->appendAngularJoint(as,rb1);
1679                                                 clusterconstaint = true;
1680                                                 break;
1681                                         }
1682
1683                                         break;
1684                                 }
1685                         default:
1686                                 {
1687
1688                                 }
1689                         };
1690                         */
1691
1692                         if (!clusterconstaint)
1693                         {
1694                                 if (rb1)
1695                                 {
1696                                         sb0->appendAnchor(node,rb1,disableCollisionBetweenLinkedBodies);
1697                                 } else
1698                                 {
1699                                         sb0->setMass(node,0.f);
1700                                 }
1701                         }
1702
1703
1704                 }
1705                 return 0;//can't remove soft body anchors yet
1706         }
1707
1708         if (sb1)
1709         {
1710                 btVector3 pivotPointAWorld = colObj0->getWorldTransform()(pivotInA);
1711                 int node=findClosestNode(sb1,pivotPointAWorld);
1712                 if (node >=0)
1713                 {
1714                         bool clusterconstaint = false;
1715
1716                         /*
1717                         switch (type)
1718                         {
1719                         case PHY_LINEHINGE_CONSTRAINT:
1720                                 {
1721                                         if (sb1->clusterCount() && rb0)
1722                                         {
1723                                                 btSoftBody::LJoint::Specs       ls;
1724                                                 ls.erp=0.5f;
1725                                                 ls.position=sb1->clusterCom(0);
1726                                                 sb1->appendLinearJoint(ls,rb0);
1727                                                 clusterconstaint = true;
1728                                                 break;
1729                                         }
1730                                 }
1731                         case PHY_GENERIC_6DOF_CONSTRAINT:
1732                                 {
1733                                         if (sb1->clusterCount() && rb0)
1734                                         {
1735                                                 btSoftBody::AJoint::Specs as;
1736                                                 as.erp = 1;
1737                                                 as.cfm = 1;
1738                                                 as.axis.setValue(axisX,axisY,axisZ);
1739                                                 sb1->appendAngularJoint(as,rb0);
1740                                                 clusterconstaint = true;
1741                                                 break;
1742                                         }
1743
1744                                         break;
1745                                 }
1746                         default:
1747                                 {
1748
1749
1750                                 }
1751                         };*/
1752
1753
1754                         if (!clusterconstaint)
1755                         {
1756                                 if (rb0)
1757                                 {
1758                                         sb1->appendAnchor(node,rb0,disableCollisionBetweenLinkedBodies);
1759                                 } else
1760                                 {
1761                                         sb1->setMass(node,0.f);
1762                                 }
1763                         }
1764
1765
1766                 }
1767                 return 0;//can't remove soft body anchors yet
1768         }
1769
1770         if (rb0static && rb1static)
1771         {
1772
1773                 return 0;
1774         }
1775
1776
1777         if (!rb0)
1778                 return 0;
1779
1780
1781         btVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) :
1782                 rb0->getCenterOfMassTransform() * pivotInA;
1783         btVector3 axisInA(axisX,axisY,axisZ);
1784
1785
1786         bool angularOnly = false;
1787
1788         switch (type)
1789         {
1790         case PHY_POINT2POINT_CONSTRAINT:
1791                 {
1792
1793                         btPoint2PointConstraint* p2p = 0;
1794
1795                         if (rb1)
1796                         {
1797                                 p2p = new btPoint2PointConstraint(*rb0,
1798                                         *rb1,pivotInA,pivotInB);
1799                         } else
1800                         {
1801                                 p2p = new btPoint2PointConstraint(*rb0,
1802                                         pivotInA);
1803                         }
1804
1805                         m_dynamicsWorld->addConstraint(p2p,disableCollisionBetweenLinkedBodies);
1806 //                      m_constraints.push_back(p2p);
1807
1808                         p2p->setUserConstraintId(gConstraintUid++);
1809                         p2p->setUserConstraintType(type);
1810                         //64 bit systems can't cast pointer to int. could use size_t instead.
1811                         return p2p->getUserConstraintId();
1812
1813                         break;
1814                 }
1815
1816         case PHY_GENERIC_6DOF_CONSTRAINT:
1817                 {
1818                         btGeneric6DofConstraint* genericConstraint = 0;
1819
1820                         if (rb1)
1821                         {
1822                                 btTransform frameInA;
1823                                 btTransform frameInB;
1824
1825                                 btVector3 axis1(axis1X,axis1Y,axis1Z), axis2(axis2X,axis2Y,axis2Z);
1826                                 if (axis1.length() == 0.0)
1827                                 {
1828                                         btPlaneSpace1( axisInA, axis1, axis2 );
1829                                 }
1830
1831                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1832                                                                   axisInA.y(), axis1.y(), axis2.y(),
1833                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1834                                 frameInA.setOrigin( pivotInA );
1835
1836                                 btTransform inv = rb1->getCenterOfMassTransform().inverse();
1837
1838                                 btTransform globalFrameA = rb0->getCenterOfMassTransform() * frameInA;
1839
1840                                 frameInB = inv  * globalFrameA;
1841                                 bool useReferenceFrameA = true;
1842
1843                                 genericConstraint = new btGeneric6DofConstraint(
1844                                         *rb0,*rb1,
1845                                         frameInA,frameInB,useReferenceFrameA);
1846
1847
1848                         } else
1849                         {
1850                                 static btRigidBody s_fixedObject2( 0,0,0);
1851                                 btTransform frameInA;
1852                                 btTransform frameInB;
1853
1854                                 btVector3 axis1, axis2;
1855                                 btPlaneSpace1( axisInA, axis1, axis2 );
1856
1857                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1858                                                                   axisInA.y(), axis1.y(), axis2.y(),
1859                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1860
1861                                 frameInA.setOrigin( pivotInA );
1862
1863                                 ///frameInB in worldspace
1864                                 frameInB = rb0->getCenterOfMassTransform() * frameInA;
1865
1866                                 bool useReferenceFrameA = true;
1867                                 genericConstraint = new btGeneric6DofConstraint(
1868                                         *rb0,s_fixedObject2,
1869                                         frameInA,frameInB,useReferenceFrameA);
1870                         }
1871
1872                         if (genericConstraint)
1873                         {
1874                                 //m_constraints.push_back(genericConstraint);
1875                                 m_dynamicsWorld->addConstraint(genericConstraint,disableCollisionBetweenLinkedBodies);
1876                                 genericConstraint->setUserConstraintId(gConstraintUid++);
1877                                 genericConstraint->setUserConstraintType(type);
1878                                 //64 bit systems can't cast pointer to int. could use size_t instead.
1879                                 return genericConstraint->getUserConstraintId();
1880                         }
1881
1882                         break;
1883                 }
1884         case PHY_CONE_TWIST_CONSTRAINT:
1885                 {
1886                         btConeTwistConstraint* coneTwistContraint = 0;
1887
1888
1889                         if (rb1)
1890                         {
1891                                 btTransform frameInA;
1892                                 btTransform frameInB;
1893
1894                                 btVector3 axis1(axis1X,axis1Y,axis1Z), axis2(axis2X,axis2Y,axis2Z);
1895                                 if (axis1.length() == 0.0)
1896                                 {
1897                                         btPlaneSpace1( axisInA, axis1, axis2 );
1898                                 }
1899
1900                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1901                                                                   axisInA.y(), axis1.y(), axis2.y(),
1902                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1903                                 frameInA.setOrigin( pivotInA );
1904
1905                                 btTransform inv = rb1->getCenterOfMassTransform().inverse();
1906
1907                                 btTransform globalFrameA = rb0->getCenterOfMassTransform() * frameInA;
1908
1909                                 frameInB = inv  * globalFrameA;
1910
1911                                 coneTwistContraint = new btConeTwistConstraint( *rb0,*rb1,
1912                                         frameInA,frameInB);
1913
1914
1915                         } else
1916                         {
1917                                 static btRigidBody s_fixedObject2( 0,0,0);
1918                                 btTransform frameInA;
1919                                 btTransform frameInB;
1920
1921                                 btVector3 axis1, axis2;
1922                                 btPlaneSpace1( axisInA, axis1, axis2 );
1923
1924                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1925                                                                   axisInA.y(), axis1.y(), axis2.y(),
1926                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1927
1928                                 frameInA.setOrigin( pivotInA );
1929
1930                                 ///frameInB in worldspace
1931                                 frameInB = rb0->getCenterOfMassTransform() * frameInA;
1932
1933                                 coneTwistContraint = new btConeTwistConstraint(
1934                                         *rb0,s_fixedObject2,
1935                                         frameInA,frameInB);
1936                         }
1937
1938                         if (coneTwistContraint)
1939                         {
1940                                 //m_constraints.push_back(genericConstraint);
1941                                 m_dynamicsWorld->addConstraint(coneTwistContraint,disableCollisionBetweenLinkedBodies);
1942                                 coneTwistContraint->setUserConstraintId(gConstraintUid++);
1943                                 coneTwistContraint->setUserConstraintType(type);
1944                                 //64 bit systems can't cast pointer to int. could use size_t instead.
1945                                 return coneTwistContraint->getUserConstraintId();
1946                         }
1947
1948
1949
1950                         break;
1951                 }
1952         case PHY_ANGULAR_CONSTRAINT:
1953                 angularOnly = true;
1954
1955
1956         case PHY_LINEHINGE_CONSTRAINT:
1957                 {
1958                         btHingeConstraint* hinge = 0;
1959
1960                         if (rb1)
1961                         {
1962                                 btVector3 axisInB = rb1 ?
1963                                 (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) :
1964                                 rb0->getCenterOfMassTransform().getBasis() * axisInA;
1965
1966                                 hinge = new btHingeConstraint(
1967                                         *rb0,
1968                                         *rb1,pivotInA,pivotInB,axisInA,axisInB);
1969
1970
1971                         } else
1972                         {
1973                                 hinge = new btHingeConstraint(*rb0,
1974                                         pivotInA,axisInA);
1975
1976                         }
1977                         hinge->setAngularOnly(angularOnly);
1978
1979                         //m_constraints.push_back(hinge);
1980                         m_dynamicsWorld->addConstraint(hinge,disableCollisionBetweenLinkedBodies);
1981                         hinge->setUserConstraintId(gConstraintUid++);
1982                         hinge->setUserConstraintType(type);
1983                         //64 bit systems can't cast pointer to int. could use size_t instead.
1984                         return hinge->getUserConstraintId();
1985                         break;
1986                 }
1987 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1988
1989         case PHY_VEHICLE_CONSTRAINT:
1990                 {
1991                         btRaycastVehicle::btVehicleTuning* tuning = new btRaycastVehicle::btVehicleTuning();
1992                         btRigidBody* chassis = rb0;
1993                         btDefaultVehicleRaycaster* raycaster = new btDefaultVehicleRaycaster(m_dynamicsWorld);
1994                         btRaycastVehicle* vehicle = new btRaycastVehicle(*tuning,chassis,raycaster);
1995                         WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
1996                         m_wrapperVehicles.push_back(wrapperVehicle);
1997                         m_dynamicsWorld->addVehicle(vehicle);
1998                         vehicle->setUserConstraintId(gConstraintUid++);
1999                         vehicle->setUserConstraintType(type);
2000                         return vehicle->getUserConstraintId();
2001
2002                         break;
2003                 };
2004 #endif //NEW_BULLET_VEHICLE_SUPPORT
2005
2006         default:
2007                 {
2008                 }
2009         };
2010
2011         //btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB
2012
2013         return 0;
2014
2015 }
2016
2017
2018
2019 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float coneradius,float coneheight)
2020 {
2021         CcdConstructionInfo     cinfo;
2022         memset(&cinfo, 0, sizeof(cinfo)); /* avoid uninitialized values */
2023         // we don't need a CcdShapeConstructionInfo for this shape:
2024         // it is simple enough for the standard copy constructor (see CcdPhysicsController::GetReplica)
2025         cinfo.m_collisionShape = new btConeShape(coneradius,coneheight);
2026         cinfo.m_MotionState = 0;
2027         cinfo.m_physicsEnv = this;
2028         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE;
2029         DefaultMotionState* motionState = new DefaultMotionState();
2030         cinfo.m_MotionState = motionState;
2031
2032         // we will add later the possibility to select the filter from option
2033         cinfo.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
2034         cinfo.m_collisionFilterGroup = CcdConstructionInfo::SensorFilter;
2035         motionState->m_worldTransform.setIdentity();
2036 //      motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
2037
2038         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
2039
2040
2041         return sphereController;
2042 }
2043
2044 float           CcdPhysicsEnvironment::getAppliedImpulse(int    constraintid)
2045 {
2046         int i;
2047         int numConstraints = m_dynamicsWorld->getNumConstraints();
2048         for (i=0;i<numConstraints;i++)
2049         {
2050                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
2051                 if (constraint->getUserConstraintId() == constraintid)
2052                 {
2053                         return constraint->getAppliedImpulse();
2054                 }
2055         }
2056
2057         return 0.f;
2058 }