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