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