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