BGE patch: dynamically update the coumpound parent shape when parenting to a compound object.

This patch modifies the way the setParent actuator and KX_GameObject::setParent() function
works when parenting to a compound object: the collision shape of the object being parented
is dynamically added to the coumpound shape. 
Similarly, unparenting an object from a compound object will cause the child collision shape
to be dynamically removed from the parent shape provided that is was previously added with 
setParent.

Note: * This also works if the object is parented to a child of a compound object: the
        collision shape is added to the compound shape of the top parent.
      * The collision shape is added with the transformation (position, scale and orientation)
        it had at the time of the parenting.
      * The child shape is rigidly attached to the compound shape, the transformation is not
        affected by any further change in position/scale/orientation of the child object.
      * While the child shape is added to the compound shape, the child object is removed from
        the dynamic world to avoid superposition of shapes (one for the object itself and
        one for the compound child shape). This means that collision sensors on the child
        object are disabled while the child object is parent to a compound object.
      * There is no difference when setParent is used on a non-compound object: the child
        object is automatically changed to a static ghost object to avoid bad interaction
        with the parent shape; collision sensors on the child object continue to be active
        while the object is parented.
      * The child shape dynamically added to a compound shape modifies the inertia of the
        compound object but not the mass. It participates to collision detection as any other
        "static" child shape.

source/gameengine/Ketsji/KX_BulletPhysicsController.cpp

@@ -17,10 +17,11 @@
 #include "BulletSoftBody/btSoftBody.h"
 
 
-KX_BulletPhysicsController::KX_BulletPhysicsController (const CcdConstructionInfo& ci, bool dyna)
-: KX_IPhysicsController(dyna,(PHY_IPhysicsController*)this),
+KX_BulletPhysicsController::KX_BulletPhysicsController (const CcdConstructionInfo& ci, bool dyna, bool compound)
+: KX_IPhysicsController(dyna,compound,(PHY_IPhysicsController*)this),
 CcdPhysicsController(ci),
-m_savedCollisionFlags(0)
+m_savedCollisionFlags(0),
+m_bulletChildShape(NULL)
 {
 
 }
@@ -175,6 +176,133 @@ void	KX_BulletPhysicsController::setRigidBody(bool rigid)
 {
 }
 
