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