BGE: ray casting works on soft body, the hit polygon is also returned. The modificati...
[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                                         // note for softbody: this assumes that the softbody shape uses the same triangle numbering 
1074                                         // than the triangle mesh shape that was used to build it
1075                                         result.m_polygon = shapeInfo->m_polygonIndexArray.at(rayCallback.m_hitTriangleIndex);
1076
1077                                         // Bullet returns the normal from "outside".
1078                                         // If the user requests the real normal, compute it now
1079                     if (filterCallback.m_faceNormal)
1080                                         {
1081                                                 // mesh shapes are shared and stored in the shapeInfo
1082                                                 btTriangleMeshShape* triangleShape = shapeInfo->GetMeshShape();
1083
1084                                                 if (shape->isSoftBody()) 
1085                                                 {
1086                                                         // we can get the real normal directly from the body
1087                                                         btSoftBody* softBody = static_cast<btSoftBody*>(rayCallback.m_collisionObject);
1088                                                         rayCallback.m_hitNormalWorld = softBody->m_faces[rayCallback.m_hitTriangleIndex].m_normal;
1089                                                 } else if (triangleShape)
1090                                                 {
1091                                                         // this code is copied from Bullet 
1092                                                         btVector3 triangle[3];
1093                                                         const unsigned char *vertexbase;
1094                                                         int numverts;
1095                                                         PHY_ScalarType type;
1096                                                         int stride;
1097                                                         const unsigned char *indexbase;
1098                                                         int indexstride;
1099                                                         int numfaces;
1100                                                         PHY_ScalarType indicestype;
1101                                                         btStridingMeshInterface* meshInterface = triangleShape->getMeshInterface();
1102
1103                                                         meshInterface->getLockedReadOnlyVertexIndexBase(
1104                                                                 &vertexbase,
1105                                                                 numverts,
1106                                                                 type,
1107                                                                 stride,
1108                                                                 &indexbase,
1109                                                                 indexstride,
1110                                                                 numfaces,
1111                                                                 indicestype,
1112                                                                 0);
1113
1114                                                         unsigned int* gfxbase = (unsigned int*)(indexbase+rayCallback.m_hitTriangleIndex*indexstride);
1115                                                         const btVector3& meshScaling = shape->getLocalScaling();
1116                                                         for (int j=2;j>=0;j--)
1117                                                         {
1118                                                                 int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
1119
1120                                                                 btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride);
1121
1122                                                                 triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());              
1123                                                         }
1124                                                         meshInterface->unLockReadOnlyVertexBase(0);
1125                                                         btVector3 triangleNormal; 
1126                                                         triangleNormal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]);
1127                                                         rayCallback.m_hitNormalWorld = rayCallback.m_collisionObject->getWorldTransform().getBasis()*triangleNormal;
1128                                                 }
1129                                         }
1130                                 }
1131                         }
1132                 }
1133                 if (rayCallback.m_hitNormalWorld.length2() > (SIMD_EPSILON*SIMD_EPSILON))
1134                 {
1135                         rayCallback.m_hitNormalWorld.normalize();
1136                 } else
1137                 {
1138                         rayCallback.m_hitNormalWorld.setValue(1,0,0);
1139                 }
1140                 result.m_hitNormal[0] = rayCallback.m_hitNormalWorld.getX();
1141                 result.m_hitNormal[1] = rayCallback.m_hitNormalWorld.getY();
1142                 result.m_hitNormal[2] = rayCallback.m_hitNormalWorld.getZ();
1143                 filterCallback.reportHit(&result);
1144         }       
1145
1146
1147         return result.m_controller;
1148 }
1149
1150 // Handles occlusion culling. 
1151 // The implementation is based on the CDTestFramework
1152 struct OcclusionBuffer
1153 {
1154         struct WriteOCL
1155         {
1156                 static inline bool Process(btScalar& q,btScalar v) { if(q<v) q=v;return(false); }
1157                 static inline void Occlusion(bool& flag) { flag = true; }
1158         };
1159         struct QueryOCL
1160         {
1161                 static inline bool Process(btScalar& q,btScalar v) { return(q<=v); }
1162                 static inline void Occlusion(bool& flag) { }
1163         };
1164         btScalar*                                               m_buffer;
1165         size_t                                                  m_bufferSize;
1166         bool                                                    m_initialized;
1167         bool                                                    m_occlusion;
1168         int                                                             m_sizes[2];
1169         btScalar                                                m_scales[2];
1170         btScalar                                                m_offsets[2];
1171         btScalar                                                m_wtc[16];              // world to clip transform
1172         btScalar                                                m_mtc[16];              // model to clip transform
1173         // constructor: size=largest dimension of the buffer. 
1174         // Buffer size depends on aspect ratio
1175         OcclusionBuffer()
1176         {
1177                 m_initialized=false;
1178                 m_occlusion = false;
1179                 m_buffer = NULL;
1180                 m_bufferSize = 0;
1181         }
1182         // multiplication of column major matrices: m=m1*m2
1183         template<typename T1, typename T2>
1184         void            CMmat4mul(btScalar* m, const T1* m1, const T2* m2)
1185         {
1186                 m[ 0] = btScalar(m1[ 0]*m2[ 0]+m1[ 4]*m2[ 1]+m1[ 8]*m2[ 2]+m1[12]*m2[ 3]);
1187                 m[ 1] = btScalar(m1[ 1]*m2[ 0]+m1[ 5]*m2[ 1]+m1[ 9]*m2[ 2]+m1[13]*m2[ 3]);
1188                 m[ 2] = btScalar(m1[ 2]*m2[ 0]+m1[ 6]*m2[ 1]+m1[10]*m2[ 2]+m1[14]*m2[ 3]);
1189                 m[ 3] = btScalar(m1[ 3]*m2[ 0]+m1[ 7]*m2[ 1]+m1[11]*m2[ 2]+m1[15]*m2[ 3]);
1190
1191                 m[ 4] = btScalar(m1[ 0]*m2[ 4]+m1[ 4]*m2[ 5]+m1[ 8]*m2[ 6]+m1[12]*m2[ 7]);
1192                 m[ 5] = btScalar(m1[ 1]*m2[ 4]+m1[ 5]*m2[ 5]+m1[ 9]*m2[ 6]+m1[13]*m2[ 7]);
1193                 m[ 6] = btScalar(m1[ 2]*m2[ 4]+m1[ 6]*m2[ 5]+m1[10]*m2[ 6]+m1[14]*m2[ 7]);
1194                 m[ 7] = btScalar(m1[ 3]*m2[ 4]+m1[ 7]*m2[ 5]+m1[11]*m2[ 6]+m1[15]*m2[ 7]);
1195
1196                 m[ 8] = btScalar(m1[ 0]*m2[ 8]+m1[ 4]*m2[ 9]+m1[ 8]*m2[10]+m1[12]*m2[11]);
1197                 m[ 9] = btScalar(m1[ 1]*m2[ 8]+m1[ 5]*m2[ 9]+m1[ 9]*m2[10]+m1[13]*m2[11]);
1198                 m[10] = btScalar(m1[ 2]*m2[ 8]+m1[ 6]*m2[ 9]+m1[10]*m2[10]+m1[14]*m2[11]);
1199                 m[11] = btScalar(m1[ 3]*m2[ 8]+m1[ 7]*m2[ 9]+m1[11]*m2[10]+m1[15]*m2[11]);
1200
1201                 m[12] = btScalar(m1[ 0]*m2[12]+m1[ 4]*m2[13]+m1[ 8]*m2[14]+m1[12]*m2[15]);
1202                 m[13] = btScalar(m1[ 1]*m2[12]+m1[ 5]*m2[13]+m1[ 9]*m2[14]+m1[13]*m2[15]);
1203                 m[14] = btScalar(m1[ 2]*m2[12]+m1[ 6]*m2[13]+m1[10]*m2[14]+m1[14]*m2[15]);
1204                 m[15] = btScalar(m1[ 3]*m2[12]+m1[ 7]*m2[13]+m1[11]*m2[14]+m1[15]*m2[15]);
1205         }
1206         void            setup(int size)
1207         {
1208                 m_initialized=false;
1209                 m_occlusion=false;
1210                 // compute the size of the buffer
1211                 GLint           v[4];
1212                 GLdouble        m[16],p[16];
1213                 int                     maxsize;
1214                 double          ratio;
1215                 glGetIntegerv(GL_VIEWPORT,v);
1216                 maxsize = (v[2] > v[3]) ? v[2] : v[3];
1217                 assert(maxsize > 0);
1218                 ratio = 1.0/(2*maxsize);
1219                 // ensure even number
1220                 m_sizes[0] = 2*((int)(size*v[2]*ratio+0.5));
1221                 m_sizes[1] = 2*((int)(size*v[3]*ratio+0.5));
1222                 m_scales[0]=btScalar(m_sizes[0]/2);
1223                 m_scales[1]=btScalar(m_sizes[1]/2);
1224                 m_offsets[0]=m_scales[0]+0.5f;
1225                 m_offsets[1]=m_scales[1]+0.5f;
1226                 // prepare matrix
1227                 // at this time of the rendering, the modelview matrix is the 
1228                 // world to camera transformation and the projection matrix is
1229                 // camera to clip transformation. combine both so that 
1230                 glGetDoublev(GL_MODELVIEW_MATRIX,m);
1231                 glGetDoublev(GL_PROJECTION_MATRIX,p);
1232                 CMmat4mul(m_wtc,p,m);
1233         }
1234         void            initialize()
1235         {
1236                 size_t newsize = (m_sizes[0]*m_sizes[1])*sizeof(btScalar);
1237                 if (m_buffer)
1238                 {
1239                         // see if we can reuse
1240                         if (newsize > m_bufferSize)
1241                         {
1242                                 free(m_buffer);
1243                                 m_buffer = NULL;
1244                                 m_bufferSize = 0;
1245                         }
1246                 }
1247                 if (!m_buffer)
1248                 {
1249                         m_buffer = (btScalar*)calloc(1, newsize);
1250                         m_bufferSize = newsize;
1251                 } else
1252                 {
1253                         // buffer exists already, just clears it
1254                         memset(m_buffer, 0, newsize);
1255                 }
1256                 // memory allocate must succeed
1257                 assert(m_buffer != NULL);
1258                 m_initialized = true;
1259                 m_occlusion = false;
1260         }
1261         void            SetModelMatrix(double *fl)
1262         {
1263                 CMmat4mul(m_mtc,m_wtc,fl);
1264                 if (!m_initialized)
1265                         initialize();
1266         }
1267
1268         // transform a segment in world coordinate to clip coordinate
1269         void            transformW(const btVector3& x, btVector4& t)
1270         {
1271                 t[0]    =       x[0]*m_wtc[0]+x[1]*m_wtc[4]+x[2]*m_wtc[8]+m_wtc[12];
1272                 t[1]    =       x[0]*m_wtc[1]+x[1]*m_wtc[5]+x[2]*m_wtc[9]+m_wtc[13];
1273                 t[2]    =       x[0]*m_wtc[2]+x[1]*m_wtc[6]+x[2]*m_wtc[10]+m_wtc[14];
1274                 t[3]    =       x[0]*m_wtc[3]+x[1]*m_wtc[7]+x[2]*m_wtc[11]+m_wtc[15];
1275         }
1276         void            transformM(const float* x, btVector4& t)
1277         {
1278                 t[0]    =       x[0]*m_mtc[0]+x[1]*m_mtc[4]+x[2]*m_mtc[8]+m_mtc[12];
1279                 t[1]    =       x[0]*m_mtc[1]+x[1]*m_mtc[5]+x[2]*m_mtc[9]+m_mtc[13];
1280                 t[2]    =       x[0]*m_mtc[2]+x[1]*m_mtc[6]+x[2]*m_mtc[10]+m_mtc[14];
1281                 t[3]    =       x[0]*m_mtc[3]+x[1]*m_mtc[7]+x[2]*m_mtc[11]+m_mtc[15];
1282         }
1283         // convert polygon to device coordinates
1284         static bool     project(btVector4* p,int n)
1285         {
1286                 for(int i=0;i<n;++i)
1287                 {                       
1288                         p[i][2]=1/p[i][3];
1289                         p[i][0]*=p[i][2];
1290                         p[i][1]*=p[i][2];
1291                 }
1292                 return(true);
1293         }
1294         // pi: closed polygon in clip coordinate, NP = number of segments
1295         // po: same polygon with clipped segments removed
1296         template <const int NP>
1297         static int      clip(const btVector4* pi,btVector4* po)
1298         {
1299                 btScalar        s[2*NP];
1300                 btVector4       pn[2*NP];
1301                 int                     i, j, m, n, ni;
1302                 // deal with near clipping
1303                 for(i=0, m=0;i<NP;++i)
1304                 {
1305                         s[i]=pi[i][2]+pi[i][3];
1306                         if(s[i]<0) m+=1<<i;
1307                 }
1308                 if(m==((1<<NP)-1)) 
1309                         return(0);
1310                 if(m!=0)
1311                 {
1312                         for(i=NP-1,j=0,n=0;j<NP;i=j++)
1313                         {
1314                                 const btVector4&        a=pi[i];
1315                                 const btVector4&        b=pi[j];
1316                                 const btScalar          t=s[i]/(a[3]+a[2]-b[3]-b[2]);
1317                                 if((t>0)&&(t<1))
1318                                 {
1319                                         pn[n][0]        =       a[0]+(b[0]-a[0])*t;
1320                                         pn[n][1]        =       a[1]+(b[1]-a[1])*t;
1321                                         pn[n][2]        =       a[2]+(b[2]-a[2])*t;
1322                                         pn[n][3]        =       a[3]+(b[3]-a[3])*t;
1323                                         ++n;
1324                                 }
1325                                 if(s[j]>0) pn[n++]=b;
1326                         }
1327                         // ready to test far clipping, start from the modified polygon
1328                         pi = pn;
1329                         ni = n;
1330                 } else
1331                 {
1332                         // no clipping on the near plane, keep same vector
1333                         ni = NP;
1334                 }
1335                 // now deal with far clipping
1336                 for(i=0, m=0;i<ni;++i)
1337                 {
1338                         s[i]=pi[i][2]-pi[i][3];
1339                         if(s[i]>0) m+=1<<i;
1340                 }
1341                 if(m==((1<<ni)-1)) 
1342                         return(0);
1343                 if(m!=0)
1344                 {
1345                         for(i=ni-1,j=0,n=0;j<ni;i=j++)
1346                         {
1347                                 const btVector4&        a=pi[i];
1348                                 const btVector4&        b=pi[j];
1349                                 const btScalar          t=s[i]/(a[2]-a[3]-b[2]+b[3]);
1350                                 if((t>0)&&(t<1))
1351                                 {
1352                                         po[n][0]        =       a[0]+(b[0]-a[0])*t;
1353                                         po[n][1]        =       a[1]+(b[1]-a[1])*t;
1354                                         po[n][2]        =       a[2]+(b[2]-a[2])*t;
1355                                         po[n][3]        =       a[3]+(b[3]-a[3])*t;
1356                                         ++n;
1357                                 }
1358                                 if(s[j]<0) po[n++]=b;
1359                         }
1360                         return(n);
1361                 }
1362                 for(int i=0;i<ni;++i) po[i]=pi[i];
1363                 return(ni);
1364         }
1365         // write or check a triangle to buffer. a,b,c in device coordinates (-1,+1)
1366         template <typename POLICY>
1367         inline bool     draw(   const btVector4& a,
1368                                                 const btVector4& b,
1369                                                 const btVector4& c,
1370                                                 const float face,
1371                                                 const btScalar minarea)
1372         {
1373                 const btScalar          a2=cross(b-a,c-a)[2];
1374                 if((face*a2)<0.f || btFabs(a2)<minarea)
1375                         return false;
1376                 // further down we are normally going to write to the Zbuffer, mark it so
1377                 POLICY::Occlusion(m_occlusion);
1378
1379                 int x[3], y[3], ib=1, ic=2;
1380                 btScalar z[3];
1381                 x[0]=(int)(a.x()*m_scales[0]+m_offsets[0]);
1382                 y[0]=(int)(a.y()*m_scales[1]+m_offsets[1]);
1383                 z[0]=a.z();
1384                 if (a2 < 0.f)
1385                 {
1386                         // negative aire is possible with double face => must
1387                         // change the order of b and c otherwise the algorithm doesn't work
1388                         ib=2;
1389                         ic=1;
1390                 }
1391                 x[ib]=(int)(b.x()*m_scales[0]+m_offsets[0]);
1392                 x[ic]=(int)(c.x()*m_scales[0]+m_offsets[0]);
1393                 y[ib]=(int)(b.y()*m_scales[1]+m_offsets[1]);
1394                 y[ic]=(int)(c.y()*m_scales[1]+m_offsets[1]);
1395                 z[ib]=b.z();
1396                 z[ic]=c.z();
1397                 const int               mix=btMax(0,btMin(x[0],btMin(x[1],x[2])));
1398                 const int               mxx=btMin(m_sizes[0],1+btMax(x[0],btMax(x[1],x[2])));
1399                 const int               miy=btMax(0,btMin(y[0],btMin(y[1],y[2])));
1400                 const int               mxy=btMin(m_sizes[1],1+btMax(y[0],btMax(y[1],y[2])));
1401                 const int               width=mxx-mix;
1402                 const int               height=mxy-miy;
1403                 if ((width*height) <= 1)
1404                 {
1405                         // degenerated in at most one single pixel
1406                         btScalar* scan=&m_buffer[miy*m_sizes[0]+mix];
1407                         // use for loop to detect the case where width or height == 0
1408                         for(int iy=miy;iy<mxy;++iy)
1409                         {
1410                                 for(int ix=mix;ix<mxx;++ix)
1411                                 {
1412                                         if(POLICY::Process(*scan,z[0])) 
1413                                                 return(true);
1414                                         if(POLICY::Process(*scan,z[1])) 
1415                                                 return(true);
1416                                         if(POLICY::Process(*scan,z[2])) 
1417                                                 return(true);
1418                                 }
1419                         }
1420                 } else if (width == 1) 
1421                 {
1422                         // Degenerated in at least 2 vertical lines
1423                         // The algorithm below doesn't work when face has a single pixel width
1424                         // We cannot use general formulas because the plane is degenerated. 
1425                         // We have to interpolate along the 3 edges that overlaps and process each pixel.
1426                         // sort the y coord to make formula simpler
1427                         int ytmp;
1428                         btScalar ztmp;
1429                         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; }
1430                         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; }
1431                         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; }
1432                         int     dy[]={  y[0]-y[1],
1433                                                 y[1]-y[2],
1434                                                 y[2]-y[0]};
1435                         btScalar dzy[3];
1436                         dzy[0] = (dy[0]) ? (z[0]-z[1])/dy[0] : btScalar(0.f);
1437                         dzy[1] = (dy[1]) ? (z[1]-z[2])/dy[1] : btScalar(0.f);
1438                         dzy[2] = (dy[2]) ? (z[2]-z[0])/dy[2] : btScalar(0.f);
1439                         btScalar v[3] = {       dzy[0]*(miy-y[0])+z[0],
1440                                                                 dzy[1]*(miy-y[1])+z[1],
1441                                                                 dzy[2]*(miy-y[2])+z[2] };
1442                         dy[0] = y[1]-y[0];
1443                         dy[1] = y[0]-y[1];
1444                         dy[2] = y[2]-y[0];
1445                         btScalar* scan=&m_buffer[miy*m_sizes[0]+mix];
1446                         for(int iy=miy;iy<mxy;++iy)
1447                         {
1448                                 if(dy[0] >= 0 && POLICY::Process(*scan,v[0])) 
1449                                         return(true);
1450                                 if(dy[1] >= 0 && POLICY::Process(*scan,v[1])) 
1451                                         return(true);
1452                                 if(dy[2] >= 0 && POLICY::Process(*scan,v[2])) 
1453                                         return(true);
1454                                 scan+=m_sizes[0];
1455                                 v[0] += dzy[0]; v[1] += dzy[1]; v[2] += dzy[2];
1456                                 dy[0]--; dy[1]++, dy[2]--;
1457                         }
1458                 } else if (height == 1)
1459                 {
1460                         // Degenerated in at least 2 horizontal lines
1461                         // The algorithm below doesn't work when face has a single pixel width
1462                         // We cannot use general formulas because the plane is degenerated. 
1463                         // We have to interpolate along the 3 edges that overlaps and process each pixel.
1464                         int xtmp;
1465                         btScalar ztmp;
1466                         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; }
1467                         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; }
1468                         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; }
1469                         int     dx[]={  x[0]-x[1],
1470                                                 x[1]-x[2],
1471                                                 x[2]-x[0]};
1472                         btScalar dzx[3];
1473                         dzx[0] = (dx[0]) ? (z[0]-z[1])/dx[0] : btScalar(0.f);
1474                         dzx[1] = (dx[1]) ? (z[1]-z[2])/dx[1] : btScalar(0.f);
1475                         dzx[2] = (dx[2]) ? (z[2]-z[0])/dx[2] : btScalar(0.f);
1476                         btScalar v[3] = { dzx[0]*(mix-x[0])+z[0],
1477                                                           dzx[1]*(mix-x[1])+z[1],
1478                                                           dzx[2]*(mix-x[2])+z[2] };
1479                         dx[0] = x[1]-x[0];
1480                         dx[1] = x[0]-x[1];
1481                         dx[2] = x[2]-x[0];
1482                         btScalar* scan=&m_buffer[miy*m_sizes[0]+mix];
1483                         for(int ix=mix;ix<mxx;++ix)
1484                         {
1485                                 if(dx[0] >= 0 && POLICY::Process(*scan,v[0])) 
1486                                         return(true);
1487                                 if(dx[1] >= 0 && POLICY::Process(*scan,v[1])) 
1488                                         return(true);
1489                                 if(dx[2] >= 0 && POLICY::Process(*scan,v[2])) 
1490                                         return(true);
1491                                 scan++;
1492                                 v[0] += dzx[0]; v[1] += dzx[1]; v[2] += dzx[2];
1493                                 dx[0]--; dx[1]++, dx[2]--;
1494                         }
1495                 } else
1496                 {
1497                         // general case
1498                         const int               dx[]={  y[0]-y[1],
1499                                                                         y[1]-y[2],
1500                                                                         y[2]-y[0]};
1501                         const int               dy[]={  x[1]-x[0]-dx[0]*width,
1502                                                                         x[2]-x[1]-dx[1]*width,
1503                                                                         x[0]-x[2]-dx[2]*width};
1504                         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];
1505                         const btScalar  ia=1/(btScalar)a;
1506                         const btScalar  dzx=ia*(y[2]*(z[1]-z[0])+y[1]*(z[0]-z[2])+y[0]*(z[2]-z[1]));
1507                         const btScalar  dzy=ia*(x[2]*(z[0]-z[1])+x[0]*(z[1]-z[2])+x[1]*(z[2]-z[0]))-(dzx*width);                
1508                         int                             c[]={   miy*x[1]+mix*y[0]-x[1]*y[0]-mix*y[1]+x[0]*y[1]-miy*x[0],
1509                                                                         miy*x[2]+mix*y[1]-x[2]*y[1]-mix*y[2]+x[1]*y[2]-miy*x[1],
1510                                                                         miy*x[0]+mix*y[2]-x[0]*y[2]-mix*y[0]+x[2]*y[0]-miy*x[2]};
1511                         btScalar                v=ia*((z[2]*c[0])+(z[0]*c[1])+(z[1]*c[2]));
1512                         btScalar*               scan=&m_buffer[miy*m_sizes[0]];
1513                         for(int iy=miy;iy<mxy;++iy)
1514                         {
1515                                 for(int ix=mix;ix<mxx;++ix)
1516                                 {
1517                                         if((c[0]>=0)&&(c[1]>=0)&&(c[2]>=0))
1518                                         {
1519                                                 if(POLICY::Process(scan[ix],v)) 
1520                                                         return(true);
1521                                         }
1522                                         c[0]+=dx[0];c[1]+=dx[1];c[2]+=dx[2];v+=dzx;
1523                                 }
1524                                 c[0]+=dy[0];c[1]+=dy[1];c[2]+=dy[2];v+=dzy;
1525                                 scan+=m_sizes[0];
1526                         }
1527                 }
1528                 return(false);
1529         }
1530         // clip than write or check a polygon 
1531         template <const int NP,typename POLICY>
1532         inline bool     clipDraw(       const btVector4* p,
1533                                                         const float face,
1534                                                         btScalar minarea)
1535         {
1536                 btVector4       o[NP*2];
1537                 int                     n=clip<NP>(p,o);
1538                 bool            earlyexit=false;
1539                 if (n)
1540                 {
1541                         project(o,n);
1542                         for(int i=2;i<n && !earlyexit;++i)
1543                         {
1544                                 earlyexit|=draw<POLICY>(o[0],o[i-1],o[i],face,minarea);
1545                         }
1546                 }
1547                 return(earlyexit);
1548         }
1549         // add a triangle (in model coordinate)
1550         // face =  0.f if face is double side, 
1551         //      =  1.f if face is single sided and scale is positive
1552         //      = -1.f if face is single sided and scale is negative
1553         void            appendOccluderM(const float* a,
1554                                                                 const float* b,
1555                                                                 const float* c,
1556                                                                 const float face)
1557         {
1558                 btVector4       p[3];
1559                 transformM(a,p[0]);
1560                 transformM(b,p[1]);
1561                 transformM(c,p[2]);
1562                 clipDraw<3,WriteOCL>(p,face,btScalar(0.f));
1563         }
1564         // add a quad (in model coordinate)
1565         void            appendOccluderM(const float* a,
1566                                                                 const float* b,
1567                                                                 const float* c,
1568                                                                 const float* d,
1569                                                                 const float face)
1570         {       
1571                 btVector4       p[4];
1572                 transformM(a,p[0]);
1573                 transformM(b,p[1]);
1574                 transformM(c,p[2]);
1575                 transformM(d,p[3]);
1576                 clipDraw<4,WriteOCL>(p,face,btScalar(0.f));
1577         }
1578         // query occluder for a box (c=center, e=extend) in world coordinate
1579         inline bool     queryOccluderW( const btVector3& c,
1580                                                                 const btVector3& e)
1581         {
1582                 if (!m_occlusion)
1583                         // no occlusion yet, no need to check
1584                         return true;
1585                 btVector4       x[8];
1586                 transformW(btVector3(c[0]-e[0],c[1]-e[1],c[2]-e[2]),x[0]);
1587                 transformW(btVector3(c[0]+e[0],c[1]-e[1],c[2]-e[2]),x[1]);
1588                 transformW(btVector3(c[0]+e[0],c[1]+e[1],c[2]-e[2]),x[2]);
1589                 transformW(btVector3(c[0]-e[0],c[1]+e[1],c[2]-e[2]),x[3]);
1590                 transformW(btVector3(c[0]-e[0],c[1]-e[1],c[2]+e[2]),x[4]);
1591                 transformW(btVector3(c[0]+e[0],c[1]-e[1],c[2]+e[2]),x[5]);
1592                 transformW(btVector3(c[0]+e[0],c[1]+e[1],c[2]+e[2]),x[6]);
1593                 transformW(btVector3(c[0]-e[0],c[1]+e[1],c[2]+e[2]),x[7]);
1594                 for(int i=0;i<8;++i)
1595                 {
1596                         // the box is clipped, it's probably a large box, don't waste our time to check
1597                         if((x[i][2]+x[i][3])<=0) return(true);
1598                 }
1599                 static const int        d[]={   1,0,3,2,
1600                                                                         4,5,6,7,
1601                                                                         4,7,3,0,
1602                                                                         6,5,1,2,
1603                                                                         7,6,2,3,
1604                                                                         5,4,0,1};
1605                 for(unsigned int i=0;i<(sizeof(d)/sizeof(d[0]));)
1606                 {
1607                         const btVector4 p[]={   x[d[i++]],
1608                                                                         x[d[i++]],
1609                                                                         x[d[i++]],
1610                                                                         x[d[i++]]};
1611                         if(clipDraw<4,QueryOCL>(p,1.f,0.f)) 
1612                                 return(true);
1613                 }
1614                 return(false);
1615         }
1616 };
1617
1618
1619 struct  DbvtCullingCallback : btDbvt::ICollide
1620 {
1621         PHY_CullingCallback m_clientCallback;
1622         void* m_userData;
1623         OcclusionBuffer *m_ocb;
1624
1625         DbvtCullingCallback(PHY_CullingCallback clientCallback, void* userData)
1626         {
1627                 m_clientCallback = clientCallback;
1628                 m_userData = userData;
1629                 m_ocb = NULL;
1630         }
1631         bool Descent(const btDbvtNode* node)
1632         {
1633                 return(m_ocb->queryOccluderW(node->volume.Center(),node->volume.Extents()));
1634         }
1635         void Process(const btDbvtNode* node,btScalar depth)
1636         {
1637                 Process(node);
1638         }
1639         void Process(const btDbvtNode* leaf)
1640         {       
1641                 btBroadphaseProxy*      proxy=(btBroadphaseProxy*)leaf->data;
1642                 // the client object is a graphic controller
1643                 CcdGraphicController* ctrl = static_cast<CcdGraphicController*>(proxy->m_clientObject);
1644                 KX_ClientObjectInfo* info = (KX_ClientObjectInfo*)ctrl->getNewClientInfo();
1645                 if (m_ocb)
1646                 {
1647                         // means we are doing occlusion culling. Check if this object is an occluders
1648                         KX_GameObject* gameobj = KX_GameObject::GetClientObject(info);
1649                         if (gameobj && gameobj->GetOccluder())
1650                         {
1651                                 double* fl = gameobj->GetOpenGLMatrixPtr()->getPointer();
1652                                 // this will create the occlusion buffer if not already done
1653                                 // and compute the transformation from model local space to clip space
1654                                 m_ocb->SetModelMatrix(fl);
1655                                 float face = (gameobj->IsNegativeScaling()) ? -1.0f : 1.0f;
1656                                 // walk through the meshes and for each add to buffer
1657                                 for (int i=0; i<gameobj->GetMeshCount(); i++)
1658                                 {
1659                                         RAS_MeshObject* meshobj = gameobj->GetMesh(i);
1660                                         const float *v1, *v2, *v3, *v4;
1661
1662                                         int polycount = meshobj->NumPolygons();
1663                                         for (int j=0; j<polycount; j++)
1664                                         {
1665                                                 RAS_Polygon* poly = meshobj->GetPolygon(j);
1666                                                 switch (poly->VertexCount())
1667                                                 {
1668                                                 case 3:
1669                                                         v1 = poly->GetVertex(0)->getXYZ();
1670                                                         v2 = poly->GetVertex(1)->getXYZ();
1671                                                         v3 = poly->GetVertex(2)->getXYZ();
1672                                                         m_ocb->appendOccluderM(v1,v2,v3,((poly->IsTwoside())?0.f:face));
1673                                                         break;
1674                                                 case 4:
1675                                                         v1 = poly->GetVertex(0)->getXYZ();
1676                                                         v2 = poly->GetVertex(1)->getXYZ();
1677                                                         v3 = poly->GetVertex(2)->getXYZ();
1678                                                         v4 = poly->GetVertex(3)->getXYZ();
1679                                                         m_ocb->appendOccluderM(v1,v2,v3,v4,((poly->IsTwoside())?0.f:face));
1680                                                         break;
1681                                                 }
1682                                         }
1683                                 }
1684                         }
1685                 }
1686                 if (info)
1687                         (*m_clientCallback)(info, m_userData);
1688         }
1689 };
1690
1691 static OcclusionBuffer gOcb;
1692 bool CcdPhysicsEnvironment::cullingTest(PHY_CullingCallback callback, void* userData, PHY__Vector4 *planes, int nplanes, int occlusionRes)
1693 {
1694         if (!m_cullingTree)
1695                 return false;
1696         DbvtCullingCallback dispatcher(callback, userData);
1697         btVector3 planes_n[6];
1698         btScalar planes_o[6];
1699         if (nplanes > 6)
1700                 nplanes = 6;
1701         for (int i=0; i<nplanes; i++)
1702         {
1703                 planes_n[i].setValue(planes[i][0], planes[i][1], planes[i][2]);
1704                 planes_o[i] = planes[i][3];
1705         }
1706         // if occlusionRes != 0 => occlusion culling
1707         if (occlusionRes)
1708         {
1709                 gOcb.setup(occlusionRes);
1710                 dispatcher.m_ocb = &gOcb;
1711                 // occlusion culling, the direction of the view is taken from the first plan which MUST be the near plane
1712                 btDbvt::collideOCL(m_cullingTree->m_sets[1].m_root,planes_n,planes_o,planes_n[0],nplanes,dispatcher);
1713                 btDbvt::collideOCL(m_cullingTree->m_sets[0].m_root,planes_n,planes_o,planes_n[0],nplanes,dispatcher);           
1714         }else 
1715         {
1716                 btDbvt::collideKDOP(m_cullingTree->m_sets[1].m_root,planes_n,planes_o,nplanes,dispatcher);
1717                 btDbvt::collideKDOP(m_cullingTree->m_sets[0].m_root,planes_n,planes_o,nplanes,dispatcher);              
1718         }
1719         return true;
1720 }
1721
1722 int     CcdPhysicsEnvironment::getNumContactPoints()
1723 {
1724         return 0;
1725 }
1726
1727 void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
1728 {
1729
1730 }
1731
1732
1733
1734
1735 btBroadphaseInterface*  CcdPhysicsEnvironment::getBroadphase()
1736
1737         return m_dynamicsWorld->getBroadphase(); 
1738 }
1739
1740 btDispatcher*   CcdPhysicsEnvironment::getDispatcher()
1741
1742         return m_dynamicsWorld->getDispatcher();
1743 }
1744
1745 void CcdPhysicsEnvironment::MergeEnvironment(CcdPhysicsEnvironment *other)
1746 {
1747         std::set<CcdPhysicsController*>::iterator it;
1748
1749         while (other->m_controllers.begin() != other->m_controllers.end())
1750         {
1751                 it= other->m_controllers.begin();
1752                 CcdPhysicsController* ctrl= (*it);
1753
1754                 other->removeCcdPhysicsController(ctrl);
1755                 this->addCcdPhysicsController(ctrl);
1756         }
1757 }
1758
1759 CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
1760 {
1761
1762 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1763         m_wrapperVehicles.clear();
1764 #endif //NEW_BULLET_VEHICLE_SUPPORT
1765
1766         //m_broadphase->DestroyScene();
1767         //delete broadphase ? release reference on broadphase ?
1768
1769         //first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher
1770         //delete m_dispatcher;
1771         delete m_dynamicsWorld;
1772         
1773
1774         if (NULL != m_ownPairCache)
1775                 delete m_ownPairCache;
1776
1777         if (NULL != m_ownDispatcher)
1778                 delete m_ownDispatcher;
1779
1780         if (NULL != m_solver)
1781                 delete m_solver;
1782
1783         if (NULL != m_debugDrawer)
1784                 delete m_debugDrawer;
1785
1786         if (NULL != m_filterCallback)
1787                 delete m_filterCallback;
1788
1789         if (NULL != m_collisionConfiguration)
1790                 delete m_collisionConfiguration;
1791
1792         if (NULL != m_broadphase)
1793                 delete m_broadphase;
1794
1795         if (NULL != m_cullingTree)
1796                 delete m_cullingTree;
1797
1798         if (NULL != m_cullingCache)
1799                 delete m_cullingCache;
1800
1801 }
1802
1803
1804 float   CcdPhysicsEnvironment::getConstraintParam(int constraintId,int param)
1805 {
1806         btTypedConstraint* typedConstraint = getConstraintById(constraintId);
1807         switch (typedConstraint->getUserConstraintType())
1808         {
1809         case PHY_GENERIC_6DOF_CONSTRAINT:
1810                 {
1811                         
1812                         switch (param)
1813                         {
1814                         case 0: case 1: case 2: 
1815                                 {
1816                                         //param = 0..2 are linear constraint values
1817                                         btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
1818                                         genCons->calculateTransforms();
1819                                         return genCons->getRelativePivotPosition(param);
1820                                         break;
1821                                 }
1822                                 case 3: case 4: case 5:
1823                                 {
1824                                         //param = 3..5 are relative constraint (Euler) angles
1825                                         btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
1826                                         genCons->calculateTransforms();
1827                                         return genCons->getAngle(param-3);
1828                                         break;
1829                                 }
1830                         default:
1831                                 {
1832                                 }
1833                         }
1834                         break;
1835                 };
1836         default:
1837                 {
1838                 };
1839         };
1840         return 0.f;
1841 }
1842
1843 void    CcdPhysicsEnvironment::setConstraintParam(int constraintId,int param,float value0,float value1)
1844 {
1845         btTypedConstraint* typedConstraint = getConstraintById(constraintId);
1846         switch (typedConstraint->getUserConstraintType())
1847         {
1848         case PHY_GENERIC_6DOF_CONSTRAINT:
1849                 {
1850                         
1851                         switch (param)
1852                         {
1853                         case 0: case 1: case 2: case 3: case 4: case 5:
1854                                 {
1855                                         //param = 0..5 are constraint limits, with low/high limit value
1856                                         btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
1857                                         genCons->setLimit(param,value0,value1);
1858                                         break;
1859                                 }
1860                         case 6: case 7: case 8:
1861                                 {
1862                                         //param = 6,7,8 are translational motors, with value0=target velocity, value1 = max motor force
1863                                         btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
1864                                         int transMotorIndex = param-6;
1865                                         btTranslationalLimitMotor* transMotor = genCons->getTranslationalLimitMotor();
1866                                         transMotor->m_targetVelocity[transMotorIndex]= value0;
1867                                         transMotor->m_maxMotorForce[transMotorIndex]=value1;
1868                                         transMotor->m_enableMotor[transMotorIndex] = (value1>0.f);
1869                                         break;
1870                                 }
1871                         case 9: case 10: case 11:
1872                                 {
1873                                         //param = 9,10,11 are rotational motors, with value0=target velocity, value1 = max motor force
1874                                         btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
1875                                         int angMotorIndex = param-9;
1876                                         btRotationalLimitMotor* rotMotor = genCons->getRotationalLimitMotor(angMotorIndex);
1877                                         rotMotor->m_enableMotor = (value1 > 0.f);
1878                                         rotMotor->m_targetVelocity = value0;
1879                                         rotMotor->m_maxMotorForce = value1;
1880                                         break;
1881                                 }
1882
1883                         case 12: case 13: case 14: case 15: case 16: case 17:
1884                         {
1885                                 //param 13-17 are for motorized springs on each of the degrees of freedom
1886                                         btGeneric6DofSpringConstraint* genCons = (btGeneric6DofSpringConstraint*)typedConstraint;
1887                                         int springIndex = param-12;
1888                                         if (value0!=0.f)
1889                                         {
1890                                                 bool springEnabled = true;
1891                                                 genCons->setStiffness(springIndex,value0);
1892                                                 genCons->setDamping(springIndex,value1);
1893                                                 genCons->enableSpring(springIndex,springEnabled);
1894                                                 genCons->setEquilibriumPoint(springIndex);
1895                                         } else
1896                                         {
1897                                                 bool springEnabled = false;
1898                                                 genCons->enableSpring(springIndex,springEnabled);
1899                                         }
1900                                         break;
1901                         }
1902
1903                         default:
1904                                 {
1905                                 }
1906                         }
1907                         break;
1908                 };
1909         default:
1910                 {
1911                 };
1912         };
1913 }
1914
1915 btTypedConstraint*      CcdPhysicsEnvironment::getConstraintById(int constraintId)
1916 {
1917
1918         int numConstraints = m_dynamicsWorld->getNumConstraints();
1919         int i;
1920         for (i=0;i<numConstraints;i++)
1921         {
1922                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
1923                 if (constraint->getUserConstraintId()==constraintId)
1924                 {
1925                         return constraint;
1926                 }
1927         }
1928         return 0;
1929 }
1930
1931
1932 void CcdPhysicsEnvironment::addSensor(PHY_IPhysicsController* ctrl)
1933 {
1934
1935         CcdPhysicsController* ctrl1 = (CcdPhysicsController* )ctrl;
1936         // addSensor() is a "light" function for bullet because it is used
1937         // dynamically when the sensor is activated. Use enableCcdPhysicsController() instead 
1938         //if (m_controllers.insert(ctrl1).second)
1939         //{
1940         //      addCcdPhysicsController(ctrl1);
1941         //}
1942         enableCcdPhysicsController(ctrl1);
1943 }
1944
1945 bool CcdPhysicsEnvironment::removeCollisionCallback(PHY_IPhysicsController* ctrl)
1946 {
1947         CcdPhysicsController* ccdCtrl = (CcdPhysicsController*)ctrl;
1948         if (!ccdCtrl->Unregister())
1949                 return false;
1950         m_triggerControllers.erase(ccdCtrl);
1951         return true;
1952 }
1953
1954
1955 void CcdPhysicsEnvironment::removeSensor(PHY_IPhysicsController* ctrl)
1956 {
1957         disableCcdPhysicsController((CcdPhysicsController*)ctrl);
1958 }
1959
1960 void CcdPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user)
1961 {
1962         /*      printf("addTouchCallback\n(response class = %i)\n",response_class);
1963
1964         //map PHY_ convention into SM_ convention
1965         switch (response_class)
1966         {
1967         case    PHY_FH_RESPONSE:
1968         printf("PHY_FH_RESPONSE\n");
1969         break;
1970         case PHY_SENSOR_RESPONSE:
1971         printf("PHY_SENSOR_RESPONSE\n");
1972         break;
1973         case PHY_CAMERA_RESPONSE:
1974         printf("PHY_CAMERA_RESPONSE\n");
1975         break;
1976         case PHY_OBJECT_RESPONSE:
1977         printf("PHY_OBJECT_RESPONSE\n");
1978         break;
1979         case PHY_STATIC_RESPONSE:
1980         printf("PHY_STATIC_RESPONSE\n");
1981         break;
1982         default:
1983         assert(0);
1984         return;
1985         }
1986         */
1987
1988         m_triggerCallbacks[response_class] = callback;
1989         m_triggerCallbacksUserPtrs[response_class] = user;
1990
1991 }
1992 bool CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctrl)
1993 {
1994         CcdPhysicsController* ccdCtrl = static_cast<CcdPhysicsController*>(ctrl);
1995
1996         if (!ccdCtrl->Register())
1997                 return false;
1998         m_triggerControllers.insert(ccdCtrl);
1999         return true;
2000 }
2001
2002 void    CcdPhysicsEnvironment::CallbackTriggers()
2003 {
2004         if (m_triggerCallbacks[PHY_OBJECT_RESPONSE] || (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)))
2005         {
2006                 //walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback
2007                 btDispatcher* dispatcher = m_dynamicsWorld->getDispatcher();
2008                 int numManifolds = dispatcher->getNumManifolds();
2009                 for (int i=0;i<numManifolds;i++)
2010                 {
2011                         btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
2012                         int numContacts = manifold->getNumContacts();
2013                         if (numContacts)
2014                         {
2015                                 btRigidBody* rb0 = static_cast<btRigidBody*>(manifold->getBody0());
2016                                 btRigidBody* rb1 = static_cast<btRigidBody*>(manifold->getBody1());
2017                                 if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints))
2018                                 {
2019                                         for (int j=0;j<numContacts;j++)
2020                                         {
2021                                                 btVector3 color(1,0,0);
2022                                                 const btManifoldPoint& cp = manifold->getContactPoint(j);
2023                                                 if (m_debugDrawer)
2024                                                         m_debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
2025                                         }
2026                                 }
2027                                 btRigidBody* obj0 = rb0;
2028                                 btRigidBody* obj1 = rb1;
2029
2030                                 //m_internalOwner is set in 'addPhysicsController'
2031                                 CcdPhysicsController* ctrl0 = static_cast<CcdPhysicsController*>(obj0->getUserPointer());
2032                                 CcdPhysicsController* ctrl1 = static_cast<CcdPhysicsController*>(obj1->getUserPointer());
2033
2034                                 std::set<CcdPhysicsController*>::const_iterator i = m_triggerControllers.find(ctrl0);
2035                                 if (i == m_triggerControllers.end())
2036                                 {
2037                                         i = m_triggerControllers.find(ctrl1);
2038                                 }
2039
2040                                 if (!(i == m_triggerControllers.end()))
2041                                 {
2042                                         m_triggerCallbacks[PHY_OBJECT_RESPONSE](m_triggerCallbacksUserPtrs[PHY_OBJECT_RESPONSE],
2043                                                 ctrl0,ctrl1,0);
2044                                 }
2045                                 // Bullet does not refresh the manifold contact point for object without contact response
2046                                 // may need to remove this when a newer Bullet version is integrated
2047                                 if (!dispatcher->needsResponse(rb0, rb1))
2048                                 {
2049                                         // Refresh algorithm fails sometimes when there is penetration 
2050                                         // (usuall the case with ghost and sensor objects)
2051                                         // Let's just clear the manifold, in any case, it is recomputed on each frame.
2052                                         manifold->clearManifold(); //refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
2053                                 }
2054                         }
2055                 }
2056
2057
2058
2059         }
2060
2061
2062 }
2063
2064 // This call back is called before a pair is added in the cache
2065 // Handy to remove objects that must be ignored by sensors
2066 bool CcdOverlapFilterCallBack::needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
2067 {
2068         btCollisionObject *colObj0, *colObj1;
2069         CcdPhysicsController *sensorCtrl, *objCtrl;
2070         bool collides;
2071         // first check the filters
2072         collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
2073         collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
2074         if (!collides)
2075                 return false;
2076
2077         // additional check for sensor object
2078         if (proxy0->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger)
2079         {
2080                 // this is a sensor object, the other one can't be a sensor object because 
2081                 // they exclude each other in the above test
2082                 assert(!(proxy1->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger));
2083                 colObj0 = (btCollisionObject*)proxy0->m_clientObject;
2084                 colObj1 = (btCollisionObject*)proxy1->m_clientObject;
2085         }
2086         else if (proxy1->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger)
2087         {
2088                 colObj0 = (btCollisionObject*)proxy1->m_clientObject;
2089                 colObj1 = (btCollisionObject*)proxy0->m_clientObject;
2090         }
2091         else
2092         {
2093                 return true;
2094         }
2095         if (!colObj0 || !colObj1)
2096                 return false;
2097         sensorCtrl = static_cast<CcdPhysicsController*>(colObj0->getUserPointer());
2098         objCtrl = static_cast<CcdPhysicsController*>(colObj1->getUserPointer());
2099         if (m_physEnv->m_triggerCallbacks[PHY_BROADPH_RESPONSE])
2100         {
2101                 return m_physEnv->m_triggerCallbacks[PHY_BROADPH_RESPONSE](m_physEnv->m_triggerCallbacksUserPtrs[PHY_BROADPH_RESPONSE], sensorCtrl, objCtrl, 0);
2102         }
2103         return true;
2104 }
2105
2106
2107 #ifdef NEW_BULLET_VEHICLE_SUPPORT
2108
2109 //complex constraint for vehicles
2110 PHY_IVehicle*   CcdPhysicsEnvironment::getVehicleConstraint(int constraintId)
2111 {
2112         int i;
2113
2114         int numVehicles = m_wrapperVehicles.size();
2115         for (i=0;i<numVehicles;i++)
2116         {
2117                 WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
2118                 if (wrapperVehicle->GetVehicle()->getUserConstraintId() == constraintId)
2119                         return wrapperVehicle;
2120         }
2121
2122         return 0;
2123 }
2124
2125 #endif //NEW_BULLET_VEHICLE_SUPPORT
2126
2127
2128 int currentController = 0;
2129 int numController = 0;
2130
2131
2132
2133
2134 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateSphereController(float radius,const PHY__Vector3& position)
2135 {
2136         
2137         CcdConstructionInfo     cinfo;
2138         memset(&cinfo, 0, sizeof(cinfo)); /* avoid uninitialized values */
2139         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
2140         cinfo.m_MotionState = 0;
2141         cinfo.m_physicsEnv = this;
2142         // declare this object as Dyamic rather then static!!
2143         // The reason as it is designed to detect all type of object, including static object
2144         // It would cause static-static message to be printed on the console otherwise
2145         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE | btCollisionObject::CF_STATIC_OBJECT;
2146         DefaultMotionState* motionState = new DefaultMotionState();
2147         cinfo.m_MotionState = motionState;
2148         // we will add later the possibility to select the filter from option
2149         cinfo.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
2150         cinfo.m_collisionFilterGroup = CcdConstructionInfo::SensorFilter;
2151         cinfo.m_bSensor = true;
2152         motionState->m_worldTransform.setIdentity();
2153         motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
2154
2155         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
2156         
2157         return sphereController;
2158 }
2159
2160 int findClosestNode(btSoftBody* sb,const btVector3& worldPoint);
2161 int findClosestNode(btSoftBody* sb,const btVector3& worldPoint)
2162 {
2163         int node = -1;
2164
2165         btSoftBody::tNodeArray&   nodes(sb->m_nodes);
2166         float maxDistSqr = 1e30f;
2167
2168         for (int n=0;n<nodes.size();n++)
2169         {
2170                 btScalar distSqr = (nodes[n].m_x - worldPoint).length2();
2171                 if (distSqr<maxDistSqr)
2172                 {
2173                         maxDistSqr = distSqr;
2174                         node = n;
2175                 }
2176         }
2177         return node;
2178 }
2179
2180 int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
2181                                                                                                         float pivotX,float pivotY,float pivotZ,
2182                                                                                                         float axisX,float axisY,float axisZ,
2183                                                                                                         float axis1X,float axis1Y,float axis1Z,
2184                                                                                                         float axis2X,float axis2Y,float axis2Z,int flags
2185                                                                                                         )
2186 {
2187
2188         bool disableCollisionBetweenLinkedBodies = (0!=(flags & CCD_CONSTRAINT_DISABLE_LINKED_COLLISION));
2189
2190
2191
2192         CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
2193         CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
2194
2195         btRigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
2196         btRigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
2197
2198         
2199
2200
2201         bool rb0static = rb0 ? rb0->isStaticOrKinematicObject() : true;
2202         bool rb1static = rb1 ? rb1->isStaticOrKinematicObject() : true;
2203
2204         btCollisionObject* colObj0 = c0->GetCollisionObject();
2205         if (!colObj0)
2206         {
2207                 return 0;
2208         }
2209
2210         btVector3 pivotInA(pivotX,pivotY,pivotZ);
2211
2212         
2213
2214         //it might be a soft body, let's try
2215         btSoftBody* sb0 = c0 ? c0->GetSoftBody() : 0;
2216         btSoftBody* sb1 = c1 ? c1->GetSoftBody() : 0;
2217         if (sb0 && sb1)
2218         {
2219                 //not between two soft bodies?
2220                 return 0;
2221         }
2222
2223         if (sb0)
2224         {
2225                 //either cluster or node attach, let's find closest node first
2226                 //the soft body doesn't have a 'real' world transform, so get its initial world transform for now
2227                 btVector3 pivotPointSoftWorld = sb0->m_initialWorldTransform(pivotInA);
2228                 int node=findClosestNode(sb0,pivotPointSoftWorld);
2229                 if (node >=0)
2230                 {
2231                         bool clusterconstaint = false;
2232 /*
2233                         switch (type)
2234                         {
2235                         case PHY_LINEHINGE_CONSTRAINT:
2236                                 {
2237                                         if (sb0->clusterCount() && rb1)
2238                                         {
2239                                                 btSoftBody::LJoint::Specs       ls;
2240                                                 ls.erp=0.5f;
2241                                                 ls.position=sb0->clusterCom(0);
2242                                                 sb0->appendLinearJoint(ls,rb1);
2243                                                 clusterconstaint = true;
2244                                                 break;
2245                                         }
2246                                 }
2247                         case PHY_GENERIC_6DOF_CONSTRAINT:
2248                                 {
2249                                         if (sb0->clusterCount() && rb1)
2250                                         {
2251                                                 btSoftBody::AJoint::Specs as;
2252                                                 as.erp = 1;
2253                                                 as.cfm = 1;
2254                                                 as.axis.setValue(axisX,axisY,axisZ);
2255                                                 sb0->appendAngularJoint(as,rb1);
2256                                                 clusterconstaint = true;
2257                                                 break;
2258                                         }
2259
2260                                         break;
2261                                 }
2262                         default:
2263                                 {
2264                                 
2265                                 }
2266                         };
2267                         */
2268
2269                         if (!clusterconstaint)
2270                         {
2271                                 if (rb1)
2272                                 {
2273                                         sb0->appendAnchor(node,rb1,disableCollisionBetweenLinkedBodies);
2274                                 } else
2275                                 {
2276                                         sb0->setMass(node,0.f);
2277                                 }
2278                         }
2279
2280                         
2281                 }
2282                 return 0;//can't remove soft body anchors yet
2283         }
2284
2285         if (sb1)
2286         {
2287                 btVector3 pivotPointAWorld = colObj0->getWorldTransform()(pivotInA);
2288                 int node=findClosestNode(sb1,pivotPointAWorld);
2289                 if (node >=0)
2290                 {
2291                         bool clusterconstaint = false;
2292
2293                         /*
2294                         switch (type)
2295                         {
2296                         case PHY_LINEHINGE_CONSTRAINT:
2297                                 {
2298                                         if (sb1->clusterCount() && rb0)
2299                                         {
2300                                                 btSoftBody::LJoint::Specs       ls;
2301                                                 ls.erp=0.5f;
2302                                                 ls.position=sb1->clusterCom(0);
2303                                                 sb1->appendLinearJoint(ls,rb0);
2304                                                 clusterconstaint = true;
2305                                                 break;
2306                                         }
2307                                 }
2308                         case PHY_GENERIC_6DOF_CONSTRAINT:
2309                                 {
2310                                         if (sb1->clusterCount() && rb0)
2311                                         {
2312                                                 btSoftBody::AJoint::Specs as;
2313                                                 as.erp = 1;
2314                                                 as.cfm = 1;
2315                                                 as.axis.setValue(axisX,axisY,axisZ);
2316                                                 sb1->appendAngularJoint(as,rb0);
2317                                                 clusterconstaint = true;
2318                                                 break;
2319                                         }
2320
2321                                         break;
2322                                 }
2323                         default:
2324                                 {
2325                                         
2326
2327                                 }
2328                         };*/
2329
2330
2331                         if (!clusterconstaint)
2332                         {
2333                                 if (rb0)
2334                                 {
2335                                         sb1->appendAnchor(node,rb0,disableCollisionBetweenLinkedBodies);
2336                                 } else
2337                                 {
2338                                         sb1->setMass(node,0.f);
2339                                 }
2340                         }
2341                         
2342
2343                 }
2344                 return 0;//can't remove soft body anchors yet
2345         }
2346
2347         if (rb0static && rb1static)
2348         {
2349                 
2350                 return 0;
2351         }
2352         
2353
2354         if (!rb0)
2355                 return 0;
2356
2357         
2358         btVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : 
2359                 rb0->getCenterOfMassTransform() * pivotInA;
2360         btVector3 axisInA(axisX,axisY,axisZ);
2361
2362
2363         bool angularOnly = false;
2364
2365         switch (type)
2366         {
2367         case PHY_POINT2POINT_CONSTRAINT:
2368                 {
2369
2370                         btPoint2PointConstraint* p2p = 0;
2371
2372                         if (rb1)
2373                         {
2374                                 p2p = new btPoint2PointConstraint(*rb0,
2375                                         *rb1,pivotInA,pivotInB);
2376                         } else
2377                         {
2378                                 p2p = new btPoint2PointConstraint(*rb0,
2379                                         pivotInA);
2380                         }
2381
2382                         m_dynamicsWorld->addConstraint(p2p,disableCollisionBetweenLinkedBodies);
2383 //                      m_constraints.push_back(p2p);
2384
2385                         p2p->setUserConstraintId(gConstraintUid++);
2386                         p2p->setUserConstraintType(type);
2387                         //64 bit systems can't cast pointer to int. could use size_t instead.
2388                         return p2p->getUserConstraintId();
2389
2390                         break;
2391                 }
2392
2393         case PHY_GENERIC_6DOF_CONSTRAINT:
2394                 {
2395                         btGeneric6DofConstraint* genericConstraint = 0;
2396
2397                         if (rb1)
2398                         {
2399                                 btTransform frameInA;
2400                                 btTransform frameInB;
2401                                 
2402                                 btVector3 axis1(axis1X,axis1Y,axis1Z), axis2(axis2X,axis2Y,axis2Z);
2403                                 if (axis1.length() == 0.0)
2404                                 {
2405                                         btPlaneSpace1( axisInA, axis1, axis2 );
2406                                 }
2407                                 
2408                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
2409                                                                   axisInA.y(), axis1.y(), axis2.y(),
2410                                                                                           axisInA.z(), axis1.z(), axis2.z() );
2411                                 frameInA.setOrigin( pivotInA );
2412
2413                                 btTransform inv = rb1->getCenterOfMassTransform().inverse();
2414
2415                                 btTransform globalFrameA = rb0->getCenterOfMassTransform() * frameInA;
2416                                 
2417                                 frameInB = inv  * globalFrameA;
2418                                 bool useReferenceFrameA = true;
2419
2420                                 genericConstraint = new btGeneric6DofSpringConstraint(
2421                                         *rb0,*rb1,
2422                                         frameInA,frameInB,useReferenceFrameA);
2423
2424
2425                         } else
2426                         {
2427                                 static btRigidBody s_fixedObject2( 0,0,0);
2428                                 btTransform frameInA;
2429                                 btTransform frameInB;
2430                                 
2431                                 btVector3 axis1, axis2;
2432                                 btPlaneSpace1( axisInA, axis1, axis2 );
2433
2434                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
2435                                                                   axisInA.y(), axis1.y(), axis2.y(),
2436                                                                                           axisInA.z(), axis1.z(), axis2.z() );
2437
2438                                 frameInA.setOrigin( pivotInA );
2439
2440                                 ///frameInB in worldspace
2441                                 frameInB = rb0->getCenterOfMassTransform() * frameInA;
2442
2443                                 bool useReferenceFrameA = true;
2444                                 genericConstraint = new btGeneric6DofSpringConstraint(
2445                                         *rb0,s_fixedObject2,
2446                                         frameInA,frameInB,useReferenceFrameA);
2447                         }
2448                         
2449                         if (genericConstraint)
2450                         {
2451                                 //m_constraints.push_back(genericConstraint);
2452                                 m_dynamicsWorld->addConstraint(genericConstraint,disableCollisionBetweenLinkedBodies);
2453                                 genericConstraint->setUserConstraintId(gConstraintUid++);
2454                                 genericConstraint->setUserConstraintType(type);
2455                                 //64 bit systems can't cast pointer to int. could use size_t instead.
2456                                 return genericConstraint->getUserConstraintId();
2457                         } 
2458
2459                         break;
2460                 }
2461         case PHY_CONE_TWIST_CONSTRAINT:
2462                 {
2463                         btConeTwistConstraint* coneTwistContraint = 0;
2464
2465                         
2466                         if (rb1)
2467                         {
2468                                 btTransform frameInA;
2469                                 btTransform frameInB;
2470                                 
2471                                 btVector3 axis1(axis1X,axis1Y,axis1Z), axis2(axis2X,axis2Y,axis2Z);
2472                                 if (axis1.length() == 0.0)
2473                                 {
2474                                         btPlaneSpace1( axisInA, axis1, axis2 );
2475                                 }
2476                                 
2477                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
2478                                                                   axisInA.y(), axis1.y(), axis2.y(),
2479                                                                                           axisInA.z(), axis1.z(), axis2.z() );
2480                                 frameInA.setOrigin( pivotInA );
2481
2482                                 btTransform inv = rb1->getCenterOfMassTransform().inverse();
2483
2484                                 btTransform globalFrameA = rb0->getCenterOfMassTransform() * frameInA;
2485                                 
2486                                 frameInB = inv  * globalFrameA;
2487                                 
2488                                 coneTwistContraint = new btConeTwistConstraint( *rb0,*rb1,
2489                                         frameInA,frameInB);
2490
2491
2492                         } else
2493                         {
2494                                 static btRigidBody s_fixedObject2( 0,0,0);
2495                                 btTransform frameInA;
2496                                 btTransform frameInB;
2497                                 
2498                                 btVector3 axis1, axis2;
2499                                 btPlaneSpace1( axisInA, axis1, axis2 );
2500
2501                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
2502                                                                   axisInA.y(), axis1.y(), axis2.y(),
2503                                                                                           axisInA.z(), axis1.z(), axis2.z() );
2504
2505                                 frameInA.setOrigin( pivotInA );
2506
2507                                 ///frameInB in worldspace
2508                                 frameInB = rb0->getCenterOfMassTransform() * frameInA;
2509
2510                                 coneTwistContraint = new btConeTwistConstraint(
2511                                         *rb0,s_fixedObject2,
2512                                         frameInA,frameInB);
2513                         }
2514                         
2515                         if (coneTwistContraint)
2516                         {
2517                                 //m_constraints.push_back(genericConstraint);
2518                                 m_dynamicsWorld->addConstraint(coneTwistContraint,disableCollisionBetweenLinkedBodies);
2519                                 coneTwistContraint->setUserConstraintId(gConstraintUid++);
2520                                 coneTwistContraint->setUserConstraintType(type);
2521                                 //64 bit systems can't cast pointer to int. could use size_t instead.
2522                                 return coneTwistContraint->getUserConstraintId();
2523                         } 
2524
2525
2526
2527                         break;
2528                 }
2529         case PHY_ANGULAR_CONSTRAINT:
2530                 angularOnly = true;
2531
2532
2533         case PHY_LINEHINGE_CONSTRAINT:
2534                 {
2535                         btHingeConstraint* hinge = 0;
2536
2537                         if (rb1)
2538                         {
2539                                 btVector3 axisInB = rb1 ? 
2540                                 (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) : 
2541                                 rb0->getCenterOfMassTransform().getBasis() * axisInA;
2542
2543                                 hinge = new btHingeConstraint(
2544                                         *rb0,
2545                                         *rb1,pivotInA,pivotInB,axisInA,axisInB);
2546
2547
2548                         } else
2549                         {
2550                                 hinge = new btHingeConstraint(*rb0,
2551                                         pivotInA,axisInA);
2552
2553                         }
2554                         hinge->setAngularOnly(angularOnly);
2555
2556                         //m_constraints.push_back(hinge);
2557                         m_dynamicsWorld->addConstraint(hinge,disableCollisionBetweenLinkedBodies);
2558                         hinge->setUserConstraintId(gConstraintUid++);
2559                         hinge->setUserConstraintType(type);
2560                         //64 bit systems can't cast pointer to int. could use size_t instead.
2561                         return hinge->getUserConstraintId();
2562                         break;
2563                 }
2564 #ifdef NEW_BULLET_VEHICLE_SUPPORT
2565
2566         case PHY_VEHICLE_CONSTRAINT:
2567                 {
2568                         btRaycastVehicle::btVehicleTuning* tuning = new btRaycastVehicle::btVehicleTuning();
2569                         btRigidBody* chassis = rb0;
2570                         btDefaultVehicleRaycaster* raycaster = new btDefaultVehicleRaycaster(m_dynamicsWorld);
2571                         btRaycastVehicle* vehicle = new btRaycastVehicle(*tuning,chassis,raycaster);
2572                         WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
2573                         m_wrapperVehicles.push_back(wrapperVehicle);
2574                         m_dynamicsWorld->addVehicle(vehicle);
2575                         vehicle->setUserConstraintId(gConstraintUid++);
2576                         vehicle->setUserConstraintType(type);
2577                         return vehicle->getUserConstraintId();
2578
2579                         break;
2580                 };
2581 #endif //NEW_BULLET_VEHICLE_SUPPORT
2582
2583         default:
2584                 {
2585                 }
2586         };
2587
2588         //btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB
2589
2590         return 0;
2591
2592 }
2593
2594
2595
2596 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float coneradius,float coneheight)
2597 {
2598         CcdConstructionInfo     cinfo;
2599         memset(&cinfo, 0, sizeof(cinfo)); /* avoid uninitialized values */
2600         // we don't need a CcdShapeConstructionInfo for this shape:
2601         // it is simple enough for the standard copy constructor (see CcdPhysicsController::GetReplica)
2602         cinfo.m_collisionShape = new btConeShape(coneradius,coneheight);
2603         cinfo.m_MotionState = 0;
2604         cinfo.m_physicsEnv = this;
2605         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE | btCollisionObject::CF_STATIC_OBJECT;
2606         DefaultMotionState* motionState = new DefaultMotionState();
2607         cinfo.m_MotionState = motionState;
2608         
2609         // we will add later the possibility to select the filter from option
2610         cinfo.m_collisionFilterMask = CcdConstructionInfo::AllFilter ^ CcdConstructionInfo::SensorFilter;
2611         cinfo.m_collisionFilterGroup = CcdConstructionInfo::SensorFilter;
2612         cinfo.m_bSensor = true;
2613         motionState->m_worldTransform.setIdentity();
2614 //      motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
2615
2616         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
2617
2618
2619         return sphereController;
2620 }
2621         
2622 float           CcdPhysicsEnvironment::getAppliedImpulse(int    constraintid)
2623 {
2624         int i;
2625         int numConstraints = m_dynamicsWorld->getNumConstraints();
2626         for (i=0;i<numConstraints;i++)
2627         {
2628                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
2629                 if (constraint->getUserConstraintId() == constraintid)
2630                 {
2631                         return constraint->getAppliedImpulse();
2632                 }
2633         }
2634
2635         return 0.f;
2636 }