+/* This function dynamically adds the collision shape of another controller to
+   the current controller shape provided it is a compound shape.
+   The idea is that dynamic parenting on a compound object will dynamically extend the shape
+*/
+void    KX_BulletPhysicsController::AddCompoundChild(KX_IPhysicsController* child)
+{ 
+	if (child == NULL || !IsCompound())
+		return;
+	// other controller must be a bullet controller too
+	// verify that body and shape exist and match
+	KX_BulletPhysicsController* childCtrl = dynamic_cast<KX_BulletPhysicsController*>(child);
+	btRigidBody* rootBody = GetRigidBody();
+	btRigidBody* childBody = childCtrl->GetRigidBody();
+	if (!rootBody || !childBody)
+		return;
+	const btCollisionShape* rootShape = rootBody->getCollisionShape();
+	const btCollisionShape* childShape = childBody->getCollisionShape();
+	if (!rootShape || 
+		!childShape || 
+		rootShape->getShapeType() != COMPOUND_SHAPE_PROXYTYPE ||
+		childShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
+		return;
+	btCompoundShape* compoundShape = (btCompoundShape*)rootShape;
+	// compute relative transformation between parent and child
+	btTransform rootTrans;
+	btTransform childTrans;
+	rootBody->getMotionState()->getWorldTransform(rootTrans);
+	childBody->getMotionState()->getWorldTransform(childTrans);
+	btVector3 rootScale = rootShape->getLocalScaling();
+	rootScale[0] = 1.0/rootScale[0];
+	rootScale[1] = 1.0/rootScale[1];
+	rootScale[2] = 1.0/rootScale[2];
+	// relative scale = child_scale/parent_scale
+	btVector3 relativeScale = childShape->getLocalScaling()*rootScale;
+	btMatrix3x3 rootRotInverse = rootTrans.getBasis().transpose();	
+	// relative pos = parent_rot^-1 * ((parent_pos-child_pos)/parent_scale)
+	btVector3 relativePos = rootRotInverse*((childTrans.getOrigin()-rootTrans.getOrigin())*rootScale);
+	// relative rot = parent_rot^-1 * child_rot
+	btMatrix3x3 relativeRot = rootRotInverse*childTrans.getBasis();
+	// create a proxy shape info to store the transformation
+	CcdShapeConstructionInfo* proxyShapeInfo = new CcdShapeConstructionInfo();
+	// store the transformation to this object shapeinfo
+	proxyShapeInfo->m_childTrans.setOrigin(relativePos);
+	proxyShapeInfo->m_childTrans.setBasis(relativeRot);
+	proxyShapeInfo->m_childScale.setValue(relativeScale[0], relativeScale[1], relativeScale[2]);
+	// we will need this to make sure that we remove the right proxy later when unparenting
+	proxyShapeInfo->m_userData = childCtrl;
+	proxyShapeInfo->SetProxy(childCtrl->GetShapeInfo()->AddRef());
+	// add to parent compound shapeinfo
+	GetShapeInfo()->AddShape(proxyShapeInfo);
+	// create new bullet collision shape from the object shapeinfo and set scaling
+	btCollisionShape* newChildShape = proxyShapeInfo->CreateBulletShape();
+	newChildShape->setLocalScaling(relativeScale);
+	// add bullet collision shape to parent compound collision shape
+	compoundShape->addChildShape(proxyShapeInfo->m_childTrans,newChildShape);
+	// remember we created this shape
+	childCtrl->m_bulletChildShape = newChildShape;
+	// recompute inertia of parent
+	if (!rootBody->isStaticOrKinematicObject())
+	{
+		btVector3 localInertia;
+		float mass = 1.f/rootBody->getInvMass();
+		compoundShape->calculateLocalInertia(mass,localInertia);
+		rootBody->setMassProps(mass,localInertia);
+	}
+	// must update the broadphase cache,
+	GetPhysicsEnvironment()->refreshCcdPhysicsController(this);
+	// remove the children
+	GetPhysicsEnvironment()->disableCcdPhysicsController(childCtrl);
+}
+
+/* Reverse function of the above, it will remove a shape from a compound shape
+   provided that the former was added to the later using  AddCompoundChild()
+*/
+void    KX_BulletPhysicsController::RemoveCompoundChild(KX_IPhysicsController* child)
+{ 
+	if (child == NULL || !IsCompound())
+		return;
+	// other controller must be a bullet controller too
+	// verify that body and shape exist and match
+	KX_BulletPhysicsController* childCtrl = dynamic_cast<KX_BulletPhysicsController*>(child);
+	btRigidBody* rootBody = GetRigidBody();
+	btRigidBody* childBody = childCtrl->GetRigidBody();
+	if (!rootBody || !childBody)
+		return;
+	const btCollisionShape* rootShape = rootBody->getCollisionShape();
+	if (!rootShape || 
+		rootShape->getShapeType() != COMPOUND_SHAPE_PROXYTYPE)
+		return;
+	btCompoundShape* compoundShape = (btCompoundShape*)rootShape;
+	// retrieve the shapeInfo
+	CcdShapeConstructionInfo* childShapeInfo = childCtrl->GetShapeInfo();
+	CcdShapeConstructionInfo* rootShapeInfo = GetShapeInfo();
+	// and verify that the child is part of the parent
+	int i = rootShapeInfo->FindChildShape(childShapeInfo, childCtrl);
+	if (i < 0)
+		return;
+	rootShapeInfo->RemoveChildShape(i);
+	if (childCtrl->m_bulletChildShape)
+	{
+		int numChildren = compoundShape->getNumChildShapes();
+		for (i=0; i<numChildren; i++)
+		{
+			if (compoundShape->getChildShape(i) == childCtrl->m_bulletChildShape)
+			{
+				compoundShape->removeChildShapeByIndex(i);
+				compoundShape->recalculateLocalAabb();
+				break;
+			}
+		}
+		delete childCtrl->m_bulletChildShape;
+		childCtrl->m_bulletChildShape = NULL;
+	}
+	// recompute inertia of parent
+	if (!rootBody->isStaticOrKinematicObject())
+	{
+		btVector3 localInertia;
+		float mass = 1.f/rootBody->getInvMass();
+		compoundShape->calculateLocalInertia(mass,localInertia);
+		rootBody->setMassProps(mass,localInertia);
+	}
+	// must update the broadphase cache,
+	GetPhysicsEnvironment()->refreshCcdPhysicsController(this);
+	// reactivate the children
+	GetPhysicsEnvironment()->enableCcdPhysicsController(childCtrl);
+}
+
 void	KX_BulletPhysicsController::SuspendDynamics(bool ghost)
 {
 	btRigidBody *body = GetRigidBody();
@@ -251,6 +379,7 @@ SG_Controller*	KX_BulletPhysicsController::GetReplica(class SG_Node* destnode)
 	physicsreplica->setParentCtrl(ccdParent);
 	physicsreplica->PostProcessReplica(motionstate,parentctrl);
 	physicsreplica->m_userdata = (PHY_IPhysicsController*)physicsreplica;
+	physicsreplica->m_bulletChildShape = NULL;
 	return physicsreplica;
 	
 }

source/gameengine/Ketsji/KX_BulletPhysicsController.h

@@ -13,10 +13,11 @@ private:
 	short int m_savedCollisionFilterGroup;
 	short int m_savedCollisionFilterMask;
 	MT_Scalar m_savedMass;
+	btCollisionShape* m_bulletChildShape;
 
 public:
 
-	KX_BulletPhysicsController (const CcdConstructionInfo& ci, bool dyna);
+	KX_BulletPhysicsController (const CcdConstructionInfo& ci, bool dyna, bool compound);
 	virtual ~KX_BulletPhysicsController ();
 
 	///////////////////////////////////
@@ -42,6 +43,8 @@ public:
 	virtual	MT_Scalar	GetMass();
 	virtual	MT_Vector3	getReactionForce();
 	virtual void	setRigidBody(bool rigid);
+	virtual void    AddCompoundChild(KX_IPhysicsController* child);
+	virtual void    RemoveCompoundChild(KX_IPhysicsController* child);
 
 	virtual void	resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ);
 

