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