f169059cf378152f057e4f52f64b454a6d7dcf1e
[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->GetActivationState() != ISLAND_SLEEPING)
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 CollisionObject* colObj0 = &constraint->GetRigidBodyA();
698                         const CollisionObject* colObj1 = &constraint->GetRigidBodyB();
699                         
700                         if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
701                                                 ((colObj1) && ((colObj1)->mergesSimulationIslands())))
702                         {
703                                 GetDispatcher()->GetUnionFind().unite((colObj0)->m_islandTag1,
704                                         (colObj1)->m_islandTag1);
705                         }
706                 }
707         }
708
709         m_collisionWorld->StoreIslandActivationState();
710
711
712
713         //contacts
714 #ifdef USE_QUICKPROF
715         Profiler::beginBlock("SolveConstraint");
716 #endif //USE_QUICKPROF
717
718
719         //solve the regular constraints (point 2 point, hinge, etc)
720
721         for (int g=0;g<numsubstep;g++)
722         {
723                 //
724                 // constraint solving
725                 //
726
727
728                 int i;
729                 int numConstraints = m_constraints.size();
730
731                 //point to point constraints
732                 for (i=0;i< numConstraints ; i++ )
733                 {
734                         TypedConstraint* constraint = m_constraints[i];
735
736                         constraint->BuildJacobian();
737                         constraint->SolveConstraint( timeStep );
738
739                 }
740
741
742         }
743
744 #ifdef USE_QUICKPROF
745         Profiler::endBlock("SolveConstraint");
746 #endif //USE_QUICKPROF
747
748         //solve the vehicles
749
750 #ifdef NEW_BULLET_VEHICLE_SUPPORT
751         //vehicles
752         int numVehicles = m_wrapperVehicles.size();
753         for (int i=0;i<numVehicles;i++)
754         {
755                 WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
756                 RaycastVehicle* vehicle = wrapperVehicle->GetVehicle();
757                 vehicle->UpdateVehicle( timeStep);
758         }
759 #endif //NEW_BULLET_VEHICLE_SUPPORT
760
761
762         struct InplaceSolverIslandCallback : public CollisionDispatcher::IslandCallback
763         {
764
765                 ContactSolverInfo& m_solverInfo;
766                 ConstraintSolver*       m_solver;
767                 IDebugDraw*     m_debugDrawer;
768
769                 InplaceSolverIslandCallback(
770                         ContactSolverInfo& solverInfo,
771                         ConstraintSolver*       solver,
772                         IDebugDraw*     debugDrawer)
773                         :m_solverInfo(solverInfo),
774                         m_solver(solver),
775                         m_debugDrawer(debugDrawer)
776                 {
777
778                 }
779
780                 virtual void    ProcessIsland(PersistentManifold**      manifolds,int numManifolds)
781                 {
782                         m_solver->SolveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer);
783                 }
784
785         };
786
787
788         m_solverInfo.m_friction = 0.9f;
789         m_solverInfo.m_numIterations = m_numIterations;
790         m_solverInfo.m_timeStep = timeStep;
791         m_solverInfo.m_restitution = 0.f;//m_restitution;
792
793         InplaceSolverIslandCallback     solverCallback(
794                 m_solverInfo,
795                 m_solver,
796                 m_debugDrawer);
797
798 #ifdef USE_QUICKPROF
799         Profiler::beginBlock("BuildAndProcessIslands");
800 #endif //USE_QUICKPROF
801
802         /// solve all the contact points and contact friction
803         GetDispatcher()->BuildAndProcessIslands(m_collisionWorld->GetCollisionObjectArray(),&solverCallback);
804
805 #ifdef USE_QUICKPROF
806         Profiler::endBlock("BuildAndProcessIslands");
807
808         Profiler::beginBlock("CallbackTriggers");
809 #endif //USE_QUICKPROF
810
811         CallbackTriggers();
812
813 #ifdef USE_QUICKPROF
814         Profiler::endBlock("CallbackTriggers");
815
816
817         Profiler::beginBlock("proceedToTransform");
818
819 #endif //USE_QUICKPROF
820         {
821
822
823
824                 {
825
826                         
827                         
828                         UpdateAabbs(timeStep);
829
830
831                         float toi = 1.f;
832
833
834
835                         if (m_ccdMode == 3)
836                         {
837                                 DispatcherInfo dispatchInfo;
838                                 dispatchInfo.m_timeStep = timeStep;
839                                 dispatchInfo.m_stepCount = 0;
840                                 dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS;
841
842                                 scene->DispatchAllCollisionPairs( *GetDispatcher(),dispatchInfo);///numsubstep,g);
843                                 toi = dispatchInfo.m_timeOfImpact;
844
845                         }
846
847                         
848
849                         //
850                         // integrating solution
851                         //
852
853                         {
854                                 
855                                 std::vector<CcdPhysicsController*>::iterator i;
856
857                                 for (i=m_controllers.begin();
858                                         !(i==m_controllers.end()); i++)
859                                 {
860
861                                         CcdPhysicsController* ctrl = *i;
862
863                                         SimdTransform predictedTrans;
864                                         RigidBody* body = ctrl->GetRigidBody();
865                                         if (body->GetActivationState() != ISLAND_SLEEPING)
866                                         {
867
868                                                 if (!body->IsStatic())
869                                                 {
870                                                         body->predictIntegratedTransform(timeStep*      toi, predictedTrans);
871                                                         body->proceedToTransform( predictedTrans);
872                                                 }
873
874                                         }
875                                 }
876
877                         }
878
879
880
881
882
883                         //
884                         // disable sleeping physics objects
885                         //
886
887                         std::vector<CcdPhysicsController*> m_sleepingControllers;
888
889                         std::vector<CcdPhysicsController*>::iterator i;
890
891                         for (i=m_controllers.begin();
892                                 !(i==m_controllers.end()); i++)
893                         {
894                                 CcdPhysicsController* ctrl = (*i);
895                                 RigidBody* body = ctrl->GetRigidBody();
896
897                                 ctrl->UpdateDeactivation(timeStep);
898
899
900                                 if (ctrl->wantsSleeping())
901                                 {
902                                         if (body->GetActivationState() == ACTIVE_TAG)
903                                                 body->SetActivationState( WANTS_DEACTIVATION );
904                                 } else
905                                 {
906                                         if (body->GetActivationState() != DISABLE_DEACTIVATION)
907                                                 body->SetActivationState( ACTIVE_TAG );
908                                 }
909
910                                 if (useIslands)
911                                 {
912                                         if (body->GetActivationState() == ISLAND_SLEEPING)
913                                         {
914                                                 m_sleepingControllers.push_back(ctrl);
915                                         }
916                                 } else
917                                 {
918                                         if (ctrl->wantsSleeping())
919                                         {
920                                                 m_sleepingControllers.push_back(ctrl);
921                                         }
922                                 }
923                         }
924
925
926
927
928                 }
929
930
931 #ifdef USE_QUICKPROF
932                 Profiler::endBlock("proceedToTransform");
933
934                 Profiler::beginBlock("SyncMotionStates");
935 #endif //USE_QUICKPROF
936
937                 SyncMotionStates(timeStep);
938
939 #ifdef USE_QUICKPROF
940                 Profiler::endBlock("SyncMotionStates");
941
942                 Profiler::endProfilingCycle();
943 #endif //USE_QUICKPROF
944
945
946 #ifdef NEW_BULLET_VEHICLE_SUPPORT
947                 //sync wheels for vehicles
948                 int numVehicles = m_wrapperVehicles.size();
949                 for (int i=0;i<numVehicles;i++)
950                 {
951                         WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
952
953                         wrapperVehicle->SyncWheels();
954                 }
955 #endif //NEW_BULLET_VEHICLE_SUPPORT
956         }
957
958         return true;
959 }
960
961 void            CcdPhysicsEnvironment::setDebugMode(int debugMode)
962 {
963         if (m_debugDrawer){
964                 m_debugDrawer->SetDebugMode(debugMode);
965         }
966 }
967
968 void            CcdPhysicsEnvironment::setNumIterations(int numIter)
969 {
970         m_numIterations = numIter;
971 }
972 void            CcdPhysicsEnvironment::setDeactivationTime(float dTime)
973 {
974         gDeactivationTime = dTime;
975 }
976 void            CcdPhysicsEnvironment::setDeactivationLinearTreshold(float linTresh)
977 {
978         gLinearSleepingTreshold = linTresh;
979 }
980 void            CcdPhysicsEnvironment::setDeactivationAngularTreshold(float angTresh) 
981 {
982         gAngularSleepingTreshold = angTresh;
983 }
984 void            CcdPhysicsEnvironment::setContactBreakingTreshold(float contactBreakingTreshold)
985 {
986         gContactBreakingTreshold = contactBreakingTreshold;
987
988 }
989
990
991 void            CcdPhysicsEnvironment::setCcdMode(int ccdMode)
992 {
993         m_ccdMode = ccdMode;
994 }
995
996
997 void            CcdPhysicsEnvironment::setSolverSorConstant(float sor)
998 {
999         m_solverInfo.m_sor = sor;
1000 }
1001
1002 void            CcdPhysicsEnvironment::setSolverTau(float tau)
1003 {
1004         m_solverInfo.m_tau = tau;
1005 }
1006 void            CcdPhysicsEnvironment::setSolverDamping(float damping)
1007 {
1008         m_solverInfo.m_damping = damping;
1009 }
1010
1011
1012 void            CcdPhysicsEnvironment::setLinearAirDamping(float damping)
1013 {
1014         gLinearAirDamping = damping;
1015 }
1016
1017 void            CcdPhysicsEnvironment::setUseEpa(bool epa)
1018 {
1019         gUseEpa = epa;
1020 }
1021
1022 void            CcdPhysicsEnvironment::setSolverType(int solverType)
1023 {
1024
1025         switch (solverType)
1026         {
1027         case 1:
1028                 {
1029                         if (m_solverType != solverType)
1030                         {
1031
1032                                 m_solver = new SimpleConstraintSolver();
1033
1034                                 break;
1035                         }
1036                 }
1037
1038         case 0:
1039         default:
1040                 if (m_solverType != solverType)
1041                 {
1042                         m_solver = new OdeConstraintSolver();
1043
1044                         break;
1045                 }
1046
1047         };
1048
1049         m_solverType = solverType ;
1050 }
1051
1052
1053
1054
1055
1056 void    CcdPhysicsEnvironment::SyncMotionStates(float timeStep)
1057 {
1058         std::vector<CcdPhysicsController*>::iterator i;
1059
1060         //
1061         // synchronize the physics and graphics transformations
1062         //
1063
1064         for (i=m_controllers.begin();
1065                 !(i==m_controllers.end()); i++)
1066         {
1067                 CcdPhysicsController* ctrl = (*i);
1068                 ctrl->SynchronizeMotionStates(timeStep);
1069
1070         }
1071
1072 }
1073 void            CcdPhysicsEnvironment::setGravity(float x,float y,float z)
1074 {
1075         m_gravity = SimdVector3(x,y,z);
1076
1077         std::vector<CcdPhysicsController*>::iterator i;
1078
1079         //todo: review this gravity stuff
1080         for (i=m_controllers.begin();
1081                 !(i==m_controllers.end()); i++)
1082         {
1083
1084                 CcdPhysicsController* ctrl = (*i);
1085                 ctrl->GetRigidBody()->setGravity(m_gravity);
1086
1087         }
1088 }
1089
1090
1091 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1092
1093 class DefaultVehicleRaycaster : public VehicleRaycaster
1094 {
1095         CcdPhysicsEnvironment* m_physEnv;
1096         PHY_IPhysicsController* m_chassis;
1097
1098 public:
1099         DefaultVehicleRaycaster(CcdPhysicsEnvironment* physEnv,PHY_IPhysicsController* chassis):
1100           m_physEnv(physEnv),
1101                   m_chassis(chassis)
1102           {
1103           }
1104
1105           /*    struct VehicleRaycasterResult
1106           {
1107           VehicleRaycasterResult() :m_distFraction(-1.f){};
1108           SimdVector3   m_hitPointInWorld;
1109           SimdVector3   m_hitNormalInWorld;
1110           SimdScalar    m_distFraction;
1111           };
1112           */
1113           virtual void* CastRay(const SimdVector3& from,const SimdVector3& to, VehicleRaycasterResult& result)
1114           {
1115
1116
1117                   float hit[3];
1118                   float normal[3];
1119
1120                   PHY_IPhysicsController*       ignore = m_chassis;
1121                   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]);
1122                   if (hitObject)
1123                   {
1124                           result.m_hitPointInWorld[0] = hit[0];
1125                           result.m_hitPointInWorld[1] = hit[1];
1126                           result.m_hitPointInWorld[2] = hit[2];
1127                           result.m_hitNormalInWorld[0] = normal[0];
1128                           result.m_hitNormalInWorld[1] = normal[1];
1129                           result.m_hitNormalInWorld[2] = normal[2];
1130                           result.m_hitNormalInWorld.normalize();
1131                           //calc fraction? or put it in the interface?
1132                           //calc for now
1133
1134                           result.m_distFraction = (result.m_hitPointInWorld-from).length() / (to-from).length();
1135                           //some safety for 'explosion' due to sudden penetration of the full 'ray'
1136                           /*                    if (result.m_distFraction<0.1)
1137                           {
1138                           printf("Vehicle Raycast: avoided instability due to penetration. Consider moving the connection points deeper inside vehicle chassis");
1139                           result.m_distFraction = 1.f;
1140                           hitObject = 0;
1141                           }
1142                           */
1143
1144                           /*                    if (result.m_distFraction>1.)
1145                           {
1146                           printf("Vehicle Raycast: avoided instability 1Consider moving the connection points deeper inside vehicle chassis");                          
1147                           result.m_distFraction = 1.f;
1148                           hitObject = 0;
1149                           }
1150                           */
1151
1152
1153
1154                   }
1155                   //?
1156                   return hitObject;
1157           }
1158 };
1159 #endif //NEW_BULLET_VEHICLE_SUPPORT
1160
1161 static int gConstraintUid = 1;
1162
1163 int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
1164                                                                                                         float pivotX,float pivotY,float pivotZ,
1165                                                                                                         float axisX,float axisY,float axisZ)
1166 {
1167
1168
1169         CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
1170         CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
1171
1172         RigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
1173         RigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
1174
1175         ASSERT(rb0);
1176
1177         SimdVector3 pivotInA(pivotX,pivotY,pivotZ);
1178         SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
1179         SimdVector3 axisInA(axisX,axisY,axisZ);
1180         SimdVector3 axisInB = rb1 ? 
1181                 (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * -axisInA)) : 
1182         rb0->getCenterOfMassTransform().getBasis() * -axisInA;
1183
1184         bool angularOnly = false;
1185
1186         switch (type)
1187         {
1188         case PHY_POINT2POINT_CONSTRAINT:
1189                 {
1190
1191                         Point2PointConstraint* p2p = 0;
1192
1193                         if (rb1)
1194                         {
1195                                 p2p = new Point2PointConstraint(*rb0,
1196                                         *rb1,pivotInA,pivotInB);
1197                         } else
1198                         {
1199                                 p2p = new Point2PointConstraint(*rb0,
1200                                         pivotInA);
1201                         }
1202
1203                         m_constraints.push_back(p2p);
1204                         p2p->SetUserConstraintId(gConstraintUid++);
1205                         p2p->SetUserConstraintType(type);
1206                         //64 bit systems can't cast pointer to int. could use size_t instead.
1207                         return p2p->GetUserConstraintId();
1208
1209                         break;
1210                 }
1211
1212         case PHY_ANGULAR_CONSTRAINT:
1213                 angularOnly = true;
1214
1215         case PHY_LINEHINGE_CONSTRAINT:
1216                 {
1217                         HingeConstraint* hinge = 0;
1218
1219                         if (rb1)
1220                         {
1221                                 hinge = new HingeConstraint(
1222                                         *rb0,
1223                                         *rb1,pivotInA,pivotInB,axisInA,axisInB);
1224
1225
1226                         } else
1227                         {
1228                                 hinge = new HingeConstraint(*rb0,
1229                                         pivotInA,axisInA);
1230
1231                         }
1232                         hinge->setAngularOnly(angularOnly);
1233
1234                         m_constraints.push_back(hinge);
1235                         hinge->SetUserConstraintId(gConstraintUid++);
1236                         hinge->SetUserConstraintType(type);
1237                         //64 bit systems can't cast pointer to int. could use size_t instead.
1238                         return hinge->GetUserConstraintId();
1239                         break;
1240                 }
1241 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1242
1243         case PHY_VEHICLE_CONSTRAINT:
1244                 {
1245                         RaycastVehicle::VehicleTuning* tuning = new RaycastVehicle::VehicleTuning();
1246                         RigidBody* chassis = rb0;
1247                         DefaultVehicleRaycaster* raycaster = new DefaultVehicleRaycaster(this,ctrl0);
1248                         RaycastVehicle* vehicle = new RaycastVehicle(*tuning,chassis,raycaster);
1249                         WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
1250                         m_wrapperVehicles.push_back(wrapperVehicle);
1251                         vehicle->SetUserConstraintId(gConstraintUid++);
1252                         vehicle->SetUserConstraintType(type);
1253                         return vehicle->GetUserConstraintId();
1254
1255                         break;
1256                 };
1257 #endif //NEW_BULLET_VEHICLE_SUPPORT
1258
1259         default:
1260                 {
1261                 }
1262         };
1263
1264         //RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB
1265
1266         return 0;
1267
1268 }
1269
1270 void            CcdPhysicsEnvironment::removeConstraint(int     constraintId)
1271 {
1272         std::vector<TypedConstraint*>::iterator i;
1273
1274         for (i=m_constraints.begin();
1275                 !(i==m_constraints.end()); i++)
1276         {
1277                 TypedConstraint* constraint = (*i);
1278                 if (constraint->GetUserConstraintId() == constraintId)
1279                 {
1280                         //activate objects
1281                         if (constraint->GetRigidBodyA().mergesSimulationIslands())
1282                                 constraint->GetRigidBodyA().activate();
1283                         if (constraint->GetRigidBodyB().mergesSimulationIslands())
1284                                 constraint->GetRigidBodyB().activate();
1285
1286                         std::swap(*i, m_constraints.back());
1287
1288                         
1289                         m_constraints.pop_back();
1290                         break;
1291                 }
1292         }
1293
1294 }
1295
1296
1297         struct  FilterClosestRayResultCallback : public CollisionWorld::ClosestRayResultCallback
1298         {
1299                 PHY_IPhysicsController* m_ignoreClient;
1300
1301                 FilterClosestRayResultCallback (PHY_IPhysicsController* ignoreClient,const SimdVector3& rayFrom,const SimdVector3& rayTo)
1302                         : CollisionWorld::ClosestRayResultCallback(rayFrom,rayTo),
1303                         m_ignoreClient(ignoreClient)
1304                 {
1305
1306                 }
1307
1308                 virtual ~FilterClosestRayResultCallback()
1309                 {
1310                 }
1311
1312                 virtual float   AddSingleResult(const CollisionWorld::LocalRayResult& rayResult)
1313                 {
1314                         CcdPhysicsController* curHit = static_cast<CcdPhysicsController*>(rayResult.m_collisionObject->m_userPointer);
1315                         //ignore client...
1316                         if (curHit != m_ignoreClient)
1317                         {               
1318                                 //if valid
1319                                 return ClosestRayResultCallback::AddSingleResult(rayResult);
1320                         }
1321                         return m_closestHitFraction;
1322                 }
1323
1324         };
1325
1326 PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ, 
1327                                                                                                            float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
1328 {
1329
1330
1331         float minFraction = 1.f;
1332
1333         SimdVector3 rayFrom(fromX,fromY,fromZ);
1334         SimdVector3 rayTo(toX,toY,toZ);
1335
1336         SimdVector3     hitPointWorld,normalWorld;
1337
1338         //Either Ray Cast with or without filtering
1339
1340         //CollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo);
1341         FilterClosestRayResultCallback   rayCallback(ignoreClient,rayFrom,rayTo);
1342
1343
1344         PHY_IPhysicsController* nearestHit = 0;
1345
1346         m_collisionWorld->RayTest(rayFrom,rayTo,rayCallback);
1347         if (rayCallback.HasHit())
1348         {
1349                 nearestHit = static_cast<CcdPhysicsController*>(rayCallback.m_collisionObject->m_userPointer);
1350                 hitX =  rayCallback.m_hitPointWorld.getX();
1351                 hitY =  rayCallback.m_hitPointWorld.getY();
1352                 hitZ =  rayCallback.m_hitPointWorld.getZ();
1353
1354                 normalX = rayCallback.m_hitNormalWorld.getX();
1355                 normalY = rayCallback.m_hitNormalWorld.getY();
1356                 normalZ = rayCallback.m_hitNormalWorld.getZ();
1357
1358         }       
1359
1360
1361         return nearestHit;
1362 }
1363
1364
1365
1366 int     CcdPhysicsEnvironment::getNumContactPoints()
1367 {
1368         return 0;
1369 }
1370
1371 void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
1372 {
1373
1374 }
1375
1376
1377
1378
1379 BroadphaseInterface*    CcdPhysicsEnvironment::GetBroadphase()
1380
1381         return m_collisionWorld->GetBroadphase(); 
1382 }
1383
1384
1385
1386 const CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher() const
1387 {
1388         return m_collisionWorld->GetDispatcher();
1389 }
1390
1391 CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher()
1392 {
1393         return m_collisionWorld->GetDispatcher();
1394 }
1395
1396 CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
1397 {
1398
1399 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1400         m_wrapperVehicles.clear();
1401 #endif //NEW_BULLET_VEHICLE_SUPPORT
1402
1403         //m_broadphase->DestroyScene();
1404         //delete broadphase ? release reference on broadphase ?
1405
1406         //first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher
1407         //delete m_dispatcher;
1408         delete m_collisionWorld;
1409
1410 }
1411
1412
1413 int     CcdPhysicsEnvironment::GetNumControllers()
1414 {
1415         return m_controllers.size();
1416 }
1417
1418
1419 CcdPhysicsController* CcdPhysicsEnvironment::GetPhysicsController( int index)
1420 {
1421         return m_controllers[index];
1422 }
1423
1424
1425 int     CcdPhysicsEnvironment::GetNumManifolds() const
1426 {
1427         return GetDispatcher()->GetNumManifolds();
1428 }
1429
1430 const PersistentManifold*       CcdPhysicsEnvironment::GetManifold(int index) const
1431 {
1432         return GetDispatcher()->GetManifoldByIndexInternal(index);
1433 }
1434
1435 TypedConstraint*        CcdPhysicsEnvironment::getConstraintById(int constraintId)
1436 {
1437         int numConstraint = m_constraints.size();
1438         int i;
1439         for (i=0;i<numConstraint;i++)
1440         {
1441                 TypedConstraint* constraint = m_constraints[i];
1442                 if (constraint->GetUserConstraintId()==constraintId)
1443                 {
1444                         return constraint;
1445                 }
1446         }
1447         return 0;
1448 }
1449
1450
1451 void CcdPhysicsEnvironment::addSensor(PHY_IPhysicsController* ctrl)
1452 {
1453
1454         CcdPhysicsController* ctrl1 = (CcdPhysicsController* )ctrl;
1455         std::vector<CcdPhysicsController*>::iterator i =
1456                 std::find(m_controllers.begin(), m_controllers.end(), ctrl);
1457         if ((i == m_controllers.end()))
1458         {
1459                 addCcdPhysicsController(ctrl1);
1460         }
1461
1462         requestCollisionCallback(ctrl);
1463         //printf("addSensor\n");
1464 }
1465
1466 void CcdPhysicsEnvironment::removeCollisionCallback(PHY_IPhysicsController* ctrl)
1467 {
1468         std::vector<CcdPhysicsController*>::iterator i =
1469                 std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl);
1470         if (!(i == m_triggerControllers.end()))
1471         {
1472                 std::swap(*i, m_triggerControllers.back());
1473                 m_triggerControllers.pop_back();
1474         }
1475 }
1476
1477
1478 void CcdPhysicsEnvironment::removeSensor(PHY_IPhysicsController* ctrl)
1479 {
1480         removeCollisionCallback(ctrl);
1481         //printf("removeSensor\n");
1482 }
1483 void CcdPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user)
1484 {
1485         /*      printf("addTouchCallback\n(response class = %i)\n",response_class);
1486
1487         //map PHY_ convention into SM_ convention
1488         switch (response_class)
1489         {
1490         case    PHY_FH_RESPONSE:
1491         printf("PHY_FH_RESPONSE\n");
1492         break;
1493         case PHY_SENSOR_RESPONSE:
1494         printf("PHY_SENSOR_RESPONSE\n");
1495         break;
1496         case PHY_CAMERA_RESPONSE:
1497         printf("PHY_CAMERA_RESPONSE\n");
1498         break;
1499         case PHY_OBJECT_RESPONSE:
1500         printf("PHY_OBJECT_RESPONSE\n");
1501         break;
1502         case PHY_STATIC_RESPONSE:
1503         printf("PHY_STATIC_RESPONSE\n");
1504         break;
1505         default:
1506         assert(0);
1507         return;
1508         }
1509         */
1510
1511         m_triggerCallbacks[response_class] = callback;
1512         m_triggerCallbacksUserPtrs[response_class] = user;
1513
1514 }
1515 void CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctrl)
1516 {
1517         CcdPhysicsController* ccdCtrl = static_cast<CcdPhysicsController*>(ctrl);
1518
1519         //printf("requestCollisionCallback\n");
1520         m_triggerControllers.push_back(ccdCtrl);
1521 }
1522
1523
1524 void    CcdPhysicsEnvironment::CallbackTriggers()
1525 {
1526         CcdPhysicsController* ctrl0=0,*ctrl1=0;
1527
1528         if (m_triggerCallbacks[PHY_OBJECT_RESPONSE])
1529         {
1530                 //walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback
1531                 int numManifolds = m_collisionWorld->GetDispatcher()->GetNumManifolds();
1532                 for (int i=0;i<numManifolds;i++)
1533                 {
1534                         PersistentManifold* manifold = m_collisionWorld->GetDispatcher()->GetManifoldByIndexInternal(i);
1535                         int numContacts = manifold->GetNumContacts();
1536                         if (numContacts)
1537                         {
1538                                 RigidBody* obj0 = static_cast<RigidBody* >(manifold->GetBody0());
1539                                 RigidBody* obj1 = static_cast<RigidBody* >(manifold->GetBody1());
1540
1541                                 //m_userPointer is set in 'addPhysicsController
1542                                 CcdPhysicsController* ctrl0 = static_cast<CcdPhysicsController*>(obj0->m_userPointer);
1543                                 CcdPhysicsController* ctrl1 = static_cast<CcdPhysicsController*>(obj1->m_userPointer);
1544
1545                                 std::vector<CcdPhysicsController*>::iterator i =
1546                                         std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl0);
1547                                 if (i == m_triggerControllers.end())
1548                                 {
1549                                         i = std::find(m_triggerControllers.begin(), m_triggerControllers.end(), ctrl1);
1550                                 }
1551
1552                                 if (!(i == m_triggerControllers.end()))
1553                                 {
1554                                         m_triggerCallbacks[PHY_OBJECT_RESPONSE](m_triggerCallbacksUserPtrs[PHY_OBJECT_RESPONSE],
1555                                                 ctrl0,ctrl1,0);
1556                                 }
1557                         }
1558                 }
1559
1560
1561
1562         }
1563
1564 }
1565
1566
1567
1568
1569
1570
1571 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1572
1573 //complex constraint for vehicles
1574 PHY_IVehicle*   CcdPhysicsEnvironment::getVehicleConstraint(int constraintId)
1575 {
1576         int i;
1577
1578         int numVehicles = m_wrapperVehicles.size();
1579         for (i=0;i<numVehicles;i++)
1580         {
1581                 WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
1582                 if (wrapperVehicle->GetVehicle()->GetUserConstraintId() == constraintId)
1583                         return wrapperVehicle;
1584         }
1585
1586         return 0;
1587 }
1588
1589 #endif //NEW_BULLET_VEHICLE_SUPPORT
1590
1591
1592
1593 void    CcdPhysicsEnvironment::UpdateAabbs(float        timeStep)
1594 {
1595         std::vector<CcdPhysicsController*>::iterator i;
1596         BroadphaseInterface* scene =  GetBroadphase();
1597
1598         //
1599                         // update aabbs, only for moving objects (!)
1600                         //
1601                         for (i=m_controllers.begin();
1602                                 !(i==m_controllers.end()); i++)
1603                         {
1604                                 CcdPhysicsController* ctrl = (*i);
1605                                 RigidBody* body = ctrl->GetRigidBody();
1606
1607
1608                                 SimdPoint3 minAabb,maxAabb;
1609                                 CollisionShape* shapeinterface = ctrl->GetCollisionShape();
1610
1611
1612
1613                                 shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(),
1614                                         body->getLinearVelocity(),
1615                                         //body->getAngularVelocity(),
1616                                         SimdVector3(0.f,0.f,0.f),//no angular effect for now //body->getAngularVelocity(),
1617                                         timeStep,minAabb,maxAabb);
1618
1619
1620                                 SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold);
1621                                 minAabb -= manifoldExtraExtents;
1622                                 maxAabb += manifoldExtraExtents;
1623
1624                                 BroadphaseProxy* bp = body->m_broadphaseHandle;
1625                                 if (bp)
1626                                 {
1627
1628                                         SimdVector3 color (1,1,0);
1629
1630                                         if (m_debugDrawer)
1631                                         {       
1632                                                 //draw aabb
1633                                                 switch (body->GetActivationState())
1634                                                 {
1635                                                 case ISLAND_SLEEPING:
1636                                                         {
1637                                                                 color.setValue(1,1,1);
1638                                                                 break;
1639                                                         }
1640                                                 case WANTS_DEACTIVATION:
1641                                                         {
1642                                                                 color.setValue(0,0,1);
1643                                                                 break;
1644                                                         }
1645                                                 case ACTIVE_TAG:
1646                                                         {
1647                                                                 break;
1648                                                         }
1649                                                 case DISABLE_DEACTIVATION:
1650                                                         {
1651                                                                 color.setValue(1,0,1);
1652                                                         };
1653
1654                                                 };
1655
1656                                                 if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)
1657                                                 {
1658                                                         DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
1659                                                 }
1660                                         }
1661
1662                                         scene->SetAabb(bp,minAabb,maxAabb);
1663
1664
1665
1666                                 }
1667                         }
1668 }
1669
1670 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateSphereController(float radius,const PHY__Vector3& position)
1671 {
1672         
1673         CcdConstructionInfo     cinfo;
1674         cinfo.m_collisionShape = new SphereShape(radius);
1675         cinfo.m_MotionState = 0;
1676         cinfo.m_physicsEnv = this;
1677         cinfo.m_collisionFlags |= CollisionObject::noContactResponse;
1678         DefaultMotionState* motionState = new DefaultMotionState();
1679         cinfo.m_MotionState = motionState;
1680         motionState->m_worldTransform.setIdentity();
1681         motionState->m_worldTransform.setOrigin(SimdVector3(position[0],position[1],position[2]));
1682
1683         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
1684         
1685
1686         return sphereController;
1687 }
1688
1689
1690 PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float coneradius,float coneheight)
1691 {
1692         CcdConstructionInfo     cinfo;
1693         cinfo.m_collisionShape = new ConeShape(coneradius,coneheight);
1694         cinfo.m_MotionState = 0;
1695         cinfo.m_physicsEnv = this;
1696         DefaultMotionState* motionState = new DefaultMotionState();
1697         cinfo.m_MotionState = motionState;
1698         motionState->m_worldTransform.setIdentity();
1699 //      motionState->m_worldTransform.setOrigin(SimdVector3(position[0],position[1],position[2]));
1700
1701         CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo);
1702
1703
1704         return sphereController;
1705 }
1706