BGE: Add physics constraints replication

This patch will add a physics constraints replication for group instances
(dupli group).
It also fix crashing when when a group instance is made from a linked
group instance and both are on the active layer.

Initial patch T31443 from moerdn (Martin Sell).

Reviewers: lordloki, sergof, moguri, sybren

Reviewed By: moguri, sybren

Differential Revision: https://developer.blender.org/D658

6 files changed, 115 insertions, 0 deletions

diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
index 44c4e284e7c..5adff8f7f02 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
@@ -1740,6 +1740,31 @@ bool CcdPhysicsController::ReinstancePhysicsShape(KX_GameObject *from_gameobj, R
 	return true;
 }
 
+void CcdPhysicsController::ReplicateConstraints(KX_GameObject *replica, std::vector<KX_GameObject*> constobj)
+{
+	if (replica->GetConstraints().size() == 0 || !replica->GetPhysicsController())
+		return;
+		
+	PHY_IPhysicsEnvironment *physEnv = GetPhysicsEnvironment();
+
+	vector<bRigidBodyJointConstraint*> constraints = replica->GetConstraints();
+	vector<bRigidBodyJointConstraint*>::iterator consit;
+
+	/* Object could have some constraints, iterate over all of theme to ensure that every constraint is recreated. */
+	for (consit = constraints.begin(); consit != constraints.end(); ++consit) {
+		/* Try to find the constraint targets in the list of group objects. */
+		bRigidBodyJointConstraint *dat = (*consit);
+		vector<KX_GameObject*>::iterator memit; 
+		for (memit = constobj.begin(); memit != constobj.end(); ++memit) {
+			KX_GameObject *member = (*memit);
+			/* If the group member is the actual target for the constraint. */
+			if (dat->tar->id.name + 2 == member->GetName() && member->GetPhysicsController())
+				physEnv->SetupObjectConstraints(replica, member, dat);
+		}
+	}
+
+}
+
 ///////////////////////////////////////////////////////////
 ///A small utility class, DefaultMotionState
 ///
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.h b/source/gameengine/Physics/Bullet/CcdPhysicsController.h
index de40624d7bb..47bc9cdfbd7 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsController.h
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.h
@@ -719,6 +719,9 @@ protected:
 
 		virtual bool ReinstancePhysicsShape(KX_GameObject *from_gameobj, RAS_MeshObject* from_meshobj);
 
+		/* Method to replicate rigid body joint contraints for group instances. */
+		virtual void ReplicateConstraints(KX_GameObject *gameobj, std::vector<KX_GameObject*> constobj);
+
 #ifdef WITH_CXX_GUARDEDALLOC
 	MEM_CXX_CLASS_ALLOC_FUNCS("GE:CcdPhysicsController")
 #endif
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
index a30ee0fe34f..6d3f6d9ddcb 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
@@ -3599,3 +3599,79 @@ void CcdPhysicsEnvironment::ConvertObject(KX_GameObject *gameobj, RAS_MeshObject
 	}
 #endif
 }
