svn merge -r 16411:16453 https://svn.blender.org/svnroot/bf-blender/trunk/blender
[blender.git] / source / gameengine / Physics / Bullet / CcdPhysicsEnvironment.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose, 
8 including commercial applications, and to alter it and redistribute it freely, 
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16
17
18
19 #include "CcdPhysicsEnvironment.h"
20 #include "CcdPhysicsController.h"
21
22 #include <algorithm>
23 #include "btBulletDynamicsCommon.h"
24 #include "LinearMath/btIDebugDraw.h"
25 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
26
27 //profiling/timings
28 #include "LinearMath/btQuickprof.h"
29
30
31 #include "PHY_IMotionState.h"
32
33
34 bool useIslands = true;
35
36 #ifdef NEW_BULLET_VEHICLE_SUPPORT
37 #include "BulletDynamics/Vehicle/btRaycastVehicle.h"
38 #include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
39 #include "BulletDynamics/Vehicle/btWheelInfo.h"
40 #include "PHY_IVehicle.h"
41 btRaycastVehicle::btVehicleTuning       gTuning;
42
43 #endif //NEW_BULLET_VEHICLE_SUPPORT
44 #include "LinearMath/btAabbUtil2.h"
45
46
47 #ifdef WIN32
48 void DrawRasterizerLine(const float* from,const float* to,int color);
49 #endif
50
51
52 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
53
54
55 #include <stdio.h>
56 #include <string.h>             // for memset
57
58 #ifdef NEW_BULLET_VEHICLE_SUPPORT
59 class WrapperVehicle : public PHY_IVehicle
60 {
61
62         btRaycastVehicle*       m_vehicle;
63         PHY_IPhysicsController* m_chassis;
64
65 public:
66
67         WrapperVehicle(btRaycastVehicle* vehicle,PHY_IPhysicsController* chassis)
68                 :m_vehicle(vehicle),
69                 m_chassis(chassis)
70         {
71         }
72
73         btRaycastVehicle*       GetVehicle()
74         {
75                 return m_vehicle;
76         }
77
78         PHY_IPhysicsController* GetChassis()
79         {
80                 return m_chassis;
81         }
82
83         virtual void    AddWheel(
84                 PHY_IMotionState*       motionState,
85                 PHY__Vector3    connectionPoint,
86                 PHY__Vector3    downDirection,
87                 PHY__Vector3    axleDirection,
88                 float   suspensionRestLength,
89                 float wheelRadius,
90                 bool hasSteering
91                 )
92         {
93                 btVector3 connectionPointCS0(connectionPoint[0],connectionPoint[1],connectionPoint[2]);
94                 btVector3 wheelDirectionCS0(downDirection[0],downDirection[1],downDirection[2]);
95                 btVector3 wheelAxle(axleDirection[0],axleDirection[1],axleDirection[2]);
96
97
98                 btWheelInfo& info = m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxle,
99                         suspensionRestLength,wheelRadius,gTuning,hasSteering);
100                 info.m_clientInfo = motionState;
101
102         }
103
104         void    SyncWheels()
105         {
106                 int numWheels = GetNumWheels();
107                 int i;
108                 for (i=0;i<numWheels;i++)
109                 {
110                         btWheelInfo& info = m_vehicle->getWheelInfo(i);
111                         PHY_IMotionState* motionState = (PHY_IMotionState*)info.m_clientInfo ;
112         //              m_vehicle->updateWheelTransformsWS(info,false);
113                         m_vehicle->updateWheelTransform(i,false);
114                         btTransform trans = m_vehicle->getWheelInfo(i).m_worldTransform;
115                         btQuaternion orn = trans.getRotation();
116                         const btVector3& pos = trans.getOrigin();
117                         motionState->setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]);
118                         motionState->setWorldPosition(pos.x(),pos.y(),pos.z());
119
120                 }
121         }
122
123         virtual int             GetNumWheels() const
124         {
125                 return m_vehicle->getNumWheels();
126         }
127
128         virtual void    GetWheelPosition(int wheelIndex,float& posX,float& posY,float& posZ) const
129         {
130                 btTransform     trans = m_vehicle->getWheelTransformWS(wheelIndex);
131                 posX = trans.getOrigin().x();
132                 posY = trans.getOrigin().y();
133                 posZ = trans.getOrigin().z();
134         }
135         virtual void    GetWheelOrientationQuaternion(int wheelIndex,float& quatX,float& quatY,float& quatZ,float& quatW) const
136         {
137                 btTransform     trans = m_vehicle->getWheelTransformWS(wheelIndex);
138                 btQuaternion quat = trans.getRotation();
139                 btMatrix3x3 orn2(quat);
140
141                 quatX = trans.getRotation().x();
142                 quatY = trans.getRotation().y();
143                 quatZ = trans.getRotation().z();
144                 quatW = trans.getRotation()[3];
145
146
147                 //printf("test");
148
149
150         }
151
152         virtual float   GetWheelRotation(int wheelIndex) const
153         {
154                 float rotation = 0.f;
155
156                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
157                 {
158                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
159                         rotation = info.m_rotation;
160                 }
161                 return rotation;
162
163         }
164
165
166
167         virtual int     GetUserConstraintId() const
168         {
169                 return m_vehicle->getUserConstraintId();
170         }
171
172         virtual int     GetUserConstraintType() const
173         {
174                 return m_vehicle->getUserConstraintType();
175         }
176
177         virtual void    SetSteeringValue(float steering,int wheelIndex)
178         {
179                 m_vehicle->setSteeringValue(steering,wheelIndex);
180         }
181
182         virtual void    ApplyEngineForce(float force,int wheelIndex)
183         {
184                 m_vehicle->applyEngineForce(force,wheelIndex);
185         }
186
187         virtual void    ApplyBraking(float braking,int wheelIndex)
188         {
189                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
190                 {
191                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
192                         info.m_brake = braking;
193                 }
194         }
195
196         virtual void    SetWheelFriction(float friction,int wheelIndex)
197         {
198                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
199                 {
200                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
201                         info.m_frictionSlip = friction;
202                 }
203
204         }
205
206         virtual void    SetSuspensionStiffness(float suspensionStiffness,int wheelIndex)
207         {
208                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
209                 {
210                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
211                         info.m_suspensionStiffness = suspensionStiffness;
212
213                 }
214         }
215
216         virtual void    SetSuspensionDamping(float suspensionDamping,int wheelIndex)
217         {
218                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
219                 {
220                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
221                         info.m_wheelsDampingRelaxation = suspensionDamping;
222                 }
223         }
224
225         virtual void    SetSuspensionCompression(float suspensionCompression,int wheelIndex)
226         {
227                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
228                 {
229                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
230                         info.m_wheelsDampingCompression = suspensionCompression;
231                 }
232         }
233
234
235
236         virtual void    SetRollInfluence(float rollInfluence,int wheelIndex)
237         {
238                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
239                 {
240                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
241                         info.m_rollInfluence = rollInfluence;
242                 }
243         }
244
245         virtual void    SetCoordinateSystem(int rightIndex,int upIndex,int forwardIndex)
246         {
247                 m_vehicle->setCoordinateSystem(rightIndex,upIndex,forwardIndex);
248         }
249
250
251
252 };
253 #endif //NEW_BULLET_VEHICLE_SUPPORT
254
255 class CcdOverlapFilterCallBack : public btOverlapFilterCallback
256 {
257 private:
258         class CcdPhysicsEnvironment* m_physEnv;
259 public:
260         CcdOverlapFilterCallBack(CcdPhysicsEnvironment* env) : 
261                 m_physEnv(env)
262         {
263         }
264         virtual ~CcdOverlapFilterCallBack()
265         {
266         }
267         // return true when pairs need collision
268         virtual bool    needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const;
269 };
270
271
272 void CcdPhysicsEnvironment::setDebugDrawer(btIDebugDraw* debugDrawer)
273 {
274         if (debugDrawer && m_dynamicsWorld)
275                 m_dynamicsWorld->setDebugDrawer(debugDrawer);
276         m_debugDrawer = debugDrawer;
277 }
278
279 static void DrawAabb(btIDebugDraw* debugDrawer,const btVector3& from,const btVector3& to,const btVector3& color)
280 {
281         btVector3 halfExtents = (to-from)* 0.5f;
282         btVector3 center = (to+from) *0.5f;
283         int i,j;
284
285         btVector3 edgecoord(1.f,1.f,1.f),pa,pb;
286         for (i=0;i<4;i++)
287         {
288                 for (j=0;j<3;j++)
289                 {
290                         pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],                
291                                 edgecoord[2]*halfExtents[2]);
292                         pa+=center;
293
294                         int othercoord = j%3;
295                         edgecoord[othercoord]*=-1.f;
296                         pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],        
297                                 edgecoord[2]*halfExtents[2]);
298                         pb+=center;
299
300                         debugDrawer->drawLine(pa,pb,color);
301                 }
302                 edgecoord = btVector3(-1.f,-1.f,-1.f);
303                 if (i<3)
304                         edgecoord[i]*=-1.f;
305         }
306
307
308 }
309
310
311
312
313
314
315 CcdPhysicsEnvironment::CcdPhysicsEnvironment(btDispatcher* dispatcher,btOverlappingPairCache* pairCache)
316 :m_scalingPropagated(false),
317 m_numIterations(10),
318 m_numTimeSubSteps(1),
319 m_ccdMode(0),
320 m_solverType(-1),
321 m_profileTimings(0),
322 m_enableSatCollisionDetection(false),
323 m_solver(NULL),
324 m_ownPairCache(NULL),
325 m_ownDispatcher(NULL),
326 m_filterCallback(NULL)
327 {
328
329         for (int i=0;i<PHY_NUM_RESPONSE;i++)
330         {
331                 m_triggerCallbacks[i] = 0;
332         }
333         m_collisionConfiguration = new btDefaultCollisionConfiguration();
334
335         if (!dispatcher)
336         {
337                 dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
338                 m_ownDispatcher = dispatcher;
339         }
340
341         m_broadphase = new btDbvtBroadphase();
342
343         m_filterCallback = new CcdOverlapFilterCallBack(this);
344         m_broadphase->getOverlappingPairCache()->setOverlapFilterCallback(m_filterCallback);
345
346         setSolverType(1);//issues with quickstep and memory allocations
347         m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
348         m_debugDrawer = 0;
349         m_gravity = btVector3(0.f,-10.f,0.f);
350         m_dynamicsWorld->setGravity(m_gravity);
351
352
353 }
354
355 void    CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
356 {
357         btRigidBody* body = ctrl->GetRigidBody();
358
359         //this m_userPointer is just used for triggers, see CallbackTriggers
360         body->setUserPointer(ctrl);
361
362         body->setGravity( m_gravity );
363         m_controllers.insert(ctrl);
364
365         //use explicit group/filter for finer control over collision in bullet => near/radar sensor
366         m_dynamicsWorld->addRigidBody(body, ctrl->GetCollisionFilterGroup(), ctrl->GetCollisionFilterMask());
367         if (body->isStaticOrKinematicObject())
368         {
369                 body->setActivationState(ISLAND_SLEEPING);
370         }
371
372
373         //CollisionObject(body,ctrl->GetCollisionFilterGroup(),ctrl->GetCollisionFilterMask());
374
375         assert(body->getBroadphaseHandle());
376
377         btBroadphaseInterface* scene =  getBroadphase();
378
379
380         btCollisionShape* shapeinterface = ctrl->GetCollisionShape();
381
382         assert(shapeinterface);
383
384         const btTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform();
385         
386
387         btPoint3 minAabb,maxAabb;
388
389         shapeinterface->getAabb(t,minAabb,maxAabb);
390
391         float timeStep = 0.02f;
392
393
394         //extent it with the motion
395
396         btVector3 linMotion = body->getLinearVelocity()*timeStep;
397
398         float maxAabbx = maxAabb.getX();
399         float maxAabby = maxAabb.getY();
400         float maxAabbz = maxAabb.getZ();
401         float minAabbx = minAabb.getX();
402         float minAabby = minAabb.getY();
403         float minAabbz = minAabb.getZ();
404
405         if (linMotion.x() > 0.f)
406                 maxAabbx += linMotion.x(); 
407         else
408                 minAabbx += linMotion.x();
409         if (linMotion.y() > 0.f)
410                 maxAabby += linMotion.y(); 
411         else
412                 minAabby += linMotion.y();
413         if (linMotion.z() > 0.f)
414                 maxAabbz += linMotion.z(); 
415         else
416                 minAabbz += linMotion.z();
417
418
419         minAabb = btVector3(minAabbx,minAabby,minAabbz);
420         maxAabb = btVector3(maxAabbx,maxAabby,maxAabbz);
421
422
423
424
425 }
426
427 void    CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctrl)
428 {
429         //also remove constraint
430
431         m_dynamicsWorld->removeRigidBody(ctrl->GetRigidBody());
432         m_controllers.erase(ctrl);
433
434         if (ctrl->m_registerCount != 0)
435                 printf("Warning: removing controller with non-zero m_registerCount: %d\n", ctrl->m_registerCount);
436
437         //remove it from the triggers
438         m_triggerControllers.erase(ctrl);
439 }
440
441 void    CcdPhysicsEnvironment::updateCcdPhysicsController(CcdPhysicsController* ctrl, btScalar newMass, int newCollisionFlags, short int newCollisionGroup, short int newCollisionMask)
442 {
443         // this function is used when the collisionning group of a controller is changed
444         // remove and add the collistioning object
445         btRigidBody* body = ctrl->GetRigidBody();
446         btVector3 inertia(0.0,0.0,0.0);
447
448         m_dynamicsWorld->removeCollisionObject(body);
449         body->setCollisionFlags(newCollisionFlags);
450         if (newMass)
451                 body->getCollisionShape()->calculateLocalInertia(newMass, inertia);
452         body->setMassProps(newMass, inertia);
453         m_dynamicsWorld->addCollisionObject(body, newCollisionGroup, newCollisionMask);
454         // to avoid nasty interaction, we must update the property of the controller as well
455         ctrl->m_cci.m_mass = newMass;
456         ctrl->m_cci.m_collisionFilterGroup = newCollisionGroup;
457         ctrl->m_cci.m_collisionFilterMask = newCollisionMask;
458         ctrl->m_cci.m_collisionFlags = newCollisionFlags;
459 }
460
461 void CcdPhysicsEnvironment::enableCcdPhysicsController(CcdPhysicsController* ctrl)
462 {
463         if (m_controllers.insert(ctrl).second)
464         {
465                 btRigidBody* body = ctrl->GetRigidBody();
466                 body->setUserPointer(ctrl);
467                 m_dynamicsWorld->addCollisionObject(body, 
468                         ctrl->GetCollisionFilterGroup(), ctrl->GetCollisionFilterMask());
469         }
470 }
471
472 void CcdPhysicsEnvironment::disableCcdPhysicsController(CcdPhysicsController* ctrl)
473 {
474         if (m_controllers.erase(ctrl))
475         {
476                 m_dynamicsWorld->removeRigidBody(ctrl->GetRigidBody());
477         }
478 }
479
480
481 void    CcdPhysicsEnvironment::beginFrame()
482 {
483
484 }
485
486
487 bool    CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
488 {
489         std::set<CcdPhysicsController*>::iterator it;
490         int i;
491
492         for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
493         {
494                 (*it)->SynchronizeMotionStates(timeStep);
495         }
496
497         float subStep = timeStep / float(m_numTimeSubSteps);
498         for (i=0;i<m_numTimeSubSteps;i++)
499         {
500                 m_dynamicsWorld->stepSimulation(subStep,0);//perform always a full simulation step
501         }
502
503         for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
504         {
505                 (*it)->SynchronizeMotionStates(timeStep);
506         }
507
508         for (i=0;i<m_wrapperVehicles.size();i++)
509         {
510                 WrapperVehicle* veh = m_wrapperVehicles[i];
511                 veh->SyncWheels();
512         }
513
514         m_dynamicsWorld->debugDrawWorld();
515
516
517         CallbackTriggers();
518
519         return true;
520 }
521
522
523 void            CcdPhysicsEnvironment::setDebugMode(int debugMode)
524 {
525         if (m_debugDrawer){
526                 m_debugDrawer->setDebugMode(debugMode);
527         }
528 }
529
530 void            CcdPhysicsEnvironment::setNumIterations(int numIter)
531 {
532         m_numIterations = numIter;
533 }
534 void            CcdPhysicsEnvironment::setDeactivationTime(float dTime)
535 {
536         gDeactivationTime = dTime;
537 }
538 void            CcdPhysicsEnvironment::setDeactivationLinearTreshold(float linTresh)
539 {
540         gLinearSleepingTreshold = linTresh;
541 }
542 void            CcdPhysicsEnvironment::setDeactivationAngularTreshold(float angTresh) 
543 {
544         gAngularSleepingTreshold = angTresh;
545 }
546
547 void            CcdPhysicsEnvironment::setContactBreakingTreshold(float contactBreakingTreshold)
548 {
549         gContactBreakingThreshold = contactBreakingTreshold;
550
551 }
552
553
554 void            CcdPhysicsEnvironment::setCcdMode(int ccdMode)
555 {
556         m_ccdMode = ccdMode;
557 }
558
559
560 void            CcdPhysicsEnvironment::setSolverSorConstant(float sor)
561 {
562         m_solverInfo.m_sor = sor;
563 }
564
565 void            CcdPhysicsEnvironment::setSolverTau(float tau)
566 {
567         m_solverInfo.m_tau = tau;
568 }
569 void            CcdPhysicsEnvironment::setSolverDamping(float damping)
570 {
571         m_solverInfo.m_damping = damping;
572 }
573
574
575 void            CcdPhysicsEnvironment::setLinearAirDamping(float damping)
576 {
577         //gLinearAirDamping = damping;
578 }
579
580 void            CcdPhysicsEnvironment::setUseEpa(bool epa)
581 {
582         //gUseEpa = epa;
583 }
584
585 void            CcdPhysicsEnvironment::setSolverType(int solverType)
586 {
587
588         switch (solverType)
589         {
590         case 1:
591                 {
592                         if (m_solverType != solverType)
593                         {
594
595                                 m_solver = new btSequentialImpulseConstraintSolver();
596 //                              ((btSequentialImpulseConstraintSolver*)m_solver)->setSolverMode(btSequentialImpulseConstraintSolver::SOLVER_USE_WARMSTARTING | btSequentialImpulseConstraintSolver::SOLVER_RANDMIZE_ORDER);
597                                 break;
598                         }
599                 }
600
601         case 0:
602         default:
603                 if (m_solverType != solverType)
604                 {
605 //                      m_solver = new OdeConstraintSolver();
606
607                         break;
608                 }
609
610         };
611
612         m_solverType = solverType ;
613 }
614
615
616
617
618
619
620 void            CcdPhysicsEnvironment::setGravity(float x,float y,float z)
621 {
622         m_gravity = btVector3(x,y,z);
623         m_dynamicsWorld->setGravity(m_gravity);
624
625 }
626
627
628
629
630 static int gConstraintUid = 1;
631
632 //Following the COLLADA physics specification for constraints
633 int                     CcdPhysicsEnvironment::createUniversalD6Constraint(
634                                                 class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
635                                                 btTransform& frameInA,
636                                                 btTransform& frameInB,
637                                                 const btVector3& linearMinLimits,
638                                                 const btVector3& linearMaxLimits,
639                                                 const btVector3& angularMinLimits,
640                                                 const btVector3& angularMaxLimits
641 )
642 {
643
644         //we could either add some logic to recognize ball-socket and hinge, or let that up to the user
645         //perhaps some warning or hint that hinge/ball-socket is more efficient?
646         
647         btGeneric6DofConstraint* genericConstraint = 0;
648         CcdPhysicsController* ctrl0 = (CcdPhysicsController*) ctrlRef;
649         CcdPhysicsController* ctrl1 = (CcdPhysicsController*) ctrlOther;
650         
651         btRigidBody* rb0 = ctrl0->GetRigidBody();
652         btRigidBody* rb1 = ctrl1->GetRigidBody();
653
654         if (rb1)
655         {
656                 
657
658                 bool useReferenceFrameA = true;
659                 genericConstraint = new btGeneric6DofConstraint(
660                         *rb0,*rb1,
661                         frameInA,frameInB,useReferenceFrameA);
662                 genericConstraint->setLinearLowerLimit(linearMinLimits);
663                 genericConstraint->setLinearUpperLimit(linearMaxLimits);
664                 genericConstraint->setAngularLowerLimit(angularMinLimits);
665                 genericConstraint->setAngularUpperLimit(angularMaxLimits);
666         } else
667         {
668                 // TODO: Implement single body case...
669                 //No, we can use a fixed rigidbody in above code, rather then unnecessary duplation of code
670
671         }
672         
673         if (genericConstraint)
674         {
675         //      m_constraints.push_back(genericConstraint);
676                 m_dynamicsWorld->addConstraint(genericConstraint);
677
678                 genericConstraint->setUserConstraintId(gConstraintUid++);
679                 genericConstraint->setUserConstraintType(PHY_GENERIC_6DOF_CONSTRAINT);
680                 //64 bit systems can't cast pointer to int. could use size_t instead.
681                 return genericConstraint->getUserConstraintId();
682         }
683         return 0;
684 }
685
686
687
688 void            CcdPhysicsEnvironment::removeConstraint(int     constraintId)
689 {
690         
691         int i;
692         int numConstraints = m_dynamicsWorld->getNumConstraints();
693         for (i=0;i<numConstraints;i++)
694         {
695                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
696                 if (constraint->getUserConstraintId() == constraintId)
697                 {
698                         constraint->getRigidBodyA().activate();
699                         constraint->getRigidBodyB().activate();
700                         m_dynamicsWorld->removeConstraint(constraint);
701                         break;
702                 }
703         }
704 }
705
706
707 struct  FilterClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
708 {
709         PHY_IRayCastFilterCallback&     m_phyRayFilter;
710         const btCollisionShape*         m_hitTriangleShape;
711         int                                                     m_hitTriangleIndex;
712
713         FilterClosestRayResultCallback (PHY_IRayCastFilterCallback& phyRayFilter,const btVector3& rayFrom,const btVector3& rayTo)
714                 : btCollisionWorld::ClosestRayResultCallback(rayFrom,rayTo),
715                 m_phyRayFilter(phyRayFilter),
716                 m_hitTriangleShape(NULL),
717                 m_hitTriangleIndex(0)
718         {
719         }
720
721         virtual ~FilterClosestRayResultCallback()
722         {
723         }
724
725         virtual bool needsCollision(btBroadphaseProxy* proxy0) const
726         {
727                 if (!(proxy0->m_collisionFilterGroup & m_collisionFilterMask))
728                         return false;
729                 if (!(m_collisionFilterGroup & proxy0->m_collisionFilterMask))
730                         return false;
731                 btCollisionObject* object = (btCollisionObject*)proxy0->m_clientObject;
732                 CcdPhysicsController* phyCtrl = static_cast<CcdPhysicsController*>(object->getUserPointer());
733                 if (phyCtrl == m_phyRayFilter.m_ignoreController)
734                         return false;
735                 return m_phyRayFilter.needBroadphaseRayCast(phyCtrl);
736         }
737
738         virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace)
739         {
740                 CcdPhysicsController* curHit = static_cast<CcdPhysicsController*>(rayResult.m_collisionObject->getUserPointer());
741                 // save shape information as ClosestRayResultCallback::AddSingleResult() does not do it
742                 if (rayResult.m_localShapeInfo)
743                 {
744                         m_hitTriangleShape = NULL;//rayResult.m_localShapeInfo->m_triangleShape;
745                         m_hitTriangleIndex = 0;//rayResult.m_localShapeInfo->m_triangleIndex;
746                 } else 
747                 {
748                         m_hitTriangleShape = NULL;
749                         m_hitTriangleIndex = 0;
750                 }
751                 return ClosestRayResultCallback::addSingleResult(rayResult,normalInWorldSpace);
752         }
753
754 };
755
756 PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IRayCastFilterCallback &filterCallback, float fromX,float fromY,float fromZ, float toX,float toY,float toZ)
757 {
758
759
760         float minFraction = 1.f;
761
762         btVector3 rayFrom(fromX,fromY,fromZ);
763         btVector3 rayTo(toX,toY,toZ);
764
765         btVector3       hitPointWorld,normalWorld;
766
767         //Either Ray Cast with or without filtering
768
769         //btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo);
770         FilterClosestRayResultCallback   rayCallback(filterCallback,rayFrom,rayTo);
771
772
773         PHY_RayCastResult result;
774         memset(&result, 0, sizeof(result));
775
776         // don't collision with sensor object
777         rayCallback.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
778         //, ,filterCallback.m_faceNormal);
779
780         m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback);
781         if (rayCallback.hasHit())
782         {
783                 CcdPhysicsController* controller = static_cast<CcdPhysicsController*>(rayCallback.m_collisionObject->getUserPointer());
784                 result.m_controller = controller;
785                 result.m_hitPoint[0] = rayCallback.m_hitPointWorld.getX();
786                 result.m_hitPoint[1] = rayCallback.m_hitPointWorld.getY();
787                 result.m_hitPoint[2] = rayCallback.m_hitPointWorld.getZ();
788
789                 if (rayCallback.m_hitNormalWorld.length2() > (SIMD_EPSILON*SIMD_EPSILON))
790                 {
791                         rayCallback.m_hitNormalWorld.normalize();
792                 } else
793                 {
794                         rayCallback.m_hitNormalWorld.setValue(1,0,0);
795                 }
796                 result.m_hitNormal[0] = rayCallback.m_hitNormalWorld.getX();
797                 result.m_hitNormal[1] = rayCallback.m_hitNormalWorld.getY();
798                 result.m_hitNormal[2] = rayCallback.m_hitNormalWorld.getZ();
799                 if (rayCallback.m_hitTriangleShape != NULL)
800                 {
801                         // identify the mesh polygon
802                         CcdShapeConstructionInfo* shapeInfo = controller->m_shapeInfo;
803                         if (shapeInfo)
804                         {
805                                 btCollisionShape* shape = controller->GetRigidBody()->getCollisionShape();
806                                 if (shape->isCompound())
807                                 {
808                                         btCompoundShape* compoundShape = (btCompoundShape*)shape;
809                                         CcdShapeConstructionInfo* compoundShapeInfo = shapeInfo;
810                                         // need to search which sub-shape has been hit
811                                         for (int i=0; i<compoundShape->getNumChildShapes(); i++)
812                                         {
813                                                 shapeInfo = compoundShapeInfo->GetChildShape(i);
814                                                 shape=compoundShape->getChildShape(i);
815                                                 if (shape == rayCallback.m_hitTriangleShape)
816                                                         break;
817                                         }
818                                 }
819                                 if (shape == rayCallback.m_hitTriangleShape && 
820                                         rayCallback.m_hitTriangleIndex < shapeInfo->m_polygonIndexArray.size())
821                                 {
822                                         result.m_meshObject = shapeInfo->m_meshObject;
823                                         result.m_polygon = shapeInfo->m_polygonIndexArray.at(rayCallback.m_hitTriangleIndex);
824                                 }
825                         }
826                 }
827                 filterCallback.reportHit(&result);
828         }       
829
830
831         return result.m_controller;
832 }
833
834
835
836 int     CcdPhysicsEnvironment::getNumContactPoints()
837 {
838         return 0;
839 }
840
841 void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
842 {
843
844 }
845
846
847
848
849 btBroadphaseInterface*  CcdPhysicsEnvironment::getBroadphase()
850
851         return m_dynamicsWorld->getBroadphase(); 
852 }
853
854
855
856
857 CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
858 {
859
860 #ifdef NEW_BULLET_VEHICLE_SUPPORT
861         m_wrapperVehicles.clear();
862 #endif //NEW_BULLET_VEHICLE_SUPPORT
863
864         //m_broadphase->DestroyScene();
865         //delete broadphase ? release reference on broadphase ?
866
867         //first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher
868         //delete m_dispatcher;
869         delete m_dynamicsWorld;
870         
871
872         if (NULL != m_ownPairCache)
873                 delete m_ownPairCache;
874
875         if (NULL != m_ownDispatcher)
876                 delete m_ownDispatcher;
877
878         if (NULL != m_solver)
879                 delete m_solver;
880
881         if (NULL != m_debugDrawer)
882                 delete m_debugDrawer;
883
884         if (NULL != m_filterCallback)
885                 delete m_filterCallback;
886
887         if (NULL != m_collisionConfiguration)
888                 delete m_collisionConfiguration;
889
890         if (NULL != m_broadphase)
891                 delete m_broadphase;
892 }
893
894
895 void    CcdPhysicsEnvironment::setConstraintParam(int constraintId,int param,float value0,float value1)
896 {
897         btTypedConstraint* typedConstraint = getConstraintById(constraintId);
898         switch (typedConstraint->getUserConstraintType())
899         {
900         case PHY_GENERIC_6DOF_CONSTRAINT:
901                 {
902                         //param = 1..12, min0,max0,min1,max1...min6,max6
903                         btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
904                         genCons->setLimit(param,value0,value1);
905                         break;
906                 };
907         default:
908                 {
909                 };
910         };
911 }
912
913 btTypedConstraint*      CcdPhysicsEnvironment::getConstraintById(int constraintId)
914 {
915
916         int numConstraints = m_dynamicsWorld->getNumConstraints();
917         int i;
918         for (i=0;i<numConstraints;i++)
919         {
920                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
921                 if (constraint->getUserConstraintId()==constraintId)
922                 {
923                         return constraint;
924                 }
925         }
926         return 0;
927 }
928
929
930 void CcdPhysicsEnvironment::addSensor(PHY_IPhysicsController* ctrl)
931 {
932
933         CcdPhysicsController* ctrl1 = (CcdPhysicsController* )ctrl;
934         // addSensor() is a "light" function for bullet because it is used
935         // dynamically when the sensor is activated. Use enableCcdPhysicsController() instead 
936         //if (m_controllers.insert(ctrl1).second)
937         //{
938         //      addCcdPhysicsController(ctrl1);
939         //}
940         enableCcdPhysicsController(ctrl1);
941
942         //Collision filter/mask is now set at the time of the creation of the controller 
943         //force collision detection with everything, including static objects (might hurt performance!)
944         //ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterMask = btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::SensorTrigger;
945         //ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterGroup = btBroadphaseProxy::SensorTrigger;
946         //todo: make this 'sensor'!
947
948         requestCollisionCallback(ctrl);
949         //printf("addSensor\n");
950 }
951
952 void CcdPhysicsEnvironment::removeCollisionCallback(PHY_IPhysicsController* ctrl)
953 {
954         CcdPhysicsController* ccdCtrl = (CcdPhysicsController*)ctrl;
955         if (ccdCtrl->Unregister())
956                 m_triggerControllers.erase(ccdCtrl);
957 }
958
959
960 void CcdPhysicsEnvironment::removeSensor(PHY_IPhysicsController* ctrl)
961 {
962         removeCollisionCallback(ctrl);
963
964         disableCcdPhysicsController((CcdPhysicsController*)ctrl);
965 }
966
967 void CcdPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user)
968 {
969         /*      printf("addTouchCallback\n(response class = %i)\n",response_class);
970
971         //map PHY_ convention into SM_ convention
972         switch (response_class)
973         {
974         case    PHY_FH_RESPONSE:
975         printf("PHY_FH_RESPONSE\n");
976         break;
977         case PHY_SENSOR_RESPONSE:
978         printf("PHY_SENSOR_RESPONSE\n");
979         break;
980         case PHY_CAMERA_RESPONSE:
981         printf("PHY_CAMERA_RESPONSE\n");
982         break;
983         case PHY_OBJECT_RESPONSE:
984         printf("PHY_OBJECT_RESPONSE\n");
985         break;
986         case PHY_STATIC_RESPONSE:
987         printf("PHY_STATIC_RESPONSE\n");
988         break;
989         default:
990         assert(0);
991         return;
992         }
993         */
994
995         m_triggerCallbacks[response_class] = callback;
996         m_triggerCallbacksUserPtrs[response_class] = user;
997
998 }
999 void CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctrl)
1000 {
1001         CcdPhysicsController* ccdCtrl = static_cast<CcdPhysicsController*>(ctrl);
1002
1003         if (ccdCtrl->Register())
1004                 m_triggerControllers.insert(ccdCtrl);
1005 }
1006
1007 void    CcdPhysicsEnvironment::CallbackTriggers()
1008 {
1009         
1010         CcdPhysicsController* ctrl0=0,*ctrl1=0;
1011
1012         if (m_triggerCallbacks[PHY_OBJECT_RESPONSE] || (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)))
1013         {
1014                 //walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback
1015                 btDispatcher* dispatcher = m_dynamicsWorld->getDispatcher();
1016                 int numManifolds = dispatcher->getNumManifolds();
1017                 for (int i=0;i<numManifolds;i++)
1018                 {
1019                         btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
1020                         int numContacts = manifold->getNumContacts();
1021                         if (numContacts)
1022                         {
1023                                 btRigidBody* rb0 = static_cast<btRigidBody*>(manifold->getBody0());
1024                                 btRigidBody* rb1 = static_cast<btRigidBody*>(manifold->getBody1());
1025                                 if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints))
1026                                 {
1027                                         for (int j=0;j<numContacts;j++)
1028                                         {
1029                                                 btVector3 color(1,0,0);
1030                                                 const btManifoldPoint& cp = manifold->getContactPoint(j);
1031                                                 if (m_debugDrawer)
1032                                                         m_debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
1033                                         }
1034                                 }
1035                                 btRigidBody* obj0 = rb0;
1036                                 btRigidBody* obj1 = rb1;
1037
1038                                 //m_internalOwner is set in 'addPhysicsController'
1039                                 CcdPhysicsController* ctrl0 = static_cast<CcdPhysicsController*>(obj0->getUserPointer());
1040                                 CcdPhysicsController* ctrl1 = static_cast<CcdPhysicsController*>(obj1->getUserPointer());
1041
1042                                 std::set<CcdPhysicsController*>::const_iterator i = m_triggerControllers.find(ctrl0);
1043                                 if (i == m_triggerControllers.end())
1044                                 {
1045                                         i = m_triggerControllers.find(ctrl1);
1046                                 }
1047
1048                                 if (!(i == m_triggerControllers.end()))
1049                                 {
1050                                         m_triggerCallbacks[PHY_OBJECT_RESPONSE](m_triggerCallbacksUserPtrs[PHY_OBJECT_RESPONSE],
1051                                                 ctrl0,ctrl1,0);
1052                                 }
1053                                 // Bullet does not refresh the manifold contact point for object without contact response
1054                                 // may need to remove this when a newer Bullet version is integrated
1055                                 if (!dispatcher->needsResponse(rb0, rb1))
1056                                 {
1057                                         // Refresh algorithm fails sometimes when there is penetration 
1058                                         // (usuall the case with ghost and sensor objects)
1059                                         // Let's just clear the manifold, in any case, it is recomputed on each frame.
1060                                         manifold->clearManifold(); //refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
1061                                 }
1062                         }
1063                 }
1064
1065
1066
1067         }
1068
1069
1070 }
1071
1072 // This call back is called before a pair is added in the cache
1073 // Handy to remove objects that must be ignored by sensors
1074 bool CcdOverlapFilterCallBack::needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
1075 {
1076         btCollisionObject *colObj0, *colObj1;
1077         CcdPhysicsController *sensorCtrl, *objCtrl;
1078         bool collides;
1079         // first check the filters
1080         collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
1081         collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
1082         if (!collides)
1083                 return false;
1084
1085         // additional check for sensor object
1086         if (proxy0->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger)
1087         {
1088                 // this is a sensor object, the other one can't be a sensor object because 
1089                 // they exclude each other in the above test
1090                 assert(!(proxy1->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger));
1091                 colObj0 = (btCollisionObject*)proxy0->m_clientObject;
1092                 colObj1 = (btCollisionObject*)proxy1->m_clientObject;
1093         }
1094         else if (proxy1->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger)
1095         {
1096                 colObj0 = (btCollisionObject*)proxy1->m_clientObject;
1097                 colObj1 = (btCollisionObject*)proxy0->m_clientObject;
1098         }
1099         else
1100         {
1101                 return true;
1102         }
1103         if (!colObj0 || !colObj1)
1104                 return false;
1105         sensorCtrl = static_cast<CcdPhysicsController*>(colObj0->getUserPointer());
1106         objCtrl = static_cast<CcdPhysicsController*>(colObj1->getUserPointer());
1107         if (m_physEnv->m_triggerCallbacks[PHY_BROADPH_RESPONSE])
1108         {
1109                 return m_physEnv->m_triggerCallbacks[PHY_BROADPH_RESPONSE](m_physEnv->m_triggerCallbacksUserPtrs[PHY_BROADPH_RESPONSE], sensorCtrl, objCtrl, 0);
1110         }
1111         return true;
1112 }
1113
1114
1115 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1116
1117 //complex constraint for vehicles
1118 PHY_IVehicle*   CcdPhysicsEnvironment::getVehicleConstraint(int constraintId)
1119 {
1120         int i;
1121
1122         int numVehicles = m_wrapperVehicles.size();
1123         for (i=0;i<numVehicles;i++)
1124         {
1125                 WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
1126                 if (wrapperVehicle->GetVehicle()->getUserConstraintId() == constraintId)
1127                         return wrapperVehicle;
1128         }
1129
1130         return 0;
1131 }
1132
1133 #endif //NEW_BULLET_VEHICLE_SUPPORT
1134
1135
1136 int currentController = 0;
1137 int numController = 0;
1138
1139
1140
1141
1142 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateSphereController(float radius,const PHY__Vector3& position)
1143 {
1144         
1145         CcdConstructionInfo     cinfo;
1146         // memory leak! The shape is not deleted by Bullet and we cannot add it to the KX_Scene.m_shapes list
1147         cinfo.m_collisionShape = new btSphereShape(radius);
1148         cinfo.m_MotionState = 0;
1149         cinfo.m_physicsEnv = this;
1150         // declare this object as Dyamic rather then static!!
1151         // The reason as it is designed to detect all type of object, including static object
1152         // It would cause static-static message to be printed on the console otherwise
1153         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE/* | btCollisionObject::CF_KINEMATIC_OBJECT*/;
1154         DefaultMotionState* motionState = new DefaultMotionState();
1155         cinfo.m_MotionState = motionState;
1156         // we will add later the possibility to select the filter from option
1157         cinfo.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
1158         cinfo.m_collisionFilterGroup = CcdConstructionInfo::SensorFilter;
1159         motionState->m_worldTransform.setIdentity();
1160         motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
1161
1162         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
1163         
1164         return sphereController;
1165 }
1166
1167 int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
1168                                                                                                         float pivotX,float pivotY,float pivotZ,
1169                                                                                                         float axisX,float axisY,float axisZ,
1170                                                                                                         float axis1X,float axis1Y,float axis1Z,
1171                                                                                                         float axis2X,float axis2Y,float axis2Z
1172                                                                                                         )
1173 {
1174
1175
1176         CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
1177         CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
1178
1179         btRigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
1180         btRigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
1181
1182         bool rb0static = rb0 ? rb0->isStaticOrKinematicObject() : true;
1183         bool rb1static = rb1 ? rb1->isStaticOrKinematicObject() : true;
1184         
1185
1186         if (rb0static && rb1static)
1187                 return 0;
1188
1189         btVector3 pivotInA(pivotX,pivotY,pivotZ);
1190         btVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : 
1191                 rb0->getCenterOfMassTransform() * pivotInA;
1192         btVector3 axisInA(axisX,axisY,axisZ);
1193
1194
1195         bool angularOnly = false;
1196
1197         switch (type)
1198         {
1199         case PHY_POINT2POINT_CONSTRAINT:
1200                 {
1201
1202                         btPoint2PointConstraint* p2p = 0;
1203
1204                         if (rb1)
1205                         {
1206                                 p2p = new btPoint2PointConstraint(*rb0,
1207                                         *rb1,pivotInA,pivotInB);
1208                         } else
1209                         {
1210                                 p2p = new btPoint2PointConstraint(*rb0,
1211                                         pivotInA);
1212                         }
1213
1214                         m_dynamicsWorld->addConstraint(p2p);
1215 //                      m_constraints.push_back(p2p);
1216
1217                         p2p->setUserConstraintId(gConstraintUid++);
1218                         p2p->setUserConstraintType(type);
1219                         //64 bit systems can't cast pointer to int. could use size_t instead.
1220                         return p2p->getUserConstraintId();
1221
1222                         break;
1223                 }
1224
1225         case PHY_GENERIC_6DOF_CONSTRAINT:
1226                 {
1227                         btGeneric6DofConstraint* genericConstraint = 0;
1228
1229                         if (rb1)
1230                         {
1231                                 btTransform frameInA;
1232                                 btTransform frameInB;
1233                                 
1234                                 btVector3 axis1(axis1X,axis1Y,axis1Z), axis2(axis2X,axis2Y,axis2Z);
1235                                 if (axis1.length() == 0.0)
1236                                 {
1237                                         btPlaneSpace1( axisInA, axis1, axis2 );
1238                                 }
1239                                 
1240                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1241                                                                   axisInA.y(), axis1.y(), axis2.y(),
1242                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1243                                 frameInA.setOrigin( pivotInA );
1244
1245                                 btTransform inv = rb1->getCenterOfMassTransform().inverse();
1246
1247                                 btTransform globalFrameA = rb0->getCenterOfMassTransform() * frameInA;
1248                                 
1249                                 frameInB = inv  * globalFrameA;
1250                                 bool useReferenceFrameA = true;
1251
1252                                 genericConstraint = new btGeneric6DofConstraint(
1253                                         *rb0,*rb1,
1254                                         frameInA,frameInB,useReferenceFrameA);
1255
1256
1257                         } else
1258                         {
1259                                 static btRigidBody s_fixedObject2( 0,0,0);
1260                                 btTransform frameInA;
1261                                 btTransform frameInB;
1262                                 
1263                                 btVector3 axis1, axis2;
1264                                 btPlaneSpace1( axisInA, axis1, axis2 );
1265
1266                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1267                                                                   axisInA.y(), axis1.y(), axis2.y(),
1268                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1269
1270                                 frameInA.setOrigin( pivotInA );
1271
1272                                 ///frameInB in worldspace
1273                                 frameInB = rb0->getCenterOfMassTransform() * frameInA;
1274
1275                                 bool useReferenceFrameA = true;
1276                                 genericConstraint = new btGeneric6DofConstraint(
1277                                         *rb0,s_fixedObject2,
1278                                         frameInA,frameInB,useReferenceFrameA);
1279                         }
1280                         
1281                         if (genericConstraint)
1282                         {
1283                                 //m_constraints.push_back(genericConstraint);
1284                                 m_dynamicsWorld->addConstraint(genericConstraint);
1285                                 genericConstraint->setUserConstraintId(gConstraintUid++);
1286                                 genericConstraint->setUserConstraintType(type);
1287                                 //64 bit systems can't cast pointer to int. could use size_t instead.
1288                                 return genericConstraint->getUserConstraintId();
1289                         } 
1290
1291                         break;
1292                 }
1293         case PHY_CONE_TWIST_CONSTRAINT:
1294                 {
1295                         btConeTwistConstraint* coneTwistContraint = 0;
1296
1297                         
1298                         if (rb1)
1299                         {
1300                                 btTransform frameInA;
1301                                 btTransform frameInB;
1302                                 
1303                                 btVector3 axis1(axis1X,axis1Y,axis1Z), axis2(axis2X,axis2Y,axis2Z);
1304                                 if (axis1.length() == 0.0)
1305                                 {
1306                                         btPlaneSpace1( axisInA, axis1, axis2 );
1307                                 }
1308                                 
1309                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1310                                                                   axisInA.y(), axis1.y(), axis2.y(),
1311                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1312                                 frameInA.setOrigin( pivotInA );
1313
1314                                 btTransform inv = rb1->getCenterOfMassTransform().inverse();
1315
1316                                 btTransform globalFrameA = rb0->getCenterOfMassTransform() * frameInA;
1317                                 
1318                                 frameInB = inv  * globalFrameA;
1319                                 
1320                                 coneTwistContraint = new btConeTwistConstraint( *rb0,*rb1,
1321                                         frameInA,frameInB);
1322
1323
1324                         } else
1325                         {
1326                                 static btRigidBody s_fixedObject2( 0,0,0);
1327                                 btTransform frameInA;
1328                                 btTransform frameInB;
1329                                 
1330                                 btVector3 axis1, axis2;
1331                                 btPlaneSpace1( axisInA, axis1, axis2 );
1332
1333                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1334                                                                   axisInA.y(), axis1.y(), axis2.y(),
1335                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1336
1337                                 frameInA.setOrigin( pivotInA );
1338
1339                                 ///frameInB in worldspace
1340                                 frameInB = rb0->getCenterOfMassTransform() * frameInA;
1341
1342                                 coneTwistContraint = new btConeTwistConstraint(
1343                                         *rb0,s_fixedObject2,
1344                                         frameInA,frameInB);
1345                         }
1346                         
1347                         if (coneTwistContraint)
1348                         {
1349                                 //m_constraints.push_back(genericConstraint);
1350                                 m_dynamicsWorld->addConstraint(coneTwistContraint);
1351                                 coneTwistContraint->setUserConstraintId(gConstraintUid++);
1352                                 coneTwistContraint->setUserConstraintType(type);
1353                                 //64 bit systems can't cast pointer to int. could use size_t instead.
1354                                 return coneTwistContraint->getUserConstraintId();
1355                         } 
1356
1357
1358
1359                         break;
1360                 }
1361         case PHY_ANGULAR_CONSTRAINT:
1362                 angularOnly = true;
1363
1364
1365         case PHY_LINEHINGE_CONSTRAINT:
1366                 {
1367                         btHingeConstraint* hinge = 0;
1368
1369                         if (rb1)
1370                         {
1371                                 btVector3 axisInB = rb1 ? 
1372                                 (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) : 
1373                                 rb0->getCenterOfMassTransform().getBasis() * axisInA;
1374
1375                                 hinge = new btHingeConstraint(
1376                                         *rb0,
1377                                         *rb1,pivotInA,pivotInB,axisInA,axisInB);
1378
1379
1380                         } else
1381                         {
1382                                 hinge = new btHingeConstraint(*rb0,
1383                                         pivotInA,axisInA);
1384
1385                         }
1386                         hinge->setAngularOnly(angularOnly);
1387
1388                         //m_constraints.push_back(hinge);
1389                         m_dynamicsWorld->addConstraint(hinge);
1390                         hinge->setUserConstraintId(gConstraintUid++);
1391                         hinge->setUserConstraintType(type);
1392                         //64 bit systems can't cast pointer to int. could use size_t instead.
1393                         return hinge->getUserConstraintId();
1394                         break;
1395                 }
1396 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1397
1398         case PHY_VEHICLE_CONSTRAINT:
1399                 {
1400                         btRaycastVehicle::btVehicleTuning* tuning = new btRaycastVehicle::btVehicleTuning();
1401                         btRigidBody* chassis = rb0;
1402                         btDefaultVehicleRaycaster* raycaster = new btDefaultVehicleRaycaster(m_dynamicsWorld);
1403                         btRaycastVehicle* vehicle = new btRaycastVehicle(*tuning,chassis,raycaster);
1404                         WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
1405                         m_wrapperVehicles.push_back(wrapperVehicle);
1406                         m_dynamicsWorld->addVehicle(vehicle);
1407                         vehicle->setUserConstraintId(gConstraintUid++);
1408                         vehicle->setUserConstraintType(type);
1409                         return vehicle->getUserConstraintId();
1410
1411                         break;
1412                 };
1413 #endif //NEW_BULLET_VEHICLE_SUPPORT
1414
1415         default:
1416                 {
1417                 }
1418         };
1419
1420         //btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB
1421
1422         return 0;
1423
1424 }
1425
1426
1427
1428 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float coneradius,float coneheight)
1429 {
1430         CcdConstructionInfo     cinfo;
1431
1432         // we don't need a CcdShapeConstructionInfo for this shape:
1433         // it is simple enough for the standard copy constructor (see CcdPhysicsController::GetReplica)
1434         cinfo.m_collisionShape = new btConeShape(coneradius,coneheight);
1435         cinfo.m_MotionState = 0;
1436         cinfo.m_physicsEnv = this;
1437         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE;
1438         DefaultMotionState* motionState = new DefaultMotionState();
1439         cinfo.m_MotionState = motionState;
1440         
1441         // we will add later the possibility to select the filter from option
1442         cinfo.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
1443         cinfo.m_collisionFilterGroup = CcdConstructionInfo::SensorFilter;
1444         motionState->m_worldTransform.setIdentity();
1445 //      motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
1446
1447         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
1448
1449
1450         return sphereController;
1451 }
1452         
1453 float           CcdPhysicsEnvironment::getAppliedImpulse(int    constraintid)
1454 {
1455         int i;
1456         int numConstraints = m_dynamicsWorld->getNumConstraints();
1457         for (i=0;i<numConstraints;i++)
1458         {
1459                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
1460                 if (constraint->getUserConstraintId() == constraintid)
1461                 {
1462                         return constraint->getAppliedImpulse();
1463                 }
1464         }
1465
1466         return 0.f;
1467 }