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