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