b951602fe1eb8f86218a7a3075762d127164c62e
[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
818         requestCollisionCallback(ctrl);
819         //printf("addSensor\n");
820 }
821
822 void CcdPhysicsEnvironment::removeCollisionCallback(PHY_IPhysicsController* ctrl)
823 {
824         std::vector<CcdPhysicsController*>::iterator i =
825                 std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl);
826         if (!(i == m_triggerControllers.end()))
827         {
828                 std::swap(*i, m_triggerControllers.back());
829                 m_triggerControllers.pop_back();
830         }
831 }
832
833
834 void CcdPhysicsEnvironment::removeSensor(PHY_IPhysicsController* ctrl)
835 {
836         removeCollisionCallback(ctrl);
837         //printf("removeSensor\n");
838 }
839 void CcdPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user)
840 {
841         /*      printf("addTouchCallback\n(response class = %i)\n",response_class);
842
843         //map PHY_ convention into SM_ convention
844         switch (response_class)
845         {
846         case    PHY_FH_RESPONSE:
847         printf("PHY_FH_RESPONSE\n");
848         break;
849         case PHY_SENSOR_RESPONSE:
850         printf("PHY_SENSOR_RESPONSE\n");
851         break;
852         case PHY_CAMERA_RESPONSE:
853         printf("PHY_CAMERA_RESPONSE\n");
854         break;
855         case PHY_OBJECT_RESPONSE:
856         printf("PHY_OBJECT_RESPONSE\n");
857         break;
858         case PHY_STATIC_RESPONSE:
859         printf("PHY_STATIC_RESPONSE\n");
860         break;
861         default:
862         assert(0);
863         return;
864         }
865         */
866
867         m_triggerCallbacks[response_class] = callback;
868         m_triggerCallbacksUserPtrs[response_class] = user;
869
870 }
871 void CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctrl)
872 {
873         CcdPhysicsController* ccdCtrl = static_cast<CcdPhysicsController*>(ctrl);
874
875         //printf("requestCollisionCallback\n");
876         m_triggerControllers.push_back(ccdCtrl);
877 }
878
879
880 void    CcdPhysicsEnvironment::CallbackTriggers()
881 {
882         
883         CcdPhysicsController* ctrl0=0,*ctrl1=0;
884
885         if (m_triggerCallbacks[PHY_OBJECT_RESPONSE] || (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)))
886         {
887                 //walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback
888                 int numManifolds = m_dynamicsWorld->getDispatcher()->getNumManifolds();
889                 for (int i=0;i<numManifolds;i++)
890                 {
891                         btPersistentManifold* manifold = m_dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i);
892                         int numContacts = manifold->getNumContacts();
893                         if (numContacts)
894                         {
895                                 if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints))
896                                 {
897                                         for (int j=0;j<numContacts;j++)
898                                         {
899                                                 btVector3 color(1,0,0);
900                                                 const btManifoldPoint& cp = manifold->getContactPoint(j);
901                                                 if (m_debugDrawer)
902                                                         m_debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
903                                         }
904                                 }
905                                 btRigidBody* obj0 = static_cast<btRigidBody* >(manifold->getBody0());
906                                 btRigidBody* obj1 = static_cast<btRigidBody* >(manifold->getBody1());
907
908                                 //m_internalOwner is set in 'addPhysicsController'
909                                 CcdPhysicsController* ctrl0 = static_cast<CcdPhysicsController*>(obj0->getUserPointer());
910                                 CcdPhysicsController* ctrl1 = static_cast<CcdPhysicsController*>(obj1->getUserPointer());
911
912                                 std::vector<CcdPhysicsController*>::iterator i =
913                                         std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl0);
914                                 if (i == m_triggerControllers.end())
915                                 {
916                                         i = std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl1);
917                                 }
918
919                                 if (!(i == m_triggerControllers.end()))
920                                 {
921                                         m_triggerCallbacks[PHY_OBJECT_RESPONSE](m_triggerCallbacksUserPtrs[PHY_OBJECT_RESPONSE],
922                                                 ctrl0,ctrl1,0);
923                                 }
924                         }
925                 }
926
927
928
929         }
930
931
932 }
933
934
935
936
937
938
939 #ifdef NEW_BULLET_VEHICLE_SUPPORT
940
941 //complex constraint for vehicles
942 PHY_IVehicle*   CcdPhysicsEnvironment::getVehicleConstraint(int constraintId)
943 {
944         int i;
945
946         int numVehicles = m_wrapperVehicles.size();
947         for (i=0;i<numVehicles;i++)
948         {
949                 WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
950                 if (wrapperVehicle->GetVehicle()->getUserConstraintId() == constraintId)
951                         return wrapperVehicle;
952         }
953
954         return 0;
955 }
956
957 #endif //NEW_BULLET_VEHICLE_SUPPORT
958
959
960 int currentController = 0;
961 int numController = 0;
962
963
964
965
966 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateSphereController(float radius,const PHY__Vector3& position)
967 {
968         
969         CcdConstructionInfo     cinfo;
970         cinfo.m_collisionShape = new btSphereShape(radius);
971         cinfo.m_MotionState = 0;
972         cinfo.m_physicsEnv = this;
973         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE;
974         DefaultMotionState* motionState = new DefaultMotionState();
975         cinfo.m_MotionState = motionState;
976         motionState->m_worldTransform.setIdentity();
977         motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
978
979         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
980         
981
982         return sphereController;
983 }
984
985 int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
986                                                                                                         float pivotX,float pivotY,float pivotZ,
987                                                                                                         float axisX,float axisY,float axisZ)
988 {
989
990
991         CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
992         CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
993
994         btRigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
995         btRigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
996
997         bool rb0static = rb0 ? rb0->isStaticOrKinematicObject() : true;
998         bool rb1static = rb1 ? rb1->isStaticOrKinematicObject() : true;
999         
1000
1001         if (rb0static && rb1static)
1002                 return 0;
1003
1004         btVector3 pivotInA(pivotX,pivotY,pivotZ);
1005         btVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
1006         btVector3 axisInA(axisX,axisY,axisZ);
1007         btVector3 axisInB = rb1 ? 
1008                 (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) : 
1009         rb0->getCenterOfMassTransform().getBasis() * axisInA;
1010
1011         bool angularOnly = false;
1012
1013         switch (type)
1014         {
1015         case PHY_POINT2POINT_CONSTRAINT:
1016                 {
1017
1018                         btPoint2PointConstraint* p2p = 0;
1019
1020                         if (rb1)
1021                         {
1022                                 p2p = new btPoint2PointConstraint(*rb0,
1023                                         *rb1,pivotInA,pivotInB);
1024                         } else
1025                         {
1026                                 p2p = new btPoint2PointConstraint(*rb0,
1027                                         pivotInA);
1028                         }
1029
1030                         m_dynamicsWorld->addConstraint(p2p);
1031 //                      m_constraints.push_back(p2p);
1032
1033                         p2p->setUserConstraintId(gConstraintUid++);
1034                         p2p->setUserConstraintType(type);
1035                         //64 bit systems can't cast pointer to int. could use size_t instead.
1036                         return p2p->getUserConstraintId();
1037
1038                         break;
1039                 }
1040
1041         case PHY_GENERIC_6DOF_CONSTRAINT:
1042                 {
1043                         btGeneric6DofConstraint* genericConstraint = 0;
1044
1045                         if (rb1)
1046                         {
1047                                 btTransform frameInA;
1048                                 btTransform frameInB;
1049                                 
1050                                 btVector3 axis1, axis2;
1051                                 btPlaneSpace1( axisInA, axis1, axis2 );
1052
1053                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1054                                                                   axisInA.y(), axis1.y(), axis2.y(),
1055                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1056
1057         
1058                                 btPlaneSpace1( axisInB, axis1, axis2 );
1059                                 frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(),
1060                                                                   axisInB.y(), axis1.y(), axis2.y(),
1061                                                                                           axisInB.z(), axis1.z(), axis2.z() );
1062
1063                                 frameInA.setOrigin( pivotInA );
1064                                 frameInB.setOrigin( pivotInB );
1065
1066                                 genericConstraint = new btGeneric6DofConstraint(
1067                                         *rb0,*rb1,
1068                                         frameInA,frameInB);
1069
1070
1071                         } else
1072                         {
1073                                 // TODO: Implement single body case...
1074
1075                         }
1076                         
1077
1078                         //m_constraints.push_back(genericConstraint);
1079                         m_dynamicsWorld->addConstraint(genericConstraint);
1080                         genericConstraint->setUserConstraintId(gConstraintUid++);
1081                         genericConstraint->setUserConstraintType(type);
1082                         //64 bit systems can't cast pointer to int. could use size_t instead.
1083                         return genericConstraint->getUserConstraintId();
1084
1085                         break;
1086                 }
1087         case PHY_ANGULAR_CONSTRAINT:
1088                 angularOnly = true;
1089
1090
1091         case PHY_LINEHINGE_CONSTRAINT:
1092                 {
1093                         btHingeConstraint* hinge = 0;
1094
1095                         if (rb1)
1096                         {
1097                                 hinge = new btHingeConstraint(
1098                                         *rb0,
1099                                         *rb1,pivotInA,pivotInB,axisInA,axisInB);
1100
1101
1102                         } else
1103                         {
1104                                 hinge = new btHingeConstraint(*rb0,
1105                                         pivotInA,axisInA);
1106
1107                         }
1108                         hinge->setAngularOnly(angularOnly);
1109
1110                         //m_constraints.push_back(hinge);
1111                         m_dynamicsWorld->addConstraint(hinge);
1112                         hinge->setUserConstraintId(gConstraintUid++);
1113                         hinge->setUserConstraintType(type);
1114                         //64 bit systems can't cast pointer to int. could use size_t instead.
1115                         return hinge->getUserConstraintId();
1116                         break;
1117                 }
1118 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1119
1120         case PHY_VEHICLE_CONSTRAINT:
1121                 {
1122                         btRaycastVehicle::btVehicleTuning* tuning = new btRaycastVehicle::btVehicleTuning();
1123                         btRigidBody* chassis = rb0;
1124                         btDefaultVehicleRaycaster* raycaster = new btDefaultVehicleRaycaster(m_dynamicsWorld);
1125                         btRaycastVehicle* vehicle = new btRaycastVehicle(*tuning,chassis,raycaster);
1126                         WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
1127                         m_wrapperVehicles.push_back(wrapperVehicle);
1128                         m_dynamicsWorld->addVehicle(vehicle);
1129                         vehicle->setUserConstraintId(gConstraintUid++);
1130                         vehicle->setUserConstraintType(type);
1131                         return vehicle->getUserConstraintId();
1132
1133                         break;
1134                 };
1135 #endif //NEW_BULLET_VEHICLE_SUPPORT
1136
1137         default:
1138                 {
1139                 }
1140         };
1141
1142         //btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB
1143
1144         return 0;
1145
1146 }
1147
1148
1149
1150 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float coneradius,float coneheight)
1151 {
1152         CcdConstructionInfo     cinfo;
1153         cinfo.m_collisionShape = new btConeShape(coneradius,coneheight);
1154         cinfo.m_MotionState = 0;
1155         cinfo.m_physicsEnv = this;
1156         DefaultMotionState* motionState = new DefaultMotionState();
1157         cinfo.m_MotionState = motionState;
1158         motionState->m_worldTransform.setIdentity();
1159 //      motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
1160
1161         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
1162
1163
1164         return sphereController;
1165 }
1166         
1167 float           CcdPhysicsEnvironment::getAppliedImpulse(int    constraintid)
1168 {
1169         int i;
1170         int numConstraints = m_dynamicsWorld->getNumConstraints();
1171         for (i=0;i<numConstraints;i++)
1172         {
1173                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
1174                 if (constraint->getUserConstraintId() == constraintid)
1175                 {
1176                         return constraint->getAppliedImpulse();
1177                 }
1178         }
1179
1180         return 0.f;
1181 }