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