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