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         {
 665
 666         }
 667
 668         virtual bool needsCollision(btBroadphaseProxy* proxy0) const
 669         {
 670                 //don't collide with self
 671                 if (proxy0->m_clientObject == m_owner)
 672                         return false;
 673
 674                 if (proxy0->m_clientObject == m_parent)
 675                         return false;
 676
 677                 return btCollisionWorld::ClosestRayResultCallback::needsCollision(proxy0);
 678         }
 679
 680 };
 681
 682 void    CcdPhysicsEnvironment::processFhSprings(double curTime,float timeStep)
 683 {
 684         std::set<CcdPhysicsController*>::iterator it;
 685
 686         for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
 687         {
 688                 CcdPhysicsController* ctrl = (*it);
 689                 btRigidBody* body = ctrl->GetRigidBody();
 690
 691                 if (body && (ctrl->getConstructionInfo().m_do_fh || ctrl->getConstructionInfo().m_do_rot_fh))
 692                 {
 693                         //printf("has Fh or RotFh\n");
 694                         //re-implement SM_FhObject.cpp using btCollisionWorld::rayTest and info from ctrl->getConstructionInfo()
 695                         //send a ray from {0.0, 0.0, 0.0} towards {0.0, 0.0, -10.0}, in local coordinates
 696
 697
 698                         CcdPhysicsController* parentCtrl = ctrl->getParentCtrl();
 699                         btRigidBody* parentBody = parentCtrl?parentCtrl->GetRigidBody() : 0;
 700                         btRigidBody* cl_object = parentBody ? parentBody : body;
 701
 702                         if (body->isStaticOrKinematicObject())
 703                                 continue;
 704
 705                         btVector3 rayDirLocal(0,0,-10);
 706
 707                         //m_dynamicsWorld
 708                         //ctrl->GetRigidBody();
 709                         btVector3 rayFromWorld = body->getCenterOfMassPosition();
 710                         //btVector3     rayToWorld = rayFromWorld + body->getCenterOfMassTransform().getBasis() * rayDirLocal;
 711                         //ray always points down the z axis in world space...
 712                         btVector3       rayToWorld = rayFromWorld + rayDirLocal;
 713
 714                         ClosestRayResultCallbackNotMe   resultCallback(rayFromWorld,rayToWorld,body,parentBody);
 715
 716                         m_dynamicsWorld->rayTest(rayFromWorld,rayToWorld,resultCallback);
 717                         if (resultCallback.hasHit())
 718                         {
 719                                 //we hit this one: resultCallback.m_collisionObject;
 720                                 CcdPhysicsController* controller = static_cast<CcdPhysicsController*>(resultCallback.m_collisionObject->getUserPointer());
 721
 722                                 if (controller)
 723                                 {
 724                                         if (controller->getConstructionInfo().m_fh_distance < SIMD_EPSILON)
 725                                                 continue;
 726
 727                                         btRigidBody* hit_object = controller->GetRigidBody();
 728                                         if (!hit_object)
 729                                                 continue;
 730
 731                                         CcdConstructionInfo& hitObjShapeProps = controller->getConstructionInfo();
 732
 733                                         float distance = resultCallback.m_closestHitFraction*rayDirLocal.length()-ctrl->getConstructionInfo().m_radius;
 734                                         if (distance >= hitObjShapeProps.m_fh_distance)
 735                                                 continue;
 736
 737
 738
 739                                         //btVector3 ray_dir = cl_object->getCenterOfMassTransform().getBasis()* rayDirLocal.normalized();
 740                                         btVector3 ray_dir = rayDirLocal.normalized();
 741                                         btVector3 normal = resultCallback.m_hitNormalWorld;
 742                                         normal.normalize();
 743
 744
 745                                         if (ctrl->getConstructionInfo().m_do_fh)
 746                                         {
 747                                                 btVector3 lspot = cl_object->getCenterOfMassPosition()
 748                                                         + rayDirLocal * resultCallback.m_closestHitFraction;
 749
 750
 751
 752
 753                                                 lspot -= hit_object->getCenterOfMassPosition();
 754                                                 btVector3 rel_vel = cl_object->getLinearVelocity() - hit_object->getVelocityInLocalPoint(lspot);
 755                                                 btScalar rel_vel_ray = ray_dir.dot(rel_vel);
 756                                                 btScalar spring_extent = 1.0 - distance / hitObjShapeProps.m_fh_distance;
 757
 758                                                 btScalar i_spring = spring_extent * hitObjShapeProps.m_fh_spring;
 759                                                 btScalar i_damp =   rel_vel_ray * hitObjShapeProps.m_fh_damping;
 760
 761                                                 cl_object->setLinearVelocity(cl_object->getLinearVelocity() + (-(i_spring + i_damp) * ray_dir));
 762                                                 if (hitObjShapeProps.m_fh_normal)
 763                                                 {
 764                                                         cl_object->setLinearVelocity(cl_object->getLinearVelocity()+(i_spring + i_damp) *(normal - normal.dot(ray_dir) * ray_dir));
 765                                                 }
 766
 767                                                 btVector3 lateral = rel_vel - rel_vel_ray * ray_dir;
 768
 769
 770                                                 if (ctrl->getConstructionInfo().m_do_anisotropic) {
 771                                                         //Bullet basis contains no scaling/shear etc.
 772                                                         const btMatrix3x3& lcs = cl_object->getCenterOfMassTransform().getBasis();
 773                                                         btVector3 loc_lateral = lateral * lcs;
 774                                                         const btVector3& friction_scaling = cl_object->getAnisotropicFriction();
 775                                                         loc_lateral *= friction_scaling;
 776                                                         lateral = lcs * loc_lateral;
 777                                                 }
 778
 779                                                 btScalar rel_vel_lateral = lateral.length();
 780
 781                                                 if (rel_vel_lateral > SIMD_EPSILON) {
 782                                                         btScalar friction_factor = hit_object->getFriction();//cl_object->getFriction();
 783
 784                                                         btScalar max_friction = friction_factor * btMax(btScalar(0.0), i_spring);
 785
 786                                                         btScalar rel_mom_lateral = rel_vel_lateral / cl_object->getInvMass();
 787
 788                                                         btVector3 friction = (rel_mom_lateral > max_friction) ?
 789                                                                 -lateral * (max_friction / rel_vel_lateral) :
 790                                                                 -lateral;
 791
 792                                                                 cl_object->applyCentralImpulse(friction);
 793                                                 }
 794                                         }
 795
 796
 797                                         if (ctrl->getConstructionInfo().m_do_rot_fh) {
 798                                                 btVector3 up2 = cl_object->getWorldTransform().getBasis().getColumn(2);
 799
 800                                                 btVector3 t_spring = up2.cross(normal) * hitObjShapeProps.m_fh_spring;
 801                                                 btVector3 ang_vel = cl_object->getAngularVelocity();
 802
 803                                                 // only rotations that tilt relative to the normal are damped
 804                                                 ang_vel -= ang_vel.dot(normal) * normal;
 805
 806                                                 btVector3 t_damp = ang_vel * hitObjShapeProps.m_fh_damping;
 807
 808                                                 cl_object->setAngularVelocity(cl_object->getAngularVelocity() + (t_spring - t_damp));
 809                                         }
 810
 811                                 }
 812
 813
 814                         }
 815
 816
 817                 }
 818         }
 819
 820 }
 821
 822 void            CcdPhysicsEnvironment::setDebugMode(int debugMode)
 823 {
 824         if (m_debugDrawer){
 825                 m_debugDrawer->setDebugMode(debugMode);
 826         }
 827 }
 828
 829 void            CcdPhysicsEnvironment::setNumIterations(int numIter)
 830 {
 831         m_numIterations = numIter;
 832 }
 833 void            CcdPhysicsEnvironment::setDeactivationTime(float dTime)
 834 {
 835         gDeactivationTime = dTime;
 836 }
 837 void            CcdPhysicsEnvironment::setDeactivationLinearTreshold(float linTresh)
 838 {
 839         gLinearSleepingTreshold = linTresh;
 840 }
 841 void            CcdPhysicsEnvironment::setDeactivationAngularTreshold(float angTresh)
 842 {
 843         gAngularSleepingTreshold = angTresh;
 844 }
 845
 846 void            CcdPhysicsEnvironment::setContactBreakingTreshold(float contactBreakingTreshold)
 847 {
 848         gContactBreakingThreshold = contactBreakingTreshold;
 849
 850 }
 851
 852
 853 void            CcdPhysicsEnvironment::setCcdMode(int ccdMode)
 854 {
 855         m_ccdMode = ccdMode;
 856 }
 857
 858
 859 void            CcdPhysicsEnvironment::setSolverSorConstant(float sor)
 860 {
 861         m_solverInfo.m_sor = sor;
 862 }
 863
 864 void            CcdPhysicsEnvironment::setSolverTau(float tau)
 865 {
 866         m_solverInfo.m_tau = tau;
 867 }
 868 void            CcdPhysicsEnvironment::setSolverDamping(float damping)
 869 {
 870         m_solverInfo.m_damping = damping;
 871 }
 872
 873
 874 void            CcdPhysicsEnvironment::setLinearAirDamping(float damping)
 875 {
 876         //gLinearAirDamping = damping;
 877 }
 878
 879 void            CcdPhysicsEnvironment::setUseEpa(bool epa)
 880 {
 881         //gUseEpa = epa;
 882 }
 883
 884 void            CcdPhysicsEnvironment::setSolverType(int solverType)
 885 {
 886
 887         switch (solverType)
 888         {
 889         case 1:
 890                 {
 891                         if (m_solverType != solverType)
 892                         {
 893
 894                                 m_solver = new btSequentialImpulseConstraintSolver();
 895
 896
 897                                 break;
 898                         }
 899                 }
 900
 901         case 0:
 902         default:
 903                 if (m_solverType != solverType)
 904                 {
 905 //                      m_solver = new OdeConstraintSolver();
 906
 907                         break;
 908                 }
 909
 910         };
 911
 912         m_solverType = solverType ;
 913 }
 914
 915
 916
 917 void            CcdPhysicsEnvironment::getGravity(PHY__Vector3& grav)
 918 {
 919                 const btVector3& gravity = m_dynamicsWorld->getGravity();
 920                 grav[0] = gravity.getX();
 921                 grav[1] = gravity.getY();
 922                 grav[2] = gravity.getZ();
 923 }
 924
 925
 926 void            CcdPhysicsEnvironment::setGravity(float x,float y,float z)
 927 {
 928         m_gravity = btVector3(x,y,z);
 929         m_dynamicsWorld->setGravity(m_gravity);
 930
 931 }
 932
 933
 934
 935
 936 static int gConstraintUid = 1;
 937
 938 //Following the COLLADA physics specification for constraints
 939 int                     CcdPhysicsEnvironment::createUniversalD6Constraint(
 940                                                 class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
 941                                                 btTransform& frameInA,
 942                                                 btTransform& frameInB,
 943                                                 const btVector3& linearMinLimits,
 944                                                 const btVector3& linearMaxLimits,
 945                                                 const btVector3& angularMinLimits,
 946                                                 const btVector3& angularMaxLimits,int flags
 947 )
 948 {
 949
 950         bool disableCollisionBetweenLinkedBodies = (0!=(flags & CCD_CONSTRAINT_DISABLE_LINKED_COLLISION));
 951
 952         //we could either add some logic to recognize ball-socket and hinge, or let that up to the user
 953         //perhaps some warning or hint that hinge/ball-socket is more efficient?
 954
 955
 956         btGeneric6DofConstraint* genericConstraint = 0;
 957         CcdPhysicsController* ctrl0 = (CcdPhysicsController*) ctrlRef;
 958         CcdPhysicsController* ctrl1 = (CcdPhysicsController*) ctrlOther;
 959
 960         btRigidBody* rb0 = ctrl0->GetRigidBody();
 961         btRigidBody* rb1 = ctrl1->GetRigidBody();
 962
 963         if (rb1)
 964         {
 965
 966
 967                 bool useReferenceFrameA = true;
 968                 genericConstraint = new btGeneric6DofConstraint(
 969                         *rb0,*rb1,
 970                         frameInA,frameInB,useReferenceFrameA);
 971                 genericConstraint->setLinearLowerLimit(linearMinLimits);
 972                 genericConstraint->setLinearUpperLimit(linearMaxLimits);
 973                 genericConstraint->setAngularLowerLimit(angularMinLimits);
 974                 genericConstraint->setAngularUpperLimit(angularMaxLimits);
 975         } else
 976         {
 977                 // TODO: Implement single body case...
 978                 //No, we can use a fixed rigidbody in above code, rather then unnecessary duplation of code
 979
 980         }
 981
 982         if (genericConstraint)
 983         {
 984         //      m_constraints.push_back(genericConstraint);
 985                 m_dynamicsWorld->addConstraint(genericConstraint,disableCollisionBetweenLinkedBodies);
 986
 987                 genericConstraint->setUserConstraintId(gConstraintUid++);
 988                 genericConstraint->setUserConstraintType(PHY_GENERIC_6DOF_CONSTRAINT);
 989                 //64 bit systems can't cast pointer to int. could use size_t instead.
 990                 return genericConstraint->getUserConstraintId();
 991         }
 992         return 0;
 993 }
 994
 995
 996
 997 void            CcdPhysicsEnvironment::removeConstraint(int     constraintId)
 998 {
 999
1000         int i;
1001         int numConstraints = m_dynamicsWorld->getNumConstraints();
1002         for (i=0;i<numConstraints;i++)
1003         {
1004                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
1005                 if (constraint->getUserConstraintId() == constraintId)
1006                 {
1007                         constraint->getRigidBodyA().activate();
1008                         constraint->getRigidBodyB().activate();
1009                         m_dynamicsWorld->removeConstraint(constraint);
1010                         break;
1011                 }
1012         }
1013 }
1014
1015
1016 struct  FilterClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
1017 {
1018         PHY_IRayCastFilterCallback&     m_phyRayFilter;
1019         const btCollisionShape*         m_hitTriangleShape;
1020         int                                                     m_hitTriangleIndex;
1021
1022         FilterClosestRayResultCallback (PHY_IRayCastFilterCallback& phyRayFilter,const btVector3& rayFrom,const btVector3& rayTo)
1023                 : btCollisionWorld::ClosestRayResultCallback(rayFrom,rayTo),
1024                 m_phyRayFilter(phyRayFilter),
1025                 m_hitTriangleShape(NULL),
1026                 m_hitTriangleIndex(0)
1027         {
1028         }
1029
1030         virtual ~FilterClosestRayResultCallback()
1031         {
1032         }
1033
1034         virtual bool needsCollision(btBroadphaseProxy* proxy0) const
1035         {
1036                 if (!(proxy0->m_collisionFilterGroup & m_collisionFilterMask))
1037                         return false;
1038                 if (!(m_collisionFilterGroup & proxy0->m_collisionFilterMask))
1039                         return false;
1040                 btCollisionObject* object = (btCollisionObject*)proxy0->m_clientObject;
1041                 CcdPhysicsController* phyCtrl = static_cast<CcdPhysicsController*>(object->getUserPointer());
1042                 if (phyCtrl == m_phyRayFilter.m_ignoreController)
1043                         return false;
1044                 return m_phyRayFilter.needBroadphaseRayCast(phyCtrl);
1045         }
1046
1047         virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace)
1048         {
1049                 CcdPhysicsController* curHit = static_cast<CcdPhysicsController*>(rayResult.m_collisionObject->getUserPointer());
1050                 // save shape information as ClosestRayResultCallback::AddSingleResult() does not do it
1051                 if (rayResult.m_localShapeInfo)
1052                 {
1053                         m_hitTriangleShape = rayResult.m_collisionObject->getCollisionShape();
1054                         m_hitTriangleIndex = rayResult.m_localShapeInfo->m_triangleIndex;
1055                 } else
1056                 {
1057                         m_hitTriangleShape = NULL;
1058                         m_hitTriangleIndex = 0;
1059                 }
1060                 return ClosestRayResultCallback::addSingleResult(rayResult,normalInWorldSpace);
1061         }
1062
1063 };
1064
1065 PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IRayCastFilterCallback &filterCallback, float fromX,float fromY,float fromZ, float toX,float toY,float toZ)
1066 {
1067
1068
1069         float minFraction = 1.f;
1070
1071         btVector3 rayFrom(fromX,fromY,fromZ);
1072         btVector3 rayTo(toX,toY,toZ);
1073
1074         btVector3       hitPointWorld,normalWorld;
1075
1076         //Either Ray Cast with or without filtering
1077
1078         //btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo);
1079         FilterClosestRayResultCallback   rayCallback(filterCallback,rayFrom,rayTo);
1080
1081
1082         PHY_RayCastResult result;
1083         memset(&result, 0, sizeof(result));
1084
1085         // don't collision with sensor object
1086         rayCallback.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
1087         //, ,filterCallback.m_faceNormal);
1088
1089         m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback);
1090         if (rayCallback.hasHit())
1091         {
1092                 CcdPhysicsController* controller = static_cast<CcdPhysicsController*>(rayCallback.m_collisionObject->getUserPointer());
1093                 result.m_controller = controller;
1094                 result.m_hitPoint[0] = rayCallback.m_hitPointWorld.getX();
1095                 result.m_hitPoint[1] = rayCallback.m_hitPointWorld.getY();
1096                 result.m_hitPoint[2] = rayCallback.m_hitPointWorld.getZ();
1097
1098                 if (rayCallback.m_hitTriangleShape != NULL)
1099                 {
1100                         // identify the mesh polygon
1101                         CcdShapeConstructionInfo* shapeInfo = controller->m_shapeInfo;
1102                         if (shapeInfo)
1103                         {
1104                                 btCollisionShape* shape = controller->GetCollisionObject()->getCollisionShape();
1105                                 if (shape->isCompound())
1106                                 {
1107                                         btCompoundShape* compoundShape = (btCompoundShape*)shape;
1108                                         CcdShapeConstructionInfo* compoundShapeInfo = shapeInfo;
1109                                         // need to search which sub-shape has been hit
1110                                         for (int i=0; i<compoundShape->getNumChildShapes(); i++)
1111                                         {
1112                                                 shapeInfo = compoundShapeInfo->GetChildShape(i);
1113                                                 shape=compoundShape->getChildShape(i);
1114                                                 if (shape == rayCallback.m_hitTriangleShape)
1115                                                         break;
1116                                         }
1117                                 }
1118                                 if (shape == rayCallback.m_hitTriangleShape &&
1119                                         rayCallback.m_hitTriangleIndex < shapeInfo->m_polygonIndexArray.size())
1120                                 {
1121                                         result.m_meshObject = shapeInfo->GetMesh();
1122                                         result.m_polygon = shapeInfo->m_polygonIndexArray.at(rayCallback.m_hitTriangleIndex);
1123
1124                                         // Bullet returns the normal from "outside".
1125                                         // If the user requests the real normal, compute it now
1126                     if (filterCallback.m_faceNormal)
1127                                         {
1128                                                 // mesh shapes are shared and stored in the shapeInfo
1129                                                 btTriangleMeshShape* triangleShape = shapeInfo->GetMeshShape();
1130                                                 if (triangleShape)
1131                                                 {
1132                                                         // this code is copied from Bullet
1133                                                         btVector3 triangle[3];
1134                                                         const unsigned char *vertexbase;
1135                                                         int numverts;
1136                                                         PHY_ScalarType type;
1137                                                         int stride;
1138                                                         const unsigned char *indexbase;
1139                                                         int indexstride;
1140                                                         int numfaces;
1141                                                         PHY_ScalarType indicestype;
1142                                                         btStridingMeshInterface* meshInterface = triangleShape->getMeshInterface();
1143
1144                                                         meshInterface->getLockedReadOnlyVertexIndexBase(
1145                                                                 &vertexbase,
1146                                                                 numverts,
1147                                                                 type,
1148                                                                 stride,
1149                                                                 &indexbase,
1150                                                                 indexstride,
1151                                                                 numfaces,
1152                                                                 indicestype,
1153                                                                 0);
1154
1155                                                         unsigned int* gfxbase = (unsigned int*)(indexbase+rayCallback.m_hitTriangleIndex*indexstride);
1156                                                         const btVector3& meshScaling = shape->getLocalScaling();
1157                                                         for (int j=2;j>=0;j--)
1158                                                         {
1159                                                                 int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
1160
1161                                                                 btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride);
1162
1163                                                                 triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());
1164                                                         }
1165                                                         meshInterface->unLockReadOnlyVertexBase(0);
1166                                                         btVector3 triangleNormal;
1167                                                         triangleNormal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]);
1168                                                         rayCallback.m_hitNormalWorld = rayCallback.m_collisionObject->getWorldTransform().getBasis()*triangleNormal;
1169                                                 }
1170                                         }
1171                                 }
1172                         }
1173                 }
1174                 if (rayCallback.m_hitNormalWorld.length2() > (SIMD_EPSILON*SIMD_EPSILON))
1175                 {
1176                         rayCallback.m_hitNormalWorld.normalize();
1177                 } else
1178                 {
1179                         rayCallback.m_hitNormalWorld.setValue(1,0,0);
1180                 }
1181                 result.m_hitNormal[0] = rayCallback.m_hitNormalWorld.getX();
1182                 result.m_hitNormal[1] = rayCallback.m_hitNormalWorld.getY();
1183                 result.m_hitNormal[2] = rayCallback.m_hitNormalWorld.getZ();
1184                 filterCallback.reportHit(&result);
1185         }
1186
1187
1188         return result.m_controller;
1189 }
1190
1191 struct  DbvtCullingCallback : btDbvt::ICollide
1192 {
1193         PHY_CullingCallback m_clientCallback;
1194         void* m_userData;
1195
1196         DbvtCullingCallback(PHY_CullingCallback clientCallback, void* userData)
1197         {
1198                 m_clientCallback = clientCallback;
1199                 m_userData = userData;
1200         }
1201
1202         void Process(const btDbvtNode* node,btScalar depth)
1203         {
1204                 Process(node);
1205         }
1206         void Process(const btDbvtNode* leaf)
1207         {
1208                 btBroadphaseProxy*      proxy=(btBroadphaseProxy*)leaf->data;
1209                 // the client object is a graphic controller
1210                 CcdGraphicController* ctrl = static_cast<CcdGraphicController*>(proxy->m_clientObject);
1211                 KX_ClientObjectInfo* info = (KX_ClientObjectInfo*)ctrl->getNewClientInfo();
1212                 if (info)
1213                         (*m_clientCallback)(info, m_userData);
1214         }
1215 };
1216
1217 bool CcdPhysicsEnvironment::cullingTest(PHY_CullingCallback callback, void* userData, PHY__Vector4 *planes, int nplanes)
1218 {
1219         if (!m_cullingTree)
1220                 return false;
1221         DbvtCullingCallback dispatcher(callback, userData);
1222         btVector3 planes_n[5];
1223         btScalar planes_o[5];
1224         if (nplanes > 5)
1225                 nplanes = 5;
1226         for (int i=0; i<nplanes; i++)
1227         {
1228                 planes_n[i].setValue(planes[i][0], planes[i][1], planes[i][2]);
1229                 planes_o[i] = planes[i][3];
1230         }
1231         btDbvt::collideKDOP(m_cullingTree->m_sets[1].m_root,planes_n,planes_o,nplanes,dispatcher);
1232         btDbvt::collideKDOP(m_cullingTree->m_sets[0].m_root,planes_n,planes_o,nplanes,dispatcher);
1233         return true;
1234 }
1235
1236
1237 int     CcdPhysicsEnvironment::getNumContactPoints()
1238 {
1239         return 0;
1240 }
1241
1242 void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
1243 {
1244
1245 }
1246
1247
1248
1249
1250 btBroadphaseInterface*  CcdPhysicsEnvironment::getBroadphase()
1251 {
1252         return m_dynamicsWorld->getBroadphase();
1253 }
1254
1255 btDispatcher*   CcdPhysicsEnvironment::getDispatcher()
1256 {
1257         return m_dynamicsWorld->getDispatcher();
1258 }
1259
1260
1261
1262
1263
1264
1265 CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
1266 {
1267
1268 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1269         m_wrapperVehicles.clear();
1270 #endif //NEW_BULLET_VEHICLE_SUPPORT
1271
1272         //m_broadphase->DestroyScene();
1273         //delete broadphase ? release reference on broadphase ?
1274
1275         //first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher
1276         //delete m_dispatcher;
1277         delete m_dynamicsWorld;
1278
1279
1280         if (NULL != m_ownPairCache)
1281                 delete m_ownPairCache;
1282
1283         if (NULL != m_ownDispatcher)
1284                 delete m_ownDispatcher;
1285
1286         if (NULL != m_solver)
1287                 delete m_solver;
1288
1289         if (NULL != m_debugDrawer)
1290                 delete m_debugDrawer;
1291
1292         if (NULL != m_filterCallback)
1293                 delete m_filterCallback;
1294
1295         if (NULL != m_collisionConfiguration)
1296                 delete m_collisionConfiguration;
1297
1298         if (NULL != m_broadphase)
1299                 delete m_broadphase;
1300
1301         if (NULL != m_cullingTree)
1302                 delete m_cullingTree;
1303
1304         if (NULL != m_cullingCache)
1305                 delete m_cullingCache;
1306
1307 }
1308
1309
1310 void    CcdPhysicsEnvironment::setConstraintParam(int constraintId,int param,float value0,float value1)
1311 {
1312         btTypedConstraint* typedConstraint = getConstraintById(constraintId);
1313         switch (typedConstraint->getUserConstraintType())
1314         {
1315         case PHY_GENERIC_6DOF_CONSTRAINT:
1316                 {
1317                         //param = 1..12, min0,max0,min1,max1...min6,max6
1318                         btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
1319                         genCons->setLimit(param,value0,value1);
1320                         break;
1321                 };
1322         default:
1323                 {
1324                 };
1325         };
1326 }
1327
1328 btTypedConstraint*      CcdPhysicsEnvironment::getConstraintById(int constraintId)
1329 {
1330
1331         int numConstraints = m_dynamicsWorld->getNumConstraints();
1332         int i;
1333         for (i=0;i<numConstraints;i++)
1334         {
1335                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
1336                 if (constraint->getUserConstraintId()==constraintId)
1337                 {
1338                         return constraint;
1339                 }
1340         }
1341         return 0;
1342 }
1343
1344
1345 void CcdPhysicsEnvironment::addSensor(PHY_IPhysicsController* ctrl)
1346 {
1347
1348         CcdPhysicsController* ctrl1 = (CcdPhysicsController* )ctrl;
1349         // addSensor() is a "light" function for bullet because it is used
1350         // dynamically when the sensor is activated. Use enableCcdPhysicsController() instead
1351         //if (m_controllers.insert(ctrl1).second)
1352         //{
1353         //      addCcdPhysicsController(ctrl1);
1354         //}
1355         enableCcdPhysicsController(ctrl1);
1356
1357         //Collision filter/mask is now set at the time of the creation of the controller
1358         //force collision detection with everything, including static objects (might hurt performance!)
1359         //ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterMask = btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::SensorTrigger;
1360         //ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterGroup = btBroadphaseProxy::SensorTrigger;
1361         //todo: make this 'sensor'!
1362
1363         requestCollisionCallback(ctrl);
1364         //printf("addSensor\n");
1365 }
1366
1367 void CcdPhysicsEnvironment::removeCollisionCallback(PHY_IPhysicsController* ctrl)
1368 {
1369         CcdPhysicsController* ccdCtrl = (CcdPhysicsController*)ctrl;
1370         if (ccdCtrl->Unregister())
1371                 m_triggerControllers.erase(ccdCtrl);
1372 }
1373
1374
1375 void CcdPhysicsEnvironment::removeSensor(PHY_IPhysicsController* ctrl)
1376 {
1377         removeCollisionCallback(ctrl);
1378
1379         disableCcdPhysicsController((CcdPhysicsController*)ctrl);
1380 }
1381
1382 void CcdPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user)
1383 {
1384         /*      printf("addTouchCallback\n(response class = %i)\n",response_class);
1385
1386         //map PHY_ convention into SM_ convention
1387         switch (response_class)
1388         {
1389         case    PHY_FH_RESPONSE:
1390         printf("PHY_FH_RESPONSE\n");
1391         break;
1392         case PHY_SENSOR_RESPONSE:
1393         printf("PHY_SENSOR_RESPONSE\n");
1394         break;
1395         case PHY_CAMERA_RESPONSE:
1396         printf("PHY_CAMERA_RESPONSE\n");
1397         break;
1398         case PHY_OBJECT_RESPONSE:
1399         printf("PHY_OBJECT_RESPONSE\n");
1400         break;
1401         case PHY_STATIC_RESPONSE:
1402         printf("PHY_STATIC_RESPONSE\n");
1403         break;
1404         default:
1405         assert(0);
1406         return;
1407         }
1408         */
1409
1410         m_triggerCallbacks[response_class] = callback;
1411         m_triggerCallbacksUserPtrs[response_class] = user;
1412
1413 }
1414 void CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctrl)
1415 {
1416         CcdPhysicsController* ccdCtrl = static_cast<CcdPhysicsController*>(ctrl);
1417
1418         if (ccdCtrl->Register())
1419                 m_triggerControllers.insert(ccdCtrl);
1420 }
1421
1422 void    CcdPhysicsEnvironment::CallbackTriggers()
1423 {
1424
1425         CcdPhysicsController* ctrl0=0,*ctrl1=0;
1426
1427         if (m_triggerCallbacks[PHY_OBJECT_RESPONSE] || (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)))
1428         {
1429                 //walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback
1430                 btDispatcher* dispatcher = m_dynamicsWorld->getDispatcher();
1431                 int numManifolds = dispatcher->getNumManifolds();
1432                 for (int i=0;i<numManifolds;i++)
1433                 {
1434                         btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
1435                         int numContacts = manifold->getNumContacts();
1436                         if (numContacts)
1437                         {
1438                                 btRigidBody* rb0 = static_cast<btRigidBody*>(manifold->getBody0());
1439                                 btRigidBody* rb1 = static_cast<btRigidBody*>(manifold->getBody1());
1440                                 if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints))
1441                                 {
1442                                         for (int j=0;j<numContacts;j++)
1443                                         {
1444                                                 btVector3 color(1,0,0);
1445                                                 const btManifoldPoint& cp = manifold->getContactPoint(j);
1446                                                 if (m_debugDrawer)
1447                                                         m_debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
1448                                         }
1449                                 }
1450                                 btRigidBody* obj0 = rb0;
1451                                 btRigidBody* obj1 = rb1;
1452
1453                                 //m_internalOwner is set in 'addPhysicsController'
1454                                 CcdPhysicsController* ctrl0 = static_cast<CcdPhysicsController*>(obj0->getUserPointer());
1455                                 CcdPhysicsController* ctrl1 = static_cast<CcdPhysicsController*>(obj1->getUserPointer());
1456
1457                                 std::set<CcdPhysicsController*>::const_iterator i = m_triggerControllers.find(ctrl0);
1458                                 if (i == m_triggerControllers.end())
1459                                 {
1460                                         i = m_triggerControllers.find(ctrl1);
1461                                 }
1462
1463                                 if (!(i == m_triggerControllers.end()))
1464                                 {
1465                                         m_triggerCallbacks[PHY_OBJECT_RESPONSE](m_triggerCallbacksUserPtrs[PHY_OBJECT_RESPONSE],
1466                                                 ctrl0,ctrl1,0);
1467                                 }
1468                                 // Bullet does not refresh the manifold contact point for object without contact response
1469                                 // may need to remove this when a newer Bullet version is integrated
1470                                 if (!dispatcher->needsResponse(rb0, rb1))
1471                                 {
1472                                         // Refresh algorithm fails sometimes when there is penetration
1473                                         // (usuall the case with ghost and sensor objects)
1474                                         // Let's just clear the manifold, in any case, it is recomputed on each frame.
1475                                         manifold->clearManifold(); //refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
1476                                 }
1477                         }
1478                 }
1479
1480
1481
1482         }
1483
1484
1485 }
1486
1487 // This call back is called before a pair is added in the cache
1488 // Handy to remove objects that must be ignored by sensors
1489 bool CcdOverlapFilterCallBack::needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
1490 {
1491         btCollisionObject *colObj0, *colObj1;
1492         CcdPhysicsController *sensorCtrl, *objCtrl;
1493         bool collides;
1494         // first check the filters
1495         collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
1496         collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
1497         if (!collides)
1498                 return false;
1499
1500         // additional check for sensor object
1501         if (proxy0->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger)
1502         {
1503                 // this is a sensor object, the other one can't be a sensor object because
1504                 // they exclude each other in the above test
1505                 assert(!(proxy1->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger));
1506                 colObj0 = (btCollisionObject*)proxy0->m_clientObject;
1507                 colObj1 = (btCollisionObject*)proxy1->m_clientObject;
1508         }
1509         else if (proxy1->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger)
1510         {
1511                 colObj0 = (btCollisionObject*)proxy1->m_clientObject;
1512                 colObj1 = (btCollisionObject*)proxy0->m_clientObject;
1513         }
1514         else
1515         {
1516                 return true;
1517         }
1518         if (!colObj0 || !colObj1)
1519                 return false;
1520         sensorCtrl = static_cast<CcdPhysicsController*>(colObj0->getUserPointer());
1521         objCtrl = static_cast<CcdPhysicsController*>(colObj1->getUserPointer());
1522         if (m_physEnv->m_triggerCallbacks[PHY_BROADPH_RESPONSE])
1523         {
1524                 return m_physEnv->m_triggerCallbacks[PHY_BROADPH_RESPONSE](m_physEnv->m_triggerCallbacksUserPtrs[PHY_BROADPH_RESPONSE], sensorCtrl, objCtrl, 0);
1525         }
1526         return true;
1527 }
1528
1529
1530 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1531
1532 //complex constraint for vehicles
1533 PHY_IVehicle*   CcdPhysicsEnvironment::getVehicleConstraint(int constraintId)
1534 {
1535         int i;
1536
1537         int numVehicles = m_wrapperVehicles.size();
1538         for (i=0;i<numVehicles;i++)
1539         {
1540                 WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
1541                 if (wrapperVehicle->GetVehicle()->getUserConstraintId() == constraintId)
1542                         return wrapperVehicle;
1543         }
1544
1545         return 0;
1546 }
1547
1548 #endif //NEW_BULLET_VEHICLE_SUPPORT
1549
1550
1551 int currentController = 0;
1552 int numController = 0;
1553
1554
1555
1556
1557 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateSphereController(float radius,const PHY__Vector3& position)
1558 {
1559
1560         CcdConstructionInfo     cinfo;
1561         // memory leak! The shape is not deleted by Bullet and we cannot add it to the KX_Scene.m_shapes list
1562         cinfo.m_collisionShape = new btSphereShape(radius);
1563         cinfo.m_MotionState = 0;
1564         cinfo.m_physicsEnv = this;
1565         // declare this object as Dyamic rather then static!!
1566         // The reason as it is designed to detect all type of object, including static object
1567         // It would cause static-static message to be printed on the console otherwise
1568         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE/* | btCollisionObject::CF_KINEMATIC_OBJECT*/;
1569         DefaultMotionState* motionState = new DefaultMotionState();
1570         cinfo.m_MotionState = motionState;
1571         // we will add later the possibility to select the filter from option
1572         cinfo.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
1573         cinfo.m_collisionFilterGroup = CcdConstructionInfo::SensorFilter;
1574         motionState->m_worldTransform.setIdentity();
1575         motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
1576
1577         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
1578
1579         return sphereController;
1580 }
1581
1582 int findClosestNode(btSoftBody* sb,const btVector3& worldPoint);
1583 int findClosestNode(btSoftBody* sb,const btVector3& worldPoint)
1584 {
1585         int node = -1;
1586
1587         btSoftBody::tNodeArray&   nodes(sb->m_nodes);
1588         float maxDistSqr = 1e30f;
1589
1590         for (int n=0;n<nodes.size();n++)
1591         {
1592                 btScalar distSqr = (nodes[n].m_x - worldPoint).length2();
1593                 if (distSqr<maxDistSqr)
1594                 {
1595                         maxDistSqr = distSqr;
1596                         node = n;
1597                 }
1598         }
1599         return node;
1600 }
1601
1602 int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
1603                                                                                                         float pivotX,float pivotY,float pivotZ,
1604                                                                                                         float axisX,float axisY,float axisZ,
1605                                                                                                         float axis1X,float axis1Y,float axis1Z,
1606                                                                                                         float axis2X,float axis2Y,float axis2Z,int flags
1607                                                                                                         )
1608 {
1609
1610         bool disableCollisionBetweenLinkedBodies = (0!=(flags & CCD_CONSTRAINT_DISABLE_LINKED_COLLISION));
1611
1612
1613
1614         CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
1615         CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
1616
1617         btRigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
1618         btRigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
1619
1620
1621
1622
1623         bool rb0static = rb0 ? rb0->isStaticOrKinematicObject() : true;
1624         bool rb1static = rb1 ? rb1->isStaticOrKinematicObject() : true;
1625
1626         btCollisionObject* colObj0 = c0->GetCollisionObject();
1627         if (!colObj0)
1628         {
1629                 return 0;
1630         }
1631
1632         btVector3 pivotInA(pivotX,pivotY,pivotZ);
1633
1634
1635
1636         //it might be a soft body, let's try
1637         btSoftBody* sb0 = c0 ? c0->GetSoftBody() : 0;
1638         btSoftBody* sb1 = c1 ? c1->GetSoftBody() : 0;
1639         if (sb0 && sb1)
1640         {
1641                 //not between two soft bodies?
1642                 return 0;
1643         }
1644
1645         if (sb0)
1646         {
1647                 //either cluster or node attach, let's find closest node first
1648                 //the soft body doesn't have a 'real' world transform, so get its initial world transform for now
1649                 btVector3 pivotPointSoftWorld = sb0->m_initialWorldTransform(pivotInA);
1650                 int node=findClosestNode(sb0,pivotPointSoftWorld);
1651                 if (node >=0)
1652                 {
1653                         bool clusterconstaint = false;
1654 /*
1655                         switch (type)
1656                         {
1657                         case PHY_LINEHINGE_CONSTRAINT:
1658                                 {
1659                                         if (sb0->clusterCount() && rb1)
1660                                         {
1661                                                 btSoftBody::LJoint::Specs       ls;
1662                                                 ls.erp=0.5f;
1663                                                 ls.position=sb0->clusterCom(0);
1664                                                 sb0->appendLinearJoint(ls,rb1);
1665                                                 clusterconstaint = true;
1666                                                 break;
1667                                         }
1668                                 }
1669                         case PHY_GENERIC_6DOF_CONSTRAINT:
1670                                 {
1671                                         if (sb0->clusterCount() && rb1)
1672                                         {
1673                                                 btSoftBody::AJoint::Specs as;
1674                                                 as.erp = 1;
1675                                                 as.cfm = 1;
1676                                                 as.axis.setValue(axisX,axisY,axisZ);
1677                                                 sb0->appendAngularJoint(as,rb1);
1678                                                 clusterconstaint = true;
1679                                                 break;
1680                                         }
1681
1682                                         break;
1683                                 }
1684                         default:
1685                                 {
1686
1687                                 }
1688                         };
1689                         */
1690
1691                         if (!clusterconstaint)
1692                         {
1693                                 if (rb1)
1694                                 {
1695                                         sb0->appendAnchor(node,rb1,disableCollisionBetweenLinkedBodies);
1696                                 } else
1697                                 {
1698                                         sb0->setMass(node,0.f);
1699                                 }
1700                         }
1701
1702
1703                 }
1704                 return 0;//can't remove soft body anchors yet
1705         }
1706
1707         if (sb1)
1708         {
1709                 btVector3 pivotPointAWorld = colObj0->getWorldTransform()(pivotInA);
1710                 int node=findClosestNode(sb1,pivotPointAWorld);
1711                 if (node >=0)
1712                 {
1713                         bool clusterconstaint = false;
1714
1715                         /*
1716                         switch (type)
1717                         {
1718                         case PHY_LINEHINGE_CONSTRAINT:
1719                                 {
1720                                         if (sb1->clusterCount() && rb0)
1721                                         {
1722                                                 btSoftBody::LJoint::Specs       ls;
1723                                                 ls.erp=0.5f;
1724                                                 ls.position=sb1->clusterCom(0);
1725                                                 sb1->appendLinearJoint(ls,rb0);
1726                                                 clusterconstaint = true;
1727                                                 break;
1728                                         }
1729                                 }
1730                         case PHY_GENERIC_6DOF_CONSTRAINT:
1731                                 {
1732                                         if (sb1->clusterCount() && rb0)
1733                                         {
1734                                                 btSoftBody::AJoint::Specs as;
1735                                                 as.erp = 1;
1736                                                 as.cfm = 1;
1737                                                 as.axis.setValue(axisX,axisY,axisZ);
1738                                                 sb1->appendAngularJoint(as,rb0);
1739                                                 clusterconstaint = true;
1740                                                 break;
1741                                         }
1742
1743                                         break;
1744                                 }
1745                         default:
1746                                 {
1747
1748
1749                                 }
1750                         };*/
1751
1752
1753                         if (!clusterconstaint)
1754                         {
1755                                 if (rb0)
1756                                 {
1757                                         sb1->appendAnchor(node,rb0,disableCollisionBetweenLinkedBodies);
1758                                 } else
1759                                 {
1760                                         sb1->setMass(node,0.f);
1761                                 }
1762                         }
1763
1764
1765                 }
1766                 return 0;//can't remove soft body anchors yet
1767         }
1768
1769         if (rb0static && rb1static)
1770         {
1771
1772                 return 0;
1773         }
1774
1775
1776         if (!rb0)
1777                 return 0;
1778
1779
1780         btVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) :
1781                 rb0->getCenterOfMassTransform() * pivotInA;
1782         btVector3 axisInA(axisX,axisY,axisZ);
1783
1784
1785         bool angularOnly = false;
1786
1787         switch (type)
1788         {
1789         case PHY_POINT2POINT_CONSTRAINT:
1790                 {
1791
1792                         btPoint2PointConstraint* p2p = 0;
1793
1794                         if (rb1)
1795                         {
1796                                 p2p = new btPoint2PointConstraint(*rb0,
1797                                         *rb1,pivotInA,pivotInB);
1798                         } else
1799                         {
1800                                 p2p = new btPoint2PointConstraint(*rb0,
1801                                         pivotInA);
1802                         }
1803
1804                         m_dynamicsWorld->addConstraint(p2p,disableCollisionBetweenLinkedBodies);
1805 //                      m_constraints.push_back(p2p);
1806
1807                         p2p->setUserConstraintId(gConstraintUid++);
1808                         p2p->setUserConstraintType(type);
1809                         //64 bit systems can't cast pointer to int. could use size_t instead.
1810                         return p2p->getUserConstraintId();
1811
1812                         break;
1813                 }
1814
1815         case PHY_GENERIC_6DOF_CONSTRAINT:
1816                 {
1817                         btGeneric6DofConstraint* genericConstraint = 0;
1818
1819                         if (rb1)
1820                         {
1821                                 btTransform frameInA;
1822                                 btTransform frameInB;
1823
1824                                 btVector3 axis1(axis1X,axis1Y,axis1Z), axis2(axis2X,axis2Y,axis2Z);
1825                                 if (axis1.length() == 0.0)
1826                                 {
1827                                         btPlaneSpace1( axisInA, axis1, axis2 );
1828                                 }
1829
1830                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1831                                                                   axisInA.y(), axis1.y(), axis2.y(),
1832                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1833                                 frameInA.setOrigin( pivotInA );
1834
1835                                 btTransform inv = rb1->getCenterOfMassTransform().inverse();
1836
1837                                 btTransform globalFrameA = rb0->getCenterOfMassTransform() * frameInA;
1838
1839                                 frameInB = inv  * globalFrameA;
1840                                 bool useReferenceFrameA = true;
1841
1842                                 genericConstraint = new btGeneric6DofConstraint(
1843                                         *rb0,*rb1,
1844                                         frameInA,frameInB,useReferenceFrameA);
1845
1846
1847                         } else
1848                         {
1849                                 static btRigidBody s_fixedObject2( 0,0,0);
1850                                 btTransform frameInA;
1851                                 btTransform frameInB;
1852
1853                                 btVector3 axis1, axis2;
1854                                 btPlaneSpace1( axisInA, axis1, axis2 );
1855
1856                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1857                                                                   axisInA.y(), axis1.y(), axis2.y(),
1858                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1859
1860                                 frameInA.setOrigin( pivotInA );
1861
1862                                 ///frameInB in worldspace
1863                                 frameInB = rb0->getCenterOfMassTransform() * frameInA;
1864
1865                                 bool useReferenceFrameA = true;
1866                                 genericConstraint = new btGeneric6DofConstraint(
1867                                         *rb0,s_fixedObject2,
1868                                         frameInA,frameInB,useReferenceFrameA);
1869                         }
1870
1871                         if (genericConstraint)
1872                         {
1873                                 //m_constraints.push_back(genericConstraint);
1874                                 m_dynamicsWorld->addConstraint(genericConstraint,disableCollisionBetweenLinkedBodies);
1875                                 genericConstraint->setUserConstraintId(gConstraintUid++);
1876                                 genericConstraint->setUserConstraintType(type);
1877                                 //64 bit systems can't cast pointer to int. could use size_t instead.
1878                                 return genericConstraint->getUserConstraintId();
1879                         }
1880
1881                         break;
1882                 }
1883         case PHY_CONE_TWIST_CONSTRAINT:
1884                 {
1885                         btConeTwistConstraint* coneTwistContraint = 0;
1886
1887
1888                         if (rb1)
1889                         {
1890                                 btTransform frameInA;
1891                                 btTransform frameInB;
1892
1893                                 btVector3 axis1(axis1X,axis1Y,axis1Z), axis2(axis2X,axis2Y,axis2Z);
1894                                 if (axis1.length() == 0.0)
1895                                 {
1896                                         btPlaneSpace1( axisInA, axis1, axis2 );
1897                                 }
1898
1899                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1900                                                                   axisInA.y(), axis1.y(), axis2.y(),
1901                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1902                                 frameInA.setOrigin( pivotInA );
1903
1904                                 btTransform inv = rb1->getCenterOfMassTransform().inverse();
1905
1906                                 btTransform globalFrameA = rb0->getCenterOfMassTransform() * frameInA;
1907
1908                                 frameInB = inv  * globalFrameA;
1909
1910                                 coneTwistContraint = new btConeTwistConstraint( *rb0,*rb1,
1911                                         frameInA,frameInB);
1912
1913
1914                         } else
1915                         {
1916                                 static btRigidBody s_fixedObject2( 0,0,0);
1917                                 btTransform frameInA;
1918                                 btTransform frameInB;
1919
1920                                 btVector3 axis1, axis2;
1921                                 btPlaneSpace1( axisInA, axis1, axis2 );
1922
1923                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1924                                                                   axisInA.y(), axis1.y(), axis2.y(),
1925                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1926
1927                                 frameInA.setOrigin( pivotInA );
1928
1929                                 ///frameInB in worldspace
1930                                 frameInB = rb0->getCenterOfMassTransform() * frameInA;
1931
1932                                 coneTwistContraint = new btConeTwistConstraint(
1933                                         *rb0,s_fixedObject2,
1934                                         frameInA,frameInB);
1935                         }
1936
1937                         if (coneTwistContraint)
1938                         {
1939                                 //m_constraints.push_back(genericConstraint);
1940                                 m_dynamicsWorld->addConstraint(coneTwistContraint,disableCollisionBetweenLinkedBodies);
1941                                 coneTwistContraint->setUserConstraintId(gConstraintUid++);
1942                                 coneTwistContraint->setUserConstraintType(type);
1943                                 //64 bit systems can't cast pointer to int. could use size_t instead.
1944                                 return coneTwistContraint->getUserConstraintId();
1945                         }
1946
1947
1948
1949                         break;
1950                 }
1951         case PHY_ANGULAR_CONSTRAINT:
1952                 angularOnly = true;
1953
1954
1955         case PHY_LINEHINGE_CONSTRAINT:
1956                 {
1957                         btHingeConstraint* hinge = 0;
1958
1959                         if (rb1)
1960                         {
1961                                 btVector3 axisInB = rb1 ?
1962                                 (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) :
1963                                 rb0->getCenterOfMassTransform().getBasis() * axisInA;
1964
1965                                 hinge = new btHingeConstraint(
1966                                         *rb0,
1967                                         *rb1,pivotInA,pivotInB,axisInA,axisInB);
1968
1969
1970                         } else
1971                         {
1972                                 hinge = new btHingeConstraint(*rb0,
1973                                         pivotInA,axisInA);
1974
1975                         }
1976                         hinge->setAngularOnly(angularOnly);
1977
1978                         //m_constraints.push_back(hinge);
1979                         m_dynamicsWorld->addConstraint(hinge,disableCollisionBetweenLinkedBodies);
1980                         hinge->setUserConstraintId(gConstraintUid++);
1981                         hinge->setUserConstraintType(type);
1982                         //64 bit systems can't cast pointer to int. could use size_t instead.
1983                         return hinge->getUserConstraintId();
1984                         break;
1985                 }
1986 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1987
1988         case PHY_VEHICLE_CONSTRAINT:
1989                 {
1990                         btRaycastVehicle::btVehicleTuning* tuning = new btRaycastVehicle::btVehicleTuning();
1991                         btRigidBody* chassis = rb0;
1992                         btDefaultVehicleRaycaster* raycaster = new btDefaultVehicleRaycaster(m_dynamicsWorld);
1993                         btRaycastVehicle* vehicle = new btRaycastVehicle(*tuning,chassis,raycaster);
1994                         WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
1995                         m_wrapperVehicles.push_back(wrapperVehicle);
1996                         m_dynamicsWorld->addVehicle(vehicle);
1997                         vehicle->setUserConstraintId(gConstraintUid++);
1998                         vehicle->setUserConstraintType(type);
1999                         return vehicle->getUserConstraintId();
2000
2001                         break;
2002                 };
2003 #endif //NEW_BULLET_VEHICLE_SUPPORT
2004
2005         default:
2006                 {
2007                 }
2008         };
2009
2010         //btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB
2011
2012         return 0;
2013
2014 }
2015
2016
2017
2018 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float coneradius,float coneheight)
2019 {
2020         CcdConstructionInfo     cinfo;
2021
2022         // we don't need a CcdShapeConstructionInfo for this shape:
2023         // it is simple enough for the standard copy constructor (see CcdPhysicsController::GetReplica)
2024         cinfo.m_collisionShape = new btConeShape(coneradius,coneheight);
2025         cinfo.m_MotionState = 0;
2026         cinfo.m_physicsEnv = this;
2027         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE;
2028         DefaultMotionState* motionState = new DefaultMotionState();
2029         cinfo.m_MotionState = motionState;
2030
2031         // we will add later the possibility to select the filter from option
2032         cinfo.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
2033         cinfo.m_collisionFilterGroup = CcdConstructionInfo::SensorFilter;
2034         motionState->m_worldTransform.setIdentity();
2035 //      motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
2036
2037         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
2038
2039
2040         return sphereController;
2041 }
2042
2043 float           CcdPhysicsEnvironment::getAppliedImpulse(int    constraintid)
2044 {
2045         int i;
2046         int numConstraints = m_dynamicsWorld->getNumConstraints();
2047         for (i=0;i<numConstraints;i++)
2048         {
2049                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
2050                 if (constraint->getUserConstraintId() == constraintid)
2051                 {
2052                         return constraint->getAppliedImpulse();
2053                 }
2054         }
2055
2056         return 0.f;
2057 }