Fix bug in rigid body constraint buttons drawing, and while we are at it, add 'Collis...
[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 #include "BulletSoftBody/btSoftRigidDynamicsWorld.h"
27 #include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
28 #include "BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h"
29
30 //profiling/timings
31 #include "LinearMath/btQuickprof.h"
32
33
34 #include "PHY_IMotionState.h"
35
36 #define CCD_CONSTRAINT_DISABLE_LINKED_COLLISION 0x80
37
38 bool useIslands = true;
39
40 #ifdef NEW_BULLET_VEHICLE_SUPPORT
41 #include "BulletDynamics/Vehicle/btRaycastVehicle.h"
42 #include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
43 #include "BulletDynamics/Vehicle/btWheelInfo.h"
44 #include "PHY_IVehicle.h"
45 btRaycastVehicle::btVehicleTuning       gTuning;
46
47 #endif //NEW_BULLET_VEHICLE_SUPPORT
48 #include "LinearMath/btAabbUtil2.h"
49
50
51 #ifdef WIN32
52 void DrawRasterizerLine(const float* from,const float* to,int color);
53 #endif
54
55
56 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
57
58
59 #include <stdio.h>
60 #include <string.h>             // for memset
61
62 #ifdef NEW_BULLET_VEHICLE_SUPPORT
63 class WrapperVehicle : public PHY_IVehicle
64 {
65
66         btRaycastVehicle*       m_vehicle;
67         PHY_IPhysicsController* m_chassis;
68
69 public:
70
71         WrapperVehicle(btRaycastVehicle* vehicle,PHY_IPhysicsController* chassis)
72                 :m_vehicle(vehicle),
73                 m_chassis(chassis)
74         {
75         }
76
77         btRaycastVehicle*       GetVehicle()
78         {
79                 return m_vehicle;
80         }
81
82         PHY_IPhysicsController* GetChassis()
83         {
84                 return m_chassis;
85         }
86
87         virtual void    AddWheel(
88                 PHY_IMotionState*       motionState,
89                 PHY__Vector3    connectionPoint,
90                 PHY__Vector3    downDirection,
91                 PHY__Vector3    axleDirection,
92                 float   suspensionRestLength,
93                 float wheelRadius,
94                 bool hasSteering
95                 )
96         {
97                 btVector3 connectionPointCS0(connectionPoint[0],connectionPoint[1],connectionPoint[2]);
98                 btVector3 wheelDirectionCS0(downDirection[0],downDirection[1],downDirection[2]);
99                 btVector3 wheelAxle(axleDirection[0],axleDirection[1],axleDirection[2]);
100
101
102                 btWheelInfo& info = m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxle,
103                         suspensionRestLength,wheelRadius,gTuning,hasSteering);
104                 info.m_clientInfo = motionState;
105
106         }
107
108         void    SyncWheels()
109         {
110                 int numWheels = GetNumWheels();
111                 int i;
112                 for (i=0;i<numWheels;i++)
113                 {
114                         btWheelInfo& info = m_vehicle->getWheelInfo(i);
115                         PHY_IMotionState* motionState = (PHY_IMotionState*)info.m_clientInfo ;
116         //              m_vehicle->updateWheelTransformsWS(info,false);
117                         m_vehicle->updateWheelTransform(i,false);
118                         btTransform trans = m_vehicle->getWheelInfo(i).m_worldTransform;
119                         btQuaternion orn = trans.getRotation();
120                         const btVector3& pos = trans.getOrigin();
121                         motionState->setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]);
122                         motionState->setWorldPosition(pos.x(),pos.y(),pos.z());
123
124                 }
125         }
126
127         virtual int             GetNumWheels() const
128         {
129                 return m_vehicle->getNumWheels();
130         }
131
132         virtual void    GetWheelPosition(int wheelIndex,float& posX,float& posY,float& posZ) const
133         {
134                 btTransform     trans = m_vehicle->getWheelTransformWS(wheelIndex);
135                 posX = trans.getOrigin().x();
136                 posY = trans.getOrigin().y();
137                 posZ = trans.getOrigin().z();
138         }
139         virtual void    GetWheelOrientationQuaternion(int wheelIndex,float& quatX,float& quatY,float& quatZ,float& quatW) const
140         {
141                 btTransform     trans = m_vehicle->getWheelTransformWS(wheelIndex);
142                 btQuaternion quat = trans.getRotation();
143                 btMatrix3x3 orn2(quat);
144
145                 quatX = trans.getRotation().x();
146                 quatY = trans.getRotation().y();
147                 quatZ = trans.getRotation().z();
148                 quatW = trans.getRotation()[3];
149
150
151                 //printf("test");
152
153
154         }
155
156         virtual float   GetWheelRotation(int wheelIndex) const
157         {
158                 float rotation = 0.f;
159
160                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
161                 {
162                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
163                         rotation = info.m_rotation;
164                 }
165                 return rotation;
166
167         }
168
169
170
171         virtual int     GetUserConstraintId() const
172         {
173                 return m_vehicle->getUserConstraintId();
174         }
175
176         virtual int     GetUserConstraintType() const
177         {
178                 return m_vehicle->getUserConstraintType();
179         }
180
181         virtual void    SetSteeringValue(float steering,int wheelIndex)
182         {
183                 m_vehicle->setSteeringValue(steering,wheelIndex);
184         }
185
186         virtual void    ApplyEngineForce(float force,int wheelIndex)
187         {
188                 m_vehicle->applyEngineForce(force,wheelIndex);
189         }
190
191         virtual void    ApplyBraking(float braking,int wheelIndex)
192         {
193                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
194                 {
195                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
196                         info.m_brake = braking;
197                 }
198         }
199
200         virtual void    SetWheelFriction(float friction,int wheelIndex)
201         {
202                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
203                 {
204                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
205                         info.m_frictionSlip = friction;
206                 }
207
208         }
209
210         virtual void    SetSuspensionStiffness(float suspensionStiffness,int wheelIndex)
211         {
212                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
213                 {
214                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
215                         info.m_suspensionStiffness = suspensionStiffness;
216
217                 }
218         }
219
220         virtual void    SetSuspensionDamping(float suspensionDamping,int wheelIndex)
221         {
222                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
223                 {
224                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
225                         info.m_wheelsDampingRelaxation = suspensionDamping;
226                 }
227         }
228
229         virtual void    SetSuspensionCompression(float suspensionCompression,int wheelIndex)
230         {
231                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
232                 {
233                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
234                         info.m_wheelsDampingCompression = suspensionCompression;
235                 }
236         }
237
238
239
240         virtual void    SetRollInfluence(float rollInfluence,int wheelIndex)
241         {
242                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
243                 {
244                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
245                         info.m_rollInfluence = rollInfluence;
246                 }
247         }
248
249         virtual void    SetCoordinateSystem(int rightIndex,int upIndex,int forwardIndex)
250         {
251                 m_vehicle->setCoordinateSystem(rightIndex,upIndex,forwardIndex);
252         }
253
254
255
256 };
257 #endif //NEW_BULLET_VEHICLE_SUPPORT
258
259 class CcdOverlapFilterCallBack : public btOverlapFilterCallback
260 {
261 private:
262         class CcdPhysicsEnvironment* m_physEnv;
263 public:
264         CcdOverlapFilterCallBack(CcdPhysicsEnvironment* env) : 
265                 m_physEnv(env)
266         {
267         }
268         virtual ~CcdOverlapFilterCallBack()
269         {
270         }
271         // return true when pairs need collision
272         virtual bool    needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const;
273 };
274
275
276 void CcdPhysicsEnvironment::setDebugDrawer(btIDebugDraw* debugDrawer)
277 {
278         if (debugDrawer && m_dynamicsWorld)
279                 m_dynamicsWorld->setDebugDrawer(debugDrawer);
280         m_debugDrawer = debugDrawer;
281 }
282
283 static void DrawAabb(btIDebugDraw* debugDrawer,const btVector3& from,const btVector3& to,const btVector3& color)
284 {
285         btVector3 halfExtents = (to-from)* 0.5f;
286         btVector3 center = (to+from) *0.5f;
287         int i,j;
288
289         btVector3 edgecoord(1.f,1.f,1.f),pa,pb;
290         for (i=0;i<4;i++)
291         {
292                 for (j=0;j<3;j++)
293                 {
294                         pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],                
295                                 edgecoord[2]*halfExtents[2]);
296                         pa+=center;
297
298                         int othercoord = j%3;
299                         edgecoord[othercoord]*=-1.f;
300                         pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],        
301                                 edgecoord[2]*halfExtents[2]);
302                         pb+=center;
303
304                         debugDrawer->drawLine(pa,pb,color);
305                 }
306                 edgecoord = btVector3(-1.f,-1.f,-1.f);
307                 if (i<3)
308                         edgecoord[i]*=-1.f;
309         }
310
311
312 }
313
314
315
316
317
318
319 CcdPhysicsEnvironment::CcdPhysicsEnvironment(btDispatcher* dispatcher,btOverlappingPairCache* pairCache)
320 :m_scalingPropagated(false),
321 m_numIterations(10),
322 m_numTimeSubSteps(1),
323 m_ccdMode(0),
324 m_solverType(-1),
325 m_profileTimings(0),
326 m_enableSatCollisionDetection(false),
327 m_solver(NULL),
328 m_ownPairCache(NULL),
329 m_ownDispatcher(NULL),
330 m_filterCallback(NULL)
331 {
332
333         for (int i=0;i<PHY_NUM_RESPONSE;i++)
334         {
335                 m_triggerCallbacks[i] = 0;
336         }
337
338 //      m_collisionConfiguration = new btDefaultCollisionConfiguration();
339         m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
340
341         if (!dispatcher)
342         {
343                 btCollisionDispatcher* disp = new btCollisionDispatcher(m_collisionConfiguration);
344                 dispatcher = disp;
345                 btGImpactCollisionAlgorithm::registerAlgorithm(disp);
346                 m_ownDispatcher = dispatcher;
347         }
348
349         //m_broadphase = new btAxisSweep3(btVector3(-1000,-1000,-1000),btVector3(1000,1000,1000));
350         //m_broadphase = new btSimpleBroadphase();
351         m_broadphase = new btDbvtBroadphase();
352
353         m_filterCallback = new CcdOverlapFilterCallBack(this);
354         m_broadphase->getOverlappingPairCache()->setOverlapFilterCallback(m_filterCallback);
355
356         setSolverType(1);//issues with quickstep and memory allocations
357 //      m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
358         m_dynamicsWorld = new btSoftRigidDynamicsWorld(dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
359
360         m_debugDrawer = 0;
361         m_gravity = btVector3(0.f,-10.f,0.f);
362         m_dynamicsWorld->setGravity(m_gravity);
363
364
365 }
366
367 void    CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
368 {
369         btRigidBody* body = ctrl->GetRigidBody();
370         btCollisionObject* obj = ctrl->GetCollisionObject();
371
372         //this m_userPointer is just used for triggers, see CallbackTriggers
373         obj->setUserPointer(ctrl);
374         if (body)
375                 body->setGravity( m_gravity );
376
377         m_controllers.insert(ctrl);
378
379         if (body)
380         {
381                 //use explicit group/filter for finer control over collision in bullet => near/radar sensor
382                 m_dynamicsWorld->addRigidBody(body, ctrl->GetCollisionFilterGroup(), ctrl->GetCollisionFilterMask());
383         } else
384         {
385                 if (ctrl->GetSoftBody())
386                 {
387                         btSoftBody* softBody = ctrl->GetSoftBody();
388                         m_dynamicsWorld->addSoftBody(softBody);
389                 } else
390                 {
391                         if (obj->getCollisionShape())
392                         {
393                                 m_dynamicsWorld->addCollisionObject(obj,ctrl->GetCollisionFilterGroup(), ctrl->GetCollisionFilterMask());
394                         }
395                 }
396         }
397         if (obj->isStaticOrKinematicObject())
398         {
399                 obj->setActivationState(ISLAND_SLEEPING);
400         }
401
402
403         //CollisionObject(body,ctrl->GetCollisionFilterGroup(),ctrl->GetCollisionFilterMask());
404
405         assert(obj->getBroadphaseHandle());
406
407         btBroadphaseInterface* scene =  getBroadphase();
408
409
410         btCollisionShape* shapeinterface = ctrl->GetCollisionShape();
411
412         assert(shapeinterface);
413
414         const btTransform& t = ctrl->GetCollisionObject()->getWorldTransform();
415         
416
417         btPoint3 minAabb,maxAabb;
418
419         shapeinterface->getAabb(t,minAabb,maxAabb);
420
421         float timeStep = 0.02f;
422
423
424         //extent it with the motion
425
426         if (body)
427         {
428                 btVector3 linMotion = body->getLinearVelocity()*timeStep;
429
430                 float maxAabbx = maxAabb.getX();
431                 float maxAabby = maxAabb.getY();
432                 float maxAabbz = maxAabb.getZ();
433                 float minAabbx = minAabb.getX();
434                 float minAabby = minAabb.getY();
435                 float minAabbz = minAabb.getZ();
436
437                 if (linMotion.x() > 0.f)
438                         maxAabbx += linMotion.x(); 
439                 else
440                         minAabbx += linMotion.x();
441                 if (linMotion.y() > 0.f)
442                         maxAabby += linMotion.y(); 
443                 else
444                         minAabby += linMotion.y();
445                 if (linMotion.z() > 0.f)
446                         maxAabbz += linMotion.z(); 
447                 else
448                         minAabbz += linMotion.z();
449
450
451                 minAabb = btVector3(minAabbx,minAabby,minAabbz);
452                 maxAabb = btVector3(maxAabbx,maxAabby,maxAabbz);
453         }
454
455
456
457 }
458
459                 
460
461 void    CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctrl)
462 {
463         //also remove constraint
464         btRigidBody* body = ctrl->GetRigidBody();
465         if (body)
466         {
467                 m_dynamicsWorld->removeRigidBody(ctrl->GetRigidBody());
468         } else
469         {
470                 //if a softbody
471                 if (ctrl->GetSoftBody())
472                 {
473                         m_dynamicsWorld->removeSoftBody(ctrl->GetSoftBody());
474                 } else
475                 {
476                         m_dynamicsWorld->removeCollisionObject(ctrl->GetCollisionObject());
477                 }
478         }
479         m_controllers.erase(ctrl);
480
481         if (ctrl->m_registerCount != 0)
482                 printf("Warning: removing controller with non-zero m_registerCount: %d\n", ctrl->m_registerCount);
483
484         //remove it from the triggers
485         m_triggerControllers.erase(ctrl);
486 }
487
488 void    CcdPhysicsEnvironment::updateCcdPhysicsController(CcdPhysicsController* ctrl, btScalar newMass, int newCollisionFlags, short int newCollisionGroup, short int newCollisionMask)
489 {
490         // this function is used when the collisionning group of a controller is changed
491         // remove and add the collistioning object
492         btRigidBody* body = ctrl->GetRigidBody();
493         btCollisionObject* obj = ctrl->GetCollisionObject();
494         if (obj)
495         {
496                 btVector3 inertia(0.0,0.0,0.0);
497                 m_dynamicsWorld->removeCollisionObject(obj);
498                 obj->setCollisionFlags(newCollisionFlags);
499                 if (body)
500                 {
501                         if (newMass)
502                                 body->getCollisionShape()->calculateLocalInertia(newMass, inertia);
503                         body->setMassProps(newMass, inertia);
504                 }
505                 m_dynamicsWorld->addCollisionObject(obj, newCollisionGroup, newCollisionMask);
506         }
507         // to avoid nasty interaction, we must update the property of the controller as well
508         ctrl->m_cci.m_mass = newMass;
509         ctrl->m_cci.m_collisionFilterGroup = newCollisionGroup;
510         ctrl->m_cci.m_collisionFilterMask = newCollisionMask;
511         ctrl->m_cci.m_collisionFlags = newCollisionFlags;
512 }
513
514 void CcdPhysicsEnvironment::enableCcdPhysicsController(CcdPhysicsController* ctrl)
515 {
516         if (m_controllers.insert(ctrl).second)
517         {
518                 btCollisionObject* obj = ctrl->GetCollisionObject();
519                 obj->setUserPointer(ctrl);
520                 m_dynamicsWorld->addCollisionObject(obj, 
521                         ctrl->GetCollisionFilterGroup(), ctrl->GetCollisionFilterMask());
522         }
523 }
524
525 void CcdPhysicsEnvironment::disableCcdPhysicsController(CcdPhysicsController* ctrl)
526 {
527         if (m_controllers.erase(ctrl))
528         {
529                 btRigidBody* body = ctrl->GetRigidBody();
530                 if (body)
531                 {
532                         m_dynamicsWorld->removeRigidBody(body);
533                 } else
534                 {
535                         if (ctrl->GetSoftBody())
536                         {
537                         } else
538                         {
539                                 m_dynamicsWorld->removeCollisionObject(body);
540                         }
541                 }
542         }
543 }
544
545
546 void    CcdPhysicsEnvironment::beginFrame()
547 {
548
549 }
550
551
552 bool    CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
553 {
554         std::set<CcdPhysicsController*>::iterator it;
555         int i;
556
557         for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
558         {
559                 (*it)->SynchronizeMotionStates(timeStep);
560         }
561
562         processFhSprings(curTime,timeStep);
563
564         float subStep = timeStep / float(m_numTimeSubSteps);
565         for (i=0;i<m_numTimeSubSteps;i++)
566         {
567 //                      m_dynamicsWorld->stepSimulation(subStep,20,1./240.);//perform always a full simulation step
568                         m_dynamicsWorld->stepSimulation(subStep,0);//perform always a full simulation step
569         }
570
571         for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
572         {
573                 (*it)->SynchronizeMotionStates(timeStep);
574         }
575
576         for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
577         {
578                 (*it)->SynchronizeMotionStates(timeStep);
579         }
580
581         for (i=0;i<m_wrapperVehicles.size();i++)
582         {
583                 WrapperVehicle* veh = m_wrapperVehicles[i];
584                 veh->SyncWheels();
585         }
586
587         if (m_dynamicsWorld->getDebugDrawer() &&  m_dynamicsWorld->getDebugDrawer()->getDebugMode() >0)
588                 m_dynamicsWorld->debugDrawWorld();
589
590
591         CallbackTriggers();
592
593         return true;
594 }
595
596 class ClosestRayResultCallbackNotMe : public btCollisionWorld::ClosestRayResultCallback
597 {
598         btCollisionObject* m_owner;
599         btCollisionObject* m_parent;
600
601 public:
602         ClosestRayResultCallbackNotMe(const btVector3& rayFromWorld,const btVector3& rayToWorld,btCollisionObject* owner,btCollisionObject* parent)
603                 :btCollisionWorld::ClosestRayResultCallback(rayFromWorld,rayToWorld),
604                 m_owner(owner)
605         {
606
607         }
608
609         virtual bool needsCollision(btBroadphaseProxy* proxy0) const
610         {
611                 //don't collide with self
612                 if (proxy0->m_clientObject == m_owner)
613                         return false;
614
615                 if (proxy0->m_clientObject == m_parent)
616                         return false;
617
618                 return btCollisionWorld::ClosestRayResultCallback::needsCollision(proxy0);
619         }
620
621 };
622
623 void    CcdPhysicsEnvironment::processFhSprings(double curTime,float timeStep)
624 {
625         std::set<CcdPhysicsController*>::iterator it;
626         
627         for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
628         {
629                 CcdPhysicsController* ctrl = (*it);
630                 btRigidBody* body = ctrl->GetRigidBody();
631
632                 if (body && (ctrl->getConstructionInfo().m_do_fh || ctrl->getConstructionInfo().m_do_rot_fh))
633                 {
634                         //printf("has Fh or RotFh\n");
635                         //re-implement SM_FhObject.cpp using btCollisionWorld::rayTest and info from ctrl->getConstructionInfo()
636                         //send a ray from {0.0, 0.0, 0.0} towards {0.0, 0.0, -10.0}, in local coordinates
637
638                         
639                         CcdPhysicsController* parentCtrl = ctrl->getParentCtrl();
640                         btRigidBody* parentBody = parentCtrl?parentCtrl->GetRigidBody() : 0;
641                         btRigidBody* cl_object = parentBody ? parentBody : body;
642
643                         if (body->isStaticOrKinematicObject())
644                                 continue;
645
646                         btVector3 rayDirLocal(0,0,-10);
647
648                         //m_dynamicsWorld
649                         //ctrl->GetRigidBody();
650                         btVector3 rayFromWorld = body->getCenterOfMassPosition();
651                         //btVector3     rayToWorld = rayFromWorld + body->getCenterOfMassTransform().getBasis() * rayDirLocal;
652                         //ray always points down the z axis in world space...
653                         btVector3       rayToWorld = rayFromWorld + rayDirLocal;
654
655                         ClosestRayResultCallbackNotMe   resultCallback(rayFromWorld,rayToWorld,body,parentBody);
656
657                         m_dynamicsWorld->rayTest(rayFromWorld,rayToWorld,resultCallback);
658                         if (resultCallback.hasHit())
659                         {
660                                 //we hit this one: resultCallback.m_collisionObject;
661                                 CcdPhysicsController* controller = static_cast<CcdPhysicsController*>(resultCallback.m_collisionObject->getUserPointer());
662
663                                 if (controller)
664                                 {
665                                         if (controller->getConstructionInfo().m_fh_distance < SIMD_EPSILON)
666                                                 continue;
667
668                                         btRigidBody* hit_object = controller->GetRigidBody();
669                                         if (!hit_object)
670                                                 continue;
671
672                                         CcdConstructionInfo& hitObjShapeProps = controller->getConstructionInfo();
673
674                                         float distance = resultCallback.m_closestHitFraction*rayDirLocal.length()-ctrl->getConstructionInfo().m_radius;
675                                         if (distance >= hitObjShapeProps.m_fh_distance)
676                                                 continue;
677                                         
678                                         
679
680                                         //btVector3 ray_dir = cl_object->getCenterOfMassTransform().getBasis()* rayDirLocal.normalized();
681                                         btVector3 ray_dir = rayDirLocal.normalized();
682                                         btVector3 normal = resultCallback.m_hitNormalWorld;
683                                         normal.normalize();
684
685                                         
686                                         if (ctrl->getConstructionInfo().m_do_fh) 
687                                         {
688                                                 btVector3 lspot = cl_object->getCenterOfMassPosition()
689                                                         + rayDirLocal * resultCallback.m_closestHitFraction;
690
691
692                                                         
693
694                                                 lspot -= hit_object->getCenterOfMassPosition();
695                                                 btVector3 rel_vel = cl_object->getLinearVelocity() - hit_object->getVelocityInLocalPoint(lspot);
696                                                 btScalar rel_vel_ray = ray_dir.dot(rel_vel);
697                                                 btScalar spring_extent = 1.0 - distance / hitObjShapeProps.m_fh_distance; 
698                                                 
699                                                 btScalar i_spring = spring_extent * hitObjShapeProps.m_fh_spring;
700                                                 btScalar i_damp =   rel_vel_ray * hitObjShapeProps.m_fh_damping;
701                                                 
702                                                 cl_object->setLinearVelocity(cl_object->getLinearVelocity() + (-(i_spring + i_damp) * ray_dir)); 
703                                                 if (hitObjShapeProps.m_fh_normal) 
704                                                 {
705                                                         cl_object->setLinearVelocity(cl_object->getLinearVelocity()+(i_spring + i_damp) *(normal - normal.dot(ray_dir) * ray_dir));
706                                                 }
707                                                 
708                                                 btVector3 lateral = rel_vel - rel_vel_ray * ray_dir;
709                                                 
710                                                 
711                                                 if (ctrl->getConstructionInfo().m_do_anisotropic) {
712                                                         //Bullet basis contains no scaling/shear etc.
713                                                         const btMatrix3x3& lcs = cl_object->getCenterOfMassTransform().getBasis();
714                                                         btVector3 loc_lateral = lateral * lcs;
715                                                         const btVector3& friction_scaling = cl_object->getAnisotropicFriction();
716                                                         loc_lateral *= friction_scaling;
717                                                         lateral = lcs * loc_lateral;
718                                                 }
719
720                                                 btScalar rel_vel_lateral = lateral.length();
721                                                 
722                                                 if (rel_vel_lateral > SIMD_EPSILON) {
723                                                         btScalar friction_factor = hit_object->getFriction();//cl_object->getFriction();
724
725                                                         btScalar max_friction = friction_factor * btMax(btScalar(0.0), i_spring);
726                                                         
727                                                         btScalar rel_mom_lateral = rel_vel_lateral / cl_object->getInvMass();
728                                                         
729                                                         btVector3 friction = (rel_mom_lateral > max_friction) ?
730                                                                 -lateral * (max_friction / rel_vel_lateral) :
731                                                                 -lateral;
732                                                         
733                                                                 cl_object->applyCentralImpulse(friction);
734                                                 }
735                                         }
736
737                                         
738                                         if (ctrl->getConstructionInfo().m_do_rot_fh) {
739                                                 btVector3 up2 = cl_object->getWorldTransform().getBasis().getColumn(2);
740
741                                                 btVector3 t_spring = up2.cross(normal) * hitObjShapeProps.m_fh_spring;
742                                                 btVector3 ang_vel = cl_object->getAngularVelocity();
743                                                 
744                                                 // only rotations that tilt relative to the normal are damped
745                                                 ang_vel -= ang_vel.dot(normal) * normal;
746                                                 
747                                                 btVector3 t_damp = ang_vel * hitObjShapeProps.m_fh_damping;  
748                                                 
749                                                 cl_object->setAngularVelocity(cl_object->getAngularVelocity() + (t_spring - t_damp));
750                                         }
751
752                                 }
753
754
755                         }
756
757
758                 }
759         }
760         
761 }
762
763 void            CcdPhysicsEnvironment::setDebugMode(int debugMode)
764 {
765         if (m_debugDrawer){
766                 m_debugDrawer->setDebugMode(debugMode);
767         }
768 }
769
770 void            CcdPhysicsEnvironment::setNumIterations(int numIter)
771 {
772         m_numIterations = numIter;
773 }
774 void            CcdPhysicsEnvironment::setDeactivationTime(float dTime)
775 {
776         gDeactivationTime = dTime;
777 }
778 void            CcdPhysicsEnvironment::setDeactivationLinearTreshold(float linTresh)
779 {
780         gLinearSleepingTreshold = linTresh;
781 }
782 void            CcdPhysicsEnvironment::setDeactivationAngularTreshold(float angTresh) 
783 {
784         gAngularSleepingTreshold = angTresh;
785 }
786
787 void            CcdPhysicsEnvironment::setContactBreakingTreshold(float contactBreakingTreshold)
788 {
789         gContactBreakingThreshold = contactBreakingTreshold;
790
791 }
792
793
794 void            CcdPhysicsEnvironment::setCcdMode(int ccdMode)
795 {
796         m_ccdMode = ccdMode;
797 }
798
799
800 void            CcdPhysicsEnvironment::setSolverSorConstant(float sor)
801 {
802         m_solverInfo.m_sor = sor;
803 }
804
805 void            CcdPhysicsEnvironment::setSolverTau(float tau)
806 {
807         m_solverInfo.m_tau = tau;
808 }
809 void            CcdPhysicsEnvironment::setSolverDamping(float damping)
810 {
811         m_solverInfo.m_damping = damping;
812 }
813
814
815 void            CcdPhysicsEnvironment::setLinearAirDamping(float damping)
816 {
817         //gLinearAirDamping = damping;
818 }
819
820 void            CcdPhysicsEnvironment::setUseEpa(bool epa)
821 {
822         //gUseEpa = epa;
823 }
824
825 void            CcdPhysicsEnvironment::setSolverType(int solverType)
826 {
827
828         switch (solverType)
829         {
830         case 1:
831                 {
832                         if (m_solverType != solverType)
833                         {
834
835                                 m_solver = new btSequentialImpulseConstraintSolver();
836 //                              ((btSequentialImpulseConstraintSolver*)m_solver)->setSolverMode(btSequentialImpulseConstraintSolver::SOLVER_USE_WARMSTARTING | btSequentialImpulseConstraintSolver::SOLVER_RANDMIZE_ORDER);
837                                 break;
838                         }
839                 }
840
841         case 0:
842         default:
843                 if (m_solverType != solverType)
844                 {
845 //                      m_solver = new OdeConstraintSolver();
846
847                         break;
848                 }
849
850         };
851
852         m_solverType = solverType ;
853 }
854
855
856
857 void            CcdPhysicsEnvironment::getGravity(PHY__Vector3& grav)
858 {
859                 const btVector3& gravity = m_dynamicsWorld->getGravity();
860                 grav[0] = gravity.getX();
861                 grav[1] = gravity.getY();
862                 grav[2] = gravity.getZ();
863 }
864
865
866 void            CcdPhysicsEnvironment::setGravity(float x,float y,float z)
867 {
868         m_gravity = btVector3(x,y,z);
869         m_dynamicsWorld->setGravity(m_gravity);
870
871 }
872
873
874
875
876 static int gConstraintUid = 1;
877
878 //Following the COLLADA physics specification for constraints
879 int                     CcdPhysicsEnvironment::createUniversalD6Constraint(
880                                                 class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
881                                                 btTransform& frameInA,
882                                                 btTransform& frameInB,
883                                                 const btVector3& linearMinLimits,
884                                                 const btVector3& linearMaxLimits,
885                                                 const btVector3& angularMinLimits,
886                                                 const btVector3& angularMaxLimits,int flags
887 )
888 {
889
890         bool disableCollisionBetweenLinkedBodies = (0==(flags & CCD_CONSTRAINT_DISABLE_LINKED_COLLISION));
891
892         //we could either add some logic to recognize ball-socket and hinge, or let that up to the user
893         //perhaps some warning or hint that hinge/ball-socket is more efficient?
894         
895         
896         btGeneric6DofConstraint* genericConstraint = 0;
897         CcdPhysicsController* ctrl0 = (CcdPhysicsController*) ctrlRef;
898         CcdPhysicsController* ctrl1 = (CcdPhysicsController*) ctrlOther;
899         
900         btRigidBody* rb0 = ctrl0->GetRigidBody();
901         btRigidBody* rb1 = ctrl1->GetRigidBody();
902
903         if (rb1)
904         {
905                 
906
907                 bool useReferenceFrameA = true;
908                 genericConstraint = new btGeneric6DofConstraint(
909                         *rb0,*rb1,
910                         frameInA,frameInB,useReferenceFrameA);
911                 genericConstraint->setLinearLowerLimit(linearMinLimits);
912                 genericConstraint->setLinearUpperLimit(linearMaxLimits);
913                 genericConstraint->setAngularLowerLimit(angularMinLimits);
914                 genericConstraint->setAngularUpperLimit(angularMaxLimits);
915         } else
916         {
917                 // TODO: Implement single body case...
918                 //No, we can use a fixed rigidbody in above code, rather then unnecessary duplation of code
919
920         }
921         
922         if (genericConstraint)
923         {
924         //      m_constraints.push_back(genericConstraint);
925                 m_dynamicsWorld->addConstraint(genericConstraint,disableCollisionBetweenLinkedBodies);
926
927                 genericConstraint->setUserConstraintId(gConstraintUid++);
928                 genericConstraint->setUserConstraintType(PHY_GENERIC_6DOF_CONSTRAINT);
929                 //64 bit systems can't cast pointer to int. could use size_t instead.
930                 return genericConstraint->getUserConstraintId();
931         }
932         return 0;
933 }
934
935
936
937 void            CcdPhysicsEnvironment::removeConstraint(int     constraintId)
938 {
939         
940         int i;
941         int numConstraints = m_dynamicsWorld->getNumConstraints();
942         for (i=0;i<numConstraints;i++)
943         {
944                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
945                 if (constraint->getUserConstraintId() == constraintId)
946                 {
947                         constraint->getRigidBodyA().activate();
948                         constraint->getRigidBodyB().activate();
949                         m_dynamicsWorld->removeConstraint(constraint);
950                         break;
951                 }
952         }
953 }
954
955
956 struct  FilterClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
957 {
958         PHY_IRayCastFilterCallback&     m_phyRayFilter;
959         const btCollisionShape*         m_hitTriangleShape;
960         int                                                     m_hitTriangleIndex;
961
962         FilterClosestRayResultCallback (PHY_IRayCastFilterCallback& phyRayFilter,const btVector3& rayFrom,const btVector3& rayTo)
963                 : btCollisionWorld::ClosestRayResultCallback(rayFrom,rayTo),
964                 m_phyRayFilter(phyRayFilter),
965                 m_hitTriangleShape(NULL),
966                 m_hitTriangleIndex(0)
967         {
968         }
969
970         virtual ~FilterClosestRayResultCallback()
971         {
972         }
973
974         virtual bool needsCollision(btBroadphaseProxy* proxy0) const
975         {
976                 if (!(proxy0->m_collisionFilterGroup & m_collisionFilterMask))
977                         return false;
978                 if (!(m_collisionFilterGroup & proxy0->m_collisionFilterMask))
979                         return false;
980                 btCollisionObject* object = (btCollisionObject*)proxy0->m_clientObject;
981                 CcdPhysicsController* phyCtrl = static_cast<CcdPhysicsController*>(object->getUserPointer());
982                 if (phyCtrl == m_phyRayFilter.m_ignoreController)
983                         return false;
984                 return m_phyRayFilter.needBroadphaseRayCast(phyCtrl);
985         }
986
987         virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace)
988         {
989                 CcdPhysicsController* curHit = static_cast<CcdPhysicsController*>(rayResult.m_collisionObject->getUserPointer());
990                 // save shape information as ClosestRayResultCallback::AddSingleResult() does not do it
991                 if (rayResult.m_localShapeInfo)
992                 {
993                         m_hitTriangleShape = rayResult.m_collisionObject->getCollisionShape();
994                         m_hitTriangleIndex = rayResult.m_localShapeInfo->m_triangleIndex;
995                 } else 
996                 {
997                         m_hitTriangleShape = NULL;
998                         m_hitTriangleIndex = 0;
999                 }
1000                 return ClosestRayResultCallback::addSingleResult(rayResult,normalInWorldSpace);
1001         }
1002
1003 };
1004
1005 PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IRayCastFilterCallback &filterCallback, float fromX,float fromY,float fromZ, float toX,float toY,float toZ)
1006 {
1007
1008
1009         float minFraction = 1.f;
1010
1011         btVector3 rayFrom(fromX,fromY,fromZ);
1012         btVector3 rayTo(toX,toY,toZ);
1013
1014         btVector3       hitPointWorld,normalWorld;
1015
1016         //Either Ray Cast with or without filtering
1017
1018         //btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo);
1019         FilterClosestRayResultCallback   rayCallback(filterCallback,rayFrom,rayTo);
1020
1021
1022         PHY_RayCastResult result;
1023         memset(&result, 0, sizeof(result));
1024
1025         // don't collision with sensor object
1026         rayCallback.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
1027         //, ,filterCallback.m_faceNormal);
1028
1029         m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback);
1030         if (rayCallback.hasHit())
1031         {
1032                 CcdPhysicsController* controller = static_cast<CcdPhysicsController*>(rayCallback.m_collisionObject->getUserPointer());
1033                 result.m_controller = controller;
1034                 result.m_hitPoint[0] = rayCallback.m_hitPointWorld.getX();
1035                 result.m_hitPoint[1] = rayCallback.m_hitPointWorld.getY();
1036                 result.m_hitPoint[2] = rayCallback.m_hitPointWorld.getZ();
1037
1038                 if (rayCallback.m_hitTriangleShape != NULL)
1039                 {
1040                         // identify the mesh polygon
1041                         CcdShapeConstructionInfo* shapeInfo = controller->m_shapeInfo;
1042                         if (shapeInfo)
1043                         {
1044                                 btCollisionShape* shape = controller->GetCollisionObject()->getCollisionShape();
1045                                 if (shape->isCompound())
1046                                 {
1047                                         btCompoundShape* compoundShape = (btCompoundShape*)shape;
1048                                         CcdShapeConstructionInfo* compoundShapeInfo = shapeInfo;
1049                                         // need to search which sub-shape has been hit
1050                                         for (int i=0; i<compoundShape->getNumChildShapes(); i++)
1051                                         {
1052                                                 shapeInfo = compoundShapeInfo->GetChildShape(i);
1053                                                 shape=compoundShape->getChildShape(i);
1054                                                 if (shape == rayCallback.m_hitTriangleShape)
1055                                                         break;
1056                                         }
1057                                 }
1058                                 if (shape == rayCallback.m_hitTriangleShape && 
1059                                         rayCallback.m_hitTriangleIndex < shapeInfo->m_polygonIndexArray.size())
1060                                 {
1061                                         result.m_meshObject = shapeInfo->GetMesh();
1062                                         result.m_polygon = shapeInfo->m_polygonIndexArray.at(rayCallback.m_hitTriangleIndex);
1063
1064                                         // Bullet returns the normal from "outside".
1065                                         // If the user requests the real normal, compute it now
1066                     if (filterCallback.m_faceNormal)
1067                                         {
1068                                                 // mesh shapes are shared and stored in the shapeInfo
1069                                                 btTriangleMeshShape* triangleShape = shapeInfo->GetMeshShape();
1070                                                 if (triangleShape)
1071                                                 {
1072                                                         // this code is copied from Bullet 
1073                                                         btVector3 triangle[3];
1074                                                         const unsigned char *vertexbase;
1075                                                         int numverts;
1076                                                         PHY_ScalarType type;
1077                                                         int stride;
1078                                                         const unsigned char *indexbase;
1079                                                         int indexstride;
1080                                                         int numfaces;
1081                                                         PHY_ScalarType indicestype;
1082                                                         btStridingMeshInterface* meshInterface = triangleShape->getMeshInterface();
1083
1084                                                         meshInterface->getLockedReadOnlyVertexIndexBase(
1085                                                                 &vertexbase,
1086                                                                 numverts,
1087                                                                 type,
1088                                                                 stride,
1089                                                                 &indexbase,
1090                                                                 indexstride,
1091                                                                 numfaces,
1092                                                                 indicestype,
1093                                                                 0);
1094
1095                                                         unsigned int* gfxbase = (unsigned int*)(indexbase+rayCallback.m_hitTriangleIndex*indexstride);
1096                                                         const btVector3& meshScaling = shape->getLocalScaling();
1097                                                         for (int j=2;j>=0;j--)
1098                                                         {
1099                                                                 int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
1100
1101                                                                 btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride);
1102
1103                                                                 triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());              
1104                                                         }
1105                                                         meshInterface->unLockReadOnlyVertexBase(0);
1106                                                         btVector3 triangleNormal; 
1107                                                         triangleNormal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]);
1108                                                         rayCallback.m_hitNormalWorld = rayCallback.m_collisionObject->getWorldTransform().getBasis()*triangleNormal;
1109                                                 }
1110                                         }
1111                                 }
1112                         }
1113                 }
1114                 if (rayCallback.m_hitNormalWorld.length2() > (SIMD_EPSILON*SIMD_EPSILON))
1115                 {
1116                         rayCallback.m_hitNormalWorld.normalize();
1117                 } else
1118                 {
1119                         rayCallback.m_hitNormalWorld.setValue(1,0,0);
1120                 }
1121                 result.m_hitNormal[0] = rayCallback.m_hitNormalWorld.getX();
1122                 result.m_hitNormal[1] = rayCallback.m_hitNormalWorld.getY();
1123                 result.m_hitNormal[2] = rayCallback.m_hitNormalWorld.getZ();
1124                 filterCallback.reportHit(&result);
1125         }       
1126
1127
1128         return result.m_controller;
1129 }
1130
1131
1132
1133 int     CcdPhysicsEnvironment::getNumContactPoints()
1134 {
1135         return 0;
1136 }
1137
1138 void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
1139 {
1140
1141 }
1142
1143
1144
1145
1146 btBroadphaseInterface*  CcdPhysicsEnvironment::getBroadphase()
1147
1148         return m_dynamicsWorld->getBroadphase(); 
1149 }
1150
1151 btDispatcher*   CcdPhysicsEnvironment::getDispatcher()
1152
1153         return m_dynamicsWorld->getDispatcher();
1154 }
1155
1156
1157
1158
1159
1160
1161 CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
1162 {
1163
1164 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1165         m_wrapperVehicles.clear();
1166 #endif //NEW_BULLET_VEHICLE_SUPPORT
1167
1168         //m_broadphase->DestroyScene();
1169         //delete broadphase ? release reference on broadphase ?
1170
1171         //first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher
1172         //delete m_dispatcher;
1173         delete m_dynamicsWorld;
1174         
1175
1176         if (NULL != m_ownPairCache)
1177                 delete m_ownPairCache;
1178
1179         if (NULL != m_ownDispatcher)
1180                 delete m_ownDispatcher;
1181
1182         if (NULL != m_solver)
1183                 delete m_solver;
1184
1185         if (NULL != m_debugDrawer)
1186                 delete m_debugDrawer;
1187
1188         if (NULL != m_filterCallback)
1189                 delete m_filterCallback;
1190
1191         if (NULL != m_collisionConfiguration)
1192                 delete m_collisionConfiguration;
1193
1194         if (NULL != m_broadphase)
1195                 delete m_broadphase;
1196 }
1197
1198
1199 void    CcdPhysicsEnvironment::setConstraintParam(int constraintId,int param,float value0,float value1)
1200 {
1201         btTypedConstraint* typedConstraint = getConstraintById(constraintId);
1202         switch (typedConstraint->getUserConstraintType())
1203         {
1204         case PHY_GENERIC_6DOF_CONSTRAINT:
1205                 {
1206                         //param = 1..12, min0,max0,min1,max1...min6,max6
1207                         btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
1208                         genCons->setLimit(param,value0,value1);
1209                         break;
1210                 };
1211         default:
1212                 {
1213                 };
1214         };
1215 }
1216
1217 btTypedConstraint*      CcdPhysicsEnvironment::getConstraintById(int constraintId)
1218 {
1219
1220         int numConstraints = m_dynamicsWorld->getNumConstraints();
1221         int i;
1222         for (i=0;i<numConstraints;i++)
1223         {
1224                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
1225                 if (constraint->getUserConstraintId()==constraintId)
1226                 {
1227                         return constraint;
1228                 }
1229         }
1230         return 0;
1231 }
1232
1233
1234 void CcdPhysicsEnvironment::addSensor(PHY_IPhysicsController* ctrl)
1235 {
1236
1237         CcdPhysicsController* ctrl1 = (CcdPhysicsController* )ctrl;
1238         // addSensor() is a "light" function for bullet because it is used
1239         // dynamically when the sensor is activated. Use enableCcdPhysicsController() instead 
1240         //if (m_controllers.insert(ctrl1).second)
1241         //{
1242         //      addCcdPhysicsController(ctrl1);
1243         //}
1244         enableCcdPhysicsController(ctrl1);
1245
1246         //Collision filter/mask is now set at the time of the creation of the controller 
1247         //force collision detection with everything, including static objects (might hurt performance!)
1248         //ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterMask = btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::SensorTrigger;
1249         //ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterGroup = btBroadphaseProxy::SensorTrigger;
1250         //todo: make this 'sensor'!
1251
1252         requestCollisionCallback(ctrl);
1253         //printf("addSensor\n");
1254 }
1255
1256 void CcdPhysicsEnvironment::removeCollisionCallback(PHY_IPhysicsController* ctrl)
1257 {
1258         CcdPhysicsController* ccdCtrl = (CcdPhysicsController*)ctrl;
1259         if (ccdCtrl->Unregister())
1260                 m_triggerControllers.erase(ccdCtrl);
1261 }
1262
1263
1264 void CcdPhysicsEnvironment::removeSensor(PHY_IPhysicsController* ctrl)
1265 {
1266         removeCollisionCallback(ctrl);
1267
1268         disableCcdPhysicsController((CcdPhysicsController*)ctrl);
1269 }
1270
1271 void CcdPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user)
1272 {
1273         /*      printf("addTouchCallback\n(response class = %i)\n",response_class);
1274
1275         //map PHY_ convention into SM_ convention
1276         switch (response_class)
1277         {
1278         case    PHY_FH_RESPONSE:
1279         printf("PHY_FH_RESPONSE\n");
1280         break;
1281         case PHY_SENSOR_RESPONSE:
1282         printf("PHY_SENSOR_RESPONSE\n");
1283         break;
1284         case PHY_CAMERA_RESPONSE:
1285         printf("PHY_CAMERA_RESPONSE\n");
1286         break;
1287         case PHY_OBJECT_RESPONSE:
1288         printf("PHY_OBJECT_RESPONSE\n");
1289         break;
1290         case PHY_STATIC_RESPONSE:
1291         printf("PHY_STATIC_RESPONSE\n");
1292         break;
1293         default:
1294         assert(0);
1295         return;
1296         }
1297         */
1298
1299         m_triggerCallbacks[response_class] = callback;
1300         m_triggerCallbacksUserPtrs[response_class] = user;
1301
1302 }
1303 void CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctrl)
1304 {
1305         CcdPhysicsController* ccdCtrl = static_cast<CcdPhysicsController*>(ctrl);
1306
1307         if (ccdCtrl->Register())
1308                 m_triggerControllers.insert(ccdCtrl);
1309 }
1310
1311 void    CcdPhysicsEnvironment::CallbackTriggers()
1312 {
1313         
1314         CcdPhysicsController* ctrl0=0,*ctrl1=0;
1315
1316         if (m_triggerCallbacks[PHY_OBJECT_RESPONSE] || (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)))
1317         {
1318                 //walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback
1319                 btDispatcher* dispatcher = m_dynamicsWorld->getDispatcher();
1320                 int numManifolds = dispatcher->getNumManifolds();
1321                 for (int i=0;i<numManifolds;i++)
1322                 {
1323                         btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
1324                         int numContacts = manifold->getNumContacts();
1325                         if (numContacts)
1326                         {
1327                                 btRigidBody* rb0 = static_cast<btRigidBody*>(manifold->getBody0());
1328                                 btRigidBody* rb1 = static_cast<btRigidBody*>(manifold->getBody1());
1329                                 if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints))
1330                                 {
1331                                         for (int j=0;j<numContacts;j++)
1332                                         {
1333                                                 btVector3 color(1,0,0);
1334                                                 const btManifoldPoint& cp = manifold->getContactPoint(j);
1335                                                 if (m_debugDrawer)
1336                                                         m_debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
1337                                         }
1338                                 }
1339                                 btRigidBody* obj0 = rb0;
1340                                 btRigidBody* obj1 = rb1;
1341
1342                                 //m_internalOwner is set in 'addPhysicsController'
1343                                 CcdPhysicsController* ctrl0 = static_cast<CcdPhysicsController*>(obj0->getUserPointer());
1344                                 CcdPhysicsController* ctrl1 = static_cast<CcdPhysicsController*>(obj1->getUserPointer());
1345
1346                                 std::set<CcdPhysicsController*>::const_iterator i = m_triggerControllers.find(ctrl0);
1347                                 if (i == m_triggerControllers.end())
1348                                 {
1349                                         i = m_triggerControllers.find(ctrl1);
1350                                 }
1351
1352                                 if (!(i == m_triggerControllers.end()))
1353                                 {
1354                                         m_triggerCallbacks[PHY_OBJECT_RESPONSE](m_triggerCallbacksUserPtrs[PHY_OBJECT_RESPONSE],
1355                                                 ctrl0,ctrl1,0);
1356                                 }
1357                                 // Bullet does not refresh the manifold contact point for object without contact response
1358                                 // may need to remove this when a newer Bullet version is integrated
1359                                 if (!dispatcher->needsResponse(rb0, rb1))
1360                                 {
1361                                         // Refresh algorithm fails sometimes when there is penetration 
1362                                         // (usuall the case with ghost and sensor objects)
1363                                         // Let's just clear the manifold, in any case, it is recomputed on each frame.
1364                                         manifold->clearManifold(); //refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
1365                                 }
1366                         }
1367                 }
1368
1369
1370
1371         }
1372
1373
1374 }
1375
1376 // This call back is called before a pair is added in the cache
1377 // Handy to remove objects that must be ignored by sensors
1378 bool CcdOverlapFilterCallBack::needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
1379 {
1380         btCollisionObject *colObj0, *colObj1;
1381         CcdPhysicsController *sensorCtrl, *objCtrl;
1382         bool collides;
1383         // first check the filters
1384         collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
1385         collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
1386         if (!collides)
1387                 return false;
1388
1389         // additional check for sensor object
1390         if (proxy0->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger)
1391         {
1392                 // this is a sensor object, the other one can't be a sensor object because 
1393                 // they exclude each other in the above test
1394                 assert(!(proxy1->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger));
1395                 colObj0 = (btCollisionObject*)proxy0->m_clientObject;
1396                 colObj1 = (btCollisionObject*)proxy1->m_clientObject;
1397         }
1398         else if (proxy1->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger)
1399         {
1400                 colObj0 = (btCollisionObject*)proxy1->m_clientObject;
1401                 colObj1 = (btCollisionObject*)proxy0->m_clientObject;
1402         }
1403         else
1404         {
1405                 return true;
1406         }
1407         if (!colObj0 || !colObj1)
1408                 return false;
1409         sensorCtrl = static_cast<CcdPhysicsController*>(colObj0->getUserPointer());
1410         objCtrl = static_cast<CcdPhysicsController*>(colObj1->getUserPointer());
1411         if (m_physEnv->m_triggerCallbacks[PHY_BROADPH_RESPONSE])
1412         {
1413                 return m_physEnv->m_triggerCallbacks[PHY_BROADPH_RESPONSE](m_physEnv->m_triggerCallbacksUserPtrs[PHY_BROADPH_RESPONSE], sensorCtrl, objCtrl, 0);
1414         }
1415         return true;
1416 }
1417
1418
1419 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1420
1421 //complex constraint for vehicles
1422 PHY_IVehicle*   CcdPhysicsEnvironment::getVehicleConstraint(int constraintId)
1423 {
1424         int i;
1425
1426         int numVehicles = m_wrapperVehicles.size();
1427         for (i=0;i<numVehicles;i++)
1428         {
1429                 WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
1430                 if (wrapperVehicle->GetVehicle()->getUserConstraintId() == constraintId)
1431                         return wrapperVehicle;
1432         }
1433
1434         return 0;
1435 }
1436
1437 #endif //NEW_BULLET_VEHICLE_SUPPORT
1438
1439
1440 int currentController = 0;
1441 int numController = 0;
1442
1443
1444
1445
1446 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateSphereController(float radius,const PHY__Vector3& position)
1447 {
1448         
1449         CcdConstructionInfo     cinfo;
1450         // memory leak! The shape is not deleted by Bullet and we cannot add it to the KX_Scene.m_shapes list
1451         cinfo.m_collisionShape = new btSphereShape(radius);
1452         cinfo.m_MotionState = 0;
1453         cinfo.m_physicsEnv = this;
1454         // declare this object as Dyamic rather then static!!
1455         // The reason as it is designed to detect all type of object, including static object
1456         // It would cause static-static message to be printed on the console otherwise
1457         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE/* | btCollisionObject::CF_KINEMATIC_OBJECT*/;
1458         DefaultMotionState* motionState = new DefaultMotionState();
1459         cinfo.m_MotionState = motionState;
1460         // we will add later the possibility to select the filter from option
1461         cinfo.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
1462         cinfo.m_collisionFilterGroup = CcdConstructionInfo::SensorFilter;
1463         motionState->m_worldTransform.setIdentity();
1464         motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
1465
1466         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
1467         
1468         return sphereController;
1469 }
1470
1471 int findClosestNode(btSoftBody* sb,const btVector3& worldPoint);
1472 int findClosestNode(btSoftBody* sb,const btVector3& worldPoint)
1473 {
1474         int node = -1;
1475
1476         btSoftBody::tNodeArray&   nodes(sb->m_nodes);
1477         float maxDistSqr = 1e30f;
1478
1479         for (int n=0;n<nodes.size();n++)
1480         {
1481                 btScalar distSqr = (nodes[n].m_x - worldPoint).length2();
1482                 if (distSqr<maxDistSqr)
1483                 {
1484                         maxDistSqr = distSqr;
1485                         node = n;
1486                 }
1487         }
1488         return node;
1489 }
1490
1491 int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
1492                                                                                                         float pivotX,float pivotY,float pivotZ,
1493                                                                                                         float axisX,float axisY,float axisZ,
1494                                                                                                         float axis1X,float axis1Y,float axis1Z,
1495                                                                                                         float axis2X,float axis2Y,float axis2Z,int flags
1496                                                                                                         )
1497 {
1498
1499         bool disableCollisionBetweenLinkedBodies = (0==(flags & CCD_CONSTRAINT_DISABLE_LINKED_COLLISION));
1500
1501
1502
1503         CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
1504         CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
1505
1506         btRigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
1507         btRigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
1508
1509         
1510
1511
1512         bool rb0static = rb0 ? rb0->isStaticOrKinematicObject() : true;
1513         bool rb1static = rb1 ? rb1->isStaticOrKinematicObject() : true;
1514
1515         btCollisionObject* colObj0 = c0->GetCollisionObject();
1516         if (!colObj0)
1517         {
1518                 return 0;
1519         }
1520
1521         btVector3 pivotInA(pivotX,pivotY,pivotZ);
1522
1523         
1524
1525         //it might be a soft body, let's try
1526         btSoftBody* sb0 = c0 ? c0->GetSoftBody() : 0;
1527         btSoftBody* sb1 = c1 ? c1->GetSoftBody() : 0;
1528         if (sb0 && sb1)
1529         {
1530                 //not between two soft bodies?
1531                 return 0;
1532         }
1533
1534         if (sb0)
1535         {
1536                 //either cluster or node attach, let's find closest node first
1537                 //the soft body doesn't have a 'real' world transform, so get its initial world transform for now
1538                 btVector3 pivotPointSoftWorld = sb0->m_initialWorldTransform(pivotInA);
1539                 int node=findClosestNode(sb0,pivotPointSoftWorld);
1540                 if (node >=0)
1541                 {
1542                         bool clusterconstaint = false;
1543 /*
1544                         switch (type)
1545                         {
1546                         case PHY_LINEHINGE_CONSTRAINT:
1547                                 {
1548                                         if (sb0->clusterCount() && rb1)
1549                                         {
1550                                                 btSoftBody::LJoint::Specs       ls;
1551                                                 ls.erp=0.5f;
1552                                                 ls.position=sb0->clusterCom(0);
1553                                                 sb0->appendLinearJoint(ls,rb1);
1554                                                 clusterconstaint = true;
1555                                                 break;
1556                                         }
1557                                 }
1558                         case PHY_GENERIC_6DOF_CONSTRAINT:
1559                                 {
1560                                         if (sb0->clusterCount() && rb1)
1561                                         {
1562                                                 btSoftBody::AJoint::Specs as;
1563                                                 as.erp = 1;
1564                                                 as.cfm = 1;
1565                                                 as.axis.setValue(axisX,axisY,axisZ);
1566                                                 sb0->appendAngularJoint(as,rb1);
1567                                                 clusterconstaint = true;
1568                                                 break;
1569                                         }
1570
1571                                         break;
1572                                 }
1573                         default:
1574                                 {
1575                                 
1576                                 }
1577                         };
1578                         */
1579
1580                         if (!clusterconstaint)
1581                         {
1582                                 if (rb1)
1583                                 {
1584                                         sb0->appendAnchor(node,rb1);
1585                                 } else
1586                                 {
1587                                         sb0->setMass(node,0.f);
1588                                 }
1589                         }
1590
1591                         
1592                 }
1593                 return 0;//can't remove soft body anchors yet
1594         }
1595
1596         if (sb1)
1597         {
1598                 btVector3 pivotPointAWorld = colObj0->getWorldTransform()(pivotInA);
1599                 int node=findClosestNode(sb1,pivotPointAWorld);
1600                 if (node >=0)
1601                 {
1602                         bool clusterconstaint = false;
1603
1604                         /*
1605                         switch (type)
1606                         {
1607                         case PHY_LINEHINGE_CONSTRAINT:
1608                                 {
1609                                         if (sb1->clusterCount() && rb0)
1610                                         {
1611                                                 btSoftBody::LJoint::Specs       ls;
1612                                                 ls.erp=0.5f;
1613                                                 ls.position=sb1->clusterCom(0);
1614                                                 sb1->appendLinearJoint(ls,rb0);
1615                                                 clusterconstaint = true;
1616                                                 break;
1617                                         }
1618                                 }
1619                         case PHY_GENERIC_6DOF_CONSTRAINT:
1620                                 {
1621                                         if (sb1->clusterCount() && rb0)
1622                                         {
1623                                                 btSoftBody::AJoint::Specs as;
1624                                                 as.erp = 1;
1625                                                 as.cfm = 1;
1626                                                 as.axis.setValue(axisX,axisY,axisZ);
1627                                                 sb1->appendAngularJoint(as,rb0);
1628                                                 clusterconstaint = true;
1629                                                 break;
1630                                         }
1631
1632                                         break;
1633                                 }
1634                         default:
1635                                 {
1636                                         
1637
1638                                 }
1639                         };*/
1640
1641
1642                         if (!clusterconstaint)
1643                         {
1644                                 if (rb0)
1645                                 {
1646                                         sb1->appendAnchor(node,rb0);
1647                                 } else
1648                                 {
1649                                         sb1->setMass(node,0.f);
1650                                 }
1651                         }
1652                         
1653
1654                 }
1655                 return 0;//can't remove soft body anchors yet
1656         }
1657
1658         if (rb0static && rb1static)
1659         {
1660                 
1661                 return 0;
1662         }
1663         
1664
1665         if (!rb0)
1666                 return 0;
1667
1668         
1669         btVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : 
1670                 rb0->getCenterOfMassTransform() * pivotInA;
1671         btVector3 axisInA(axisX,axisY,axisZ);
1672
1673
1674         bool angularOnly = false;
1675
1676         switch (type)
1677         {
1678         case PHY_POINT2POINT_CONSTRAINT:
1679                 {
1680
1681                         btPoint2PointConstraint* p2p = 0;
1682
1683                         if (rb1)
1684                         {
1685                                 p2p = new btPoint2PointConstraint(*rb0,
1686                                         *rb1,pivotInA,pivotInB);
1687                         } else
1688                         {
1689                                 p2p = new btPoint2PointConstraint(*rb0,
1690                                         pivotInA);
1691                         }
1692
1693                         m_dynamicsWorld->addConstraint(p2p,disableCollisionBetweenLinkedBodies);
1694 //                      m_constraints.push_back(p2p);
1695
1696                         p2p->setUserConstraintId(gConstraintUid++);
1697                         p2p->setUserConstraintType(type);
1698                         //64 bit systems can't cast pointer to int. could use size_t instead.
1699                         return p2p->getUserConstraintId();
1700
1701                         break;
1702                 }
1703
1704         case PHY_GENERIC_6DOF_CONSTRAINT:
1705                 {
1706                         btGeneric6DofConstraint* genericConstraint = 0;
1707
1708                         if (rb1)
1709                         {
1710                                 btTransform frameInA;
1711                                 btTransform frameInB;
1712                                 
1713                                 btVector3 axis1(axis1X,axis1Y,axis1Z), axis2(axis2X,axis2Y,axis2Z);
1714                                 if (axis1.length() == 0.0)
1715                                 {
1716                                         btPlaneSpace1( axisInA, axis1, axis2 );
1717                                 }
1718                                 
1719                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1720                                                                   axisInA.y(), axis1.y(), axis2.y(),
1721                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1722                                 frameInA.setOrigin( pivotInA );
1723
1724                                 btTransform inv = rb1->getCenterOfMassTransform().inverse();
1725
1726                                 btTransform globalFrameA = rb0->getCenterOfMassTransform() * frameInA;
1727                                 
1728                                 frameInB = inv  * globalFrameA;
1729                                 bool useReferenceFrameA = true;
1730
1731                                 genericConstraint = new btGeneric6DofConstraint(
1732                                         *rb0,*rb1,
1733                                         frameInA,frameInB,useReferenceFrameA);
1734
1735
1736                         } else
1737                         {
1738                                 static btRigidBody s_fixedObject2( 0,0,0);
1739                                 btTransform frameInA;
1740                                 btTransform frameInB;
1741                                 
1742                                 btVector3 axis1, axis2;
1743                                 btPlaneSpace1( axisInA, axis1, axis2 );
1744
1745                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1746                                                                   axisInA.y(), axis1.y(), axis2.y(),
1747                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1748
1749                                 frameInA.setOrigin( pivotInA );
1750
1751                                 ///frameInB in worldspace
1752                                 frameInB = rb0->getCenterOfMassTransform() * frameInA;
1753
1754                                 bool useReferenceFrameA = true;
1755                                 genericConstraint = new btGeneric6DofConstraint(
1756                                         *rb0,s_fixedObject2,
1757                                         frameInA,frameInB,useReferenceFrameA);
1758                         }
1759                         
1760                         if (genericConstraint)
1761                         {
1762                                 //m_constraints.push_back(genericConstraint);
1763                                 m_dynamicsWorld->addConstraint(genericConstraint,disableCollisionBetweenLinkedBodies);
1764                                 genericConstraint->setUserConstraintId(gConstraintUid++);
1765                                 genericConstraint->setUserConstraintType(type);
1766                                 //64 bit systems can't cast pointer to int. could use size_t instead.
1767                                 return genericConstraint->getUserConstraintId();
1768                         } 
1769
1770                         break;
1771                 }
1772         case PHY_CONE_TWIST_CONSTRAINT:
1773                 {
1774                         btConeTwistConstraint* coneTwistContraint = 0;
1775
1776                         
1777                         if (rb1)
1778                         {
1779                                 btTransform frameInA;
1780                                 btTransform frameInB;
1781                                 
1782                                 btVector3 axis1(axis1X,axis1Y,axis1Z), axis2(axis2X,axis2Y,axis2Z);
1783                                 if (axis1.length() == 0.0)
1784                                 {
1785                                         btPlaneSpace1( axisInA, axis1, axis2 );
1786                                 }
1787                                 
1788                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1789                                                                   axisInA.y(), axis1.y(), axis2.y(),
1790                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1791                                 frameInA.setOrigin( pivotInA );
1792
1793                                 btTransform inv = rb1->getCenterOfMassTransform().inverse();
1794
1795                                 btTransform globalFrameA = rb0->getCenterOfMassTransform() * frameInA;
1796                                 
1797                                 frameInB = inv  * globalFrameA;
1798                                 
1799                                 coneTwistContraint = new btConeTwistConstraint( *rb0,*rb1,
1800                                         frameInA,frameInB);
1801
1802
1803                         } else
1804                         {
1805                                 static btRigidBody s_fixedObject2( 0,0,0);
1806                                 btTransform frameInA;
1807                                 btTransform frameInB;
1808                                 
1809                                 btVector3 axis1, axis2;
1810                                 btPlaneSpace1( axisInA, axis1, axis2 );
1811
1812                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1813                                                                   axisInA.y(), axis1.y(), axis2.y(),
1814                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1815
1816                                 frameInA.setOrigin( pivotInA );
1817
1818                                 ///frameInB in worldspace
1819                                 frameInB = rb0->getCenterOfMassTransform() * frameInA;
1820
1821                                 coneTwistContraint = new btConeTwistConstraint(
1822                                         *rb0,s_fixedObject2,
1823                                         frameInA,frameInB);
1824                         }
1825                         
1826                         if (coneTwistContraint)
1827                         {
1828                                 //m_constraints.push_back(genericConstraint);
1829                                 m_dynamicsWorld->addConstraint(coneTwistContraint,disableCollisionBetweenLinkedBodies);
1830                                 coneTwistContraint->setUserConstraintId(gConstraintUid++);
1831                                 coneTwistContraint->setUserConstraintType(type);
1832                                 //64 bit systems can't cast pointer to int. could use size_t instead.
1833                                 return coneTwistContraint->getUserConstraintId();
1834                         } 
1835
1836
1837
1838                         break;
1839                 }
1840         case PHY_ANGULAR_CONSTRAINT:
1841                 angularOnly = true;
1842
1843
1844         case PHY_LINEHINGE_CONSTRAINT:
1845                 {
1846                         btHingeConstraint* hinge = 0;
1847
1848                         if (rb1)
1849                         {
1850                                 btVector3 axisInB = rb1 ? 
1851                                 (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) : 
1852                                 rb0->getCenterOfMassTransform().getBasis() * axisInA;
1853
1854                                 hinge = new btHingeConstraint(
1855                                         *rb0,
1856                                         *rb1,pivotInA,pivotInB,axisInA,axisInB);
1857
1858
1859                         } else
1860                         {
1861                                 hinge = new btHingeConstraint(*rb0,
1862                                         pivotInA,axisInA);
1863
1864                         }
1865                         hinge->setAngularOnly(angularOnly);
1866
1867                         //m_constraints.push_back(hinge);
1868                         m_dynamicsWorld->addConstraint(hinge,disableCollisionBetweenLinkedBodies);
1869                         hinge->setUserConstraintId(gConstraintUid++);
1870                         hinge->setUserConstraintType(type);
1871                         //64 bit systems can't cast pointer to int. could use size_t instead.
1872                         return hinge->getUserConstraintId();
1873                         break;
1874                 }
1875 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1876
1877         case PHY_VEHICLE_CONSTRAINT:
1878                 {
1879                         btRaycastVehicle::btVehicleTuning* tuning = new btRaycastVehicle::btVehicleTuning();
1880                         btRigidBody* chassis = rb0;
1881                         btDefaultVehicleRaycaster* raycaster = new btDefaultVehicleRaycaster(m_dynamicsWorld);
1882                         btRaycastVehicle* vehicle = new btRaycastVehicle(*tuning,chassis,raycaster);
1883                         WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
1884                         m_wrapperVehicles.push_back(wrapperVehicle);
1885                         m_dynamicsWorld->addVehicle(vehicle);
1886                         vehicle->setUserConstraintId(gConstraintUid++);
1887                         vehicle->setUserConstraintType(type);
1888                         return vehicle->getUserConstraintId();
1889
1890                         break;
1891                 };
1892 #endif //NEW_BULLET_VEHICLE_SUPPORT
1893
1894         default:
1895                 {
1896                 }
1897         };
1898
1899         //btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB
1900
1901         return 0;
1902
1903 }
1904
1905
1906
1907 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float coneradius,float coneheight)
1908 {
1909         CcdConstructionInfo     cinfo;
1910
1911         // we don't need a CcdShapeConstructionInfo for this shape:
1912         // it is simple enough for the standard copy constructor (see CcdPhysicsController::GetReplica)
1913         cinfo.m_collisionShape = new btConeShape(coneradius,coneheight);
1914         cinfo.m_MotionState = 0;
1915         cinfo.m_physicsEnv = this;
1916         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE;
1917         DefaultMotionState* motionState = new DefaultMotionState();
1918         cinfo.m_MotionState = motionState;
1919         
1920         // we will add later the possibility to select the filter from option
1921         cinfo.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
1922         cinfo.m_collisionFilterGroup = CcdConstructionInfo::SensorFilter;
1923         motionState->m_worldTransform.setIdentity();
1924 //      motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
1925
1926         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
1927
1928
1929         return sphereController;
1930 }
1931         
1932 float           CcdPhysicsEnvironment::getAppliedImpulse(int    constraintid)
1933 {
1934         int i;
1935         int numConstraints = m_dynamicsWorld->getNumConstraints();
1936         for (i=0;i<numConstraints;i++)
1937         {
1938                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
1939                 if (constraint->getUserConstraintId() == constraintid)
1940                 {
1941                         return constraint->getAppliedImpulse();
1942                 }
1943         }
1944
1945         return 0.f;
1946 }