Merge with 2.5 -r 21788:22040.
[blender.git] / source / gameengine / Ketsji / KX_BulletPhysicsController.cpp
1 //under visual studio the #define in KX_ConvertPhysicsObject.h is quicker for recompilation
2 #include "KX_ConvertPhysicsObject.h"
3
4 #ifdef USE_BULLET
5
6 #include "KX_BulletPhysicsController.h"
7
8 #include "btBulletDynamicsCommon.h"
9 #include "SG_Spatial.h"
10
11 #include "KX_GameObject.h"
12 #include "KX_MotionState.h"
13 #include "KX_ClientObjectInfo.h"
14
15 #include "PHY_IPhysicsEnvironment.h"
16 #include "CcdPhysicsEnvironment.h"
17 #include "BulletSoftBody/btSoftBody.h"
18
19
20 KX_BulletPhysicsController::KX_BulletPhysicsController (const CcdConstructionInfo& ci, bool dyna, bool sensor, bool compound)
21 : KX_IPhysicsController(dyna,sensor,compound,(PHY_IPhysicsController*)this),
22 CcdPhysicsController(ci),
23 m_savedCollisionFlags(0),
24 m_savedCollisionFilterGroup(0),
25 m_savedCollisionFilterMask(0),
26 m_savedMass(0.0),
27 m_savedDyna(false),
28 m_suspended(false),
29 m_bulletChildShape(NULL)
30 {
31 }
32         
33 KX_BulletPhysicsController::~KX_BulletPhysicsController ()
34 {
35         // The game object has a direct link to 
36         if (m_pObject)
37         {
38                 // If we cheat in SetObject, we must also cheat here otherwise the 
39                 // object will still things it has a physical controller
40                 // Note that it requires that m_pObject is reset in case the object is deleted
41                 // before the controller (usual case, see KX_Scene::RemoveNodeDestructObjec)
42                 // The non usual case is when the object is not deleted because its reference is hanging
43                 // in a AddObject actuator but the node is deleted. This case is covered here.
44                 KX_GameObject* gameobj = (KX_GameObject*)       m_pObject->GetSGClientObject();
45                 gameobj->SetPhysicsController(NULL,false);
46         }
47 }
48
49 void    KX_BulletPhysicsController::resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ)
50 {
51         CcdPhysicsController::resolveCombinedVelocities(linvelX,linvelY,linvelZ,angVelX,angVelY,angVelZ);
52
53 }
54
55
56         ///////////////////////////////////
57         //      KX_IPhysicsController interface
58         ////////////////////////////////////
59
60 void    KX_BulletPhysicsController::applyImpulse(const MT_Point3& attach, const MT_Vector3& impulse)
61 {
62                 CcdPhysicsController::applyImpulse(attach[0],attach[1],attach[2],impulse[0],impulse[1],impulse[2]);
63
64 }
65
66 float KX_BulletPhysicsController::GetLinVelocityMin()
67 {
68         return (float)CcdPhysicsController::GetLinVelocityMin();
69 }
70 void  KX_BulletPhysicsController::SetLinVelocityMin(float val)
71 {
72         CcdPhysicsController::SetLinVelocityMin(val);
73 }
74
75 float KX_BulletPhysicsController::GetLinVelocityMax()
76 {
77         return (float)CcdPhysicsController::GetLinVelocityMax();
78 }
79 void  KX_BulletPhysicsController::SetLinVelocityMax(float val)
80 {
81         CcdPhysicsController::SetLinVelocityMax(val);
82 }
83
84 void    KX_BulletPhysicsController::SetObject (SG_IObject* object)
85 {
86         SG_Controller::SetObject(object);
87
88         // cheating here...
89         //should not be necessary, is it for duplicates ?
90
91         KX_GameObject* gameobj = (KX_GameObject*)       object->GetSGClientObject();
92         gameobj->SetPhysicsController(this,gameobj->IsDynamic());
93         CcdPhysicsController::setNewClientInfo(gameobj->getClientInfo());
94
95         if (m_bSensor)
96         {
97                 // use a different callback function for sensor object, 
98                 // bullet will not synchronize, we must do it explicitely
99                 SG_Callbacks& callbacks = gameobj->GetSGNode()->GetCallBackFunctions();
100                 callbacks.m_updatefunc = KX_GameObject::SynchronizeTransformFunc;
101         } 
102 }
103
104 MT_Scalar KX_BulletPhysicsController::GetRadius()
105 {
106         return MT_Scalar(CcdPhysicsController::GetRadius());
107 }
108
109 void    KX_BulletPhysicsController::setMargin (float collisionMargin)
110 {
111         CcdPhysicsController::SetMargin(collisionMargin);
112 }
113 void    KX_BulletPhysicsController::RelativeTranslate(const MT_Vector3& dloc,bool local)
114 {
115         CcdPhysicsController::RelativeTranslate(dloc[0],dloc[1],dloc[2],local);
116
117 }
118
119 void    KX_BulletPhysicsController::RelativeRotate(const MT_Matrix3x3& drot,bool local)
120 {
121         float   rotval[12];
122         drot.getValue(rotval);
123         CcdPhysicsController::RelativeRotate(rotval,local);
124 }
125
126 void    KX_BulletPhysicsController::ApplyTorque(const MT_Vector3& torque,bool local)
127 {
128                 CcdPhysicsController::ApplyTorque(torque.x(),torque.y(),torque.z(),local);
129 }
130 void    KX_BulletPhysicsController::ApplyForce(const MT_Vector3& force,bool local)
131 {
132         CcdPhysicsController::ApplyForce(force.x(),force.y(),force.z(),local);
133 }
134 MT_Vector3 KX_BulletPhysicsController::GetLinearVelocity()
135 {
136         float angVel[3];
137         //CcdPhysicsController::GetAngularVelocity(angVel[0],angVel[1],angVel[2]);
138         CcdPhysicsController::GetLinearVelocity(angVel[0],angVel[1],angVel[2]);//rcruiz
139         return MT_Vector3(angVel[0],angVel[1],angVel[2]);
140 }
141 MT_Vector3 KX_BulletPhysicsController::GetAngularVelocity()
142 {
143         float angVel[3];
144         //CcdPhysicsController::GetAngularVelocity(angVel[0],angVel[1],angVel[2]);
145         CcdPhysicsController::GetAngularVelocity(angVel[0],angVel[1],angVel[2]);//rcruiz
146         return MT_Vector3(angVel[0],angVel[1],angVel[2]);
147 }
148 MT_Vector3 KX_BulletPhysicsController::GetVelocity(const MT_Point3& pos)
149 {
150         float linVel[3];
151         CcdPhysicsController::GetVelocity(pos[0], pos[1], pos[2], linVel[0],linVel[1],linVel[2]);
152         return MT_Vector3(linVel[0],linVel[1],linVel[2]);
153 }
154
155 void    KX_BulletPhysicsController::SetAngularVelocity(const MT_Vector3& ang_vel,bool local)
156 {
157         CcdPhysicsController::SetAngularVelocity(ang_vel.x(),ang_vel.y(),ang_vel.z(),local);
158
159 }
160 void    KX_BulletPhysicsController::SetLinearVelocity(const MT_Vector3& lin_vel,bool local)
161 {
162         CcdPhysicsController::SetLinearVelocity(lin_vel.x(),lin_vel.y(),lin_vel.z(),local);
163 }
164 void    KX_BulletPhysicsController::getOrientation(MT_Quaternion& orn)
165 {
166         float myorn[4];
167         CcdPhysicsController::getOrientation(myorn[0],myorn[1],myorn[2],myorn[3]);
168         orn = MT_Quaternion(myorn[0],myorn[1],myorn[2],myorn[3]);
169 }
170 void KX_BulletPhysicsController::setOrientation(const MT_Matrix3x3& orn)
171 {
172         btMatrix3x3 btmat(orn[0][0], orn[0][1], orn[0][2], orn[1][0], orn[1][1], orn[1][2], orn[2][0], orn[2][1], orn[2][2]);
173         CcdPhysicsController::setWorldOrientation(btmat);
174 }
175 void KX_BulletPhysicsController::setPosition(const MT_Point3& pos)
176 {
177         CcdPhysicsController::setPosition(pos.x(),pos.y(),pos.z());
178 }
179 void KX_BulletPhysicsController::setScaling(const MT_Vector3& scaling)
180 {
181         CcdPhysicsController::setScaling(scaling.x(),scaling.y(),scaling.z());
182 }
183 void KX_BulletPhysicsController::SetTransform()
184 {
185         btVector3 pos;
186         btVector3 scale;
187         float ori[12];
188         m_MotionState->getWorldPosition(pos.m_floats[0],pos.m_floats[1],pos.m_floats[2]);
189         m_MotionState->getWorldScaling(scale.m_floats[0],scale.m_floats[1],scale.m_floats[2]);
190         m_MotionState->getWorldOrientation(ori);
191         btMatrix3x3 rot(ori[0], ori[4], ori[8],
192                                         ori[1], ori[5], ori[9],
193                                         ori[2], ori[6], ori[10]);
194         CcdPhysicsController::forceWorldTransform(rot, pos);
195 }
196
197 MT_Scalar       KX_BulletPhysicsController::GetMass()
198 {
199         if (GetSoftBody())
200                 return GetSoftBody()->getTotalMass();
201         
202         MT_Scalar invmass = 0.f;
203         if (GetRigidBody())
204                 invmass = GetRigidBody()->getInvMass();
205         if (invmass)
206                 return 1.f/invmass;
207         return 0.f;
208
209 }
210
211 MT_Vector3 KX_BulletPhysicsController::GetLocalInertia()
212 {
213     MT_Vector3 inertia(0.f, 0.f, 0.f);
214     btVector3 inv_inertia;
215     if (GetRigidBody()) {
216         inv_inertia = GetRigidBody()->getInvInertiaDiagLocal();
217                 if (!btFuzzyZero(inv_inertia.getX()) && 
218                         !btFuzzyZero(inv_inertia.getY()) && 
219                         !btFuzzyZero(inv_inertia.getZ()))
220                         inertia = MT_Vector3(1.f/inv_inertia.getX(), 1.f/inv_inertia.getY(), 1.f/inv_inertia.getZ());
221     }
222     return inertia;
223 }
224
225 MT_Vector3      KX_BulletPhysicsController::getReactionForce()
226 {
227         assert(0);
228         return MT_Vector3(0.f,0.f,0.f);
229 }
230 void    KX_BulletPhysicsController::setRigidBody(bool rigid)
231 {
232 }
233
234 /* This function dynamically adds the collision shape of another controller to
235    the current controller shape provided it is a compound shape.
236    The idea is that dynamic parenting on a compound object will dynamically extend the shape
237 */
238 void    KX_BulletPhysicsController::AddCompoundChild(KX_IPhysicsController* child)
239
240         if (child == NULL || !IsCompound())
241                 return;
242         // other controller must be a bullet controller too
243         // verify that body and shape exist and match
244         KX_BulletPhysicsController* childCtrl = dynamic_cast<KX_BulletPhysicsController*>(child);
245         btRigidBody* rootBody = GetRigidBody();
246         btRigidBody* childBody = childCtrl->GetRigidBody();
247         if (!rootBody || !childBody)
248                 return;
249         const btCollisionShape* rootShape = rootBody->getCollisionShape();
250         const btCollisionShape* childShape = childBody->getCollisionShape();
251         if (!rootShape || 
252                 !childShape || 
253                 rootShape->getShapeType() != COMPOUND_SHAPE_PROXYTYPE ||
254                 childShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
255                 return;
256         btCompoundShape* compoundShape = (btCompoundShape*)rootShape;
257         // compute relative transformation between parent and child
258         btTransform rootTrans;
259         btTransform childTrans;
260         rootBody->getMotionState()->getWorldTransform(rootTrans);
261         childBody->getMotionState()->getWorldTransform(childTrans);
262         btVector3 rootScale = rootShape->getLocalScaling();
263         rootScale[0] = 1.0/rootScale[0];
264         rootScale[1] = 1.0/rootScale[1];
265         rootScale[2] = 1.0/rootScale[2];
266         // relative scale = child_scale/parent_scale
267         btVector3 relativeScale = childShape->getLocalScaling()*rootScale;
268         btMatrix3x3 rootRotInverse = rootTrans.getBasis().transpose();  
269         // relative pos = parent_rot^-1 * ((parent_pos-child_pos)/parent_scale)
270         btVector3 relativePos = rootRotInverse*((childTrans.getOrigin()-rootTrans.getOrigin())*rootScale);
271         // relative rot = parent_rot^-1 * child_rot
272         btMatrix3x3 relativeRot = rootRotInverse*childTrans.getBasis();
273         // create a proxy shape info to store the transformation
274         CcdShapeConstructionInfo* proxyShapeInfo = new CcdShapeConstructionInfo();
275         // store the transformation to this object shapeinfo
276         proxyShapeInfo->m_childTrans.setOrigin(relativePos);
277         proxyShapeInfo->m_childTrans.setBasis(relativeRot);
278         proxyShapeInfo->m_childScale.setValue(relativeScale[0], relativeScale[1], relativeScale[2]);
279         // we will need this to make sure that we remove the right proxy later when unparenting
280         proxyShapeInfo->m_userData = childCtrl;
281         proxyShapeInfo->SetProxy(childCtrl->GetShapeInfo()->AddRef());
282         // add to parent compound shapeinfo
283         GetShapeInfo()->AddShape(proxyShapeInfo);
284         // create new bullet collision shape from the object shapeinfo and set scaling
285         btCollisionShape* newChildShape = proxyShapeInfo->CreateBulletShape(childCtrl->GetMargin());
286         newChildShape->setLocalScaling(relativeScale);
287         // add bullet collision shape to parent compound collision shape
288         compoundShape->addChildShape(proxyShapeInfo->m_childTrans,newChildShape);
289         // remember we created this shape
290         childCtrl->m_bulletChildShape = newChildShape;
291         // recompute inertia of parent
292         if (!rootBody->isStaticOrKinematicObject())
293         {
294                 btVector3 localInertia;
295                 float mass = 1.f/rootBody->getInvMass();
296                 compoundShape->calculateLocalInertia(mass,localInertia);
297                 rootBody->setMassProps(mass,localInertia);
298         }
299         // must update the broadphase cache,
300         GetPhysicsEnvironment()->refreshCcdPhysicsController(this);
301         // remove the children
302         GetPhysicsEnvironment()->disableCcdPhysicsController(childCtrl);
303 }
304
305 /* Reverse function of the above, it will remove a shape from a compound shape
306    provided that the former was added to the later using  AddCompoundChild()
307 */
308 void    KX_BulletPhysicsController::RemoveCompoundChild(KX_IPhysicsController* child)
309
310         if (child == NULL || !IsCompound())
311                 return;
312         // other controller must be a bullet controller too
313         // verify that body and shape exist and match
314         KX_BulletPhysicsController* childCtrl = dynamic_cast<KX_BulletPhysicsController*>(child);
315         btRigidBody* rootBody = GetRigidBody();
316         btRigidBody* childBody = childCtrl->GetRigidBody();
317         if (!rootBody || !childBody)
318                 return;
319         const btCollisionShape* rootShape = rootBody->getCollisionShape();
320         if (!rootShape || 
321                 rootShape->getShapeType() != COMPOUND_SHAPE_PROXYTYPE)
322                 return;
323         btCompoundShape* compoundShape = (btCompoundShape*)rootShape;
324         // retrieve the shapeInfo
325         CcdShapeConstructionInfo* childShapeInfo = childCtrl->GetShapeInfo();
326         CcdShapeConstructionInfo* rootShapeInfo = GetShapeInfo();
327         // and verify that the child is part of the parent
328         int i = rootShapeInfo->FindChildShape(childShapeInfo, childCtrl);
329         if (i < 0)
330                 return;
331         rootShapeInfo->RemoveChildShape(i);
332         if (childCtrl->m_bulletChildShape)
333         {
334                 int numChildren = compoundShape->getNumChildShapes();
335                 for (i=0; i<numChildren; i++)
336                 {
337                         if (compoundShape->getChildShape(i) == childCtrl->m_bulletChildShape)
338                         {
339                                 compoundShape->removeChildShapeByIndex(i);
340                                 compoundShape->recalculateLocalAabb();
341                                 break;
342                         }
343                 }
344                 delete childCtrl->m_bulletChildShape;
345                 childCtrl->m_bulletChildShape = NULL;
346         }
347         // recompute inertia of parent
348         if (!rootBody->isStaticOrKinematicObject())
349         {
350                 btVector3 localInertia;
351                 float mass = 1.f/rootBody->getInvMass();
352                 compoundShape->calculateLocalInertia(mass,localInertia);
353                 rootBody->setMassProps(mass,localInertia);
354         }
355         // must update the broadphase cache,
356         GetPhysicsEnvironment()->refreshCcdPhysicsController(this);
357         // reactivate the children
358         GetPhysicsEnvironment()->enableCcdPhysicsController(childCtrl);
359 }
360
361 void KX_BulletPhysicsController::SetMass(MT_Scalar newmass)
362 {
363         btRigidBody *body = GetRigidBody();
364         if (body && !m_suspended && newmass>MT_EPSILON && GetMass()>MT_EPSILON)
365         {
366                 btVector3 grav = body->getGravity();
367                 btVector3 accel = grav / GetMass();
368                 
369                 btBroadphaseProxy* handle = body->getBroadphaseHandle();
370                 GetPhysicsEnvironment()->updateCcdPhysicsController(this, 
371                         newmass,
372                         body->getCollisionFlags(),
373                         handle->m_collisionFilterGroup, 
374                         handle->m_collisionFilterMask);
375                 body->setGravity(accel);
376         }
377 }
378
379 void    KX_BulletPhysicsController::SuspendDynamics(bool ghost)
380 {
381         btRigidBody *body = GetRigidBody();
382         if (body && !m_suspended && !IsSensor())
383         {
384                 btBroadphaseProxy* handle = body->getBroadphaseHandle();
385                 m_savedCollisionFlags = body->getCollisionFlags();
386                 m_savedMass = GetMass();
387                 m_savedDyna = m_bDyna;
388                 m_savedCollisionFilterGroup = handle->m_collisionFilterGroup;
389                 m_savedCollisionFilterMask = handle->m_collisionFilterMask;
390                 m_suspended = true;
391                 GetPhysicsEnvironment()->updateCcdPhysicsController(this, 
392                         0.0,
393                         btCollisionObject::CF_STATIC_OBJECT|((ghost)?btCollisionObject::CF_NO_CONTACT_RESPONSE:(m_savedCollisionFlags&btCollisionObject::CF_NO_CONTACT_RESPONSE)),
394                         btBroadphaseProxy::StaticFilter, 
395                         btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
396                 m_bDyna = false;
397         }
398 }
399
400 void    KX_BulletPhysicsController::RestoreDynamics()
401 {
402         btRigidBody *body = GetRigidBody();
403         if (body && m_suspended)
404         {
405                 // before make sure any position change that was done in this logic frame are accounted for
406                 SetTransform();
407                 GetPhysicsEnvironment()->updateCcdPhysicsController(this, 
408                         m_savedMass,
409                         m_savedCollisionFlags,
410                         m_savedCollisionFilterGroup,
411                         m_savedCollisionFilterMask);
412                 body->activate();
413                 m_bDyna = m_savedDyna;
414                 m_suspended = false;
415         }
416 }
417
418 SG_Controller*  KX_BulletPhysicsController::GetReplica(class SG_Node* destnode)
419 {
420         PHY_IMotionState* motionstate = new KX_MotionState(destnode);
421
422         KX_BulletPhysicsController* physicsreplica = new KX_BulletPhysicsController(*this);
423
424         //parentcontroller is here be able to avoid collisions between parent/child
425
426         PHY_IPhysicsController* parentctrl = NULL;
427         KX_BulletPhysicsController* parentKxCtrl = NULL;
428         CcdPhysicsController* ccdParent = NULL;
429
430         
431         if (destnode != destnode->GetRootSGParent())
432         {
433                 KX_GameObject* clientgameobj = (KX_GameObject*) destnode->GetRootSGParent()->GetSGClientObject();
434                 if (clientgameobj)
435                 {
436                         parentctrl = (KX_BulletPhysicsController*)clientgameobj->GetPhysicsController();
437                 } else
438                 {
439                         // it could be a false node, try the children
440                         NodeList::const_iterator childit;
441                         for (
442                                 childit = destnode->GetSGChildren().begin();
443                         childit!= destnode->GetSGChildren().end();
444                         ++childit
445                                 ) {
446                                 KX_GameObject *clientgameobj_child = static_cast<KX_GameObject*>( (*childit)->GetSGClientObject());
447                                 if (clientgameobj_child)
448                                 {
449                                         parentKxCtrl = (KX_BulletPhysicsController*)clientgameobj_child->GetPhysicsController();
450                                         parentctrl = parentKxCtrl;
451                                         ccdParent = parentKxCtrl;
452                                 }
453                         }
454                 }
455         }
456
457         physicsreplica->setParentCtrl(ccdParent);
458         physicsreplica->PostProcessReplica(motionstate,parentctrl);
459         physicsreplica->m_userdata = (PHY_IPhysicsController*)physicsreplica;
460         physicsreplica->m_bulletChildShape = NULL;
461         return physicsreplica;
462         
463 }
464
465
466
467 void    KX_BulletPhysicsController::SetSumoTransform(bool nondynaonly)
468 {
469
470         if (!m_bDyna && !m_bSensor)
471         {
472                 btCollisionObject* object = GetRigidBody();
473                 object->setActivationState(ACTIVE_TAG);
474                 object->setCollisionFlags(object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
475         } else
476         {
477                 if (!nondynaonly)
478                 {
479                         /*
480                         btTransform worldTrans;
481                         if (GetRigidBody())
482                         {
483                                 GetRigidBody()->getMotionState()->getWorldTransform(worldTrans);
484                                 GetRigidBody()->setCenterOfMassTransform(worldTrans);
485                         }
486                         */
487                         /*
488                         scaling?
489                         if (m_bDyna)
490                         {
491                                 m_sumoObj->setScaling(MT_Vector3(1,1,1));
492                         } else
493                         {
494                                 MT_Vector3 scale;
495                                 GetWorldScaling(scale);
496                                 m_sumoObj->setScaling(scale);
497                         }
498                         */
499
500                 }
501         }
502 }
503
504 // todo: remove next line !
505 void    KX_BulletPhysicsController::SetSimulatedTime(double time)
506 {
507 }
508         
509 // call from scene graph to update
510 bool KX_BulletPhysicsController::Update(double time)
511 {
512         return false;
513
514         // todo: check this code
515         //if (GetMass())
516         //{
517         //      return false;//true;
518 //      }
519 //      return false;
520 }
521
522 #endif //#ifdef USE_BULLET