style cleanup
[blender.git] / source / gameengine / Physics / Bullet / CcdPhysicsEnvironment.cpp
1 /** \file gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
2  *  \ingroup physbullet
3  */
4 /*
5 Bullet Continuous Collision Detection and Physics Library
6 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
7
8 This software is provided 'as-is', without any express or implied warranty.
9 In no event will the authors be held liable for any damages arising from the use of this software.
10 Permission is granted to anyone to use this software for any purpose, 
11 including commercial applications, and to alter it and redistribute it freely, 
12 subject to the following restrictions:
13
14 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.
15 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
16 3. This notice may not be removed or altered from any source distribution.
17 */
18
19
20
21
22 #include "CcdPhysicsEnvironment.h"
23 #include "CcdPhysicsController.h"
24 #include "CcdGraphicController.h"
25
26 #include <algorithm>
27 #include "btBulletDynamicsCommon.h"
28 #include "LinearMath/btIDebugDraw.h"
29 #include "BulletCollision/CollisionDispatch/btGhostObject.h"
30 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
31 #include "BulletSoftBody/btSoftRigidDynamicsWorld.h"
32 #include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
33 #include "BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h"
34
35 //profiling/timings
36 #include "LinearMath/btQuickprof.h"
37
38
39 #include "PHY_IMotionState.h"
40 #include "KX_GameObject.h"
41 #include "RAS_MeshObject.h"
42 #include "RAS_Polygon.h"
43 #include "RAS_TexVert.h"
44
45 #define CCD_CONSTRAINT_DISABLE_LINKED_COLLISION 0x80
46
47 bool useIslands = true;
48
49 #ifdef NEW_BULLET_VEHICLE_SUPPORT
50 #include "BulletDynamics/Vehicle/btRaycastVehicle.h"
51 #include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
52 #include "BulletDynamics/Vehicle/btWheelInfo.h"
53 #include "PHY_IVehicle.h"
54 btRaycastVehicle::btVehicleTuning       gTuning;
55
56 #endif //NEW_BULLET_VEHICLE_SUPPORT
57 #include "LinearMath/btAabbUtil2.h"
58 #include "MT_Matrix4x4.h"
59 #include "MT_Vector3.h"
60 #include "GL/glew.h"
61
62 #ifdef WIN32
63 void DrawRasterizerLine(const float* from,const float* to,int color);
64 #endif
65
66
67 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
68
69
70 #include <stdio.h>
71 #include <string.h>             // for memset
72
73 #ifdef NEW_BULLET_VEHICLE_SUPPORT
74 class WrapperVehicle : public PHY_IVehicle
75 {
76
77         btRaycastVehicle*       m_vehicle;
78         PHY_IPhysicsController* m_chassis;
79
80 public:
81
82         WrapperVehicle(btRaycastVehicle* vehicle,PHY_IPhysicsController* chassis)
83                 :m_vehicle(vehicle),
84                 m_chassis(chassis)
85         {
86         }
87
88         btRaycastVehicle*       GetVehicle()
89         {
90                 return m_vehicle;
91         }
92
93         PHY_IPhysicsController* GetChassis()
94         {
95                 return m_chassis;
96         }
97
98         virtual void    AddWheel(
99                 PHY_IMotionState*       motionState,
100                 PHY__Vector3    connectionPoint,
101                 PHY__Vector3    downDirection,
102                 PHY__Vector3    axleDirection,
103                 float   suspensionRestLength,
104                 float wheelRadius,
105                 bool hasSteering
106                 )
107         {
108                 btVector3 connectionPointCS0(connectionPoint[0],connectionPoint[1],connectionPoint[2]);
109                 btVector3 wheelDirectionCS0(downDirection[0],downDirection[1],downDirection[2]);
110                 btVector3 wheelAxle(axleDirection[0],axleDirection[1],axleDirection[2]);
111
112
113                 btWheelInfo& info = m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxle,
114                         suspensionRestLength,wheelRadius,gTuning,hasSteering);
115                 info.m_clientInfo = motionState;
116
117         }
118
119         void    SyncWheels()
120         {
121                 int numWheels = GetNumWheels();
122                 int i;
123                 for (i=0;i<numWheels;i++)
124                 {
125                         btWheelInfo& info = m_vehicle->getWheelInfo(i);
126                         PHY_IMotionState* motionState = (PHY_IMotionState*)info.m_clientInfo;
127         //              m_vehicle->updateWheelTransformsWS(info,false);
128                         m_vehicle->updateWheelTransform(i,false);
129                         btTransform trans = m_vehicle->getWheelInfo(i).m_worldTransform;
130                         btQuaternion orn = trans.getRotation();
131                         const btVector3& pos = trans.getOrigin();
132                         motionState->setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]);
133                         motionState->setWorldPosition(pos.x(),pos.y(),pos.z());
134
135                 }
136         }
137
138         virtual int             GetNumWheels() const
139         {
140                 return m_vehicle->getNumWheels();
141         }
142
143         virtual void    GetWheelPosition(int wheelIndex,float& posX,float& posY,float& posZ) const
144         {
145                 btTransform     trans = m_vehicle->getWheelTransformWS(wheelIndex);
146                 posX = trans.getOrigin().x();
147                 posY = trans.getOrigin().y();
148                 posZ = trans.getOrigin().z();
149         }
150         virtual void    GetWheelOrientationQuaternion(int wheelIndex,float& quatX,float& quatY,float& quatZ,float& quatW) const
151         {
152                 btTransform     trans = m_vehicle->getWheelTransformWS(wheelIndex);
153                 btQuaternion quat = trans.getRotation();
154                 btMatrix3x3 orn2(quat);
155
156                 quatX = trans.getRotation().x();
157                 quatY = trans.getRotation().y();
158                 quatZ = trans.getRotation().z();
159                 quatW = trans.getRotation()[3];
160
161
162                 //printf("test");
163
164
165         }
166
167         virtual float   GetWheelRotation(int wheelIndex) const
168         {
169                 float rotation = 0.f;
170
171                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
172                 {
173                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
174                         rotation = info.m_rotation;
175                 }
176                 return rotation;
177
178         }
179
180
181
182         virtual int     GetUserConstraintId() const
183         {
184                 return m_vehicle->getUserConstraintId();
185         }
186
187         virtual int     GetUserConstraintType() const
188         {
189                 return m_vehicle->getUserConstraintType();
190         }
191
192         virtual void    SetSteeringValue(float steering,int wheelIndex)
193         {
194                 m_vehicle->setSteeringValue(steering,wheelIndex);
195         }
196
197         virtual void    ApplyEngineForce(float force,int wheelIndex)
198         {
199                 m_vehicle->applyEngineForce(force,wheelIndex);
200         }
201
202         virtual void    ApplyBraking(float braking,int wheelIndex)
203         {
204                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
205                 {
206                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
207                         info.m_brake = braking;
208                 }
209         }
210
211         virtual void    SetWheelFriction(float friction,int wheelIndex)
212         {
213                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
214                 {
215                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
216                         info.m_frictionSlip = friction;
217                 }
218
219         }
220
221         virtual void    SetSuspensionStiffness(float suspensionStiffness,int wheelIndex)
222         {
223                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
224                 {
225                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
226                         info.m_suspensionStiffness = suspensionStiffness;
227
228                 }
229         }
230
231         virtual void    SetSuspensionDamping(float suspensionDamping,int wheelIndex)
232         {
233                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
234                 {
235                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
236                         info.m_wheelsDampingRelaxation = suspensionDamping;
237                 }
238         }
239
240         virtual void    SetSuspensionCompression(float suspensionCompression,int wheelIndex)
241         {
242                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
243                 {
244                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
245                         info.m_wheelsDampingCompression = suspensionCompression;
246                 }
247         }
248
249
250
251         virtual void    SetRollInfluence(float rollInfluence,int wheelIndex)
252         {
253                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
254                 {
255                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
256                         info.m_rollInfluence = rollInfluence;
257                 }
258         }
259
260         virtual void    SetCoordinateSystem(int rightIndex,int upIndex,int forwardIndex)
261         {
262                 m_vehicle->setCoordinateSystem(rightIndex,upIndex,forwardIndex);
263         }
264
265
266
267 };
268 #endif //NEW_BULLET_VEHICLE_SUPPORT
269
270 class CcdOverlapFilterCallBack : public btOverlapFilterCallback
271 {
272 private:
273         class CcdPhysicsEnvironment* m_physEnv;
274 public:
275         CcdOverlapFilterCallBack(CcdPhysicsEnvironment* env) : 
276                 m_physEnv(env)
277         {
278         }
279         virtual ~CcdOverlapFilterCallBack()
280         {
281         }
282         // return true when pairs need collision
283         virtual bool    needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const;
284 };
285
286
287 void CcdPhysicsEnvironment::setDebugDrawer(btIDebugDraw* debugDrawer)
288 {
289         if (debugDrawer && m_dynamicsWorld)
290                 m_dynamicsWorld->setDebugDrawer(debugDrawer);
291         m_debugDrawer = debugDrawer;
292 }
293
294 #if 0
295 static void DrawAabb(btIDebugDraw* debugDrawer,const btVector3& from,const btVector3& to,const btVector3& color)
296 {
297         btVector3 halfExtents = (to-from)* 0.5f;
298         btVector3 center = (to+from) *0.5f;
299         int i,j;
300
301         btVector3 edgecoord(1.f,1.f,1.f),pa,pb;
302         for (i=0;i<4;i++)
303         {
304                 for (j=0;j<3;j++)
305                 {
306                         pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],                
307                                 edgecoord[2]*halfExtents[2]);
308                         pa+=center;
309
310                         int othercoord = j%3;
311                         edgecoord[othercoord]*=-1.f;
312                         pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],        
313                                 edgecoord[2]*halfExtents[2]);
314                         pb+=center;
315
316                         debugDrawer->drawLine(pa,pb,color);
317                 }
318                 edgecoord = btVector3(-1.f,-1.f,-1.f);
319                 if (i<3)
320                         edgecoord[i]*=-1.f;
321         }
322 }
323 #endif
324
325
326
327
328
329 CcdPhysicsEnvironment::CcdPhysicsEnvironment(bool useDbvtCulling,btDispatcher* dispatcher,btOverlappingPairCache* pairCache)
330 :m_cullingCache(NULL),
331 m_cullingTree(NULL),
332 m_numIterations(10),
333 m_numTimeSubSteps(1),
334 m_ccdMode(0),
335 m_solverType(-1),
336 m_profileTimings(0),
337 m_enableSatCollisionDetection(false),
338 m_solver(NULL),
339 m_ownPairCache(NULL),
340 m_filterCallback(NULL),
341 m_ownDispatcher(NULL),
342 m_scalingPropagated(false)
343 {
344
345         for (int i=0;i<PHY_NUM_RESPONSE;i++)
346         {
347                 m_triggerCallbacks[i] = 0;
348         }
349
350 //      m_collisionConfiguration = new btDefaultCollisionConfiguration();
351         m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
352         //m_collisionConfiguration->setConvexConvexMultipointIterations();
353
354         if (!dispatcher)
355         {
356                 btCollisionDispatcher* disp = new btCollisionDispatcher(m_collisionConfiguration);
357                 dispatcher = disp;
358                 btGImpactCollisionAlgorithm::registerAlgorithm(disp);
359                 m_ownDispatcher = dispatcher;
360         }
361
362         //m_broadphase = new btAxisSweep3(btVector3(-1000,-1000,-1000),btVector3(1000,1000,1000));
363         //m_broadphase = new btSimpleBroadphase();
364         m_broadphase = new btDbvtBroadphase();
365         // avoid any collision in the culling tree
366         if (useDbvtCulling) {
367                 m_cullingCache = new btNullPairCache();
368                 m_cullingTree = new btDbvtBroadphase(m_cullingCache);
369         }
370
371         m_filterCallback = new CcdOverlapFilterCallBack(this);
372         m_broadphase->getOverlappingPairCache()->setOverlapFilterCallback(m_filterCallback);
373         m_broadphase->getOverlappingPairCache()->setInternalGhostPairCallback(new btGhostPairCallback());
374
375         setSolverType(1);//issues with quickstep and memory allocations
376 //      m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
377         m_dynamicsWorld = new btSoftRigidDynamicsWorld(dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
378         //m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.01f;
379         //m_dynamicsWorld->getSolverInfo().m_solverMode=        SOLVER_USE_WARMSTARTING +       SOLVER_USE_2_FRICTION_DIRECTIONS +      SOLVER_RANDMIZE_ORDER + SOLVER_USE_FRICTION_WARMSTARTING;
380
381         m_debugDrawer = 0;
382         setGravity(0.f,0.f,-9.81f);
383 }
384
385 void    CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
386 {
387         btRigidBody* body = ctrl->GetRigidBody();
388         btCollisionObject* obj = ctrl->GetCollisionObject();
389
390         //this m_userPointer is just used for triggers, see CallbackTriggers
391         obj->setUserPointer(ctrl);
392         if (body)
393                 body->setGravity( m_gravity );
394
395         m_controllers.insert(ctrl);
396
397         if (body)
398         {
399                 //use explicit group/filter for finer control over collision in bullet => near/radar sensor
400                 m_dynamicsWorld->addRigidBody(body, ctrl->GetCollisionFilterGroup(), ctrl->GetCollisionFilterMask());
401         } else
402         {
403                 if (ctrl->GetSoftBody())
404                 {
405                         btSoftBody* softBody = ctrl->GetSoftBody();
406                         m_dynamicsWorld->addSoftBody(softBody);
407                 } else
408                 {
409                         if (obj->getCollisionShape())
410                         {
411                                 m_dynamicsWorld->addCollisionObject(obj, ctrl->GetCollisionFilterGroup(), ctrl->GetCollisionFilterMask());
412                         }
413                         if (ctrl->GetCharacterController())
414                         {
415                                 m_dynamicsWorld->addAction(ctrl->GetCharacterController());
416                         }
417                 }
418         }
419         if (obj->isStaticOrKinematicObject())
420         {
421                 obj->setActivationState(ISLAND_SLEEPING);
422         }
423
424         assert(obj->getBroadphaseHandle());
425 }
426
427                 
428
429 bool    CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctrl)
430 {
431         //also remove constraint
432         btRigidBody* body = ctrl->GetRigidBody();
433         if (body)
434         {
435                 for (int i=body->getNumConstraintRefs()-1;i>=0;i--)
436                 {
437                         btTypedConstraint* con = body->getConstraintRef(i);
438                         m_dynamicsWorld->removeConstraint(con);
439                         body->removeConstraintRef(con);
440                         //delete con; //might be kept by python KX_ConstraintWrapper
441                 }
442                 m_dynamicsWorld->removeRigidBody(ctrl->GetRigidBody());
443         } else
444         {
445                 //if a softbody
446                 if (ctrl->GetSoftBody())
447                 {
448                         m_dynamicsWorld->removeSoftBody(ctrl->GetSoftBody());
449                 } else
450                 {
451                         m_dynamicsWorld->removeCollisionObject(ctrl->GetCollisionObject());
452                 }
453         }
454         if (ctrl->m_registerCount != 0)
455                 printf("Warning: removing controller with non-zero m_registerCount: %d\n", ctrl->m_registerCount);
456
457         //remove it from the triggers
458         m_triggerControllers.erase(ctrl);
459
460         return (m_controllers.erase(ctrl) != 0);
461 }
462
463 void    CcdPhysicsEnvironment::updateCcdPhysicsController(CcdPhysicsController* ctrl, btScalar newMass, int newCollisionFlags, short int newCollisionGroup, short int newCollisionMask)
464 {
465         // this function is used when the collisionning group of a controller is changed
466         // remove and add the collistioning object
467         btRigidBody* body = ctrl->GetRigidBody();
468         btCollisionObject* obj = ctrl->GetCollisionObject();
469         if (obj)
470         {
471                 btVector3 inertia(0.0,0.0,0.0);
472                 m_dynamicsWorld->removeCollisionObject(obj);
473                 obj->setCollisionFlags(newCollisionFlags);
474                 if (body)
475                 {
476                         if (newMass)
477                                 body->getCollisionShape()->calculateLocalInertia(newMass, inertia);
478                         body->setMassProps(newMass, inertia);
479                         m_dynamicsWorld->addRigidBody(body, newCollisionGroup, newCollisionMask);
480                 }       
481                 else
482                 {
483                         m_dynamicsWorld->addCollisionObject(obj, newCollisionGroup, newCollisionMask);
484                 }
485         }
486         // to avoid nasty interaction, we must update the property of the controller as well
487         ctrl->m_cci.m_mass = newMass;
488         ctrl->m_cci.m_collisionFilterGroup = newCollisionGroup;
489         ctrl->m_cci.m_collisionFilterMask = newCollisionMask;
490         ctrl->m_cci.m_collisionFlags = newCollisionFlags;
491 }
492
493 void CcdPhysicsEnvironment::enableCcdPhysicsController(CcdPhysicsController* ctrl)
494 {
495         if (m_controllers.insert(ctrl).second)
496         {
497                 btCollisionObject* obj = ctrl->GetCollisionObject();
498                 obj->setUserPointer(ctrl);
499                 // update the position of the object from the user
500                 if (ctrl->GetMotionState()) 
501                 {
502                         btTransform xform = CcdPhysicsController::GetTransformFromMotionState(ctrl->GetMotionState());
503                         ctrl->SetCenterOfMassTransform(xform);
504                 }
505                 m_dynamicsWorld->addCollisionObject(obj, 
506                         ctrl->GetCollisionFilterGroup(), ctrl->GetCollisionFilterMask());
507         }
508 }
509
510 void CcdPhysicsEnvironment::disableCcdPhysicsController(CcdPhysicsController* ctrl)
511 {
512         if (m_controllers.erase(ctrl))
513         {
514                 btRigidBody* body = ctrl->GetRigidBody();
515                 if (body)
516                 {
517                         m_dynamicsWorld->removeRigidBody(body);
518                 } else
519                 {
520                         if (ctrl->GetSoftBody())
521                         {
522                         } else
523                         {
524                                 m_dynamicsWorld->removeCollisionObject(ctrl->GetCollisionObject());
525                         }
526                 }
527         }
528 }
529
530 void CcdPhysicsEnvironment::refreshCcdPhysicsController(CcdPhysicsController* ctrl)
531 {
532         btCollisionObject* obj = ctrl->GetCollisionObject();
533         if (obj)
534         {
535                 btBroadphaseProxy* proxy = obj->getBroadphaseHandle();
536                 if (proxy)
537                 {
538                         m_dynamicsWorld->getPairCache()->cleanProxyFromPairs(proxy,m_dynamicsWorld->getDispatcher());
539                 }
540         }
541 }
542
543 void CcdPhysicsEnvironment::addCcdGraphicController(CcdGraphicController* ctrl)
544 {
545         if (m_cullingTree && !ctrl->getBroadphaseHandle())
546         {
547                 btVector3       minAabb;
548                 btVector3       maxAabb;
549                 ctrl->getAabb(minAabb, maxAabb);
550
551                 ctrl->setBroadphaseHandle(m_cullingTree->createProxy(
552                                 minAabb,
553                                 maxAabb,
554                                 INVALID_SHAPE_PROXYTYPE,        // this parameter is not used
555                                 ctrl,
556                                 0,                                                      // this object does not collision with anything
557                                 0,
558                                 NULL,                                           // dispatcher => this parameter is not used
559                                 0));
560
561                 assert(ctrl->getBroadphaseHandle());
562         }
563 }
564
565 void CcdPhysicsEnvironment::removeCcdGraphicController(CcdGraphicController* ctrl)
566 {
567         if (m_cullingTree)
568         {
569                 btBroadphaseProxy* bp = ctrl->getBroadphaseHandle();
570                 if (bp)
571                 {
572                         m_cullingTree->destroyProxy(bp,NULL);
573                         ctrl->setBroadphaseHandle(0);
574                 }
575         }
576 }
577
578 void    CcdPhysicsEnvironment::beginFrame()
579 {
580
581 }
582
583 void CcdPhysicsEnvironment::debugDrawWorld()
584 {
585         if (m_dynamicsWorld->getDebugDrawer() &&  m_dynamicsWorld->getDebugDrawer()->getDebugMode() >0)
586                         m_dynamicsWorld->debugDrawWorld();
587 }
588
589 bool    CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep,float interval)
590 {
591         std::set<CcdPhysicsController*>::iterator it;
592         int i;
593
594         for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
595         {
596                 (*it)->SynchronizeMotionStates(timeStep);
597         }
598
599         float subStep = timeStep / float(m_numTimeSubSteps);
600         i = m_dynamicsWorld->stepSimulation(interval,25,subStep);//perform always a full simulation step
601 //uncomment next line to see where Bullet spend its time (printf in console)    
602 //CProfileManager::dumpAll();
603
604         processFhSprings(curTime,i*subStep);
605
606         for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
607         {
608                 (*it)->SynchronizeMotionStates(timeStep);
609         }
610
611         //for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
612         //{
613         //      (*it)->SynchronizeMotionStates(timeStep);
614         //}
615
616         for (i=0;i<m_wrapperVehicles.size();i++)
617         {
618                 WrapperVehicle* veh = m_wrapperVehicles[i];
619                 veh->SyncWheels();
620         }
621
622
623         CallbackTriggers();
624
625         return true;
626 }
627
628 class ClosestRayResultCallbackNotMe : public btCollisionWorld::ClosestRayResultCallback
629 {
630         btCollisionObject* m_owner;
631         btCollisionObject* m_parent;
632
633 public:
634         ClosestRayResultCallbackNotMe(const btVector3& rayFromWorld,const btVector3& rayToWorld,btCollisionObject* owner,btCollisionObject* parent)
635                 :btCollisionWorld::ClosestRayResultCallback(rayFromWorld,rayToWorld),
636                 m_owner(owner),
637                 m_parent(parent)
638         {
639                 
640         }
641
642         virtual bool needsCollision(btBroadphaseProxy* proxy0) const
643         {
644                 //don't collide with self
645                 if (proxy0->m_clientObject == m_owner)
646                         return false;
647
648                 if (proxy0->m_clientObject == m_parent)
649                         return false;
650
651                 return btCollisionWorld::ClosestRayResultCallback::needsCollision(proxy0);
652         }
653
654 };
655
656 void    CcdPhysicsEnvironment::processFhSprings(double curTime,float interval)
657 {
658         std::set<CcdPhysicsController*>::iterator it;
659         // dynamic of Fh spring is based on a timestep of 1/60
660         int numIter = (int)(interval*60.0001f);
661         
662         for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
663         {
664                 CcdPhysicsController* ctrl = (*it);
665                 btRigidBody* body = ctrl->GetRigidBody();
666
667                 if (body && (ctrl->getConstructionInfo().m_do_fh || ctrl->getConstructionInfo().m_do_rot_fh))
668                 {
669                         //printf("has Fh or RotFh\n");
670                         //re-implement SM_FhObject.cpp using btCollisionWorld::rayTest and info from ctrl->getConstructionInfo()
671                         //send a ray from {0.0, 0.0, 0.0} towards {0.0, 0.0, -10.0}, in local coordinates
672                         CcdPhysicsController* parentCtrl = ctrl->getParentCtrl();
673                         btRigidBody* parentBody = parentCtrl?parentCtrl->GetRigidBody() : 0;
674                         btRigidBody* cl_object = parentBody ? parentBody : body;
675
676                         if (body->isStaticOrKinematicObject())
677                                 continue;
678
679                         btVector3 rayDirLocal(0,0,-10);
680
681                         //m_dynamicsWorld
682                         //ctrl->GetRigidBody();
683                         btVector3 rayFromWorld = body->getCenterOfMassPosition();
684                         //btVector3     rayToWorld = rayFromWorld + body->getCenterOfMassTransform().getBasis() * rayDirLocal;
685                         //ray always points down the z axis in world space...
686                         btVector3       rayToWorld = rayFromWorld + rayDirLocal;
687                         
688                         ClosestRayResultCallbackNotMe   resultCallback(rayFromWorld,rayToWorld,body,parentBody);
689
690                         m_dynamicsWorld->rayTest(rayFromWorld,rayToWorld,resultCallback);
691                         if (resultCallback.hasHit())
692                         {
693                                 //we hit this one: resultCallback.m_collisionObject;
694                                 CcdPhysicsController* controller = static_cast<CcdPhysicsController*>(resultCallback.m_collisionObject->getUserPointer());
695
696                                 if (controller)
697                                 {
698                                         if (controller->getConstructionInfo().m_fh_distance < SIMD_EPSILON)
699                                                 continue;
700
701                                         btRigidBody* hit_object = controller->GetRigidBody();
702                                         if (!hit_object)
703                                                 continue;
704
705                                         CcdConstructionInfo& hitObjShapeProps = controller->getConstructionInfo();
706
707                                         float distance = resultCallback.m_closestHitFraction*rayDirLocal.length()-ctrl->getConstructionInfo().m_radius;
708                                         if (distance >= hitObjShapeProps.m_fh_distance)
709                                                 continue;
710                                         
711                                         
712
713                                         //btVector3 ray_dir = cl_object->getCenterOfMassTransform().getBasis()* rayDirLocal.normalized();
714                                         btVector3 ray_dir = rayDirLocal.normalized();
715                                         btVector3 normal = resultCallback.m_hitNormalWorld;
716                                         normal.normalize();
717
718                                         for (int i=0; i<numIter; i++)
719                                         {
720                                                 if (ctrl->getConstructionInfo().m_do_fh) 
721                                                 {
722                                                         btVector3 lspot = cl_object->getCenterOfMassPosition() +
723                                                                 rayDirLocal * resultCallback.m_closestHitFraction;
724
725
726                                                                 
727
728                                                         lspot -= hit_object->getCenterOfMassPosition();
729                                                         btVector3 rel_vel = cl_object->getLinearVelocity() - hit_object->getVelocityInLocalPoint(lspot);
730                                                         btScalar rel_vel_ray = ray_dir.dot(rel_vel);
731                                                         btScalar spring_extent = 1.0 - distance / hitObjShapeProps.m_fh_distance; 
732                                                         
733                                                         btScalar i_spring = spring_extent * hitObjShapeProps.m_fh_spring;
734                                                         btScalar i_damp =   rel_vel_ray * hitObjShapeProps.m_fh_damping;
735                                                         
736                                                         cl_object->setLinearVelocity(cl_object->getLinearVelocity() + (-(i_spring + i_damp) * ray_dir)); 
737                                                         if (hitObjShapeProps.m_fh_normal) 
738                                                         {
739                                                                 cl_object->setLinearVelocity(cl_object->getLinearVelocity()+(i_spring + i_damp) *(normal - normal.dot(ray_dir) * ray_dir));
740                                                         }
741                                                         
742                                                         btVector3 lateral = rel_vel - rel_vel_ray * ray_dir;
743                                                         
744                                                         
745                                                         if (ctrl->getConstructionInfo().m_do_anisotropic) {
746                                                                 //Bullet basis contains no scaling/shear etc.
747                                                                 const btMatrix3x3& lcs = cl_object->getCenterOfMassTransform().getBasis();
748                                                                 btVector3 loc_lateral = lateral * lcs;
749                                                                 const btVector3& friction_scaling = cl_object->getAnisotropicFriction();
750                                                                 loc_lateral *= friction_scaling;
751                                                                 lateral = lcs * loc_lateral;
752                                                         }
753
754                                                         btScalar rel_vel_lateral = lateral.length();
755                                                         
756                                                         if (rel_vel_lateral > SIMD_EPSILON) {
757                                                                 btScalar friction_factor = hit_object->getFriction();//cl_object->getFriction();
758
759                                                                 btScalar max_friction = friction_factor * btMax(btScalar(0.0), i_spring);
760                                                                 
761                                                                 btScalar rel_mom_lateral = rel_vel_lateral / cl_object->getInvMass();
762                                                                 
763                                                                 btVector3 friction = (rel_mom_lateral > max_friction) ?
764                                                                         -lateral * (max_friction / rel_vel_lateral) :
765                                                                         -lateral;
766                                                                 
767                                                                         cl_object->applyCentralImpulse(friction);
768                                                         }
769                                                 }
770
771                                                 
772                                                 if (ctrl->getConstructionInfo().m_do_rot_fh) {
773                                                         btVector3 up2 = cl_object->getWorldTransform().getBasis().getColumn(2);
774
775                                                         btVector3 t_spring = up2.cross(normal) * hitObjShapeProps.m_fh_spring;
776                                                         btVector3 ang_vel = cl_object->getAngularVelocity();
777                                                         
778                                                         // only rotations that tilt relative to the normal are damped
779                                                         ang_vel -= ang_vel.dot(normal) * normal;
780                                                         
781                                                         btVector3 t_damp = ang_vel * hitObjShapeProps.m_fh_damping;  
782                                                         
783                                                         cl_object->setAngularVelocity(cl_object->getAngularVelocity() + (t_spring - t_damp));
784                                                 }
785                                         }
786                                 }
787                         }
788                 }
789         }
790 }
791
792 void            CcdPhysicsEnvironment::setDebugMode(int debugMode)
793 {
794         if (m_debugDrawer) {
795                 m_debugDrawer->setDebugMode(debugMode);
796         }
797 }
798
799 void            CcdPhysicsEnvironment::setNumIterations(int numIter)
800 {
801         m_numIterations = numIter;
802 }
803 void            CcdPhysicsEnvironment::setDeactivationTime(float dTime)
804 {
805         gDeactivationTime = dTime;
806 }
807 void            CcdPhysicsEnvironment::setDeactivationLinearTreshold(float linTresh)
808 {
809         gLinearSleepingTreshold = linTresh;
810 }
811 void            CcdPhysicsEnvironment::setDeactivationAngularTreshold(float angTresh) 
812 {
813         gAngularSleepingTreshold = angTresh;
814 }
815
816 void            CcdPhysicsEnvironment::setContactBreakingTreshold(float contactBreakingTreshold)
817 {
818         gContactBreakingThreshold = contactBreakingTreshold;
819
820 }
821
822
823 void            CcdPhysicsEnvironment::setCcdMode(int ccdMode)
824 {
825         m_ccdMode = ccdMode;
826 }
827
828
829 void            CcdPhysicsEnvironment::setSolverSorConstant(float sor)
830 {
831         m_solverInfo.m_sor = sor;
832 }
833
834 void            CcdPhysicsEnvironment::setSolverTau(float tau)
835 {
836         m_solverInfo.m_tau = tau;
837 }
838 void            CcdPhysicsEnvironment::setSolverDamping(float damping)
839 {
840         m_solverInfo.m_damping = damping;
841 }
842
843
844 void            CcdPhysicsEnvironment::setLinearAirDamping(float damping)
845 {
846         //gLinearAirDamping = damping;
847 }
848
849 void            CcdPhysicsEnvironment::setUseEpa(bool epa)
850 {
851         //gUseEpa = epa;
852 }
853
854 void            CcdPhysicsEnvironment::setSolverType(int solverType)
855 {
856
857         switch (solverType)
858         {
859         case 1:
860                 {
861                         if (m_solverType != solverType)
862                         {
863
864                                 m_solver = new btSequentialImpulseConstraintSolver();
865                                 
866                                 
867                                 break;
868                         }
869                 }
870
871         case 0:
872         default:
873                 if (m_solverType != solverType)
874                 {
875 //                      m_solver = new OdeConstraintSolver();
876
877                         break;
878                 }
879
880         };
881
882         m_solverType = solverType;
883 }
884
885
886
887 void            CcdPhysicsEnvironment::getGravity(PHY__Vector3& grav)
888 {
889                 const btVector3& gravity = m_dynamicsWorld->getGravity();
890                 grav[0] = gravity.getX();
891                 grav[1] = gravity.getY();
892                 grav[2] = gravity.getZ();
893 }
894
895
896 void            CcdPhysicsEnvironment::setGravity(float x,float y,float z)
897 {
898         m_gravity = btVector3(x,y,z);
899         m_dynamicsWorld->setGravity(m_gravity);
900         m_dynamicsWorld->getWorldInfo().m_gravity.setValue(x,y,z);
901 }
902
903
904
905
906 static int gConstraintUid = 1;
907
908 //Following the COLLADA physics specification for constraints
909 int                     CcdPhysicsEnvironment::createUniversalD6Constraint(
910                                                 class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
911                                                 btTransform& frameInA,
912                                                 btTransform& frameInB,
913                                                 const btVector3& linearMinLimits,
914                                                 const btVector3& linearMaxLimits,
915                                                 const btVector3& angularMinLimits,
916                                                 const btVector3& angularMaxLimits,int flags
917 )
918 {
919
920         bool disableCollisionBetweenLinkedBodies = (0!=(flags & CCD_CONSTRAINT_DISABLE_LINKED_COLLISION));
921
922         //we could either add some logic to recognize ball-socket and hinge, or let that up to the user
923         //perhaps some warning or hint that hinge/ball-socket is more efficient?
924         
925         
926         btGeneric6DofConstraint* genericConstraint = 0;
927         CcdPhysicsController* ctrl0 = (CcdPhysicsController*) ctrlRef;
928         CcdPhysicsController* ctrl1 = (CcdPhysicsController*) ctrlOther;
929         
930         btRigidBody* rb0 = ctrl0->GetRigidBody();
931         btRigidBody* rb1 = ctrl1->GetRigidBody();
932
933         if (rb1)
934         {
935                 
936
937                 bool useReferenceFrameA = true;
938                 genericConstraint = new btGeneric6DofSpringConstraint(
939                         *rb0,*rb1,
940                         frameInA,frameInB,useReferenceFrameA);
941                 genericConstraint->setLinearLowerLimit(linearMinLimits);
942                 genericConstraint->setLinearUpperLimit(linearMaxLimits);
943                 genericConstraint->setAngularLowerLimit(angularMinLimits);
944                 genericConstraint->setAngularUpperLimit(angularMaxLimits);
945         } else
946         {
947                 // TODO: Implement single body case...
948                 //No, we can use a fixed rigidbody in above code, rather than unnecessary duplation of code
949
950         }
951         
952         if (genericConstraint)
953         {
954         //      m_constraints.push_back(genericConstraint);
955                 m_dynamicsWorld->addConstraint(genericConstraint,disableCollisionBetweenLinkedBodies);
956
957                 genericConstraint->setUserConstraintId(gConstraintUid++);
958                 genericConstraint->setUserConstraintType(PHY_GENERIC_6DOF_CONSTRAINT);
959                 //64 bit systems can't cast pointer to int. could use size_t instead.
960                 return genericConstraint->getUserConstraintId();
961         }
962         return 0;
963 }
964
965
966
967 void            CcdPhysicsEnvironment::removeConstraint(int     constraintId)
968 {
969         
970         int i;
971         int numConstraints = m_dynamicsWorld->getNumConstraints();
972         for (i=0;i<numConstraints;i++)
973         {
974                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
975                 if (constraint->getUserConstraintId() == constraintId)
976                 {
977                         constraint->getRigidBodyA().activate();
978                         constraint->getRigidBodyB().activate();
979                         m_dynamicsWorld->removeConstraint(constraint);
980                         break;
981                 }
982         }
983 }
984
985
986 struct  FilterClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
987 {
988         PHY_IRayCastFilterCallback&     m_phyRayFilter;
989         const btCollisionShape*         m_hitTriangleShape;
990         int                                                     m_hitTriangleIndex;
991
992
993         FilterClosestRayResultCallback (PHY_IRayCastFilterCallback& phyRayFilter,const btVector3& rayFrom,const btVector3& rayTo)
994                 : btCollisionWorld::ClosestRayResultCallback(rayFrom,rayTo),
995                 m_phyRayFilter(phyRayFilter),
996                 m_hitTriangleShape(NULL),
997                 m_hitTriangleIndex(0)
998         {
999         }
1000
1001         virtual ~FilterClosestRayResultCallback()
1002         {
1003         }
1004
1005         virtual bool needsCollision(btBroadphaseProxy* proxy0) const
1006         {
1007                 if (!(proxy0->m_collisionFilterGroup & m_collisionFilterMask))
1008                         return false;
1009                 if (!(m_collisionFilterGroup & proxy0->m_collisionFilterMask))
1010                         return false;
1011                 btCollisionObject* object = (btCollisionObject*)proxy0->m_clientObject;
1012                 CcdPhysicsController* phyCtrl = static_cast<CcdPhysicsController*>(object->getUserPointer());
1013                 if (phyCtrl == m_phyRayFilter.m_ignoreController)
1014                         return false;
1015                 return m_phyRayFilter.needBroadphaseRayCast(phyCtrl);
1016         }
1017
1018         virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace)
1019         {
1020                 //CcdPhysicsController* curHit = static_cast<CcdPhysicsController*>(rayResult.m_collisionObject->getUserPointer());
1021                 // save shape information as ClosestRayResultCallback::AddSingleResult() does not do it
1022                 if (rayResult.m_localShapeInfo)
1023                 {
1024                         m_hitTriangleShape = rayResult.m_collisionObject->getCollisionShape();
1025                         m_hitTriangleIndex = rayResult.m_localShapeInfo->m_triangleIndex;
1026                 } else 
1027                 {
1028                         m_hitTriangleShape = NULL;
1029                         m_hitTriangleIndex = 0;
1030                 }
1031                 return ClosestRayResultCallback::addSingleResult(rayResult,normalInWorldSpace);
1032         }
1033
1034 };
1035
1036 static bool GetHitTriangle(btCollisionShape* shape, CcdShapeConstructionInfo* shapeInfo, int hitTriangleIndex, btVector3 triangle[])
1037 {
1038         // this code is copied from Bullet 
1039         const unsigned char *vertexbase;
1040         int numverts;
1041         PHY_ScalarType type;
1042         int stride;
1043         const unsigned char *indexbase;
1044         int indexstride;
1045         int numfaces;
1046         PHY_ScalarType indicestype;
1047         btStridingMeshInterface* meshInterface = NULL;
1048         btTriangleMeshShape* triangleShape = shapeInfo->GetMeshShape();
1049
1050         if (triangleShape)
1051                 meshInterface = triangleShape->getMeshInterface();
1052         else
1053         {
1054                 // other possibility is gImpact
1055                 if (shape->getShapeType() == GIMPACT_SHAPE_PROXYTYPE)
1056                         meshInterface = (static_cast<btGImpactMeshShape*>(shape))->getMeshInterface();
1057         }
1058         if (!meshInterface)
1059                 return false;
1060
1061         meshInterface->getLockedReadOnlyVertexIndexBase(
1062                 &vertexbase,
1063                 numverts,
1064                 type,
1065                 stride,
1066                 &indexbase,
1067                 indexstride,
1068                 numfaces,
1069                 indicestype,
1070                 0);
1071
1072         unsigned int* gfxbase = (unsigned int*)(indexbase+hitTriangleIndex*indexstride);
1073         const btVector3& meshScaling = shape->getLocalScaling();
1074         for (int j=2;j>=0;j--)
1075         {
1076                 int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
1077
1078                 btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride);
1079
1080                 triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());              
1081         }
1082         meshInterface->unLockReadOnlyVertexBase(0);
1083         return true;
1084 }
1085
1086 PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IRayCastFilterCallback &filterCallback, float fromX,float fromY,float fromZ, float toX,float toY,float toZ)
1087 {
1088         btVector3 rayFrom(fromX,fromY,fromZ);
1089         btVector3 rayTo(toX,toY,toZ);
1090
1091         btVector3       hitPointWorld,normalWorld;
1092
1093         //Either Ray Cast with or without filtering
1094
1095         //btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo);
1096         FilterClosestRayResultCallback   rayCallback(filterCallback,rayFrom,rayTo);
1097
1098
1099         PHY_RayCastResult result;
1100         memset(&result, 0, sizeof(result));
1101
1102         // don't collision with sensor object
1103         rayCallback.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
1104         //, ,filterCallback.m_faceNormal);
1105
1106         m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback);
1107         if (rayCallback.hasHit())
1108         {
1109                 CcdPhysicsController* controller = static_cast<CcdPhysicsController*>(rayCallback.m_collisionObject->getUserPointer());
1110                 result.m_controller = controller;
1111                 result.m_hitPoint[0] = rayCallback.m_hitPointWorld.getX();
1112                 result.m_hitPoint[1] = rayCallback.m_hitPointWorld.getY();
1113                 result.m_hitPoint[2] = rayCallback.m_hitPointWorld.getZ();
1114
1115                 if (rayCallback.m_hitTriangleShape != NULL)
1116                 {
1117                         // identify the mesh polygon
1118                         CcdShapeConstructionInfo* shapeInfo = controller->m_shapeInfo;
1119                         if (shapeInfo)
1120                         {
1121                                 btCollisionShape* shape = controller->GetCollisionObject()->getCollisionShape();
1122                                 if (shape->isCompound())
1123                                 {
1124                                         btCompoundShape* compoundShape = (btCompoundShape*)shape;
1125                                         CcdShapeConstructionInfo* compoundShapeInfo = shapeInfo;
1126                                         // need to search which sub-shape has been hit
1127                                         for (int i=0; i<compoundShape->getNumChildShapes(); i++)
1128                                         {
1129                                                 shapeInfo = compoundShapeInfo->GetChildShape(i);
1130                                                 shape=compoundShape->getChildShape(i);
1131                                                 if (shape == rayCallback.m_hitTriangleShape)
1132                                                         break;
1133                                         }
1134                                 }
1135                                 if (shape == rayCallback.m_hitTriangleShape && 
1136                                         rayCallback.m_hitTriangleIndex < shapeInfo->m_polygonIndexArray.size())
1137                                 {
1138                                         // save original collision shape triangle for soft body
1139                                         int hitTriangleIndex = rayCallback.m_hitTriangleIndex;
1140
1141                                         result.m_meshObject = shapeInfo->GetMesh();
1142                                         if (shape->isSoftBody())
1143                                         {
1144                                                 // soft body using different face numbering because of randomization
1145                                                 // hopefully we have stored the original face number in m_tag
1146                                                 btSoftBody* softBody = static_cast<btSoftBody*>(rayCallback.m_collisionObject);
1147                                                 if (softBody->m_faces[hitTriangleIndex].m_tag != 0)
1148                                                 {
1149                                                         rayCallback.m_hitTriangleIndex = (int)((uintptr_t)(softBody->m_faces[hitTriangleIndex].m_tag)-1);
1150                                                 }
1151                                         }
1152                                         // retrieve the original mesh polygon (in case of quad->tri conversion)
1153                                         result.m_polygon = shapeInfo->m_polygonIndexArray.at(rayCallback.m_hitTriangleIndex);
1154                                         // hit triangle in world coordinate, for face normal and UV coordinate
1155                                         btVector3 triangle[3];
1156                                         bool triangleOK = false;
1157                                         if (filterCallback.m_faceUV && (3*rayCallback.m_hitTriangleIndex) < shapeInfo->m_triFaceUVcoArray.size())
1158                                         {
1159                                                 // interpolate the UV coordinate of the hit point
1160                                                 CcdShapeConstructionInfo::UVco* uvCo = &shapeInfo->m_triFaceUVcoArray[3*rayCallback.m_hitTriangleIndex];
1161                                                 // 1. get the 3 coordinate of the triangle in world space
1162                                                 btVector3 v1, v2, v3;
1163                                                 if (shape->isSoftBody())
1164                                                 {
1165                                                         // soft body give points directly in world coordinate
1166                                                         btSoftBody* softBody = static_cast<btSoftBody*>(rayCallback.m_collisionObject);
1167                                                         v1 = softBody->m_faces[hitTriangleIndex].m_n[0]->m_x;
1168                                                         v2 = softBody->m_faces[hitTriangleIndex].m_n[1]->m_x;
1169                                                         v3 = softBody->m_faces[hitTriangleIndex].m_n[2]->m_x;
1170                                                 } else 
1171                                                 {
1172                                                         // for rigid body we must apply the world transform
1173                                                         triangleOK = GetHitTriangle(shape, shapeInfo, hitTriangleIndex, triangle);
1174                                                         if (!triangleOK)
1175                                                                 // if we cannot get the triangle, no use to continue
1176                                                                 goto SKIP_UV_NORMAL;
1177                                                         v1 = rayCallback.m_collisionObject->getWorldTransform()(triangle[0]);
1178                                                         v2 = rayCallback.m_collisionObject->getWorldTransform()(triangle[1]);
1179                                                         v3 = rayCallback.m_collisionObject->getWorldTransform()(triangle[2]);
1180                                                 }
1181                                                 // 2. compute barycentric coordinate of the hit point
1182                                                 btVector3 v = v2-v1;
1183                                                 btVector3 w = v3-v1;
1184                                                 btVector3 u = v.cross(w);
1185                                                 btScalar A = u.length();
1186
1187                                                 v = v2-rayCallback.m_hitPointWorld;
1188                                                 w = v3-rayCallback.m_hitPointWorld;
1189                                                 u = v.cross(w);
1190                                                 btScalar A1 = u.length();
1191
1192                                                 v = rayCallback.m_hitPointWorld-v1;
1193                                                 w = v3-v1;
1194                                                 u = v.cross(w);
1195                                                 btScalar A2 = u.length();
1196
1197                                                 btVector3 baryCo;
1198                                                 baryCo.setX(A1/A);
1199                                                 baryCo.setY(A2/A);
1200                                                 baryCo.setZ(1.0f-baryCo.getX()-baryCo.getY());
1201                                                 // 3. compute UV coordinate
1202                                                 result.m_hitUV[0] = baryCo.getX()*uvCo[0].uv[0] + baryCo.getY()*uvCo[1].uv[0] + baryCo.getZ()*uvCo[2].uv[0];
1203                                                 result.m_hitUV[1] = baryCo.getX()*uvCo[0].uv[1] + baryCo.getY()*uvCo[1].uv[1] + baryCo.getZ()*uvCo[2].uv[1];
1204                                                 result.m_hitUVOK = 1;
1205                                         }
1206                                                 
1207                                         // Bullet returns the normal from "outside".
1208                                         // If the user requests the real normal, compute it now
1209                                         if (filterCallback.m_faceNormal)
1210                                         {
1211                                                 if (shape->isSoftBody()) 
1212                                                 {
1213                                                         // we can get the real normal directly from the body
1214                                                         btSoftBody* softBody = static_cast<btSoftBody*>(rayCallback.m_collisionObject);
1215                                                         rayCallback.m_hitNormalWorld = softBody->m_faces[hitTriangleIndex].m_normal;
1216                                                 } else
1217                                                 {
1218                                                         if (!triangleOK)
1219                                                                 triangleOK = GetHitTriangle(shape, shapeInfo, hitTriangleIndex, triangle);
1220                                                         if (triangleOK)
1221                                                         {
1222                                                                 btVector3 triangleNormal; 
1223                                                                 triangleNormal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]);
1224                                                                 rayCallback.m_hitNormalWorld = rayCallback.m_collisionObject->getWorldTransform().getBasis()*triangleNormal;
1225                                                         }
1226                                                 }
1227                                         }
1228                                 SKIP_UV_NORMAL:
1229                                         ;
1230                                 }
1231                         }
1232                 }
1233                 if (rayCallback.m_hitNormalWorld.length2() > (SIMD_EPSILON*SIMD_EPSILON))
1234                 {
1235                         rayCallback.m_hitNormalWorld.normalize();
1236                 } else
1237                 {
1238                         rayCallback.m_hitNormalWorld.setValue(1,0,0);
1239                 }
1240                 result.m_hitNormal[0] = rayCallback.m_hitNormalWorld.getX();
1241                 result.m_hitNormal[1] = rayCallback.m_hitNormalWorld.getY();
1242                 result.m_hitNormal[2] = rayCallback.m_hitNormalWorld.getZ();
1243                 filterCallback.reportHit(&result);
1244         }       
1245
1246
1247         return result.m_controller;
1248 }
1249
1250 // Handles occlusion culling. 
1251 // The implementation is based on the CDTestFramework
1252 struct OcclusionBuffer
1253 {
1254         struct WriteOCL
1255         {
1256                 static inline bool Process(btScalar& q,btScalar v) { if (q<v) q=v;return(false); }
1257                 static inline void Occlusion(bool& flag) { flag = true; }
1258         };
1259         struct QueryOCL
1260         {
1261                 static inline bool Process(btScalar& q,btScalar v) { return(q<=v); }
1262                 static inline void Occlusion(bool& flag) { }
1263         };
1264         btScalar*                                               m_buffer;
1265         size_t                                                  m_bufferSize;
1266         bool                                                    m_initialized;
1267         bool                                                    m_occlusion;
1268         int                                                             m_sizes[2];
1269         btScalar                                                m_scales[2];
1270         btScalar                                                m_offsets[2];
1271         btScalar                                                m_wtc[16];              // world to clip transform
1272         btScalar                                                m_mtc[16];              // model to clip transform
1273         // constructor: size=largest dimension of the buffer. 
1274         // Buffer size depends on aspect ratio
1275         OcclusionBuffer()
1276         {
1277                 m_initialized=false;
1278                 m_occlusion = false;
1279                 m_buffer = NULL;
1280                 m_bufferSize = 0;
1281         }
1282         // multiplication of column major matrices: m=m1*m2
1283         template<typename T1, typename T2>
1284         void            CMmat4mul(btScalar* m, const T1* m1, const T2* m2)
1285         {
1286                 m[ 0] = btScalar(m1[ 0]*m2[ 0]+m1[ 4]*m2[ 1]+m1[ 8]*m2[ 2]+m1[12]*m2[ 3]);
1287                 m[ 1] = btScalar(m1[ 1]*m2[ 0]+m1[ 5]*m2[ 1]+m1[ 9]*m2[ 2]+m1[13]*m2[ 3]);
1288                 m[ 2] = btScalar(m1[ 2]*m2[ 0]+m1[ 6]*m2[ 1]+m1[10]*m2[ 2]+m1[14]*m2[ 3]);
1289                 m[ 3] = btScalar(m1[ 3]*m2[ 0]+m1[ 7]*m2[ 1]+m1[11]*m2[ 2]+m1[15]*m2[ 3]);
1290
1291                 m[ 4] = btScalar(m1[ 0]*m2[ 4]+m1[ 4]*m2[ 5]+m1[ 8]*m2[ 6]+m1[12]*m2[ 7]);
1292                 m[ 5] = btScalar(m1[ 1]*m2[ 4]+m1[ 5]*m2[ 5]+m1[ 9]*m2[ 6]+m1[13]*m2[ 7]);
1293                 m[ 6] = btScalar(m1[ 2]*m2[ 4]+m1[ 6]*m2[ 5]+m1[10]*m2[ 6]+m1[14]*m2[ 7]);
1294                 m[ 7] = btScalar(m1[ 3]*m2[ 4]+m1[ 7]*m2[ 5]+m1[11]*m2[ 6]+m1[15]*m2[ 7]);
1295
1296                 m[ 8] = btScalar(m1[ 0]*m2[ 8]+m1[ 4]*m2[ 9]+m1[ 8]*m2[10]+m1[12]*m2[11]);
1297                 m[ 9] = btScalar(m1[ 1]*m2[ 8]+m1[ 5]*m2[ 9]+m1[ 9]*m2[10]+m1[13]*m2[11]);
1298                 m[10] = btScalar(m1[ 2]*m2[ 8]+m1[ 6]*m2[ 9]+m1[10]*m2[10]+m1[14]*m2[11]);
1299                 m[11] = btScalar(m1[ 3]*m2[ 8]+m1[ 7]*m2[ 9]+m1[11]*m2[10]+m1[15]*m2[11]);
1300
1301                 m[12] = btScalar(m1[ 0]*m2[12]+m1[ 4]*m2[13]+m1[ 8]*m2[14]+m1[12]*m2[15]);
1302                 m[13] = btScalar(m1[ 1]*m2[12]+m1[ 5]*m2[13]+m1[ 9]*m2[14]+m1[13]*m2[15]);
1303                 m[14] = btScalar(m1[ 2]*m2[12]+m1[ 6]*m2[13]+m1[10]*m2[14]+m1[14]*m2[15]);
1304                 m[15] = btScalar(m1[ 3]*m2[12]+m1[ 7]*m2[13]+m1[11]*m2[14]+m1[15]*m2[15]);
1305         }
1306         void            setup(int size)
1307         {
1308                 m_initialized=false;
1309                 m_occlusion=false;
1310                 // compute the size of the buffer
1311                 GLint           v[4];
1312                 GLdouble        m[16],p[16];
1313                 int                     maxsize;
1314                 double          ratio;
1315                 glGetIntegerv(GL_VIEWPORT,v);
1316                 maxsize = (v[2] > v[3]) ? v[2] : v[3];
1317                 assert(maxsize > 0);
1318                 ratio = 1.0/(2*maxsize);
1319                 // ensure even number
1320                 m_sizes[0] = 2*((int)(size*v[2]*ratio+0.5));
1321                 m_sizes[1] = 2*((int)(size*v[3]*ratio+0.5));
1322                 m_scales[0]=btScalar(m_sizes[0]/2);
1323                 m_scales[1]=btScalar(m_sizes[1]/2);
1324                 m_offsets[0]=m_scales[0]+0.5f;
1325                 m_offsets[1]=m_scales[1]+0.5f;
1326                 // prepare matrix
1327                 // at this time of the rendering, the modelview matrix is the 
1328                 // world to camera transformation and the projection matrix is
1329                 // camera to clip transformation. combine both so that 
1330                 glGetDoublev(GL_MODELVIEW_MATRIX,m);
1331                 glGetDoublev(GL_PROJECTION_MATRIX,p);
1332                 CMmat4mul(m_wtc,p,m);
1333         }
1334         void            initialize()
1335         {
1336                 size_t newsize = (m_sizes[0]*m_sizes[1])*sizeof(btScalar);
1337                 if (m_buffer)
1338                 {
1339                         // see if we can reuse
1340                         if (newsize > m_bufferSize)
1341                         {
1342                                 free(m_buffer);
1343                                 m_buffer = NULL;
1344                                 m_bufferSize = 0;
1345                         }
1346                 }
1347                 if (!m_buffer)
1348                 {
1349                         m_buffer = (btScalar*)calloc(1, newsize);
1350                         m_bufferSize = newsize;
1351                 } else
1352                 {
1353                         // buffer exists already, just clears it
1354                         memset(m_buffer, 0, newsize);
1355                 }
1356                 // memory allocate must succeed
1357                 assert(m_buffer != NULL);
1358                 m_initialized = true;
1359                 m_occlusion = false;
1360         }
1361         void            SetModelMatrix(double *fl)
1362         {
1363                 CMmat4mul(m_mtc,m_wtc,fl);
1364                 if (!m_initialized)
1365                         initialize();
1366         }
1367
1368         // transform a segment in world coordinate to clip coordinate
1369         void            transformW(const btVector3& x, btVector4& t)
1370         {
1371                 t[0]    =       x[0]*m_wtc[0]+x[1]*m_wtc[4]+x[2]*m_wtc[8]+m_wtc[12];
1372                 t[1]    =       x[0]*m_wtc[1]+x[1]*m_wtc[5]+x[2]*m_wtc[9]+m_wtc[13];
1373                 t[2]    =       x[0]*m_wtc[2]+x[1]*m_wtc[6]+x[2]*m_wtc[10]+m_wtc[14];
1374                 t[3]    =       x[0]*m_wtc[3]+x[1]*m_wtc[7]+x[2]*m_wtc[11]+m_wtc[15];
1375         }
1376         void            transformM(const float* x, btVector4& t)
1377         {
1378                 t[0]    =       x[0]*m_mtc[0]+x[1]*m_mtc[4]+x[2]*m_mtc[8]+m_mtc[12];
1379                 t[1]    =       x[0]*m_mtc[1]+x[1]*m_mtc[5]+x[2]*m_mtc[9]+m_mtc[13];
1380                 t[2]    =       x[0]*m_mtc[2]+x[1]*m_mtc[6]+x[2]*m_mtc[10]+m_mtc[14];
1381                 t[3]    =       x[0]*m_mtc[3]+x[1]*m_mtc[7]+x[2]*m_mtc[11]+m_mtc[15];
1382         }
1383         // convert polygon to device coordinates
1384         static bool     project(btVector4* p,int n)
1385         {
1386                 for (int i=0;i<n;++i)
1387                 {                       
1388                         p[i][2]=1/p[i][3];
1389                         p[i][0]*=p[i][2];
1390                         p[i][1]*=p[i][2];
1391                 }
1392                 return(true);
1393         }
1394         // pi: closed polygon in clip coordinate, NP = number of segments
1395         // po: same polygon with clipped segments removed
1396         template <const int NP>
1397         static int      clip(const btVector4* pi,btVector4* po)
1398         {
1399                 btScalar        s[2*NP];
1400                 btVector4       pn[2*NP];
1401                 int                     i, j, m, n, ni;
1402                 // deal with near clipping
1403                 for (i=0, m=0;i<NP;++i)
1404                 {
1405                         s[i]=pi[i][2]+pi[i][3];
1406                         if (s[i]<0) m+=1<<i;
1407                 }
1408                 if (m==((1<<NP)-1)) 
1409                         return(0);
1410                 if (m!=0)
1411                 {
1412                         for (i=NP-1,j=0,n=0;j<NP;i=j++)
1413                         {
1414                                 const btVector4&        a=pi[i];
1415                                 const btVector4&        b=pi[j];
1416                                 const btScalar          t=s[i]/(a[3]+a[2]-b[3]-b[2]);
1417                                 if ((t>0)&&(t<1))
1418                                 {
1419                                         pn[n][0]        =       a[0]+(b[0]-a[0])*t;
1420                                         pn[n][1]        =       a[1]+(b[1]-a[1])*t;
1421                                         pn[n][2]        =       a[2]+(b[2]-a[2])*t;
1422                                         pn[n][3]        =       a[3]+(b[3]-a[3])*t;
1423                                         ++n;
1424                                 }
1425                                 if (s[j]>0) pn[n++]=b;
1426                         }
1427                         // ready to test far clipping, start from the modified polygon
1428                         pi = pn;
1429                         ni = n;
1430                 } else
1431                 {
1432                         // no clipping on the near plane, keep same vector
1433                         ni = NP;
1434                 }
1435                 // now deal with far clipping
1436                 for (i=0, m=0;i<ni;++i)
1437                 {
1438                         s[i]=pi[i][2]-pi[i][3];
1439                         if (s[i]>0) m+=1<<i;
1440                 }
1441                 if (m==((1<<ni)-1)) 
1442                         return(0);
1443                 if (m!=0)
1444                 {
1445                         for (i=ni-1,j=0,n=0;j<ni;i=j++)
1446                         {
1447                                 const btVector4&        a=pi[i];
1448                                 const btVector4&        b=pi[j];
1449                                 const btScalar          t=s[i]/(a[2]-a[3]-b[2]+b[3]);
1450                                 if ((t>0)&&(t<1))
1451                                 {
1452                                         po[n][0]        =       a[0]+(b[0]-a[0])*t;
1453                                         po[n][1]        =       a[1]+(b[1]-a[1])*t;
1454                                         po[n][2]        =       a[2]+(b[2]-a[2])*t;
1455                                         po[n][3]        =       a[3]+(b[3]-a[3])*t;
1456                                         ++n;
1457                                 }
1458                                 if (s[j]<0) po[n++]=b;
1459                         }
1460                         return(n);
1461                 }
1462                 for (int i=0;i<ni;++i) po[i]=pi[i];
1463                 return(ni);
1464         }
1465         // write or check a triangle to buffer. a,b,c in device coordinates (-1,+1)
1466         template <typename POLICY>
1467         inline bool     draw(   const btVector4& a,
1468                                                 const btVector4& b,
1469                                                 const btVector4& c,
1470                                                 const float face,
1471                                                 const btScalar minarea)
1472         {
1473                 const btScalar          a2=btCross(b-a,c-a)[2];
1474                 if ((face*a2)<0.f || btFabs(a2)<minarea)
1475                         return false;
1476                 // further down we are normally going to write to the Zbuffer, mark it so
1477                 POLICY::Occlusion(m_occlusion);
1478
1479                 int x[3], y[3], ib=1, ic=2;
1480                 btScalar z[3];
1481                 x[0]=(int)(a.x()*m_scales[0]+m_offsets[0]);
1482                 y[0]=(int)(a.y()*m_scales[1]+m_offsets[1]);
1483                 z[0]=a.z();
1484                 if (a2 < 0.f)
1485                 {
1486                         // negative aire is possible with double face => must
1487                         // change the order of b and c otherwise the algorithm doesn't work
1488                         ib=2;
1489                         ic=1;
1490                 }
1491                 x[ib]=(int)(b.x()*m_scales[0]+m_offsets[0]);
1492                 x[ic]=(int)(c.x()*m_scales[0]+m_offsets[0]);
1493                 y[ib]=(int)(b.y()*m_scales[1]+m_offsets[1]);
1494                 y[ic]=(int)(c.y()*m_scales[1]+m_offsets[1]);
1495                 z[ib]=b.z();
1496                 z[ic]=c.z();
1497                 const int               mix=btMax(0,btMin(x[0],btMin(x[1],x[2])));
1498                 const int               mxx=btMin(m_sizes[0],1+btMax(x[0],btMax(x[1],x[2])));
1499                 const int               miy=btMax(0,btMin(y[0],btMin(y[1],y[2])));
1500                 const int               mxy=btMin(m_sizes[1],1+btMax(y[0],btMax(y[1],y[2])));
1501                 const int               width=mxx-mix;
1502                 const int               height=mxy-miy;
1503                 if ((width*height) <= 1)
1504                 {
1505                         // degenerated in at most one single pixel
1506                         btScalar* scan=&m_buffer[miy*m_sizes[0]+mix];
1507                         // use for loop to detect the case where width or height == 0
1508                         for (int iy=miy;iy<mxy;++iy)
1509                         {
1510                                 for (int ix=mix;ix<mxx;++ix)
1511                                 {
1512                                         if (POLICY::Process(*scan,z[0])) 
1513                                                 return(true);
1514                                         if (POLICY::Process(*scan,z[1])) 
1515                                                 return(true);
1516                                         if (POLICY::Process(*scan,z[2])) 
1517                                                 return(true);
1518                                 }
1519                         }
1520                 } else if (width == 1) 
1521                 {
1522                         // Degenerated in at least 2 vertical lines
1523                         // The algorithm below doesn't work when face has a single pixel width
1524                         // We cannot use general formulas because the plane is degenerated. 
1525                         // We have to interpolate along the 3 edges that overlaps and process each pixel.
1526                         // sort the y coord to make formula simpler
1527                         int ytmp;
1528                         btScalar ztmp;
1529                         if (y[0] > y[1]) { ytmp=y[1];y[1]=y[0];y[0]=ytmp;ztmp=z[1];z[1]=z[0];z[0]=ztmp; }
1530                         if (y[0] > y[2]) { ytmp=y[2];y[2]=y[0];y[0]=ytmp;ztmp=z[2];z[2]=z[0];z[0]=ztmp; }
1531                         if (y[1] > y[2]) { ytmp=y[2];y[2]=y[1];y[1]=ytmp;ztmp=z[2];z[2]=z[1];z[1]=ztmp; }
1532                         int     dy[]={  y[0]-y[1],
1533                                                 y[1]-y[2],
1534                                                 y[2]-y[0]};
1535                         btScalar dzy[3];
1536                         dzy[0] = (dy[0]) ? (z[0]-z[1])/dy[0] : btScalar(0.f);
1537                         dzy[1] = (dy[1]) ? (z[1]-z[2])/dy[1] : btScalar(0.f);
1538                         dzy[2] = (dy[2]) ? (z[2]-z[0])/dy[2] : btScalar(0.f);
1539                         btScalar v[3] = {       dzy[0]*(miy-y[0])+z[0],
1540                                                                 dzy[1]*(miy-y[1])+z[1],
1541                                                                 dzy[2]*(miy-y[2])+z[2] };
1542                         dy[0] = y[1]-y[0];
1543                         dy[1] = y[0]-y[1];
1544                         dy[2] = y[2]-y[0];
1545                         btScalar* scan=&m_buffer[miy*m_sizes[0]+mix];
1546                         for (int iy=miy;iy<mxy;++iy)
1547                         {
1548                                 if (dy[0] >= 0 && POLICY::Process(*scan,v[0])) 
1549                                         return(true);
1550                                 if (dy[1] >= 0 && POLICY::Process(*scan,v[1])) 
1551                                         return(true);
1552                                 if (dy[2] >= 0 && POLICY::Process(*scan,v[2])) 
1553                                         return(true);
1554                                 scan+=m_sizes[0];
1555                                 v[0] += dzy[0]; v[1] += dzy[1]; v[2] += dzy[2];
1556                                 dy[0]--; dy[1]++, dy[2]--;
1557                         }
1558                 } else if (height == 1)
1559                 {
1560                         // Degenerated in at least 2 horizontal lines
1561                         // The algorithm below doesn't work when face has a single pixel width
1562                         // We cannot use general formulas because the plane is degenerated. 
1563                         // We have to interpolate along the 3 edges that overlaps and process each pixel.
1564                         int xtmp;
1565                         btScalar ztmp;
1566                         if (x[0] > x[1]) { xtmp=x[1];x[1]=x[0];x[0]=xtmp;ztmp=z[1];z[1]=z[0];z[0]=ztmp; }
1567                         if (x[0] > x[2]) { xtmp=x[2];x[2]=x[0];x[0]=xtmp;ztmp=z[2];z[2]=z[0];z[0]=ztmp; }
1568                         if (x[1] > x[2]) { xtmp=x[2];x[2]=x[1];x[1]=xtmp;ztmp=z[2];z[2]=z[1];z[1]=ztmp; }
1569                         int     dx[]={  x[0]-x[1],
1570                                                 x[1]-x[2],
1571                                                 x[2]-x[0]};
1572                         btScalar dzx[3];
1573                         dzx[0] = (dx[0]) ? (z[0]-z[1])/dx[0] : btScalar(0.f);
1574                         dzx[1] = (dx[1]) ? (z[1]-z[2])/dx[1] : btScalar(0.f);
1575                         dzx[2] = (dx[2]) ? (z[2]-z[0])/dx[2] : btScalar(0.f);
1576                         btScalar v[3] = { dzx[0]*(mix-x[0])+z[0],
1577                                                           dzx[1]*(mix-x[1])+z[1],
1578                                                           dzx[2]*(mix-x[2])+z[2] };
1579                         dx[0] = x[1]-x[0];
1580                         dx[1] = x[0]-x[1];
1581                         dx[2] = x[2]-x[0];
1582                         btScalar* scan=&m_buffer[miy*m_sizes[0]+mix];
1583                         for (int ix=mix;ix<mxx;++ix)
1584                         {
1585                                 if (dx[0] >= 0 && POLICY::Process(*scan,v[0])) 
1586                                         return(true);
1587                                 if (dx[1] >= 0 && POLICY::Process(*scan,v[1])) 
1588                                         return(true);
1589                                 if (dx[2] >= 0 && POLICY::Process(*scan,v[2])) 
1590                                         return(true);
1591                                 scan++;
1592                                 v[0] += dzx[0]; v[1] += dzx[1]; v[2] += dzx[2];
1593                                 dx[0]--; dx[1]++, dx[2]--;
1594                         }
1595                 } else
1596                 {
1597                         // general case
1598                         const int               dx[]={  y[0]-y[1],
1599                                                                         y[1]-y[2],
1600                                                                         y[2]-y[0]};
1601                         const int               dy[]={  x[1]-x[0]-dx[0]*width,
1602                                                                         x[2]-x[1]-dx[1]*width,
1603                                                                         x[0]-x[2]-dx[2]*width};
1604                         const int               a=x[2]*y[0]+x[0]*y[1]-x[2]*y[1]-x[0]*y[2]+x[1]*y[2]-x[1]*y[0];
1605                         const btScalar  ia=1/(btScalar)a;
1606                         const btScalar  dzx=ia*(y[2]*(z[1]-z[0])+y[1]*(z[0]-z[2])+y[0]*(z[2]-z[1]));
1607                         const btScalar  dzy=ia*(x[2]*(z[0]-z[1])+x[0]*(z[1]-z[2])+x[1]*(z[2]-z[0]))-(dzx*width);                
1608                         int                             c[]={   miy*x[1]+mix*y[0]-x[1]*y[0]-mix*y[1]+x[0]*y[1]-miy*x[0],
1609                                                                         miy*x[2]+mix*y[1]-x[2]*y[1]-mix*y[2]+x[1]*y[2]-miy*x[1],
1610                                                                         miy*x[0]+mix*y[2]-x[0]*y[2]-mix*y[0]+x[2]*y[0]-miy*x[2]};
1611                         btScalar                v=ia*((z[2]*c[0])+(z[0]*c[1])+(z[1]*c[2]));
1612                         btScalar*               scan=&m_buffer[miy*m_sizes[0]];
1613                         for (int iy=miy;iy<mxy;++iy)
1614                         {
1615                                 for (int ix=mix;ix<mxx;++ix)
1616                                 {
1617                                         if ((c[0]>=0)&&(c[1]>=0)&&(c[2]>=0))
1618                                         {
1619                                                 if (POLICY::Process(scan[ix],v)) 
1620                                                         return(true);
1621                                         }
1622                                         c[0]+=dx[0];c[1]+=dx[1];c[2]+=dx[2];v+=dzx;
1623                                 }
1624                                 c[0]+=dy[0];c[1]+=dy[1];c[2]+=dy[2];v+=dzy;
1625                                 scan+=m_sizes[0];
1626                         }
1627                 }
1628                 return(false);
1629         }
1630         // clip than write or check a polygon 
1631         template <const int NP,typename POLICY>
1632         inline bool     clipDraw(       const btVector4* p,
1633                                                         const float face,
1634                                                         btScalar minarea)
1635         {
1636                 btVector4       o[NP*2];
1637                 int                     n=clip<NP>(p,o);
1638                 bool            earlyexit=false;
1639                 if (n)
1640                 {
1641                         project(o,n);
1642                         for (int i=2;i<n && !earlyexit;++i)
1643                         {
1644                                 earlyexit|=draw<POLICY>(o[0],o[i-1],o[i],face,minarea);
1645                         }
1646                 }
1647                 return(earlyexit);
1648         }
1649         // add a triangle (in model coordinate)
1650         // face =  0.f if face is double side, 
1651         //      =  1.f if face is single sided and scale is positive
1652         //      = -1.f if face is single sided and scale is negative
1653         void            appendOccluderM(const float* a,
1654                                                                 const float* b,
1655                                                                 const float* c,
1656                                                                 const float face)
1657         {
1658                 btVector4       p[3];
1659                 transformM(a,p[0]);
1660                 transformM(b,p[1]);
1661                 transformM(c,p[2]);
1662                 clipDraw<3,WriteOCL>(p,face,btScalar(0.f));
1663         }
1664         // add a quad (in model coordinate)
1665         void            appendOccluderM(const float* a,
1666                                                                 const float* b,
1667                                                                 const float* c,
1668                                                                 const float* d,
1669                                                                 const float face)
1670         {       
1671                 btVector4       p[4];
1672                 transformM(a,p[0]);
1673                 transformM(b,p[1]);
1674                 transformM(c,p[2]);
1675                 transformM(d,p[3]);
1676                 clipDraw<4,WriteOCL>(p,face,btScalar(0.f));
1677         }
1678         // query occluder for a box (c=center, e=extend) in world coordinate
1679         inline bool     queryOccluderW( const btVector3& c,
1680                                                                 const btVector3& e)
1681         {
1682                 if (!m_occlusion)
1683                         // no occlusion yet, no need to check
1684                         return true;
1685                 btVector4       x[8];
1686                 transformW(btVector3(c[0]-e[0],c[1]-e[1],c[2]-e[2]),x[0]);
1687                 transformW(btVector3(c[0]+e[0],c[1]-e[1],c[2]-e[2]),x[1]);
1688                 transformW(btVector3(c[0]+e[0],c[1]+e[1],c[2]-e[2]),x[2]);
1689                 transformW(btVector3(c[0]-e[0],c[1]+e[1],c[2]-e[2]),x[3]);
1690                 transformW(btVector3(c[0]-e[0],c[1]-e[1],c[2]+e[2]),x[4]);
1691                 transformW(btVector3(c[0]+e[0],c[1]-e[1],c[2]+e[2]),x[5]);
1692                 transformW(btVector3(c[0]+e[0],c[1]+e[1],c[2]+e[2]),x[6]);
1693                 transformW(btVector3(c[0]-e[0],c[1]+e[1],c[2]+e[2]),x[7]);
1694                 for (int i=0;i<8;++i)
1695                 {
1696                         // the box is clipped, it's probably a large box, don't waste our time to check
1697                         if ((x[i][2]+x[i][3])<=0) return(true);
1698                 }
1699                 static const int        d[]={   1,0,3,2,
1700                                                                         4,5,6,7,
1701                                                                         4,7,3,0,
1702                                                                         6,5,1,2,
1703                                                                         7,6,2,3,
1704                                                                         5,4,0,1};
1705                 for (unsigned int i=0;i<(sizeof(d)/sizeof(d[0]));)
1706                 {
1707                         const btVector4 p[]={   x[d[i++]],
1708                                                                         x[d[i++]],
1709                                                                         x[d[i++]],
1710                                                                         x[d[i++]]};
1711                         if (clipDraw<4,QueryOCL>(p,1.f,0.f)) 
1712                                 return(true);
1713                 }
1714                 return(false);
1715         }
1716 };
1717
1718
1719 struct  DbvtCullingCallback : btDbvt::ICollide
1720 {
1721         PHY_CullingCallback m_clientCallback;
1722         void* m_userData;
1723         OcclusionBuffer *m_ocb;
1724
1725         DbvtCullingCallback(PHY_CullingCallback clientCallback, void* userData)
1726         {
1727                 m_clientCallback = clientCallback;
1728                 m_userData = userData;
1729                 m_ocb = NULL;
1730         }
1731         bool Descent(const btDbvtNode* node)
1732         {
1733                 return(m_ocb->queryOccluderW(node->volume.Center(),node->volume.Extents()));
1734         }
1735         void Process(const btDbvtNode* node,btScalar depth)
1736         {
1737                 Process(node);
1738         }
1739         void Process(const btDbvtNode* leaf)
1740         {       
1741                 btBroadphaseProxy*      proxy=(btBroadphaseProxy*)leaf->data;
1742                 // the client object is a graphic controller
1743                 CcdGraphicController* ctrl = static_cast<CcdGraphicController*>(proxy->m_clientObject);
1744                 KX_ClientObjectInfo* info = (KX_ClientObjectInfo*)ctrl->getNewClientInfo();
1745                 if (m_ocb)
1746                 {
1747                         // means we are doing occlusion culling. Check if this object is an occluders
1748                         KX_GameObject* gameobj = KX_GameObject::GetClientObject(info);
1749                         if (gameobj && gameobj->GetOccluder())
1750                         {
1751                                 double* fl = gameobj->GetOpenGLMatrixPtr()->getPointer();
1752                                 // this will create the occlusion buffer if not already done
1753                                 // and compute the transformation from model local space to clip space
1754                                 m_ocb->SetModelMatrix(fl);
1755                                 float face = (gameobj->IsNegativeScaling()) ? -1.0f : 1.0f;
1756                                 // walk through the meshes and for each add to buffer
1757                                 for (int i=0; i<gameobj->GetMeshCount(); i++)
1758                                 {
1759                                         RAS_MeshObject* meshobj = gameobj->GetMesh(i);
1760                                         const float *v1, *v2, *v3, *v4;
1761
1762                                         int polycount = meshobj->NumPolygons();
1763                                         for (int j=0; j<polycount; j++)
1764                                         {
1765                                                 RAS_Polygon* poly = meshobj->GetPolygon(j);
1766                                                 switch (poly->VertexCount())
1767                                                 {
1768                                                 case 3:
1769                                                         v1 = poly->GetVertex(0)->getXYZ();
1770                                                         v2 = poly->GetVertex(1)->getXYZ();
1771                                                         v3 = poly->GetVertex(2)->getXYZ();
1772                                                         m_ocb->appendOccluderM(v1,v2,v3,((poly->IsTwoside())?0.f:face));
1773                                                         break;
1774                                                 case 4:
1775                                                         v1 = poly->GetVertex(0)->getXYZ();
1776                                                         v2 = poly->GetVertex(1)->getXYZ();
1777                                                         v3 = poly->GetVertex(2)->getXYZ();
1778                                                         v4 = poly->GetVertex(3)->getXYZ();
1779                                                         m_ocb->appendOccluderM(v1,v2,v3,v4,((poly->IsTwoside())?0.f:face));
1780                                                         break;
1781                                                 }
1782                                         }
1783                                 }
1784                         }
1785                 }
1786                 if (info)
1787                         (*m_clientCallback)(info, m_userData);
1788         }
1789 };
1790
1791 static OcclusionBuffer gOcb;
1792 bool CcdPhysicsEnvironment::cullingTest(PHY_CullingCallback callback, void* userData, PHY__Vector4 *planes, int nplanes, int occlusionRes)
1793 {
1794         if (!m_cullingTree)
1795                 return false;
1796         DbvtCullingCallback dispatcher(callback, userData);
1797         btVector3 planes_n[6];
1798         btScalar planes_o[6];
1799         if (nplanes > 6)
1800                 nplanes = 6;
1801         for (int i=0; i<nplanes; i++)
1802         {
1803                 planes_n[i].setValue(planes[i][0], planes[i][1], planes[i][2]);
1804                 planes_o[i] = planes[i][3];
1805         }
1806         // if occlusionRes != 0 => occlusion culling
1807         if (occlusionRes)
1808         {
1809                 gOcb.setup(occlusionRes);
1810                 dispatcher.m_ocb = &gOcb;
1811                 // occlusion culling, the direction of the view is taken from the first plan which MUST be the near plane
1812                 btDbvt::collideOCL(m_cullingTree->m_sets[1].m_root,planes_n,planes_o,planes_n[0],nplanes,dispatcher);
1813                 btDbvt::collideOCL(m_cullingTree->m_sets[0].m_root,planes_n,planes_o,planes_n[0],nplanes,dispatcher);           
1814         }else 
1815         {
1816                 btDbvt::collideKDOP(m_cullingTree->m_sets[1].m_root,planes_n,planes_o,nplanes,dispatcher);
1817                 btDbvt::collideKDOP(m_cullingTree->m_sets[0].m_root,planes_n,planes_o,nplanes,dispatcher);              
1818         }
1819         return true;
1820 }
1821
1822 int     CcdPhysicsEnvironment::getNumContactPoints()
1823 {
1824         return 0;
1825 }
1826
1827 void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
1828 {
1829
1830 }
1831
1832
1833
1834
1835 btBroadphaseInterface*  CcdPhysicsEnvironment::getBroadphase()
1836
1837         return m_dynamicsWorld->getBroadphase(); 
1838 }
1839
1840 btDispatcher*   CcdPhysicsEnvironment::getDispatcher()
1841
1842         return m_dynamicsWorld->getDispatcher();
1843 }
1844
1845 void CcdPhysicsEnvironment::MergeEnvironment(CcdPhysicsEnvironment *other)
1846 {
1847         std::set<CcdPhysicsController*>::iterator it;
1848
1849         while (other->m_controllers.begin() != other->m_controllers.end())
1850         {
1851                 it= other->m_controllers.begin();
1852                 CcdPhysicsController* ctrl= (*it);
1853
1854                 other->removeCcdPhysicsController(ctrl);
1855                 this->addCcdPhysicsController(ctrl);
1856         }
1857 }
1858
1859 CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
1860 {
1861
1862 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1863         m_wrapperVehicles.clear();
1864 #endif //NEW_BULLET_VEHICLE_SUPPORT
1865
1866         //m_broadphase->DestroyScene();
1867         //delete broadphase ? release reference on broadphase ?
1868
1869         //first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher
1870         //delete m_dispatcher;
1871         delete m_dynamicsWorld;
1872         
1873
1874         if (NULL != m_ownPairCache)
1875                 delete m_ownPairCache;
1876
1877         if (NULL != m_ownDispatcher)
1878                 delete m_ownDispatcher;
1879
1880         if (NULL != m_solver)
1881                 delete m_solver;
1882
1883         if (NULL != m_debugDrawer)
1884                 delete m_debugDrawer;
1885
1886         if (NULL != m_filterCallback)
1887                 delete m_filterCallback;
1888
1889         if (NULL != m_collisionConfiguration)
1890                 delete m_collisionConfiguration;
1891
1892         if (NULL != m_broadphase)
1893                 delete m_broadphase;
1894
1895         if (NULL != m_cullingTree)
1896                 delete m_cullingTree;
1897
1898         if (NULL != m_cullingCache)
1899                 delete m_cullingCache;
1900
1901 }
1902
1903
1904 float   CcdPhysicsEnvironment::getConstraintParam(int constraintId,int param)
1905 {
1906         btTypedConstraint* typedConstraint = getConstraintById(constraintId);
1907         switch (typedConstraint->getUserConstraintType())
1908         {
1909         case PHY_GENERIC_6DOF_CONSTRAINT:
1910                 {
1911                         
1912                         switch (param)
1913                         {
1914                         case 0: case 1: case 2: 
1915                                 {
1916                                         //param = 0..2 are linear constraint values
1917                                         btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
1918                                         genCons->calculateTransforms();
1919                                         return genCons->getRelativePivotPosition(param);
1920                                         break;
1921                                 }
1922                                 case 3: case 4: case 5:
1923                                 {
1924                                         //param = 3..5 are relative constraint (Euler) angles
1925                                         btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
1926                                         genCons->calculateTransforms();
1927                                         return genCons->getAngle(param-3);
1928                                         break;
1929                                 }
1930                         default:
1931                                 {
1932                                 }
1933                         }
1934                         break;
1935                 };
1936         default:
1937                 {
1938                 };
1939         };
1940         return 0.f;
1941 }
1942
1943 void    CcdPhysicsEnvironment::setConstraintParam(int constraintId,int param,float value0,float value1)
1944 {
1945         btTypedConstraint* typedConstraint = getConstraintById(constraintId);
1946         switch (typedConstraint->getUserConstraintType())
1947         {
1948         case PHY_GENERIC_6DOF_CONSTRAINT:
1949                 {
1950                         
1951                         switch (param)
1952                         {
1953                         case 0: case 1: case 2: case 3: case 4: case 5:
1954                                 {
1955                                         //param = 0..5 are constraint limits, with low/high limit value
1956                                         btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
1957                                         genCons->setLimit(param,value0,value1);
1958                                         break;
1959                                 }
1960                         case 6: case 7: case 8:
1961                                 {
1962                                         //param = 6,7,8 are translational motors, with value0=target velocity, value1 = max motor force
1963                                         btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
1964                                         int transMotorIndex = param-6;
1965                                         btTranslationalLimitMotor* transMotor = genCons->getTranslationalLimitMotor();
1966                                         transMotor->m_targetVelocity[transMotorIndex]= value0;
1967                                         transMotor->m_maxMotorForce[transMotorIndex]=value1;
1968                                         transMotor->m_enableMotor[transMotorIndex] = (value1>0.f);
1969                                         break;
1970                                 }
1971                         case 9: case 10: case 11:
1972                                 {
1973                                         //param = 9,10,11 are rotational motors, with value0=target velocity, value1 = max motor force
1974                                         btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
1975                                         int angMotorIndex = param-9;
1976                                         btRotationalLimitMotor* rotMotor = genCons->getRotationalLimitMotor(angMotorIndex);
1977                                         rotMotor->m_enableMotor = (value1 > 0.f);
1978                                         rotMotor->m_targetVelocity = value0;
1979                                         rotMotor->m_maxMotorForce = value1;
1980                                         break;
1981                                 }
1982
1983                         case 12: case 13: case 14: case 15: case 16: case 17:
1984                         {
1985                                 //param 13-17 are for motorized springs on each of the degrees of freedom
1986                                         btGeneric6DofSpringConstraint* genCons = (btGeneric6DofSpringConstraint*)typedConstraint;
1987                                         int springIndex = param-12;
1988                                         if (value0!=0.f)
1989                                         {
1990                                                 bool springEnabled = true;
1991                                                 genCons->setStiffness(springIndex,value0);
1992                                                 genCons->setDamping(springIndex,value1);
1993                                                 genCons->enableSpring(springIndex,springEnabled);
1994                                                 genCons->setEquilibriumPoint(springIndex);
1995                                         } else
1996                                         {
1997                                                 bool springEnabled = false;
1998                                                 genCons->enableSpring(springIndex,springEnabled);
1999                                         }
2000                                         break;
2001                         }
2002
2003                         default:
2004                                 {
2005                                 }
2006                         };
2007                         break;
2008                 };
2009         case PHY_CONE_TWIST_CONSTRAINT:
2010                 {
2011                         switch (param)
2012                         {
2013                         case 3: case 4: case 5:
2014                                 {
2015                                         //param = 3,4,5 are constraint limits, high limit values
2016                                         btConeTwistConstraint* coneTwist = (btConeTwistConstraint*)typedConstraint;
2017                                         if (value1<0.0f)
2018                                                 coneTwist->setLimit(param,btScalar(BT_LARGE_FLOAT));
2019                                         else
2020                                                 coneTwist->setLimit(param,value1);
2021                                         break;
2022                                 }
2023                         default:
2024                                 {
2025                                 }
2026                         };
2027                         break;
2028                 };
2029         case PHY_ANGULAR_CONSTRAINT:
2030         case PHY_LINEHINGE_CONSTRAINT:
2031                 {
2032                         switch (param)
2033                         {
2034                         case 3:
2035                                 {
2036                                         //param = 3 is a constraint limit, with low/high limit value
2037                                         btHingeConstraint* hingeCons = (btHingeConstraint*)typedConstraint;
2038                                         hingeCons->setLimit(value0,value1);
2039                                         break;
2040                                 }
2041                         default:
2042                                 {
2043                                 }
2044                         }
2045                         break;
2046                 };
2047         default:
2048                 {
2049                 };
2050         };
2051 }
2052
2053 btTypedConstraint*      CcdPhysicsEnvironment::getConstraintById(int constraintId)
2054 {
2055
2056         int numConstraints = m_dynamicsWorld->getNumConstraints();
2057         int i;
2058         for (i=0;i<numConstraints;i++)
2059         {
2060                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
2061                 if (constraint->getUserConstraintId()==constraintId)
2062                 {
2063                         return constraint;
2064                 }
2065         }
2066         return 0;
2067 }
2068
2069
2070 void CcdPhysicsEnvironment::addSensor(PHY_IPhysicsController* ctrl)
2071 {
2072
2073         CcdPhysicsController* ctrl1 = (CcdPhysicsController* )ctrl;
2074         // addSensor() is a "light" function for bullet because it is used
2075         // dynamically when the sensor is activated. Use enableCcdPhysicsController() instead 
2076         //if (m_controllers.insert(ctrl1).second)
2077         //{
2078         //      addCcdPhysicsController(ctrl1);
2079         //}
2080         enableCcdPhysicsController(ctrl1);
2081 }
2082
2083 bool CcdPhysicsEnvironment::removeCollisionCallback(PHY_IPhysicsController* ctrl)
2084 {
2085         CcdPhysicsController* ccdCtrl = (CcdPhysicsController*)ctrl;
2086         if (!ccdCtrl->Unregister())
2087                 return false;
2088         m_triggerControllers.erase(ccdCtrl);
2089         return true;
2090 }
2091
2092
2093 void CcdPhysicsEnvironment::removeSensor(PHY_IPhysicsController* ctrl)
2094 {
2095         disableCcdPhysicsController((CcdPhysicsController*)ctrl);
2096 }
2097
2098 void CcdPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user)
2099 {
2100         /*      printf("addTouchCallback\n(response class = %i)\n",response_class);
2101
2102         //map PHY_ convention into SM_ convention
2103         switch (response_class)
2104         {
2105         case    PHY_FH_RESPONSE:
2106         printf("PHY_FH_RESPONSE\n");
2107         break;
2108         case PHY_SENSOR_RESPONSE:
2109         printf("PHY_SENSOR_RESPONSE\n");
2110         break;
2111         case PHY_CAMERA_RESPONSE:
2112         printf("PHY_CAMERA_RESPONSE\n");
2113         break;
2114         case PHY_OBJECT_RESPONSE:
2115         printf("PHY_OBJECT_RESPONSE\n");
2116         break;
2117         case PHY_STATIC_RESPONSE:
2118         printf("PHY_STATIC_RESPONSE\n");
2119         break;
2120         default:
2121         assert(0);
2122         return;
2123         }
2124         */
2125
2126         m_triggerCallbacks[response_class] = callback;
2127         m_triggerCallbacksUserPtrs[response_class] = user;
2128
2129 }
2130 bool CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctrl)
2131 {
2132         CcdPhysicsController* ccdCtrl = static_cast<CcdPhysicsController*>(ctrl);
2133
2134         if (!ccdCtrl->Register())
2135                 return false;
2136         m_triggerControllers.insert(ccdCtrl);
2137         return true;
2138 }
2139
2140 void    CcdPhysicsEnvironment::CallbackTriggers()
2141 {
2142         if (m_triggerCallbacks[PHY_OBJECT_RESPONSE] || (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)))
2143         {
2144                 //walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback
2145                 btDispatcher* dispatcher = m_dynamicsWorld->getDispatcher();
2146                 int numManifolds = dispatcher->getNumManifolds();
2147                 for (int i=0;i<numManifolds;i++)
2148                 {
2149                         btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
2150                         int numContacts = manifold->getNumContacts();
2151                         if (numContacts)
2152                         {
2153                                 btRigidBody* rb0 = static_cast<btRigidBody*>(manifold->getBody0());
2154                                 btRigidBody* rb1 = static_cast<btRigidBody*>(manifold->getBody1());
2155                                 if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints))
2156                                 {
2157                                         for (int j=0;j<numContacts;j++)
2158                                         {
2159                                                 btVector3 color(1,0,0);
2160                                                 const btManifoldPoint& cp = manifold->getContactPoint(j);
2161                                                 if (m_debugDrawer)
2162                                                         m_debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
2163                                         }
2164                                 }
2165                                 btRigidBody* obj0 = rb0;
2166                                 btRigidBody* obj1 = rb1;
2167
2168                                 //m_internalOwner is set in 'addPhysicsController'
2169                                 CcdPhysicsController* ctrl0 = static_cast<CcdPhysicsController*>(obj0->getUserPointer());
2170                                 CcdPhysicsController* ctrl1 = static_cast<CcdPhysicsController*>(obj1->getUserPointer());
2171
2172                                 std::set<CcdPhysicsController*>::const_iterator i = m_triggerControllers.find(ctrl0);
2173                                 if (i == m_triggerControllers.end())
2174                                 {
2175                                         i = m_triggerControllers.find(ctrl1);
2176                                 }
2177
2178                                 if (!(i == m_triggerControllers.end()))
2179                                 {
2180                                         m_triggerCallbacks[PHY_OBJECT_RESPONSE](m_triggerCallbacksUserPtrs[PHY_OBJECT_RESPONSE],
2181                                                 ctrl0,ctrl1,0);
2182                                 }
2183                                 // Bullet does not refresh the manifold contact point for object without contact response
2184                                 // may need to remove this when a newer Bullet version is integrated
2185                                 if (!dispatcher->needsResponse(rb0, rb1))
2186                                 {
2187                                         // Refresh algorithm fails sometimes when there is penetration 
2188                                         // (usuall the case with ghost and sensor objects)
2189                                         // Let's just clear the manifold, in any case, it is recomputed on each frame.
2190                                         manifold->clearManifold(); //refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
2191                                 }
2192                         }
2193                 }
2194
2195
2196
2197         }
2198
2199
2200 }
2201
2202 // This call back is called before a pair is added in the cache
2203 // Handy to remove objects that must be ignored by sensors
2204 bool CcdOverlapFilterCallBack::needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
2205 {
2206         btCollisionObject *colObj0, *colObj1;
2207         CcdPhysicsController *sensorCtrl, *objCtrl;
2208         bool collides;
2209         // first check the filters
2210         collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
2211         collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
2212         if (!collides)
2213                 return false;
2214
2215         // additional check for sensor object
2216         if (proxy0->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger)
2217         {
2218                 // this is a sensor object, the other one can't be a sensor object because 
2219                 // they exclude each other in the above test
2220                 assert(!(proxy1->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger));
2221                 colObj0 = (btCollisionObject*)proxy0->m_clientObject;
2222                 colObj1 = (btCollisionObject*)proxy1->m_clientObject;
2223         }
2224         else if (proxy1->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger)
2225         {
2226                 colObj0 = (btCollisionObject*)proxy1->m_clientObject;
2227                 colObj1 = (btCollisionObject*)proxy0->m_clientObject;
2228         }
2229         else
2230         {
2231                 return true;
2232         }
2233         if (!colObj0 || !colObj1)
2234                 return false;
2235         sensorCtrl = static_cast<CcdPhysicsController*>(colObj0->getUserPointer());
2236         objCtrl = static_cast<CcdPhysicsController*>(colObj1->getUserPointer());
2237         if (m_physEnv->m_triggerCallbacks[PHY_BROADPH_RESPONSE])
2238         {
2239                 return m_physEnv->m_triggerCallbacks[PHY_BROADPH_RESPONSE](m_physEnv->m_triggerCallbacksUserPtrs[PHY_BROADPH_RESPONSE], sensorCtrl, objCtrl, 0);
2240         }
2241         return true;
2242 }
2243
2244
2245 #ifdef NEW_BULLET_VEHICLE_SUPPORT
2246
2247 //complex constraint for vehicles
2248 PHY_IVehicle*   CcdPhysicsEnvironment::getVehicleConstraint(int constraintId)
2249 {
2250         int i;
2251
2252         int numVehicles = m_wrapperVehicles.size();
2253         for (i=0;i<numVehicles;i++)
2254         {
2255                 WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
2256                 if (wrapperVehicle->GetVehicle()->getUserConstraintId() == constraintId)
2257                         return wrapperVehicle;
2258         }
2259
2260         return 0;
2261 }
2262
2263 #endif //NEW_BULLET_VEHICLE_SUPPORT
2264
2265
2266 int currentController = 0;
2267 int numController = 0;
2268
2269
2270
2271
2272 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateSphereController(float radius,const PHY__Vector3& position)
2273 {
2274         
2275         CcdConstructionInfo     cinfo;
2276         memset(&cinfo, 0, sizeof(cinfo)); /* avoid uninitialized values */
2277         cinfo.m_collisionShape = new btSphereShape(radius); // memory leak! The shape is not deleted by Bullet and we cannot add it to the KX_Scene.m_shapes list
2278         cinfo.m_MotionState = 0;
2279         cinfo.m_physicsEnv = this;
2280         // declare this object as Dyamic rather than static!!
2281         // The reason as it is designed to detect all type of object, including static object
2282         // It would cause static-static message to be printed on the console otherwise
2283         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE | btCollisionObject::CF_STATIC_OBJECT;
2284         DefaultMotionState* motionState = new DefaultMotionState();
2285         cinfo.m_MotionState = motionState;
2286         // we will add later the possibility to select the filter from option
2287         cinfo.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
2288         cinfo.m_collisionFilterGroup = CcdConstructionInfo::SensorFilter;
2289         cinfo.m_bSensor = true;
2290         motionState->m_worldTransform.setIdentity();
2291         motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
2292
2293         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
2294         
2295         return sphereController;
2296 }
2297
2298 int findClosestNode(btSoftBody* sb,const btVector3& worldPoint);
2299 int findClosestNode(btSoftBody* sb,const btVector3& worldPoint)
2300 {
2301         int node = -1;
2302
2303         btSoftBody::tNodeArray&   nodes(sb->m_nodes);
2304         float maxDistSqr = 1e30f;
2305
2306         for (int n=0;n<nodes.size();n++)
2307         {
2308                 btScalar distSqr = (nodes[n].m_x - worldPoint).length2();
2309                 if (distSqr<maxDistSqr)
2310                 {
2311                         maxDistSqr = distSqr;
2312                         node = n;
2313                 }
2314         }
2315         return node;
2316 }
2317
2318 int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
2319                                                                                                         float pivotX,float pivotY,float pivotZ,
2320                                                                                                         float axisX,float axisY,float axisZ,
2321                                                                                                         float axis1X,float axis1Y,float axis1Z,
2322                                                                                                         float axis2X,float axis2Y,float axis2Z,int flags
2323                                                                                                         )
2324 {
2325
2326         bool disableCollisionBetweenLinkedBodies = (0!=(flags & CCD_CONSTRAINT_DISABLE_LINKED_COLLISION));
2327
2328
2329
2330         CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
2331         CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
2332
2333         btRigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
2334         btRigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
2335
2336         
2337
2338
2339         bool rb0static = rb0 ? rb0->isStaticOrKinematicObject() : true;
2340         bool rb1static = rb1 ? rb1->isStaticOrKinematicObject() : true;
2341
2342         btCollisionObject* colObj0 = c0->GetCollisionObject();
2343         if (!colObj0)
2344         {
2345                 return 0;
2346         }
2347
2348         btVector3 pivotInA(pivotX,pivotY,pivotZ);
2349
2350         
2351
2352         //it might be a soft body, let's try
2353         btSoftBody* sb0 = c0 ? c0->GetSoftBody() : 0;
2354         btSoftBody* sb1 = c1 ? c1->GetSoftBody() : 0;
2355         if (sb0 && sb1)
2356         {
2357                 //not between two soft bodies?
2358                 return 0;
2359         }
2360
2361         if (sb0)
2362         {
2363                 //either cluster or node attach, let's find closest node first
2364                 //the soft body doesn't have a 'real' world transform, so get its initial world transform for now
2365                 btVector3 pivotPointSoftWorld = sb0->m_initialWorldTransform(pivotInA);
2366                 int node=findClosestNode(sb0,pivotPointSoftWorld);
2367                 if (node >=0)
2368                 {
2369                         bool clusterconstaint = false;
2370 /*
2371                         switch (type)
2372                         {
2373                         case PHY_LINEHINGE_CONSTRAINT:
2374                                 {
2375                                         if (sb0->clusterCount() && rb1)
2376                                         {
2377                                                 btSoftBody::LJoint::Specs       ls;
2378                                                 ls.erp=0.5f;
2379                                                 ls.position=sb0->clusterCom(0);
2380                                                 sb0->appendLinearJoint(ls,rb1);
2381                                                 clusterconstaint = true;
2382                                                 break;
2383                                         }
2384                                 }
2385                         case PHY_GENERIC_6DOF_CONSTRAINT:
2386                                 {
2387                                         if (sb0->clusterCount() && rb1)
2388                                         {
2389                                                 btSoftBody::AJoint::Specs as;
2390                                                 as.erp = 1;
2391                                                 as.cfm = 1;
2392                                                 as.axis.setValue(axisX,axisY,axisZ);
2393                                                 sb0->appendAngularJoint(as,rb1);
2394                                                 clusterconstaint = true;
2395                                                 break;
2396                                         }
2397
2398                                         break;
2399                                 }
2400                         default:
2401                                 {
2402                                 
2403                                 }
2404                         };
2405                         */
2406
2407                         if (!clusterconstaint)
2408                         {
2409                                 if (rb1)
2410                                 {
2411                                         sb0->appendAnchor(node,rb1,disableCollisionBetweenLinkedBodies);
2412                                 } else
2413                                 {
2414                                         sb0->setMass(node,0.f);
2415                                 }
2416                         }
2417
2418                         
2419                 }
2420                 return 0;//can't remove soft body anchors yet
2421         }
2422
2423         if (sb1)
2424         {
2425                 btVector3 pivotPointAWorld = colObj0->getWorldTransform()(pivotInA);
2426                 int node=findClosestNode(sb1,pivotPointAWorld);
2427                 if (node >=0)
2428                 {
2429                         bool clusterconstaint = false;
2430
2431                         /*
2432                         switch (type)
2433                         {
2434                         case PHY_LINEHINGE_CONSTRAINT:
2435                                 {
2436                                         if (sb1->clusterCount() && rb0)
2437                                         {
2438                                                 btSoftBody::LJoint::Specs       ls;
2439                                                 ls.erp=0.5f;
2440                                                 ls.position=sb1->clusterCom(0);
2441                                                 sb1->appendLinearJoint(ls,rb0);
2442                                                 clusterconstaint = true;
2443                                                 break;
2444                                         }
2445                                 }
2446                         case PHY_GENERIC_6DOF_CONSTRAINT:
2447                                 {
2448                                         if (sb1->clusterCount() && rb0)
2449                                         {
2450                                                 btSoftBody::AJoint::Specs as;
2451                                                 as.erp = 1;
2452                                                 as.cfm = 1;
2453                                                 as.axis.setValue(axisX,axisY,axisZ);
2454                                                 sb1->appendAngularJoint(as,rb0);
2455                                                 clusterconstaint = true;
2456                                                 break;
2457                                         }
2458
2459                                         break;
2460                                 }
2461                         default:
2462                                 {
2463                                         
2464
2465                                 }
2466                         };*/
2467
2468
2469                         if (!clusterconstaint)
2470                         {
2471                                 if (rb0)
2472                                 {
2473                                         sb1->appendAnchor(node,rb0,disableCollisionBetweenLinkedBodies);
2474                                 } else
2475                                 {
2476                                         sb1->setMass(node,0.f);
2477                                 }
2478                         }
2479                         
2480
2481                 }
2482                 return 0;//can't remove soft body anchors yet
2483         }
2484
2485         if (rb0static && rb1static)
2486         {
2487                 
2488                 return 0;
2489         }
2490         
2491
2492         if (!rb0)
2493                 return 0;
2494
2495         
2496         btVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : 
2497                 rb0->getCenterOfMassTransform() * pivotInA;
2498         btVector3 axisInA(axisX,axisY,axisZ);
2499
2500
2501         bool angularOnly = false;
2502
2503         switch (type)
2504         {
2505         case PHY_POINT2POINT_CONSTRAINT:
2506                 {
2507
2508                         btPoint2PointConstraint* p2p = 0;
2509
2510                         if (rb1)
2511                         {
2512                                 p2p = new btPoint2PointConstraint(*rb0,
2513                                         *rb1,pivotInA,pivotInB);
2514                         } else
2515                         {
2516                                 p2p = new btPoint2PointConstraint(*rb0,
2517                                         pivotInA);
2518                         }
2519
2520                         m_dynamicsWorld->addConstraint(p2p,disableCollisionBetweenLinkedBodies);
2521 //                      m_constraints.push_back(p2p);
2522
2523                         p2p->setUserConstraintId(gConstraintUid++);
2524                         p2p->setUserConstraintType(type);
2525                         //64 bit systems can't cast pointer to int. could use size_t instead.
2526                         return p2p->getUserConstraintId();
2527
2528                         break;
2529                 }
2530
2531         case PHY_GENERIC_6DOF_CONSTRAINT:
2532                 {
2533                         btGeneric6DofConstraint* genericConstraint = 0;
2534
2535                         if (rb1)
2536                         {
2537                                 btTransform frameInA;
2538                                 btTransform frameInB;
2539                                 
2540                                 btVector3 axis1(axis1X,axis1Y,axis1Z), axis2(axis2X,axis2Y,axis2Z);
2541                                 if (axis1.length() == 0.0)
2542                                 {
2543                                         btPlaneSpace1( axisInA, axis1, axis2 );
2544                                 }
2545                                 
2546                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
2547                                                                   axisInA.y(), axis1.y(), axis2.y(),
2548                                                                                           axisInA.z(), axis1.z(), axis2.z() );
2549                                 frameInA.setOrigin( pivotInA );
2550
2551                                 btTransform inv = rb1->getCenterOfMassTransform().inverse();
2552
2553                                 btTransform globalFrameA = rb0->getCenterOfMassTransform() * frameInA;
2554                                 
2555                                 frameInB = inv  * globalFrameA;
2556                                 bool useReferenceFrameA = true;
2557
2558                                 genericConstraint = new btGeneric6DofSpringConstraint(
2559                                         *rb0,*rb1,
2560                                         frameInA,frameInB,useReferenceFrameA);
2561
2562
2563                         } else
2564                         {
2565                                 static btRigidBody s_fixedObject2( 0,0,0);
2566                                 btTransform frameInA;
2567                                 btTransform frameInB;
2568                                 
2569                                 btVector3 axis1, axis2;
2570                                 btPlaneSpace1( axisInA, axis1, axis2 );
2571
2572                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
2573                                                               axisInA.y(), axis1.y(), axis2.y(),
2574                                                               axisInA.z(), axis1.z(), axis2.z() );
2575
2576                                 frameInA.setOrigin( pivotInA );
2577
2578                                 ///frameInB in worldspace
2579                                 frameInB = rb0->getCenterOfMassTransform() * frameInA;
2580
2581                                 bool useReferenceFrameA = true;
2582                                 genericConstraint = new btGeneric6DofSpringConstraint(
2583                                         *rb0,s_fixedObject2,
2584                                         frameInA,frameInB,useReferenceFrameA);
2585                         }
2586                         
2587                         if (genericConstraint)
2588                         {
2589                                 //m_constraints.push_back(genericConstraint);
2590                                 m_dynamicsWorld->addConstraint(genericConstraint,disableCollisionBetweenLinkedBodies);
2591                                 genericConstraint->setUserConstraintId(gConstraintUid++);
2592                                 genericConstraint->setUserConstraintType(type);
2593                                 //64 bit systems can't cast pointer to int. could use size_t instead.
2594                                 return genericConstraint->getUserConstraintId();
2595                         } 
2596
2597                         break;
2598                 }
2599         case PHY_CONE_TWIST_CONSTRAINT:
2600                 {
2601                         btConeTwistConstraint* coneTwistContraint = 0;
2602
2603                         
2604                         if (rb1)
2605                         {
2606                                 btTransform frameInA;
2607                                 btTransform frameInB;
2608                                 
2609                                 btVector3 axis1(axis1X,axis1Y,axis1Z), axis2(axis2X,axis2Y,axis2Z);
2610                                 if (axis1.length() == 0.0)
2611                                 {
2612                                         btPlaneSpace1( axisInA, axis1, axis2 );
2613                                 }
2614                                 
2615                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
2616                                                                   axisInA.y(), axis1.y(), axis2.y(),
2617                                                                                           axisInA.z(), axis1.z(), axis2.z() );
2618                                 frameInA.setOrigin( pivotInA );
2619
2620                                 btTransform inv = rb1->getCenterOfMassTransform().inverse();
2621
2622                                 btTransform globalFrameA = rb0->getCenterOfMassTransform() * frameInA;
2623                                 
2624                                 frameInB = inv  * globalFrameA;
2625                                 
2626                                 coneTwistContraint = new btConeTwistConstraint( *rb0,*rb1,
2627                                         frameInA,frameInB);
2628
2629
2630                         } else
2631                         {
2632                                 static btRigidBody s_fixedObject2( 0,0,0);
2633                                 btTransform frameInA;
2634                                 btTransform frameInB;
2635                                 
2636                                 btVector3 axis1, axis2;
2637                                 btPlaneSpace1( axisInA, axis1, axis2 );
2638
2639                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
2640                                                                   axisInA.y(), axis1.y(), axis2.y(),
2641                                                                                           axisInA.z(), axis1.z(), axis2.z() );
2642
2643                                 frameInA.setOrigin( pivotInA );
2644
2645                                 ///frameInB in worldspace
2646                                 frameInB = rb0->getCenterOfMassTransform() * frameInA;
2647
2648                                 coneTwistContraint = new btConeTwistConstraint(
2649                                         *rb0,s_fixedObject2,
2650                                         frameInA,frameInB);
2651                         }
2652                         
2653                         if (coneTwistContraint)
2654                         {
2655                                 //m_constraints.push_back(genericConstraint);
2656                                 m_dynamicsWorld->addConstraint(coneTwistContraint,disableCollisionBetweenLinkedBodies);
2657                                 coneTwistContraint->setUserConstraintId(gConstraintUid++);
2658                                 coneTwistContraint->setUserConstraintType(type);
2659                                 //64 bit systems can't cast pointer to int. could use size_t instead.
2660                                 return coneTwistContraint->getUserConstraintId();
2661                         } 
2662
2663
2664
2665                         break;
2666                 }
2667         case PHY_ANGULAR_CONSTRAINT:
2668                 angularOnly = true;
2669
2670
2671         case PHY_LINEHINGE_CONSTRAINT:
2672                 {
2673                         btHingeConstraint* hinge = 0;
2674
2675                         if (rb1)
2676                         {
2677                                 // We know the orientations so we should use them instead of
2678                                 // having btHingeConstraint fill in the blanks any way it wants to.
2679                                 btTransform frameInA;
2680                                 btTransform frameInB;
2681                                 
2682                                 btVector3 axis1(axis1X,axis1Y,axis1Z), axis2(axis2X,axis2Y,axis2Z);
2683                                 if (axis1.length() == 0.0)
2684                                 {
2685                                         btPlaneSpace1( axisInA, axis1, axis2 );
2686                                 }
2687                                 
2688                                 // Internally btHingeConstraint's hinge-axis is z
2689                                 frameInA.getBasis().setValue( axis1.x(), axis2.x(), axisInA.x(),
2690                                                                                         axis1.y(), axis2.y(), axisInA.y(),
2691                                                                                         axis1.z(), axis2.z(), axisInA.z() );
2692                                                                                         
2693                                 frameInA.setOrigin( pivotInA );
2694
2695                                 btTransform inv = rb1->getCenterOfMassTransform().inverse();
2696
2697                                 btTransform globalFrameA = rb0->getCenterOfMassTransform() * frameInA;
2698                                 
2699                                 frameInB = inv  * globalFrameA;
2700                                 
2701                                 hinge = new btHingeConstraint(*rb0,*rb1,frameInA,frameInB);
2702
2703
2704                         } else
2705                         {
2706                                 static btRigidBody s_fixedObject2( 0,0,0);
2707
2708                                 btTransform frameInA;
2709                                 btTransform frameInB;
2710                                 
2711                                 btVector3 axis1(axis1X,axis1Y,axis1Z), axis2(axis2X,axis2Y,axis2Z);
2712                                 if (axis1.length() == 0.0)
2713                                 {
2714                                         btPlaneSpace1( axisInA, axis1, axis2 );
2715                                 }
2716
2717                                 // Internally btHingeConstraint's hinge-axis is z
2718                                 frameInA.getBasis().setValue( axis1.x(), axis2.x(), axisInA.x(),
2719                                                                                         axis1.y(), axis2.y(), axisInA.y(),
2720                                                                                         axis1.z(), axis2.z(), axisInA.z() );
2721                                 frameInA.setOrigin( pivotInA );
2722                                 frameInB = rb0->getCenterOfMassTransform() * frameInA;
2723
2724                                 hinge = new btHingeConstraint(*rb0, s_fixedObject2, frameInA, frameInB);
2725                         }
2726                         hinge->setAngularOnly(angularOnly);
2727
2728                         //m_constraints.push_bac