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