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.

1 files changed, 132 insertions, 3 deletions

diff --git a/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp b/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp
index 3a20bbfbb11..a67f4a54b3f 100644
--- a/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp
+++ b/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;
 	
 }