more graphics patches from Snailrose,
[blender.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 #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(p2p->GetConstraintId());
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(p2p->GetConstraintId());
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                 //
232                 // only clear the cached algorithms
233                 //
234                 scene->CleanProxyFromPairs(bp);
235                 scene->DestroyProxy(bp);//??
236         }
237         {
238                 std::vector<CcdPhysicsController*>::iterator i =
239                         std::find(m_controllers.begin(), m_controllers.end(), ctrl);
240                 if (!(i == m_controllers.end()))
241                 {
242                         std::swap(*i, m_controllers.back());
243                         m_controllers.pop_back();
244                 }
245         }
246 }
247
248 void    CcdPhysicsEnvironment::UpdateActivationState()
249 {
250         m_dispatcher->InitUnionFind();
251         
252         // put the index into m_controllers into m_tag  
253         {
254                 std::vector<CcdPhysicsController*>::iterator i;
255                 
256                 int index = 0;
257                 for (i=m_controllers.begin();
258                 !(i==m_controllers.end()); i++)
259                 {
260                         CcdPhysicsController* ctrl = (*i);
261                         RigidBody* body = ctrl->GetRigidBody();
262                         body->m_islandTag1 = index;
263                         body->m_hitFraction = 1.f;
264                         index++;
265                         
266                 }
267         }
268         // do the union find
269         
270         m_dispatcher->FindUnions();
271         
272         // put the islandId ('find' value) into m_tag   
273         {
274                 UnionFind& unionFind = m_dispatcher->GetUnionFind();
275                 
276                 std::vector<CcdPhysicsController*>::iterator i;
277                 
278                 int index = 0;
279                 for (i=m_controllers.begin();
280                 !(i==m_controllers.end()); i++)
281                 {
282                         CcdPhysicsController* ctrl = (*i);
283                         RigidBody* body = ctrl->GetRigidBody();
284                         
285                         
286                         if (body->mergesSimulationIslands())
287                         {
288                                 body->m_islandTag1 = unionFind.find(index);
289                         } else
290                         {
291                                 body->m_islandTag1 = -1;
292                         }
293                         index++;
294                 }
295         }
296         
297 }
298
299 void    CcdPhysicsEnvironment::beginFrame()
300 {
301
302 }
303
304 bool    CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
305 {
306
307         if (!SimdFuzzyZero(timeStep))
308         {
309                 //Blender runs 30hertz, so subdivide so we get 60 hertz
310                 proceedDeltaTimeOneStep(0.5f*timeStep);
311                 proceedDeltaTimeOneStep(0.5f*timeStep);
312         } else
313         {
314                 //todo: interpolate
315         }
316         return true;
317 }
318 /// Perform an integration step of duration 'timeStep'.
319 bool    CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
320 {
321         
322         
323 //      printf("CcdPhysicsEnvironment::proceedDeltaTime\n");
324         
325         if (SimdFuzzyZero(timeStep))
326                 return true;
327
328         if (m_debugDrawer)
329         {
330                 gDisableDeactivation = (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_NoDeactivation);
331         }
332
333
334
335         
336         //this is needed because scaling is not known in advance, and scaling has to propagate to the shape
337         if (!m_scalingPropagated)
338         {
339                 SyncMotionStates(timeStep);
340                 m_scalingPropagated = true;
341         }
342
343
344
345         {
346 //              std::vector<CcdPhysicsController*>::iterator i;
347                 
348                 
349                 
350                 int k;
351                 for (k=0;k<GetNumControllers();k++)
352                 {
353                         CcdPhysicsController* ctrl = m_controllers[k];
354                         //              SimdTransform predictedTrans;
355                         RigidBody* body = ctrl->GetRigidBody();
356                         if (body->GetActivationState() != ISLAND_SLEEPING)
357                         {
358                                 body->applyForces( timeStep);
359                                 body->integrateVelocities( timeStep);
360                         }
361                         
362                 }
363         }
364         BroadphaseInterface*    scene = m_broadphase;
365         
366         
367         //
368         // collision detection (?)
369         //
370         
371         
372         
373         
374         
375         int numsubstep = m_numIterations;
376         
377         
378         DispatcherInfo dispatchInfo;
379         dispatchInfo.m_timeStep = timeStep;
380         dispatchInfo.m_stepCount = 0;
381
382         scene->DispatchAllCollisionPairs(*m_dispatcher,dispatchInfo);///numsubstep,g);
383
384
385
386         
387                 
388         
389         int numRigidBodies = m_controllers.size();
390         
391         UpdateActivationState();
392
393         //contacts
394
395         
396         m_dispatcher->SolveConstraints(timeStep, m_numIterations ,numRigidBodies,m_debugDrawer);
397
398         for (int g=0;g<numsubstep;g++)
399         {
400                 //
401                 // constraint solving
402                 //
403                 
404                 
405                 int i;
406                 int numPoint2Point = m_p2pConstraints.size();
407                 
408                 //point to point constraints
409                 for (i=0;i< numPoint2Point ; i++ )
410                 {
411                         Point2PointConstraint* p2p = m_p2pConstraints[i];
412                         
413                         p2p->BuildJacobian();
414                         p2p->SolveConstraint( timeStep );
415                         
416                 }
417                 /*
418                 //vehicles
419                 int numVehicles = m_vehicles.size();
420                 for (i=0;i<numVehicles;i++)
421                 {
422                         Vehicle* vehicle = m_vehicles[i];
423                         vehicle->UpdateVehicle( timeStep );
424                 }
425                 */
426                 
427                 
428                 
429         }
430         
431         {
432                 
433                 
434                 
435                 {
436                         
437                         std::vector<CcdPhysicsController*>::iterator i;
438                         
439                         //
440                         // update aabbs, only for moving objects (!)
441                         //
442                         for (i=m_controllers.begin();
443                         !(i==m_controllers.end()); i++)
444                         {
445                                 CcdPhysicsController* ctrl = (*i);
446                                 RigidBody* body = ctrl->GetRigidBody();
447                                 
448                                 
449                                 SimdPoint3 minAabb,maxAabb;
450                                 CollisionShape* shapeinterface = ctrl->GetCollisionShape();
451
452
453
454                                 shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(),
455                                         body->getLinearVelocity(),body->getAngularVelocity(),
456                                         timeStep,minAabb,maxAabb);
457
458                                 shapeinterface->GetAabb(body->getCenterOfMassTransform(),
459                                         minAabb,maxAabb);
460
461                                 SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold);
462                                 minAabb -= manifoldExtraExtents;
463                                 maxAabb += manifoldExtraExtents;
464                                 
465                                 BroadphaseProxy* bp = (BroadphaseProxy*) ctrl->m_broadphaseHandle;
466                                 if (bp)
467                                 {
468                                         
469 #ifdef WIN32
470                                         SimdVector3 color (1,1,0);
471                                         
472                                         if (m_debugDrawer)
473                                         {       
474                                                 //draw aabb
475                                                 switch (body->GetActivationState())
476                                                 {
477                                                 case ISLAND_SLEEPING:
478                                                         {
479                                                                 color.setValue(1,1,1);
480                                                                 break;
481                                                         }
482                                                 case WANTS_DEACTIVATION:
483                                                         {
484                                                                 color.setValue(0,0,1);
485                                                                 break;
486                                                         }
487                                                 case ACTIVE_TAG:
488                                                         {
489                                                                 break;
490                                                         }
491                                                         
492                                                 };
493
494                                                 if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)
495                                                 {
496                                                         DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
497                                                 }
498                                         }
499 #endif
500
501                                         scene->SetAabb(bp,minAabb,maxAabb);
502
503
504                                         
505                                 }
506                         }
507                         
508                         float toi = 1.f;
509
510
511                         
512                         if (m_ccdMode == 3)
513                         {
514                                 DispatcherInfo dispatchInfo;
515                                 dispatchInfo.m_timeStep = timeStep;
516                                 dispatchInfo.m_stepCount = 0;
517                                 dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS;
518                                 
519                                 scene->DispatchAllCollisionPairs( *m_dispatcher,dispatchInfo);///numsubstep,g);
520                                 toi = dispatchInfo.m_timeOfImpact;
521                         }
522                         
523                         //
524                         // integrating solution
525                         //
526                         
527                         {
528                                 std::vector<CcdPhysicsController*>::iterator i;
529                                 
530                                 for (i=m_controllers.begin();
531                                 !(i==m_controllers.end()); i++)
532                                 {
533                                         
534                                         CcdPhysicsController* ctrl = *i;
535                                         
536                                         SimdTransform predictedTrans;
537                                         RigidBody* body = ctrl->GetRigidBody();
538                                         if (body->GetActivationState() != ISLAND_SLEEPING)
539                                         {
540                                                 body->predictIntegratedTransform(timeStep*      toi, predictedTrans);
541                                                 body->proceedToTransform( predictedTrans);
542
543                                         }
544                                 }
545                                 
546                         }
547                         
548                         
549                         
550                         
551                         
552                         //
553                         // disable sleeping physics objects
554                         //
555                         
556                         std::vector<CcdPhysicsController*> m_sleepingControllers;
557                         
558                         for (i=m_controllers.begin();
559                         !(i==m_controllers.end()); i++)
560                         {
561                                 CcdPhysicsController* ctrl = (*i);
562                                 RigidBody* body = ctrl->GetRigidBody();
563
564                                 ctrl->UpdateDeactivation(timeStep);
565
566                                 
567                                 if (ctrl->wantsSleeping())
568                                 {
569                                         if (body->GetActivationState() == ACTIVE_TAG)
570                                                 body->SetActivationState( WANTS_DEACTIVATION );
571                                 } else
572                                 {
573                                         body->SetActivationState( ACTIVE_TAG );
574                                 }
575
576                                 if (useIslands)
577                                 {
578                                         if (body->GetActivationState() == ISLAND_SLEEPING)
579                                         {
580                                                 m_sleepingControllers.push_back(ctrl);
581                                         }
582                                 } else
583                                 {
584                                         if (ctrl->wantsSleeping())
585                                         {
586                                                 m_sleepingControllers.push_back(ctrl);
587                                         }
588                                 }
589                         }
590                         
591         
592                         
593                         
594         }
595         
596         SyncMotionStates(timeStep);
597
598         }
599         return true;
600 }
601
602 void            CcdPhysicsEnvironment::setDebugMode(int debugMode)
603 {
604         if (m_debugDrawer){
605                 m_debugDrawer->SetDebugMode(debugMode);
606         }
607 }
608
609 void            CcdPhysicsEnvironment::setNumIterations(int numIter)
610 {
611         m_numIterations = numIter;
612 }
613 void            CcdPhysicsEnvironment::setDeactivationTime(float dTime)
614 {
615         gDeactivationTime = dTime;
616 }
617 void            CcdPhysicsEnvironment::setDeactivationLinearTreshold(float linTresh)
618 {
619         gLinearSleepingTreshold = linTresh;
620 }
621 void            CcdPhysicsEnvironment::setDeactivationAngularTreshold(float angTresh) 
622 {
623         gAngularSleepingTreshold = angTresh;
624 }
625 void            CcdPhysicsEnvironment::setContactBreakingTreshold(float contactBreakingTreshold)
626 {
627         gContactBreakingTreshold = contactBreakingTreshold;
628         
629 }
630
631
632 void            CcdPhysicsEnvironment::setCcdMode(int ccdMode)
633 {
634         m_ccdMode = ccdMode;
635 }
636
637
638 void            CcdPhysicsEnvironment::setSolverSorConstant(float sor)
639 {
640         m_dispatcher->SetSor(sor);
641 }
642
643 void            CcdPhysicsEnvironment::setSolverTau(float tau)
644 {
645         m_dispatcher->SetTau(tau);
646 }
647 void            CcdPhysicsEnvironment::setSolverDamping(float damping)
648 {
649         m_dispatcher->SetDamping(damping);
650 }
651
652
653 void            CcdPhysicsEnvironment::setLinearAirDamping(float damping)
654 {
655         gLinearAirDamping = damping;
656 }
657
658 void            CcdPhysicsEnvironment::setUseEpa(bool epa)
659 {
660         gUseEpa = epa;
661 }
662
663 void            CcdPhysicsEnvironment::setSolverType(int solverType)
664 {
665
666         switch (solverType)
667         {
668         case 1:
669                 {
670                         if (m_solverType != solverType)
671                         {
672                                 if (m_dispatcher)
673                                         delete m_dispatcher;
674
675                                 SimpleConstraintSolver* solver= new SimpleConstraintSolver();
676                                 m_dispatcher = new ToiContactDispatcher(solver);
677                                 break;
678                         }
679                 }
680         
681         case 0:
682         default:
683                         if (m_solverType != solverType)
684                         {
685                                 if (m_dispatcher)
686                                         delete m_dispatcher;
687
688                                 OdeConstraintSolver* solver= new OdeConstraintSolver();
689                                 m_dispatcher = new ToiContactDispatcher(solver);
690                                 break;
691                         }
692
693         };
694         
695         m_solverType = solverType ;
696 }
697
698
699
700
701
702 void    CcdPhysicsEnvironment::SyncMotionStates(float timeStep)
703 {
704         std::vector<CcdPhysicsController*>::iterator i;
705
706         //
707         // synchronize the physics and graphics transformations
708         //
709
710         for (i=m_controllers.begin();
711         !(i==m_controllers.end()); i++)
712         {
713                 CcdPhysicsController* ctrl = (*i);
714                 ctrl->SynchronizeMotionStates(timeStep);
715                 
716         }
717
718 }
719 void            CcdPhysicsEnvironment::setGravity(float x,float y,float z)
720 {
721         m_gravity = SimdVector3(x,y,z);
722
723         std::vector<CcdPhysicsController*>::iterator i;
724
725         //todo: review this gravity stuff
726         for (i=m_controllers.begin();
727         !(i==m_controllers.end()); i++)
728         {
729
730                 CcdPhysicsController* ctrl = (*i);
731                 ctrl->GetRigidBody()->setGravity(m_gravity);
732
733         }
734 }
735
736
737 int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
738                                                                                                                 float pivotX,float pivotY,float pivotZ,
739                                                                                                                 float axisX,float axisY,float axisZ)
740 {
741         
742         
743         CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
744         CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
745         
746         RigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
747         RigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
748         
749         ASSERT(rb0);
750         
751         SimdVector3 pivotInA(pivotX,pivotY,pivotZ);
752         SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
753         
754         switch (type)
755         {
756         case PHY_POINT2POINT_CONSTRAINT:
757                 {
758                         
759                         Point2PointConstraint* p2p = 0;
760                         
761                         if (rb1)
762                         {
763                                 p2p = new Point2PointConstraint(*rb0,
764                                         *rb1,pivotInA,pivotInB);
765                         } else
766                         {
767                                 p2p = new Point2PointConstraint(*rb0,
768                                         pivotInA);
769                         }
770                         
771                         m_p2pConstraints.push_back(p2p);
772                         
773                         //64 bit systems can't cast pointer to int. could use size_t instead.
774                         return p2p->GetConstraintId();
775                         
776                         break;
777                 }
778         default:
779                 {
780                 }
781         };
782         
783         //RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB
784         
785         return 0;
786         
787 }
788
789 void            CcdPhysicsEnvironment::removeConstraint(int     constraintId)
790 {
791         std::vector<Point2PointConstraint*>::iterator i;
792                 
793                 //std::find(m_p2pConstraints.begin(), m_p2pConstraints.end(), 
794                 //              (Point2PointConstraint *)p2p);
795         
796                 for (i=m_p2pConstraints.begin();
797                 !(i==m_p2pConstraints.end()); i++)
798                 {
799                         Point2PointConstraint* p2p = (*i);
800                         if (p2p->GetConstraintId() == constraintId)
801                         {
802                                 std::swap(*i, m_p2pConstraints.back());
803                                 m_p2pConstraints.pop_back();
804                                 break;
805                         }
806                 }
807         
808 }
809 PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ, 
810                                                                 float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
811 {
812
813         float minFraction = 1.f;
814
815         SimdTransform   rayFromTrans,rayToTrans;
816         rayFromTrans.setIdentity();
817         rayFromTrans.setOrigin(SimdVector3(fromX,fromY,fromZ));
818         rayToTrans.setIdentity();
819         rayToTrans.setOrigin(SimdVector3(toX,toY,toZ));
820
821
822         CcdPhysicsController* nearestHit = 0;
823         
824         std::vector<CcdPhysicsController*>::iterator i;
825         SphereShape pointShape(0.0f);
826
827         /// brute force go over all objects. Once there is a broadphase, use that, or
828         /// add a raycast against aabb first.
829         for (i=m_controllers.begin();
830         !(i==m_controllers.end()); i++)
831         {
832                 CcdPhysicsController* ctrl = (*i);
833                 RigidBody* body = ctrl->GetRigidBody();
834
835                 if (body->GetCollisionShape()->IsConvex())
836                 {
837                         ConvexCast::CastResult rayResult;
838                         rayResult.m_fraction = 1.f;
839
840                         ConvexShape* convexShape = (ConvexShape*) body->GetCollisionShape();
841                         VoronoiSimplexSolver    simplexSolver;
842                         SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
843                         
844                         if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,body->getCenterOfMassTransform(),body->getCenterOfMassTransform(),rayResult))
845                         {
846                                 //add hit
847                                 rayResult.m_normal.normalize();
848                                 if (rayResult.m_fraction < minFraction)
849                                 {
850
851
852                                         minFraction = rayResult.m_fraction;
853                                         nearestHit = ctrl;
854                                         normalX = rayResult.m_normal.getX();
855                                         normalY = rayResult.m_normal.getY();
856                                         normalZ = rayResult.m_normal.getZ();
857                                         hitX = rayResult.m_hitTransformA.getOrigin().getX();
858                                         hitY = rayResult.m_hitTransformA.getOrigin().getY();
859                                         hitZ = rayResult.m_hitTransformA.getOrigin().getZ();
860                                 }
861                         }
862                 }
863                 else
864                 {
865                                 if (body->GetCollisionShape()->IsConcave())
866                                 {
867
868                                         TriangleMeshShape* triangleMesh = (TriangleMeshShape*)body->GetCollisionShape();
869                                         
870                                         SimdTransform worldToBody = body->getCenterOfMassTransform().inverse();
871
872                                         SimdVector3 rayFromLocal = worldToBody * rayFromTrans.getOrigin();
873                                         SimdVector3 rayToLocal = worldToBody * rayToTrans.getOrigin();
874
875                                         RaycastCallback rcb(rayFromLocal,rayToLocal);
876                                         rcb.m_hitFraction = minFraction;
877
878                                         SimdVector3 aabbMax(1e30f,1e30f,1e30f);
879
880                                         triangleMesh->ProcessAllTriangles(&rcb,-aabbMax,aabbMax);
881                                         if (rcb.m_hitFound)// && (rcb.m_hitFraction < minFraction))
882                                         {
883                                                 nearestHit = ctrl;
884                                                 minFraction = rcb.m_hitFraction;
885                                                 SimdVector3 hitNormalWorld = body->getCenterOfMassTransform()(rcb.m_hitNormalLocal);
886
887                                                 normalX = hitNormalWorld.getX();
888                                                 normalY = hitNormalWorld.getY();
889                                                 normalZ = hitNormalWorld.getZ();
890                                                 SimdVector3 hitWorld;
891                                                 hitWorld.setInterpolate3(rayFromTrans.getOrigin(),rayToTrans.getOrigin(),rcb.m_hitFraction);
892                                                 hitX = hitWorld.getX();
893                                                 hitY = hitWorld.getY();
894                                                 hitZ = hitWorld.getZ();
895                                                 
896                                         }
897                                 }
898                 }
899         }
900
901         return nearestHit;
902 }
903
904
905
906 int     CcdPhysicsEnvironment::getNumContactPoints()
907 {
908         return 0;
909 }
910
911 void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
912 {
913         
914 }
915
916
917
918
919
920 Dispatcher* CcdPhysicsEnvironment::GetDispatcher()
921 {
922         return m_dispatcher;
923 }
924
925 CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
926 {
927         
928         
929         m_vehicles.clear();
930         
931         //m_broadphase->DestroyScene();
932         //delete broadphase ? release reference on broadphase ?
933         
934         //first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher
935         delete m_dispatcher;
936         
937 }
938
939
940 int     CcdPhysicsEnvironment::GetNumControllers()
941 {
942         return m_controllers.size();
943 }
944
945
946 CcdPhysicsController* CcdPhysicsEnvironment::GetPhysicsController( int index)
947 {
948         return m_controllers[index];
949 }
950
951
952 int     CcdPhysicsEnvironment::GetNumManifolds() const
953 {
954         return m_dispatcher->GetNumManifolds();
955 }
956
957 const PersistentManifold*       CcdPhysicsEnvironment::GetManifold(int index) const
958 {
959         return m_dispatcher->GetManifoldByIndexInternal(index);
960 }