soc-2008-mxcurioni: merged changes to revision 15705
[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 class CcdOverlapFilterCallBack : public btOverlapFilterCallback
255 {
256 private:
257         class CcdPhysicsEnvironment* m_physEnv;
258 public:
259         CcdOverlapFilterCallBack(CcdPhysicsEnvironment* env) : 
260                 m_physEnv(env)
261         {
262         }
263         virtual ~CcdOverlapFilterCallBack()
264         {
265         }
266         // return true when pairs need collision
267         virtual bool    needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const;
268 };
269
270
271 void CcdPhysicsEnvironment::setDebugDrawer(btIDebugDraw* debugDrawer)
272 {
273         if (debugDrawer && m_dynamicsWorld)
274                 m_dynamicsWorld->setDebugDrawer(debugDrawer);
275         m_debugDrawer = debugDrawer;
276 }
277
278 static void DrawAabb(btIDebugDraw* debugDrawer,const btVector3& from,const btVector3& to,const btVector3& color)
279 {
280         btVector3 halfExtents = (to-from)* 0.5f;
281         btVector3 center = (to+from) *0.5f;
282         int i,j;
283
284         btVector3 edgecoord(1.f,1.f,1.f),pa,pb;
285         for (i=0;i<4;i++)
286         {
287                 for (j=0;j<3;j++)
288                 {
289                         pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],                
290                                 edgecoord[2]*halfExtents[2]);
291                         pa+=center;
292
293                         int othercoord = j%3;
294                         edgecoord[othercoord]*=-1.f;
295                         pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],        
296                                 edgecoord[2]*halfExtents[2]);
297                         pb+=center;
298
299                         debugDrawer->drawLine(pa,pb,color);
300                 }
301                 edgecoord = btVector3(-1.f,-1.f,-1.f);
302                 if (i<3)
303                         edgecoord[i]*=-1.f;
304         }
305
306
307 }
308
309
310
311
312
313
314 CcdPhysicsEnvironment::CcdPhysicsEnvironment(btDispatcher* dispatcher,btOverlappingPairCache* pairCache)
315 :m_scalingPropagated(false),
316 m_numIterations(10),
317 m_numTimeSubSteps(1),
318 m_ccdMode(0),
319 m_solverType(-1),
320 m_profileTimings(0),
321 m_enableSatCollisionDetection(false),
322 m_solver(NULL),
323 m_ownPairCache(NULL),
324 m_ownDispatcher(NULL),
325 m_filterCallback(NULL)
326 {
327
328         for (int i=0;i<PHY_NUM_RESPONSE;i++)
329         {
330                 m_triggerCallbacks[i] = 0;
331         }
332         if (!dispatcher)
333         {
334                 dispatcher = new btCollisionDispatcher();
335                 m_ownDispatcher = dispatcher;
336         }
337
338         if(!pairCache)
339         {
340
341                 //todo: calculate/let user specify this world sizes
342                 btVector3 worldMin(-10000,-10000,-10000);
343                 btVector3 worldMax(10000,10000,10000);
344
345                 pairCache = new btAxisSweep3(worldMin,worldMax);
346                 // remember that this was allocated by us so that we can release it
347                 m_ownPairCache = pairCache;
348                 //broadphase = new btSimpleBroadphase();
349         }
350
351         m_filterCallback = new CcdOverlapFilterCallBack(this);
352         pairCache->setOverlapFilterCallback(m_filterCallback);
353
354         setSolverType(1);//issues with quickstep and memory allocations
355         m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,pairCache,m_solver);
356         m_debugDrawer = 0;
357         m_gravity = btVector3(0.f,-10.f,0.f);
358         m_dynamicsWorld->setGravity(m_gravity);
359
360
361 }
362
363 void    CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
364 {
365         btRigidBody* body = ctrl->GetRigidBody();
366
367         //this m_userPointer is just used for triggers, see CallbackTriggers
368         body->setUserPointer(ctrl);
369
370         body->setGravity( m_gravity );
371         m_controllers.push_back(ctrl);
372
373         //use explicit group/filter for finer control over collision in bullet => near/radar sensor
374         m_dynamicsWorld->addRigidBody(body, ctrl->GetCollisionFilterGroup(), ctrl->GetCollisionFilterMask());
375         if (body->isStaticOrKinematicObject())
376         {
377                 body->setActivationState(ISLAND_SLEEPING);
378         }
379
380
381         //CollisionObject(body,ctrl->GetCollisionFilterGroup(),ctrl->GetCollisionFilterMask());
382
383         assert(body->getBroadphaseHandle());
384
385         btBroadphaseInterface* scene =  getBroadphase();
386
387
388         btCollisionShape* shapeinterface = ctrl->GetCollisionShape();
389
390         assert(shapeinterface);
391
392         const btTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform();
393         
394
395         btPoint3 minAabb,maxAabb;
396
397         shapeinterface->getAabb(t,minAabb,maxAabb);
398
399         float timeStep = 0.02f;
400
401
402         //extent it with the motion
403
404         btVector3 linMotion = body->getLinearVelocity()*timeStep;
405
406         float maxAabbx = maxAabb.getX();
407         float maxAabby = maxAabb.getY();
408         float maxAabbz = maxAabb.getZ();
409         float minAabbx = minAabb.getX();
410         float minAabby = minAabb.getY();
411         float minAabbz = minAabb.getZ();
412
413         if (linMotion.x() > 0.f)
414                 maxAabbx += linMotion.x(); 
415         else
416                 minAabbx += linMotion.x();
417         if (linMotion.y() > 0.f)
418                 maxAabby += linMotion.y(); 
419         else
420                 minAabby += linMotion.y();
421         if (linMotion.z() > 0.f)
422                 maxAabbz += linMotion.z(); 
423         else
424                 minAabbz += linMotion.z();
425
426
427         minAabb = btVector3(minAabbx,minAabby,minAabbz);
428         maxAabb = btVector3(maxAabbx,maxAabby,maxAabbz);
429
430
431
432
433 }
434
435 void    CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctrl)
436 {
437
438         //also remove constraint
439
440         
441
442         m_dynamicsWorld->removeRigidBody(ctrl->GetRigidBody());
443
444
445         {
446                 std::vector<CcdPhysicsController*>::iterator i =
447                         std::find(m_controllers.begin(), m_controllers.end(), ctrl);
448                 if (!(i == m_controllers.end()))
449                 {
450                         std::swap(*i, m_controllers.back());
451                         m_controllers.pop_back();
452                 }
453         }
454
455         //remove it from the triggers
456         {
457                 std::vector<CcdPhysicsController*>::iterator i =
458                         std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl);
459                 if (!(i == m_triggerControllers.end()))
460                 {
461                         std::swap(*i, m_triggerControllers.back());
462                         m_triggerControllers.pop_back();
463                 }
464         }
465
466
467 }
468
469 void    CcdPhysicsEnvironment::updateCcdPhysicsController(CcdPhysicsController* ctrl, btScalar newMass, int newCollisionFlags, short int newCollisionGroup, short int newCollisionMask)
470 {
471         // this function is used when the collisionning group of a controller is changed
472         // remove and add the collistioning object
473         btRigidBody* body = ctrl->GetRigidBody();
474         btVector3 inertia;
475
476         m_dynamicsWorld->removeCollisionObject(body);
477         body->setCollisionFlags(newCollisionFlags);
478         body->getCollisionShape()->calculateLocalInertia(newMass, inertia);
479         body->setMassProps(newMass, inertia);
480         m_dynamicsWorld->addCollisionObject(body, newCollisionGroup, newCollisionMask);
481         // to avoid nasty interaction, we must update the property of the controller as well
482         ctrl->m_cci.m_mass = newMass;
483         ctrl->m_cci.m_collisionFilterGroup = newCollisionGroup;
484         ctrl->m_cci.m_collisionFilterMask = newCollisionMask;
485         ctrl->m_cci.m_collisionFlags = newCollisionFlags;
486 }
487
488 void CcdPhysicsEnvironment::enableCcdPhysicsController(CcdPhysicsController* ctrl)
489 {
490         std::vector<CcdPhysicsController*>::iterator i =
491                 std::find(m_controllers.begin(), m_controllers.end(), ctrl);
492         if (i == m_controllers.end())
493         {
494                 btRigidBody* body = ctrl->GetRigidBody();
495                 m_dynamicsWorld->addCollisionObject(body, 
496                         ctrl->GetCollisionFilterGroup(), ctrl->GetCollisionFilterMask());
497         }
498 }
499
500
501
502 void    CcdPhysicsEnvironment::beginFrame()
503 {
504
505 }
506
507
508 bool    CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
509 {
510
511         int i,numCtrl = GetNumControllers();
512         for (i=0;i<numCtrl;i++)
513         {
514                 CcdPhysicsController* ctrl = GetPhysicsController(i);
515                 ctrl->SynchronizeMotionStates(timeStep);
516         }
517
518         float subStep = timeStep / float(m_numTimeSubSteps);
519         for (i=0;i<m_numTimeSubSteps;i++)
520         {
521                 m_dynamicsWorld->stepSimulation(subStep,0);//perform always a full simulation step
522         }
523
524         numCtrl = GetNumControllers();
525         for (i=0;i<numCtrl;i++)
526         {
527                 CcdPhysicsController* ctrl = GetPhysicsController(i);
528                 ctrl->SynchronizeMotionStates(timeStep);
529         }
530
531         for (i=0;i<m_wrapperVehicles.size();i++)
532         {
533                 WrapperVehicle* veh = m_wrapperVehicles[i];
534                 veh->SyncWheels();
535         }
536
537         CallbackTriggers();
538
539         return true;
540 }
541
542
543 void            CcdPhysicsEnvironment::setDebugMode(int debugMode)
544 {
545         if (m_debugDrawer){
546                 m_debugDrawer->setDebugMode(debugMode);
547         }
548 }
549
550 void            CcdPhysicsEnvironment::setNumIterations(int numIter)
551 {
552         m_numIterations = numIter;
553 }
554 void            CcdPhysicsEnvironment::setDeactivationTime(float dTime)
555 {
556         gDeactivationTime = dTime;
557 }
558 void            CcdPhysicsEnvironment::setDeactivationLinearTreshold(float linTresh)
559 {
560         gLinearSleepingTreshold = linTresh;
561 }
562 void            CcdPhysicsEnvironment::setDeactivationAngularTreshold(float angTresh) 
563 {
564         gAngularSleepingTreshold = angTresh;
565 }
566
567 void            CcdPhysicsEnvironment::setContactBreakingTreshold(float contactBreakingTreshold)
568 {
569         gContactBreakingThreshold = contactBreakingTreshold;
570
571 }
572
573
574 void            CcdPhysicsEnvironment::setCcdMode(int ccdMode)
575 {
576         m_ccdMode = ccdMode;
577 }
578
579
580 void            CcdPhysicsEnvironment::setSolverSorConstant(float sor)
581 {
582         m_solverInfo.m_sor = sor;
583 }
584
585 void            CcdPhysicsEnvironment::setSolverTau(float tau)
586 {
587         m_solverInfo.m_tau = tau;
588 }
589 void            CcdPhysicsEnvironment::setSolverDamping(float damping)
590 {
591         m_solverInfo.m_damping = damping;
592 }
593
594
595 void            CcdPhysicsEnvironment::setLinearAirDamping(float damping)
596 {
597         gLinearAirDamping = damping;
598 }
599
600 void            CcdPhysicsEnvironment::setUseEpa(bool epa)
601 {
602         //gUseEpa = epa;
603 }
604
605 void            CcdPhysicsEnvironment::setSolverType(int solverType)
606 {
607
608         switch (solverType)
609         {
610         case 1:
611                 {
612                         if (m_solverType != solverType)
613                         {
614
615                                 m_solver = new btSequentialImpulseConstraintSolver();
616                                 ((btSequentialImpulseConstraintSolver*)m_solver)->setSolverMode(btSequentialImpulseConstraintSolver::SOLVER_USE_WARMSTARTING | btSequentialImpulseConstraintSolver::SOLVER_RANDMIZE_ORDER);
617                                 break;
618                         }
619                 }
620
621         case 0:
622         default:
623                 if (m_solverType != solverType)
624                 {
625 //                      m_solver = new OdeConstraintSolver();
626
627                         break;
628                 }
629
630         };
631
632         m_solverType = solverType ;
633 }
634
635
636
637
638
639
640 void            CcdPhysicsEnvironment::setGravity(float x,float y,float z)
641 {
642         m_gravity = btVector3(x,y,z);
643         m_dynamicsWorld->setGravity(m_gravity);
644
645 }
646
647
648
649
650 static int gConstraintUid = 1;
651
652 //Following the COLLADA physics specification for constraints
653 int                     CcdPhysicsEnvironment::createUniversalD6Constraint(
654                                                 class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
655                                                 btTransform& frameInA,
656                                                 btTransform& frameInB,
657                                                 const btVector3& linearMinLimits,
658                                                 const btVector3& linearMaxLimits,
659                                                 const btVector3& angularMinLimits,
660                                                 const btVector3& angularMaxLimits
661 )
662 {
663
664         //we could either add some logic to recognize ball-socket and hinge, or let that up to the user
665         //perhaps some warning or hint that hinge/ball-socket is more efficient?
666         
667         btGeneric6DofConstraint* genericConstraint = 0;
668         CcdPhysicsController* ctrl0 = (CcdPhysicsController*) ctrlRef;
669         CcdPhysicsController* ctrl1 = (CcdPhysicsController*) ctrlOther;
670         
671         btRigidBody* rb0 = ctrl0->GetRigidBody();
672         btRigidBody* rb1 = ctrl1->GetRigidBody();
673
674         if (rb1)
675         {
676                 
677
678                 genericConstraint = new btGeneric6DofConstraint(
679                         *rb0,*rb1,
680                         frameInA,frameInB);
681                 genericConstraint->setLinearLowerLimit(linearMinLimits);
682                 genericConstraint->setLinearUpperLimit(linearMaxLimits);
683                 genericConstraint->setAngularLowerLimit(angularMinLimits);
684                 genericConstraint->setAngularUpperLimit(angularMaxLimits);
685         } else
686         {
687                 // TODO: Implement single body case...
688                 //No, we can use a fixed rigidbody in above code, rather then unnecessary duplation of code
689
690         }
691         
692         if (genericConstraint)
693         {
694         //      m_constraints.push_back(genericConstraint);
695                 m_dynamicsWorld->addConstraint(genericConstraint);
696
697                 genericConstraint->setUserConstraintId(gConstraintUid++);
698                 genericConstraint->setUserConstraintType(PHY_GENERIC_6DOF_CONSTRAINT);
699                 //64 bit systems can't cast pointer to int. could use size_t instead.
700                 return genericConstraint->getUserConstraintId();
701         }
702         return 0;
703 }
704
705
706
707 void            CcdPhysicsEnvironment::removeConstraint(int     constraintId)
708 {
709         
710         int i;
711         int numConstraints = m_dynamicsWorld->getNumConstraints();
712         for (i=0;i<numConstraints;i++)
713         {
714                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
715                 if (constraint->getUserConstraintId() == constraintId)
716                 {
717                         constraint->getRigidBodyA().activate();
718                         constraint->getRigidBodyB().activate();
719                         m_dynamicsWorld->removeConstraint(constraint);
720                         break;
721                 }
722         }
723 }
724
725
726 struct  FilterClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
727 {
728         PHY_IPhysicsController* m_ignoreClient;
729
730         FilterClosestRayResultCallback (PHY_IPhysicsController* ignoreClient,const btVector3& rayFrom,const btVector3& rayTo)
731                 : btCollisionWorld::ClosestRayResultCallback(rayFrom,rayTo),
732                 m_ignoreClient(ignoreClient)
733         {
734
735         }
736
737         virtual ~FilterClosestRayResultCallback()
738         {
739         }
740
741         virtual float   AddSingleResult( btCollisionWorld::LocalRayResult& rayResult)
742         {
743                 CcdPhysicsController* curHit = static_cast<CcdPhysicsController*>(rayResult.m_collisionObject->getUserPointer());
744                 //ignore client...
745                 if (curHit != m_ignoreClient)
746                 {               
747                         //if valid
748                         return ClosestRayResultCallback::AddSingleResult(rayResult);
749                 }
750                 return m_closestHitFraction;
751         }
752
753 };
754
755 PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ, 
756                                                                                                            float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
757 {
758
759
760         float minFraction = 1.f;
761
762         btVector3 rayFrom(fromX,fromY,fromZ);
763         btVector3 rayTo(toX,toY,toZ);
764
765         btVector3       hitPointWorld,normalWorld;
766
767         //Either Ray Cast with or without filtering
768
769         //btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo);
770         FilterClosestRayResultCallback   rayCallback(ignoreClient,rayFrom,rayTo);
771
772
773         PHY_IPhysicsController* nearestHit = 0;
774         // don't collision with sensor object
775         m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback, CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter);
776         if (rayCallback.HasHit())
777         {
778                 nearestHit = static_cast<CcdPhysicsController*>(rayCallback.m_collisionObject->getUserPointer());
779                 hitX =  rayCallback.m_hitPointWorld.getX();
780                 hitY =  rayCallback.m_hitPointWorld.getY();
781                 hitZ =  rayCallback.m_hitPointWorld.getZ();
782
783                 if (rayCallback.m_hitNormalWorld.length2() > (SIMD_EPSILON*SIMD_EPSILON))
784                 {
785                         rayCallback.m_hitNormalWorld.normalize();
786                 } else
787                 {
788                         rayCallback.m_hitNormalWorld.setValue(1,0,0);
789                 }
790                 normalX = rayCallback.m_hitNormalWorld.getX();
791                 normalY = rayCallback.m_hitNormalWorld.getY();
792                 normalZ = rayCallback.m_hitNormalWorld.getZ();
793
794         }       
795
796
797         return nearestHit;
798 }
799
800
801
802 int     CcdPhysicsEnvironment::getNumContactPoints()
803 {
804         return 0;
805 }
806
807 void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
808 {
809
810 }
811
812
813
814
815 btBroadphaseInterface*  CcdPhysicsEnvironment::getBroadphase()
816
817         return m_dynamicsWorld->getBroadphase(); 
818 }
819
820
821
822
823 CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
824 {
825
826 #ifdef NEW_BULLET_VEHICLE_SUPPORT
827         m_wrapperVehicles.clear();
828 #endif //NEW_BULLET_VEHICLE_SUPPORT
829
830         //m_broadphase->DestroyScene();
831         //delete broadphase ? release reference on broadphase ?
832
833         //first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher
834         //delete m_dispatcher;
835         delete m_dynamicsWorld;
836         
837
838         if (NULL != m_ownPairCache)
839                 delete m_ownPairCache;
840
841         if (NULL != m_ownDispatcher)
842                 delete m_ownDispatcher;
843
844         if (NULL != m_solver)
845                 delete m_solver;
846
847         if (NULL != m_debugDrawer)
848                 delete m_debugDrawer;
849
850         if (NULL != m_filterCallback)
851                 delete m_filterCallback;
852 }
853
854
855 int     CcdPhysicsEnvironment::GetNumControllers()
856 {
857         return m_controllers.size();
858 }
859
860
861 CcdPhysicsController* CcdPhysicsEnvironment::GetPhysicsController( int index)
862 {
863         return m_controllers[index];
864 }
865
866
867
868
869 void    CcdPhysicsEnvironment::setConstraintParam(int constraintId,int param,float value0,float value1)
870 {
871         btTypedConstraint* typedConstraint = getConstraintById(constraintId);
872         switch (typedConstraint->getUserConstraintType())
873         {
874         case PHY_GENERIC_6DOF_CONSTRAINT:
875                 {
876                         //param = 1..12, min0,max0,min1,max1...min6,max6
877                         btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
878                         genCons->SetLimit(param,value0,value1);
879                         break;
880                 };
881         default:
882                 {
883                 };
884         };
885 }
886
887 btTypedConstraint*      CcdPhysicsEnvironment::getConstraintById(int constraintId)
888 {
889
890         int numConstraints = m_dynamicsWorld->getNumConstraints();
891         int i;
892         for (i=0;i<numConstraints;i++)
893         {
894                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
895                 if (constraint->getUserConstraintId()==constraintId)
896                 {
897                         return constraint;
898                 }
899         }
900         return 0;
901 }
902
903
904 void CcdPhysicsEnvironment::addSensor(PHY_IPhysicsController* ctrl)
905 {
906
907         CcdPhysicsController* ctrl1 = (CcdPhysicsController* )ctrl;
908         std::vector<CcdPhysicsController*>::iterator i =
909                 std::find(m_controllers.begin(), m_controllers.end(), ctrl);
910         if ((i == m_controllers.end()))
911         {
912                 addCcdPhysicsController(ctrl1);
913         }
914         //Collision filter/mask is now set at the time of the creation of the controller 
915         //force collision detection with everything, including static objects (might hurt performance!)
916         //ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterMask = btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::SensorTrigger;
917         //ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterGroup = btBroadphaseProxy::SensorTrigger;
918         //todo: make this 'sensor'!
919
920         requestCollisionCallback(ctrl);
921         //printf("addSensor\n");
922 }
923
924 void CcdPhysicsEnvironment::removeCollisionCallback(PHY_IPhysicsController* ctrl)
925 {
926         std::vector<CcdPhysicsController*>::iterator i =
927                 std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl);
928         if (!(i == m_triggerControllers.end()))
929         {
930                 std::swap(*i, m_triggerControllers.back());
931                 m_triggerControllers.pop_back();
932         }
933 }
934
935
936 void CcdPhysicsEnvironment::removeSensor(PHY_IPhysicsController* ctrl)
937 {
938         removeCollisionCallback(ctrl);
939         //printf("removeSensor\n");
940 }
941 void CcdPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user)
942 {
943         /*      printf("addTouchCallback\n(response class = %i)\n",response_class);
944
945         //map PHY_ convention into SM_ convention
946         switch (response_class)
947         {
948         case    PHY_FH_RESPONSE:
949         printf("PHY_FH_RESPONSE\n");
950         break;
951         case PHY_SENSOR_RESPONSE:
952         printf("PHY_SENSOR_RESPONSE\n");
953         break;
954         case PHY_CAMERA_RESPONSE:
955         printf("PHY_CAMERA_RESPONSE\n");
956         break;
957         case PHY_OBJECT_RESPONSE:
958         printf("PHY_OBJECT_RESPONSE\n");
959         break;
960         case PHY_STATIC_RESPONSE:
961         printf("PHY_STATIC_RESPONSE\n");
962         break;
963         default:
964         assert(0);
965         return;
966         }
967         */
968
969         m_triggerCallbacks[response_class] = callback;
970         m_triggerCallbacksUserPtrs[response_class] = user;
971
972 }
973 void CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctrl)
974 {
975         CcdPhysicsController* ccdCtrl = static_cast<CcdPhysicsController*>(ctrl);
976
977         //printf("requestCollisionCallback\n");
978         m_triggerControllers.push_back(ccdCtrl);
979 }
980
981
982 void    CcdPhysicsEnvironment::CallbackTriggers()
983 {
984         
985         CcdPhysicsController* ctrl0=0,*ctrl1=0;
986
987         if (m_triggerCallbacks[PHY_OBJECT_RESPONSE] || (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)))
988         {
989                 //walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback
990                 int numManifolds = m_dynamicsWorld->getDispatcher()->getNumManifolds();
991                 for (int i=0;i<numManifolds;i++)
992                 {
993                         btPersistentManifold* manifold = m_dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i);
994                         int numContacts = manifold->getNumContacts();
995                         if (numContacts)
996                         {
997                                 if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints))
998                                 {
999                                         for (int j=0;j<numContacts;j++)
1000                                         {
1001                                                 btVector3 color(1,0,0);
1002                                                 const btManifoldPoint& cp = manifold->getContactPoint(j);
1003                                                 if (m_debugDrawer)
1004                                                         m_debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
1005                                         }
1006                                 }
1007                                 btRigidBody* obj0 = static_cast<btRigidBody* >(manifold->getBody0());
1008                                 btRigidBody* obj1 = static_cast<btRigidBody* >(manifold->getBody1());
1009
1010                                 //m_internalOwner is set in 'addPhysicsController'
1011                                 CcdPhysicsController* ctrl0 = static_cast<CcdPhysicsController*>(obj0->getUserPointer());
1012                                 CcdPhysicsController* ctrl1 = static_cast<CcdPhysicsController*>(obj1->getUserPointer());
1013
1014                                 std::vector<CcdPhysicsController*>::iterator i =
1015                                         std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl0);
1016                                 if (i == m_triggerControllers.end())
1017                                 {
1018                                         i = std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl1);
1019                                 }
1020
1021                                 if (!(i == m_triggerControllers.end()))
1022                                 {
1023                                         m_triggerCallbacks[PHY_OBJECT_RESPONSE](m_triggerCallbacksUserPtrs[PHY_OBJECT_RESPONSE],
1024                                                 ctrl0,ctrl1,0);
1025                                 }
1026                         }
1027                 }
1028
1029
1030
1031         }
1032
1033
1034 }
1035
1036 // This call back is called before a pair is added in the cache
1037 // Handy to remove objects that must be ignored by sensors
1038 bool CcdOverlapFilterCallBack::needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
1039 {
1040         btCollisionObject *colObj0, *colObj1;
1041         CcdPhysicsController *sensorCtrl, *objCtrl;
1042         bool collides;
1043         // first check the filters
1044         collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
1045         collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
1046         if (!collides)
1047                 return false;
1048
1049         // additional check for sensor object
1050         if (proxy0->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger)
1051         {
1052                 // this is a sensor object, the other one can't be a sensor object because 
1053                 // they exclude each other in the above test
1054                 assert(!(proxy1->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger));
1055                 colObj0 = (btCollisionObject*)proxy0->m_clientObject;
1056                 colObj1 = (btCollisionObject*)proxy1->m_clientObject;
1057         }
1058         else if (proxy1->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger)
1059         {
1060                 colObj0 = (btCollisionObject*)proxy1->m_clientObject;
1061                 colObj1 = (btCollisionObject*)proxy0->m_clientObject;
1062         }
1063         else
1064         {
1065                 return true;
1066         }
1067         if (!colObj0 || !colObj1)
1068                 return false;
1069         sensorCtrl = static_cast<CcdPhysicsController*>(colObj0->getUserPointer());
1070         objCtrl = static_cast<CcdPhysicsController*>(colObj1->getUserPointer());
1071         if (m_physEnv->m_triggerCallbacks[PHY_BROADPH_RESPONSE])
1072         {
1073                 return m_physEnv->m_triggerCallbacks[PHY_BROADPH_RESPONSE](m_physEnv->m_triggerCallbacksUserPtrs[PHY_BROADPH_RESPONSE], sensorCtrl, objCtrl, 0);
1074         }
1075         return true;
1076 }
1077
1078
1079 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1080
1081 //complex constraint for vehicles
1082 PHY_IVehicle*   CcdPhysicsEnvironment::getVehicleConstraint(int constraintId)
1083 {
1084         int i;
1085
1086         int numVehicles = m_wrapperVehicles.size();
1087         for (i=0;i<numVehicles;i++)
1088         {
1089                 WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
1090                 if (wrapperVehicle->GetVehicle()->getUserConstraintId() == constraintId)
1091                         return wrapperVehicle;
1092         }
1093
1094         return 0;
1095 }
1096
1097 #endif //NEW_BULLET_VEHICLE_SUPPORT
1098
1099
1100 int currentController = 0;
1101 int numController = 0;
1102
1103
1104
1105
1106 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateSphereController(float radius,const PHY__Vector3& position)
1107 {
1108         
1109         CcdConstructionInfo     cinfo;
1110         // memory leak! The shape is not deleted by Bullet and we cannot add it to the KX_Scene.m_shapes list
1111         cinfo.m_collisionShape = new btSphereShape(radius);
1112         cinfo.m_MotionState = 0;
1113         cinfo.m_physicsEnv = this;
1114         // declare this object as Dyamic rather then static!!
1115         // The reason as it is designed to detect all type of object, including static object
1116         // It would cause static-static message to be printed on the console otherwise
1117         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE/* | btCollisionObject::CF_KINEMATIC_OBJECT*/;
1118         DefaultMotionState* motionState = new DefaultMotionState();
1119         cinfo.m_MotionState = motionState;
1120         // we will add later the possibility to select the filter from option
1121         cinfo.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
1122         cinfo.m_collisionFilterGroup = CcdConstructionInfo::SensorFilter;
1123         motionState->m_worldTransform.setIdentity();
1124         motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
1125
1126         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
1127         
1128
1129         return sphereController;
1130 }
1131
1132 int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
1133                                                                                                         float pivotX,float pivotY,float pivotZ,
1134                                                                                                         float axisX,float axisY,float axisZ,
1135                                                                                                         float axis1X,float axis1Y,float axis1Z,
1136                                                                                                         float axis2X,float axis2Y,float axis2Z
1137                                                                                                         )
1138 {
1139
1140
1141         CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
1142         CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
1143
1144         btRigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
1145         btRigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
1146
1147         bool rb0static = rb0 ? rb0->isStaticOrKinematicObject() : true;
1148         bool rb1static = rb1 ? rb1->isStaticOrKinematicObject() : true;
1149         
1150
1151         if (rb0static && rb1static)
1152                 return 0;
1153
1154         btVector3 pivotInA(pivotX,pivotY,pivotZ);
1155         btVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : 
1156                 rb0->getCenterOfMassTransform() * pivotInA;
1157         btVector3 axisInA(axisX,axisY,axisZ);
1158
1159
1160         bool angularOnly = false;
1161
1162         switch (type)
1163         {
1164         case PHY_POINT2POINT_CONSTRAINT:
1165                 {
1166
1167                         btPoint2PointConstraint* p2p = 0;
1168
1169                         if (rb1)
1170                         {
1171                                 p2p = new btPoint2PointConstraint(*rb0,
1172                                         *rb1,pivotInA,pivotInB);
1173                         } else
1174                         {
1175                                 p2p = new btPoint2PointConstraint(*rb0,
1176                                         pivotInA);
1177                         }
1178
1179                         m_dynamicsWorld->addConstraint(p2p);
1180 //                      m_constraints.push_back(p2p);
1181
1182                         p2p->setUserConstraintId(gConstraintUid++);
1183                         p2p->setUserConstraintType(type);
1184                         //64 bit systems can't cast pointer to int. could use size_t instead.
1185                         return p2p->getUserConstraintId();
1186
1187                         break;
1188                 }
1189
1190         case PHY_GENERIC_6DOF_CONSTRAINT:
1191                 {
1192                         btGeneric6DofConstraint* genericConstraint = 0;
1193
1194                         if (rb1)
1195                         {
1196                                 btTransform frameInA;
1197                                 btTransform frameInB;
1198                                 
1199                                 btVector3 axis1(axis1X,axis1Y,axis1Z), axis2(axis2X,axis2Y,axis2Z);
1200                                 if (axis1.length() == 0.0)
1201                                 {
1202                                         btPlaneSpace1( axisInA, axis1, axis2 );
1203                                 }
1204                                 
1205                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1206                                                                   axisInA.y(), axis1.y(), axis2.y(),
1207                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1208                                 frameInA.setOrigin( pivotInA );
1209
1210                                 btTransform inv = rb1->getCenterOfMassTransform().inverse();
1211
1212                                 btTransform globalFrameA = rb0->getCenterOfMassTransform() * frameInA;
1213                                 
1214                                 frameInB = inv  * globalFrameA;
1215                                 
1216                                 genericConstraint = new btGeneric6DofConstraint(
1217                                         *rb0,*rb1,
1218                                         frameInA,frameInB);
1219
1220
1221                         } else
1222                         {
1223                                 static btRigidBody s_fixedObject2( 0,0,0);
1224                                 btTransform frameInA;
1225                                 btTransform frameInB;
1226                                 
1227                                 btVector3 axis1, axis2;
1228                                 btPlaneSpace1( axisInA, axis1, axis2 );
1229
1230                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1231                                                                   axisInA.y(), axis1.y(), axis2.y(),
1232                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1233
1234                                 frameInA.setOrigin( pivotInA );
1235
1236                                 ///frameInB in worldspace
1237                                 frameInB = rb0->getCenterOfMassTransform() * frameInA;
1238
1239                                 genericConstraint = new btGeneric6DofConstraint(
1240                                         *rb0,s_fixedObject2,
1241                                         frameInA,frameInB);
1242                         }
1243                         
1244                         if (genericConstraint)
1245                         {
1246                                 //m_constraints.push_back(genericConstraint);
1247                                 m_dynamicsWorld->addConstraint(genericConstraint);
1248                                 genericConstraint->setUserConstraintId(gConstraintUid++);
1249                                 genericConstraint->setUserConstraintType(type);
1250                                 //64 bit systems can't cast pointer to int. could use size_t instead.
1251                                 return genericConstraint->getUserConstraintId();
1252                         } 
1253
1254                         break;
1255                 }
1256         case PHY_CONE_TWIST_CONSTRAINT:
1257                 {
1258                         btConeTwistConstraint* coneTwistContraint = 0;
1259
1260                         
1261                         if (rb1)
1262                         {
1263                                 btTransform frameInA;
1264                                 btTransform frameInB;
1265                                 
1266                                 btVector3 axis1(axis1X,axis1Y,axis1Z), axis2(axis2X,axis2Y,axis2Z);
1267                                 if (axis1.length() == 0.0)
1268                                 {
1269                                         btPlaneSpace1( axisInA, axis1, axis2 );
1270                                 }
1271                                 
1272                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1273                                                                   axisInA.y(), axis1.y(), axis2.y(),
1274                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1275                                 frameInA.setOrigin( pivotInA );
1276
1277                                 btTransform inv = rb1->getCenterOfMassTransform().inverse();
1278
1279                                 btTransform globalFrameA = rb0->getCenterOfMassTransform() * frameInA;
1280                                 
1281                                 frameInB = inv  * globalFrameA;
1282                                 
1283                                 coneTwistContraint = new btConeTwistConstraint( *rb0,*rb1,
1284                                         frameInA,frameInB);
1285
1286
1287                         } else
1288                         {
1289                                 static btRigidBody s_fixedObject2( 0,0,0);
1290                                 btTransform frameInA;
1291                                 btTransform frameInB;
1292                                 
1293                                 btVector3 axis1, axis2;
1294                                 btPlaneSpace1( axisInA, axis1, axis2 );
1295
1296                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1297                                                                   axisInA.y(), axis1.y(), axis2.y(),
1298                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1299
1300                                 frameInA.setOrigin( pivotInA );
1301
1302                                 ///frameInB in worldspace
1303                                 frameInB = rb0->getCenterOfMassTransform() * frameInA;
1304
1305                                 coneTwistContraint = new btConeTwistConstraint(
1306                                         *rb0,s_fixedObject2,
1307                                         frameInA,frameInB);
1308                         }
1309                         
1310                         if (coneTwistContraint)
1311                         {
1312                                 //m_constraints.push_back(genericConstraint);
1313                                 m_dynamicsWorld->addConstraint(coneTwistContraint);
1314                                 coneTwistContraint->setUserConstraintId(gConstraintUid++);
1315                                 coneTwistContraint->setUserConstraintType(type);
1316                                 //64 bit systems can't cast pointer to int. could use size_t instead.
1317                                 return coneTwistContraint->getUserConstraintId();
1318                         } 
1319
1320
1321
1322                         break;
1323                 }
1324         case PHY_ANGULAR_CONSTRAINT:
1325                 angularOnly = true;
1326
1327
1328         case PHY_LINEHINGE_CONSTRAINT:
1329                 {
1330                         btHingeConstraint* hinge = 0;
1331
1332                         if (rb1)
1333                         {
1334                                 btVector3 axisInB = rb1 ? 
1335                                 (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) : 
1336                                 rb0->getCenterOfMassTransform().getBasis() * axisInA;
1337
1338                                 hinge = new btHingeConstraint(
1339                                         *rb0,
1340                                         *rb1,pivotInA,pivotInB,axisInA,axisInB);
1341
1342
1343                         } else
1344                         {
1345                                 hinge = new btHingeConstraint(*rb0,
1346                                         pivotInA,axisInA);
1347
1348                         }
1349                         hinge->setAngularOnly(angularOnly);
1350
1351                         //m_constraints.push_back(hinge);
1352                         m_dynamicsWorld->addConstraint(hinge);
1353                         hinge->setUserConstraintId(gConstraintUid++);
1354                         hinge->setUserConstraintType(type);
1355                         //64 bit systems can't cast pointer to int. could use size_t instead.
1356                         return hinge->getUserConstraintId();
1357                         break;
1358                 }
1359 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1360
1361         case PHY_VEHICLE_CONSTRAINT:
1362                 {
1363                         btRaycastVehicle::btVehicleTuning* tuning = new btRaycastVehicle::btVehicleTuning();
1364                         btRigidBody* chassis = rb0;
1365                         btDefaultVehicleRaycaster* raycaster = new btDefaultVehicleRaycaster(m_dynamicsWorld);
1366                         btRaycastVehicle* vehicle = new btRaycastVehicle(*tuning,chassis,raycaster);
1367                         WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
1368                         m_wrapperVehicles.push_back(wrapperVehicle);
1369                         m_dynamicsWorld->addVehicle(vehicle);
1370                         vehicle->setUserConstraintId(gConstraintUid++);
1371                         vehicle->setUserConstraintType(type);
1372                         return vehicle->getUserConstraintId();
1373
1374                         break;
1375                 };
1376 #endif //NEW_BULLET_VEHICLE_SUPPORT
1377
1378         default:
1379                 {
1380                 }
1381         };
1382
1383         //btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB
1384
1385         return 0;
1386
1387 }
1388
1389
1390
1391 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float coneradius,float coneheight)
1392 {
1393         CcdConstructionInfo     cinfo;
1394         //This is a memory leak: Bullet does not delete the shape and it cannot be added to 
1395         //the KX_Scene.m_shapes list -- too bad but that's not a lot of data
1396         cinfo.m_collisionShape = new btConeShape(coneradius,coneheight);
1397         cinfo.m_MotionState = 0;
1398         cinfo.m_physicsEnv = this;
1399         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE;
1400         DefaultMotionState* motionState = new DefaultMotionState();
1401         cinfo.m_MotionState = motionState;
1402         
1403         // we will add later the possibility to select the filter from option
1404         cinfo.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
1405         cinfo.m_collisionFilterGroup = CcdConstructionInfo::SensorFilter;
1406         motionState->m_worldTransform.setIdentity();
1407 //      motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
1408
1409         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
1410
1411
1412         return sphereController;
1413 }
1414         
1415 float           CcdPhysicsEnvironment::getAppliedImpulse(int    constraintid)
1416 {
1417         int i;
1418         int numConstraints = m_dynamicsWorld->getNumConstraints();
1419         for (i=0;i<numConstraints;i++)
1420         {
1421                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
1422                 if (constraint->getUserConstraintId() == constraintid)
1423                 {
1424                         return constraint->getAppliedImpulse();
1425                 }
1426         }
1427
1428         return 0.f;
1429 }