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