source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp

@@ -1084,7 +1084,7 @@ void	KX_ConvertBulletObject(	class	KX_GameObject* gameobj,
 	ci.m_bSoft = objprop->m_softbody;
 	MT_Vector3 scaling = gameobj->NodeGetWorldScaling();
 	ci.m_scaling.setValue(scaling[0], scaling[1], scaling[2]);
-	KX_BulletPhysicsController* physicscontroller = new KX_BulletPhysicsController(ci,isbulletdyna);
+	KX_BulletPhysicsController* physicscontroller = new KX_BulletPhysicsController(ci,isbulletdyna,objprop->m_hasCompoundChildren);
 	// shapeInfo is reference counted, decrement now as we don't use it anymore
 	if (shapeInfo)
 		shapeInfo->Release();

source/gameengine/Ketsji/KX_GameObject.cpp

@@ -252,6 +252,20 @@ void KX_GameObject::SetParent(KX_Scene *scene, KX_GameObject* obj)
 		if (rootlist->RemoveValue(this))
 			// the object was in parent list, decrement ref count as it's now removed
 			Release();
+		// if the new parent is a compound object, add this object shape to the compound shape.
+		// step 0: verify this object has physical controller
+		if (m_pPhysicsController1)
+		{
+			// step 1: find the top parent (not necessarily obj)
+			KX_GameObject* rootobj = (KX_GameObject*)obj->GetSGNode()->GetRootSGParent()->GetSGClientObject();
+			// step 2: verify it has a physical controller and compound shape
+			if (rootobj != NULL && 
+				rootobj->m_pPhysicsController1 != NULL &&
+				rootobj->m_pPhysicsController1->IsCompound())
+			{
+				rootobj->m_pPhysicsController1->AddCompoundChild(m_pPhysicsController1);
+			}
+		}
 	}
 }
 
