2c4f98c56241d9c3ca2a8584180a2156e3c080c7
[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 #include "BroadphaseCollision/AxisSweep3.h"
10
11 #include "CollisionDispatch/CollisionWorld.h"
12
13 #include "CollisionShapes/ConvexShape.h"
14 #include "BroadphaseCollision/Dispatcher.h"
15 #include "NarrowPhaseCollision/PersistentManifold.h"
16 #include "CollisionShapes/TriangleMeshShape.h"
17 #include "ConstraintSolver/OdeConstraintSolver.h"
18 #include "ConstraintSolver/SimpleConstraintSolver.h"
19
20
21 #include "IDebugDraw.h"
22
23 #include "NarrowPhaseCollision/VoronoiSimplexSolver.h"
24 #include "NarrowPhaseCollision/SubSimplexConvexCast.h"
25 #include "NarrowPhaseCollision/GjkConvexCast.h"
26
27
28 #include "CollisionDispatch/CollisionDispatcher.h"
29 #include "PHY_IMotionState.h"
30
31 #include "CollisionDispatch/EmptyCollisionAlgorithm.h"
32 #include "CollisionDispatch/UnionFind.h"
33
34 #include "NarrowPhaseCollision/RaycastCallback.h"
35 #include "CollisionShapes/SphereShape.h"
36
37 bool useIslands = true;
38
39 #ifdef NEW_BULLET_VEHICLE_SUPPORT
40 #include "Vehicle/RaycastVehicle.h"
41 #include "Vehicle/VehicleRaycaster.h"
42
43 #include "Vehicle/WheelInfo.h"
44 #include "PHY_IVehicle.h"
45 RaycastVehicle::VehicleTuning   gTuning;
46
47 #endif //NEW_BULLET_VEHICLE_SUPPORT
48 #include "AabbUtil2.h"
49
50 #include "ConstraintSolver/ConstraintSolver.h"
51 #include "ConstraintSolver/Point2PointConstraint.h"
52 //#include "BroadphaseCollision/QueryDispatcher.h"
53 //#include "BroadphaseCollision/QueryBox.h"
54 //todo: change this to allow dynamic registration of types!
55
56 #ifdef WIN32
57 void DrawRasterizerLine(const float* from,const float* to,int color);
58 #endif
59
60
61 #include "ConstraintSolver/ContactConstraint.h"
62
63
64 #include <stdio.h>
65
66 #ifdef NEW_BULLET_VEHICLE_SUPPORT
67 class WrapperVehicle : public PHY_IVehicle
68 {
69
70         RaycastVehicle* m_vehicle;
71         PHY_IPhysicsController* m_chassis;
72                 
73 public:
74
75         WrapperVehicle(RaycastVehicle* vehicle,PHY_IPhysicsController* chassis)
76                 :m_vehicle(vehicle),
77                 m_chassis(chassis)
78         {
79         }
80
81         RaycastVehicle* GetVehicle()
82         {
83                 return m_vehicle;
84         }
85
86         PHY_IPhysicsController* GetChassis()
87         {
88                 return m_chassis;
89         }
90
91         virtual void    AddWheel(
92                         PHY_IMotionState*       motionState,
93                         PHY__Vector3    connectionPoint,
94                         PHY__Vector3    downDirection,
95                         PHY__Vector3    axleDirection,
96                         float   suspensionRestLength,
97                         float wheelRadius,
98                         bool hasSteering
99                 )
100         {
101                 SimdVector3 connectionPointCS0(connectionPoint[0],connectionPoint[1],connectionPoint[2]);
102                 SimdVector3 wheelDirectionCS0(downDirection[0],downDirection[1],downDirection[2]);
103                 SimdVector3 wheelAxle(axleDirection[0],axleDirection[1],axleDirection[2]);
104                 
105
106                 WheelInfo& info = m_vehicle->AddWheel(connectionPointCS0,wheelDirectionCS0,wheelAxle,
107                         suspensionRestLength,wheelRadius,gTuning,hasSteering);
108                 info.m_clientInfo = motionState;
109                 
110         }
111
112         void    SyncWheels()
113         {
114                 int numWheels = GetNumWheels();
115                 int i;
116                 for (i=0;i<numWheels;i++)
117                 {
118                         WheelInfo& info = m_vehicle->GetWheelInfo(i);
119                         PHY_IMotionState* motionState = (PHY_IMotionState*)info.m_clientInfo ;
120                         m_vehicle->UpdateWheelTransform(i);
121                         SimdTransform trans = m_vehicle->GetWheelTransformWS(i);
122                         SimdQuaternion orn = trans.getRotation();
123                         const SimdVector3& pos = trans.getOrigin();
124                         motionState->setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]);
125                         motionState->setWorldPosition(pos.x(),pos.y(),pos.z());
126
127                 }
128         }
129
130         virtual int             GetNumWheels() const
131         {
132                 return m_vehicle->GetNumWheels();
133         }
134         
135         virtual void    GetWheelPosition(int wheelIndex,float& posX,float& posY,float& posZ) const
136         {
137                 SimdTransform   trans = m_vehicle->GetWheelTransformWS(wheelIndex);
138                 posX = trans.getOrigin().x();
139                 posY = trans.getOrigin().y();
140                 posZ = trans.getOrigin().z();
141         }
142         virtual void    GetWheelOrientationQuaternion(int wheelIndex,float& quatX,float& quatY,float& quatZ,float& quatW) const
143         {
144                 SimdTransform   trans = m_vehicle->GetWheelTransformWS(wheelIndex);
145                 SimdQuaternion quat = trans.getRotation();
146                 SimdMatrix3x3 orn2(quat);
147
148                 quatX = trans.getRotation().x();
149                 quatY = trans.getRotation().y();
150                 quatZ = trans.getRotation().z();
151                 quatW = trans.getRotation()[3];
152                 
153
154                 //printf("test");
155
156
157         }
158
159         virtual float   GetWheelRotation(int wheelIndex) const
160         {
161                 float rotation = 0.f;
162
163                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
164                 {
165                         WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
166                         rotation = info.m_rotation;
167                 }
168                 return rotation;
169
170         }
171
172
173         
174         virtual int     GetUserConstraintId() const
175         {
176                 return m_vehicle->GetUserConstraintId();
177         }
178
179         virtual int     GetUserConstraintType() const
180         {
181                 return m_vehicle->GetUserConstraintType();
182         }
183
184         virtual void    SetSteeringValue(float steering,int wheelIndex)
185         {
186                 m_vehicle->SetSteeringValue(steering,wheelIndex);
187         }
188
189         virtual void    ApplyEngineForce(float force,int wheelIndex)
190         {
191                 m_vehicle->ApplyEngineForce(force,wheelIndex);
192         }
193
194         virtual void    ApplyBraking(float braking,int wheelIndex)
195         {
196                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
197                 {
198                         WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
199                         info.m_brake = braking;
200                 }
201         }
202
203         virtual void    SetWheelFriction(float friction,int wheelIndex)
204         {
205                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
206                 {
207                         WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
208                         info.m_frictionSlip = friction;
209                 }
210
211         }
212         
213         virtual void    SetSuspensionStiffness(float suspensionStiffness,int wheelIndex)
214         {
215                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
216                 {
217                         WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
218                         info.m_suspensionStiffness = suspensionStiffness;
219
220                 }
221         }
222
223         virtual void    SetSuspensionDamping(float suspensionDamping,int wheelIndex)
224         {
225                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
226                 {
227                         WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
228                         info.m_wheelsDampingRelaxation = suspensionDamping;
229                 }
230         }
231
232         virtual void    SetSuspensionCompression(float suspensionCompression,int wheelIndex)
233         {
234                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
235                 {
236                         WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
237                         info.m_wheelsDampingCompression = suspensionCompression;
238                 }
239         }
240
241
242         
243         virtual void    SetRollInfluence(float rollInfluence,int wheelIndex)
244         {
245                 if ((wheelIndex>=0) && (wheelIndex< m_vehicle->GetNumWheels()))
246                 {
247                         WheelInfo& info = m_vehicle->GetWheelInfo(wheelIndex);
248                         info.m_rollInfluence = rollInfluence;
249                 }
250         }
251
252
253
254 };
255 #endif //NEW_BULLET_VEHICLE_SUPPORT
256
257
258
259 static void DrawAabb(IDebugDraw* debugDrawer,const SimdVector3& from,const SimdVector3& to,const SimdVector3& color)
260 {
261         SimdVector3 halfExtents = (to-from)* 0.5f;
262         SimdVector3 center = (to+from) *0.5f;
263         int i,j;
264
265         SimdVector3 edgecoord(1.f,1.f,1.f),pa,pb;
266         for (i=0;i<4;i++)
267         {
268                 for (j=0;j<3;j++)
269                 {
270                         pa = SimdVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],              
271                                 edgecoord[2]*halfExtents[2]);
272                         pa+=center;
273                         
274                         int othercoord = j%3;
275                         edgecoord[othercoord]*=-1.f;
276                         pb = SimdVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],      
277                                 edgecoord[2]*halfExtents[2]);
278                         pb+=center;
279                         
280                         debugDrawer->DrawLine(pa,pb,color);
281                 }
282                 edgecoord = SimdVector3(-1.f,-1.f,-1.f);
283                 if (i<3)
284                         edgecoord[i]*=-1.f;
285         }
286
287
288 }
289
290
291
292
293
294
295 CcdPhysicsEnvironment::CcdPhysicsEnvironment(CollisionDispatcher* dispatcher,BroadphaseInterface* broadphase)
296 :m_scalingPropagated(false),
297 m_numIterations(10),
298 m_ccdMode(0),
299 m_solverType(-1)
300 {
301
302         if (!dispatcher)
303                 dispatcher = new CollisionDispatcher();
304
305         
306         if(!broadphase)
307         {
308
309                 //todo: calculate/let user specify this world sizes
310                 SimdVector3 worldMin(-10000,-10000,-10000);
311                 SimdVector3 worldMax(10000,10000,10000);
312
313                 broadphase = new AxisSweep3(worldMin,worldMax);
314
315                 //broadphase = new SimpleBroadphase();
316         }
317                 
318         setSolverType(1);
319         
320         m_collisionWorld = new CollisionWorld(dispatcher,broadphase);
321         
322         m_debugDrawer = 0;
323         m_gravity = SimdVector3(0.f,-10.f,0.f);
324         
325
326 }
327
328 void    CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
329 {
330         RigidBody* body = ctrl->GetRigidBody();
331
332         body->setGravity( m_gravity );
333         m_controllers.push_back(ctrl);
334         
335         m_collisionWorld->AddCollisionObject(body);
336
337         assert(body->m_broadphaseHandle);
338
339         BroadphaseInterface* scene =  GetBroadphase();
340
341         
342         CollisionShape* shapeinterface = ctrl->GetCollisionShape();
343         
344         assert(shapeinterface);
345         
346         const SimdTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform();
347         
348         
349         SimdPoint3 minAabb,maxAabb;
350         
351         shapeinterface->GetAabb(t,minAabb,maxAabb);
352         
353         float timeStep = 0.02f;
354         
355         
356         //extent it with the motion
357         
358         SimdVector3 linMotion = body->getLinearVelocity()*timeStep;
359         
360         float maxAabbx = maxAabb.getX();
361         float maxAabby = maxAabb.getY();
362         float maxAabbz = maxAabb.getZ();
363         float minAabbx = minAabb.getX();
364         float minAabby = minAabb.getY();
365         float minAabbz = minAabb.getZ();
366
367         if (linMotion.x() > 0.f)
368                 maxAabbx += linMotion.x(); 
369         else
370                 minAabbx += linMotion.x();
371         if (linMotion.y() > 0.f)
372                 maxAabby += linMotion.y(); 
373         else
374                 minAabby += linMotion.y();
375         if (linMotion.z() > 0.f)
376                 maxAabbz += linMotion.z(); 
377         else
378                 minAabbz += linMotion.z();
379         
380
381         minAabb = SimdVector3(minAabbx,minAabby,minAabbz);
382         maxAabb = SimdVector3(maxAabbx,maxAabby,maxAabbz);
383         
384         
385         
386         
387 }
388
389 void    CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctrl)
390 {
391         
392         //also remove constraint
393         
394         {
395                 std::vector<Point2PointConstraint*>::iterator i;
396                 
397                 for (i=m_p2pConstraints.begin();
398                 !(i==m_p2pConstraints.end()); i++)
399                 {
400                         Point2PointConstraint* p2p = (*i);
401                         if  ((&p2p->GetRigidBodyA() == ctrl->GetRigidBody() ||
402                                 (&p2p->GetRigidBodyB() == ctrl->GetRigidBody())))
403                         {
404                                  removeConstraint(p2p->GetUserConstraintId());
405                                 //only 1 constraint per constroller
406                                 break;
407                         }
408                 }
409         }
410         
411         {
412                 std::vector<Point2PointConstraint*>::iterator i;
413                 
414                 for (i=m_p2pConstraints.begin();
415                 !(i==m_p2pConstraints.end()); i++)
416                 {
417                         Point2PointConstraint* p2p = (*i);
418                         if  ((&p2p->GetRigidBodyA() == ctrl->GetRigidBody() ||
419                                 (&p2p->GetRigidBodyB() == ctrl->GetRigidBody())))
420                         {
421                                 removeConstraint(p2p->GetUserConstraintId());
422                                 //only 1 constraint per constroller
423                                 break;
424                         }
425                 }
426         }
427         
428         
429         m_collisionWorld->RemoveCollisionObject(ctrl->GetRigidBody());
430
431         
432         {
433                 std::vector<CcdPhysicsController*>::iterator i =
434                         std::find(m_controllers.begin(), m_controllers.end(), ctrl);
435                 if (!(i == m_controllers.end()))
436                 {
437                         std::swap(*i, m_controllers.back());
438                         m_controllers.pop_back();
439                 }
440         }
441 }
442
443
444 void    CcdPhysicsEnvironment::beginFrame()
445 {
446
447 }
448
449 bool    CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
450 {
451
452         if (!SimdFuzzyZero(timeStep))
453         {
454                 //Blender runs 30hertz, so subdivide so we get 60 hertz
455                 proceedDeltaTimeOneStep(0.5f*timeStep);
456                 proceedDeltaTimeOneStep(0.5f*timeStep);
457         } else
458         {
459                 //todo: interpolate
460         }
461         return true;
462 }
463 /// Perform an integration step of duration 'timeStep'.
464 bool    CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
465 {
466         
467         
468 //      printf("CcdPhysicsEnvironment::proceedDeltaTime\n");
469         
470         if (SimdFuzzyZero(timeStep))
471                 return true;
472
473         if (m_debugDrawer)
474         {
475                 gDisableDeactivation = (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_NoDeactivation);
476         }
477
478
479
480         
481         //this is needed because scaling is not known in advance, and scaling has to propagate to the shape
482         if (!m_scalingPropagated)
483         {
484                 SyncMotionStates(timeStep);
485                 m_scalingPropagated = true;
486         }
487
488
489
490         {
491 //              std::vector<CcdPhysicsController*>::iterator i;
492                 
493                 
494                 
495                 int k;
496                 for (k=0;k<GetNumControllers();k++)
497                 {
498                         CcdPhysicsController* ctrl = m_controllers[k];
499                         //              SimdTransform predictedTrans;
500                         RigidBody* body = ctrl->GetRigidBody();
501                         if (body->GetActivationState() != ISLAND_SLEEPING)
502                         {
503                                 body->applyForces( timeStep);
504                                 body->integrateVelocities( timeStep);
505                                 body->predictIntegratedTransform(timeStep,body->m_nextPredictedWorldTransform);
506                         }
507                         
508                 }
509         }
510         BroadphaseInterface*    scene = GetBroadphase();
511         
512         
513         //
514         // collision detection (?)
515         //
516         
517         
518         
519         
520         
521         int numsubstep = m_numIterations;
522         
523         
524         DispatcherInfo dispatchInfo;
525         dispatchInfo.m_timeStep = timeStep;
526         dispatchInfo.m_stepCount = 0;
527         dispatchInfo.m_debugDraw = m_debugDrawer;
528
529         scene->DispatchAllCollisionPairs(*GetDispatcher(),dispatchInfo);///numsubstep,g);
530
531
532
533         
534                 
535         
536         int numRigidBodies = m_controllers.size();
537         
538         m_collisionWorld->UpdateActivationState();
539
540         
541
542         //contacts
543
544         
545         struct InplaceSolverIslandCallback : public CollisionDispatcher::IslandCallback
546         {
547
548                 ContactSolverInfo& m_solverInfo;
549                 ConstraintSolver*       m_solver;
550                 IDebugDraw*     m_debugDrawer;
551                 
552                 InplaceSolverIslandCallback(
553                         ContactSolverInfo& solverInfo,
554                         ConstraintSolver*       solver,
555                         IDebugDraw*     debugDrawer)
556                         :m_solverInfo(solverInfo),
557                         m_solver(solver),
558                         m_debugDrawer(debugDrawer)
559                 {
560                         
561                 }
562                 
563                 virtual void    ProcessIsland(PersistentManifold**      manifolds,int numManifolds)
564                 {
565                         m_solver->SolveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer);
566                 }
567                 
568         };
569
570
571         m_solverInfo.m_friction = 0.9f;
572         m_solverInfo.m_numIterations = m_numIterations;
573         m_solverInfo.m_timeStep = timeStep;
574         m_solverInfo.m_restitution = 0.f;//m_restitution;
575
576         InplaceSolverIslandCallback     solverCallback(
577                         m_solverInfo,
578                         m_solver,
579                         m_debugDrawer);
580
581         GetDispatcher()->BuildAndProcessIslands(numRigidBodies,&solverCallback);
582
583
584         for (int g=0;g<numsubstep;g++)
585         {
586                 //
587                 // constraint solving
588                 //
589                 
590                 
591                 int i;
592                 int numPoint2Point = m_p2pConstraints.size();
593                 
594                 //point to point constraints
595                 for (i=0;i< numPoint2Point ; i++ )
596                 {
597                         Point2PointConstraint* p2p = m_p2pConstraints[i];
598                         
599                         p2p->BuildJacobian();
600                         p2p->SolveConstraint( timeStep );
601                         
602                 }
603
604
605                 
606                 
607         }
608         
609
610         #ifdef NEW_BULLET_VEHICLE_SUPPORT
611                 //vehicles
612                 int numVehicles = m_wrapperVehicles.size();
613                 for (int i=0;i<numVehicles;i++)
614                 {
615                         WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
616                         RaycastVehicle* vehicle = wrapperVehicle->GetVehicle();
617                         vehicle->UpdateVehicle( timeStep);
618                 }
619 #endif //NEW_BULLET_VEHICLE_SUPPORT
620
621                 
622
623
624         {
625                 
626                 
627                 
628                 {
629                         
630                         std::vector<CcdPhysicsController*>::iterator i;
631                         
632                         //
633                         // update aabbs, only for moving objects (!)
634                         //
635                         for (i=m_controllers.begin();
636                         !(i==m_controllers.end()); i++)
637                         {
638                                 CcdPhysicsController* ctrl = (*i);
639                                 RigidBody* body = ctrl->GetRigidBody();
640                                 
641                                 
642                                 SimdPoint3 minAabb,maxAabb;
643                                 CollisionShape* shapeinterface = ctrl->GetCollisionShape();
644
645
646
647                                 shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(),
648                                         body->getLinearVelocity(),body->getAngularVelocity(),
649                                         timeStep,minAabb,maxAabb);
650
651                                 shapeinterface->GetAabb(body->getCenterOfMassTransform(),
652                                         minAabb,maxAabb);
653
654                                 SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold);
655                                 minAabb -= manifoldExtraExtents;
656                                 maxAabb += manifoldExtraExtents;
657                                 
658                                 BroadphaseProxy* bp = body->m_broadphaseHandle;
659                                 if (bp)
660                                 {
661                                         
662 #ifdef WIN32
663                                         SimdVector3 color (1,1,0);
664                                         
665                                         if (m_debugDrawer)
666                                         {       
667                                                 //draw aabb
668                                                 switch (body->GetActivationState())
669                                                 {
670                                                 case ISLAND_SLEEPING:
671                                                         {
672                                                                 color.setValue(1,1,1);
673                                                                 break;
674                                                         }
675                                                 case WANTS_DEACTIVATION:
676                                                         {
677                                                                 color.setValue(0,0,1);
678                                                                 break;
679                                                         }
680                                                 case ACTIVE_TAG:
681                                                         {
682                                                                 break;
683                                                         }
684                                                 case DISABLE_DEACTIVATION:
685                                                         {
686                                                                 color.setValue(1,0,1);
687                                                         };
688                                                         
689                                                 };
690
691                                                 if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)
692                                                 {
693                                                         DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
694                                                 }
695                                         }
696 #endif
697
698                                         scene->SetAabb(bp,minAabb,maxAabb);
699
700
701                                         
702                                 }
703                         }
704                         
705                         float toi = 1.f;
706
707
708                         
709                         if (m_ccdMode == 3)
710                         {
711                                 DispatcherInfo dispatchInfo;
712                                 dispatchInfo.m_timeStep = timeStep;
713                                 dispatchInfo.m_stepCount = 0;
714                                 dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS;
715                                 
716                                 scene->DispatchAllCollisionPairs( *GetDispatcher(),dispatchInfo);///numsubstep,g);
717                                 toi = dispatchInfo.m_timeOfImpact;
718                         }
719                         
720                         //
721                         // integrating solution
722                         //
723                         
724                         {
725                                 std::vector<CcdPhysicsController*>::iterator i;
726                                 
727                                 for (i=m_controllers.begin();
728                                 !(i==m_controllers.end()); i++)
729                                 {
730                                         
731                                         CcdPhysicsController* ctrl = *i;
732                                         
733                                         SimdTransform predictedTrans;
734                                         RigidBody* body = ctrl->GetRigidBody();
735                                         if (body->GetActivationState() != ISLAND_SLEEPING)
736                                         {
737
738                                                 body->predictIntegratedTransform(timeStep*      toi, predictedTrans);
739                                                 body->proceedToTransform( predictedTrans);
740                         
741                                         }
742                                 }
743                                 
744                         }
745                         
746                         
747                         
748                         
749                         
750                         //
751                         // disable sleeping physics objects
752                         //
753                         
754                         std::vector<CcdPhysicsController*> m_sleepingControllers;
755                         
756                         for (i=m_controllers.begin();
757                         !(i==m_controllers.end()); i++)
758                         {
759                                 CcdPhysicsController* ctrl = (*i);
760                                 RigidBody* body = ctrl->GetRigidBody();
761
762                                 ctrl->UpdateDeactivation(timeStep);
763
764                                 
765                                 if (ctrl->wantsSleeping())
766                                 {
767                                         if (body->GetActivationState() == ACTIVE_TAG)
768                                                 body->SetActivationState( WANTS_DEACTIVATION );
769                                 } else
770                                 {
771                                         if (body->GetActivationState() != DISABLE_DEACTIVATION)
772                                                 body->SetActivationState( ACTIVE_TAG );
773                                 }
774
775                                 if (useIslands)
776                                 {
777                                         if (body->GetActivationState() == ISLAND_SLEEPING)
778                                         {
779                                                 m_sleepingControllers.push_back(ctrl);
780                                         }
781                                 } else
782                                 {
783                                         if (ctrl->wantsSleeping())
784                                         {
785                                                 m_sleepingControllers.push_back(ctrl);
786                                         }
787                                 }
788                         }
789                         
790         
791                         
792                         
793         }
794         
795
796
797         SyncMotionStates(timeStep);
798
799         
800 #ifdef NEW_BULLET_VEHICLE_SUPPORT
801                 //sync wheels for vehicles
802                 int numVehicles = m_wrapperVehicles.size();
803                 for (int i=0;i<numVehicles;i++)
804                 {
805                         WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
806                 
807                         wrapperVehicle->SyncWheels();
808                 }
809 #endif //NEW_BULLET_VEHICLE_SUPPORT
810         }
811         return true;
812 }
813
814 void            CcdPhysicsEnvironment::setDebugMode(int debugMode)
815 {
816         if (m_debugDrawer){
817                 m_debugDrawer->SetDebugMode(debugMode);
818         }
819 }
820
821 void            CcdPhysicsEnvironment::setNumIterations(int numIter)
822 {
823         m_numIterations = numIter;
824 }
825 void            CcdPhysicsEnvironment::setDeactivationTime(float dTime)
826 {
827         gDeactivationTime = dTime;
828 }
829 void            CcdPhysicsEnvironment::setDeactivationLinearTreshold(float linTresh)
830 {
831         gLinearSleepingTreshold = linTresh;
832 }
833 void            CcdPhysicsEnvironment::setDeactivationAngularTreshold(float angTresh) 
834 {
835         gAngularSleepingTreshold = angTresh;
836 }
837 void            CcdPhysicsEnvironment::setContactBreakingTreshold(float contactBreakingTreshold)
838 {
839         gContactBreakingTreshold = contactBreakingTreshold;
840         
841 }
842
843
844 void            CcdPhysicsEnvironment::setCcdMode(int ccdMode)
845 {
846         m_ccdMode = ccdMode;
847 }
848
849
850 void            CcdPhysicsEnvironment::setSolverSorConstant(float sor)
851 {
852         m_solverInfo.m_sor = sor;
853 }
854
855 void            CcdPhysicsEnvironment::setSolverTau(float tau)
856 {
857         m_solverInfo.m_tau = tau;
858 }
859 void            CcdPhysicsEnvironment::setSolverDamping(float damping)
860 {
861         m_solverInfo.m_damping = damping;
862 }
863
864
865 void            CcdPhysicsEnvironment::setLinearAirDamping(float damping)
866 {
867         gLinearAirDamping = damping;
868 }
869
870 void            CcdPhysicsEnvironment::setUseEpa(bool epa)
871 {
872         gUseEpa = epa;
873 }
874
875 void            CcdPhysicsEnvironment::setSolverType(int solverType)
876 {
877
878         switch (solverType)
879         {
880         case 1:
881                 {
882                         if (m_solverType != solverType)
883                         {
884                                 
885                                 m_solver = new SimpleConstraintSolver();
886                                 //printf("Iterative Impulse ConstraintSolver\n");
887                                 break;
888                         }
889                 }
890         
891         case 0:
892         default:
893                         if (m_solverType != solverType)
894                         {
895                                 m_solver = new OdeConstraintSolver();
896                                 //printf("Quickstep ConstraintSolver\n");
897                 
898                                 break;
899                         }
900
901         };
902         
903         m_solverType = solverType ;
904 }
905
906
907
908
909
910 void    CcdPhysicsEnvironment::SyncMotionStates(float timeStep)
911 {
912         std::vector<CcdPhysicsController*>::iterator i;
913
914         //
915         // synchronize the physics and graphics transformations
916         //
917
918         for (i=m_controllers.begin();
919         !(i==m_controllers.end()); i++)
920         {
921                 CcdPhysicsController* ctrl = (*i);
922                 ctrl->SynchronizeMotionStates(timeStep);
923                 
924         }
925
926 }
927 void            CcdPhysicsEnvironment::setGravity(float x,float y,float z)
928 {
929         m_gravity = SimdVector3(x,y,z);
930
931         std::vector<CcdPhysicsController*>::iterator i;
932
933         //todo: review this gravity stuff
934         for (i=m_controllers.begin();
935         !(i==m_controllers.end()); i++)
936         {
937
938                 CcdPhysicsController* ctrl = (*i);
939                 ctrl->GetRigidBody()->setGravity(m_gravity);
940
941         }
942 }
943
944
945 #ifdef NEW_BULLET_VEHICLE_SUPPORT
946
947 class BlenderVehicleRaycaster : public VehicleRaycaster
948 {
949         CcdPhysicsEnvironment* m_physEnv;
950         PHY_IPhysicsController* m_chassis;
951
952 public:
953         BlenderVehicleRaycaster(CcdPhysicsEnvironment* physEnv,PHY_IPhysicsController* chassis):
954         m_physEnv(physEnv),
955                 m_chassis(chassis)
956         {
957         }
958
959         /*      struct VehicleRaycasterResult
960         {
961                 VehicleRaycasterResult() :m_distFraction(-1.f){};
962                 SimdVector3     m_hitPointInWorld;
963                 SimdVector3     m_hitNormalInWorld;
964                 SimdScalar      m_distFraction;
965         };
966 */
967         virtual void* CastRay(const SimdVector3& from,const SimdVector3& to, VehicleRaycasterResult& result)
968         {
969                 
970                 
971                 float hit[3];
972                 float normal[3];
973
974                 PHY_IPhysicsController* ignore = m_chassis;
975                 void* hitObject = m_physEnv->rayTest(ignore,from.x(),from.y(),from.z(),to.x(),to.y(),to.z(),hit[0],hit[1],hit[2],normal[0],normal[1],normal[2]);
976                 if (hitObject)
977                 {
978                         result.m_hitPointInWorld[0] = hit[0];
979                         result.m_hitPointInWorld[1] = hit[1];
980                         result.m_hitPointInWorld[2] = hit[2];
981                         result.m_hitNormalInWorld[0] = normal[0];
982                         result.m_hitNormalInWorld[1] = normal[1];
983                         result.m_hitNormalInWorld[2] = normal[2];
984                         result.m_hitNormalInWorld.normalize();
985                         //calc fraction? or put it in the interface?
986                         //calc for now
987                         
988                         result.m_distFraction = (result.m_hitPointInWorld-from).length() / (to-from).length();
989                         //some safety for 'explosion' due to sudden penetration of the full 'ray'
990 /*                      if (result.m_distFraction<0.1)
991                         {
992                                 printf("Vehicle Raycast: avoided instability due to penetration. Consider moving the connection points deeper inside vehicle chassis");
993                                 result.m_distFraction = 1.f;
994                                 hitObject = 0;
995                         }
996                         */
997
998 /*                      if (result.m_distFraction>1.)
999                         {
1000                                 printf("Vehicle Raycast: avoided instability 1Consider moving the connection points deeper inside vehicle chassis");                            
1001                                 result.m_distFraction = 1.f;
1002                                 hitObject = 0;
1003                         }
1004                         */
1005
1006                                 
1007                         
1008                 }
1009                 //?
1010                 return hitObject;
1011         }
1012 };
1013 #endif //NEW_BULLET_VEHICLE_SUPPORT
1014
1015 static int gConstraintUid = 1;
1016
1017 int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
1018                                                                                                                 float pivotX,float pivotY,float pivotZ,
1019                                                                                                                 float axisX,float axisY,float axisZ)
1020 {
1021         
1022         
1023         CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
1024         CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
1025         
1026         RigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
1027         RigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
1028         
1029         ASSERT(rb0);
1030         
1031         SimdVector3 pivotInA(pivotX,pivotY,pivotZ);
1032         SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
1033         
1034         switch (type)
1035         {
1036         case PHY_POINT2POINT_CONSTRAINT:
1037                 {
1038                         
1039                         Point2PointConstraint* p2p = 0;
1040                         
1041                         if (rb1)
1042                         {
1043                                 p2p = new Point2PointConstraint(*rb0,
1044                                         *rb1,pivotInA,pivotInB);
1045                         } else
1046                         {
1047                                 p2p = new Point2PointConstraint(*rb0,
1048                                         pivotInA);
1049                         }
1050                         
1051                         m_p2pConstraints.push_back(p2p);
1052                         p2p->SetUserConstraintId(gConstraintUid++);
1053                         p2p->SetUserConstraintType(type);
1054                         //64 bit systems can't cast pointer to int. could use size_t instead.
1055                         return p2p->GetUserConstraintId();
1056                         
1057                         break;
1058                 }
1059 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1060
1061         case PHY_VEHICLE_CONSTRAINT:
1062                 {
1063                         RaycastVehicle::VehicleTuning* tuning = new RaycastVehicle::VehicleTuning();
1064                         RigidBody* chassis = rb0;
1065                         BlenderVehicleRaycaster* raycaster = new BlenderVehicleRaycaster(this,ctrl0);
1066                         RaycastVehicle* vehicle = new RaycastVehicle(*tuning,chassis,raycaster);
1067                         WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
1068                         m_wrapperVehicles.push_back(wrapperVehicle);
1069                         vehicle->SetUserConstraintId(gConstraintUid++);
1070                         vehicle->SetUserConstraintType(type);
1071                         return vehicle->GetUserConstraintId();
1072
1073                         break;
1074                 };
1075 #endif //NEW_BULLET_VEHICLE_SUPPORT
1076
1077         default:
1078                 {
1079                 }
1080         };
1081         
1082         //RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB
1083         
1084         return 0;
1085         
1086 }
1087
1088 void            CcdPhysicsEnvironment::removeConstraint(int     constraintId)
1089 {
1090         std::vector<Point2PointConstraint*>::iterator i;
1091                 
1092                 //std::find(m_p2pConstraints.begin(), m_p2pConstraints.end(), 
1093                 //              (Point2PointConstraint *)p2p);
1094         
1095                 for (i=m_p2pConstraints.begin();
1096                 !(i==m_p2pConstraints.end()); i++)
1097                 {
1098                         Point2PointConstraint* p2p = (*i);
1099                         if (p2p->GetUserConstraintId() == constraintId)
1100                         {
1101                                 std::swap(*i, m_p2pConstraints.back());
1102                                 m_p2pConstraints.pop_back();
1103                                 break;
1104                         }
1105                 }
1106         
1107 }
1108 PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ, 
1109                                                                 float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
1110 {
1111
1112         float minFraction = 1.f;
1113
1114         SimdTransform   rayFromTrans,rayToTrans;
1115         rayFromTrans.setIdentity();
1116
1117         SimdVector3 rayFrom(fromX,fromY,fromZ);
1118
1119         rayFromTrans.setOrigin(rayFrom);
1120         rayToTrans.setIdentity();
1121         SimdVector3 rayTo(toX,toY,toZ);
1122         rayToTrans.setOrigin(rayTo);
1123
1124         //do culling based on aabb (rayFrom/rayTo)
1125         SimdVector3 rayAabbMin = rayFrom;
1126         SimdVector3 rayAabbMax = rayFrom;
1127         rayAabbMin.setMin(rayTo);
1128         rayAabbMax.setMax(rayTo);
1129
1130         
1131         CcdPhysicsController* nearestHit = 0;
1132         
1133         std::vector<CcdPhysicsController*>::iterator i;
1134         SphereShape pointShape(0.0f);
1135
1136         /// brute force go over all objects. Once there is a broadphase, use that, or
1137         /// add a raycast against aabb first.
1138         for (i=m_controllers.begin();
1139         !(i==m_controllers.end()); i++)
1140         {
1141                 CcdPhysicsController* ctrl = (*i);
1142                 if (ctrl == ignoreClient)
1143                         continue;
1144                 RigidBody* body = ctrl->GetRigidBody();
1145                 SimdVector3 bodyAabbMin,bodyAabbMax;
1146                 body->getAabb(bodyAabbMin,bodyAabbMax);
1147
1148                 //check aabb overlap
1149
1150                 if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,bodyAabbMin,bodyAabbMax))
1151                 {
1152                         if (body->GetCollisionShape()->IsConvex())
1153                         {
1154                                 ConvexCast::CastResult rayResult;
1155                                 rayResult.m_fraction = 1.f;
1156
1157                                 ConvexShape* convexShape = (ConvexShape*) body->GetCollisionShape();
1158                                 VoronoiSimplexSolver    simplexSolver;
1159                                 SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
1160                                 //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
1161                                 
1162                                 if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,body->getCenterOfMassTransform(),body->getCenterOfMassTransform(),rayResult))
1163                                 {
1164                                         //add hit
1165                                         if (rayResult.m_normal.length2() > 0.0001f)
1166                                         {
1167                                                 rayResult.m_normal.normalize();
1168                                                 if (rayResult.m_fraction < minFraction)
1169                                                 {
1170
1171
1172                                                         minFraction = rayResult.m_fraction;
1173                                                         nearestHit = ctrl;
1174                                                         normalX = rayResult.m_normal.getX();
1175                                                         normalY = rayResult.m_normal.getY();
1176                                                         normalZ = rayResult.m_normal.getZ();
1177                                                         SimdVector3 hitWorld;
1178                                                         hitWorld.setInterpolate3(rayFromTrans.getOrigin(),rayToTrans.getOrigin(),rayResult.m_fraction);
1179                                                         hitX = hitWorld.getX();
1180                                                         hitY = hitWorld.getY();
1181                                                         hitZ = hitWorld.getZ();
1182
1183                                                 }
1184                                         }
1185                                 }
1186                         }
1187                         else
1188                         {
1189                                         if (body->GetCollisionShape()->IsConcave())
1190                                         {
1191
1192                                                 TriangleMeshShape* triangleMesh = (TriangleMeshShape*)body->GetCollisionShape();
1193                                                 
1194                                                 SimdTransform worldToBody = body->getCenterOfMassTransform().inverse();
1195
1196                                                 SimdVector3 rayFromLocal = worldToBody * rayFromTrans.getOrigin();
1197                                                 SimdVector3 rayToLocal = worldToBody * rayToTrans.getOrigin();
1198
1199                                                 RaycastCallback rcb(rayFromLocal,rayToLocal);
1200                                                 rcb.m_hitFraction = minFraction;
1201
1202                                                 SimdVector3 rayAabbMinLocal = rayFromLocal;
1203                                                 rayAabbMinLocal.setMin(rayToLocal);
1204                                                 SimdVector3 rayAabbMaxLocal = rayFromLocal;
1205                                                 rayAabbMaxLocal.setMax(rayToLocal);
1206
1207                                                 triangleMesh->ProcessAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
1208                                                 if (rcb.m_hitFound)
1209                                                 {
1210                                                         nearestHit = ctrl;
1211                                                         minFraction = rcb.m_hitFraction;
1212                                                         SimdVector3 hitNormalWorld = body->getCenterOfMassTransform().getBasis()*rcb.m_hitNormalLocal;
1213                                                         hitNormalWorld.normalize();
1214
1215                                                         normalX = hitNormalWorld.getX();
1216                                                         normalY = hitNormalWorld.getY();
1217                                                         normalZ = hitNormalWorld.getZ();
1218                                                         SimdVector3 hitWorld;
1219                                                         hitWorld.setInterpolate3(rayFromTrans.getOrigin(),rayToTrans.getOrigin(),rcb.m_hitFraction);
1220                                                         hitX = hitWorld.getX();
1221                                                         hitY = hitWorld.getY();
1222                                                         hitZ = hitWorld.getZ();
1223                                                         
1224                                                 }
1225                                         }
1226                         }
1227                 }
1228         }
1229
1230         return nearestHit;
1231 }
1232
1233
1234
1235 int     CcdPhysicsEnvironment::getNumContactPoints()
1236 {
1237         return 0;
1238 }
1239
1240 void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
1241 {
1242         
1243 }
1244
1245
1246
1247
1248 BroadphaseInterface*    CcdPhysicsEnvironment::GetBroadphase()
1249
1250         return m_collisionWorld->GetBroadphase(); 
1251 }
1252
1253
1254
1255 const CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher() const
1256 {
1257         return m_collisionWorld->GetDispatcher();
1258 }
1259
1260 CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher()
1261 {
1262         return m_collisionWorld->GetDispatcher();
1263 }
1264
1265 CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
1266 {
1267         
1268 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1269         m_wrapperVehicles.clear();
1270 #endif //NEW_BULLET_VEHICLE_SUPPORT
1271         
1272         //m_broadphase->DestroyScene();
1273         //delete broadphase ? release reference on broadphase ?
1274         
1275         //first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher
1276         //delete m_dispatcher;
1277         delete m_collisionWorld;
1278         
1279 }
1280
1281
1282 int     CcdPhysicsEnvironment::GetNumControllers()
1283 {
1284         return m_controllers.size();
1285 }
1286
1287
1288 CcdPhysicsController* CcdPhysicsEnvironment::GetPhysicsController( int index)
1289 {
1290         return m_controllers[index];
1291 }
1292
1293
1294 int     CcdPhysicsEnvironment::GetNumManifolds() const
1295 {
1296         return GetDispatcher()->GetNumManifolds();
1297 }
1298
1299 const PersistentManifold*       CcdPhysicsEnvironment::GetManifold(int index) const
1300 {
1301         return GetDispatcher()->GetManifoldByIndexInternal(index);
1302 }
1303
1304
1305
1306 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1307
1308 //complex constraint for vehicles
1309 PHY_IVehicle*   CcdPhysicsEnvironment::getVehicleConstraint(int constraintId)
1310 {
1311         int i;
1312
1313         int numVehicles = m_wrapperVehicles.size();
1314         for (i=0;i<numVehicles;i++)
1315         {
1316                 WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
1317                 if (wrapperVehicle->GetVehicle()->GetUserConstraintId() == constraintId)
1318                         return wrapperVehicle;
1319         }
1320
1321         return 0;
1322 }
1323
1324 #endif //NEW_BULLET_VEHICLE_SUPPORT
1325