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