@@ -260,6 +274,8 @@ void KX_GameObject::RemoveParent(KX_Scene *scene)
 	// check on valid node in case a python controller holds a reference to a deleted object
 	if (GetSGNode() && GetSGNode()->GetSGParent())
 	{
+		// get the root object to remove us from compound object if needed
+		KX_GameObject* rootobj = (KX_GameObject*)GetSGNode()->GetRootSGParent()->GetSGClientObject();
 		// Set us to the right spot 
 		GetSGNode()->SetLocalScale(GetSGNode()->GetWorldScaling());
 		GetSGNode()->SetLocalOrientation(GetSGNode()->GetWorldOrientation());
@@ -275,6 +291,13 @@ void KX_GameObject::RemoveParent(KX_Scene *scene)
 			rootlist->Add(AddRef());
 		if (m_pPhysicsController1) 
 		{
+			// in case this controller was added as a child shape to the parent
+			if (rootobj != NULL && 
+				rootobj->m_pPhysicsController1 != NULL &&
+				rootobj->m_pPhysicsController1->IsCompound())
+			{
+				rootobj->m_pPhysicsController1->RemoveCompoundChild(m_pPhysicsController1);
+			}
 			m_pPhysicsController1->RestoreDynamics();
 		}
 	}

source/gameengine/Ketsji/KX_IPhysicsController.cpp

@@ -35,9 +35,10 @@
 
 #include "PHY_DynamicTypes.h"
 
-KX_IPhysicsController::KX_IPhysicsController(bool dyna,void* userdata)
+KX_IPhysicsController::KX_IPhysicsController(bool dyna, bool compound, void* userdata)
 
 :	m_bDyna(dyna),
