edd4c2e2bb4a5912e482cc4a4f4ee7b4461ad985
[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->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 {
1007
1008
1009         CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
1010         CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
1011
1012         btRigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
1013         btRigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
1014
1015         bool rb0static = rb0 ? rb0->isStaticOrKinematicObject() : true;
1016         bool rb1static = rb1 ? rb1->isStaticOrKinematicObject() : true;
1017         
1018
1019         if (rb0static && rb1static)
1020                 return 0;
1021
1022         btVector3 pivotInA(pivotX,pivotY,pivotZ);
1023         btVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : 
1024                 rb0->getCenterOfMassTransform() * pivotInA;
1025         btVector3 axisInA(axisX,axisY,axisZ);
1026         btVector3 axisInB = rb1 ? 
1027                 (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) : 
1028         rb0->getCenterOfMassTransform().getBasis() * axisInA;
1029
1030         bool angularOnly = false;
1031
1032         switch (type)
1033         {
1034         case PHY_POINT2POINT_CONSTRAINT:
1035                 {
1036
1037                         btPoint2PointConstraint* p2p = 0;
1038
1039                         if (rb1)
1040                         {
1041                                 p2p = new btPoint2PointConstraint(*rb0,
1042                                         *rb1,pivotInA,pivotInB);
1043                         } else
1044                         {
1045                                 p2p = new btPoint2PointConstraint(*rb0,
1046                                         pivotInA);
1047                         }
1048
1049                         m_dynamicsWorld->addConstraint(p2p);
1050 //                      m_constraints.push_back(p2p);
1051
1052                         p2p->setUserConstraintId(gConstraintUid++);
1053                         p2p->setUserConstraintType(type);
1054                         //64 bit systems can't cast pointer to int. could use size_t instead.
1055                         return p2p->getUserConstraintId();
1056
1057                         break;
1058                 }
1059
1060         case PHY_GENERIC_6DOF_CONSTRAINT:
1061                 {
1062                         btGeneric6DofConstraint* genericConstraint = 0;
1063
1064                         if (rb1)
1065                         {
1066                                 btTransform frameInA;
1067                                 btTransform frameInB;
1068                                 
1069                                 btVector3 axis1, axis2;
1070                                 btPlaneSpace1( axisInA, axis1, axis2 );
1071
1072                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1073                                                                   axisInA.y(), axis1.y(), axis2.y(),
1074                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1075
1076         
1077                                 btPlaneSpace1( axisInB, axis1, axis2 );
1078                                 frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(),
1079                                                                   axisInB.y(), axis1.y(), axis2.y(),
1080                                                                                           axisInB.z(), axis1.z(), axis2.z() );
1081
1082                                 frameInA.setOrigin( pivotInA );
1083                                 frameInB.setOrigin( pivotInB );
1084
1085                                 genericConstraint = new btGeneric6DofConstraint(
1086                                         *rb0,*rb1,
1087                                         frameInA,frameInB);
1088
1089
1090                         } else
1091                         {
1092                                 static btRigidBody s_fixedObject2( 0,0,0);
1093                                         btTransform frameInA;
1094                                 btTransform frameInB;
1095                                 
1096                                 btVector3 axis1, axis2;
1097                                 btPlaneSpace1( axisInA, axis1, axis2 );
1098
1099                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1100                                                                   axisInA.y(), axis1.y(), axis2.y(),
1101                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1102
1103         
1104                                 btPlaneSpace1( axisInB, axis1, axis2 );
1105                                 frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(),
1106                                                                   axisInB.y(), axis1.y(), axis2.y(),
1107                                                                                           axisInB.z(), axis1.z(), axis2.z() );
1108
1109                                 frameInA.setOrigin( pivotInA );
1110                                 frameInB.setOrigin( pivotInB );
1111
1112
1113                                 genericConstraint = new btGeneric6DofConstraint(
1114                                         *rb0,s_fixedObject2,
1115                                         frameInA,frameInB);
1116                         }
1117                         
1118                         if (genericConstraint)
1119                         {
1120                                 //m_constraints.push_back(genericConstraint);
1121                                 m_dynamicsWorld->addConstraint(genericConstraint);
1122                                 genericConstraint->setUserConstraintId(gConstraintUid++);
1123                                 genericConstraint->setUserConstraintType(type);
1124                                 //64 bit systems can't cast pointer to int. could use size_t instead.
1125                                 return genericConstraint->getUserConstraintId();
1126                         } 
1127
1128                         break;
1129                 }
1130         case PHY_ANGULAR_CONSTRAINT:
1131                 angularOnly = true;
1132
1133
1134         case PHY_LINEHINGE_CONSTRAINT:
1135                 {
1136                         btHingeConstraint* hinge = 0;
1137
1138                         if (rb1)
1139                         {
1140                                 hinge = new btHingeConstraint(
1141                                         *rb0,
1142                                         *rb1,pivotInA,pivotInB,axisInA,axisInB);
1143
1144
1145                         } else
1146                         {
1147                                 hinge = new btHingeConstraint(*rb0,
1148                                         pivotInA,axisInA);
1149
1150                         }
1151                         hinge->setAngularOnly(angularOnly);
1152
1153                         //m_constraints.push_back(hinge);
1154                         m_dynamicsWorld->addConstraint(hinge);
1155                         hinge->setUserConstraintId(gConstraintUid++);
1156                         hinge->setUserConstraintType(type);
1157                         //64 bit systems can't cast pointer to int. could use size_t instead.
1158                         return hinge->getUserConstraintId();
1159                         break;
1160                 }
1161 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1162
1163         case PHY_VEHICLE_CONSTRAINT:
1164                 {
1165                         btRaycastVehicle::btVehicleTuning* tuning = new btRaycastVehicle::btVehicleTuning();
1166                         btRigidBody* chassis = rb0;
1167                         btDefaultVehicleRaycaster* raycaster = new btDefaultVehicleRaycaster(m_dynamicsWorld);
1168                         btRaycastVehicle* vehicle = new btRaycastVehicle(*tuning,chassis,raycaster);
1169                         WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
1170                         m_wrapperVehicles.push_back(wrapperVehicle);
1171                         m_dynamicsWorld->addVehicle(vehicle);
1172                         vehicle->setUserConstraintId(gConstraintUid++);
1173                         vehicle->setUserConstraintType(type);
1174                         return vehicle->getUserConstraintId();
1175
1176                         break;
1177                 };
1178 #endif //NEW_BULLET_VEHICLE_SUPPORT
1179
1180         default:
1181                 {
1182                 }
1183         };
1184
1185         //btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB
1186
1187         return 0;
1188
1189 }
1190
1191
1192
1193 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float coneradius,float coneheight)
1194 {
1195         CcdConstructionInfo     cinfo;
1196         cinfo.m_collisionShape = new btConeShape(coneradius,coneheight);
1197         cinfo.m_MotionState = 0;
1198         cinfo.m_physicsEnv = this;
1199         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE;
1200         DefaultMotionState* motionState = new DefaultMotionState();
1201         cinfo.m_MotionState = motionState;
1202         motionState->m_worldTransform.setIdentity();
1203 //      motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
1204
1205         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
1206
1207
1208         return sphereController;
1209 }
1210         
1211 float           CcdPhysicsEnvironment::getAppliedImpulse(int    constraintid)
1212 {
1213         int i;
1214         int numConstraints = m_dynamicsWorld->getNumConstraints();
1215         for (i=0;i<numConstraints;i++)
1216         {
1217                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
1218                 if (constraint->getUserConstraintId() == constraintid)
1219                 {
1220                         return constraint->getAppliedImpulse();
1221                 }
1222         }
1223
1224         return 0.f;
1225 }