fixing some issues: force needs to wake up objects, property sensor issue, island...
[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                         constraint->getRigidBodyA().activate();
657                         constraint->getRigidBodyB().activate();
658                         m_dynamicsWorld->removeConstraint(constraint);
659                         break;
660                 }
661         }
662 }
663
664
665 struct  FilterClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
666 {
667         PHY_IPhysicsController* m_ignoreClient;
668
669         FilterClosestRayResultCallback (PHY_IPhysicsController* ignoreClient,const btVector3& rayFrom,const btVector3& rayTo)
670                 : btCollisionWorld::ClosestRayResultCallback(rayFrom,rayTo),
671                 m_ignoreClient(ignoreClient)
672         {
673
674         }
675
676         virtual ~FilterClosestRayResultCallback()
677         {
678         }
679
680         virtual float   AddSingleResult( btCollisionWorld::LocalRayResult& rayResult)
681         {
682                 CcdPhysicsController* curHit = static_cast<CcdPhysicsController*>(rayResult.m_collisionObject->getUserPointer());
683                 //ignore client...
684                 if (curHit != m_ignoreClient)
685                 {               
686                         //if valid
687                         return ClosestRayResultCallback::AddSingleResult(rayResult);
688                 }
689                 return m_closestHitFraction;
690         }
691
692 };
693
694 PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ, 
695                                                                                                            float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
696 {
697
698
699         float minFraction = 1.f;
700
701         btVector3 rayFrom(fromX,fromY,fromZ);
702         btVector3 rayTo(toX,toY,toZ);
703
704         btVector3       hitPointWorld,normalWorld;
705
706         //Either Ray Cast with or without filtering
707
708         //btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo);
709         FilterClosestRayResultCallback   rayCallback(ignoreClient,rayFrom,rayTo);
710
711
712         PHY_IPhysicsController* nearestHit = 0;
713
714         m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback);
715         if (rayCallback.HasHit())
716         {
717                 nearestHit = static_cast<CcdPhysicsController*>(rayCallback.m_collisionObject->getUserPointer());
718                 hitX =  rayCallback.m_hitPointWorld.getX();
719                 hitY =  rayCallback.m_hitPointWorld.getY();
720                 hitZ =  rayCallback.m_hitPointWorld.getZ();
721
722                 normalX = rayCallback.m_hitNormalWorld.getX();
723                 normalY = rayCallback.m_hitNormalWorld.getY();
724                 normalZ = rayCallback.m_hitNormalWorld.getZ();
725
726         }       
727
728
729         return nearestHit;
730 }
731
732
733
734 int     CcdPhysicsEnvironment::getNumContactPoints()
735 {
736         return 0;
737 }
738
739 void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
740 {
741
742 }
743
744
745
746
747 btBroadphaseInterface*  CcdPhysicsEnvironment::getBroadphase()
748
749         return m_dynamicsWorld->getBroadphase(); 
750 }
751
752
753
754
755 CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
756 {
757
758 #ifdef NEW_BULLET_VEHICLE_SUPPORT
759         m_wrapperVehicles.clear();
760 #endif //NEW_BULLET_VEHICLE_SUPPORT
761
762         //m_broadphase->DestroyScene();
763         //delete broadphase ? release reference on broadphase ?
764
765         //first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher
766         //delete m_dispatcher;
767         delete m_dynamicsWorld;
768         
769
770
771 }
772
773
774 int     CcdPhysicsEnvironment::GetNumControllers()
775 {
776         return m_controllers.size();
777 }
778
779
780 CcdPhysicsController* CcdPhysicsEnvironment::GetPhysicsController( int index)
781 {
782         return m_controllers[index];
783 }
784
785
786
787
788 void    CcdPhysicsEnvironment::setConstraintParam(int constraintId,int param,float value0,float value1)
789 {
790         btTypedConstraint* typedConstraint = getConstraintById(constraintId);
791         switch (typedConstraint->getUserConstraintType())
792         {
793         case PHY_GENERIC_6DOF_CONSTRAINT:
794                 {
795                         //param = 1..12, min0,max0,min1,max1...min6,max6
796                         btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
797                         genCons->SetLimit(param,value0,value1);
798                         break;
799                 };
800         default:
801                 {
802                 };
803         };
804 }
805
806 btTypedConstraint*      CcdPhysicsEnvironment::getConstraintById(int constraintId)
807 {
808
809         int numConstraints = m_dynamicsWorld->getNumConstraints();
810         int i;
811         for (i=0;i<numConstraints;i++)
812         {
813                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
814                 if (constraint->getUserConstraintId()==constraintId)
815                 {
816                         return constraint;
817                 }
818         }
819         return 0;
820 }
821
822
823 void CcdPhysicsEnvironment::addSensor(PHY_IPhysicsController* ctrl)
824 {
825
826         CcdPhysicsController* ctrl1 = (CcdPhysicsController* )ctrl;
827         std::vector<CcdPhysicsController*>::iterator i =
828                 std::find(m_controllers.begin(), m_controllers.end(), ctrl);
829         if ((i == m_controllers.end()))
830         {
831                 addCcdPhysicsController(ctrl1);
832         }
833         //force collision detection with everything, including static objects (might hurt performance!)
834         ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterMask = btBroadphaseProxy::AllFilter;
835         ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterGroup = btBroadphaseProxy::AllFilter;
836         //todo: make this 'sensor'!
837
838         requestCollisionCallback(ctrl);
839         //printf("addSensor\n");
840 }
841
842 void CcdPhysicsEnvironment::removeCollisionCallback(PHY_IPhysicsController* ctrl)
843 {
844         std::vector<CcdPhysicsController*>::iterator i =
845                 std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl);
846         if (!(i == m_triggerControllers.end()))
847         {
848                 std::swap(*i, m_triggerControllers.back());
849                 m_triggerControllers.pop_back();
850         }
851 }
852
853
854 void CcdPhysicsEnvironment::removeSensor(PHY_IPhysicsController* ctrl)
855 {
856         removeCollisionCallback(ctrl);
857         //printf("removeSensor\n");
858 }
859 void CcdPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user)
860 {
861         /*      printf("addTouchCallback\n(response class = %i)\n",response_class);
862
863         //map PHY_ convention into SM_ convention
864         switch (response_class)
865         {
866         case    PHY_FH_RESPONSE:
867         printf("PHY_FH_RESPONSE\n");
868         break;
869         case PHY_SENSOR_RESPONSE:
870         printf("PHY_SENSOR_RESPONSE\n");
871         break;
872         case PHY_CAMERA_RESPONSE:
873         printf("PHY_CAMERA_RESPONSE\n");
874         break;
875         case PHY_OBJECT_RESPONSE:
876         printf("PHY_OBJECT_RESPONSE\n");
877         break;
878         case PHY_STATIC_RESPONSE:
879         printf("PHY_STATIC_RESPONSE\n");
880         break;
881         default:
882         assert(0);
883         return;
884         }
885         */
886
887         m_triggerCallbacks[response_class] = callback;
888         m_triggerCallbacksUserPtrs[response_class] = user;
889
890 }
891 void CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctrl)
892 {
893         CcdPhysicsController* ccdCtrl = static_cast<CcdPhysicsController*>(ctrl);
894
895         //printf("requestCollisionCallback\n");
896         m_triggerControllers.push_back(ccdCtrl);
897 }
898
899
900 void    CcdPhysicsEnvironment::CallbackTriggers()
901 {
902         
903         CcdPhysicsController* ctrl0=0,*ctrl1=0;
904
905         if (m_triggerCallbacks[PHY_OBJECT_RESPONSE] || (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)))
906         {
907                 //walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback
908                 int numManifolds = m_dynamicsWorld->getDispatcher()->getNumManifolds();
909                 for (int i=0;i<numManifolds;i++)
910                 {
911                         btPersistentManifold* manifold = m_dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i);
912                         int numContacts = manifold->getNumContacts();
913                         if (numContacts)
914                         {
915                                 if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints))
916                                 {
917                                         for (int j=0;j<numContacts;j++)
918                                         {
919                                                 btVector3 color(1,0,0);
920                                                 const btManifoldPoint& cp = manifold->getContactPoint(j);
921                                                 if (m_debugDrawer)
922                                                         m_debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
923                                         }
924                                 }
925                                 btRigidBody* obj0 = static_cast<btRigidBody* >(manifold->getBody0());
926                                 btRigidBody* obj1 = static_cast<btRigidBody* >(manifold->getBody1());
927
928                                 //m_internalOwner is set in 'addPhysicsController'
929                                 CcdPhysicsController* ctrl0 = static_cast<CcdPhysicsController*>(obj0->getUserPointer());
930                                 CcdPhysicsController* ctrl1 = static_cast<CcdPhysicsController*>(obj1->getUserPointer());
931
932                                 std::vector<CcdPhysicsController*>::iterator i =
933                                         std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl0);
934                                 if (i == m_triggerControllers.end())
935                                 {
936                                         i = std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl1);
937                                 }
938
939                                 if (!(i == m_triggerControllers.end()))
940                                 {
941                                         m_triggerCallbacks[PHY_OBJECT_RESPONSE](m_triggerCallbacksUserPtrs[PHY_OBJECT_RESPONSE],
942                                                 ctrl0,ctrl1,0);
943                                 }
944                         }
945                 }
946
947
948
949         }
950
951
952 }
953
954
955
956
957
958
959 #ifdef NEW_BULLET_VEHICLE_SUPPORT
960
961 //complex constraint for vehicles
962 PHY_IVehicle*   CcdPhysicsEnvironment::getVehicleConstraint(int constraintId)
963 {
964         int i;
965
966         int numVehicles = m_wrapperVehicles.size();
967         for (i=0;i<numVehicles;i++)
968         {
969                 WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
970                 if (wrapperVehicle->GetVehicle()->getUserConstraintId() == constraintId)
971                         return wrapperVehicle;
972         }
973
974         return 0;
975 }
976
977 #endif //NEW_BULLET_VEHICLE_SUPPORT
978
979
980 int currentController = 0;
981 int numController = 0;
982
983
984
985
986 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateSphereController(float radius,const PHY__Vector3& position)
987 {
988         
989         CcdConstructionInfo     cinfo;
990         cinfo.m_collisionShape = new btSphereShape(radius);
991         cinfo.m_MotionState = 0;
992         cinfo.m_physicsEnv = this;
993         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE | btCollisionObject::CF_KINEMATIC_OBJECT;
994         DefaultMotionState* motionState = new DefaultMotionState();
995         cinfo.m_MotionState = motionState;
996         motionState->m_worldTransform.setIdentity();
997         motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
998
999         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
1000         
1001
1002         return sphereController;
1003 }
1004
1005 int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
1006                                                                                                         float pivotX,float pivotY,float pivotZ,
1007                                                                                                         float axisX,float axisY,float axisZ,
1008                                                                                                         float axis1X,float axis1Y,float axis1Z,
1009                                                                                                         float axis2X,float axis2Y,float axis2Z
1010                                                                                                         )
1011 {
1012
1013
1014         CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
1015         CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
1016
1017         btRigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
1018         btRigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
1019
1020         bool rb0static = rb0 ? rb0->isStaticOrKinematicObject() : true;
1021         bool rb1static = rb1 ? rb1->isStaticOrKinematicObject() : true;
1022         
1023
1024         if (rb0static && rb1static)
1025                 return 0;
1026
1027         btVector3 pivotInA(pivotX,pivotY,pivotZ);
1028         btVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : 
1029                 rb0->getCenterOfMassTransform() * pivotInA;
1030         btVector3 axisInA(axisX,axisY,axisZ);
1031         btVector3 axisInB = rb1 ? 
1032                 (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) : 
1033         rb0->getCenterOfMassTransform().getBasis() * axisInA;
1034
1035         bool angularOnly = false;
1036
1037         switch (type)
1038         {
1039         case PHY_POINT2POINT_CONSTRAINT:
1040                 {
1041
1042                         btPoint2PointConstraint* p2p = 0;
1043
1044                         if (rb1)
1045                         {
1046                                 p2p = new btPoint2PointConstraint(*rb0,
1047                                         *rb1,pivotInA,pivotInB);
1048                         } else
1049                         {
1050                                 p2p = new btPoint2PointConstraint(*rb0,
1051                                         pivotInA);
1052                         }
1053
1054                         m_dynamicsWorld->addConstraint(p2p);
1055 //                      m_constraints.push_back(p2p);
1056
1057                         p2p->setUserConstraintId(gConstraintUid++);
1058                         p2p->setUserConstraintType(type);
1059                         //64 bit systems can't cast pointer to int. could use size_t instead.
1060                         return p2p->getUserConstraintId();
1061
1062                         break;
1063                 }
1064
1065         case PHY_GENERIC_6DOF_CONSTRAINT:
1066                 {
1067                         btGeneric6DofConstraint* genericConstraint = 0;
1068
1069                         if (rb1)
1070                         {
1071                                 btTransform frameInA;
1072                                 btTransform frameInB;
1073                                 
1074                                 btVector3 axis1(axis1X,axis1Y,axis1Z), axis2(axis2X,axis2Y,axis2Z);
1075                                 if (axis1.length() == 0.0)
1076                                 {
1077                                         btPlaneSpace1( axisInA, axis1, axis2 );
1078                                 }
1079                                 
1080                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1081                                                                   axisInA.y(), axis1.y(), axis2.y(),
1082                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1083
1084         
1085                                 btPlaneSpace1( axisInB, axis1, axis2 );
1086                                 frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(),
1087                                                                   axisInB.y(), axis1.y(), axis2.y(),
1088                                                                                           axisInB.z(), axis1.z(), axis2.z() );
1089
1090                                 frameInA.setOrigin( pivotInA );
1091                                 frameInB.setOrigin( pivotInB );
1092
1093                                 genericConstraint = new btGeneric6DofConstraint(
1094                                         *rb0,*rb1,
1095                                         frameInA,frameInB);
1096
1097
1098                         } else
1099                         {
1100                                 static btRigidBody s_fixedObject2( 0,0,0);
1101                                         btTransform frameInA;
1102                                 btTransform frameInB;
1103                                 
1104                                 btVector3 axis1, axis2;
1105                                 btPlaneSpace1( axisInA, axis1, axis2 );
1106
1107                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1108                                                                   axisInA.y(), axis1.y(), axis2.y(),
1109                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1110
1111         
1112                                 btPlaneSpace1( axisInB, axis1, axis2 );
1113                                 frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(),
1114                                                                   axisInB.y(), axis1.y(), axis2.y(),
1115                                                                                           axisInB.z(), axis1.z(), axis2.z() );
1116
1117                                 frameInA.setOrigin( pivotInA );
1118                                 frameInB.setOrigin( pivotInB );
1119
1120
1121                                 genericConstraint = new btGeneric6DofConstraint(
1122                                         *rb0,s_fixedObject2,
1123                                         frameInA,frameInB);
1124                         }
1125                         
1126                         if (genericConstraint)
1127                         {
1128                                 //m_constraints.push_back(genericConstraint);
1129                                 m_dynamicsWorld->addConstraint(genericConstraint);
1130                                 genericConstraint->setUserConstraintId(gConstraintUid++);
1131                                 genericConstraint->setUserConstraintType(type);
1132                                 //64 bit systems can't cast pointer to int. could use size_t instead.
1133                                 return genericConstraint->getUserConstraintId();
1134                         } 
1135
1136                         break;
1137                 }
1138         case PHY_ANGULAR_CONSTRAINT:
1139                 angularOnly = true;
1140
1141
1142         case PHY_LINEHINGE_CONSTRAINT:
1143                 {
1144                         btHingeConstraint* hinge = 0;
1145
1146                         if (rb1)
1147                         {
1148                                 hinge = new btHingeConstraint(
1149                                         *rb0,
1150                                         *rb1,pivotInA,pivotInB,axisInA,axisInB);
1151
1152
1153                         } else
1154                         {
1155                                 hinge = new btHingeConstraint(*rb0,
1156                                         pivotInA,axisInA);
1157
1158                         }
1159                         hinge->setAngularOnly(angularOnly);
1160
1161                         //m_constraints.push_back(hinge);
1162                         m_dynamicsWorld->addConstraint(hinge);
1163                         hinge->setUserConstraintId(gConstraintUid++);
1164                         hinge->setUserConstraintType(type);
1165                         //64 bit systems can't cast pointer to int. could use size_t instead.
1166                         return hinge->getUserConstraintId();
1167                         break;
1168                 }
1169 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1170
1171         case PHY_VEHICLE_CONSTRAINT:
1172                 {
1173                         btRaycastVehicle::btVehicleTuning* tuning = new btRaycastVehicle::btVehicleTuning();
1174                         btRigidBody* chassis = rb0;
1175                         btDefaultVehicleRaycaster* raycaster = new btDefaultVehicleRaycaster(m_dynamicsWorld);
1176                         btRaycastVehicle* vehicle = new btRaycastVehicle(*tuning,chassis,raycaster);
1177                         WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
1178                         m_wrapperVehicles.push_back(wrapperVehicle);
1179                         m_dynamicsWorld->addVehicle(vehicle);
1180                         vehicle->setUserConstraintId(gConstraintUid++);
1181                         vehicle->setUserConstraintType(type);
1182                         return vehicle->getUserConstraintId();
1183
1184                         break;
1185                 };
1186 #endif //NEW_BULLET_VEHICLE_SUPPORT
1187
1188         default:
1189                 {
1190                 }
1191         };
1192
1193         //btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB
1194
1195         return 0;
1196
1197 }
1198
1199
1200
1201 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float coneradius,float coneheight)
1202 {
1203         CcdConstructionInfo     cinfo;
1204         cinfo.m_collisionShape = new btConeShape(coneradius,coneheight);
1205         cinfo.m_MotionState = 0;
1206         cinfo.m_physicsEnv = this;
1207         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE;
1208         DefaultMotionState* motionState = new DefaultMotionState();
1209         cinfo.m_MotionState = motionState;
1210         motionState->m_worldTransform.setIdentity();
1211 //      motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
1212
1213         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
1214
1215
1216         return sphereController;
1217 }
1218         
1219 float           CcdPhysicsEnvironment::getAppliedImpulse(int    constraintid)
1220 {
1221         int i;
1222         int numConstraints = m_dynamicsWorld->getNumConstraints();
1223         for (i=0;i<numConstraints;i++)
1224         {
1225                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
1226                 if (constraint->getUserConstraintId() == constraintid)
1227                 {
1228                         return constraint->getAppliedImpulse();
1229                 }
1230         }
1231
1232         return 0.f;
1233 }