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