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