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