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