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