+	m_bCompound(compound),
 	m_suspendDynamics(false),
 	m_userdata(userdata)
 {

source/gameengine/Ketsji/KX_IPhysicsController.h

@@ -32,6 +32,7 @@
 #include "SG_Controller.h"
 #include "MT_Vector3.h"
 #include "MT_Point3.h"
+#include "MT_Transform.h"
 #include "MT_Matrix3x3.h"
 
 struct KX_ClientObjectInfo;
@@ -48,10 +49,11 @@ class KX_IPhysicsController : public SG_Controller
 {
 protected:
 	bool		m_bDyna;
+	bool		m_bCompound;
 	bool		m_suspendDynamics;
 	void*		m_userdata;
 public:
-	KX_IPhysicsController(bool dyna,void* userdata);
+	KX_IPhysicsController(bool dyna,bool compound, void* userdata);
 	virtual ~KX_IPhysicsController();
 
 
@@ -78,6 +80,8 @@ public:
 	virtual	MT_Scalar	GetMass()=0;
 	virtual	MT_Vector3	getReactionForce()=0;
 	virtual void	setRigidBody(bool rigid)=0;
+	virtual void    AddCompoundChild(KX_IPhysicsController* child) = 0;
+	virtual void    RemoveCompoundChild(KX_IPhysicsController* child) = 0;
 
 	virtual void	SuspendDynamics(bool ghost=false)=0;
 	virtual void	RestoreDynamics()=0;
@@ -92,6 +96,10 @@ public:
 		return m_bDyna;
 	}
 
+	bool	IsCompound(void) {
+		return m_bCompound;
+	}
+
 	virtual MT_Scalar GetRadius()=0;
 	virtual void	SetSumoTransform(bool nondynaonly)=0;
 	// todo: remove next line !

source/gameengine/Ketsji/KX_OdePhysicsController.cpp

@@ -50,7 +50,7 @@ KX_OdePhysicsController::KX_OdePhysicsController(
 												 float	extends[3],
 												 float	radius
 												 ) 
-: KX_IPhysicsController(dyna,(PHY_IPhysicsController*)this),
+: KX_IPhysicsController(dyna,false,(PHY_IPhysicsController*)this),
 ODEPhysicsController(
 dyna,fullRigidBody,phantom,motionstate,
 space,world,mass,friction,restitution,

source/gameengine/Ketsji/KX_OdePhysicsController.h

@@ -73,6 +73,8 @@ public:
 	virtual	MT_Scalar	GetMass();
 	virtual	MT_Vector3	getReactionForce();
 	virtual void	setRigidBody(bool rigid);
+	virtual void    AddCompoundChild(KX_IPhysicsController* child) { }
+	virtual void    RemoveCompoundChild(KX_IPhysicsController* child) { }
 
 	virtual void	SuspendDynamics(bool);
 	virtual void	RestoreDynamics();

source/gameengine/Ketsji/KX_SumoPhysicsController.h

@@ -53,7 +53,7 @@ public:
 		class SM_Object* sumoObj,	
 		class PHY_IMotionState* motionstate
 		,bool dyna) 
-		: KX_IPhysicsController(dyna,NULL) ,
+		: KX_IPhysicsController(dyna,false,NULL) ,
 		  SumoPhysicsController(sumoScene,/*solidscene,*/sumoObj,motionstate,dyna)
 	{
 	};
@@ -78,6 +78,9 @@ public:
 
 	void	SuspendDynamics(bool);
 	void	RestoreDynamics();
+	virtual void    AddCompoundChild(KX_IPhysicsController* child) { }
+	virtual void    RemoveCompoundChild(KX_IPhysicsController* child) { }
+
 	virtual	void	getOrientation(MT_Quaternion& orn);
 	virtual	void setOrientation(const MT_Matrix3x3& orn);
 	

source/gameengine/Physics/Bullet/CcdPhysicsController.cpp

@@ -1276,7 +1276,7 @@ bool CcdShapeConstructionInfo::SetMesh(RAS_MeshObject* meshobj, bool polytope,bo
 
 	// assume no shape information
 	// no support for dynamic change of shape yet
-	assert(m_meshObject == NULL);
+	assert(IsUnused());
 	m_shapeType = PHY_SHAPE_NONE;
 	m_vertexArray.clear();
 	m_polygonIndexArray.clear();
@@ -1398,6 +1398,17 @@ bool CcdShapeConstructionInfo::SetMesh(RAS_MeshObject* meshobj, bool polytope,bo
 	return true;
 }
 
+bool CcdShapeConstructionInfo::SetProxy(CcdShapeConstructionInfo* shapeInfo)
+{
+	if (shapeInfo == NULL)
+		return false;
+	// no support for dynamic change
+	assert(IsUnused());
+	m_shapeType = PHY_SHAPE_PROXY;
+	m_shapeProxy = shapeInfo;
+	return true;
+}
+
 btCollisionShape* CcdShapeConstructionInfo::CreateBulletShape()
 {
 	btCollisionShape* collisionShape = 0;
@@ -1406,9 +1417,12 @@ btCollisionShape* CcdShapeConstructionInfo::CreateBulletShape()
 	btCompoundShape* compoundShape = 0;
 	CcdShapeConstructionInfo* nextShapeInfo;
 
+	if (m_shapeType == PHY_SHAPE_PROXY && m_shapeProxy != NULL)
+		return m_shapeProxy->CreateBulletShape();
+
 	switch (m_shapeType) 
 	{
-	case PHY_SHAPE_NONE:
+	default:
 		break;
 
 	case PHY_SHAPE_BOX:
@@ -1522,6 +1536,10 @@ CcdShapeConstructionInfo::~CcdShapeConstructionInfo()
 			m_meshShapeMap.erase(mit);
 		}
 	}
+	if (m_shapeType == PHY_SHAPE_PROXY && m_shapeProxy != NULL)
+	{
+		m_shapeProxy->Release();
+	}
 }
 
 

source/gameengine/Physics/Bullet/CcdPhysicsController.h

@@ -67,11 +67,13 @@ public:
 		m_height(1.0),
 		m_halfExtend(0.f,0.f,0.f),
 		m_childScale(1.0f,1.0f,1.0f),
+		m_userData(NULL),
 		m_refCount(1),
 		m_meshObject(NULL),
 		m_unscaledShape(NULL),
 		m_useGimpact(false),
-		m_weldingThreshold(0.f)
+		m_weldingThreshold(0.f),
+		m_shapeProxy(NULL)
 	{
 		m_childTrans.setIdentity();
 	}
@@ -92,6 +94,11 @@ public:
 		return 0;
 	}
 
+	bool IsUnused(void)
+	{
+		return (m_meshObject==NULL && m_shapeArray.size() == 0 && m_shapeProxy == NULL);
+	}
+
 	void AddShape(CcdShapeConstructionInfo* shapeInfo);
 
 	btTriangleMeshShape* GetMeshShape(void)
@@ -105,6 +112,32 @@ public:
 
 		return m_shapeArray.at(i);
 	}
