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