Created a KX_SoftBodyDeformer for real-time soft bodies.
[blender.git] / source / gameengine / Physics / Bullet / CcdPhysicsEnvironment.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose, 
8 including commercial applications, and to alter it and redistribute it freely, 
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16
17
18
19 #include "CcdPhysicsEnvironment.h"
20 #include "CcdPhysicsController.h"
21
22 #include <algorithm>
23 #include "btBulletDynamicsCommon.h"
24 #include "LinearMath/btIDebugDraw.h"
25 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
26 #include "BulletSoftBody/btSoftRigidDynamicsWorld.h"
27 #include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
28
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                 
456
457 void    CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctrl)
458 {
459         //also remove constraint
460         btRigidBody* body = ctrl->GetRigidBody();
461         if (body)
462         {
463                 m_dynamicsWorld->removeRigidBody(ctrl->GetRigidBody());
464         } else
465         {
466                 //if a softbody
467                 if (ctrl->GetSoftBody())
468                 {
469                         m_dynamicsWorld->removeSoftBody(ctrl->GetSoftBody());
470                 } else
471                 {
472                         m_dynamicsWorld->removeCollisionObject(ctrl->GetCollisionObject());
473                 }
474         }
475         m_controllers.erase(ctrl);
476
477         if (ctrl->m_registerCount != 0)
478                 printf("Warning: removing controller with non-zero m_registerCount: %d\n", ctrl->m_registerCount);
479
480         //remove it from the triggers
481         m_triggerControllers.erase(ctrl);
482 }
483
484 void    CcdPhysicsEnvironment::updateCcdPhysicsController(CcdPhysicsController* ctrl, btScalar newMass, int newCollisionFlags, short int newCollisionGroup, short int newCollisionMask)
485 {
486         // this function is used when the collisionning group of a controller is changed
487         // remove and add the collistioning object
488         btRigidBody* body = ctrl->GetRigidBody();
489         btCollisionObject* obj = ctrl->GetCollisionObject();
490         if (obj)
491         {
492                 btVector3 inertia(0.0,0.0,0.0);
493                 m_dynamicsWorld->removeCollisionObject(obj);
494                 obj->setCollisionFlags(newCollisionFlags);
495                 if (body)
496                 {
497                         if (newMass)
498                                 body->getCollisionShape()->calculateLocalInertia(newMass, inertia);
499                         body->setMassProps(newMass, inertia);
500                 }
501                 m_dynamicsWorld->addCollisionObject(obj, newCollisionGroup, newCollisionMask);
502         }
503         // to avoid nasty interaction, we must update the property of the controller as well
504         ctrl->m_cci.m_mass = newMass;
505         ctrl->m_cci.m_collisionFilterGroup = newCollisionGroup;
506         ctrl->m_cci.m_collisionFilterMask = newCollisionMask;
507         ctrl->m_cci.m_collisionFlags = newCollisionFlags;
508 }
509
510 void CcdPhysicsEnvironment::enableCcdPhysicsController(CcdPhysicsController* ctrl)
511 {
512         if (m_controllers.insert(ctrl).second)
513         {
514                 btCollisionObject* obj = ctrl->GetCollisionObject();
515                 obj->setUserPointer(ctrl);
516                 m_dynamicsWorld->addCollisionObject(obj, 
517                         ctrl->GetCollisionFilterGroup(), ctrl->GetCollisionFilterMask());
518         }
519 }
520
521 void CcdPhysicsEnvironment::disableCcdPhysicsController(CcdPhysicsController* ctrl)
522 {
523         if (m_controllers.erase(ctrl))
524         {
525                 btRigidBody* body = ctrl->GetRigidBody();
526                 if (body)
527                 {
528                         m_dynamicsWorld->removeRigidBody(body);
529                 } else
530                 {
531                         if (ctrl->GetSoftBody())
532                         {
533                         } else
534                         {
535                                 m_dynamicsWorld->removeCollisionObject(body);
536                         }
537                 }
538         }
539 }
540
541
542 void    CcdPhysicsEnvironment::beginFrame()
543 {
544
545 }
546
547
548 bool    CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
549 {
550         std::set<CcdPhysicsController*>::iterator it;
551         int i;
552
553         for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
554         {
555                 (*it)->SynchronizeMotionStates(timeStep);
556         }
557
558         float subStep = timeStep / float(m_numTimeSubSteps);
559         for (i=0;i<m_numTimeSubSteps;i++)
560         {
561 //                      m_dynamicsWorld->stepSimulation(subStep,20,1./240.);//perform always a full simulation step
562                         m_dynamicsWorld->stepSimulation(subStep,0);//perform always a full simulation step
563         }
564
565         for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
566         {
567                 (*it)->SynchronizeMotionStates(timeStep);
568         }
569
570         for (i=0;i<m_wrapperVehicles.size();i++)
571         {
572                 WrapperVehicle* veh = m_wrapperVehicles[i];
573                 veh->SyncWheels();
574         }
575
576         m_dynamicsWorld->debugDrawWorld();
577
578
579         CallbackTriggers();
580
581         return true;
582 }
583
584
585 void            CcdPhysicsEnvironment::setDebugMode(int debugMode)
586 {
587         if (m_debugDrawer){
588                 m_debugDrawer->setDebugMode(debugMode);
589         }
590 }
591
592 void            CcdPhysicsEnvironment::setNumIterations(int numIter)
593 {
594         m_numIterations = numIter;
595 }
596 void            CcdPhysicsEnvironment::setDeactivationTime(float dTime)
597 {
598         gDeactivationTime = dTime;
599 }
600 void            CcdPhysicsEnvironment::setDeactivationLinearTreshold(float linTresh)
601 {
602         gLinearSleepingTreshold = linTresh;
603 }
604 void            CcdPhysicsEnvironment::setDeactivationAngularTreshold(float angTresh) 
605 {
606         gAngularSleepingTreshold = angTresh;
607 }
608
609 void            CcdPhysicsEnvironment::setContactBreakingTreshold(float contactBreakingTreshold)
610 {
611         gContactBreakingThreshold = contactBreakingTreshold;
612
613 }
614
615
616 void            CcdPhysicsEnvironment::setCcdMode(int ccdMode)
617 {
618         m_ccdMode = ccdMode;
619 }
620
621
622 void            CcdPhysicsEnvironment::setSolverSorConstant(float sor)
623 {
624         m_solverInfo.m_sor = sor;
625 }
626
627 void            CcdPhysicsEnvironment::setSolverTau(float tau)
628 {
629         m_solverInfo.m_tau = tau;
630 }
631 void            CcdPhysicsEnvironment::setSolverDamping(float damping)
632 {
633         m_solverInfo.m_damping = damping;
634 }
635
636
637 void            CcdPhysicsEnvironment::setLinearAirDamping(float damping)
638 {
639         //gLinearAirDamping = damping;
640 }
641
642 void            CcdPhysicsEnvironment::setUseEpa(bool epa)
643 {
644         //gUseEpa = epa;
645 }
646
647 void            CcdPhysicsEnvironment::setSolverType(int solverType)
648 {
649
650         switch (solverType)
651         {
652         case 1:
653                 {
654                         if (m_solverType != solverType)
655                         {
656
657                                 m_solver = new btSequentialImpulseConstraintSolver();
658 //                              ((btSequentialImpulseConstraintSolver*)m_solver)->setSolverMode(btSequentialImpulseConstraintSolver::SOLVER_USE_WARMSTARTING | btSequentialImpulseConstraintSolver::SOLVER_RANDMIZE_ORDER);
659                                 break;
660                         }
661                 }
662
663         case 0:
664         default:
665                 if (m_solverType != solverType)
666                 {
667 //                      m_solver = new OdeConstraintSolver();
668
669                         break;
670                 }
671
672         };
673
674         m_solverType = solverType ;
675 }
676
677
678
679 void            CcdPhysicsEnvironment::getGravity(PHY__Vector3& grav)
680 {
681                 const btVector3& gravity = m_dynamicsWorld->getGravity();
682                 grav[0] = gravity.getX();
683                 grav[1] = gravity.getY();
684                 grav[2] = gravity.getZ();
685 }
686
687
688 void            CcdPhysicsEnvironment::setGravity(float x,float y,float z)
689 {
690         m_gravity = btVector3(x,y,z);
691         m_dynamicsWorld->setGravity(m_gravity);
692
693 }
694
695
696
697
698 static int gConstraintUid = 1;
699
700 //Following the COLLADA physics specification for constraints
701 int                     CcdPhysicsEnvironment::createUniversalD6Constraint(
702                                                 class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
703                                                 btTransform& frameInA,
704                                                 btTransform& frameInB,
705                                                 const btVector3& linearMinLimits,
706                                                 const btVector3& linearMaxLimits,
707                                                 const btVector3& angularMinLimits,
708                                                 const btVector3& angularMaxLimits
709 )
710 {
711
712         //we could either add some logic to recognize ball-socket and hinge, or let that up to the user
713         //perhaps some warning or hint that hinge/ball-socket is more efficient?
714         
715         btGeneric6DofConstraint* genericConstraint = 0;
716         CcdPhysicsController* ctrl0 = (CcdPhysicsController*) ctrlRef;
717         CcdPhysicsController* ctrl1 = (CcdPhysicsController*) ctrlOther;
718         
719         btRigidBody* rb0 = ctrl0->GetRigidBody();
720         btRigidBody* rb1 = ctrl1->GetRigidBody();
721
722         if (rb1)
723         {
724                 
725
726                 bool useReferenceFrameA = true;
727                 genericConstraint = new btGeneric6DofConstraint(
728                         *rb0,*rb1,
729                         frameInA,frameInB,useReferenceFrameA);
730                 genericConstraint->setLinearLowerLimit(linearMinLimits);
731                 genericConstraint->setLinearUpperLimit(linearMaxLimits);
732                 genericConstraint->setAngularLowerLimit(angularMinLimits);
733                 genericConstraint->setAngularUpperLimit(angularMaxLimits);
734         } else
735         {
736                 // TODO: Implement single body case...
737                 //No, we can use a fixed rigidbody in above code, rather then unnecessary duplation of code
738
739         }
740         
741         if (genericConstraint)
742         {
743         //      m_constraints.push_back(genericConstraint);
744                 m_dynamicsWorld->addConstraint(genericConstraint);
745
746                 genericConstraint->setUserConstraintId(gConstraintUid++);
747                 genericConstraint->setUserConstraintType(PHY_GENERIC_6DOF_CONSTRAINT);
748                 //64 bit systems can't cast pointer to int. could use size_t instead.
749                 return genericConstraint->getUserConstraintId();
750         }
751         return 0;
752 }
753
754
755
756 void            CcdPhysicsEnvironment::removeConstraint(int     constraintId)
757 {
758         
759         int i;
760         int numConstraints = m_dynamicsWorld->getNumConstraints();
761         for (i=0;i<numConstraints;i++)
762         {
763                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
764                 if (constraint->getUserConstraintId() == constraintId)
765                 {
766                         constraint->getRigidBodyA().activate();
767                         constraint->getRigidBodyB().activate();
768                         m_dynamicsWorld->removeConstraint(constraint);
769                         break;
770                 }
771         }
772 }
773
774
775 struct  FilterClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
776 {
777         PHY_IRayCastFilterCallback&     m_phyRayFilter;
778         const btCollisionShape*         m_hitTriangleShape;
779         int                                                     m_hitTriangleIndex;
780
781         FilterClosestRayResultCallback (PHY_IRayCastFilterCallback& phyRayFilter,const btVector3& rayFrom,const btVector3& rayTo)
782                 : btCollisionWorld::ClosestRayResultCallback(rayFrom,rayTo),
783                 m_phyRayFilter(phyRayFilter),
784                 m_hitTriangleShape(NULL),
785                 m_hitTriangleIndex(0)
786         {
787         }
788
789         virtual ~FilterClosestRayResultCallback()
790         {
791         }
792
793         virtual bool needsCollision(btBroadphaseProxy* proxy0) const
794         {
795                 if (!(proxy0->m_collisionFilterGroup & m_collisionFilterMask))
796                         return false;
797                 if (!(m_collisionFilterGroup & proxy0->m_collisionFilterMask))
798                         return false;
799                 btCollisionObject* object = (btCollisionObject*)proxy0->m_clientObject;
800                 CcdPhysicsController* phyCtrl = static_cast<CcdPhysicsController*>(object->getUserPointer());
801                 if (phyCtrl == m_phyRayFilter.m_ignoreController)
802                         return false;
803                 return m_phyRayFilter.needBroadphaseRayCast(phyCtrl);
804         }
805
806         virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace)
807         {
808                 CcdPhysicsController* curHit = static_cast<CcdPhysicsController*>(rayResult.m_collisionObject->getUserPointer());
809                 // save shape information as ClosestRayResultCallback::AddSingleResult() does not do it
810                 if (rayResult.m_localShapeInfo)
811                 {
812                         m_hitTriangleShape = rayResult.m_collisionObject->getCollisionShape();
813                         m_hitTriangleIndex = rayResult.m_localShapeInfo->m_triangleIndex;
814                 } else 
815                 {
816                         m_hitTriangleShape = NULL;
817                         m_hitTriangleIndex = 0;
818                 }
819                 return ClosestRayResultCallback::addSingleResult(rayResult,normalInWorldSpace);
820         }
821
822 };
823
824 PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IRayCastFilterCallback &filterCallback, float fromX,float fromY,float fromZ, float toX,float toY,float toZ)
825 {
826
827
828         float minFraction = 1.f;
829
830         btVector3 rayFrom(fromX,fromY,fromZ);
831         btVector3 rayTo(toX,toY,toZ);
832
833         btVector3       hitPointWorld,normalWorld;
834
835         //Either Ray Cast with or without filtering
836
837         //btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo);
838         FilterClosestRayResultCallback   rayCallback(filterCallback,rayFrom,rayTo);
839
840
841         PHY_RayCastResult result;
842         memset(&result, 0, sizeof(result));
843
844         // don't collision with sensor object
845         rayCallback.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
846         //, ,filterCallback.m_faceNormal);
847
848         m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback);
849         if (rayCallback.hasHit())
850         {
851                 CcdPhysicsController* controller = static_cast<CcdPhysicsController*>(rayCallback.m_collisionObject->getUserPointer());
852                 result.m_controller = controller;
853                 result.m_hitPoint[0] = rayCallback.m_hitPointWorld.getX();
854                 result.m_hitPoint[1] = rayCallback.m_hitPointWorld.getY();
855                 result.m_hitPoint[2] = rayCallback.m_hitPointWorld.getZ();
856
857                 if (rayCallback.m_hitTriangleShape != NULL)
858                 {
859                         // identify the mesh polygon
860                         CcdShapeConstructionInfo* shapeInfo = controller->m_shapeInfo;
861                         if (shapeInfo)
862                         {
863                                 btCollisionShape* shape = controller->GetCollisionObject()->getCollisionShape();
864                                 if (shape->isCompound())
865                                 {
866                                         btCompoundShape* compoundShape = (btCompoundShape*)shape;
867                                         CcdShapeConstructionInfo* compoundShapeInfo = shapeInfo;
868                                         // need to search which sub-shape has been hit
869                                         for (int i=0; i<compoundShape->getNumChildShapes(); i++)
870                                         {
871                                                 shapeInfo = compoundShapeInfo->GetChildShape(i);
872                                                 shape=compoundShape->getChildShape(i);
873                                                 if (shape == rayCallback.m_hitTriangleShape)
874                                                         break;
875                                         }
876                                 }
877                                 if (shape == rayCallback.m_hitTriangleShape && 
878                                         rayCallback.m_hitTriangleIndex < shapeInfo->m_polygonIndexArray.size())
879                                 {
880                                         result.m_meshObject = shapeInfo->GetMesh();
881                                         result.m_polygon = shapeInfo->m_polygonIndexArray.at(rayCallback.m_hitTriangleIndex);
882
883                                         // Bullet returns the normal from "outside".
884                                         // If the user requests the real normal, compute it now
885                     if (filterCallback.m_faceNormal)
886                                         {
887                                                 // mesh shapes are shared and stored in the shapeInfo
888                                                 btTriangleMeshShape* triangleShape = shapeInfo->GetMeshShape();
889                                                 if (triangleShape)
890                                                 {
891                                                         // this code is copied from Bullet 
892                                                         btVector3 triangle[3];
893                                                         const unsigned char *vertexbase;
894                                                         int numverts;
895                                                         PHY_ScalarType type;
896                                                         int stride;
897                                                         const unsigned char *indexbase;
898                                                         int indexstride;
899                                                         int numfaces;
900                                                         PHY_ScalarType indicestype;
901                                                         btStridingMeshInterface* meshInterface = triangleShape->getMeshInterface();
902
903                                                         meshInterface->getLockedReadOnlyVertexIndexBase(
904                                                                 &vertexbase,
905                                                                 numverts,
906                                                                 type,
907                                                                 stride,
908                                                                 &indexbase,
909                                                                 indexstride,
910                                                                 numfaces,
911                                                                 indicestype,
912                                                                 0);
913
914                                                         unsigned int* gfxbase = (unsigned int*)(indexbase+rayCallback.m_hitTriangleIndex*indexstride);
915                                                         const btVector3& meshScaling = shape->getLocalScaling();
916                                                         for (int j=2;j>=0;j--)
917                                                         {
918                                                                 int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
919
920                                                                 btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride);
921
922                                                                 triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());              
923                                                         }
924                                                         meshInterface->unLockReadOnlyVertexBase(0);
925                                                         btVector3 triangleNormal; 
926                                                         triangleNormal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]);
927                                                         rayCallback.m_hitNormalWorld = rayCallback.m_collisionObject->getWorldTransform().getBasis()*triangleNormal;
928                                                 }
929                                         }
930                                 }
931                         }
932                 }
933                 if (rayCallback.m_hitNormalWorld.length2() > (SIMD_EPSILON*SIMD_EPSILON))
934                 {
935                         rayCallback.m_hitNormalWorld.normalize();
936                 } else
937                 {
938                         rayCallback.m_hitNormalWorld.setValue(1,0,0);
939                 }
940                 result.m_hitNormal[0] = rayCallback.m_hitNormalWorld.getX();
941                 result.m_hitNormal[1] = rayCallback.m_hitNormalWorld.getY();
942                 result.m_hitNormal[2] = rayCallback.m_hitNormalWorld.getZ();
943                 filterCallback.reportHit(&result);
944         }       
945
946
947         return result.m_controller;
948 }
949
950
951
952 int     CcdPhysicsEnvironment::getNumContactPoints()
953 {
954         return 0;
955 }
956
957 void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
958 {
959
960 }
961
962
963
964
965 btBroadphaseInterface*  CcdPhysicsEnvironment::getBroadphase()
966
967         return m_dynamicsWorld->getBroadphase(); 
968 }
969
970 btDispatcher*   CcdPhysicsEnvironment::getDispatcher()
971
972         return m_dynamicsWorld->getDispatcher();
973 }
974
975
976
977
978
979
980 CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
981 {
982
983 #ifdef NEW_BULLET_VEHICLE_SUPPORT
984         m_wrapperVehicles.clear();
985 #endif //NEW_BULLET_VEHICLE_SUPPORT
986
987         //m_broadphase->DestroyScene();
988         //delete broadphase ? release reference on broadphase ?
989
990         //first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher
991         //delete m_dispatcher;
992         delete m_dynamicsWorld;
993         
994
995         if (NULL != m_ownPairCache)
996                 delete m_ownPairCache;
997
998         if (NULL != m_ownDispatcher)
999                 delete m_ownDispatcher;
1000
1001         if (NULL != m_solver)
1002                 delete m_solver;
1003
1004         if (NULL != m_debugDrawer)
1005                 delete m_debugDrawer;
1006
1007         if (NULL != m_filterCallback)
1008                 delete m_filterCallback;
1009
1010         if (NULL != m_collisionConfiguration)
1011                 delete m_collisionConfiguration;
1012
1013         if (NULL != m_broadphase)
1014                 delete m_broadphase;
1015 }
1016
1017
1018 void    CcdPhysicsEnvironment::setConstraintParam(int constraintId,int param,float value0,float value1)
1019 {
1020         btTypedConstraint* typedConstraint = getConstraintById(constraintId);
1021         switch (typedConstraint->getUserConstraintType())
1022         {
1023         case PHY_GENERIC_6DOF_CONSTRAINT:
1024                 {
1025                         //param = 1..12, min0,max0,min1,max1...min6,max6
1026                         btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
1027                         genCons->setLimit(param,value0,value1);
1028                         break;
1029                 };
1030         default:
1031                 {
1032                 };
1033         };
1034 }
1035
1036 btTypedConstraint*      CcdPhysicsEnvironment::getConstraintById(int constraintId)
1037 {
1038
1039         int numConstraints = m_dynamicsWorld->getNumConstraints();
1040         int i;
1041         for (i=0;i<numConstraints;i++)
1042         {
1043                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
1044                 if (constraint->getUserConstraintId()==constraintId)
1045                 {
1046                         return constraint;
1047                 }
1048         }
1049         return 0;
1050 }
1051
1052
1053 void CcdPhysicsEnvironment::addSensor(PHY_IPhysicsController* ctrl)
1054 {
1055
1056         CcdPhysicsController* ctrl1 = (CcdPhysicsController* )ctrl;
1057         // addSensor() is a "light" function for bullet because it is used
1058         // dynamically when the sensor is activated. Use enableCcdPhysicsController() instead 
1059         //if (m_controllers.insert(ctrl1).second)
1060         //{
1061         //      addCcdPhysicsController(ctrl1);
1062         //}
1063         enableCcdPhysicsController(ctrl1);
1064
1065         //Collision filter/mask is now set at the time of the creation of the controller 
1066         //force collision detection with everything, including static objects (might hurt performance!)
1067         //ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterMask = btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::SensorTrigger;
1068         //ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterGroup = btBroadphaseProxy::SensorTrigger;
1069         //todo: make this 'sensor'!
1070
1071         requestCollisionCallback(ctrl);
1072         //printf("addSensor\n");
1073 }
1074
1075 void CcdPhysicsEnvironment::removeCollisionCallback(PHY_IPhysicsController* ctrl)
1076 {
1077         CcdPhysicsController* ccdCtrl = (CcdPhysicsController*)ctrl;
1078         if (ccdCtrl->Unregister())
1079                 m_triggerControllers.erase(ccdCtrl);
1080 }
1081
1082
1083 void CcdPhysicsEnvironment::removeSensor(PHY_IPhysicsController* ctrl)
1084 {
1085         removeCollisionCallback(ctrl);
1086
1087         disableCcdPhysicsController((CcdPhysicsController*)ctrl);
1088 }
1089
1090 void CcdPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user)
1091 {
1092         /*      printf("addTouchCallback\n(response class = %i)\n",response_class);
1093
1094         //map PHY_ convention into SM_ convention
1095         switch (response_class)
1096         {
1097         case    PHY_FH_RESPONSE:
1098         printf("PHY_FH_RESPONSE\n");
1099         break;
1100         case PHY_SENSOR_RESPONSE:
1101         printf("PHY_SENSOR_RESPONSE\n");
1102         break;
1103         case PHY_CAMERA_RESPONSE:
1104         printf("PHY_CAMERA_RESPONSE\n");
1105         break;
1106         case PHY_OBJECT_RESPONSE:
1107         printf("PHY_OBJECT_RESPONSE\n");
1108         break;
1109         case PHY_STATIC_RESPONSE:
1110         printf("PHY_STATIC_RESPONSE\n");
1111         break;
1112         default:
1113         assert(0);
1114         return;
1115         }
1116         */
1117
1118         m_triggerCallbacks[response_class] = callback;
1119         m_triggerCallbacksUserPtrs[response_class] = user;
1120
1121 }
1122 void CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctrl)
1123 {
1124         CcdPhysicsController* ccdCtrl = static_cast<CcdPhysicsController*>(ctrl);
1125
1126         if (ccdCtrl->Register())
1127                 m_triggerControllers.insert(ccdCtrl);
1128 }
1129
1130 void    CcdPhysicsEnvironment::CallbackTriggers()
1131 {
1132         
1133         CcdPhysicsController* ctrl0=0,*ctrl1=0;
1134
1135         if (m_triggerCallbacks[PHY_OBJECT_RESPONSE] || (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)))
1136         {
1137                 //walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback
1138                 btDispatcher* dispatcher = m_dynamicsWorld->getDispatcher();
1139                 int numManifolds = dispatcher->getNumManifolds();
1140                 for (int i=0;i<numManifolds;i++)
1141                 {
1142                         btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
1143                         int numContacts = manifold->getNumContacts();
1144                         if (numContacts)
1145                         {
1146                                 btRigidBody* rb0 = static_cast<btRigidBody*>(manifold->getBody0());
1147                                 btRigidBody* rb1 = static_cast<btRigidBody*>(manifold->getBody1());
1148                                 if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints))
1149                                 {
1150                                         for (int j=0;j<numContacts;j++)
1151                                         {
1152                                                 btVector3 color(1,0,0);
1153                                                 const btManifoldPoint& cp = manifold->getContactPoint(j);
1154                                                 if (m_debugDrawer)
1155                                                         m_debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
1156                                         }
1157                                 }
1158                                 btRigidBody* obj0 = rb0;
1159                                 btRigidBody* obj1 = rb1;
1160
1161                                 //m_internalOwner is set in 'addPhysicsController'
1162                                 CcdPhysicsController* ctrl0 = static_cast<CcdPhysicsController*>(obj0->getUserPointer());
1163                                 CcdPhysicsController* ctrl1 = static_cast<CcdPhysicsController*>(obj1->getUserPointer());
1164
1165                                 std::set<CcdPhysicsController*>::const_iterator i = m_triggerControllers.find(ctrl0);
1166                                 if (i == m_triggerControllers.end())
1167                                 {
1168                                         i = m_triggerControllers.find(ctrl1);
1169                                 }
1170
1171                                 if (!(i == m_triggerControllers.end()))
1172                                 {
1173                                         m_triggerCallbacks[PHY_OBJECT_RESPONSE](m_triggerCallbacksUserPtrs[PHY_OBJECT_RESPONSE],
1174                                                 ctrl0,ctrl1,0);
1175                                 }
1176                                 // Bullet does not refresh the manifold contact point for object without contact response
1177                                 // may need to remove this when a newer Bullet version is integrated
1178                                 if (!dispatcher->needsResponse(rb0, rb1))
1179                                 {
1180                                         // Refresh algorithm fails sometimes when there is penetration 
1181                                         // (usuall the case with ghost and sensor objects)
1182                                         // Let's just clear the manifold, in any case, it is recomputed on each frame.
1183                                         manifold->clearManifold(); //refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
1184                                 }
1185                         }
1186                 }
1187
1188
1189
1190         }
1191
1192
1193 }
1194
1195 // This call back is called before a pair is added in the cache
1196 // Handy to remove objects that must be ignored by sensors
1197 bool CcdOverlapFilterCallBack::needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
1198 {
1199         btCollisionObject *colObj0, *colObj1;
1200         CcdPhysicsController *sensorCtrl, *objCtrl;
1201         bool collides;
1202         // first check the filters
1203         collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
1204         collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
1205         if (!collides)
1206                 return false;
1207
1208         // additional check for sensor object
1209         if (proxy0->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger)
1210         {
1211                 // this is a sensor object, the other one can't be a sensor object because 
1212                 // they exclude each other in the above test
1213                 assert(!(proxy1->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger));
1214                 colObj0 = (btCollisionObject*)proxy0->m_clientObject;
1215                 colObj1 = (btCollisionObject*)proxy1->m_clientObject;
1216         }
1217         else if (proxy1->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger)
1218         {
1219                 colObj0 = (btCollisionObject*)proxy1->m_clientObject;
1220                 colObj1 = (btCollisionObject*)proxy0->m_clientObject;
1221         }
1222         else
1223         {
1224                 return true;
1225         }
1226         if (!colObj0 || !colObj1)
1227                 return false;
1228         sensorCtrl = static_cast<CcdPhysicsController*>(colObj0->getUserPointer());
1229         objCtrl = static_cast<CcdPhysicsController*>(colObj1->getUserPointer());
1230         if (m_physEnv->m_triggerCallbacks[PHY_BROADPH_RESPONSE])
1231         {
1232                 return m_physEnv->m_triggerCallbacks[PHY_BROADPH_RESPONSE](m_physEnv->m_triggerCallbacksUserPtrs[PHY_BROADPH_RESPONSE], sensorCtrl, objCtrl, 0);
1233         }
1234         return true;
1235 }
1236
1237
1238 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1239
1240 //complex constraint for vehicles
1241 PHY_IVehicle*   CcdPhysicsEnvironment::getVehicleConstraint(int constraintId)
1242 {
1243         int i;
1244
1245         int numVehicles = m_wrapperVehicles.size();
1246         for (i=0;i<numVehicles;i++)
1247         {
1248                 WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
1249                 if (wrapperVehicle->GetVehicle()->getUserConstraintId() == constraintId)
1250                         return wrapperVehicle;
1251         }
1252
1253         return 0;
1254 }
1255
1256 #endif //NEW_BULLET_VEHICLE_SUPPORT
1257
1258
1259 int currentController = 0;
1260 int numController = 0;
1261
1262
1263
1264
1265 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateSphereController(float radius,const PHY__Vector3& position)
1266 {
1267         
1268         CcdConstructionInfo     cinfo;
1269         // memory leak! The shape is not deleted by Bullet and we cannot add it to the KX_Scene.m_shapes list
1270         cinfo.m_collisionShape = new btSphereShape(radius);
1271         cinfo.m_MotionState = 0;
1272         cinfo.m_physicsEnv = this;
1273         // declare this object as Dyamic rather then static!!
1274         // The reason as it is designed to detect all type of object, including static object
1275         // It would cause static-static message to be printed on the console otherwise
1276         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE/* | btCollisionObject::CF_KINEMATIC_OBJECT*/;
1277         DefaultMotionState* motionState = new DefaultMotionState();
1278         cinfo.m_MotionState = motionState;
1279         // we will add later the possibility to select the filter from option
1280         cinfo.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
1281         cinfo.m_collisionFilterGroup = CcdConstructionInfo::SensorFilter;
1282         motionState->m_worldTransform.setIdentity();
1283         motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
1284
1285         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
1286         
1287         return sphereController;
1288 }
1289
1290 int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
1291                                                                                                         float pivotX,float pivotY,float pivotZ,
1292                                                                                                         float axisX,float axisY,float axisZ,
1293                                                                                                         float axis1X,float axis1Y,float axis1Z,
1294                                                                                                         float axis2X,float axis2Y,float axis2Z
1295                                                                                                         )
1296 {
1297
1298
1299         CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
1300         CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
1301
1302         btRigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
1303         btRigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
1304
1305         bool rb0static = rb0 ? rb0->isStaticOrKinematicObject() : true;
1306         bool rb1static = rb1 ? rb1->isStaticOrKinematicObject() : true;
1307         
1308
1309         if (rb0static && rb1static)
1310                 return 0;
1311
1312         btVector3 pivotInA(pivotX,pivotY,pivotZ);
1313         btVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : 
1314                 rb0->getCenterOfMassTransform() * pivotInA;
1315         btVector3 axisInA(axisX,axisY,axisZ);
1316
1317
1318         bool angularOnly = false;
1319
1320         switch (type)
1321         {
1322         case PHY_POINT2POINT_CONSTRAINT:
1323                 {
1324
1325                         btPoint2PointConstraint* p2p = 0;
1326
1327                         if (rb1)
1328                         {
1329                                 p2p = new btPoint2PointConstraint(*rb0,
1330                                         *rb1,pivotInA,pivotInB);
1331                         } else
1332                         {
1333                                 p2p = new btPoint2PointConstraint(*rb0,
1334                                         pivotInA);
1335                         }
1336
1337                         m_dynamicsWorld->addConstraint(p2p);
1338 //                      m_constraints.push_back(p2p);
1339
1340                         p2p->setUserConstraintId(gConstraintUid++);
1341                         p2p->setUserConstraintType(type);
1342                         //64 bit systems can't cast pointer to int. could use size_t instead.
1343                         return p2p->getUserConstraintId();
1344
1345                         break;
1346                 }
1347
1348         case PHY_GENERIC_6DOF_CONSTRAINT:
1349                 {
1350                         btGeneric6DofConstraint* genericConstraint = 0;
1351
1352                         if (rb1)
1353                         {
1354                                 btTransform frameInA;
1355                                 btTransform frameInB;
1356                                 
1357                                 btVector3 axis1(axis1X,axis1Y,axis1Z), axis2(axis2X,axis2Y,axis2Z);
1358                                 if (axis1.length() == 0.0)
1359                                 {
1360                                         btPlaneSpace1( axisInA, axis1, axis2 );
1361                                 }
1362                                 
1363                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1364                                                                   axisInA.y(), axis1.y(), axis2.y(),
1365                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1366                                 frameInA.setOrigin( pivotInA );
1367
1368                                 btTransform inv = rb1->getCenterOfMassTransform().inverse();
1369
1370                                 btTransform globalFrameA = rb0->getCenterOfMassTransform() * frameInA;
1371                                 
1372                                 frameInB = inv  * globalFrameA;
1373                                 bool useReferenceFrameA = true;
1374
1375                                 genericConstraint = new btGeneric6DofConstraint(
1376                                         *rb0,*rb1,
1377                                         frameInA,frameInB,useReferenceFrameA);
1378
1379
1380                         } else
1381                         {
1382                                 static btRigidBody s_fixedObject2( 0,0,0);
1383                                 btTransform frameInA;
1384                                 btTransform frameInB;
1385                                 
1386                                 btVector3 axis1, axis2;
1387                                 btPlaneSpace1( axisInA, axis1, axis2 );
1388
1389                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1390                                                                   axisInA.y(), axis1.y(), axis2.y(),
1391                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1392
1393                                 frameInA.setOrigin( pivotInA );
1394
1395                                 ///frameInB in worldspace
1396                                 frameInB = rb0->getCenterOfMassTransform() * frameInA;
1397
1398                                 bool useReferenceFrameA = true;
1399                                 genericConstraint = new btGeneric6DofConstraint(
1400                                         *rb0,s_fixedObject2,
1401                                         frameInA,frameInB,useReferenceFrameA);
1402                         }
1403                         
1404                         if (genericConstraint)
1405                         {
1406                                 //m_constraints.push_back(genericConstraint);
1407                                 m_dynamicsWorld->addConstraint(genericConstraint);
1408                                 genericConstraint->setUserConstraintId(gConstraintUid++);
1409                                 genericConstraint->setUserConstraintType(type);
1410                                 //64 bit systems can't cast pointer to int. could use size_t instead.
1411                                 return genericConstraint->getUserConstraintId();
1412                         } 
1413
1414                         break;
1415                 }
1416         case PHY_CONE_TWIST_CONSTRAINT:
1417                 {
1418                         btConeTwistConstraint* coneTwistContraint = 0;
1419
1420                         
1421                         if (rb1)
1422                         {
1423                                 btTransform frameInA;
1424                                 btTransform frameInB;
1425                                 
1426                                 btVector3 axis1(axis1X,axis1Y,axis1Z), axis2(axis2X,axis2Y,axis2Z);
1427                                 if (axis1.length() == 0.0)
1428                                 {
1429                                         btPlaneSpace1( axisInA, axis1, axis2 );
1430                                 }
1431                                 
1432                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1433                                                                   axisInA.y(), axis1.y(), axis2.y(),
1434                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1435                                 frameInA.setOrigin( pivotInA );
1436
1437                                 btTransform inv = rb1->getCenterOfMassTransform().inverse();
1438
1439                                 btTransform globalFrameA = rb0->getCenterOfMassTransform() * frameInA;
1440                                 
1441                                 frameInB = inv  * globalFrameA;
1442                                 
1443                                 coneTwistContraint = new btConeTwistConstraint( *rb0,*rb1,
1444                                         frameInA,frameInB);
1445
1446
1447                         } else
1448                         {
1449                                 static btRigidBody s_fixedObject2( 0,0,0);
1450                                 btTransform frameInA;
1451                                 btTransform frameInB;
1452                                 
1453                                 btVector3 axis1, axis2;
1454                                 btPlaneSpace1( axisInA, axis1, axis2 );
1455
1456                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1457                                                                   axisInA.y(), axis1.y(), axis2.y(),
1458                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1459
1460                                 frameInA.setOrigin( pivotInA );
1461
1462                                 ///frameInB in worldspace
1463                                 frameInB = rb0->getCenterOfMassTransform() * frameInA;
1464
1465                                 coneTwistContraint = new btConeTwistConstraint(
1466                                         *rb0,s_fixedObject2,
1467                                         frameInA,frameInB);
1468                         }
1469                         
1470                         if (coneTwistContraint)
1471                         {
1472                                 //m_constraints.push_back(genericConstraint);
1473                                 m_dynamicsWorld->addConstraint(coneTwistContraint);
1474                                 coneTwistContraint->setUserConstraintId(gConstraintUid++);
1475                                 coneTwistContraint->setUserConstraintType(type);
1476                                 //64 bit systems can't cast pointer to int. could use size_t instead.
1477                                 return coneTwistContraint->getUserConstraintId();
1478                         } 
1479
1480
1481
1482                         break;
1483                 }
1484         case PHY_ANGULAR_CONSTRAINT:
1485                 angularOnly = true;
1486
1487
1488         case PHY_LINEHINGE_CONSTRAINT:
1489                 {
1490                         btHingeConstraint* hinge = 0;
1491
1492                         if (rb1)
1493                         {
1494                                 btVector3 axisInB = rb1 ? 
1495                                 (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) : 
1496                                 rb0->getCenterOfMassTransform().getBasis() * axisInA;
1497
1498                                 hinge = new btHingeConstraint(
1499                                         *rb0,
1500                                         *rb1,pivotInA,pivotInB,axisInA,axisInB);
1501
1502
1503                         } else
1504                         {
1505                                 hinge = new btHingeConstraint(*rb0,
1506                                         pivotInA,axisInA);
1507
1508                         }
1509                         hinge->setAngularOnly(angularOnly);
1510
1511                         //m_constraints.push_back(hinge);
1512                         m_dynamicsWorld->addConstraint(hinge);
1513                         hinge->setUserConstraintId(gConstraintUid++);
1514                         hinge->setUserConstraintType(type);
1515                         //64 bit systems can't cast pointer to int. could use size_t instead.
1516                         return hinge->getUserConstraintId();
1517                         break;
1518                 }
1519 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1520
1521         case PHY_VEHICLE_CONSTRAINT:
1522                 {
1523                         btRaycastVehicle::btVehicleTuning* tuning = new btRaycastVehicle::btVehicleTuning();
1524                         btRigidBody* chassis = rb0;
1525                         btDefaultVehicleRaycaster* raycaster = new btDefaultVehicleRaycaster(m_dynamicsWorld);
1526                         btRaycastVehicle* vehicle = new btRaycastVehicle(*tuning,chassis,raycaster);
1527                         WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
1528                         m_wrapperVehicles.push_back(wrapperVehicle);
1529                         m_dynamicsWorld->addVehicle(vehicle);
1530                         vehicle->setUserConstraintId(gConstraintUid++);
1531                         vehicle->setUserConstraintType(type);
1532                         return vehicle->getUserConstraintId();
1533
1534                         break;
1535                 };
1536 #endif //NEW_BULLET_VEHICLE_SUPPORT
1537
1538         default:
1539                 {
1540                 }
1541         };
1542
1543         //btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB
1544
1545         return 0;
1546
1547 }
1548
1549
1550
1551 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float coneradius,float coneheight)
1552 {
1553         CcdConstructionInfo     cinfo;
1554
1555         // we don't need a CcdShapeConstructionInfo for this shape:
1556         // it is simple enough for the standard copy constructor (see CcdPhysicsController::GetReplica)
1557         cinfo.m_collisionShape = new btConeShape(coneradius,coneheight);
1558         cinfo.m_MotionState = 0;
1559         cinfo.m_physicsEnv = this;
1560         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE;
1561         DefaultMotionState* motionState = new DefaultMotionState();
1562         cinfo.m_MotionState = motionState;
1563         
1564         // we will add later the possibility to select the filter from option
1565         cinfo.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
1566         cinfo.m_collisionFilterGroup = CcdConstructionInfo::SensorFilter;
1567         motionState->m_worldTransform.setIdentity();
1568 //      motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
1569
1570         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
1571
1572
1573         return sphereController;
1574 }
1575         
1576 float           CcdPhysicsEnvironment::getAppliedImpulse(int    constraintid)
1577 {
1578         int i;
1579         int numConstraints = m_dynamicsWorld->getNumConstraints();
1580         for (i=0;i<numConstraints;i++)
1581         {
1582                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
1583                 if (constraint->getUserConstraintId() == constraintid)
1584                 {
1585                         return constraint->getAppliedImpulse();
1586                 }
1587         }
1588
1589         return 0.f;
1590 }