+	int FindChildShape(CcdShapeConstructionInfo* shapeInfo, void* userData)
+	{
+		if (shapeInfo == NULL)
+			return -1;
+		for (int i=0; i<m_shapeArray.size(); i++)
+		{
+			CcdShapeConstructionInfo* childInfo = m_shapeArray.at(i);
+			if ((userData == NULL || userData == childInfo->m_userData) &&
+				(childInfo == shapeInfo ||
+				 (childInfo->m_shapeType == PHY_SHAPE_PROXY && 
+				  childInfo->m_shapeProxy == shapeInfo)))
+				return i;
+		}
+		return -1;
+	}
+
+	bool RemoveChildShape(int i)
+	{
+		if (i < 0 || i >= m_shapeArray.size())
+			return false;
+		m_shapeArray.at(i)->Release();
+		if (i < m_shapeArray.size()-1)
+			m_shapeArray[i] = m_shapeArray.back();
+		m_shapeArray.pop_back();
+		return true;
+	}
 
 	bool SetMesh(RAS_MeshObject* mesh, bool polytope,bool useGimpact);
 	RAS_MeshObject* GetMesh(void)
@@ -112,6 +145,12 @@ public:
 		return m_meshObject;
 	}
 
+	bool SetProxy(CcdShapeConstructionInfo* shapeInfo);
+	CcdShapeConstructionInfo* GetProxy(void)
+	{
+		return m_shapeProxy;
+	}
+
 	btCollisionShape* CreateBulletShape();
 
 	// member variables
@@ -121,6 +160,7 @@ public:
 	btVector3				m_halfExtend;
 	btTransform				m_childTrans;
 	btVector3				m_childScale;
+	void*					m_userData;	
 	std::vector<btPoint3>	m_vertexArray;	// Contains both vertex array for polytope shape and
 											// triangle array for concave mesh shape.
 											// In this case a triangle is made of 3 consecutive points
@@ -146,7 +186,7 @@ protected:
 	std::vector<CcdShapeConstructionInfo*> m_shapeArray;	// for compound shapes
 	bool	m_useGimpact; //use gimpact for concave dynamic/moving collision detection
 	float	m_weldingThreshold;	//welding closeby vertices together can improve softbody stability etc.
-
+	CcdShapeConstructionInfo* m_shapeProxy;	// only used for PHY_SHAPE_PROXY, pointer to actual shape info
 };
 
 struct CcdConstructionInfo

source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp

@@ -536,12 +536,24 @@ void CcdPhysicsEnvironment::disableCcdPhysicsController(CcdPhysicsController* ct
 			{
 			} else
 			{
-				m_dynamicsWorld->removeCollisionObject(body);
+				m_dynamicsWorld->removeCollisionObject(ctrl->GetCollisionObject());
 			}
 		}
 	}
 }
 
+void CcdPhysicsEnvironment::refreshCcdPhysicsController(CcdPhysicsController* ctrl)
+{
+	btCollisionObject* obj = ctrl->GetCollisionObject();
+	if (obj)
+	{
+		btBroadphaseProxy* proxy = obj->getBroadphaseHandle();
+		if (proxy)
+		{
+			m_dynamicsWorld->getPairCache()->cleanProxyFromPairs(proxy,m_dynamicsWorld->getDispatcher());
+		}
+	}
+}
 
 void	CcdPhysicsEnvironment::beginFrame()
 {

source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h

@@ -196,6 +196,8 @@ protected:
 
 		void	enableCcdPhysicsController(CcdPhysicsController* ctrl);
 
+		void	refreshCcdPhysicsController(CcdPhysicsController* ctrl);
+
 		btBroadphaseInterface*	getBroadphase();
 
 		btDispatcher*	getDispatcher();

source/gameengine/Physics/common/PHY_DynamicTypes.h

@@ -95,7 +95,8 @@ typedef enum PHY_ShapeType {
 	PHY_SHAPE_CONE,
 	PHY_SHAPE_MESH,
 	PHY_SHAPE_POLYTOPE,
-	PHY_SHAPE_COMPOUND
+	PHY_SHAPE_COMPOUND,
+	PHY_SHAPE_PROXY
 } PHY_ShapeType;