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