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