BGE bug #18091: Hitbox of object X doesn't move along when object X is parented to...
[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 compound)
21 : KX_IPhysicsController(dyna,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
96 }
97
98 MT_Scalar KX_BulletPhysicsController::GetRadius()
99 {
100         return MT_Scalar(CcdPhysicsController::GetRadius());
101 }
102
103 void    KX_BulletPhysicsController::setMargin (float collisionMargin)
104 {
105         CcdPhysicsController::SetMargin(collisionMargin);
106 }
107 void    KX_BulletPhysicsController::RelativeTranslate(const MT_Vector3& dloc,bool local)
108 {
109         CcdPhysicsController::RelativeTranslate(dloc[0],dloc[1],dloc[2],local);
110
111 }
112
113 void    KX_BulletPhysicsController::RelativeRotate(const MT_Matrix3x3& drot,bool local)
114 {
115         float   rotval[12];
116         drot.getValue(rotval);
117         CcdPhysicsController::RelativeRotate(rotval,local);
118 }
119
120 void    KX_BulletPhysicsController::ApplyTorque(const MT_Vector3& torque,bool local)
121 {
122                 CcdPhysicsController::ApplyTorque(torque.x(),torque.y(),torque.z(),local);
123 }
124 void    KX_BulletPhysicsController::ApplyForce(const MT_Vector3& force,bool local)
125 {
126         CcdPhysicsController::ApplyForce(force.x(),force.y(),force.z(),local);
127 }
128 MT_Vector3 KX_BulletPhysicsController::GetLinearVelocity()
129 {
130         float angVel[3];
131         //CcdPhysicsController::GetAngularVelocity(angVel[0],angVel[1],angVel[2]);
132         CcdPhysicsController::GetLinearVelocity(angVel[0],angVel[1],angVel[2]);//rcruiz
133         return MT_Vector3(angVel[0],angVel[1],angVel[2]);
134 }
135 MT_Vector3 KX_BulletPhysicsController::GetAngularVelocity()
136 {
137         float angVel[3];
138         //CcdPhysicsController::GetAngularVelocity(angVel[0],angVel[1],angVel[2]);
139         CcdPhysicsController::GetAngularVelocity(angVel[0],angVel[1],angVel[2]);//rcruiz
140         return MT_Vector3(angVel[0],angVel[1],angVel[2]);
141 }
142 MT_Vector3 KX_BulletPhysicsController::GetVelocity(const MT_Point3& pos)
143 {
144         float linVel[3];
145         CcdPhysicsController::GetVelocity(pos[0], pos[1], pos[2], linVel[0],linVel[1],linVel[2]);
146         return MT_Vector3(linVel[0],linVel[1],linVel[2]);
147 }
148
149 void    KX_BulletPhysicsController::SetAngularVelocity(const MT_Vector3& ang_vel,bool local)
150 {
151         CcdPhysicsController::SetAngularVelocity(ang_vel.x(),ang_vel.y(),ang_vel.z(),local);
152
153 }
154 void    KX_BulletPhysicsController::SetLinearVelocity(const MT_Vector3& lin_vel,bool local)
155 {
156         CcdPhysicsController::SetLinearVelocity(lin_vel.x(),lin_vel.y(),lin_vel.z(),local);
157 }
158 void    KX_BulletPhysicsController::getOrientation(MT_Quaternion& orn)
159 {
160         float myorn[4];
161         CcdPhysicsController::getOrientation(myorn[0],myorn[1],myorn[2],myorn[3]);
162         orn = MT_Quaternion(myorn[0],myorn[1],myorn[2],myorn[3]);
163 }
164 void KX_BulletPhysicsController::setOrientation(const MT_Matrix3x3& orn)
165 {
166         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]);
167         CcdPhysicsController::setWorldOrientation(btmat);
168 }
169 void KX_BulletPhysicsController::setPosition(const MT_Point3& pos)
170 {
171         CcdPhysicsController::setPosition(pos.x(),pos.y(),pos.z());
172 }
173 void KX_BulletPhysicsController::setScaling(const MT_Vector3& scaling)
174 {
175         CcdPhysicsController::setScaling(scaling.x(),scaling.y(),scaling.z());
176 }
177 MT_Scalar       KX_BulletPhysicsController::GetMass()
178 {
179         if (GetSoftBody())
180                 return GetSoftBody()->getTotalMass();
181         
182         MT_Scalar invmass = 0.f;
183         if (GetRigidBody())
184                 invmass = GetRigidBody()->getInvMass();
185         if (invmass)
186                 return 1.f/invmass;
187         return 0.f;
188
189 }
190
191 MT_Vector3 KX_BulletPhysicsController::GetLocalInertia()
192 {
193     MT_Vector3 inertia(0.f, 0.f, 0.f);
194     btVector3 inv_inertia;
195     if (GetRigidBody()) {
196         inv_inertia = GetRigidBody()->getInvInertiaDiagLocal();
197                 if (!btFuzzyZero(inv_inertia.getX()) && 
198                         !btFuzzyZero(inv_inertia.getY()) && 
199                         !btFuzzyZero(inv_inertia.getZ()))
200                         inertia = MT_Vector3(1.f/inv_inertia.getX(), 1.f/inv_inertia.getY(), 1.f/inv_inertia.getZ());
201     }
202     return inertia;
203 }
204
205 MT_Vector3      KX_BulletPhysicsController::getReactionForce()
206 {
207         assert(0);
208         return MT_Vector3(0.f,0.f,0.f);
209 }
210 void    KX_BulletPhysicsController::setRigidBody(bool rigid)
211 {
212 }
213
214 /* This function dynamically adds the collision shape of another controller to
215    the current controller shape provided it is a compound shape.
216    The idea is that dynamic parenting on a compound object will dynamically extend the shape
217 */
218 void    KX_BulletPhysicsController::AddCompoundChild(KX_IPhysicsController* child)
219
220         if (child == NULL || !IsCompound())
221                 return;
222         // other controller must be a bullet controller too
223         // verify that body and shape exist and match
224         KX_BulletPhysicsController* childCtrl = dynamic_cast<KX_BulletPhysicsController*>(child);
225         btRigidBody* rootBody = GetRigidBody();
226         btRigidBody* childBody = childCtrl->GetRigidBody();
227         if (!rootBody || !childBody)
228                 return;
229         const btCollisionShape* rootShape = rootBody->getCollisionShape();
230         const btCollisionShape* childShape = childBody->getCollisionShape();
231         if (!rootShape || 
232                 !childShape || 
233                 rootShape->getShapeType() != COMPOUND_SHAPE_PROXYTYPE ||
234                 childShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
235                 return;
236         btCompoundShape* compoundShape = (btCompoundShape*)rootShape;
237         // compute relative transformation between parent and child
238         btTransform rootTrans;
239         btTransform childTrans;
240         rootBody->getMotionState()->getWorldTransform(rootTrans);
241         childBody->getMotionState()->getWorldTransform(childTrans);
242         btVector3 rootScale = rootShape->getLocalScaling();
243         rootScale[0] = 1.0/rootScale[0];
244         rootScale[1] = 1.0/rootScale[1];
245         rootScale[2] = 1.0/rootScale[2];
246         // relative scale = child_scale/parent_scale
247         btVector3 relativeScale = childShape->getLocalScaling()*rootScale;
248         btMatrix3x3 rootRotInverse = rootTrans.getBasis().transpose();  
249         // relative pos = parent_rot^-1 * ((parent_pos-child_pos)/parent_scale)
250         btVector3 relativePos = rootRotInverse*((childTrans.getOrigin()-rootTrans.getOrigin())*rootScale);
251         // relative rot = parent_rot^-1 * child_rot
252         btMatrix3x3 relativeRot = rootRotInverse*childTrans.getBasis();
253         // create a proxy shape info to store the transformation
254         CcdShapeConstructionInfo* proxyShapeInfo = new CcdShapeConstructionInfo();
255         // store the transformation to this object shapeinfo
256         proxyShapeInfo->m_childTrans.setOrigin(relativePos);
257         proxyShapeInfo->m_childTrans.setBasis(relativeRot);
258         proxyShapeInfo->m_childScale.setValue(relativeScale[0], relativeScale[1], relativeScale[2]);
259         // we will need this to make sure that we remove the right proxy later when unparenting
260         proxyShapeInfo->m_userData = childCtrl;
261         proxyShapeInfo->SetProxy(childCtrl->GetShapeInfo()->AddRef());
262         // add to parent compound shapeinfo
263         GetShapeInfo()->AddShape(proxyShapeInfo);
264         // create new bullet collision shape from the object shapeinfo and set scaling
265         btCollisionShape* newChildShape = proxyShapeInfo->CreateBulletShape();
266         newChildShape->setLocalScaling(relativeScale);
267         // add bullet collision shape to parent compound collision shape
268         compoundShape->addChildShape(proxyShapeInfo->m_childTrans,newChildShape);
269         // remember we created this shape
270         childCtrl->m_bulletChildShape = newChildShape;
271         // recompute inertia of parent
272         if (!rootBody->isStaticOrKinematicObject())
273         {
274                 btVector3 localInertia;
275                 float mass = 1.f/rootBody->getInvMass();
276                 compoundShape->calculateLocalInertia(mass,localInertia);
277                 rootBody->setMassProps(mass,localInertia);
278         }
279         // must update the broadphase cache,
280         GetPhysicsEnvironment()->refreshCcdPhysicsController(this);
281         // remove the children
282         GetPhysicsEnvironment()->disableCcdPhysicsController(childCtrl);
283 }
284
285 /* Reverse function of the above, it will remove a shape from a compound shape
286    provided that the former was added to the later using  AddCompoundChild()
287 */
288 void    KX_BulletPhysicsController::RemoveCompoundChild(KX_IPhysicsController* child)
289
290         if (child == NULL || !IsCompound())
291                 return;
292         // other controller must be a bullet controller too
293         // verify that body and shape exist and match
294         KX_BulletPhysicsController* childCtrl = dynamic_cast<KX_BulletPhysicsController*>(child);
295         btRigidBody* rootBody = GetRigidBody();
296         btRigidBody* childBody = childCtrl->GetRigidBody();
297         if (!rootBody || !childBody)
298                 return;
299         const btCollisionShape* rootShape = rootBody->getCollisionShape();
300         if (!rootShape || 
301                 rootShape->getShapeType() != COMPOUND_SHAPE_PROXYTYPE)
302                 return;
303         btCompoundShape* compoundShape = (btCompoundShape*)rootShape;
304         // retrieve the shapeInfo
305         CcdShapeConstructionInfo* childShapeInfo = childCtrl->GetShapeInfo();
306         CcdShapeConstructionInfo* rootShapeInfo = GetShapeInfo();
307         // and verify that the child is part of the parent
308         int i = rootShapeInfo->FindChildShape(childShapeInfo, childCtrl);
309         if (i < 0)
310                 return;
311         rootShapeInfo->RemoveChildShape(i);
312         if (childCtrl->m_bulletChildShape)
313         {
314                 int numChildren = compoundShape->getNumChildShapes();
315                 for (i=0; i<numChildren; i++)
316                 {
317                         if (compoundShape->getChildShape(i) == childCtrl->m_bulletChildShape)
318                         {
319                                 compoundShape->removeChildShapeByIndex(i);
320                                 compoundShape->recalculateLocalAabb();
321                                 break;
322                         }
323                 }
324                 delete childCtrl->m_bulletChildShape;
325                 childCtrl->m_bulletChildShape = NULL;
326         }
327         // recompute inertia of parent
328         if (!rootBody->isStaticOrKinematicObject())
329         {
330                 btVector3 localInertia;
331                 float mass = 1.f/rootBody->getInvMass();
332                 compoundShape->calculateLocalInertia(mass,localInertia);
333                 rootBody->setMassProps(mass,localInertia);
334         }
335         // must update the broadphase cache,
336         GetPhysicsEnvironment()->refreshCcdPhysicsController(this);
337         // reactivate the children
338         GetPhysicsEnvironment()->enableCcdPhysicsController(childCtrl);
339 }
340
341 void KX_BulletPhysicsController::SetMass(MT_Scalar newmass)
342 {
343         btRigidBody *body = GetRigidBody();
344         if (body && !m_suspended && newmass>MT_EPSILON && GetMass()>MT_EPSILON)
345         {
346                 btVector3 grav = body->getGravity();
347                 btVector3 accel = grav / GetMass();
348                 
349                 btBroadphaseProxy* handle = body->getBroadphaseHandle();
350                 GetPhysicsEnvironment()->updateCcdPhysicsController(this, 
351                         newmass,
352                         body->getCollisionFlags(),
353                         handle->m_collisionFilterGroup, 
354                         handle->m_collisionFilterMask);
355                 body->setGravity(accel);
356         }
357 }
358
359 void    KX_BulletPhysicsController::SuspendDynamics(bool ghost)
360 {
361         btRigidBody *body = GetRigidBody();
362         if (body && !m_suspended)
363         {
364                 btBroadphaseProxy* handle = body->getBroadphaseHandle();
365                 m_savedCollisionFlags = body->getCollisionFlags();
366                 m_savedMass = GetMass();
367                 m_savedDyna = m_bDyna;
368                 m_savedCollisionFilterGroup = handle->m_collisionFilterGroup;
369                 m_savedCollisionFilterMask = handle->m_collisionFilterMask;
370                 m_suspended = true;
371                 GetPhysicsEnvironment()->updateCcdPhysicsController(this, 
372                         0.0,
373                         btCollisionObject::CF_STATIC_OBJECT|((ghost)?btCollisionObject::CF_NO_CONTACT_RESPONSE:(m_savedCollisionFlags&btCollisionObject::CF_NO_CONTACT_RESPONSE)),
374                         btBroadphaseProxy::StaticFilter, 
375                         btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
376                 m_bDyna = false;
377         }
378 }
379
380 void    KX_BulletPhysicsController::RestoreDynamics()
381 {
382         btRigidBody *body = GetRigidBody();
383         if (body && m_suspended)
384         {
385                 GetPhysicsEnvironment()->updateCcdPhysicsController(this, 
386                         m_savedMass,
387                         m_savedCollisionFlags,
388                         m_savedCollisionFilterGroup,
389                         m_savedCollisionFilterMask);
390                 body->activate();
391                 m_bDyna = m_savedDyna;
392                 m_suspended = false;
393         }
394 }
395
396 SG_Controller*  KX_BulletPhysicsController::GetReplica(class SG_Node* destnode)
397 {
398         PHY_IMotionState* motionstate = new KX_MotionState(destnode);
399
400         KX_BulletPhysicsController* physicsreplica = new KX_BulletPhysicsController(*this);
401
402         //parentcontroller is here be able to avoid collisions between parent/child
403
404         PHY_IPhysicsController* parentctrl = NULL;
405         KX_BulletPhysicsController* parentKxCtrl = NULL;
406         CcdPhysicsController* ccdParent = NULL;
407
408         
409         if (destnode != destnode->GetRootSGParent())
410         {
411                 KX_GameObject* clientgameobj = (KX_GameObject*) destnode->GetRootSGParent()->GetSGClientObject();
412                 if (clientgameobj)
413                 {
414                         parentctrl = (KX_BulletPhysicsController*)clientgameobj->GetPhysicsController();
415                 } else
416                 {
417                         // it could be a false node, try the children
418                         NodeList::const_iterator childit;
419                         for (
420                                 childit = destnode->GetSGChildren().begin();
421                         childit!= destnode->GetSGChildren().end();
422                         ++childit
423                                 ) {
424                                 KX_GameObject *clientgameobj_child = static_cast<KX_GameObject*>( (*childit)->GetSGClientObject());
425                                 if (clientgameobj_child)
426                                 {
427                                         parentKxCtrl = (KX_BulletPhysicsController*)clientgameobj_child->GetPhysicsController();
428                                         parentctrl = parentKxCtrl;
429                                         ccdParent = parentKxCtrl;
430                                 }
431                         }
432                 }
433         }
434
435         physicsreplica->setParentCtrl(ccdParent);
436         physicsreplica->PostProcessReplica(motionstate,parentctrl);
437         physicsreplica->m_userdata = (PHY_IPhysicsController*)physicsreplica;
438         physicsreplica->m_bulletChildShape = NULL;
439         return physicsreplica;
440         
441 }
442
443
444
445 void    KX_BulletPhysicsController::SetSumoTransform(bool nondynaonly)
446 {
447
448         if (!m_bDyna)
449         {
450                 btCollisionObject* object = GetRigidBody();
451                 object->setActivationState(ACTIVE_TAG);
452                 object->setCollisionFlags(object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
453         } else
454         {
455                 if (!nondynaonly)
456                 {
457                         /*
458                         btTransform worldTrans;
459                         if (GetRigidBody())
460                         {
461                                 GetRigidBody()->getMotionState()->getWorldTransform(worldTrans);
462                                 GetRigidBody()->setCenterOfMassTransform(worldTrans);
463                         }
464                         */
465                         /*
466                         scaling?
467                         if (m_bDyna)
468                         {
469                                 m_sumoObj->setScaling(MT_Vector3(1,1,1));
470                         } else
471                         {
472                                 MT_Vector3 scale;
473                                 GetWorldScaling(scale);
474                                 m_sumoObj->setScaling(scale);
475                         }
476                         */
477
478                 }
479         }
480 }
481
482 // todo: remove next line !
483 void    KX_BulletPhysicsController::SetSimulatedTime(double time)
484 {
485 }
486         
487 // call from scene graph to update
488 bool KX_BulletPhysicsController::Update(double time)
489 {
490         return false;
491
492         // todo: check this code
493         //if (GetMass())
494         //{
495         //      return false;//true;
496 //      }
497 //      return false;
498 }
499
500 #endif //#ifdef USE_BULLET