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