aac699468d0262b3da3838699a08ca4920aa14fe
[blender-staging.git] / source / gameengine / Physics / Bullet / CcdPhysicsEnvironment.cpp
1 #include "CcdPhysicsEnvironment.h"
2 #include "CcdPhysicsController.h"
3
4 #include <algorithm>
5 #include "SimdTransform.h"
6 #include "Dynamics/RigidBody.h"
7 #include "BroadphaseCollision/BroadphaseInterface.h"
8 #include "BroadphaseCollision/SimpleBroadphase.h"
9
10 #include "CollisionShapes/ConvexShape.h"
11 #include "BroadphaseCollision/CollisionDispatcher.h"
12 #include "NarrowPhaseCollision/PersistentManifold.h"
13 #include "CollisionShapes/TriangleMeshShape.h"
14 #include "ConstraintSolver/OdeConstraintSolver.h"
15 #include "ConstraintSolver/SimpleConstraintSolver.h"
16
17 #include "IDebugDraw.h"
18
19
20 #include "CollisionDispatch/ToiContactDispatcher.h"
21
22
23 #include "CollisionDispatch/EmptyCollisionAlgorithm.h"
24 #include "CollisionDispatch/UnionFind.h"
25
26 #include "NarrowPhaseCollision/RaycastCallback.h"
27
28 bool useIslands = true;
29
30 #include "ConstraintSolver/ConstraintSolver.h"
31 #include "ConstraintSolver/Point2PointConstraint.h"
32 //#include "BroadphaseCollision/QueryDispatcher.h"
33 //#include "BroadphaseCollision/QueryBox.h"
34 //todo: change this to allow dynamic registration of types!
35
36 #ifdef WIN32
37 void DrawRasterizerLine(const float* from,const float* to,int color);
38 #endif
39
40
41 #include "ConstraintSolver/ContactConstraint.h"
42
43
44
45 #include <stdio.h>
46
47
48
49
50 static void DrawAabb(IDebugDraw* debugDrawer,const SimdVector3& from,const SimdVector3& to,const SimdVector3& color)
51 {
52         SimdVector3 halfExtents = (to-from)* 0.5f;
53         SimdVector3 center = (to+from) *0.5f;
54         int i,j;
55
56         SimdVector3 edgecoord(1.f,1.f,1.f),pa,pb;
57         for (i=0;i<4;i++)
58         {
59                 for (j=0;j<3;j++)
60                 {
61                         pa = SimdVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],              
62                                 edgecoord[2]*halfExtents[2]);
63                         pa+=center;
64                         
65                         int othercoord = j%3;
66                         edgecoord[othercoord]*=-1.f;
67                         pb = SimdVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],      
68                                 edgecoord[2]*halfExtents[2]);
69                         pb+=center;
70                         
71                         debugDrawer->DrawLine(pa,pb,color);
72                 }
73                 edgecoord = SimdVector3(-1.f,-1.f,-1.f);
74                 if (i<3)
75                         edgecoord[i]*=-1.f;
76         }
77
78
79 }
80
81
82
83
84
85
86 CcdPhysicsEnvironment::CcdPhysicsEnvironment(ToiContactDispatcher* dispatcher,BroadphaseInterface* bp)
87 :m_dispatcher(dispatcher),
88 m_broadphase(bp),
89 m_scalingPropagated(false),
90 m_numIterations(30),
91 m_ccdMode(0),
92 m_solverType(-1)
93 {
94
95         if (!m_dispatcher)
96         {
97                 setSolverType(0);
98         }
99
100         if (!m_broadphase)
101         {
102                 m_broadphase = new SimpleBroadphase();
103         }
104         
105         m_debugDrawer = 0;
106         m_gravity = SimdVector3(0.f,-10.f,0.f);
107         
108
109 }
110
111 void    CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
112 {
113         ctrl->GetRigidBody()->setGravity( m_gravity );
114         m_controllers.push_back(ctrl);
115         
116         BroadphaseInterface* scene =  m_broadphase;
117         
118         CollisionShape* shapeinterface = ctrl->GetCollisionShape();
119         
120         assert(shapeinterface);
121         
122         const SimdTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform();
123         
124         RigidBody* body = ctrl->GetRigidBody();
125         
126         SimdPoint3 minAabb,maxAabb;
127         
128         shapeinterface->GetAabb(t,minAabb,maxAabb);
129         
130         float timeStep = 0.02f;
131         
132         
133         //extent it with the motion
134         
135         SimdVector3 linMotion = body->getLinearVelocity()*timeStep;
136         
137         float maxAabbx = maxAabb.getX();
138         float maxAabby = maxAabb.getY();
139         float maxAabbz = maxAabb.getZ();
140         float minAabbx = minAabb.getX();
141         float minAabby = minAabb.getY();
142         float minAabbz = minAabb.getZ();
143
144         if (linMotion.x() > 0.f)
145                 maxAabbx += linMotion.x(); 
146         else
147                 minAabbx += linMotion.x();
148         if (linMotion.y() > 0.f)
149                 maxAabby += linMotion.y(); 
150         else
151                 minAabby += linMotion.y();
152         if (linMotion.z() > 0.f)
153                 maxAabbz += linMotion.z(); 
154         else
155                 minAabbz += linMotion.z();
156         
157
158         minAabb = SimdVector3(minAabbx,minAabby,minAabbz);
159         maxAabb = SimdVector3(maxAabbx,maxAabby,maxAabbz);
160         
161         if (!ctrl->m_broadphaseHandle)
162         {
163                 int type = shapeinterface->GetShapeType();
164                 ctrl->m_broadphaseHandle = scene->CreateProxy(
165                         ctrl->GetRigidBody(),
166                         type,
167                         minAabb, 
168                         maxAabb);
169         }
170         
171         body->SetCollisionShape( shapeinterface );
172         
173         
174         
175 }
176
177 void    CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctrl)
178 {
179         
180         //also remove constraint
181         
182         {
183                 std::vector<Point2PointConstraint*>::iterator i;
184                 
185                 for (i=m_p2pConstraints.begin();
186                 !(i==m_p2pConstraints.end()); i++)
187                 {
188                         Point2PointConstraint* p2p = (*i);
189                         if  ((&p2p->GetRigidBodyA() == ctrl->GetRigidBody() ||
190                                 (&p2p->GetRigidBodyB() == ctrl->GetRigidBody())))
191                         {
192                                 removeConstraint(int(p2p));
193                                 //only 1 constraint per constroller
194                                 break;
195                         }
196                 }
197         }
198         
199         {
200                 std::vector<Point2PointConstraint*>::iterator i;
201                 
202                 for (i=m_p2pConstraints.begin();
203                 !(i==m_p2pConstraints.end()); i++)
204                 {
205                         Point2PointConstraint* p2p = (*i);
206                         if  ((&p2p->GetRigidBodyA() == ctrl->GetRigidBody() ||
207                                 (&p2p->GetRigidBodyB() == ctrl->GetRigidBody())))
208                         {
209                                 removeConstraint(int(p2p));
210                                 //only 1 constraint per constroller
211                                 break;
212                         }
213                 }
214         }
215         
216         
217         
218         bool removeFromBroadphase = false;
219         
220         {
221                 BroadphaseInterface* scene = m_broadphase;
222                 BroadphaseProxy* bp = (BroadphaseProxy*)ctrl->m_broadphaseHandle;
223                 
224                 if (removeFromBroadphase)
225                 {
226                 }
227                 //
228                 // only clear the cached algorithms
229                 //
230                 scene->CleanProxyFromPairs(bp);
231         }
232         {
233                 std::vector<CcdPhysicsController*>::iterator i =
234                         std::find(m_controllers.begin(), m_controllers.end(), ctrl);
235                 if (!(i == m_controllers.end()))
236                 {
237                         std::swap(*i, m_controllers.back());
238                         m_controllers.pop_back();
239                 }
240         }
241 }
242
243 void    CcdPhysicsEnvironment::UpdateActivationState()
244 {
245         m_dispatcher->InitUnionFind();
246         
247         // put the index into m_controllers into m_tag  
248         {
249                 std::vector<CcdPhysicsController*>::iterator i;
250                 
251                 int index = 0;
252                 for (i=m_controllers.begin();
253                 !(i==m_controllers.end()); i++)
254                 {
255                         CcdPhysicsController* ctrl = (*i);
256                         RigidBody* body = ctrl->GetRigidBody();
257                         body->m_islandTag1 = index;
258                         body->m_hitFraction = 1.f;
259                         index++;
260                         
261                 }
262         }
263         // do the union find
264         
265         m_dispatcher->FindUnions();
266         
267         // put the islandId ('find' value) into m_tag   
268         {
269                 UnionFind& unionFind = m_dispatcher->GetUnionFind();
270                 
271                 std::vector<CcdPhysicsController*>::iterator i;
272                 
273                 int index = 0;
274                 for (i=m_controllers.begin();
275                 !(i==m_controllers.end()); i++)
276                 {
277                         CcdPhysicsController* ctrl = (*i);
278                         RigidBody* body = ctrl->GetRigidBody();
279                         
280                         
281                         if (body->mergesSimulationIslands())
282                         {
283                                 body->m_islandTag1 = unionFind.find(index);
284                         } else
285                         {
286                                 body->m_islandTag1 = -1;
287                         }
288                         index++;
289                 }
290         }
291         
292 }
293
294
295
296 /// Perform an integration step of duration 'timeStep'.
297 bool    CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
298 {
299         
300         
301 //      printf("CcdPhysicsEnvironment::proceedDeltaTime\n");
302         
303         if (timeStep == 0.f)
304                 return true;
305
306         //clamp hardcoded for now
307         if (timeStep > 0.02)
308                 timeStep = 0.02;
309         
310         //this is needed because scaling is not known in advance, and scaling has to propagate to the shape
311         if (!m_scalingPropagated)
312         {
313                 //SyncMotionStates(timeStep);
314                 //m_scalingPropagated = true;
315         }
316
317
318
319         {
320 //              std::vector<CcdPhysicsController*>::iterator i;
321                 
322                 
323                 
324                 int k;
325                 for (k=0;k<GetNumControllers();k++)
326                 {
327                         CcdPhysicsController* ctrl = m_controllers[k];
328                         //              SimdTransform predictedTrans;
329                         RigidBody* body = ctrl->GetRigidBody();
330                         if (body->GetActivationState() != ISLAND_SLEEPING)
331                         {
332                                 body->applyForces( timeStep);
333                                 body->integrateVelocities( timeStep);
334                         }
335                         
336                 }
337         }
338         BroadphaseInterface*    scene = m_broadphase;
339         
340         
341         //
342         // collision detection (?)
343         //
344         
345         
346         
347         
348         
349         int numsubstep = m_numIterations;
350         
351         
352         DispatcherInfo dispatchInfo;
353         dispatchInfo.m_timeStep = timeStep;
354         dispatchInfo.m_stepCount = 0;
355
356         scene->DispatchAllCollisionPairs(*m_dispatcher,dispatchInfo);///numsubstep,g);
357
358
359
360         
361                 
362         
363         int numRigidBodies = m_controllers.size();
364         
365         UpdateActivationState();
366
367         //contacts
368
369         
370         m_dispatcher->SolveConstraints(timeStep, m_numIterations ,numRigidBodies,m_debugDrawer);
371
372         for (int g=0;g<numsubstep;g++)
373         {
374                 //
375                 // constraint solving
376                 //
377                 
378                 
379                 int i;
380                 int numPoint2Point = m_p2pConstraints.size();
381                 
382                 //point to point constraints
383                 for (i=0;i< numPoint2Point ; i++ )
384                 {
385                         Point2PointConstraint* p2p = m_p2pConstraints[i];
386                         
387                         p2p->BuildJacobian();
388                         p2p->SolveConstraint( timeStep );
389                         
390                 }
391                 /*
392                 //vehicles
393                 int numVehicles = m_vehicles.size();
394                 for (i=0;i<numVehicles;i++)
395                 {
396                         Vehicle* vehicle = m_vehicles[i];
397                         vehicle->UpdateVehicle( timeStep );
398                 }
399                 */
400                 
401                 
402                 
403         }
404         
405         {
406                 
407                 
408                 
409                 {
410                         
411                         std::vector<CcdPhysicsController*>::iterator i;
412                         
413                         //
414                         // update aabbs, only for moving objects (!)
415                         //
416                         for (i=m_controllers.begin();
417                         !(i==m_controllers.end()); i++)
418                         {
419                                 CcdPhysicsController* ctrl = (*i);
420                                 RigidBody* body = ctrl->GetRigidBody();
421                                 
422                                 
423                                 SimdPoint3 minAabb,maxAabb;
424                                 CollisionShape* shapeinterface = ctrl->GetCollisionShape();
425
426
427
428                                 shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(),
429                                         body->getLinearVelocity(),body->getAngularVelocity(),
430                                         timeStep,minAabb,maxAabb);
431
432                                 shapeinterface->GetAabb(body->getCenterOfMassTransform(),
433                                         minAabb,maxAabb);
434
435                                 
436                                 BroadphaseProxy* bp = (BroadphaseProxy*) ctrl->m_broadphaseHandle;
437                                 if (bp)
438                                 {
439                                         
440 #ifdef WIN32
441                                         SimdVector3 color (1,0,0);
442                                         
443                                 
444
445                                 
446                                         if (m_debugDrawer)
447                                         {       
448                                                 //draw aabb
449                                                 switch (body->GetActivationState())
450                                                 {
451                                                 case ISLAND_SLEEPING:
452                                                         {
453                                                                 color.setValue(0,1,0);
454                                                                 break;
455                                                         }
456                                                 case WANTS_DEACTIVATION:
457                                                         {
458                                                                 color.setValue(0,0,1);
459                                                                 break;
460                                                         }
461                                                 case ACTIVE_TAG:
462                                                         {
463                                                                 break;
464                                                         }
465
466                                                         
467                                                 };
468
469                                                 DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
470                                         }
471 #endif
472
473                                         scene->SetAabb(bp,minAabb,maxAabb);
474
475
476                                         
477                                 }
478                         }
479                         
480                         float toi = 1.f;
481
482
483                         
484                         if (m_ccdMode == 3)
485                         {
486                                 DispatcherInfo dispatchInfo;
487                                 dispatchInfo.m_timeStep = timeStep;
488                                 dispatchInfo.m_stepCount = 0;
489                                 dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS;
490                                 
491                                 scene->DispatchAllCollisionPairs( *m_dispatcher,dispatchInfo);///numsubstep,g);
492                                 toi = dispatchInfo.m_timeOfImpact;
493                         }
494                         
495                         //
496                         // integrating solution
497                         //
498                         
499                         {
500                                 std::vector<CcdPhysicsController*>::iterator i;
501                                 
502                                 for (i=m_controllers.begin();
503                                 !(i==m_controllers.end()); i++)
504                                 {
505                                         
506                                         CcdPhysicsController* ctrl = *i;
507                                         
508                                         SimdTransform predictedTrans;
509                                         RigidBody* body = ctrl->GetRigidBody();
510                                         if (body->GetActivationState() != ISLAND_SLEEPING)
511                                         {
512                                                 body->predictIntegratedTransform(timeStep*      toi, predictedTrans);
513                                                 body->proceedToTransform( predictedTrans);
514
515                                         }
516                                 }
517                                 
518                         }
519                         
520                         
521                         
522                         
523                         
524                         //
525                         // disable sleeping physics objects
526                         //
527                         
528                         std::vector<CcdPhysicsController*> m_sleepingControllers;
529                         
530                         for (i=m_controllers.begin();
531                         !(i==m_controllers.end()); i++)
532                         {
533                                 CcdPhysicsController* ctrl = (*i);
534                                 RigidBody* body = ctrl->GetRigidBody();
535
536                                 ctrl->UpdateDeactivation(timeStep);
537
538                                 
539                                 if (ctrl->wantsSleeping())
540                                 {
541                                         if (body->GetActivationState() == ACTIVE_TAG)
542                                                 body->SetActivationState( WANTS_DEACTIVATION );
543                                 } else
544                                 {
545                                         body->SetActivationState( ACTIVE_TAG );
546                                 }
547
548                                 if (useIslands)
549                                 {
550                                         if (body->GetActivationState() == ISLAND_SLEEPING)
551                                         {
552                                                 m_sleepingControllers.push_back(ctrl);
553                                         }
554                                 } else
555                                 {
556                                         if (ctrl->wantsSleeping())
557                                         {
558                                                 m_sleepingControllers.push_back(ctrl);
559                                         }
560                                 }
561                         }
562                         
563         
564                         
565                         
566         }
567         
568         SyncMotionStates(timeStep);
569
570         }
571         return true;
572 }
573
574 void            CcdPhysicsEnvironment::setDebugMode(int debugMode)
575 {
576         if (m_debugDrawer){
577                 m_debugDrawer->SetDebugMode(debugMode);
578         }
579 }
580
581 void            CcdPhysicsEnvironment::setNumIterations(int numIter)
582 {
583         m_numIterations = numIter;
584 }
585 void            CcdPhysicsEnvironment::setDeactivationTime(float dTime)
586 {
587         gDeactivationTime = dTime;
588 }
589 void            CcdPhysicsEnvironment::setDeactivationLinearTreshold(float linTresh)
590 {
591         gLinearSleepingTreshold = linTresh;
592 }
593 void            CcdPhysicsEnvironment::setDeactivationAngularTreshold(float angTresh) 
594 {
595         gAngularSleepingTreshold = angTresh;
596 }
597 void            CcdPhysicsEnvironment::setContactBreakingTreshold(float contactBreakingTreshold)
598 {
599         gContactBreakingTreshold = contactBreakingTreshold;
600         
601 }
602
603
604 void            CcdPhysicsEnvironment::setCcdMode(int ccdMode)
605 {
606         m_ccdMode = ccdMode;
607 }
608
609
610 void            CcdPhysicsEnvironment::setSolverSorConstant(float sor)
611 {
612         m_dispatcher->SetSor(sor);
613 }
614
615 void            CcdPhysicsEnvironment::setSolverTau(float tau)
616 {
617         m_dispatcher->SetTau(tau);
618 }
619 void            CcdPhysicsEnvironment::setSolverDamping(float damping)
620 {
621         m_dispatcher->SetDamping(damping);
622 }
623
624
625 void            CcdPhysicsEnvironment::setLinearAirDamping(float damping)
626 {
627         gLinearAirDamping = damping;
628 }
629
630 void            CcdPhysicsEnvironment::setUseEpa(bool epa)
631 {
632         gUseEpa = epa;
633 }
634
635 void            CcdPhysicsEnvironment::setSolverType(int solverType)
636 {
637
638         switch (solverType)
639         {
640         case 1:
641                 {
642                         if (m_solverType != solverType)
643                         {
644                                 if (m_dispatcher)
645                                         delete m_dispatcher;
646
647                                 SimpleConstraintSolver* solver= new SimpleConstraintSolver();
648                                 m_dispatcher = new ToiContactDispatcher(solver);
649                                 break;
650                         }
651                 }
652         
653         case 0:
654         default:
655                         if (m_solverType != solverType)
656                         {
657                                 if (m_dispatcher)
658                                         delete m_dispatcher;
659
660                                 OdeConstraintSolver* solver= new OdeConstraintSolver();
661                                 m_dispatcher = new ToiContactDispatcher(solver);
662                                 break;
663                         }
664
665         };
666         
667         m_solverType = solverType ;
668 }
669
670
671
672
673
674 void    CcdPhysicsEnvironment::SyncMotionStates(float timeStep)
675 {
676         std::vector<CcdPhysicsController*>::iterator i;
677
678         //
679         // synchronize the physics and graphics transformations
680         //
681         for (i=m_controllers.begin();
682         !(i==m_controllers.end()); i++)
683         {
684                 CcdPhysicsController* ctrl = (*i);
685                 ctrl->SynchronizeMotionStates(timeStep);
686                 
687         }
688
689 }
690 void            CcdPhysicsEnvironment::setGravity(float x,float y,float z)
691 {
692         m_gravity = SimdVector3(x,y,z);
693
694         std::vector<CcdPhysicsController*>::iterator i;
695
696         //todo: review this gravity stuff
697         for (i=m_controllers.begin();
698         !(i==m_controllers.end()); i++)
699         {
700
701                 CcdPhysicsController* ctrl = (*i);
702                 ctrl->GetRigidBody()->setGravity(m_gravity);
703
704         }
705 }
706
707 #ifdef DASHDASJKHASDJK
708 class RaycastingQueryBox : public QueryBox
709 {
710         
711         SimdVector3 m_aabbMin;
712         
713         SimdVector3 m_aabbMax;
714         
715         
716         
717 public:
718         
719         RaycastCallback m_raycastCallback;
720         
721         
722         RaycastingQueryBox(QueryBoxConstructionInfo& ci,const SimdVector3& from,const SimdVector3& to)
723                 : QueryBox(ci),
724                 m_raycastCallback(from,to)
725         {
726                 for (int i=0;i<3;i++)
727                 {
728                         float fromI = from[i];
729                         float toI = to[i];
730                         if (fromI < toI)
731                         {
732                                 m_aabbMin[i] = fromI;
733                                 m_aabbMax[i] = toI;
734                         } else
735                         {
736                                 m_aabbMin[i] = toI;
737                                 m_aabbMax[i] = fromI;
738                         }
739                 }
740                 
741         }
742         virtual void AddCollider( BroadphaseProxy* proxy)
743         {
744                 //perform raycast if wanted, and update the m_hitFraction
745                 
746                 if (proxy->GetClientObjectType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
747                 {
748                         //do it
749                         RigidBody* body = (RigidBody*)proxy->m_clientObject;
750                         TriangleMeshInterface* meshInterface = (TriangleMeshInterface*)
751                                 body->m_minkowski1;
752                         
753                         //if the hit is closer, record the proxy!
754                         float curFraction = m_raycastCallback.m_hitFraction;
755                         
756                         meshInterface->ProcessAllTriangles(&m_raycastCallback,m_aabbMin,m_aabbMax);
757                         
758                         if (m_raycastCallback.m_hitFraction < curFraction)
759                         {
760                                 m_raycastCallback.m_hitProxy = proxy;
761                         }
762                         
763                 }
764                 
765         }
766 };
767
768 struct InternalVehicleRaycaster : public VehicleRaycaster
769 {
770         
771         CcdPhysicsEnvironment* m_env;
772         
773 public:
774         
775         InternalVehicleRaycaster(CcdPhysicsEnvironment* env)
776                 :       m_env(env)
777         {
778                 
779         }
780         
781         virtual void* CastRay(const SimdVector3& from,const SimdVector3& to, VehicleRaycasterResult& result)
782         {
783
784                 return 0;
785         }
786         
787 };
788
789 #endif 
790 int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
791                                                                                                                 float pivotX,float pivotY,float pivotZ,
792                                                                                                                 float axisX,float axisY,float axisZ)
793 {
794         
795         
796         CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
797         CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
798         
799         RigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
800         RigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
801         
802         ASSERT(rb0);
803         
804         SimdVector3 pivotInA(pivotX,pivotY,pivotZ);
805         SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
806         
807         switch (type)
808         {
809         case PHY_POINT2POINT_CONSTRAINT:
810                 {
811                         
812                         Point2PointConstraint* p2p = 0;
813                         
814                         if (rb1)
815                         {
816                                 p2p = new Point2PointConstraint(*rb0,
817                                         *rb1,pivotInA,pivotInB);
818                         } else
819                         {
820                                 p2p = new Point2PointConstraint(*rb0,
821                                         pivotInA);
822                         }
823                         
824                         m_p2pConstraints.push_back(p2p);
825                         return 0;
826                         
827                         break;
828                 }
829         default:
830                 {
831                 }
832         };
833         
834         //RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB
835         
836         return 0;
837         
838 }
839
840 void            CcdPhysicsEnvironment::removeConstraint(int constraintid)
841 {
842         
843         Point2PointConstraint* p2p = (Point2PointConstraint*) constraintid;
844         
845         std::vector<Point2PointConstraint*>::iterator i =
846                 std::find(m_p2pConstraints.begin(), m_p2pConstraints.end(), p2p);
847         
848         if (!(i == m_p2pConstraints.end()) )
849         {
850                 std::swap(*i, m_p2pConstraints.back());
851                 m_p2pConstraints.pop_back();
852         }
853         
854 }
855 PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ, 
856                                                                 float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
857 {
858
859
860 //      m_broadphase->cast(
861         return 0;
862 }
863
864
865
866 int     CcdPhysicsEnvironment::getNumContactPoints()
867 {
868         return 0;
869 }
870
871 void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
872 {
873         
874 }
875
876
877
878
879
880 Dispatcher* CcdPhysicsEnvironment::GetDispatcher()
881 {
882         return m_dispatcher;
883 }
884
885 CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
886 {
887         
888         
889         m_vehicles.clear();
890         
891         //m_broadphase->DestroyScene();
892         //delete broadphase ? release reference on broadphase ?
893         
894         //first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher
895         delete m_dispatcher;
896         
897 }
898
899
900 int     CcdPhysicsEnvironment::GetNumControllers()
901 {
902         return m_controllers.size();
903 }
904
905
906 CcdPhysicsController* CcdPhysicsEnvironment::GetPhysicsController( int index)
907 {
908         return m_controllers[index];
909 }
910
911
912 int     CcdPhysicsEnvironment::GetNumManifolds() const
913 {
914         return m_dispatcher->GetNumManifolds();
915 }
916
917 const PersistentManifold*       CcdPhysicsEnvironment::GetManifold(int index) const
918 {
919         return m_dispatcher->GetManifoldByIndexInternal(index);
920 }