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