Hopefully a working merge with trunk (could be one error left in raytrace.c - will...
[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_savedCollisionFilterGroup = handle->m_collisionFilterGroup;
365                 m_savedCollisionFilterMask = handle->m_collisionFilterMask;
366                 m_savedActivationState = body->getActivationState();
367                 body->forceActivationState(DISABLE_SIMULATION);
368                 GetPhysicsEnvironment()->updateCcdPhysicsController(this, 
369                         0.0,
370                         btCollisionObject::CF_STATIC_OBJECT|((ghost)?btCollisionObject::CF_NO_CONTACT_RESPONSE:(m_savedCollisionFlags&btCollisionObject::CF_NO_CONTACT_RESPONSE)),
371                         btBroadphaseProxy::StaticFilter, 
372                         btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
373         }
374 }
375
376 void    KX_BulletPhysicsController::RestoreDynamics()
377 {
378         btRigidBody *body = GetRigidBody();
379         if (body && body->getActivationState() == DISABLE_SIMULATION)
380         {
381                 GetPhysicsEnvironment()->updateCcdPhysicsController(this, 
382                         m_savedMass,
383                         m_savedCollisionFlags,
384                         m_savedCollisionFilterGroup,
385                         m_savedCollisionFilterMask);
386                 body->forceActivationState(m_savedActivationState);
387         }
388 }
389
390 SG_Controller*  KX_BulletPhysicsController::GetReplica(class SG_Node* destnode)
391 {
392         PHY_IMotionState* motionstate = new KX_MotionState(destnode);
393
394         KX_BulletPhysicsController* physicsreplica = new KX_BulletPhysicsController(*this);
395
396         //parentcontroller is here be able to avoid collisions between parent/child
397
398         PHY_IPhysicsController* parentctrl = NULL;
399         KX_BulletPhysicsController* parentKxCtrl = NULL;
400         CcdPhysicsController* ccdParent = NULL;
401
402         
403         if (destnode != destnode->GetRootSGParent())
404         {
405                 KX_GameObject* clientgameobj = (KX_GameObject*) destnode->GetRootSGParent()->GetSGClientObject();
406                 if (clientgameobj)
407                 {
408                         parentctrl = (KX_BulletPhysicsController*)clientgameobj->GetPhysicsController();
409                 } else
410                 {
411                         // it could be a false node, try the children
412                         NodeList::const_iterator childit;
413                         for (
414                                 childit = destnode->GetSGChildren().begin();
415                         childit!= destnode->GetSGChildren().end();
416                         ++childit
417                                 ) {
418                                 KX_GameObject *clientgameobj_child = static_cast<KX_GameObject*>( (*childit)->GetSGClientObject());
419                                 if (clientgameobj_child)
420                                 {
421                                         parentKxCtrl = (KX_BulletPhysicsController*)clientgameobj_child->GetPhysicsController();
422                                         parentctrl = parentKxCtrl;
423                                         ccdParent = parentKxCtrl;
424                                 }
425                         }
426                 }
427         }
428
429         physicsreplica->setParentCtrl(ccdParent);
430         physicsreplica->PostProcessReplica(motionstate,parentctrl);
431         physicsreplica->m_userdata = (PHY_IPhysicsController*)physicsreplica;
432         physicsreplica->m_bulletChildShape = NULL;
433         return physicsreplica;
434         
435 }
436
437
438
439 void    KX_BulletPhysicsController::SetSumoTransform(bool nondynaonly)
440 {
441         if (GetRigidBody())
442                 GetRigidBody()->activate(true);
443
444         if (!m_bDyna)
445         {
446                 GetCollisionObject()->setCollisionFlags(GetRigidBody()->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
447         } else
448         {
449                 if (!nondynaonly)
450                 {
451                         /*
452                         btTransform worldTrans;
453                         if (GetRigidBody())
454                         {
455                                 GetRigidBody()->getMotionState()->getWorldTransform(worldTrans);
456                                 GetRigidBody()->setCenterOfMassTransform(worldTrans);
457                         }
458                         */
459                         /*
460                         scaling?
461                         if (m_bDyna)
462                         {
463                                 m_sumoObj->setScaling(MT_Vector3(1,1,1));
464                         } else
465                         {
466                                 MT_Vector3 scale;
467                                 GetWorldScaling(scale);
468                                 m_sumoObj->setScaling(scale);
469                         }
470                         */
471
472                 }
473         }
474 }
475
476 // todo: remove next line !
477 void    KX_BulletPhysicsController::SetSimulatedTime(double time)
478 {
479 }
480         
481 // call from scene graph to update
482 bool KX_BulletPhysicsController::Update(double time)
483 {
484         return false;
485
486         // todo: check this code
487         //if (GetMass())
488         //{
489         //      return false;//true;
490 //      }
491 //      return false;
492 }
493
494 #endif //#ifdef USE_BULLET