+
+
+void CcdPhysicsEnvironment::SetupObjectConstraints(KX_GameObject *obj_src, KX_GameObject *obj_dest,
+	                                               bRigidBodyJointConstraint *dat)
+{
+	PHY_IPhysicsController *phy_src = obj_src->GetPhysicsController();
+	PHY_IPhysicsController *phy_dest = obj_dest->GetPhysicsController();
+	PHY_IPhysicsEnvironment *phys_env = obj_src->GetScene()->GetPhysicsEnvironment();
+
+	/* We need to pass a full constraint frame, not just axis. */
+	MT_Matrix3x3 localCFrame(MT_Vector3(dat->axX,dat->axY,dat->axZ));
+	MT_Vector3 axis0 = localCFrame.getColumn(0);
+	MT_Vector3 axis1 = localCFrame.getColumn(1);
+	MT_Vector3 axis2 = localCFrame.getColumn(2);
+	MT_Vector3 scale = obj_src->NodeGetWorldScaling();
+
+	/* Apply not only the pivot and axis values, but also take scale into count
+	 * this is not working well, if only one or two axis are scaled, but works ok on
+	 * homogeneous scaling. */
+	int constraintId = phys_env->CreateConstraint(
+		phy_src, phy_dest, (PHY_ConstraintType)dat->type,
+	    (float)(dat->pivX * scale.x()), (float)(dat->pivY * scale.y()), (float)(dat->pivZ * scale.z()),
+	    (float)(axis0.x() * scale.x()), (float)(axis0.y() * scale.y()), (float)(axis0.z() * scale.z()),
+	    (float)(axis1.x() * scale.x()), (float)(axis1.y() * scale.y()), (float)(axis1.z() * scale.z()),
+	    (float)(axis2.x() * scale.x()), (float)(axis2.y() * scale.y()), (float)(axis2.z() * scale.z()),
+		dat->flag);
+
+	/* PHY_POINT2POINT_CONSTRAINT = 1,
+	 * PHY_LINEHINGE_CONSTRAINT = 2,
+	 * PHY_ANGULAR_CONSTRAINT = 3,
+	 * PHY_CONE_TWIST_CONSTRAINT = 4,
+	 * PHY_VEHICLE_CONSTRAINT = 11,
+	 * PHY_GENERIC_6DOF_CONSTRAINT = 12 */
+
+	if (!constraintId)
+		return;
+
+	int dof = 0;
+	int dof_max = 0;
+	int dofbit = 0;
+
+	switch (dat->type) {
+		/* Set all the limits for generic 6DOF constraint. */
+		case PHY_GENERIC_6DOF_CONSTRAINT:
+			dof_max = 6;
+			dofbit = 1;
+			break;
+		/* Set XYZ angular limits for cone twist constraint. */
+		case PHY_CONE_TWIST_CONSTRAINT:
+			dof = 3;
+			dof_max = 6;
+			dofbit = 1 << 3;
+			break;
+		/* Set only X angular limits for line hinge and angular constraint. */
+		case PHY_LINEHINGE_CONSTRAINT:
+		case PHY_ANGULAR_CONSTRAINT:
+			dof = 3;
+			dof_max = 4;
+			dofbit = 1 << 3;
+			break;
+		default:
+			break;
+	}
+
+	for (dof; dof < dof_max; dof++) {
+		if (dat->flag & dofbit) {
+			phys_env->SetConstraintParam(constraintId, dof, dat->minLimit[dof], dat->maxLimit[dof]);
+		}
+		else {
+			/* minLimit > maxLimit means free (no limit) for this degree of freedom. */
+			phys_env->SetConstraintParam(constraintId, dof, 1.0f, -1.0f);
+		}
+		dofbit <<= 1;
+	}
+
+}
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
index 009eb5cde24..113db3348ca 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
@@ -275,6 +275,10 @@ protected:
 							bool isCompoundChild,
 							bool hasCompoundChildren);
 
+		/* Set the rigid body joints constraints values for converted objects and replicated group instances. */
+		virtual void SetupObjectConstraints(KX_GameObject *obj_src, KX_GameObject *obj_dest,
+		                                    bRigidBodyJointConstraint *dat);
+
 	protected:
 		
 		
diff --git a/source/gameengine/Physics/common/PHY_IPhysicsController.h b/source/gameengine/Physics/common/PHY_IPhysicsController.h
index 1717f8d90cb..68763be4db9 100644
--- a/source/gameengine/Physics/common/PHY_IPhysicsController.h
+++ b/source/gameengine/Physics/common/PHY_IPhysicsController.h
@@ -32,6 +32,7 @@
 #ifndef __PHY_IPHYSICSCONTROLLER_H__
 #define __PHY_IPHYSICSCONTROLLER_H__
 
+#include <vector>
 #include "PHY_IController.h"
 
 class PHY_IMotionState;
@@ -135,6 +136,8 @@ class PHY_IPhysicsController : public PHY_IController
 
 		virtual bool ReinstancePhysicsShape(KX_GameObject *from_gameobj, RAS_MeshObject* from_meshobj) = 0;
 
+		/* Method to replicate rigid body joint contraints for group instances. */
+		virtual void ReplicateConstraints(KX_GameObject *gameobj, std::vector<KX_GameObject*> constobj) = 0;
 
 #ifdef WITH_CXX_GUARDEDALLOC
 	MEM_CXX_CLASS_ALLOC_FUNCS("GE:PHY_IPhysicsController")
diff --git a/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h b/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h
index dd762b02b4e..44b61136d3f 100644
--- a/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h
+++ b/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h
@@ -55,6 +55,7 @@ class KX_Scene;
 struct PHY_ShapeProps;
 struct PHY_MaterialProps;
 class PHY_IMotionState;
+struct bRigidBodyJointConstraint;
 
 /**
  * pass back information from rayTest
@@ -213,6 +214,9 @@ class PHY_IPhysicsEnvironment
 							bool isCompoundChild,
 							bool hasCompoundChildren) = 0;
 
+		/* Set the rigid body joints constraints values for converted objects and replicated group instances. */
+		virtual void SetupObjectConstraints(KX_GameObject *obj_src, KX_GameObject *obj_dest,
+		                                    bRigidBodyJointConstraint *dat) {}
 
 #ifdef WITH_CXX_GUARDEDALLOC
 	MEM_CXX_CLASS_ALLOC_FUNCS("GE:PHY_IPhysicsEnvironment")