f715387a1891c26f31ffdf2b70dbbdb97361814a
[blender.git] / extern / bullet / Extras / PhysicsInterface / CcdPhysics / CcdPhysicsEnvironment.cpp
1 #include "CcdPhysicsEnvironment.h"
2 #include "CcdPhysicsController.h"
3
4 #include <algorithm>
5 #include "SimdTransform.h"
6 #include "Dynamics/RigidBody.h"
7 #include "BroadphaseCollision/BroadphaseInterface.h"
8 #include "BroadphaseCollision/SimpleBroadphase.h"
9 #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
319         setSolverType(1);
320         
321         m_collisionWorld = new CollisionWorld(dispatcher,broadphase);
322         
323         m_debugDrawer = 0;
324         m_gravity = SimdVector3(0.f,-10.f,0.f);
325         
326
327 }
328
329 void    CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
330 {
331         RigidBody* body = ctrl->GetRigidBody();
332
333         body->setGravity( m_gravity );
334         m_controllers.push_back(ctrl);
335         
336         m_collisionWorld->AddCollisionObject(body);
337
338         assert(body->m_broadphaseHandle);
339
340         BroadphaseInterface* scene =  GetBroadphase();
341
342         
343         CollisionShape* shapeinterface = ctrl->GetCollisionShape();
344         
345         assert(shapeinterface);
346         
347         const SimdTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform();
348         
349         
350         SimdPoint3 minAabb,maxAabb;
351         
352         shapeinterface->GetAabb(t,minAabb,maxAabb);
353         
354         float timeStep = 0.02f;
355         
356         
357         //extent it with the motion
358         
359         SimdVector3 linMotion = body->getLinearVelocity()*timeStep;
360         
361         float maxAabbx = maxAabb.getX();
362         float maxAabby = maxAabb.getY();
363         float maxAabbz = maxAabb.getZ();
364         float minAabbx = minAabb.getX();
365         float minAabby = minAabb.getY();
366         float minAabbz = minAabb.getZ();
367
368         if (linMotion.x() > 0.f)
369                 maxAabbx += linMotion.x(); 
370         else
371                 minAabbx += linMotion.x();
372         if (linMotion.y() > 0.f)
373                 maxAabby += linMotion.y(); 
374         else
375                 minAabby += linMotion.y();
376         if (linMotion.z() > 0.f)
377                 maxAabbz += linMotion.z(); 
378         else
379                 minAabbz += linMotion.z();
380         
381
382         minAabb = SimdVector3(minAabbx,minAabby,minAabbz);
383         maxAabb = SimdVector3(maxAabbx,maxAabby,maxAabbz);
384         
385         
386         
387         
388 }
389
390 void    CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctrl)
391 {
392         
393         //also remove constraint
394         
395         {
396                 std::vector<Point2PointConstraint*>::iterator i;
397                 
398                 for (i=m_p2pConstraints.begin();
399                 !(i==m_p2pConstraints.end()); i++)
400                 {
401                         Point2PointConstraint* p2p = (*i);
402                         if  ((&p2p->GetRigidBodyA() == ctrl->GetRigidBody() ||
403                                 (&p2p->GetRigidBodyB() == ctrl->GetRigidBody())))
404                         {
405                                  removeConstraint(p2p->GetUserConstraintId());
406                                 //only 1 constraint per constroller
407                                 break;
408                         }
409                 }
410         }
411         
412         {
413                 std::vector<Point2PointConstraint*>::iterator i;
414                 
415                 for (i=m_p2pConstraints.begin();
416                 !(i==m_p2pConstraints.end()); i++)
417                 {
418                         Point2PointConstraint* p2p = (*i);
419                         if  ((&p2p->GetRigidBodyA() == ctrl->GetRigidBody() ||
420                                 (&p2p->GetRigidBodyB() == ctrl->GetRigidBody())))
421                         {
422                                 removeConstraint(p2p->GetUserConstraintId());
423                                 //only 1 constraint per constroller
424                                 break;
425                         }
426                 }
427         }
428         
429         
430         m_collisionWorld->RemoveCollisionObject(ctrl->GetRigidBody());
431
432         
433         {
434                 std::vector<CcdPhysicsController*>::iterator i =
435                         std::find(m_controllers.begin(), m_controllers.end(), ctrl);
436                 if (!(i == m_controllers.end()))
437                 {
438                         std::swap(*i, m_controllers.back());
439                         m_controllers.pop_back();
440                 }
441         }
442 }
443
444
445 void    CcdPhysicsEnvironment::beginFrame()
446 {
447
448 }
449
450 bool    CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
451 {
452
453         if (!SimdFuzzyZero(timeStep))
454         {
455 #ifdef SPLIT_TIMESTEP
456                 proceedDeltaTimeOneStep(0.5f*timeStep);
457                 proceedDeltaTimeOneStep(0.5f*timeStep);
458 #else           
459                 proceedDeltaTimeOneStep(timeStep);
460 #endif
461         } else
462         {
463                 //todo: interpolate
464         }
465         return true;
466 }
467 /// Perform an integration step of duration 'timeStep'.
468 bool    CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
469 {
470         
471         
472 //      printf("CcdPhysicsEnvironment::proceedDeltaTime\n");
473         
474         if (SimdFuzzyZero(timeStep))
475                 return true;
476
477         if (m_debugDrawer)
478         {
479                 gDisableDeactivation = (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_NoDeactivation);
480         }
481
482
483
484         
485         //this is needed because scaling is not known in advance, and scaling has to propagate to the shape
486         if (!m_scalingPropagated)
487         {
488                 SyncMotionStates(timeStep);
489                 m_scalingPropagated = true;
490         }
491
492
493
494         {
495 //              std::vector<CcdPhysicsController*>::iterator i;
496                 
497                 
498                 
499                 int k;
500                 for (k=0;k<GetNumControllers();k++)
501                 {
502                         CcdPhysicsController* ctrl = m_controllers[k];
503                         //              SimdTransform predictedTrans;
504                         RigidBody* body = ctrl->GetRigidBody();
505                         if (body->GetActivationState() != ISLAND_SLEEPING)
506                         {
507                                 body->applyForces( timeStep);
508                                 body->integrateVelocities( timeStep);
509                                 body->predictIntegratedTransform(timeStep,body->m_nextPredictedWorldTransform);
510                         }
511                         
512                 }
513         }
514         BroadphaseInterface*    scene = GetBroadphase();
515         
516         
517         //
518         // collision detection (?)
519         //
520         
521         
522         
523         
524         
525         int numsubstep = m_numIterations;
526         
527         
528         DispatcherInfo dispatchInfo;
529         dispatchInfo.m_timeStep = timeStep;
530         dispatchInfo.m_stepCount = 0;
531
532         scene->DispatchAllCollisionPairs(*GetDispatcher(),dispatchInfo);///numsubstep,g);
533
534
535
536         
537                 
538         
539         int numRigidBodies = m_controllers.size();
540         
541         m_collisionWorld->UpdateActivationState();
542
543         
544
545         //contacts
546
547         
548         //solve the regular constraints (point 2 point, hinge, etc)
549
550         for (int g=0;g<numsubstep;g++)
551         {
552                 //
553                 // constraint solving
554                 //
555                 
556                 
557                 int i;
558                 int numPoint2Point = m_p2pConstraints.size();
559                 
560                 //point to point constraints
561                 for (i=0;i< numPoint2Point ; i++ )
562                 {
563                         Point2PointConstraint* p2p = m_p2pConstraints[i];
564                         
565                         p2p->BuildJacobian();
566                         p2p->SolveConstraint( timeStep );
567                         
568                 }
569
570
571                 
572                 
573         }
574         
575         //solve the vehicles
576
577         #ifdef NEW_BULLET_VEHICLE_SUPPORT
578                 //vehicles
579                 int numVehicles = m_wrapperVehicles.size();
580                 for (int i=0;i<numVehicles;i++)
581                 {
582                         WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
583                         RaycastVehicle* vehicle = wrapperVehicle->GetVehicle();
584                         vehicle->UpdateVehicle( timeStep);
585                 }
586 #endif //NEW_BULLET_VEHICLE_SUPPORT
587
588         
589                 struct InplaceSolverIslandCallback : public CollisionDispatcher::IslandCallback
590         {
591
592                 ContactSolverInfo& m_solverInfo;
593                 ConstraintSolver*       m_solver;
594                 IDebugDraw*     m_debugDrawer;
595                 
596                 InplaceSolverIslandCallback(
597                         ContactSolverInfo& solverInfo,
598                         ConstraintSolver*       solver,
599                         IDebugDraw*     debugDrawer)
600                         :m_solverInfo(solverInfo),
601                         m_solver(solver),
602                         m_debugDrawer(debugDrawer)
603                 {
604                         
605                 }
606                 
607                 virtual void    ProcessIsland(PersistentManifold**      manifolds,int numManifolds)
608                 {
609                         m_solver->SolveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer);
610                 }
611                 
612         };
613
614
615         m_solverInfo.m_friction = 0.9f;
616         m_solverInfo.m_numIterations = m_numIterations;
617         m_solverInfo.m_timeStep = timeStep;
618         m_solverInfo.m_restitution = 0.f;//m_restitution;
619
620         InplaceSolverIslandCallback     solverCallback(
621                         m_solverInfo,
622                         m_solver,
623                         m_debugDrawer);
624
625         /// solve all the contact points and contact friction
626         GetDispatcher()->BuildAndProcessIslands(numRigidBodies,&solverCallback);
627
628
629
630
631         {
632                 
633                 
634                 
635                 {
636                         
637                         std::vector<CcdPhysicsController*>::iterator i;
638                         
639                         //
640                         // update aabbs, only for moving objects (!)
641                         //
642                         for (i=m_controllers.begin();
643                         !(i==m_controllers.end()); i++)
644                         {
645                                 CcdPhysicsController* ctrl = (*i);
646                                 RigidBody* body = ctrl->GetRigidBody();
647                                 
648                                 
649                                 SimdPoint3 minAabb,maxAabb;
650                                 CollisionShape* shapeinterface = ctrl->GetCollisionShape();
651
652
653
654                                 shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(),
655                                         body->getLinearVelocity(),body->getAngularVelocity(),
656                                         timeStep,minAabb,maxAabb);
657
658                                 shapeinterface->GetAabb(body->getCenterOfMassTransform(),
659                                         minAabb,maxAabb);
660
661                                 SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold);
662                                 minAabb -= manifoldExtraExtents;
663                                 maxAabb += manifoldExtraExtents;
664                                 
665                                 BroadphaseProxy* bp = body->m_broadphaseHandle;
666                                 if (bp)
667                                 {
668                                         
669 #ifdef WIN32
670                                         SimdVector3 color (1,1,0);
671                                         
672                                         if (m_debugDrawer)
673                                         {       
674                                                 //draw aabb
675                                                 switch (body->GetActivationState())
676                                                 {
677                                                 case ISLAND_SLEEPING:
678                                                         {
679                                                                 color.setValue(1,1,1);
680                                                                 break;
681                                                         }
682                                                 case WANTS_DEACTIVATION:
683                                                         {
684                                                                 color.setValue(0,0,1);
685                                                                 break;
686                                                         }
687                                                 case ACTIVE_TAG:
688                                                         {
689                                                                 break;
690                                                         }
691                                                 case DISABLE_DEACTIVATION:
692                                                         {
693                                                                 color.setValue(1,0,1);
694                                                         };
695                                                         
696                                                 };
697
698                                                 if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)
699                                                 {
700                                                         DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
701                                                 }
702                                         }
703 #endif
704
705                                         scene->SetAabb(bp,minAabb,maxAabb);
706
707
708                                         
709                                 }
710                         }
711                         
712                         float toi = 1.f;
713
714
715                         
716                         if (m_ccdMode == 3)
717                         {
718                                 DispatcherInfo dispatchInfo;
719                                 dispatchInfo.m_timeStep = timeStep;
720                                 dispatchInfo.m_stepCount = 0;
721                                 dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS;
722                                 
723                                 scene->DispatchAllCollisionPairs( *GetDispatcher(),dispatchInfo);///numsubstep,g);
724                                 toi = dispatchInfo.m_timeOfImpact;
725                         }
726                         
727                         //
728                         // integrating solution
729                         //
730                         
731                         {
732                                 std::vector<CcdPhysicsController*>::iterator i;
733                                 
734                                 for (i=m_controllers.begin();
735                                 !(i==m_controllers.end()); i++)
736                                 {
737                                         
738                                         CcdPhysicsController* ctrl = *i;
739                                         
740                                         SimdTransform predictedTrans;
741                                         RigidBody* body = ctrl->GetRigidBody();
742                                         if (body->GetActivationState() != ISLAND_SLEEPING)
743                                         {
744
745                                                 body->predictIntegratedTransform(timeStep*      toi, predictedTrans);
746                                                 body->proceedToTransform( predictedTrans);
747                         
748                                         }
749                                 }
750                                 
751                         }
752                         
753                         
754                         
755                         
756                         
757                         //
758                         // disable sleeping physics objects
759                         //
760                         
761                         std::vector<CcdPhysicsController*> m_sleepingControllers;
762                         
763                         for (i=m_controllers.begin();
764                         !(i==m_controllers.end()); i++)
765                         {
766                                 CcdPhysicsController* ctrl = (*i);
767                                 RigidBody* body = ctrl->GetRigidBody();
768
769                                 ctrl->UpdateDeactivation(timeStep);
770
771                                 
772                                 if (ctrl->wantsSleeping())
773                                 {
774                                         if (body->GetActivationState() == ACTIVE_TAG)
775                                                 body->SetActivationState( WANTS_DEACTIVATION );
776                                 } else
777                                 {
778                                         if (body->GetActivationState() != DISABLE_DEACTIVATION)
779                                                 body->SetActivationState( ACTIVE_TAG );
780                                 }
781
782                                 if (useIslands)
783                                 {
784                                         if (body->GetActivationState() == ISLAND_SLEEPING)
785                                         {
786                                                 m_sleepingControllers.push_back(ctrl);
787                                         }
788                                 } else
789                                 {
790                                         if (ctrl->wantsSleeping())
791                                         {
792                                                 m_sleepingControllers.push_back(ctrl);
793                                         }
794                                 }
795                         }
796                         
797         
798                         
799                         
800         }
801         
802
803
804         SyncMotionStates(timeStep);
805
806         
807 #ifdef NEW_BULLET_VEHICLE_SUPPORT
808                 //sync wheels for vehicles
809                 int numVehicles = m_wrapperVehicles.size();
810                 for (int i=0;i<numVehicles;i++)
811                 {
812                         WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
813                 
814                         wrapperVehicle->SyncWheels();
815                 }
816 #endif //NEW_BULLET_VEHICLE_SUPPORT
817         }
818         return true;
819 }
820
821 void            CcdPhysicsEnvironment::setDebugMode(int debugMode)
822 {
823         if (m_debugDrawer){
824                 m_debugDrawer->SetDebugMode(debugMode);
825         }
826 }
827
828 void            CcdPhysicsEnvironment::setNumIterations(int numIter)
829 {
830         m_numIterations = numIter;
831 }
832 void            CcdPhysicsEnvironment::setDeactivationTime(float dTime)
833 {
834         gDeactivationTime = dTime;
835 }
836 void            CcdPhysicsEnvironment::setDeactivationLinearTreshold(float linTresh)
837 {
838         gLinearSleepingTreshold = linTresh;
839 }
840 void            CcdPhysicsEnvironment::setDeactivationAngularTreshold(float angTresh) 
841 {
842         gAngularSleepingTreshold = angTresh;
843 }
844 void            CcdPhysicsEnvironment::setContactBreakingTreshold(float contactBreakingTreshold)
845 {
846         gContactBreakingTreshold = contactBreakingTreshold;
847         
848 }
849
850
851 void            CcdPhysicsEnvironment::setCcdMode(int ccdMode)
852 {
853         m_ccdMode = ccdMode;
854 }
855
856
857 void            CcdPhysicsEnvironment::setSolverSorConstant(float sor)
858 {
859         m_solverInfo.m_sor = sor;
860 }
861
862 void            CcdPhysicsEnvironment::setSolverTau(float tau)
863 {
864         m_solverInfo.m_tau = tau;
865 }
866 void            CcdPhysicsEnvironment::setSolverDamping(float damping)
867 {
868         m_solverInfo.m_damping = damping;
869 }
870
871
872 void            CcdPhysicsEnvironment::setLinearAirDamping(float damping)
873 {
874         gLinearAirDamping = damping;
875 }
876
877 void            CcdPhysicsEnvironment::setUseEpa(bool epa)
878 {
879         gUseEpa = epa;
880 }
881
882 void            CcdPhysicsEnvironment::setSolverType(int solverType)
883 {
884
885         switch (solverType)
886         {
887         case 1:
888                 {
889                         if (m_solverType != solverType)
890                         {
891                                 
892                                 m_solver = new SimpleConstraintSolver();
893                                 
894                                 break;
895                         }
896                 }
897         
898         case 0:
899         default:
900                         if (m_solverType != solverType)
901                         {
902                                 m_solver = new OdeConstraintSolver();
903                 
904                                 break;
905                         }
906
907         };
908         
909         m_solverType = solverType ;
910 }
911
912
913
914
915
916 void    CcdPhysicsEnvironment::SyncMotionStates(float timeStep)
917 {
918         std::vector<CcdPhysicsController*>::iterator i;
919
920         //
921         // synchronize the physics and graphics transformations
922         //
923
924         for (i=m_controllers.begin();
925         !(i==m_controllers.end()); i++)
926         {
927                 CcdPhysicsController* ctrl = (*i);
928                 ctrl->SynchronizeMotionStates(timeStep);
929                 
930         }
931
932 }
933 void            CcdPhysicsEnvironment::setGravity(float x,float y,float z)
934 {
935         m_gravity = SimdVector3(x,y,z);
936
937         std::vector<CcdPhysicsController*>::iterator i;
938
939         //todo: review this gravity stuff
940         for (i=m_controllers.begin();
941         !(i==m_controllers.end()); i++)
942         {
943
944                 CcdPhysicsController* ctrl = (*i);
945                 ctrl->GetRigidBody()->setGravity(m_gravity);
946
947         }
948 }
949
950
951 #ifdef NEW_BULLET_VEHICLE_SUPPORT
952
953 class DefaultVehicleRaycaster : public VehicleRaycaster
954 {
955         CcdPhysicsEnvironment* m_physEnv;
956         PHY_IPhysicsController* m_chassis;
957
958 public:
959         DefaultVehicleRaycaster(CcdPhysicsEnvironment* physEnv,PHY_IPhysicsController* chassis):
960         m_physEnv(physEnv),
961                 m_chassis(chassis)
962         {
963         }
964
965         /*      struct VehicleRaycasterResult
966         {
967                 VehicleRaycasterResult() :m_distFraction(-1.f){};
968                 SimdVector3     m_hitPointInWorld;
969                 SimdVector3     m_hitNormalInWorld;
970                 SimdScalar      m_distFraction;
971         };
972 */
973         virtual void* CastRay(const SimdVector3& from,const SimdVector3& to, VehicleRaycasterResult& result)
974         {
975                 
976                 
977                 float hit[3];
978                 float normal[3];
979
980                 PHY_IPhysicsController* ignore = m_chassis;
981                 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]);
982                 if (hitObject)
983                 {
984                         result.m_hitPointInWorld[0] = hit[0];
985                         result.m_hitPointInWorld[1] = hit[1];
986                         result.m_hitPointInWorld[2] = hit[2];
987                         result.m_hitNormalInWorld[0] = normal[0];
988                         result.m_hitNormalInWorld[1] = normal[1];
989                         result.m_hitNormalInWorld[2] = normal[2];
990                         result.m_hitNormalInWorld.normalize();
991                         //calc fraction? or put it in the interface?
992                         //calc for now
993                         
994                         result.m_distFraction = (result.m_hitPointInWorld-from).length() / (to-from).length();
995                         //some safety for 'explosion' due to sudden penetration of the full 'ray'
996 /*                      if (result.m_distFraction<0.1)
997                         {
998                                 printf("Vehicle Raycast: avoided instability due to penetration. Consider moving the connection points deeper inside vehicle chassis");
999                                 result.m_distFraction = 1.f;
1000                                 hitObject = 0;
1001                         }
1002                         */
1003
1004 /*                      if (result.m_distFraction>1.)
1005                         {
1006                                 printf("Vehicle Raycast: avoided instability 1Consider moving the connection points deeper inside vehicle chassis");                            
1007                                 result.m_distFraction = 1.f;
1008                                 hitObject = 0;
1009                         }
1010                         */
1011
1012                                 
1013                         
1014                 }
1015                 //?
1016                 return hitObject;
1017         }
1018 };
1019 #endif //NEW_BULLET_VEHICLE_SUPPORT
1020
1021 static int gConstraintUid = 1;
1022
1023 int                     CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl0,class PHY_IPhysicsController* ctrl1,PHY_ConstraintType type,
1024                                                                                                                 float pivotX,float pivotY,float pivotZ,
1025                                                                                                                 float axisX,float axisY,float axisZ)
1026 {
1027         
1028         
1029         CcdPhysicsController* c0 = (CcdPhysicsController*)ctrl0;
1030         CcdPhysicsController* c1 = (CcdPhysicsController*)ctrl1;
1031         
1032         RigidBody* rb0 = c0 ? c0->GetRigidBody() : 0;
1033         RigidBody* rb1 = c1 ? c1->GetRigidBody() : 0;
1034         
1035         ASSERT(rb0);
1036         
1037         SimdVector3 pivotInA(pivotX,pivotY,pivotZ);
1038         SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
1039         
1040         switch (type)
1041         {
1042         case PHY_POINT2POINT_CONSTRAINT:
1043                 {
1044                         
1045                         Point2PointConstraint* p2p = 0;
1046                         
1047                         if (rb1)
1048                         {
1049                                 p2p = new Point2PointConstraint(*rb0,
1050                                         *rb1,pivotInA,pivotInB);
1051                         } else
1052                         {
1053                                 p2p = new Point2PointConstraint(*rb0,
1054                                         pivotInA);
1055                         }
1056                         
1057                         m_p2pConstraints.push_back(p2p);
1058                         p2p->SetUserConstraintId(gConstraintUid++);
1059                         p2p->SetUserConstraintType(type);
1060                         //64 bit systems can't cast pointer to int. could use size_t instead.
1061                         return p2p->GetUserConstraintId();
1062                         
1063                         break;
1064                 }
1065 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1066
1067         case PHY_VEHICLE_CONSTRAINT:
1068                 {
1069                         RaycastVehicle::VehicleTuning* tuning = new RaycastVehicle::VehicleTuning();
1070                         RigidBody* chassis = rb0;
1071                         DefaultVehicleRaycaster* raycaster = new DefaultVehicleRaycaster(this,ctrl0);
1072                         RaycastVehicle* vehicle = new RaycastVehicle(*tuning,chassis,raycaster);
1073                         WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0);
1074                         m_wrapperVehicles.push_back(wrapperVehicle);
1075                         vehicle->SetUserConstraintId(gConstraintUid++);
1076                         vehicle->SetUserConstraintType(type);
1077                         return vehicle->GetUserConstraintId();
1078
1079                         break;
1080                 };
1081 #endif //NEW_BULLET_VEHICLE_SUPPORT
1082
1083         default:
1084                 {
1085                 }
1086         };
1087         
1088         //RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB
1089         
1090         return 0;
1091         
1092 }
1093
1094 void            CcdPhysicsEnvironment::removeConstraint(int     constraintId)
1095 {
1096         std::vector<Point2PointConstraint*>::iterator i;
1097                 
1098                 //std::find(m_p2pConstraints.begin(), m_p2pConstraints.end(), 
1099                 //              (Point2PointConstraint *)p2p);
1100         
1101                 for (i=m_p2pConstraints.begin();
1102                 !(i==m_p2pConstraints.end()); i++)
1103                 {
1104                         Point2PointConstraint* p2p = (*i);
1105                         if (p2p->GetUserConstraintId() == constraintId)
1106                         {
1107                                 std::swap(*i, m_p2pConstraints.back());
1108                                 m_p2pConstraints.pop_back();
1109                                 break;
1110                         }
1111                 }
1112         
1113 }
1114 PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ, 
1115                                                                 float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
1116 {
1117
1118         float minFraction = 1.f;
1119
1120         SimdTransform   rayFromTrans,rayToTrans;
1121         rayFromTrans.setIdentity();
1122
1123         SimdVector3 rayFrom(fromX,fromY,fromZ);
1124
1125         rayFromTrans.setOrigin(rayFrom);
1126         rayToTrans.setIdentity();
1127         SimdVector3 rayTo(toX,toY,toZ);
1128         rayToTrans.setOrigin(rayTo);
1129
1130         //do culling based on aabb (rayFrom/rayTo)
1131         SimdVector3 rayAabbMin = rayFrom;
1132         SimdVector3 rayAabbMax = rayFrom;
1133         rayAabbMin.setMin(rayTo);
1134         rayAabbMax.setMax(rayTo);
1135
1136         
1137         CcdPhysicsController* nearestHit = 0;
1138         
1139         std::vector<CcdPhysicsController*>::iterator i;
1140         SphereShape pointShape(0.0f);
1141
1142         /// brute force go over all objects. Once there is a broadphase, use that, or
1143         /// add a raycast against aabb first.
1144         for (i=m_controllers.begin();
1145         !(i==m_controllers.end()); i++)
1146         {
1147                 CcdPhysicsController* ctrl = (*i);
1148                 if (ctrl == ignoreClient)
1149                         continue;
1150                 RigidBody* body = ctrl->GetRigidBody();
1151                 SimdVector3 bodyAabbMin,bodyAabbMax;
1152                 body->getAabb(bodyAabbMin,bodyAabbMax);
1153
1154                 //check aabb overlap
1155
1156                 if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,bodyAabbMin,bodyAabbMax))
1157                 {
1158                         if (body->GetCollisionShape()->IsConvex())
1159                         {
1160                                 ConvexCast::CastResult rayResult;
1161                                 rayResult.m_fraction = 1.f;
1162
1163                                 ConvexShape* convexShape = (ConvexShape*) body->GetCollisionShape();
1164                                 VoronoiSimplexSolver    simplexSolver;
1165                                 SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
1166                                 //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
1167                                 
1168                                 if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,body->getCenterOfMassTransform(),body->getCenterOfMassTransform(),rayResult))
1169                                 {
1170                                         //add hit
1171                                         if (rayResult.m_normal.length2() > 0.0001f)
1172                                         {
1173                                                 rayResult.m_normal.normalize();
1174                                                 if (rayResult.m_fraction < minFraction)
1175                                                 {
1176
1177
1178                                                         minFraction = rayResult.m_fraction;
1179                                                         nearestHit = ctrl;
1180                                                         normalX = rayResult.m_normal.getX();
1181                                                         normalY = rayResult.m_normal.getY();
1182                                                         normalZ = rayResult.m_normal.getZ();
1183                                                         SimdVector3 hitWorld;
1184                                                         hitWorld.setInterpolate3(rayFromTrans.getOrigin(),rayToTrans.getOrigin(),rayResult.m_fraction);
1185                                                         hitX = hitWorld.getX();
1186                                                         hitY = hitWorld.getY();
1187                                                         hitZ = hitWorld.getZ();
1188
1189                                                 }
1190                                         }
1191                                 }
1192                         }
1193                         else
1194                         {
1195                                         if (body->GetCollisionShape()->IsConcave())
1196                                         {
1197
1198                                                 TriangleMeshShape* triangleMesh = (TriangleMeshShape*)body->GetCollisionShape();
1199                                                 
1200                                                 SimdTransform worldToBody = body->getCenterOfMassTransform().inverse();
1201
1202                                                 SimdVector3 rayFromLocal = worldToBody * rayFromTrans.getOrigin();
1203                                                 SimdVector3 rayToLocal = worldToBody * rayToTrans.getOrigin();
1204
1205                                                 RaycastCallback rcb(rayFromLocal,rayToLocal);
1206                                                 rcb.m_hitFraction = minFraction;
1207
1208                                                 SimdVector3 rayAabbMinLocal = rayFromLocal;
1209                                                 rayAabbMinLocal.setMin(rayToLocal);
1210                                                 SimdVector3 rayAabbMaxLocal = rayFromLocal;
1211                                                 rayAabbMaxLocal.setMax(rayToLocal);
1212
1213                                                 triangleMesh->ProcessAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
1214                                                 if (rcb.m_hitFound)
1215                                                 {
1216                                                         nearestHit = ctrl;
1217                                                         minFraction = rcb.m_hitFraction;
1218                                                         SimdVector3 hitNormalWorld = body->getCenterOfMassTransform().getBasis()*rcb.m_hitNormalLocal;
1219                                                         hitNormalWorld.normalize();
1220
1221                                                         normalX = hitNormalWorld.getX();
1222                                                         normalY = hitNormalWorld.getY();
1223                                                         normalZ = hitNormalWorld.getZ();
1224                                                         SimdVector3 hitWorld;
1225                                                         hitWorld.setInterpolate3(rayFromTrans.getOrigin(),rayToTrans.getOrigin(),rcb.m_hitFraction);
1226                                                         hitX = hitWorld.getX();
1227                                                         hitY = hitWorld.getY();
1228                                                         hitZ = hitWorld.getZ();
1229                                                         
1230                                                 }
1231                                         }
1232                         }
1233                 }
1234         }
1235
1236         return nearestHit;
1237 }
1238
1239
1240
1241 int     CcdPhysicsEnvironment::getNumContactPoints()
1242 {
1243         return 0;
1244 }
1245
1246 void CcdPhysicsEnvironment::getContactPoint(int i,float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)
1247 {
1248         
1249 }
1250
1251
1252
1253
1254 BroadphaseInterface*    CcdPhysicsEnvironment::GetBroadphase()
1255
1256         return m_collisionWorld->GetBroadphase(); 
1257 }
1258
1259
1260
1261 const CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher() const
1262 {
1263         return m_collisionWorld->GetDispatcher();
1264 }
1265
1266 CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher()
1267 {
1268         return m_collisionWorld->GetDispatcher();
1269 }
1270
1271 CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
1272 {
1273         
1274 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1275         m_wrapperVehicles.clear();
1276 #endif //NEW_BULLET_VEHICLE_SUPPORT
1277         
1278         //m_broadphase->DestroyScene();
1279         //delete broadphase ? release reference on broadphase ?
1280         
1281         //first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher
1282         //delete m_dispatcher;
1283         delete m_collisionWorld;
1284         
1285 }
1286
1287
1288 int     CcdPhysicsEnvironment::GetNumControllers()
1289 {
1290         return m_controllers.size();
1291 }
1292
1293
1294 CcdPhysicsController* CcdPhysicsEnvironment::GetPhysicsController( int index)
1295 {
1296         return m_controllers[index];
1297 }
1298
1299
1300 int     CcdPhysicsEnvironment::GetNumManifolds() const
1301 {
1302         return GetDispatcher()->GetNumManifolds();
1303 }
1304
1305 const PersistentManifold*       CcdPhysicsEnvironment::GetManifold(int index) const
1306 {
1307         return GetDispatcher()->GetManifoldByIndexInternal(index);
1308 }
1309
1310 Point2PointConstraint*  CcdPhysicsEnvironment::getPoint2PointConstraint(int constraintId)
1311 {
1312         int nump2p = m_p2pConstraints.size();
1313         int i;
1314         for (i=0;i<nump2p;i++)
1315         {
1316                 Point2PointConstraint* p2p = m_p2pConstraints[i];
1317                 if (p2p->GetUserConstraintId()==constraintId)
1318                 {
1319                         return p2p;
1320                 }
1321         }
1322         return 0;
1323 }
1324
1325
1326 #ifdef NEW_BULLET_VEHICLE_SUPPORT
1327
1328 //complex constraint for vehicles
1329 PHY_IVehicle*   CcdPhysicsEnvironment::getVehicleConstraint(int constraintId)
1330 {
1331         int i;
1332
1333         int numVehicles = m_wrapperVehicles.size();
1334         for (i=0;i<numVehicles;i++)
1335         {
1336                 WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
1337                 if (wrapperVehicle->GetVehicle()->GetUserConstraintId() == constraintId)
1338                         return wrapperVehicle;
1339         }
1340
1341         return 0;
1342 }
1343
1344 #endif //NEW_BULLET_VEHICLE_SUPPORT
1345