workaround for a irregularity/bug in physics system (happens very seldom, just 1...
[blender.git] / extern / bullet / Extras / PhysicsInterface / CcdPhysics / 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 "SimdTransform.h"
24 #include "Dynamics/RigidBody.h"
25 #include "BroadphaseCollision/BroadphaseInterface.h"
26 #include "BroadphaseCollision/SimpleBroadphase.h"
27 #include "BroadphaseCollision/AxisSweep3.h"
28
29 #include "CollisionDispatch/CollisionWorld.h"
30
31 #include "CollisionShapes/ConvexShape.h"
32 #include "CollisionShapes/ConeShape.h"
33
34
35
36 #include "BroadphaseCollision/Dispatcher.h"
37 #include "NarrowPhaseCollision/PersistentManifold.h"
38 #include "CollisionShapes/TriangleMeshShape.h"
39 #include "ConstraintSolver/OdeConstraintSolver.h"
40 #include "ConstraintSolver/SimpleConstraintSolver.h"
41
42
43 //profiling/timings
44 #include "quickprof.h"
45
46
47 #include "IDebugDraw.h"
48
49 #include "NarrowPhaseCollision/VoronoiSimplexSolver.h"
50 #include "NarrowPhaseCollision/SubSimplexConvexCast.h"
51 #include "NarrowPhaseCollision/GjkConvexCast.h"
52 #include "NarrowPhaseCollision/ContinuousConvexCollision.h"
53
54
55 #include "CollisionDispatch/CollisionDispatcher.h"
56 #include "PHY_IMotionState.h"
57
58 #include "CollisionDispatch/EmptyCollisionAlgorithm.h"
59 #include "CollisionDispatch/UnionFind.h"
60
61
62 #include "CollisionShapes/SphereShape.h"
63
64 bool useIslands = true;
65
66 #ifdef NEW_BULLET_VEHICLE_SUPPORT
67 #include "Vehicle/RaycastVehicle.h"
68 #include "Vehicle/VehicleRaycaster.h"
69
70 #include "Vehicle/WheelInfo.h"
71 #include "PHY_IVehicle.h"
72 RaycastVehicle::VehicleTuning   gTuning;
73
74 #endif //NEW_BULLET_VEHICLE_SUPPORT
75 #include "AabbUtil2.h"
76
77 #include "ConstraintSolver/ConstraintSolver.h"
78 #include "ConstraintSolver/Point2PointConstraint.h"
79 #include "ConstraintSolver/HingeConstraint.h"
80
81
82 //#include "BroadphaseCollision/QueryDispatcher.h"
83 //#include "BroadphaseCollision/QueryBox.h"
84 //todo: change this to allow dynamic registration of types!
85
86 #ifdef WIN32
87 void DrawRasterizerLine(const float* from,const float* to,int color);
88 #endif
89
90
91 #include "ConstraintSolver/ContactConstraint.h"
92
93
94 #include <stdio.h>
95
96 #ifdef NEW_BULLET_VEHICLE_SUPPORT
97 class WrapperVehicle : public PHY_IVehicle
98 {
99
100         RaycastVehicle* m_vehicle;
101         PHY_IPhysicsController* m_chassis;
102
103 public:
104
105         WrapperVehicle(RaycastVehicle* vehicle,PHY_IPhysicsController* chassis)
106                 :m_vehicle(vehicle),
107                 m_chassis(chassis)
108         {
109         }
110
111         RaycastVehicle* GetVehicle()
112         {
113                 return m_vehicle;
114         }
115
116         PHY_IPhysicsController* GetChassis()
117         {
118                 return m_chassis;
119         }
120
121         virtual void    AddWheel(
122                 PHY_IMotionState*       motionState,
123                 PHY__Vector3    connectionPoint,
124                 PHY__Vector3    downDirection,
125                 PHY__Vector3    axleDirection,
126                 float   suspensionRestLength,
127                 float wheelRadius,
128                 bool hasSteering
129                 )
130         {
131                 SimdVector3 connectionPointCS0(connectionPoint[0],connectionPoint[1],connectionPoint[2]);
132                 SimdVector3 wheelDirectionCS0(downDirection[0],downDirection[1],downDirection[2]);
133                 SimdVector3 wheelAxle(axleDirection[0],axleDirection[1],axleDirection[2]);
134
135
136                 WheelInfo& info = m_vehicle->AddWheel(connectionPointCS0,wheelDirectionCS0,wheelAxle,
137                         suspensionRestLength,wheelRadius,gTuning,hasSteering);
138                 info.m_clientInfo = motionState;
139
140         }
141
142         void    SyncWheels()
143         {
144                 int numWheels = GetNumWheels();
145                 int i;
146                 for (i=0;i<numWheels;i++)
147                 {
148                         WheelInfo& info = m_vehicle->GetWheelInfo(i);
149                         PHY_IMotionState* motionState = (PHY_IMotionState*)info.m_clientInfo ;
150                         m_vehicle->UpdateWheelTransform(i);
151                         SimdTransform trans = m_vehicle->GetWheelTransformWS(i);
152                         SimdQuaternion orn = trans.getRotation();
153                         const SimdVector3& pos = trans.getOrigin();
154                         motionState->setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]);
155                         motionState->setWorldPosition(pos.x(),pos.y(),pos.z());
156
157                 }
158         }
159
160         virtual int             GetNumWheels() const
161         {
162                 return m_vehicle->GetNumWheels();
163         }
164
165         virtual void    GetWheelPosition(int wheelIndex,float& posX,float& posY,float& posZ) const
166         {
167                 SimdTransform   trans = m_vehicle->GetWheelTransformWS(wheelIndex);
168                 posX = trans.getOrigin().x();
169                 posY = trans.getOrigin().y();
170                 posZ = trans.getOrigin().z();
171         }
172         virtual void    GetWheelOrientationQuaternion(int wheelIndex,float& quatX,float& quatY,float& quatZ,float& quatW) const
173         {
174                 SimdTransform   trans = m_vehicle->GetWheelTransformWS(wheelIndex);
175                 SimdQuaternion quat = trans.getRotation();
176                 SimdMatrix3x3 orn2(quat);
177
178                 quatX = trans.getRotation().x();
179                 quatY = trans.getRotation().y();
180                 quatZ = trans.getRotation().z();
181                 quatW = trans.getRotation()[3];
182
183
184                 //printf("test");
185
186
187         }
188
189         virtual float   GetWheelRotation(int wheelIndex) const
190         {
191                 float rotation = 0.f;
192
193                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
194                 {
195                         WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
196                         rotation = info.m_rotation;
197                 }
198                 return rotation;
199
200         }
201
202
203
204         virtual int     GetUserConstraintId() const
205         {
206                 return m_vehicle->GetUserConstraintId();
207         }
208
209         virtual int     GetUserConstraintType() const
210         {
211                 return m_vehicle->GetUserConstraintType();
212         }
213
214         virtual void    SetSteeringValue(float steering,int wheelIndex)
215         {
216                 m_vehicle->SetSteeringValue(steering,wheelIndex);
217         }
218
219         virtual void    ApplyEngineForce(float force,int wheelIndex)
220         {
221                 m_vehicle->ApplyEngineForce(force,wheelIndex);
222         }
223
224         virtual void    ApplyBraking(float braking,int wheelIndex)
225         {
226                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
227                 {
228                         WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
229                         info.m_brake = braking;
230                 }
231         }
232
233         virtual void    SetWheelFriction(float friction,int wheelIndex)
234         {
235                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
236                 {
237                         WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
238                         info.m_frictionSlip = friction;
239                 }
240
241         }
242
243         virtual void    SetSuspensionStiffness(float suspensionStiffness,int wheelIndex)
244         {
245                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
246                 {
247                         WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
248                         info.m_suspensionStiffness = suspensionStiffness;
249
250                 }
251         }
252
253         virtual void    SetSuspensionDamping(float suspensionDamping,int wheelIndex)
254         {
255                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
256                 {
257                         WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
258                         info.m_wheelsDampingRelaxation = suspensionDamping;
259                 }
260         }
261
262         virtual void    SetSuspensionCompression(float suspensionCompression,int wheelIndex)
263         {
264                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
265                 {
266                         WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
267                         info.m_wheelsDampingCompression = suspensionCompression;
268                 }
269         }
270
271
272
273         virtual void    SetRollInfluence(float rollInfluence,int wheelIndex)
274         {
275                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
276                 {
277                         WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
278                         info.m_rollInfluence = rollInfluence;
279                 }
280         }
281
282
283
284 };
285 #endif //NEW_BULLET_VEHICLE_SUPPORT
286
287
288
289 static void DrawAabb(IDebugDraw* debugDrawer,const SimdVector3& from,const SimdVector3& to,const SimdVector3& color)
290 {
291         SimdVector3 halfExtents = (to-from)* 0.5f;
292         SimdVector3 center = (to+from) *0.5f;
293         int i,j;
294
295         SimdVector3 edgecoord(1.f,1.f,1.f),pa,pb;
296         for (i=0;i<4;i++)
297         {
298                 for (j=0;j<3;j++)
299                 {
300                         pa = SimdVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],              
301                                 edgecoord[2]*halfExtents[2]);
302                         pa+=center;
303
304                         int othercoord = j%3;
305                         edgecoord[othercoord]*=-1.f;
306                         pb = SimdVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],      
307                                 edgecoord[2]*halfExtents[2]);
308                         pb+=center;
309
310                         debugDrawer->DrawLine(pa,pb,color);
311                 }
312                 edgecoord = SimdVector3(-1.f,-1.f,-1.f);
313                 if (i<3)
314                         edgecoord[i]*=-1.f;
315         }
316
317
318 }
319
320
321
322
323
324
325 CcdPhysicsEnvironment::CcdPhysicsEnvironment(CollisionDispatcher* dispatcher,BroadphaseInterface* broadphase)
326 :m_scalingPropagated(false),
327 m_numIterations(10),
328 m_numTimeSubSteps(1),
329 m_ccdMode(0),
330 m_solverType(-1),
331 m_profileTimings(0),
332 m_enableSatCollisionDetection(false)
333 {
334
335         for (int i=0;i<PHY_NUM_RESPONSE;i++)
336         {
337                 m_triggerCallbacks[i] = 0;
338         }
339         if (!dispatcher)
340                 dispatcher = new CollisionDispatcher();
341
342
343         if(!broadphase)
344         {
345
346                 //todo: calculate/let user specify this world sizes
347                 SimdVector3 worldMin(-10000,-10000,-10000);
348                 SimdVector3 worldMax(10000,10000,10000);
349
350                 broadphase = new AxisSweep3(worldMin,worldMax);
351
352                 //broadphase = new SimpleBroadphase();
353         }
354
355
356         setSolverType(1);//issues with quickstep and memory allocations
357
358         m_collisionWorld = new CollisionWorld(dispatcher,broadphase);
359
360         m_debugDrawer = 0;
361         m_gravity = SimdVector3(0.f,-10.f,0.f);
362
363
364 }
365
366 void    CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
367 {
368         RigidBody* body = ctrl->GetRigidBody();
369
370         //this m_userPointer is just used for triggers, see CallbackTriggers
371         body->m_userPointer = ctrl;
372
373         body->setGravity( m_gravity );
374         m_controllers.push_back(ctrl);
375
376         m_collisionWorld->AddCollisionObject(body);
377
378         assert(body->m_broadphaseHandle);
379
380         BroadphaseInterface* scene =  GetBroadphase();
381
382
383         CollisionShape* shapeinterface = ctrl->GetCollisionShape();
384
385         assert(shapeinterface);
386
387         const SimdTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform();
388
389
390         SimdPoint3 minAabb,maxAabb;
391
392         shapeinterface->GetAabb(t,minAabb,maxAabb);
393
394         float timeStep = 0.02f;
395
396
397         //extent it with the motion
398
399         SimdVector3 linMotion = body->getLinearVelocity()*timeStep;
400
401         float maxAabbx = maxAabb.getX();
402         float maxAabby = maxAabb.getY();
403         float maxAabbz = maxAabb.getZ();
404         float minAabbx = minAabb.getX();
405         float minAabby = minAabb.getY();
406         float minAabbz = minAabb.getZ();
407
408         if (linMotion.x() > 0.f)
409                 maxAabbx += linMotion.x(); 
410         else
411                 minAabbx += linMotion.x();
412         if (linMotion.y() > 0.f)
413                 maxAabby += linMotion.y(); 
414         else
415                 minAabby += linMotion.y();
416         if (linMotion.z() > 0.f)
417                 maxAabbz += linMotion.z(); 
418         else
419                 minAabbz += linMotion.z();
420
421
422         minAabb = SimdVector3(minAabbx,minAabby,minAabbz);
423         maxAabb = SimdVector3(maxAabbx,maxAabby,maxAabbz);
424
425
426
427
428 }
429
430 void    CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctrl)
431 {
432
433         //also remove constraint
434
435         {
436                 std::vector<TypedConstraint*>::iterator i;
437
438                 for (i=m_constraints.begin();
439                         !(i==m_constraints.end()); i++)
440                 {
441                         TypedConstraint* constraint = (*i);
442                         if  ((&constraint->GetRigidBodyA() == ctrl->GetRigidBody() ||
443                                 (&constraint->GetRigidBodyB() == ctrl->GetRigidBody())))
444                         {
445                                 removeConstraint(constraint->GetUserConstraintId());
446                                 //only 1 constraint per constroller
447                                 break;
448                         }
449                 }
450         }
451
452         {
453                 std::vector<TypedConstraint*>::iterator i;
454
455                 for (i=m_constraints.begin();
456                         !(i==m_constraints.end()); i++)
457                 {
458                         TypedConstraint* constraint = (*i);
459                         if  ((&constraint->GetRigidBodyA() == ctrl->GetRigidBody() ||
460                                 (&constraint->GetRigidBodyB() == ctrl->GetRigidBody())))
461                         {
462                                 removeConstraint(constraint->GetUserConstraintId());
463                                 //only 1 constraint per constroller
464                                 break;
465                         }
466                 }
467         }
468
469
470         m_collisionWorld->RemoveCollisionObject(ctrl->GetRigidBody());
471
472
473         {
474                 std::vector<CcdPhysicsController*>::iterator i =
475                         std::find(m_controllers.begin(), m_controllers.end(), ctrl);
476                 if (!(i == m_controllers.end()))
477                 {
478                         std::swap(*i, m_controllers.back());
479                         m_controllers.pop_back();
480                 }
481         }
482
483         //remove it from the triggers
484         {
485                 std::vector<CcdPhysicsController*>::iterator i =
486                         std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl);
487                 if (!(i == m_triggerControllers.end()))
488                 {
489                         std::swap(*i, m_triggerControllers.back());
490                         m_triggerControllers.pop_back();
491                 }
492         }
493
494
495 }
496
497
498 void    CcdPhysicsEnvironment::beginFrame()
499 {
500
501 }
502
503
504 bool    CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
505 {
506         //don't simulate without timesubsteps
507         if (m_numTimeSubSteps<1)
508                 return true;
509
510         //printf("proceedDeltaTime\n");
511         
512
513 #ifdef USE_QUICKPROF
514         //toggle Profiler
515         if ( m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_ProfileTimings)
516         {
517                 if (!m_profileTimings)
518                 {
519                         m_profileTimings = 1;
520                         // To disable profiling, simply comment out the following line.
521                         static int counter = 0;
522
523                         char filename[128];
524                         sprintf(filename,"quickprof_bullet_timings%i.csv",counter++);
525                         Profiler::init(filename, Profiler::BLOCK_CYCLE_SECONDS);//BLOCK_TOTAL_MICROSECONDS
526
527                 }
528         } else
529         {
530                 if (m_profileTimings)
531                 {
532                         m_profileTimings = 0;
533                         Profiler::destroy();
534                 }
535         }
536 #endif //USE_QUICKPROF
537
538
539
540         if (!SimdFuzzyZero(timeStep))
541         {
542                 
543                 {
544                         //do the kinematic calculation here, over the full timestep
545                         std::vector<CcdPhysicsController*>::iterator i;
546                         for (i=m_controllers.begin();
547                                                 !(i==m_controllers.end()); i++)
548                         {
549
550                                                 CcdPhysicsController* ctrl = *i;
551
552                                                 SimdTransform predictedTrans;
553                                                 RigidBody* body = ctrl->GetRigidBody();
554                                                 if (body->GetActivationState() != ISLAND_SLEEPING)
555                                                 {
556
557                                                         if (body->IsStatic())
558                                                         {
559                                                                 //to calculate velocities next frame
560                                                                 body->saveKinematicState(timeStep);
561                                                         }
562                                                 }
563                         }
564                 }
565
566
567                 int i;
568                 float subTimeStep = timeStep / float(m_numTimeSubSteps);
569
570                 for (i=0;i<this->m_numTimeSubSteps;i++)
571                 {
572                         proceedDeltaTimeOneStep(subTimeStep);
573                 }
574         } else
575         {
576                 //todo: interpolate
577         }
578
579         return true;
580 }
581
582
583
584
585
586
587
588
589
590
591
592
593
594 /// Perform an integration step of duration 'timeStep'.
595 bool    CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
596 {
597
598
599         //printf("CcdPhysicsEnvironment::proceedDeltaTime\n");
600
601         if (SimdFuzzyZero(timeStep))
602                 return true;
603
604         if (m_debugDrawer)
605         {
606                 gDisableDeactivation = (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_NoDeactivation);
607         }
608
609
610 #ifdef USE_QUICKPROF
611         Profiler::beginBlock("SyncMotionStates");
612 #endif //USE_QUICKPROF
613
614
615         //this is needed because scaling is not known in advance, and scaling has to propagate to the shape
616         if (!m_scalingPropagated)
617         {
618                 SyncMotionStates(timeStep);
619                 m_scalingPropagated = true;
620         }
621
622
623 #ifdef USE_QUICKPROF
624         Profiler::endBlock("SyncMotionStates");
625
626         Profiler::beginBlock("predictIntegratedTransform");
627 #endif //USE_QUICKPROF
628
629         {
630                 //              std::vector<CcdPhysicsController*>::iterator i;
631
632
633
634                 int k;
635                 for (k=0;k<GetNumControllers();k++)
636                 {
637                         CcdPhysicsController* ctrl = m_controllers[k];
638                         //              SimdTransform predictedTrans;
639                         RigidBody* body = ctrl->GetRigidBody();
640                         if (body->IsActive())
641                         {
642                                 if (!body->IsStatic())
643                                 {
644                                         body->applyForces( timeStep);
645                                         body->integrateVelocities( timeStep);
646                                         body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform);
647                                 }
648                         }
649
650                 }
651         }
652
653 #ifdef USE_QUICKPROF
654         Profiler::endBlock("predictIntegratedTransform");
655 #endif //USE_QUICKPROF
656
657         BroadphaseInterface*    scene = GetBroadphase();
658
659
660         //
661         // collision detection (?)
662         //
663
664
665 #ifdef USE_QUICKPROF
666         Profiler::beginBlock("DispatchAllCollisionPairs");
667 #endif //USE_QUICKPROF
668
669
670         int numsubstep = m_numIterations;
671
672
673         DispatcherInfo dispatchInfo;
674         dispatchInfo.m_timeStep = timeStep;
675         dispatchInfo.m_stepCount = 0;
676         dispatchInfo.m_enableSatConvex = m_enableSatCollisionDetection;
677
678         scene->DispatchAllCollisionPairs(*GetDispatcher(),dispatchInfo);///numsubstep,g);
679
680
681 #ifdef USE_QUICKPROF
682         Profiler::endBlock("DispatchAllCollisionPairs");
683 #endif //USE_QUICKPROF
684
685
686         int numRigidBodies = m_controllers.size();
687
688         m_collisionWorld->UpdateActivationState();
689
690         {
691                 int i;
692                 int numConstraints = m_constraints.size();
693                 for (i=0;i< numConstraints ; i++ )
694                 {
695                         TypedConstraint* constraint = m_constraints[i];
696
697                         const RigidBody* colObj0 = &constraint->GetRigidBodyA();
698                         const RigidBody* colObj1 = &constraint->GetRigidBodyB();
699                         
700                         if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
701                                                 ((colObj1) && ((colObj1)->mergesSimulationIslands())))
702                         {
703                                 if (colObj0->IsActive() || colObj1->IsActive())
704                                 {
705                                         GetDispatcher()->GetUnionFind().unite((colObj0)->m_islandTag1,
706                                                 (colObj1)->m_islandTag1);
707                                 }
708                         }
709                 }
710         }
711
712         m_collisionWorld->StoreIslandActivationState();
713
714
715
716         //contacts
717 #ifdef USE_QUICKPROF
718         Profiler::beginBlock("SolveConstraint");
719 #endif //USE_QUICKPROF
720
721
722         //solve the regular constraints (point 2 point, hinge, etc)
723
724         for (int g=0;g<numsubstep;g++)
725         {
726                 //
727                 // constraint solving
728                 //
729
730
731                 int i;
732                 int numConstraints = m_constraints.size();
733
734                 //point to point constraints
735                 for (i=0;i< numConstraints ; i++ )
736                 {
737                         TypedConstraint* constraint = m_constraints[i];
738
739                         constraint->BuildJacobian();
740                         constraint->SolveConstraint( timeStep );
741
742                 }
743
744
745         }
746
747 #ifdef USE_QUICKPROF
748         Profiler::endBlock("SolveConstraint");
749 #endif //USE_QUICKPROF
750
751         //solve the vehicles
752
753 #ifdef NEW_BULLET_VEHICLE_SUPPORT
754         //vehicles
755         int numVehicles = m_wrapperVehicles.size();
756         for (int i=0;i<numVehicles;i++)
757         {
758                 WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
759                 RaycastVehicle* vehicle = wrapperVehicle->GetVehicle();
760                 vehicle->UpdateVehicle( timeStep);
761         }
762 #endif //NEW_BULLET_VEHICLE_SUPPORT
763
764
765         struct InplaceSolverIslandCallback : public CollisionDispatcher::IslandCallback
766         {
767
768                 ContactSolverInfo& m_solverInfo;
769                 ConstraintSolver*       m_solver;
770                 IDebugDraw*     m_debugDrawer;
771
772                 InplaceSolverIslandCallback(
773                         ContactSolverInfo& solverInfo,
774                         ConstraintSolver*       solver,
775                         IDebugDraw*     debugDrawer)
776                         :m_solverInfo(solverInfo),
777                         m_solver(solver),
778                         m_debugDrawer(debugDrawer)
779                 {
780
781                 }
782
783                 virtual void    ProcessIsland(PersistentManifold**      manifolds,int numManifolds)
784                 {
785                         m_solver->SolveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer);
786                 }
787
788         };
789
790
791         m_solverInfo.m_friction = 0.9f;
792         m_solverInfo.m_numIterations = m_numIterations;
793         m_solverInfo.m_timeStep = timeStep;
794         m_solverInfo.m_restitution = 0.f;//m_restitution;
795
796         InplaceSolverIslandCallback     solverCallback(
797                 m_solverInfo,
798                 m_solver,
799                 m_debugDrawer);
800
801 #ifdef USE_QUICKPROF
802         Profiler::beginBlock("BuildAndProcessIslands");
803 #endif //USE_QUICKPROF
804
805         /// solve all the contact points and contact friction
806         GetDispatcher()->BuildAndProcessIslands(m_collisionWorld->GetCollisionObjectArray(),&solverCallback);
807
808 #ifdef USE_QUICKPROF
809         Profiler::endBlock("BuildAndProcessIslands");
810
811         Profiler::beginBlock("CallbackTriggers");
812 #endif //USE_QUICKPROF
813
814         CallbackTriggers();
815
816 #ifdef USE_QUICKPROF
817         Profiler::endBlock("CallbackTriggers");
818
819
820         Profiler::beginBlock("proceedToTransform");
821
822 #endif //USE_QUICKPROF
823         {
824
825
826
827                 {
828
829                         
830                         
831                         UpdateAabbs(timeStep);
832
833
834                         float toi = 1.f;
835
836
837
838                         if (m_ccdMode == 3)
839                         {
840                                 DispatcherInfo dispatchInfo;
841                                 dispatchInfo.m_timeStep = timeStep;
842                                 dispatchInfo.m_stepCount = 0;
843                                 dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS;
844
845                                 scene->DispatchAllCollisionPairs( *GetDispatcher(),dispatchInfo);///numsubstep,g);
846                                 toi = dispatchInfo.m_timeOfImpact;
847
848                         }
849
850                         
851
852                         //
853                         // integrating solution
854                         //
855
856                         {
857                                 
858                                 std::vector<CcdPhysicsController*>::iterator i;
859
860                                 for (i=m_controllers.begin();
861                                         !(i==m_controllers.end()); i++)
862                                 {
863
864                                         CcdPhysicsController* ctrl = *i;
865
866                                         SimdTransform predictedTrans;
867                                         RigidBody* body = ctrl->GetRigidBody();
868                                         
869                                         if (body->IsActive())
870                                         {
871
872                                                 if (!body->IsStatic())
873                                                 {
874                                                         body->predictIntegratedTransform(timeStep*      toi, predictedTrans);
875                                                         body->proceedToTransform( predictedTrans);
876                                                 }
877
878                                         }
879                                 }
880
881                         }
882
883
884
885
886
887                         //
888                         // disable sleeping physics objects
889                         //
890
891                         std::vector<CcdPhysicsController*> m_sleepingControllers;
892
893                         std::vector<CcdPhysicsController*>::iterator i;
894
895                         for (i=m_controllers.begin();
896                                 !(i==m_controllers.end()); i++)
897                         {
898                                 CcdPhysicsController* ctrl = (*i);
899                                 RigidBody* body = ctrl->GetRigidBody();
900
901                                 ctrl->UpdateDeactivation(timeStep);
902
903
904                                 if (ctrl->wantsSleeping())
905                                 {
906                                         if (body->GetActivationState() == ACTIVE_TAG)
907                                                 body->SetActivationState( WANTS_DEACTIVATION );
908                                 } else
909                                 {
910                                         if (body->GetActivationState() != DISABLE_DEACTIVATION)
911                                                 body->SetActivationState( ACTIVE_TAG );
912                                 }
913
914                                 if (useIslands)
915                                 {
916                                         if (body->GetActivationState() == ISLAND_SLEEPING)
917                                         {
918                                                 m_sleepingControllers.push_back(ctrl);
919                                         }
920                                 } else
921                                 {
922                                         if (ctrl->wantsSleeping())
923                                         {
924                                                 m_sleepingControllers.push_back(ctrl);
925                                         }
926                                 }
927                         }
928
929
930
931
932                 }
933
934
935 #ifdef USE_QUICKPROF
936                 Profiler::endBlock("proceedToTransform");
937
938                 Profiler::beginBlock("SyncMotionStates");
939 #endif //USE_QUICKPROF
940
941                 SyncMotionStates(timeStep);
942
943 #ifdef USE_QUICKPROF
944                 Profiler::endBlock("SyncMotionStates");
945
946                 Profiler::endProfilingCycle();
947 #endif //USE_QUICKPROF
948
949
950 #ifdef NEW_BULLET_VEHICLE_SUPPORT
951                 //sync wheels for vehicles
952                 int numVehicles = m_wrapperVehicles.size();
953                 for (int i=0;i<numVehicles;i++)
954                 {
955                         WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
956
957                         wrapperVehicle->SyncWheels();
958                 }
959 #endif //NEW_BULLET_VEHICLE_SUPPORT
960         }
961
962         return true;
963 }
964
965 void            CcdPhysicsEnvironment::setDebugMode(int debugMode)
966 {
967         if (m_debugDrawer){
968                 m_debugDrawer->SetDebugMode(debugMode);
969         }
970 }
971
972 void            CcdPhysicsEnvironment::setNumIterations(int numIter)
973 {
974         m_numIterations = numIter;
975 }
976 void            CcdPhysicsEnvironment::setDeactivationTime(float dTime)
977 {
978         gDeactivationTime = dTime;
979 }
980 void            CcdPhysicsEnvironment::setDeactivationLinearTreshold(float linTresh)
981 {
982         gLinearSleepingTreshold = linTresh;
983 }
984 void            CcdPhysicsEnvironment::setDeactivationAngularTreshold(float angTresh) 
985 {
986         gAngularSleepingTreshold = angTresh;
987 }
988 void            CcdPhysicsEnvironment::setContactBreakingTreshold(float contactBreakingTreshold)
989 {
990         gContactBreakingTreshold = contactBreakingTreshold;
991
992 }
993
994
995 void            CcdPhysicsEnvironment::setCcdMode(int ccdMode)
996 {
997         m_ccdMode = ccdMode;
998 }
999
1000
1001 void            CcdPhysicsEnvironment::setSolverSorConstant(float sor)
1002 {
1003         m_solverInfo.m_sor = sor;
1004 }
1005
1006 void            CcdPhysicsEnvironment::setSolverTau(float tau)
1007 {
1008         m_solverInfo.m_tau = tau;
1009 }
1010 void            CcdPhysicsEnvironment::setSolverDamping(float damping)
1011 {
1012         m_solverInfo.m_damping = damping;
1013 }
1014
1015
1016 void            CcdPhysicsEnvironment::setLinearAirDamping(float damping)
1017 {
1018         gLinearAirDamping = damping;
1019 }
1020
1021 void            CcdPhysicsEnvironment::setUseEpa(bool epa)
1022 {
1023         gUseEpa = epa;
1024 }
1025
1026 void            CcdPhysicsEnvironment::setSolverType(int solverType)
1027 {
1028
1029         switch (solverType)
1030         {
1031         case 1:
1032                 {
1033                         if (m_solverType != solverType)
1034                         {
1035
1036                                 m_solver = new SimpleConstraintSolver();
1037
1038                                 break;
1039                         }
1040                 }
1041
1042         case 0:
1043         default:
1044                 if (m_solverType != solverType)
1045                 {
1046                         m_solver = new OdeConstraintSolver();
1047
1048                         break;
1049                 }
1050
1051         };
1052
1053         m_solverType = solverType ;
1054 }
1055
1056
1057
1058
1059
1060 void    CcdPhysicsEnvironment::SyncMotionStates(float timeStep)
1061 {
1062         std::vector<CcdPhysicsController*>::iterator i;
1063
1064         //
1065         // synchronize the physics and graphics transformations
1066         //
1067
1068         for (i=m_controllers.begin();
1069                 !(i==m_controllers.end()); i++)
1070         {
1071                 CcdPhysicsController* ctrl = (*i);
1072                 ctrl->SynchronizeMotionStates(timeStep);
1073
1074         }
1075
1076 }
1077 void            CcdPhysicsEnvironment::setGravity(float x,float y,float z)
1078 {
1079         m_gravity = SimdVector3(x,y,z);
1080
1081         std::vector<CcdPhysicsController*>::iterator i;
1082
1083         //todo: review this gravity stuff
1084         for (i=m_controllers.begin();
1085                 !(i==m_controllers.end()); i++)
1086         {
1087
1088                 CcdPhysicsController* ctrl = (*i);
1089                 ctrl->GetRigidBody()->setGravity(m_gravity);
1090
1091         }
1092 }
1093
1094
1095 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1096
1097 class DefaultVehicleRaycaster : public VehicleRaycaster
1098 {
1099         CcdPhysicsEnvironment* m_physEnv;
1100         PHY_IPhysicsController* m_chassis;
1101
1102 public:
1103         DefaultVehicleRaycaster(CcdPhysicsEnvironment* physEnv,PHY_IPhysicsController* chassis):
1104           m_physEnv(physEnv),
1105                   m_chassis(chassis)
1106           {
1107           }
1108
1109           /*    struct VehicleRaycasterResult
1110           {
1111           VehicleRaycasterResult() :m_distFraction(-1.f){};
1112           SimdVector3   m_hitPointInWorld;
1113           SimdVector3   m_hitNormalInWorld;
1114           SimdScalar    m_distFraction;
1115           };
1116           */
1117           virtual void* CastRay(const SimdVector3& from,const SimdVector3& to, VehicleRaycasterResult& result)
1118           {
1119
1120
1121                   float hit[3];
1122                   float normal[3];
1123
1124                   PHY_IPhysicsController*       ignore = m_chassis;
1125                   void* hitObject = m_physEnv->rayTest(ignore,from.x(),from.y(),from.z(),to.x(),to.y(),to.z(),hit[0],hit[1],hit[2],normal[0],normal[1],normal[2]);
1126                   if (hitObject)
1127                   {
1128                           result.m_hitPointInWorld[0] = hit[0];
1129                           result.m_hitPointInWorld[1] = hit[1];
1130                           result.m_hitPointInWorld[2] = hit[2];
1131                           result.m_hitNormalInWorld[0] = normal[0];
1132                           result.m_hitNormalInWorld[1] = normal[1];
1133                           result.m_hitNormalInWorld[2] = normal[2];
1134                           result.m_hitNormalInWorld.normalize();
1135                           //calc fraction? or put it in the interface?
1136                           //calc for now
1137
1138                           result.m_distFraction = (result.m_hitPointInWorld-from).length() / (to-from).length();
1139                           //some safety for 'explosion' due to sudden penetration of the full 'ray'
1140                           /*                    if (result.m_distFraction<0.1)
1141                           {
1142                           printf("Vehicle Raycast: avoided instability due to penetration. Consider moving the connection points deeper inside vehicle chassis");
1143                           result.m_distFraction = 1.f;
1144                           hitObject = 0;
1145                           }
1146                           */
1147
1148                           /*                    if (result.m_distFraction>1.)
1149                           {
1150                           printf("Vehicle Raycast: avoided instability 1Consider moving the connection points deeper inside vehicle chassis");                          
1151                           result.m_distFraction = 1.f;
1152                           hitObject = 0;
1153                           }
1154                           */
1155
1156
1157
1158                   }
1159                   //?
1160                   return hitObject;
1161           }
1162 };
1163 #endif //NEW_BULLET_VEHICLE_SUPPORT
1164
1165 static int gConstraintUid = 1;
1166
1167 int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
1168                                                                                                         float pivotX,float pivotY,float pivotZ,
1169                                                                                                         float axisX,float axisY,float axisZ)
1170 {
1171
1172
1173         CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
1174         CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
1175
1176         RigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
1177         RigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
1178
1179         ASSERT(rb0);
1180
1181         SimdVector3 pivotInA(pivotX,pivotY,pivotZ);
1182         SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
1183         SimdVector3 axisInA(axisX,axisY,axisZ);
1184         SimdVector3 axisInB = rb1 ? 
1185                 (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * -axisInA)) : 
1186         rb0->getCenterOfMassTransform().getBasis() * -axisInA;
1187
1188         bool angularOnly = false;
1189
1190         switch (type)
1191         {
1192         case PHY_POINT2POINT_CONSTRAINT:
1193                 {
1194
1195                         Point2PointConstraint* p2p = 0;
1196
1197                         if (rb1)
1198                         {
1199                                 p2p = new Point2PointConstraint(*rb0,
1200                                         *rb1,pivotInA,pivotInB);
1201                         } else
1202                         {
1203                                 p2p = new Point2PointConstraint(*rb0,
1204                                         pivotInA);
1205                         }
1206
1207                         m_constraints.push_back(p2p);
1208                         p2p->SetUserConstraintId(gConstraintUid++);
1209                         p2p->SetUserConstraintType(type);
1210                         //64 bit systems can't cast pointer to int. could use size_t instead.
1211                         return p2p->GetUserConstraintId();
1212
1213                         break;
1214                 }
1215
1216         case PHY_ANGULAR_CONSTRAINT:
1217                 angularOnly = true;
1218
1219         case PHY_LINEHINGE_CONSTRAINT:
1220                 {
1221                         HingeConstraint* hinge = 0;
1222
1223                         if (rb1)
1224                         {
1225                                 hinge = new HingeConstraint(
1226                                         *rb0,
1227                                         *rb1,pivotInA,pivotInB,axisInA,axisInB);
1228
1229
1230                         } else
1231                         {
1232                                 hinge = new HingeConstraint(*rb0,
1233                                         pivotInA,axisInA);
1234
1235                         }
1236                         hinge->setAngularOnly(angularOnly);
1237
1238                         m_constraints.push_back(hinge);
1239                         hinge->SetUserConstraintId(gConstraintUid++);
1240                         hinge->SetUserConstraintType(type);
1241                         //64 bit systems can't cast pointer to int. could use size_t instead.
1242                         return hinge->GetUserConstraintId();
1243                         break;
1244                 }
1245 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1246
1247         case PHY_VEHICLE_CONSTRAINT:
1248                 {
1249                         RaycastVehicle::VehicleTuning* tuning = new RaycastVehicle::VehicleTuning();
1250                         RigidBody* chassis = rb0;
1251                         DefaultVehicleRaycaster* raycaster = new DefaultVehicleRaycaster(this,ctrl0);
1252                         RaycastVehicle* vehicle = new RaycastVehicle(*tuning,chassis,raycaster);
1253                         WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
1254                         m_wrapperVehicles.push_back(wrapperVehicle);
1255                         vehicle->SetUserConstraintId(gConstraintUid++);
1256                         vehicle->SetUserConstraintType(type);
1257                         return vehicle->GetUserConstraintId();
1258
1259                         break;
1260                 };
1261 #endif //NEW_BULLET_VEHICLE_SUPPORT
1262
1263         default:
1264                 {
1265                 }
1266         };
1267
1268         //RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB
1269
1270         return 0;
1271
1272 }
1273
1274 void            CcdPhysicsEnvironment::removeConstraint(int     constraintId)
1275 {
1276         std::vector<TypedConstraint*>::iterator i;
1277
1278         for (i=m_constraints.begin();
1279                 !(i==m_constraints.end()); i++)
1280         {
1281                 TypedConstraint* constraint = (*i);
1282                 if (constraint->GetUserConstraintId() == constraintId)
1283                 {
1284                         //activate objects
1285                         if (constraint->GetRigidBodyA().mergesSimulationIslands())
1286                                 constraint->GetRigidBodyA().activate();
1287                         if (constraint->GetRigidBodyB().mergesSimulationIslands())
1288                                 constraint->GetRigidBodyB().activate();
1289
1290                         std::swap(*i, m_constraints.back());
1291
1292                         
1293                         m_constraints.pop_back();
1294                         break;
1295                 }
1296         }
1297
1298 }
1299
1300
1301         struct  FilterClosestRayResultCallback : public CollisionWorld::ClosestRayResultCallback
1302         {
1303                 PHY_IPhysicsController* m_ignoreClient;
1304
1305                 FilterClosestRayResultCallback (PHY_IPhysicsController* ignoreClient,const SimdVector3& rayFrom,const SimdVector3& rayTo)
1306                         : CollisionWorld::ClosestRayResultCallback(rayFrom,rayTo),
1307                         m_ignoreClient(ignoreClient)
1308                 {
1309
1310                 }
1311
1312                 virtual ~FilterClosestRayResultCallback()
1313                 {
1314                 }
1315
1316                 virtual float   AddSingleResult(const CollisionWorld::LocalRayResult& rayResult)
1317                 {
1318                         CcdPhysicsController* curHit = static_cast<CcdPhysicsController*>(rayResult.m_collisionObject->m_userPointer);
1319                         //ignore client...
1320                         if (curHit != m_ignoreClient)
1321                         {               
1322                                 //if valid
1323                                 return ClosestRayResultCallback::AddSingleResult(rayResult);
1324                         }
1325                         return m_closestHitFraction;
1326                 }
1327
1328         };
1329
1330 PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ, 
1331                                                                                                            float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
1332 {
1333
1334
1335         float minFraction = 1.f;
1336
1337         SimdVector3 rayFrom(fromX,fromY,fromZ);
1338         SimdVector3 rayTo(toX,toY,toZ);
1339
1340         SimdVector3     hitPointWorld,normalWorld;
1341
1342         //Either Ray Cast with or without filtering
1343
1344         //CollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo);
1345         FilterClosestRayResultCallback   rayCallback(ignoreClient,rayFrom,rayTo);
1346
1347
1348         PHY_IPhysicsController* nearestHit = 0;
1349
1350         m_collisionWorld->RayTest(rayFrom,rayTo,rayCallback);
1351         if (rayCallback.HasHit())
1352         {
1353                 nearestHit = static_cast<CcdPhysicsController*>(rayCallback.m_collisionObject->m_userPointer);
1354                 hitX =  rayCallback.m_hitPointWorld.getX();
1355                 hitY =  rayCallback.m_hitPointWorld.getY();
1356                 hitZ =  rayCallback.m_hitPointWorld.getZ();
1357
1358                 normalX = rayCallback.m_hitNormalWorld.getX();
1359                 normalY = rayCallback.m_hitNormalWorld.getY();
1360                 normalZ = rayCallback.m_hitNormalWorld.getZ();
1361
1362         }       
1363
1364
1365         return nearestHit;
1366 }
1367
1368
1369
1370 int     CcdPhysicsEnvironment::getNumContactPoints()
1371 {
1372         return 0;
1373 }
1374
1375 void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
1376 {
1377
1378 }
1379
1380
1381
1382
1383 BroadphaseInterface*    CcdPhysicsEnvironment::GetBroadphase()
1384
1385         return m_collisionWorld->GetBroadphase(); 
1386 }
1387
1388
1389
1390 const CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher() const
1391 {
1392         return m_collisionWorld->GetDispatcher();
1393 }
1394
1395 CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher()
1396 {
1397         return m_collisionWorld->GetDispatcher();
1398 }
1399
1400 CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
1401 {
1402
1403 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1404         m_wrapperVehicles.clear();
1405 #endif //NEW_BULLET_VEHICLE_SUPPORT
1406
1407         //m_broadphase->DestroyScene();
1408         //delete broadphase ? release reference on broadphase ?
1409
1410         //first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher
1411         //delete m_dispatcher;
1412         delete m_collisionWorld;
1413
1414 }
1415
1416
1417 int     CcdPhysicsEnvironment::GetNumControllers()
1418 {
1419         return m_controllers.size();
1420 }
1421
1422
1423 CcdPhysicsController* CcdPhysicsEnvironment::GetPhysicsController( int index)
1424 {
1425         return m_controllers[index];
1426 }
1427
1428
1429 int     CcdPhysicsEnvironment::GetNumManifolds() const
1430 {
1431         return GetDispatcher()->GetNumManifolds();
1432 }
1433
1434 const PersistentManifold*       CcdPhysicsEnvironment::GetManifold(int index) const
1435 {
1436         return GetDispatcher()->GetManifoldByIndexInternal(index);
1437 }
1438
1439 TypedConstraint*        CcdPhysicsEnvironment::getConstraintById(int constraintId)
1440 {
1441         int numConstraint = m_constraints.size();
1442         int i;
1443         for (i=0;i<numConstraint;i++)
1444         {
1445                 TypedConstraint* constraint = m_constraints[i];
1446                 if (constraint->GetUserConstraintId()==constraintId)
1447                 {
1448                         return constraint;
1449                 }
1450         }
1451         return 0;
1452 }
1453
1454
1455 void CcdPhysicsEnvironment::addSensor(PHY_IPhysicsController* ctrl)
1456 {
1457
1458         CcdPhysicsController* ctrl1 = (CcdPhysicsController* )ctrl;
1459         std::vector<CcdPhysicsController*>::iterator i =
1460                 std::find(m_controllers.begin(), m_controllers.end(), ctrl);
1461         if ((i == m_controllers.end()))
1462         {
1463                 addCcdPhysicsController(ctrl1);
1464         }
1465
1466         requestCollisionCallback(ctrl);
1467         //printf("addSensor\n");
1468 }
1469
1470 void CcdPhysicsEnvironment::removeCollisionCallback(PHY_IPhysicsController* ctrl)
1471 {
1472         std::vector<CcdPhysicsController*>::iterator i =
1473                 std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl);
1474         if (!(i == m_triggerControllers.end()))
1475         {
1476                 std::swap(*i, m_triggerControllers.back());
1477                 m_triggerControllers.pop_back();
1478         }
1479 }
1480
1481
1482 void CcdPhysicsEnvironment::removeSensor(PHY_IPhysicsController* ctrl)
1483 {
1484         removeCollisionCallback(ctrl);
1485         //printf("removeSensor\n");
1486 }
1487 void CcdPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user)
1488 {
1489         /*      printf("addTouchCallback\n(response class = %i)\n",response_class);
1490
1491         //map PHY_ convention into SM_ convention
1492         switch (response_class)
1493         {
1494         case    PHY_FH_RESPONSE:
1495         printf("PHY_FH_RESPONSE\n");
1496         break;
1497         case PHY_SENSOR_RESPONSE:
1498         printf("PHY_SENSOR_RESPONSE\n");
1499         break;
1500         case PHY_CAMERA_RESPONSE:
1501         printf("PHY_CAMERA_RESPONSE\n");
1502         break;
1503         case PHY_OBJECT_RESPONSE:
1504         printf("PHY_OBJECT_RESPONSE\n");
1505         break;
1506         case PHY_STATIC_RESPONSE:
1507         printf("PHY_STATIC_RESPONSE\n");
1508         break;
1509         default:
1510         assert(0);
1511         return;
1512         }
1513         */
1514
1515         m_triggerCallbacks[response_class] = callback;
1516         m_triggerCallbacksUserPtrs[response_class] = user;
1517
1518 }
1519 void CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctrl)
1520 {
1521         CcdPhysicsController* ccdCtrl = static_cast<CcdPhysicsController*>(ctrl);
1522
1523         //printf("requestCollisionCallback\n");
1524         m_triggerControllers.push_back(ccdCtrl);
1525 }
1526
1527
1528 void    CcdPhysicsEnvironment::CallbackTriggers()
1529 {
1530         CcdPhysicsController* ctrl0=0,*ctrl1=0;
1531
1532         if (m_triggerCallbacks[PHY_OBJECT_RESPONSE])
1533         {
1534                 //walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback
1535                 int numManifolds = m_collisionWorld->GetDispatcher()->GetNumManifolds();
1536                 for (int i=0;i<numManifolds;i++)
1537                 {
1538                         PersistentManifold* manifold = m_collisionWorld->GetDispatcher()->GetManifoldByIndexInternal(i);
1539                         int numContacts = manifold->GetNumContacts();
1540                         if (numContacts)
1541                         {
1542                                 RigidBody* obj0 = static_cast<RigidBody* >(manifold->GetBody0());
1543                                 RigidBody* obj1 = static_cast<RigidBody* >(manifold->GetBody1());
1544
1545                                 //m_userPointer is set in 'addPhysicsController
1546                                 CcdPhysicsController* ctrl0 = static_cast<CcdPhysicsController*>(obj0->m_userPointer);
1547                                 CcdPhysicsController* ctrl1 = static_cast<CcdPhysicsController*>(obj1->m_userPointer);
1548
1549                                 std::vector<CcdPhysicsController*>::iterator i =
1550                                         std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl0);
1551                                 if (i == m_triggerControllers.end())
1552                                 {
1553                                         i = std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl1);
1554                                 }
1555
1556                                 if (!(i == m_triggerControllers.end()))
1557                                 {
1558                                         m_triggerCallbacks[PHY_OBJECT_RESPONSE](m_triggerCallbacksUserPtrs[PHY_OBJECT_RESPONSE],
1559                                                 ctrl0,ctrl1,0);
1560                                 }
1561                         }
1562                 }
1563
1564
1565
1566         }
1567
1568 }
1569
1570
1571
1572
1573
1574
1575 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1576
1577 //complex constraint for vehicles
1578 PHY_IVehicle*   CcdPhysicsEnvironment::getVehicleConstraint(int constraintId)
1579 {
1580         int i;
1581
1582         int numVehicles = m_wrapperVehicles.size();
1583         for (i=0;i<numVehicles;i++)
1584         {
1585                 WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
1586                 if (wrapperVehicle->GetVehicle()->GetUserConstraintId() == constraintId)
1587                         return wrapperVehicle;
1588         }
1589
1590         return 0;
1591 }
1592
1593 #endif //NEW_BULLET_VEHICLE_SUPPORT
1594
1595
1596
1597 void    CcdPhysicsEnvironment::UpdateAabbs(float        timeStep)
1598 {
1599         std::vector<CcdPhysicsController*>::iterator i;
1600         BroadphaseInterface* scene =  GetBroadphase();
1601
1602         //
1603                         // update aabbs, only for moving objects (!)
1604                         //
1605                         for (i=m_controllers.begin();
1606                                 !(i==m_controllers.end()); i++)
1607                         {
1608                                 CcdPhysicsController* ctrl = (*i);
1609                                 RigidBody* body = ctrl->GetRigidBody();
1610
1611
1612                                 SimdPoint3 minAabb,maxAabb;
1613                                 CollisionShape* shapeinterface = ctrl->GetCollisionShape();
1614
1615
1616
1617                                 shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(),
1618                                         body->getLinearVelocity(),
1619                                         //body->getAngularVelocity(),
1620                                         SimdVector3(0.f,0.f,0.f),//no angular effect for now //body->getAngularVelocity(),
1621                                         timeStep,minAabb,maxAabb);
1622
1623
1624                                 SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold);
1625                                 minAabb -= manifoldExtraExtents;
1626                                 maxAabb += manifoldExtraExtents;
1627
1628                                 BroadphaseProxy* bp = body->m_broadphaseHandle;
1629                                 if (bp)
1630                                 {
1631
1632                                         SimdVector3 color (1,1,0);
1633
1634                                         if (m_debugDrawer)
1635                                         {       
1636                                                 //draw aabb
1637                                                 switch (body->GetActivationState())
1638                                                 {
1639                                                 case ISLAND_SLEEPING:
1640                                                         {
1641                                                                 color.setValue(1,1,1);
1642                                                                 break;
1643                                                         }
1644                                                 case WANTS_DEACTIVATION:
1645                                                         {
1646                                                                 color.setValue(0,0,1);
1647                                                                 break;
1648                                                         }
1649                                                 case ACTIVE_TAG:
1650                                                         {
1651                                                                 break;
1652                                                         }
1653                                                 case DISABLE_DEACTIVATION:
1654                                                         {
1655                                                                 color.setValue(1,0,1);
1656                                                         };
1657
1658                                                 };
1659
1660                                                 if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)
1661                                                 {
1662                                                         DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
1663                                                 }
1664                                         }
1665
1666                         
1667                                         if ( (maxAabb-minAabb).length2() < 1e12f)
1668                                         {
1669                                                 scene->SetAabb(bp,minAabb,maxAabb);
1670                                         } else
1671                                         {
1672                                                 //something went wrong, investigate
1673                                                 //removeCcdPhysicsController(ctrl);
1674                                                 body->SetActivationState(DISABLE_SIMULATION);
1675
1676                                                 static bool reportMe = true;
1677                                                 if (reportMe)
1678                                                 {
1679                                                         reportMe = false;
1680                                                         printf("Overflow in AABB, object removed from simulation \n");
1681                                                         printf("If you can reproduce this, please email bugs@continuousphysics.com\n");
1682                                                         printf("Please include above information, your Platform, version of OS.\n");
1683                                                         printf("Thanks.\n");
1684                                                 }
1685                                                 
1686                                         }
1687
1688                                 }
1689                         }
1690 }
1691
1692 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateSphereController(float radius,const PHY__Vector3& position)
1693 {
1694         
1695         CcdConstructionInfo     cinfo;
1696         cinfo.m_collisionShape = new SphereShape(radius);
1697         cinfo.m_MotionState = 0;
1698         cinfo.m_physicsEnv = this;
1699         cinfo.m_collisionFlags |= CollisionObject::noContactResponse;
1700         DefaultMotionState* motionState = new DefaultMotionState();
1701         cinfo.m_MotionState = motionState;
1702         motionState->m_worldTransform.setIdentity();
1703         motionState->m_worldTransform.setOrigin(SimdVector3(position[0],position[1],position[2]));
1704
1705         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
1706         
1707
1708         return sphereController;
1709 }
1710
1711
1712 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float coneradius,float coneheight)
1713 {
1714         CcdConstructionInfo     cinfo;
1715         cinfo.m_collisionShape = new ConeShape(coneradius,coneheight);
1716         cinfo.m_MotionState = 0;
1717         cinfo.m_physicsEnv = this;
1718         DefaultMotionState* motionState = new DefaultMotionState();
1719         cinfo.m_MotionState = motionState;
1720         motionState->m_worldTransform.setIdentity();
1721 //      motionState->m_worldTransform.setOrigin(SimdVector3(position[0],position[1],position[2]));
1722
1723         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
1724
1725
1726         return sphereController;
1727 }
1728