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