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