- Added support for kinematic objects (interaction between rigidbodies), deriving...
[blender-staging.git] / source / gameengine / Physics / Bullet / CcdPhysicsController.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose, 
8 including commercial applications, and to alter it and redistribute it freely, 
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16 #include "CcdPhysicsController.h"
17
18 #include "Dynamics/RigidBody.h"
19 #include "PHY_IMotionState.h"
20 #include "BroadphaseCollision/BroadphaseProxy.h"
21 #include "BroadphaseCollision/BroadphaseInterface.h"
22 #include "CollisionShapes/ConvexShape.h"
23 #include "CcdPhysicsEnvironment.h"
24 #include "SimdTransformUtil.h"
25
26 class BP_Proxy;
27
28 ///todo: fill all the empty CcdPhysicsController methods, hook them up to the RigidBody class
29
30 //'temporarily' global variables
31 float   gDeactivationTime = 2.f;
32 bool    gDisableDeactivation = false;
33
34 float gLinearSleepingTreshold = 0.4f;
35 float gAngularSleepingTreshold = 1.0f;
36
37 #include "Dynamics/MassProps.h"
38
39 SimdVector3 startVel(0,0,0);//-10000);
40 CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
41 :m_cci(ci)
42 {
43         m_collisionDelay = 0;
44         m_newClientInfo = 0;
45         
46         m_MotionState = ci.m_MotionState;
47
48         
49         
50         CreateRigidbody();
51         
52
53         
54         #ifdef WIN32
55         if (m_body->getInvMass())
56                 m_body->setLinearVelocity(startVel);
57         #endif
58
59 }
60
61 SimdTransform   CcdPhysicsController::GetTransformFromMotionState(PHY_IMotionState* motionState)
62 {
63         SimdTransform trans;
64         float tmp[3];
65         motionState->getWorldPosition(tmp[0],tmp[1],tmp[2]);
66         trans.setOrigin(SimdVector3(tmp[0],tmp[1],tmp[2]));
67
68         SimdQuaternion orn;
69         motionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
70         trans.setRotation(orn);
71         return trans;
72
73 }
74
75 void CcdPhysicsController::CreateRigidbody()
76 {
77
78         SimdTransform trans = GetTransformFromMotionState(m_cci.m_MotionState);
79
80         MassProps mp(m_cci.m_mass, m_cci.m_localInertiaTensor);
81
82         m_body = new RigidBody(mp,0,0,m_cci.m_friction,m_cci.m_restitution);
83         m_body->m_collisionShape = m_cci.m_collisionShape;
84         
85
86         //
87         // init the rigidbody properly
88         //
89         
90         m_body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
91         //setMassProps this also sets collisionFlags
92         m_body->m_collisionFlags = m_cci.m_collisionFlags;
93         
94         m_body->setGravity( m_cci.m_gravity);
95         m_body->setDamping(m_cci.m_linearDamping, m_cci.m_angularDamping);
96         m_body->setCenterOfMassTransform( trans );
97
98
99 }
100
101 CcdPhysicsController::~CcdPhysicsController()
102 {
103         //will be reference counted, due to sharing
104         m_cci.m_physicsEnv->removeCcdPhysicsController(this);
105         delete m_MotionState;
106         delete m_body;
107 }
108
109                 /**
110                         SynchronizeMotionStates ynchronizes dynas, kinematic and deformable entities (and do 'late binding')
111                 */
112 bool            CcdPhysicsController::SynchronizeMotionStates(float time)
113 {
114         //sync non-static to motionstate, and static from motionstate (todo: add kinematic etc.)
115
116         if (!m_body->IsStatic())
117         {
118                 const SimdVector3& worldPos = m_body->getCenterOfMassPosition();
119                 m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
120                 
121                 const SimdQuaternion& worldquat = m_body->getOrientation();
122                 m_MotionState->setWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
123
124                 m_MotionState->calculateWorldTransformations();
125
126                 float scale[3];
127                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
128                 SimdVector3 scaling(scale[0],scale[1],scale[2]);
129                 GetCollisionShape()->setLocalScaling(scaling);
130         } else
131         {
132                 SimdVector3 worldPos;
133                 SimdQuaternion worldquat;
134
135                 m_MotionState->getWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
136                 m_MotionState->getWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
137                 SimdTransform oldTrans = m_body->getCenterOfMassTransform();
138                 SimdTransform newTrans(worldquat,worldPos);
139                                 
140                 m_body->setCenterOfMassTransform(newTrans);
141                 //need to keep track of previous position for friction effects...
142                 
143                 m_MotionState->calculateWorldTransformations();
144
145                 float scale[3];
146                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
147                 SimdVector3 scaling(scale[0],scale[1],scale[2]);
148                 GetCollisionShape()->setLocalScaling(scaling);
149         }
150         return true;
151
152 }
153
154                 /**
155                         WriteMotionStateToDynamics synchronizes dynas, kinematic and deformable entities (and do 'late binding')
156                 */
157                 
158 void            CcdPhysicsController::WriteMotionStateToDynamics(bool nondynaonly)
159 {
160
161 }
162 void            CcdPhysicsController::WriteDynamicsToMotionState()
163 {
164 }
165                 // controller replication
166 void            CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl)
167 {
168         m_MotionState = motionstate;
169
170         
171
172         m_body = 0;
173         CreateRigidbody();
174         
175         m_cci.m_physicsEnv->addCcdPhysicsController(this);
176
177
178 /*      SM_Object* dynaparent=0;
179         SumoPhysicsController* sumoparentctrl = (SumoPhysicsController* )parentctrl;
180         
181         if (sumoparentctrl)
182         {
183                 dynaparent = sumoparentctrl->GetSumoObject();
184         }
185         
186         SM_Object* orgsumoobject = m_sumoObj;
187         
188         
189         m_sumoObj       =       new SM_Object(
190                 orgsumoobject->getShapeHandle(), 
191                 orgsumoobject->getMaterialProps(),                      
192                 orgsumoobject->getShapeProps(),
193                 dynaparent);
194         
195         m_sumoObj->setRigidBody(orgsumoobject->isRigidBody());
196         
197         m_sumoObj->setMargin(orgsumoobject->getMargin());
198         m_sumoObj->setPosition(orgsumoobject->getPosition());
199         m_sumoObj->setOrientation(orgsumoobject->getOrientation());
200         //if it is a dyna, register for a callback
201         m_sumoObj->registerCallback(*this);
202         
203         m_sumoScene->add(* (m_sumoObj));
204         */
205
206
207
208 }
209
210                 // kinematic methods
211 void            CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
212 {
213         if (m_body)
214         {
215                 m_body->activate();
216
217                 SimdVector3 dloc(dlocX,dlocY,dlocZ);
218                 SimdTransform xform = m_body->getCenterOfMassTransform();
219
220                 if (local)
221                 {
222                         dloc = xform.getBasis()*dloc;
223                 }
224
225                 xform.setOrigin(xform.getOrigin() + dloc);
226                 m_body->setCenterOfMassTransform(xform);
227         }
228
229 }
230
231 void            CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
232 {
233         if (m_body )
234         {
235                 m_body->activate();
236
237                 SimdMatrix3x3 drotmat(  rotval[0],rotval[1],rotval[2],
238                                                                 rotval[4],rotval[5],rotval[6],
239                                                                 rotval[8],rotval[9],rotval[10]);
240
241
242                 SimdMatrix3x3 currentOrn;
243                 GetWorldOrientation(currentOrn);
244
245                 SimdTransform xform = m_body->getCenterOfMassTransform();
246
247                 xform.setBasis(xform.getBasis()*(local ? 
248                 drotmat : (currentOrn.inverse() * drotmat * currentOrn)));
249
250                 m_body->setCenterOfMassTransform(xform);
251         }
252
253 }
254
255 void CcdPhysicsController::GetWorldOrientation(SimdMatrix3x3& mat)
256 {
257         float orn[4];
258         m_MotionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
259         SimdQuaternion quat(orn[0],orn[1],orn[2],orn[3]);
260         mat.setRotation(quat);
261 }
262
263 void            CcdPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
264 {
265         SimdQuaternion q = m_body->getCenterOfMassTransform().getRotation();
266         quatImag0 = q[0];
267         quatImag1 = q[1];
268         quatImag2 = q[2];
269         quatReal = q[3];
270 }
271 void            CcdPhysicsController::setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal)
272 {
273         m_body->activate();
274
275         SimdTransform xform  = m_body->getCenterOfMassTransform();
276         xform.setRotation(SimdQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
277         m_body->setCenterOfMassTransform(xform);
278
279 }
280
281 void            CcdPhysicsController::setPosition(float posX,float posY,float posZ)
282 {
283         m_body->activate();
284
285         SimdTransform xform  = m_body->getCenterOfMassTransform();
286         xform.setOrigin(SimdVector3(posX,posY,posZ));
287         m_body->setCenterOfMassTransform(xform);
288
289 }
290 void            CcdPhysicsController::resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ)
291 {
292 }
293
294 void            CcdPhysicsController::getPosition(PHY__Vector3& pos) const
295 {
296         const SimdTransform& xform = m_body->getCenterOfMassTransform();
297         pos[0] = xform.getOrigin().x();
298         pos[1] = xform.getOrigin().y();
299         pos[2] = xform.getOrigin().z();
300 }
301
302 void            CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
303 {
304         if (!SimdFuzzyZero(m_cci.m_scaling.x()-scaleX) ||
305                 !SimdFuzzyZero(m_cci.m_scaling.y()-scaleY) ||
306                 !SimdFuzzyZero(m_cci.m_scaling.z()-scaleZ))
307         {
308                 m_cci.m_scaling = SimdVector3(scaleX,scaleY,scaleZ);
309
310                 if (m_body && m_body->GetCollisionShape())
311                 {
312                         m_body->GetCollisionShape()->setLocalScaling(m_cci.m_scaling);
313                         m_body->GetCollisionShape()->CalculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor);
314                         m_body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
315                 }
316         }
317 }
318                 
319                 // physics methods
320 void            CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)
321 {
322         SimdVector3 torque(torqueX,torqueY,torqueZ);
323         SimdTransform xform = m_body->getCenterOfMassTransform();
324         if (local)
325         {
326                 torque  = xform.getBasis()*torque;
327         }
328         m_body->applyTorque(torque);
329 }
330
331 void            CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local)
332 {
333         SimdVector3 force(forceX,forceY,forceZ);
334         SimdTransform xform = m_body->getCenterOfMassTransform();
335         if (local)
336         {
337                 force   = xform.getBasis()*force;
338         }
339         m_body->applyCentralForce(force);
340 }
341 void            CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
342 {
343         SimdVector3 angvel(ang_velX,ang_velY,ang_velZ);
344         if (angvel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
345         {
346                 m_body->activate();
347         }
348
349         {
350                 SimdTransform xform = m_body->getCenterOfMassTransform();
351                 if (local)
352                 {
353                         angvel  = xform.getBasis()*angvel;
354                 }
355
356                 m_body->setAngularVelocity(angvel);
357         }
358
359 }
360 void            CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)
361 {
362
363         SimdVector3 linVel(lin_velX,lin_velY,lin_velZ);
364         if (linVel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
365         {
366                 m_body->activate();
367         }
368         
369         {
370                 SimdTransform xform = m_body->getCenterOfMassTransform();
371                 if (local)
372                 {
373                         linVel  = xform.getBasis()*linVel;
374                 }
375                 m_body->setLinearVelocity(linVel);
376         }
377 }
378 void            CcdPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)
379 {
380         SimdVector3 impulse(impulseX,impulseY,impulseZ);
381
382         if (impulse.length2() > (SIMD_EPSILON*SIMD_EPSILON))
383         {
384                 m_body->activate();
385                 
386                 SimdVector3 pos(attachX,attachY,attachZ);
387
388                 m_body->activate();
389
390                 m_body->applyImpulse(impulse,pos);
391         }
392
393 }
394 void            CcdPhysicsController::SetActive(bool active)
395 {
396 }
397                 // reading out information from physics
398 void            CcdPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& linvZ)
399 {
400         const SimdVector3& linvel = this->m_body->getLinearVelocity();
401         linvX = linvel.x();
402         linvY = linvel.y();
403         linvZ = linvel.z();
404
405 }
406
407 void            CcdPhysicsController::GetAngularVelocity(float& angVelX,float& angVelY,float& angVelZ)
408 {
409         const SimdVector3& angvel= m_body->getAngularVelocity();
410         angVelX = angvel.x();
411         angVelY = angvel.y();
412         angVelZ = angvel.z();
413 }
414
415 void            CcdPhysicsController::GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ)
416 {
417         SimdVector3 pos(posX,posY,posZ);
418         SimdVector3 rel_pos = pos-m_body->getCenterOfMassPosition();
419         SimdVector3 linvel = m_body->getVelocityInLocalPoint(rel_pos);
420         linvX = linvel.x();
421         linvY = linvel.y();
422         linvZ = linvel.z();
423 }
424 void            CcdPhysicsController::getReactionForce(float& forceX,float& forceY,float& forceZ)
425 {
426 }
427
428                 // dyna's that are rigidbody are free in orientation, dyna's with non-rigidbody are restricted 
429 void            CcdPhysicsController::setRigidBody(bool rigid)
430 {
431         if (!rigid)
432         {
433                 //fake it for now
434                 SimdVector3 inertia = m_body->getInvInertiaDiagLocal();
435                 inertia[1] = 0.f;
436                 m_body->setInvInertiaDiagLocal(inertia);
437                 m_body->updateInertiaTensor();
438         }
439 }
440
441                 // clientinfo for raycasts for example
442 void*           CcdPhysicsController::getNewClientInfo()
443 {
444         return m_newClientInfo;
445 }
446 void            CcdPhysicsController::setNewClientInfo(void* clientinfo)
447 {
448         m_newClientInfo = clientinfo;
449 }
450
451
452 void    CcdPhysicsController::UpdateDeactivation(float timeStep)
453 {
454         if ( (m_body->GetActivationState() == ISLAND_SLEEPING) || (m_body->GetActivationState() == DISABLE_DEACTIVATION))
455                 return;
456
457         if ((m_body->getLinearVelocity().length2() < gLinearSleepingTreshold*gLinearSleepingTreshold) &&
458                 (m_body->getAngularVelocity().length2() < gAngularSleepingTreshold*gAngularSleepingTreshold))
459         {
460                 m_body->m_deactivationTime += timeStep;
461         } else
462         {
463                 m_body->m_deactivationTime=0.f;
464                 m_body->SetActivationState(0);
465         }
466
467 }
468
469 bool CcdPhysicsController::wantsSleeping()
470 {
471
472         if (m_body->GetActivationState() == DISABLE_DEACTIVATION)
473                 return false;
474
475         //disable deactivation
476         if (gDisableDeactivation || (gDeactivationTime == 0.f))
477                 return false;
478
479         if ( (m_body->GetActivationState() == ISLAND_SLEEPING) || (m_body->GetActivationState() == WANTS_DEACTIVATION))
480                 return true;
481
482         if (m_body->m_deactivationTime> gDeactivationTime)
483         {
484                 return true;
485         }
486         return false;
487 }
488