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