Fix for bug #5345, sculpt tool doesn't seems to use correctly the undo setting on...
[blender-staging.git] / source / gameengine / Physics / Bullet / CcdPhysicsEnvironment.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose, 
8 including commercial applications, and to alter it and redistribute it freely, 
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16
17
18
19 #include "CcdPhysicsEnvironment.h"
20 #include "CcdPhysicsController.h"
21
22 #include <algorithm>
23 #include "btBulletDynamicsCommon.h"
24 #include "LinearMath/btIDebugDraw.h"
25 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
26
27 //profiling/timings
28 #include "LinearMath/btQuickprof.h"
29
30
31 #include "PHY_IMotionState.h"
32
33
34 bool useIslands = true;
35
36 #ifdef NEW_BULLET_VEHICLE_SUPPORT
37 #include "BulletDynamics/Vehicle/btRaycastVehicle.h"
38 #include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
39 #include "BulletDynamics/Vehicle/btWheelInfo.h"
40 #include "PHY_IVehicle.h"
41 btRaycastVehicle::btVehicleTuning       gTuning;
42
43 #endif //NEW_BULLET_VEHICLE_SUPPORT
44 #include "LinearMath/btAabbUtil2.h"
45
46
47 #ifdef WIN32
48 void DrawRasterizerLine(const float* from,const float* to,int color);
49 #endif
50
51
52 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
53
54
55 #include <stdio.h>
56
57 #ifdef NEW_BULLET_VEHICLE_SUPPORT
58 class WrapperVehicle : public PHY_IVehicle
59 {
60
61         btRaycastVehicle*       m_vehicle;
62         PHY_IPhysicsController* m_chassis;
63
64 public:
65
66         WrapperVehicle(btRaycastVehicle* vehicle,PHY_IPhysicsController* chassis)
67                 :m_vehicle(vehicle),
68                 m_chassis(chassis)
69         {
70         }
71
72         btRaycastVehicle*       GetVehicle()
73         {
74                 return m_vehicle;
75         }
76
77         PHY_IPhysicsController* GetChassis()
78         {
79                 return m_chassis;
80         }
81
82         virtual void    AddWheel(
83                 PHY_IMotionState*       motionState,
84                 PHY__Vector3    connectionPoint,
85                 PHY__Vector3    downDirection,
86                 PHY__Vector3    axleDirection,
87                 float   suspensionRestLength,
88                 float wheelRadius,
89                 bool hasSteering
90                 )
91         {
92                 btVector3 connectionPointCS0(connectionPoint[0],connectionPoint[1],connectionPoint[2]);
93                 btVector3 wheelDirectionCS0(downDirection[0],downDirection[1],downDirection[2]);
94                 btVector3 wheelAxle(axleDirection[0],axleDirection[1],axleDirection[2]);
95
96
97                 btWheelInfo& info = m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxle,
98                         suspensionRestLength,wheelRadius,gTuning,hasSteering);
99                 info.m_clientInfo = motionState;
100
101         }
102
103         void    SyncWheels()
104         {
105                 int numWheels = GetNumWheels();
106                 int i;
107                 for (i=0;i<numWheels;i++)
108                 {
109                         btWheelInfo& info = m_vehicle->getWheelInfo(i);
110                         PHY_IMotionState* motionState = (PHY_IMotionState*)info.m_clientInfo ;
111                         m_vehicle->updateWheelTransform(i,true);
112                         btTransform trans = m_vehicle->getWheelInfo(i).m_worldTransform;
113                         btQuaternion orn = trans.getRotation();
114                         const btVector3& pos = trans.getOrigin();
115                         motionState->setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]);
116                         motionState->setWorldPosition(pos.x(),pos.y(),pos.z());
117
118                 }
119         }
120
121         virtual int             GetNumWheels() const
122         {
123                 return m_vehicle->getNumWheels();
124         }
125
126         virtual void    GetWheelPosition(int wheelIndex,float& posX,float& posY,float& posZ) const
127         {
128                 btTransform     trans = m_vehicle->getWheelTransformWS(wheelIndex);
129                 posX = trans.getOrigin().x();
130                 posY = trans.getOrigin().y();
131                 posZ = trans.getOrigin().z();
132         }
133         virtual void    GetWheelOrientationQuaternion(int wheelIndex,float& quatX,float& quatY,float& quatZ,float& quatW) const
134         {
135                 btTransform     trans = m_vehicle->getWheelTransformWS(wheelIndex);
136                 btQuaternion quat = trans.getRotation();
137                 btMatrix3x3 orn2(quat);
138
139                 quatX = trans.getRotation().x();
140                 quatY = trans.getRotation().y();
141                 quatZ = trans.getRotation().z();
142                 quatW = trans.getRotation()[3];
143
144
145                 //printf("test");
146
147
148         }
149
150         virtual float   GetWheelRotation(int wheelIndex) const
151         {
152                 float rotation = 0.f;
153
154                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
155                 {
156                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
157                         rotation = info.m_rotation;
158                 }
159                 return rotation;
160
161         }
162
163
164
165         virtual int     GetUserConstraintId() const
166         {
167                 return m_vehicle->getUserConstraintId();
168         }
169
170         virtual int     GetUserConstraintType() const
171         {
172                 return m_vehicle->getUserConstraintType();
173         }
174
175         virtual void    SetSteeringValue(float steering,int wheelIndex)
176         {
177                 m_vehicle->setSteeringValue(steering,wheelIndex);
178         }
179
180         virtual void    ApplyEngineForce(float force,int wheelIndex)
181         {
182                 m_vehicle->applyEngineForce(force,wheelIndex);
183         }
184
185         virtual void    ApplyBraking(float braking,int wheelIndex)
186         {
187                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
188                 {
189                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
190                         info.m_brake = braking;
191                 }
192         }
193
194         virtual void    SetWheelFriction(float friction,int wheelIndex)
195         {
196                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
197                 {
198                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
199                         info.m_frictionSlip = friction;
200                 }
201
202         }
203
204         virtual void    SetSuspensionStiffness(float suspensionStiffness,int wheelIndex)
205         {
206                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
207                 {
208                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
209                         info.m_suspensionStiffness = suspensionStiffness;
210
211                 }
212         }
213
214         virtual void    SetSuspensionDamping(float suspensionDamping,int wheelIndex)
215         {
216                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
217                 {
218                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
219                         info.m_wheelsDampingRelaxation = suspensionDamping;
220                 }
221         }
222
223         virtual void    SetSuspensionCompression(float suspensionCompression,int wheelIndex)
224         {
225                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
226                 {
227                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
228                         info.m_wheelsDampingCompression = suspensionCompression;
229                 }
230         }
231
232
233
234         virtual void    SetRollInfluence(float rollInfluence,int wheelIndex)
235         {
236                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->getNumWheels()))
237                 {
238                         btWheelInfo& info = m_vehicle->getWheelInfo(wheelIndex);
239                         info.m_rollInfluence = rollInfluence;
240                 }
241         }
242
243         virtual void    SetCoordinateSystem(int rightIndex,int upIndex,int forwardIndex)
244         {
245                 m_vehicle->setCoordinateSystem(rightIndex,upIndex,forwardIndex);
246         }
247
248
249
250 };
251 #endif //NEW_BULLET_VEHICLE_SUPPORT
252
253
254 void CcdPhysicsEnvironment::setDebugDrawer(btIDebugDraw* debugDrawer)
255 {
256         if (debugDrawer && m_dynamicsWorld)
257                 m_dynamicsWorld->setDebugDrawer(debugDrawer);
258         m_debugDrawer = debugDrawer;
259 }
260
261 static void DrawAabb(btIDebugDraw* debugDrawer,const btVector3& from,const btVector3& to,const btVector3& color)
262 {
263         btVector3 halfExtents = (to-from)* 0.5f;
264         btVector3 center = (to+from) *0.5f;
265         int i,j;
266
267         btVector3 edgecoord(1.f,1.f,1.f),pa,pb;
268         for (i=0;i<4;i++)
269         {
270                 for (j=0;j<3;j++)
271                 {
272                         pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],                
273                                 edgecoord[2]*halfExtents[2]);
274                         pa+=center;
275
276                         int othercoord = j%3;
277                         edgecoord[othercoord]*=-1.f;
278                         pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],        
279                                 edgecoord[2]*halfExtents[2]);
280                         pb+=center;
281
282                         debugDrawer->drawLine(pa,pb,color);
283                 }
284                 edgecoord = btVector3(-1.f,-1.f,-1.f);
285                 if (i<3)
286                         edgecoord[i]*=-1.f;
287         }
288
289
290 }
291
292
293
294
295
296
297 CcdPhysicsEnvironment::CcdPhysicsEnvironment(btDispatcher* dispatcher,btOverlappingPairCache* pairCache)
298 :m_scalingPropagated(false),
299 m_numIterations(10),
300 m_numTimeSubSteps(1),
301 m_ccdMode(0),
302 m_solverType(-1),
303 m_profileTimings(0),
304 m_enableSatCollisionDetection(false)
305 {
306
307         for (int i=0;i<PHY_NUM_RESPONSE;i++)
308         {
309                 m_triggerCallbacks[i] = 0;
310         }
311         if (!dispatcher)
312                 dispatcher = new btCollisionDispatcher();
313
314
315         if(!pairCache)
316         {
317
318                 //todo: calculate/let user specify this world sizes
319                 btVector3 worldMin(-10000,-10000,-10000);
320                 btVector3 worldMax(10000,10000,10000);
321
322                 pairCache = new btAxisSweep3(worldMin,worldMax);
323
324                 //broadphase = new btSimpleBroadphase();
325         }
326
327
328         setSolverType(1);//issues with quickstep and memory allocations
329
330         m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,pairCache,new btSequentialImpulseConstraintSolver());
331         m_debugDrawer = 0;
332         m_gravity = btVector3(0.f,-10.f,0.f);
333         m_dynamicsWorld->setGravity(m_gravity);
334
335
336 }
337
338 void    CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
339 {
340         btRigidBody* body = ctrl->GetRigidBody();
341
342         //this m_userPointer is just used for triggers, see CallbackTriggers
343         body->setUserPointer(ctrl);
344
345         body->setGravity( m_gravity );
346         m_controllers.push_back(ctrl);
347
348         m_dynamicsWorld->addRigidBody(body);
349         if (body->isStaticOrKinematicObject())
350         {
351                 body->setActivationState(ISLAND_SLEEPING);
352         }
353
354
355         //CollisionObject(body,ctrl->GetCollisionFilterGroup(),ctrl->GetCollisionFilterMask());
356
357         assert(body->getBroadphaseHandle());
358
359         btBroadphaseInterface* scene =  getBroadphase();
360
361
362         btCollisionShape* shapeinterface = ctrl->GetCollisionShape();
363
364         assert(shapeinterface);
365
366         const btTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform();
367         
368
369         btPoint3 minAabb,maxAabb;
370
371         shapeinterface->getAabb(t,minAabb,maxAabb);
372
373         float timeStep = 0.02f;
374
375
376         //extent it with the motion
377
378         btVector3 linMotion = body->getLinearVelocity()*timeStep;
379
380         float maxAabbx = maxAabb.getX();
381         float maxAabby = maxAabb.getY();
382         float maxAabbz = maxAabb.getZ();
383         float minAabbx = minAabb.getX();
384         float minAabby = minAabb.getY();
385         float minAabbz = minAabb.getZ();
386
387         if (linMotion.x() > 0.f)
388                 maxAabbx += linMotion.x(); 
389         else
390                 minAabbx += linMotion.x();
391         if (linMotion.y() > 0.f)
392                 maxAabby += linMotion.y(); 
393         else
394                 minAabby += linMotion.y();
395         if (linMotion.z() > 0.f)
396                 maxAabbz += linMotion.z(); 
397         else
398                 minAabbz += linMotion.z();
399
400
401         minAabb = btVector3(minAabbx,minAabby,minAabbz);
402         maxAabb = btVector3(maxAabbx,maxAabby,maxAabbz);
403
404
405
406
407 }
408
409 void    CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctrl)
410 {
411
412         //also remove constraint
413
414         
415
416         m_dynamicsWorld->removeRigidBody(ctrl->GetRigidBody());
417
418
419         {
420                 std::vector<CcdPhysicsController*>::iterator i =
421                         std::find(m_controllers.begin(), m_controllers.end(), ctrl);
422                 if (!(i == m_controllers.end()))
423                 {
424                         std::swap(*i, m_controllers.back());
425                         m_controllers.pop_back();
426                 }
427         }
428
429         //remove it from the triggers
430         {
431                 std::vector<CcdPhysicsController*>::iterator i =
432                         std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl);
433                 if (!(i == m_triggerControllers.end()))
434                 {
435                         std::swap(*i, m_triggerControllers.back());
436                         m_triggerControllers.pop_back();
437                 }
438         }
439
440
441 }
442
443
444 void    CcdPhysicsEnvironment::beginFrame()
445 {
446
447 }
448
449
450 bool    CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
451 {
452
453         int i,numCtrl = GetNumControllers();
454         for (i=0;i<numCtrl;i++)
455         {
456                 CcdPhysicsController* ctrl = GetPhysicsController(i);
457                 ctrl->SynchronizeMotionStates(timeStep);
458         }
459
460         m_dynamicsWorld->stepSimulation(timeStep,0);//perform always a full simulation step
461         
462         numCtrl = GetNumControllers();
463         for (i=0;i<numCtrl;i++)
464         {
465                 CcdPhysicsController* ctrl = GetPhysicsController(i);
466                 //ctrl->SynchronizeMotionStates(timeStep);
467         }
468
469         for (i=0;i<m_wrapperVehicles.size();i++)
470         {
471                 WrapperVehicle* veh = m_wrapperVehicles[i];
472                 veh->SyncWheels();
473         }
474
475         CallbackTriggers();
476
477         return true;
478 }
479
480
481 void            CcdPhysicsEnvironment::setDebugMode(int debugMode)
482 {
483         if (m_debugDrawer){
484                 m_debugDrawer->setDebugMode(debugMode);
485         }
486 }
487
488 void            CcdPhysicsEnvironment::setNumIterations(int numIter)
489 {
490         m_numIterations = numIter;
491 }
492 void            CcdPhysicsEnvironment::setDeactivationTime(float dTime)
493 {
494         gDeactivationTime = dTime;
495 }
496 void            CcdPhysicsEnvironment::setDeactivationLinearTreshold(float linTresh)
497 {
498         gLinearSleepingTreshold = linTresh;
499 }
500 void            CcdPhysicsEnvironment::setDeactivationAngularTreshold(float angTresh) 
501 {
502         gAngularSleepingTreshold = angTresh;
503 }
504
505 void            CcdPhysicsEnvironment::setContactBreakingTreshold(float contactBreakingTreshold)
506 {
507         gContactBreakingThreshold = contactBreakingTreshold;
508
509 }
510
511
512 void            CcdPhysicsEnvironment::setCcdMode(int ccdMode)
513 {
514         m_ccdMode = ccdMode;
515 }
516
517
518 void            CcdPhysicsEnvironment::setSolverSorConstant(float sor)
519 {
520         m_solverInfo.m_sor = sor;
521 }
522
523 void            CcdPhysicsEnvironment::setSolverTau(float tau)
524 {
525         m_solverInfo.m_tau = tau;
526 }
527 void            CcdPhysicsEnvironment::setSolverDamping(float damping)
528 {
529         m_solverInfo.m_damping = damping;
530 }
531
532
533 void            CcdPhysicsEnvironment::setLinearAirDamping(float damping)
534 {
535         gLinearAirDamping = damping;
536 }
537
538 void            CcdPhysicsEnvironment::setUseEpa(bool epa)
539 {
540         //gUseEpa = epa;
541 }
542
543 void            CcdPhysicsEnvironment::setSolverType(int solverType)
544 {
545
546         switch (solverType)
547         {
548         case 1:
549                 {
550                         if (m_solverType != solverType)
551                         {
552
553                                 m_solver = new btSequentialImpulseConstraintSolver();
554
555                                 break;
556                         }
557                 }
558
559         case 0:
560         default:
561                 if (m_solverType != solverType)
562                 {
563 //                      m_solver = new OdeConstraintSolver();
564
565                         break;
566                 }
567
568         };
569
570         m_solverType = solverType ;
571 }
572
573
574
575
576
577
578 void            CcdPhysicsEnvironment::setGravity(float x,float y,float z)
579 {
580         m_gravity = btVector3(x,y,z);
581         m_dynamicsWorld->setGravity(m_gravity);
582
583 }
584
585
586
587
588 static int gConstraintUid = 1;
589
590 //Following the COLLADA physics specification for constraints
591 int                     CcdPhysicsEnvironment::createUniversalD6Constraint(
592                                                 class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
593                                                 btTransform& frameInA,
594                                                 btTransform& frameInB,
595                                                 const btVector3& linearMinLimits,
596                                                 const btVector3& linearMaxLimits,
597                                                 const btVector3& angularMinLimits,
598                                                 const btVector3& angularMaxLimits
599 )
600 {
601
602         //we could either add some logic to recognize ball-socket and hinge, or let that up to the user
603         //perhaps some warning or hint that hinge/ball-socket is more efficient?
604         
605         btGeneric6DofConstraint* genericConstraint = 0;
606         CcdPhysicsController* ctrl0 = (CcdPhysicsController*) ctrlRef;
607         CcdPhysicsController* ctrl1 = (CcdPhysicsController*) ctrlOther;
608         
609         btRigidBody* rb0 = ctrl0->GetRigidBody();
610         btRigidBody* rb1 = ctrl1->GetRigidBody();
611
612         if (rb1)
613         {
614                 
615
616                 genericConstraint = new btGeneric6DofConstraint(
617                         *rb0,*rb1,
618                         frameInA,frameInB);
619                 genericConstraint->setLinearLowerLimit(linearMinLimits);
620                 genericConstraint->setLinearUpperLimit(linearMaxLimits);
621                 genericConstraint->setAngularLowerLimit(angularMinLimits);
622                 genericConstraint->setAngularUpperLimit(angularMaxLimits);
623         } else
624         {
625                 // TODO: Implement single body case...
626                 //No, we can use a fixed rigidbody in above code, rather then unnecessary duplation of code
627
628         }
629         
630         if (genericConstraint)
631         {
632         //      m_constraints.push_back(genericConstraint);
633                 m_dynamicsWorld->addConstraint(genericConstraint);
634
635                 genericConstraint->setUserConstraintId(gConstraintUid++);
636                 genericConstraint->setUserConstraintType(PHY_GENERIC_6DOF_CONSTRAINT);
637                 //64 bit systems can't cast pointer to int. could use size_t instead.
638                 return genericConstraint->getUserConstraintId();
639         }
640         return 0;
641 }
642
643
644
645 void            CcdPhysicsEnvironment::removeConstraint(int     constraintId)
646 {
647         
648         int i;
649         int numConstraints = m_dynamicsWorld->getNumConstraints();
650         for (i=0;i<numConstraints;i++)
651         {
652                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
653                 if (constraint->getUserConstraintId() == constraintId)
654                 {
655                         m_dynamicsWorld->removeConstraint(constraint);
656                         break;
657                 }
658         }
659 }
660
661
662 struct  FilterClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
663 {
664         PHY_IPhysicsController* m_ignoreClient;
665
666         FilterClosestRayResultCallback (PHY_IPhysicsController* ignoreClient,const btVector3& rayFrom,const btVector3& rayTo)
667                 : btCollisionWorld::ClosestRayResultCallback(rayFrom,rayTo),
668                 m_ignoreClient(ignoreClient)
669         {
670
671         }
672
673         virtual ~FilterClosestRayResultCallback()
674         {
675         }
676
677         virtual float   AddSingleResult( btCollisionWorld::LocalRayResult& rayResult)
678         {
679                 CcdPhysicsController* curHit = static_cast<CcdPhysicsController*>(rayResult.m_collisionObject->getUserPointer());
680                 //ignore client...
681                 if (curHit != m_ignoreClient)
682                 {               
683                         //if valid
684                         return ClosestRayResultCallback::AddSingleResult(rayResult);
685                 }
686                 return m_closestHitFraction;
687         }
688
689 };
690
691 PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ, 
692                                                                                                            float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
693 {
694
695
696         float minFraction = 1.f;
697
698         btVector3 rayFrom(fromX,fromY,fromZ);
699         btVector3 rayTo(toX,toY,toZ);
700
701         btVector3       hitPointWorld,normalWorld;
702
703         //Either Ray Cast with or without filtering
704
705         //btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo);
706         FilterClosestRayResultCallback   rayCallback(ignoreClient,rayFrom,rayTo);
707
708
709         PHY_IPhysicsController* nearestHit = 0;
710
711         m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback);
712         if (rayCallback.HasHit())
713         {
714                 nearestHit = static_cast<CcdPhysicsController*>(rayCallback.m_collisionObject->getUserPointer());
715                 hitX =  rayCallback.m_hitPointWorld.getX();
716                 hitY =  rayCallback.m_hitPointWorld.getY();
717                 hitZ =  rayCallback.m_hitPointWorld.getZ();
718
719                 normalX = rayCallback.m_hitNormalWorld.getX();
720                 normalY = rayCallback.m_hitNormalWorld.getY();
721                 normalZ = rayCallback.m_hitNormalWorld.getZ();
722
723         }       
724
725
726         return nearestHit;
727 }
728
729
730
731 int     CcdPhysicsEnvironment::getNumContactPoints()
732 {
733         return 0;
734 }
735
736 void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
737 {
738
739 }
740
741
742
743
744 btBroadphaseInterface*  CcdPhysicsEnvironment::getBroadphase()
745
746         return m_dynamicsWorld->getBroadphase(); 
747 }
748
749
750
751
752 CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
753 {
754
755 #ifdef NEW_BULLET_VEHICLE_SUPPORT
756         m_wrapperVehicles.clear();
757 #endif //NEW_BULLET_VEHICLE_SUPPORT
758
759         //m_broadphase->DestroyScene();
760         //delete broadphase ? release reference on broadphase ?
761
762         //first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher
763         //delete m_dispatcher;
764         delete m_dynamicsWorld;
765         
766
767
768 }
769
770
771 int     CcdPhysicsEnvironment::GetNumControllers()
772 {
773         return m_controllers.size();
774 }
775
776
777 CcdPhysicsController* CcdPhysicsEnvironment::GetPhysicsController( int index)
778 {
779         return m_controllers[index];
780 }
781
782
783
784
785 void    CcdPhysicsEnvironment::setConstraintParam(int constraintId,int param,float value0,float value1)
786 {
787         btTypedConstraint* typedConstraint = getConstraintById(constraintId);
788         switch (typedConstraint->getUserConstraintType())
789         {
790         case PHY_GENERIC_6DOF_CONSTRAINT:
791                 {
792                         //param = 1..12, min0,max0,min1,max1...min6,max6
793                         btGeneric6DofConstraint* genCons = (btGeneric6DofConstraint*)typedConstraint;
794                         genCons->SetLimit(param,value0,value1);
795                         break;
796                 };
797         default:
798                 {
799                 };
800         };
801 }
802
803 btTypedConstraint*      CcdPhysicsEnvironment::getConstraintById(int constraintId)
804 {
805
806         int numConstraints = m_dynamicsWorld->getNumConstraints();
807         int i;
808         for (i=0;i<numConstraints;i++)
809         {
810                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
811                 if (constraint->getUserConstraintId()==constraintId)
812                 {
813                         return constraint;
814                 }
815         }
816         return 0;
817 }
818
819
820 void CcdPhysicsEnvironment::addSensor(PHY_IPhysicsController* ctrl)
821 {
822
823         CcdPhysicsController* ctrl1 = (CcdPhysicsController* )ctrl;
824         std::vector<CcdPhysicsController*>::iterator i =
825                 std::find(m_controllers.begin(), m_controllers.end(), ctrl);
826         if ((i == m_controllers.end()))
827         {
828                 addCcdPhysicsController(ctrl1);
829         }
830         //force collision detection with everything, including static objects (might hurt performance!)
831         ctrl1->GetRigidBody()->getBroadphaseHandle()->m_collisionFilterMask = btBroadphaseProxy::AllFilter;
832
833         requestCollisionCallback(ctrl);
834         //printf("addSensor\n");
835 }
836
837 void CcdPhysicsEnvironment::removeCollisionCallback(PHY_IPhysicsController* ctrl)
838 {
839         std::vector<CcdPhysicsController*>::iterator i =
840                 std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl);
841         if (!(i == m_triggerControllers.end()))
842         {
843                 std::swap(*i, m_triggerControllers.back());
844                 m_triggerControllers.pop_back();
845         }
846 }
847
848
849 void CcdPhysicsEnvironment::removeSensor(PHY_IPhysicsController* ctrl)
850 {
851         removeCollisionCallback(ctrl);
852         //printf("removeSensor\n");
853 }
854 void CcdPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user)
855 {
856         /*      printf("addTouchCallback\n(response class = %i)\n",response_class);
857
858         //map PHY_ convention into SM_ convention
859         switch (response_class)
860         {
861         case    PHY_FH_RESPONSE:
862         printf("PHY_FH_RESPONSE\n");
863         break;
864         case PHY_SENSOR_RESPONSE:
865         printf("PHY_SENSOR_RESPONSE\n");
866         break;
867         case PHY_CAMERA_RESPONSE:
868         printf("PHY_CAMERA_RESPONSE\n");
869         break;
870         case PHY_OBJECT_RESPONSE:
871         printf("PHY_OBJECT_RESPONSE\n");
872         break;
873         case PHY_STATIC_RESPONSE:
874         printf("PHY_STATIC_RESPONSE\n");
875         break;
876         default:
877         assert(0);
878         return;
879         }
880         */
881
882         m_triggerCallbacks[response_class] = callback;
883         m_triggerCallbacksUserPtrs[response_class] = user;
884
885 }
886 void CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctrl)
887 {
888         CcdPhysicsController* ccdCtrl = static_cast<CcdPhysicsController*>(ctrl);
889
890         //printf("requestCollisionCallback\n");
891         m_triggerControllers.push_back(ccdCtrl);
892 }
893
894
895 void    CcdPhysicsEnvironment::CallbackTriggers()
896 {
897         
898         CcdPhysicsController* ctrl0=0,*ctrl1=0;
899
900         if (m_triggerCallbacks[PHY_OBJECT_RESPONSE] || (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)))
901         {
902                 //walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback
903                 int numManifolds = m_dynamicsWorld->getDispatcher()->getNumManifolds();
904                 for (int i=0;i<numManifolds;i++)
905                 {
906                         btPersistentManifold* manifold = m_dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i);
907                         int numContacts = manifold->getNumContacts();
908                         if (numContacts)
909                         {
910                                 if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints))
911                                 {
912                                         for (int j=0;j<numContacts;j++)
913                                         {
914                                                 btVector3 color(1,0,0);
915                                                 const btManifoldPoint& cp = manifold->getContactPoint(j);
916                                                 if (m_debugDrawer)
917                                                         m_debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
918                                         }
919                                 }
920                                 btRigidBody* obj0 = static_cast<btRigidBody* >(manifold->getBody0());
921                                 btRigidBody* obj1 = static_cast<btRigidBody* >(manifold->getBody1());
922
923                                 //m_internalOwner is set in 'addPhysicsController'
924                                 CcdPhysicsController* ctrl0 = static_cast<CcdPhysicsController*>(obj0->getUserPointer());
925                                 CcdPhysicsController* ctrl1 = static_cast<CcdPhysicsController*>(obj1->getUserPointer());
926
927                                 std::vector<CcdPhysicsController*>::iterator i =
928                                         std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl0);
929                                 if (i == m_triggerControllers.end())
930                                 {
931                                         i = std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl1);
932                                 }
933
934                                 if (!(i == m_triggerControllers.end()))
935                                 {
936                                         m_triggerCallbacks[PHY_OBJECT_RESPONSE](m_triggerCallbacksUserPtrs[PHY_OBJECT_RESPONSE],
937                                                 ctrl0,ctrl1,0);
938                                 }
939                         }
940                 }
941
942
943
944         }
945
946
947 }
948
949
950
951
952
953
954 #ifdef NEW_BULLET_VEHICLE_SUPPORT
955
956 //complex constraint for vehicles
957 PHY_IVehicle*   CcdPhysicsEnvironment::getVehicleConstraint(int constraintId)
958 {
959         int i;
960
961         int numVehicles = m_wrapperVehicles.size();
962         for (i=0;i<numVehicles;i++)
963         {
964                 WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
965                 if (wrapperVehicle->GetVehicle()->getUserConstraintId() == constraintId)
966                         return wrapperVehicle;
967         }
968
969         return 0;
970 }
971
972 #endif //NEW_BULLET_VEHICLE_SUPPORT
973
974
975 int currentController = 0;
976 int numController = 0;
977
978
979
980
981 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateSphereController(float radius,const PHY__Vector3& position)
982 {
983         
984         CcdConstructionInfo     cinfo;
985         cinfo.m_collisionShape = new btSphereShape(radius);
986         cinfo.m_MotionState = 0;
987         cinfo.m_physicsEnv = this;
988         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE | btCollisionObject::CF_KINEMATIC_OBJECT;
989         DefaultMotionState* motionState = new DefaultMotionState();
990         cinfo.m_MotionState = motionState;
991         motionState->m_worldTransform.setIdentity();
992         motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
993
994         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
995         
996
997         return sphereController;
998 }
999
1000 int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
1001                                                                                                         float pivotX,float pivotY,float pivotZ,
1002                                                                                                         float axisX,float axisY,float axisZ)
1003 {
1004
1005
1006         CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
1007         CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
1008
1009         btRigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
1010         btRigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
1011
1012         bool rb0static = rb0 ? rb0->isStaticOrKinematicObject() : true;
1013         bool rb1static = rb1 ? rb1->isStaticOrKinematicObject() : true;
1014         
1015
1016         if (rb0static && rb1static)
1017                 return 0;
1018
1019         btVector3 pivotInA(pivotX,pivotY,pivotZ);
1020         btVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : 
1021                 rb0->getCenterOfMassTransform() * pivotInA;
1022         btVector3 axisInA(axisX,axisY,axisZ);
1023         btVector3 axisInB = rb1 ? 
1024                 (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) : 
1025         rb0->getCenterOfMassTransform().getBasis() * axisInA;
1026
1027         bool angularOnly = false;
1028
1029         switch (type)
1030         {
1031         case PHY_POINT2POINT_CONSTRAINT:
1032                 {
1033
1034                         btPoint2PointConstraint* p2p = 0;
1035
1036                         if (rb1)
1037                         {
1038                                 p2p = new btPoint2PointConstraint(*rb0,
1039                                         *rb1,pivotInA,pivotInB);
1040                         } else
1041                         {
1042                                 p2p = new btPoint2PointConstraint(*rb0,
1043                                         pivotInA);
1044                         }
1045
1046                         m_dynamicsWorld->addConstraint(p2p);
1047 //                      m_constraints.push_back(p2p);
1048
1049                         p2p->setUserConstraintId(gConstraintUid++);
1050                         p2p->setUserConstraintType(type);
1051                         //64 bit systems can't cast pointer to int. could use size_t instead.
1052                         return p2p->getUserConstraintId();
1053
1054                         break;
1055                 }
1056
1057         case PHY_GENERIC_6DOF_CONSTRAINT:
1058                 {
1059                         btGeneric6DofConstraint* genericConstraint = 0;
1060
1061                         if (rb1)
1062                         {
1063                                 btTransform frameInA;
1064                                 btTransform frameInB;
1065                                 
1066                                 btVector3 axis1, axis2;
1067                                 btPlaneSpace1( axisInA, axis1, axis2 );
1068
1069                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1070                                                                   axisInA.y(), axis1.y(), axis2.y(),
1071                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1072
1073         
1074                                 btPlaneSpace1( axisInB, axis1, axis2 );
1075                                 frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(),
1076                                                                   axisInB.y(), axis1.y(), axis2.y(),
1077                                                                                           axisInB.z(), axis1.z(), axis2.z() );
1078
1079                                 frameInA.setOrigin( pivotInA );
1080                                 frameInB.setOrigin( pivotInB );
1081
1082                                 genericConstraint = new btGeneric6DofConstraint(
1083                                         *rb0,*rb1,
1084                                         frameInA,frameInB);
1085
1086
1087                         } else
1088                         {
1089                                 static btRigidBody s_fixedObject2( 0,0,0);
1090                                         btTransform frameInA;
1091                                 btTransform frameInB;
1092                                 
1093                                 btVector3 axis1, axis2;
1094                                 btPlaneSpace1( axisInA, axis1, axis2 );
1095
1096                                 frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
1097                                                                   axisInA.y(), axis1.y(), axis2.y(),
1098                                                                                           axisInA.z(), axis1.z(), axis2.z() );
1099
1100         
1101                                 btPlaneSpace1( axisInB, axis1, axis2 );
1102                                 frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(),
1103                                                                   axisInB.y(), axis1.y(), axis2.y(),
1104                                                                                           axisInB.z(), axis1.z(), axis2.z() );
1105
1106                                 frameInA.setOrigin( pivotInA );
1107                                 frameInB.setOrigin( pivotInB );
1108
1109
1110                                 genericConstraint = new btGeneric6DofConstraint(
1111                                         *rb0,s_fixedObject2,
1112                                         frameInA,frameInB);
1113                         }
1114                         
1115                         if (genericConstraint)
1116                         {
1117                                 //m_constraints.push_back(genericConstraint);
1118                                 m_dynamicsWorld->addConstraint(genericConstraint);
1119                                 genericConstraint->setUserConstraintId(gConstraintUid++);
1120                                 genericConstraint->setUserConstraintType(type);
1121                                 //64 bit systems can't cast pointer to int. could use size_t instead.
1122                                 return genericConstraint->getUserConstraintId();
1123                         } 
1124
1125                         break;
1126                 }
1127         case PHY_ANGULAR_CONSTRAINT:
1128                 angularOnly = true;
1129
1130
1131         case PHY_LINEHINGE_CONSTRAINT:
1132                 {
1133                         btHingeConstraint* hinge = 0;
1134
1135                         if (rb1)
1136                         {
1137                                 hinge = new btHingeConstraint(
1138                                         *rb0,
1139                                         *rb1,pivotInA,pivotInB,axisInA,axisInB);
1140
1141
1142                         } else
1143                         {
1144                                 hinge = new btHingeConstraint(*rb0,
1145                                         pivotInA,axisInA);
1146
1147                         }
1148                         hinge->setAngularOnly(angularOnly);
1149
1150                         //m_constraints.push_back(hinge);
1151                         m_dynamicsWorld->addConstraint(hinge);
1152                         hinge->setUserConstraintId(gConstraintUid++);
1153                         hinge->setUserConstraintType(type);
1154                         //64 bit systems can't cast pointer to int. could use size_t instead.
1155                         return hinge->getUserConstraintId();
1156                         break;
1157                 }
1158 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1159
1160         case PHY_VEHICLE_CONSTRAINT:
1161                 {
1162                         btRaycastVehicle::btVehicleTuning* tuning = new btRaycastVehicle::btVehicleTuning();
1163                         btRigidBody* chassis = rb0;
1164                         btDefaultVehicleRaycaster* raycaster = new btDefaultVehicleRaycaster(m_dynamicsWorld);
1165                         btRaycastVehicle* vehicle = new btRaycastVehicle(*tuning,chassis,raycaster);
1166                         WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
1167                         m_wrapperVehicles.push_back(wrapperVehicle);
1168                         m_dynamicsWorld->addVehicle(vehicle);
1169                         vehicle->setUserConstraintId(gConstraintUid++);
1170                         vehicle->setUserConstraintType(type);
1171                         return vehicle->getUserConstraintId();
1172
1173                         break;
1174                 };
1175 #endif //NEW_BULLET_VEHICLE_SUPPORT
1176
1177         default:
1178                 {
1179                 }
1180         };
1181
1182         //btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB
1183
1184         return 0;
1185
1186 }
1187
1188
1189
1190 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float coneradius,float coneheight)
1191 {
1192         CcdConstructionInfo     cinfo;
1193         cinfo.m_collisionShape = new btConeShape(coneradius,coneheight);
1194         cinfo.m_MotionState = 0;
1195         cinfo.m_physicsEnv = this;
1196         cinfo.m_collisionFlags |= btCollisionObject::CF_NO_CONTACT_RESPONSE;
1197         DefaultMotionState* motionState = new DefaultMotionState();
1198         cinfo.m_MotionState = motionState;
1199         motionState->m_worldTransform.setIdentity();
1200 //      motionState->m_worldTransform.setOrigin(btVector3(position[0],position[1],position[2]));
1201
1202         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
1203
1204
1205         return sphereController;
1206 }
1207         
1208 float           CcdPhysicsEnvironment::getAppliedImpulse(int    constraintid)
1209 {
1210         int i;
1211         int numConstraints = m_dynamicsWorld->getNumConstraints();
1212         for (i=0;i<numConstraints;i++)
1213         {
1214                 btTypedConstraint* constraint = m_dynamicsWorld->getConstraint(i);
1215                 if (constraint->getUserConstraintId() == constraintid)
1216                 {
1217                         return constraint->getAppliedImpulse();
1218                 }
1219         }
1220
1221         return 0.f;
1222 }