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