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
  27 //profiling/timings
  28 #include "LinearMath/btQuickprof.h"
  29
  30
  31 #include "PHY_IMotionState.h"
  32
  33
  34 bool useIslands = true;
  35
  36 #ifdef NEW_BULLET_VEHICLE_SUPPORT
  37 #include "BulletDynamics/Vehicle/btRaycastVehicle.h"
  38 #include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
  39 #include "BulletDynamics/Vehicle/btWheelInfo.h"
  40 #include "PHY_IVehicle.h"
  41 btRaycastVehicle::btVehicleTuning       gTuning;
  42
  43 #endif //NEW_BULLET_VEHICLE_SUPPORT
  44 #include "LinearMath/btAabbUtil2.h"
  45
  46
  47 #ifdef WIN32
  48 void DrawRasterizerLine(const float* from,const float* to,int color);
  49 #endif
  50
  51
  52 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
  53
  54
  55 #include <stdio.h>
  56
  57 #ifdef NEW_BULLET_VEHICLE_SUPPORT
  58 class WrapperVehicle : public PHY_IVehicle
  59 {
  60
  61         btRaycastVehicle*       m_vehicle;
  62         PHY_IPhysicsController* m_chassis;
  63
  64 public:
  65
  66         WrapperVehicle(btRaycastVehicle* vehicle,PHY_IPhysicsController* chassis)
  67                 :m_vehicle(vehicle),
  68                 m_chassis(chassis)
  69         {
  70         }
  71
  72         btRaycastVehicle*       GetVehicle()
  73         {
  74                 return m_vehicle;
  75         }
  76
  77         PHY_IPhysicsController* GetChassis()
  78         {
  79                 return m_chassis;
  80         }
  81
  82         virtual void    AddWheel(
  83                 PHY_IMotionState*       motionState,
  84                 PHY__Vector3    connectionPoint,
  85                 PHY__Vector3    downDirection,
  86                 PHY__Vector3    axleDirection,
  87                 float   suspensionRestLength,
  88                 float wheelRadius,
  89                 bool hasSteering
  90                 )
  91         {
  92                 btVector3 connectionPointCS0(connectionPoint[0],connectionPoint[1],connectionPoint[2]);
  93                 btVector3 wheelDirectionCS0(downDirection[0],downDirection[1],downDirection[2]);
  94                 btVector3 wheelAxle(axleDirection[0],axleDirection[1],axleDirection[2]);
  95
  96
  97                 btWheelInfo& info = m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxle,
  98                         suspensionRestLength,wheelRadius,gTuning,hasSteering);
  99                 info.m_clientInfo = motionState;
 100
 101         }
 102
 103         void    SyncWheels()
 104         {
 105                 int numWheels = GetNumWheels();
 106                 int i;
 107                 for (i=0;i<numWheels;i++)
 108                 {
 109                         btWheelInfo& info = m_vehicle->getWheelInfo(i);
 110                         PHY_IMotionState* motionState = (PHY_IMotionState*)info.m_clientInfo ;
 111         //              m_vehicle->updateWheelTransformsWS(info,false);
 112                         m_vehicle->updateWheelTransform(i,false);
 113                         btTransform trans = m_vehicle->getWheelInfo(i).m_worldTransform;
 114                         btQuaternion orn = trans.getRotation();
 115                         const btVector3& pos = trans.getOrigin();
 116                         motionState->setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]);
 117                         motionState->setWorldPosition(pos.x(),pos.y(),pos.z());
 118
 119                 }
 120         }
 121
 122         virtual int             GetNumWheels() const
 123         {
 124                 return m_vehicle->getNumWheels();
 125         }
 126
 127         virtual void    GetWheelPosition(int wheelIndex,float& posX,float& posY,float& posZ) const
 128         {
 129                 btTransform     trans = m_vehicle->getWheelTransformWS(wheelIndex);
 130                 posX = trans.getOrigin().x();
 131                 posY = trans.getOrigin().y();
 132                 posZ = trans.getOrigin().z();
 133         }
 134         virtual void    GetWheelOrientationQuaternion(int wheelIndex,float& quatX,float& quatY,float& quatZ,float& quatW) const
 135         {
 136                 btTransform     trans = m_vehicle->getWheelTransformWS(wheelIndex);
 137                 btQuaternion quat = trans.getRotation();
 138                 btMatrix3x3 orn2(quat);
 139
 140                 quatX = trans.getRotation().x();
 141                 quatY = trans.getRotation().y();
 142                 quatZ = trans.getRotation().z();
 143                 quatW = trans.getRotation()[3];
 144
 145
 146                 //printf("test");
 147
 148
 149         }
 150
 151         virtual float   GetWheelRotation(int wheelIndex) const
 152         {
 153                 float rotation = 0.f;
 154
 155                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
 156                 {
 157                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
 158                         rotation = info.m_rotation;
 159                 }
 160                 return rotation;
 161
 162         }
 163
 164
 165
 166         virtual int     GetUserConstraintId() const
 167         {
 168                 return m_vehicle->getUserConstraintId();
 169         }
 170
 171         virtual int     GetUserConstraintType() const
 172         {
 173                 return m_vehicle->getUserConstraintType();
 174         }
 175
 176         virtual void    SetSteeringValue(float steering,int wheelIndex)
 177         {
 178                 m_vehicle->setSteeringValue(steering,wheelIndex);
 179         }
 180
 181         virtual void    ApplyEngineForce(float force,int wheelIndex)
 182         {
 183                 m_vehicle->applyEngineForce(force,wheelIndex);
 184         }
 185
 186         virtual void    ApplyBraking(float braking,int wheelIndex)
 187         {
 188                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
 189                 {
 190                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
 191                         info.m_brake = braking;
 192                 }
 193         }
 194
 195         virtual void    SetWheelFriction(float friction,int wheelIndex)
 196         {
 197                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
 198                 {
 199                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
 200                         info.m_frictionSlip = friction;
 201                 }
 202
 203         }
 204
 205         virtual void    SetSuspensionStiffness(float suspensionStiffness,int wheelIndex)
 206         {
 207                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
 208                 {
 209                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
 210                         info.m_suspensionStiffness = suspensionStiffness;
 211
 212                 }
 213         }
 214
 215         virtual void    SetSuspensionDamping(float suspensionDamping,int wheelIndex)
 216         {
 217                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
 218                 {
 219                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
 220                         info.m_wheelsDampingRelaxation = suspensionDamping;
 221                 }
 222         }
 223
 224         virtual void    SetSuspensionCompression(float suspensionCompression,int wheelIndex)
 225         {
 226                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
 227                 {
 228                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
 229                         info.m_wheelsDampingCompression = suspensionCompression;
 230                 }
 231         }
 232
 233
 234
 235         virtual void    SetRollInfluence(float rollInfluence,int wheelIndex)
 236         {
 237                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
 238                 {
 239                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
 240                         info.m_rollInfluence = rollInfluence;
 241                 }
 242         }
 243
 244         virtual void    SetCoordinateSystem(int rightIndex,int upIndex,int forwardIndex)
 245         {
 246                 m_vehicle->setCoordinateSystem(rightIndex,upIndex,forwardIndex);
 247         }
 248
 249
 250
 251 };
 252 #endif //NEW_BULLET_VEHICLE_SUPPORT
 253
 254
 255 void CcdPhysicsEnvironment::setDebugDrawer(btIDebugDraw* debugDrawer)
 256 {
 257         if (debugDrawer && m_dynamicsWorld)
 258                 m_dynamicsWorld->setDebugDrawer(debugDrawer);
 259         m_debugDrawer = debugDrawer;
 260 }
 261
 262 static void DrawAabb(btIDebugDraw* debugDrawer,const btVector3& from,const btVector3& to,const btVector3& color)
 263 {
 264         btVector3 halfExtents = (to-from)* 0.5f;
 265         btVector3 center = (to+from) *0.5f;
 266         int i,j;
 267
 268         btVector3 edgecoord(1.f,1.f,1.f),pa,pb;
 269         for (i=0;i<4;i++)
 270         {
 271                 for (j=0;j<3;j++)
 272                 {
 273                         pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],
 274                                 edgecoord[2]*halfExtents[2]);
 275                         pa+=center;
 276
 277                         int othercoord = j%3;
 278                         edgecoord[othercoord]*=-1.f;
 279                         pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],
 280                                 edgecoord[2]*halfExtents[2]);
 281                         pb+=center;
 282
 283                         debugDrawer->drawLine(pa,pb,color);
 284                 }
 285                 edgecoord = btVector3(-1.f,-1.f,-1.f);
 286                 if (i<3)
 287                         edgecoord[i]*=-1.f;
 288         }
 289
 290
 291 }
 292
 293
 294
 295
 296
 297
 298 CcdPhysicsEnvironment::CcdPhysicsEnvironment(btDispatcher* dispatcher,btOverlappingPairCache* pairCache)
 299 :m_scalingPropagated(false),
 300 m_numIterations(10),
 301 m_numTimeSubSteps(1),
 302 m_ccdMode(0),
 303 m_solverType(-1),
 304 m_profileTimings(0),
 305 m_enableSatCollisionDetection(false)
 306 {
 307
 308         for (int i=0;i<PHY_NUM_RESPONSE;i++)
 309         {
 310                 m_triggerCallbacks[i] = 0;
 311         }
 312         if (!dispatcher)
 313                 dispatcher = new btCollisionDispatcher();
 314
 315
 316         if(!pairCache)
 317         {
 318
 319                 //todo: calculate/let user specify this world sizes
 320                 btVector3 worldMin(-10000,-10000,-10000);
 321                 btVector3 worldMax(10000,10000,10000);
 322
 323                 pairCache = new btAxisSweep3(worldMin,worldMax);
 324
 325                 //broadphase = new btSimpleBroadphase();
 326         }
 327
 328
 329         setSolverType(1);//issues with quickstep and memory allocations
 330
 331         m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,pairCache,new btSequentialImpulseConstraintSolver());
 332         m_debugDrawer = 0;
 333         m_gravity = btVector3(0.f,-10.f,0.f);
 334         m_dynamicsWorld->setGravity(m_gravity);
 335
 336
 337 }
 338
 339 void    CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
 340 {
 341         btRigidBody* body = ctrl->GetRigidBody();
 342
 343         //this m_userPointer is just used for triggers, see CallbackTriggers
 344         body->setUserPointer(ctrl);
 345
 346         body->setGravity( m_gravity );
 347         m_controllers.push_back(ctrl);
 348
 349         m_dynamicsWorld->addRigidBody(body);
 350         if (body->isStaticOrKinematicObject())
 351         {
 352                 body->setActivationState(ISLAND_SLEEPING);
 353         }
 354
 355
 356         //CollisionObject(body,ctrl->GetCollisionFilterGroup(),ctrl->GetCollisionFilterMask());
 357
 358         assert(body->getBroadphaseHandle());
 359
 360         btBroadphaseInterface* scene =  getBroadphase();
 361
 362
 363         btCollisionShape* shapeinterface = ctrl->GetCollisionShape();
 364
 365         assert(shapeinterface);
 366
 367         const btTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform();
 368
 369
 370         btPoint3 minAabb,maxAabb;
 371
 372         shapeinterface->getAabb(t,minAabb,maxAabb);
 373
 374         float timeStep = 0.02f;
 375
 376
 377         //extent it with the motion
 378
 379         btVector3 linMotion = body->getLinearVelocity()*timeStep;
 380
 381         float maxAabbx = maxAabb.getX();
 382         float maxAabby = maxAabb.getY();
 383         float maxAabbz = maxAabb.getZ();
 384         float minAabbx = minAabb.getX();
 385         float minAabby = minAabb.getY();
 386         float minAabbz = minAabb.getZ();
 387
 388         if (linMotion.x() > 0.f)
 389                 maxAabbx += linMotion.x();
 390         else
 391                 minAabbx += linMotion.x();
 392         if (linMotion.y() > 0.f)
 393                 maxAabby += linMotion.y();
 394         else
 395                 minAabby += linMotion.y();
 396         if (linMotion.z() > 0.f)
 397                 maxAabbz += linMotion.z();
 398         else
 399                 minAabbz += linMotion.z();
 400
 401
 402         minAabb = btVector3(minAabbx,minAabby,minAabbz);
 403         maxAabb = btVector3(maxAabbx,maxAabby,maxAabbz);
 404
 405
 406
 407
 408 }
 409
 410 void    CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctrl)
 411 {
 412
 413         //also remove constraint
 414
 415
 416
 417         m_dynamicsWorld->removeRigidBody(ctrl->GetRigidBody());
 418
 419
 420         {
 421                 std::vector<CcdPhysicsController*>::iterator i =
 422                         std::find(m_controllers.begin(), m_controllers.end(), ctrl);
 423                 if (!(i == m_controllers.end()))
 424                 {
 425                         std::swap(*i, m_controllers.back());
 426                         m_controllers.pop_back();
 427                 }
 428         }
 429
 430         //remove it from the triggers
 431         {
 432                 std::vector<CcdPhysicsController*>::iterator i =
 433                         std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl);
 434                 if (!(i == m_triggerControllers.end()))
 435                 {
 436                         std::swap(*i, m_triggerControllers.back());
 437                         m_triggerControllers.pop_back();
 438                 }
 439         }
 440
 441
 442 }
 443
 444
 445 void    CcdPhysicsEnvironment::beginFrame()
 446 {
 447
 448 }
 449
 450
 451 bool    CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
 452 {
 453
 454         int i,numCtrl = GetNumControllers();
 455         for (i=0;i<numCtrl;i++)
 456         {
 457                 CcdPhysicsController* ctrl = GetPhysicsController(i);
 458                 ctrl->SynchronizeMotionStates(timeStep);
 459         }
 460
 461         m_dynamicsWorld->stepSimulation(timeStep,0);//perform always a full simulation step
 462
 463         numCtrl = GetNumControllers();
 464         for (i=0;i<numCtrl;i++)
 465         {
 466                 CcdPhysicsController* ctrl = GetPhysicsController(i);
 467                 ctrl->SynchronizeMotionStates(timeStep);
 468         }
 469
 470         for (i=0;i<m_wrapperVehicles.size();i++)
 471         {
 472                 WrapperVehicle* veh = m_wrapperVehicles[i];
 473                 veh->SyncWheels();
 474         }
 475
 476         CallbackTriggers();
 477
 478         return true;
 479 }
 480
 481
 482 void            CcdPhysicsEnvironment::setDebugMode(int debugMode)
 483 {
 484         if (m_debugDrawer){
 485                 m_debugDrawer->setDebugMode(debugMode);
 486         }
 487 }
 488
 489 void            CcdPhysicsEnvironment::setNumIterations(int numIter)
 490 {
 491         m_numIterations = numIter;
 492 }
 493 void            CcdPhysicsEnvironment::setDeactivationTime(float dTime)
 494 {
 495         gDeactivationTime = dTime;
 496 }
 497 void            CcdPhysicsEnvironment::setDeactivationLinearTreshold(float linTresh)
 498 {
 499         gLinearSleepingTreshold = linTresh;
 500 }
 501 void            CcdPhysicsEnvironment::setDeactivationAngularTreshold(float angTresh)
 502 {
 503         gAngularSleepingTreshold = angTresh;
 504 }
 505
 506 void            CcdPhysicsEnvironment::setContactBreakingTreshold(float contactBreakingTreshold)
 507 {
 508         gContactBreakingThreshold = contactBreakingTreshold;
 509
 510 }
 511
 512
 513 void            CcdPhysicsEnvironment::setCcdMode(int ccdMode)
 514 {
 515         m_ccdMode = ccdMode;
 516 }
 517
 518
 519 void            CcdPhysicsEnvironment::setSolverSorConstant(float sor)
 520 {
 521         m_solverInfo.m_sor = sor;
 522 }
 523
 524 void            CcdPhysicsEnvironment::setSolverTau(float tau)
 525 {
 526         m_solverInfo.m_tau = tau;
 527 }
 528 void            CcdPhysicsEnvironment::setSolverDamping(float damping)
 529 {
 530         m_solverInfo.m_damping = damping;
 531 }
 532
 533
 534 void            CcdPhysicsEnvironment::setLinearAirDamping(float damping)
 535 {
 536         gLinearAirDamping = damping;
 537 }
 538
 539 void            CcdPhysicsEnvironment::setUseEpa(bool epa)
 540 {
 541         //gUseEpa = epa;
 542 }
 543
 544 void            CcdPhysicsEnvironment::setSolverType(int solverType)
 545 {
 546
 547         switch (solverType)
 548         {
 549         case 1:
 550                 {
 551                         if (m_solverType != solverType)
 552                         {
 553
 554                                 m_solver = new btSequentialImpulseConstraintSolver();
 555
 556                                 break;
 557                         }
 558                 }
 559
 560         case 0:
 561         default:
 562                 if (m_solverType != solverType)
 563                 {
 564 //                      m_solver = new OdeConstraintSolver();
 565
 566                         break;
 567                 }
 568
 569         };
 570
 571         m_solverType = solverType ;
 572 }
 573
 574
 575
 576
 577
 578
 579 void            CcdPhysicsEnvironment::setGravity(float x,float y,float z)
 580 {
 581         m_gravity = btVector3(x,y,z);
 582         m_dynamicsWorld->setGravity(m_gravity);
 583
 584 }
 585
 586
 587
 588
 589 static int gConstraintUid = 1;
 590
 591 //Following the COLLADA physics specification for constraints
 592 int                     CcdPhysicsEnvironment::createUniversalD6Constraint(
 593                                                 class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
 594                                                 btTransform& frameInA,
 595                                                 btTransform& frameInB,
 596                                                 const btVector3& linearMinLimits,
 597                                                 const btVector3& linearMaxLimits,
 598                                                 const btVector3& angularMinLimits,
 599                                                 const btVector3& angularMaxLimits
 600 )
 601 {
 602
 603         //we could either add some logic to recognize ball-socket and hinge, or let that up to the user
 604         //perhaps some warning or hint that hinge/ball-socket is more efficient?
 605
 606         btGeneric6DofConstraint* genericConstraint = 0;
 607         CcdPhysicsController* ctrl0 = (CcdPhysicsController*) ctrlRef;
 608         CcdPhysicsController* ctrl1 = (CcdPhysicsController*) ctrlOther;
 609
 610         btRigidBody* rb0 = ctrl0->GetRigidBody();
 611         btRigidBody* rb1 = ctrl1->GetRigidBody();
 612
 613         if (rb1)
 614         {
 615
 616
 617                 genericConstraint = new btGeneric6DofConstraint(
 618                         *rb0,*rb1,
 619                         frameInA,frameInB);
 620                 genericConstraint->setLinearLowerLimit(linearMinLimits);
 621                 genericConstraint->setLinearUpperLimit(linearMaxLimits);
 622                 genericConstraint->setAngularLowerLimit(angularMinLimits);
 623                 genericConstraint->setAngularUpperLimit(angularMaxLimits);
 624         } else
 625         {
 626                 // TODO: Implement single body case...
 627                 //No, we can use a fixed rigidbody in above code, rather then unnecessary duplation of code
 628
 629         }
 630
 631         if (genericConstraint)
 632         {
 633         //      m_constraints.push_back(genericConstraint);
 634                 m_dynamicsWorld->addConstraint(genericConstraint);
 635
 636                 genericConstraint->setUserConstraintId(gConstraintUid++);
 637                 genericConstraint->setUserConstraintType(PHY_GENERIC_6DOF_CONSTRAINT);
 638                 //64 bit systems can't cast pointer to int. could use size_t instead.
 639                 return genericConstraint->getUserConstraintId();
 640         }
 641         return 0;
 642 }
 643
 644
 645
 646 void            CcdPhysicsEnvironment::removeConstraint(int     constraintId)
 647 {
 648
 649         int i;
 650         int numConstraints = m_dynamicsWorld->getNumConstraints();
 651         for (i=0;i<numConstraints;i++)
 652         {
 653                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
 654                 if (constraint->getUserConstraintId() == constraintId)
 655                 {
 656                         m_dynamicsWorld->removeConstraint(constraint);
 657                         break;
 658                 }
 659         }
 660 }
 661
 662
 663 struct  FilterClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
 664 {
 665         PHY_IPhysicsController* m_ignoreClient;
 666
 667         FilterClosestRayResultCallback (PHY_IPhysicsController* ignoreClient,const btVector3& rayFrom,const btVector3& rayTo)
 668                 : btCollisionWorld::ClosestRayResultCallback(rayFrom,rayTo),
 669                 m_ignoreClient(ignoreClient)
 670         {
 671
 672         }
 673
 674         virtual ~FilterClosestRayResultCallback()
 675         {
 676         }
 677
 678         virtual float   AddSingleResult( btCollisionWorld::LocalRayResult& rayResult)
 679         {
 680                 CcdPhysicsController* curHit = static_cast<CcdPhysicsController*>(rayResult.m_collisionObject->getUserPointer());
 681                 //ignore client...
 682                 if (curHit != m_ignoreClient)
 683                 {
 684                         //if valid
 685                         return ClosestRayResultCallback::AddSingleResult(rayResult);
 686                 }
 687                 return m_closestHitFraction;
 688         }
 689
 690 };
 691
 692 PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ,
 693                                                                                                            float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
 694 {
 695
 696
 697         float minFraction = 1.f;
 698
 699         btVector3 rayFrom(fromX,fromY,fromZ);
 700         btVector3 rayTo(toX,toY,toZ);
 701
 702         btVector3       hitPointWorld,normalWorld;
 703
 704         //Either Ray Cast with or without filtering
 705
 706         //btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo);
 707         FilterClosestRayResultCallback   rayCallback(ignoreClient,rayFrom,rayTo);
 708
 709
 710         PHY_IPhysicsController* nearestHit = 0;
 711
 712         m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback);
 713         if (rayCallback.HasHit())
 714         {
 715                 nearestHit = static_cast<CcdPhysicsController*>(rayCallback.m_collisionObject->getUserPointer());
 716                 hitX =  rayCallback.m_hitPointWorld.getX();
 717                 hitY =  rayCallback.m_hitPointWorld.getY();
 718                 hitZ =  rayCallback.m_hitPointWorld.getZ();
 719
 720                 normalX = rayCallback.m_hitNormalWorld.getX();
 721                 normalY = rayCallback.m_hitNormalWorld.getY();
 722                 normalZ = rayCallback.m_hitNormalWorld.getZ();
 723
 724         }
 725
 726
 727         return nearestHit;
 728 }
 729
 730
 731
 732 int     CcdPhysicsEnvironment::getNumContactPoints()
 733 {
 734         return 0;
 735 }
 736
 737 void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
 738 {
 739
 740 }
 741
 742
 743
 744
 745 btBroadphaseInterface*  CcdPhysicsEnvironment::getBroadphase()
 746 {
 747         return m_dynamicsWorld->getBroadphase();
 748 }
 749
 750
 751
 752
 753 CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
 754 {
 755
 756 #ifdef NEW_BULLET_VEHICLE_SUPPORT
 757         m_wrapperVehicles.clear();
 758 #endif //NEW_BULLET_VEHICLE_SUPPORT
 759
 760         //m_broadphase->DestroyScene();
 761         //delete broadphase ? release reference on broadphase ?
 762
 763         //first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher
 764         //delete m_dispatcher;
 765         delete m_dynamicsWorld;
 766
 767
 768
 769 }
 770
 771
 772 int     CcdPhysicsEnvironment::GetNumControllers()
 773 {
 774         return m_controllers.size();
 775 }
 776
 777
 778 CcdPhysicsController* CcdPhysicsEnvironment::GetPhysicsController( int index)
 779 {
 780         return m_controllers[index];
 781 }
 782
 783
 784
 785
 786 void    CcdPhysicsEnvironment::setConstraintParam(int constraintId,int param,float value0,float value1)
 787 {
 788         btTypedConstraint* typedConstraint = getConstraintById(constraintId);
 789         switch (typedConstraint->getUserConstraintType())
 790         {
 791         case PHY_GENERIC_6DOF_CONSTRAINT:
 792                 {
 793                         //param = 1..12, min0,max0,min1,max1...min6,max6
 794                         btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
 795                         genCons->SetLimit(param,value0,value1);
 796                         break;
 797                 };
 798         default:
 799                 {
 800                 };
 801         };
 802 }
 803
 804 btTypedConstraint*      CcdPhysicsEnvironment::getConstraintById(int constraintId)
 805 {
 806
 807         int numConstraints = m_dynamicsWorld->getNumConstraints();
 808         int i;
 809         for (i=0;i<numConstraints;i++)
 810         {
 811                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
 812                 if (constraint->getUserConstraintId()==constraintId)
 813                 {
 814                         return constraint;
 815                 }
 816         }
 817         return 0;
 818 }
 819
 820
 821 void CcdPhysicsEnvironment::addSensor(PHY_IPhysicsController* ctrl)
 822 {
 823
 824         CcdPhysicsController* ctrl1 = (CcdPhysicsController* )ctrl;
 825         std::vector<CcdPhysicsController*>::iterator i =
 826                 std::find(m_controllers.begin(), m_controllers.end(), ctrl);
 827         if ((i == m_controllers.end()))
 828         {
 829                 addCcdPhysicsController(ctrl1);
 830         }
 831         //force collision detection with everything, including static objects (might hurt performance!)
 832         ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterMask = btBroadphaseProxy::AllFilter;
 833         ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterGroup = btBroadphaseProxy::AllFilter;
 834         //todo: make this 'sensor'!
 835
 836         requestCollisionCallback(ctrl);
 837         //printf("addSensor\n");
 838 }
 839
 840 void CcdPhysicsEnvironment::removeCollisionCallback(PHY_IPhysicsController* ctrl)
 841 {
 842         std::vector<CcdPhysicsController*>::iterator i =
 843                 std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl);
 844         if (!(i == m_triggerControllers.end()))
 845         {
 846                 std::swap(*i, m_triggerControllers.back());
 847                 m_triggerControllers.pop_back();
 848         }
 849 }
 850
 851
 852 void CcdPhysicsEnvironment::removeSensor(PHY_IPhysicsController* ctrl)
 853 {
 854         removeCollisionCallback(ctrl);
 855         //printf("removeSensor\n");
 856 }
 857 void CcdPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user)
 858 {
 859         /*      printf("addTouchCallback\n(response class = %i)\n",response_class);
 860
 861         //map PHY_ convention into SM_ convention
 862         switch (response_class)
 863         {
 864         case    PHY_FH_RESPONSE:
 865         printf("PHY_FH_RESPONSE\n");
 866         break;
 867         case PHY_SENSOR_RESPONSE:
 868         printf("PHY_SENSOR_RESPONSE\n");
 869         break;
 870         case PHY_CAMERA_RESPONSE:
 871         printf("PHY_CAMERA_RESPONSE\n");
 872         break;
 873         case PHY_OBJECT_RESPONSE:
 874         printf("PHY_OBJECT_RESPONSE\n");
 875         break;
 876         case PHY_STATIC_RESPONSE:
 877         printf("PHY_STATIC_RESPONSE\n");
 878         break;
 879         default:
 880         assert(0);
 881         return;
 882         }
 883         */
 884
 885         m_triggerCallbacks[response_class] = callback;
 886         m_triggerCallbacksUserPtrs[response_class] = user;
 887
 888 }
 889 void CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctrl)
 890 {
 891         CcdPhysicsController* ccdCtrl = static_cast<CcdPhysicsController*>(ctrl);
 892
 893         //printf("requestCollisionCallback\n");
 894         m_triggerControllers.push_back(ccdCtrl);
 895 }
 896
 897
 898 void    CcdPhysicsEnvironment::CallbackTriggers()
 899 {
 900
 901         CcdPhysicsController* ctrl0=0,*ctrl1=0;
 902
 903         if (m_triggerCallbacks[PHY_OBJECT_RESPONSE] || (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)))
 904         {
 905                 //walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback
 906                 int numManifolds = m_dynamicsWorld->getDispatcher()->getNumManifolds();
 907                 for (int i=0;i<numManifolds;i++)
 908                 {
 909                         btPersistentManifold* manifold = m_dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i);
 910                         int numContacts = manifold->getNumContacts();
 911                         if (numContacts)
 912                         {
 913                                 if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints))
 914                                 {
 915                                         for (int j=0;j<numContacts;j++)
 916                                         {
 917                                                 btVector3 color(1,0,0);
 918                                                 const btManifoldPoint& cp = manifold->getContactPoint(j);
 919                                                 if (m_debugDrawer)
 920                                                         m_debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
 921                                         }
 922                                 }
 923                                 btRigidBody* obj0 = static_cast<btRigidBody* >(manifold->getBody0());
 924                                 btRigidBody* obj1 = static_cast<btRigidBody* >(manifold->getBody1());
 925
 926                                 //m_internalOwner is set in 'addPhysicsController'
 927                                 CcdPhysicsController* ctrl0 = static_cast<CcdPhysicsController*>(obj0->getUserPointer());
 928                                 CcdPhysicsController* ctrl1 = static_cast<CcdPhysicsController*>(obj1->getUserPointer());
 929
 930                                 std::vector<CcdPhysicsController*>::iterator i =
 931                                         std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl0);
 932                                 if (i == m_triggerControllers.end())
 933                                 {
 934                                         i = std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl1);
 935                                 }
 936
 937                                 if (!(i == m_triggerControllers.end()))
 938                                 {
 939                                         m_triggerCallbacks[PHY_OBJECT_RESPONSE](m_triggerCallbacksUserPtrs[PHY_OBJECT_RESPONSE],
 940                                                 ctrl0,ctrl1,0);
 941                                 }
 942                         }
 943                 }
 944
 945
 946
 947         }
 948
 949
 950 }
 951
 952
 953
 954
 955
 956
 957 #ifdef NEW_BULLET_VEHICLE_SUPPORT
 958
 959 //complex constraint for vehicles
 960 PHY_IVehicle*   CcdPhysicsEnvironment::getVehicleConstraint(int constraintId)
 961 {
 962         int i;
 963
 964         int numVehicles = m_wrapperVehicles.size();
 965         for (i=0;i<numVehicles;i++)
 966         {
 967                 WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
 968                 if (wrapperVehicle->GetVehicle()->getUserConstraintId() == constraintId)
 969                         return wrapperVehicle;
 970         }
 971
 972         return 0;
 973 }
 974
 975 #endif //NEW_BULLET_VEHICLE_SUPPORT
 976
 977
 978 int currentController = 0;
 979 int numController = 0;
 980
 981
 982
 983
 984 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateSphereController(float radius,const PHY__Vector3& position)
 985 {
 986
 987         CcdConstructionInfo     cinfo;
 988         cinfo.m_collisionShape = new btSphereShape(radius);
 989         cinfo.m_MotionState = 0;
 990         cinfo.m_physicsEnv = this;
 991         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE | btCollisionObject::CF_KINEMATIC_OBJECT;
 992         DefaultMotionState* motionState = new DefaultMotionState();
 993         cinfo.m_MotionState = motionState;
 994         motionState->m_worldTransform.setIdentity();
 995         motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
 996
 997         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
 998
 999
1000         return sphereController;
1001 }
1002
1003 int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
1004                                                                                                         float pivotX,float pivotY,float pivotZ,
1005                                                                                                         float axisX,float axisY,float axisZ,
1006                                                                                                         float axis1X,float axis1Y,float axis1Z,
1007                                                                                                         float axis2X,float axis2Y,float axis2Z
1008                                                                                                         )
1009 {
1010
1011
1012         CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
1013         CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
1014
1015         btRigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
1016         btRigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
1017
1018         bool rb0static = rb0 ? rb0->isStaticOrKinematicObject() : true;
1019         bool rb1static = rb1 ? rb1->isStaticOrKinematicObject() : true;
1020
1021
1022         if (rb0static && rb1static)
1023                 return 0;
1024
1025         btVector3 pivotInA(pivotX,pivotY,pivotZ);
1026         btVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) :
1027                 rb0->getCenterOfMassTransform() * pivotInA;
1028         btVector3 axisInA(axisX,axisY,axisZ);
1029         btVector3 axisInB = rb1 ?
1030                 (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) :
1031         rb0->getCenterOfMassTransform().getBasis() * axisInA;
1032
1033         bool angularOnly = false;
1034
1035         switch (type)
1036         {
1037         case PHY_POINT2POINT_CONSTRAINT:
1038                 {
1039
1040                         btPoint2PointConstraint* p2p = 0;
1041
1042                         if (rb1)
1043                         {
1044                                 p2p = new btPoint2PointConstraint(*rb0,
1045                                         *rb1,pivotInA,pivotInB);
1046                         } else
1047                         {
1048                                 p2p = new btPoint2PointConstraint(*rb0,
1049                                         pivotInA);
1050                         }
1051
1052                         m_dynamicsWorld->addConstraint(p2p);
1053 //                      m_constraints.push_back(p2p);
1054
1055                         p2p->setUserConstraintId(gConstraintUid++);
1056                         p2p->setUserConstraintType(type);
1057                         //64 bit systems can't cast pointer to int. could use size_t instead.
1058                         return p2p->getUserConstraintId();
1059
1060                         break;
1061                 }
1062
1063         case PHY_GENERIC_6DOF_CONSTRAINT:
1064                 {
1065                         btGeneric6DofConstraint* genericConstraint = 0;
1066
1067                         if (rb1)
1068                         {
1069                                 btTransform frameInA;
1070                                 btTransform frameInB;
1071
1072                                 btVector3 axis1(axis1X,axis1Y,axis1Z), axis2(axis2X,axis2Y,axis2Z);
1073                                 if (axis1.length() == 0.0)
1074                                 {
1075                                         btPlaneSpace1( axisInA, axis1, axis2 );
1076                                 }
1077
1078                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1079                                                                   axisInA.y(), axis1.y(), axis2.y(),
1080                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1081
1082
1083                                 btPlaneSpace1( axisInB, axis1, axis2 );
1084                                 frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(),
1085                                                                   axisInB.y(), axis1.y(), axis2.y(),
1086                                                                                           axisInB.z(), axis1.z(), axis2.z() );
1087
1088                                 frameInA.setOrigin( pivotInA );
1089                                 frameInB.setOrigin( pivotInB );
1090
1091                                 genericConstraint = new btGeneric6DofConstraint(
1092                                         *rb0,*rb1,
1093                                         frameInA,frameInB);
1094
1095
1096                         } else
1097                         {
1098                                 static btRigidBody s_fixedObject2( 0,0,0);
1099                                         btTransform frameInA;
1100                                 btTransform frameInB;
1101
1102                                 btVector3 axis1, axis2;
1103                                 btPlaneSpace1( axisInA, axis1, axis2 );
1104
1105                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1106                                                                   axisInA.y(), axis1.y(), axis2.y(),
1107                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1108
1109
1110                                 btPlaneSpace1( axisInB, axis1, axis2 );
1111                                 frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(),
1112                                                                   axisInB.y(), axis1.y(), axis2.y(),
1113                                                                                           axisInB.z(), axis1.z(), axis2.z() );
1114
1115                                 frameInA.setOrigin( pivotInA );
1116                                 frameInB.setOrigin( pivotInB );
1117
1118
1119                                 genericConstraint = new btGeneric6DofConstraint(
1120                                         *rb0,s_fixedObject2,
1121                                         frameInA,frameInB);
1122                         }
1123
1124                         if (genericConstraint)
1125                         {
1126                                 //m_constraints.push_back(genericConstraint);
1127                                 m_dynamicsWorld->addConstraint(genericConstraint);
1128                                 genericConstraint->setUserConstraintId(gConstraintUid++);
1129                                 genericConstraint->setUserConstraintType(type);
1130                                 //64 bit systems can't cast pointer to int. could use size_t instead.
1131                                 return genericConstraint->getUserConstraintId();
1132                         }
1133
1134                         break;
1135                 }
1136         case PHY_ANGULAR_CONSTRAINT:
1137                 angularOnly = true;
1138
1139
1140         case PHY_LINEHINGE_CONSTRAINT:
1141                 {
1142                         btHingeConstraint* hinge = 0;
1143
1144                         if (rb1)
1145                         {
1146                                 hinge = new btHingeConstraint(
1147                                         *rb0,
1148                                         *rb1,pivotInA,pivotInB,axisInA,axisInB);
1149
1150
1151                         } else
1152                         {
1153                                 hinge = new btHingeConstraint(*rb0,
1154                                         pivotInA,axisInA);
1155
1156                         }
1157                         hinge->setAngularOnly(angularOnly);
1158
1159                         //m_constraints.push_back(hinge);
1160                         m_dynamicsWorld->addConstraint(hinge);
1161                         hinge->setUserConstraintId(gConstraintUid++);
1162                         hinge->setUserConstraintType(type);
1163                         //64 bit systems can't cast pointer to int. could use size_t instead.
1164                         return hinge->getUserConstraintId();
1165                         break;
1166                 }
1167 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1168
1169         case PHY_VEHICLE_CONSTRAINT:
1170                 {
1171                         btRaycastVehicle::btVehicleTuning* tuning = new btRaycastVehicle::btVehicleTuning();
1172                         btRigidBody* chassis = rb0;
1173                         btDefaultVehicleRaycaster* raycaster = new btDefaultVehicleRaycaster(m_dynamicsWorld);
1174                         btRaycastVehicle* vehicle = new btRaycastVehicle(*tuning,chassis,raycaster);
1175                         WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
1176                         m_wrapperVehicles.push_back(wrapperVehicle);
1177                         m_dynamicsWorld->addVehicle(vehicle);
1178                         vehicle->setUserConstraintId(gConstraintUid++);
1179                         vehicle->setUserConstraintType(type);
1180                         return vehicle->getUserConstraintId();
1181
1182                         break;
1183                 };
1184 #endif //NEW_BULLET_VEHICLE_SUPPORT
1185
1186         default:
1187                 {
1188                 }
1189         };
1190
1191         //btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB
1192
1193         return 0;
1194
1195 }
1196
1197
1198
1199 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float coneradius,float coneheight)
1200 {
1201         CcdConstructionInfo     cinfo;
1202         cinfo.m_collisionShape = new btConeShape(coneradius,coneheight);
1203         cinfo.m_MotionState = 0;
1204         cinfo.m_physicsEnv = this;
1205         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE;
1206         DefaultMotionState* motionState = new DefaultMotionState();
1207         cinfo.m_MotionState = motionState;
1208         motionState->m_worldTransform.setIdentity();
1209 //      motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
1210
1211         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
1212
1213
1214         return sphereController;
1215 }
1216
1217 float           CcdPhysicsEnvironment::getAppliedImpulse(int    constraintid)
1218 {
1219         int i;
1220         int numConstraints = m_dynamicsWorld->getNumConstraints();
1221         for (i=0;i<numConstraints;i++)
1222         {
1223                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
1224                 if (constraint->getUserConstraintId() == constraintid)
1225                 {
1226                         return constraint->getAppliedImpulse();
1227                 }
1228         }
1229
1230         return 0.f;
1231 }