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