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

10 files changed, 199 insertions, 75 deletions

diff --git a/source/gameengine/Converter/BL_BlenderDataConversion.cpp b/source/gameengine/Converter/BL_BlenderDataConversion.cpp
index 7e0588fc7d0..cd2e2151034 100644
--- a/source/gameengine/Converter/BL_BlenderDataConversion.cpp
+++ b/source/gameengine/Converter/BL_BlenderDataConversion.cpp
@@ -1741,6 +1741,16 @@ static KX_GameObject* getGameOb(STR_String busc,CListValue* sumolist)
 
 }
 
+static bool bl_isConstraintInList(KX_GameObject *gameobj, set<KX_GameObject*> convertedlist)
+{
+	set<KX_GameObject*>::iterator gobit;
+	for (gobit = convertedlist.begin(); gobit != convertedlist.end(); gobit++) {
+		if ((*gobit)->GetName() == gameobj->GetName())
+			return true;
+	}
+	return false;
+}
+
 /* helper for BL_ConvertBlenderObjects, avoids code duplication
  * note: all var names match args are passed from the caller */
 static void bl_ConvertBlenderObject_Single(
@@ -1900,6 +1910,12 @@ void BL_ConvertBlenderObjects(struct Main* maggie,
 	// is not in a separate thread.
 	BL_Texture::GetMaxUnits();
 
+	/* We have to ensure that group definitions are only converted once
+	 * push all converted group members to this set.
+	 * This will happen when a group instance is made from a linked group instance
+	 * and both are on the active layer. */
+	set<KX_GameObject*> convertedlist;
+
 	if (alwaysUseExpandFraming) {
 		frame_type = RAS_FrameSettings::e_frame_extend;
 		aspect_width = canvas->GetWidth();
@@ -2031,6 +2047,11 @@ void BL_ConvertBlenderObjects(struct Main* maggie,
 														converter,
 														libloading);
 
+						/* Insert object to the constraint game object list
+						 * so we can check later if there is a instance in the scene or
+						 * an instance and its actual group definition. */
+						convertedlist.insert((KX_GameObject*)gameobj->AddRef());
+
 						bool isInActiveLayer = false;
 						if (gameobj)
 						{
@@ -2270,93 +2291,50 @@ void BL_ConvertBlenderObjects(struct Main* maggie,
 	// create physics joints
 	for (i=0;i<sumolist->GetCount();i++)
 	{
-		KX_GameObject* gameobj = (KX_GameObject*) sumolist->GetValue(i);
-		struct Object* blenderobject = gameobj->GetBlenderObject();
-		ListBase *conlist;
+		PHY_IPhysicsEnvironment *physEnv = kxscene->GetPhysicsEnvironment();
+		KX_GameObject *gameobj = (KX_GameObject *)sumolist->GetValue(i);
+		struct Object *blenderobject = gameobj->GetBlenderObject();
+		ListBase *conlist = get_active_constraints2(blenderobject);
 		bConstraint *curcon;
 
-		if ((gameobj->GetLayer()&activeLayerBitInfo)==0)
+		if (!conlist)
 			continue;
 
-		conlist = get_active_constraints2(blenderobject);
-		if (conlist) {
-			for (curcon = (bConstraint *)conlist->first; curcon; curcon = (bConstraint *)curcon->next) {
-				if (curcon->type==CONSTRAINT_TYPE_RIGIDBODYJOINT) {
-
-					bRigidBodyJointConstraint *dat=(bRigidBodyJointConstraint *)curcon->data;
+		for (curcon = (bConstraint *)conlist->first; curcon; curcon = (bConstraint *)curcon->next) {
+			if (curcon->type != CONSTRAINT_TYPE_RIGIDBODYJOINT)
+				continue;
 
-					if (!dat->child && !(curcon->flag & CONSTRAINT_OFF)) {
+			bRigidBodyJointConstraint *dat = (bRigidBodyJointConstraint *)curcon->data;
+					
+			/* Skip if no target or a child object is selected or constraints are deactivated */
+			if (!dat->tar || dat->child || (curcon->flag & CONSTRAINT_OFF))
+				continue;
 
-						PHY_IPhysicsController* physctr2 = 0;
+			/* Store constraints of grouped and instanced objects for all layers */
+			gameobj->AddConstraint(dat);
+						
+			/* Skipped already converted constraints. 
+			 * This will happen when a group instance is made from a linked group instance
+			 * and both are on the active layer. */
+			if (bl_isConstraintInList(gameobj, convertedlist))
+				continue;
 
-						if (dat->tar) {
-							KX_GameObject *gotar=getGameOb(dat->tar->id.name+2,sumolist);
-							if (gotar && ((gotar->GetLayer()&activeLayerBitInfo)!=0) && gotar->GetPhysicsController())
-								physctr2 = gotar->GetPhysicsController();
-						}
+			KX_GameObject *gotar = getGameOb(dat->tar->id.name + 2, sumolist);
 
-						if (gameobj->GetPhysicsController()) {
-							PHY_IPhysicsController* physctrl = gameobj->GetPhysicsController();
-							//we need to pass a full constraint frame, not just axis
-
-							//localConstraintFrameBasis
-							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);
-
-							int constraintId = kxscene->GetPhysicsEnvironment()->CreateConstraint(physctrl,physctr2,(PHY_ConstraintType)dat->type,(float)dat->pivX,
-								(float)dat->pivY,(float)dat->pivZ,
-								(float)axis0.x(),(float)axis0.y(),(float)axis0.z(),
-								(float)axis1.x(),(float)axis1.y(),(float)axis1.z(),
-								(float)axis2.x(),(float)axis2.y(),(float)axis2.z(),dat->flag);
-							if (constraintId) {
-								//if it is a generic 6DOF constraint, set all the limits accordingly
-								if (dat->type == PHY_GENERIC_6DOF_CONSTRAINT) {
-									int dof;
-									int dofbit=1;
-									for (dof=0;dof<6;dof++) {
-										if (dat->flag & dofbit) {
-											kxscene->GetPhysicsEnvironment()->SetConstraintParam(constraintId,dof,dat->minLimit[dof],dat->maxLimit[dof]);
-										} else {
-											//minLimit > maxLimit means free(disabled limit) for this degree of freedom
-											kxscene->GetPhysicsEnvironment()->SetConstraintParam(constraintId,dof,1,-1);
-										}
-										dofbit<<=1;
-									}
-								} else if (dat->type == PHY_CONE_TWIST_CONSTRAINT) {
-									int dof;
-									int dofbit = 1<<3; // bitflag use_angular_limit_x
-
-									for (dof=3;dof<6;dof++) {
-										if (dat->flag & dofbit) {
-											kxscene->GetPhysicsEnvironment()->SetConstraintParam(constraintId,dof,dat->minLimit[dof],dat->maxLimit[dof]);
-										} else {
-											//maxLimit < 0 means free(disabled limit) for this degree of freedom
-											kxscene->GetPhysicsEnvironment()->SetConstraintParam(constraintId,dof,1,-1);
-										}
-										dofbit<<=1;
-									}
-								} else if (dat->type == PHY_LINEHINGE_CONSTRAINT) {
-									int dof = 3; // dof for angular x
-									int dofbit = 1<<3; // bitflag use_angular_limit_x
-
-									if (dat->flag & dofbit) {
-										kxscene->GetPhysicsEnvironment()->SetConstraintParam(constraintId,dof,
-												dat->minLimit[dof],dat->maxLimit[dof]);
-									} else {
-										//minLimit > maxLimit means free(disabled limit) for this degree of freedom
-										kxscene->GetPhysicsEnvironment()->SetConstraintParam(constraintId,dof,1,-1);
-									}
-								}
-							}
-						}
-					}
-				}
+			if (gotar && (gotar->GetLayer()&activeLayerBitInfo) && gotar->GetPhysicsController() &&
+				(gameobj->GetLayer()&activeLayerBitInfo) && gameobj->GetPhysicsController())
+			{
+				physEnv->SetupObjectConstraints(gameobj, gotar, dat);
 			}
 		}
 	}
 
+	/* cleanup converted set of group objects */
+	set<KX_GameObject*>::iterator gobit;
+	for (gobit = convertedlist.begin(); gobit != convertedlist.end(); gobit++)
+		(*gobit)->Release();
+		
+	convertedlist.clear();
 	sumolist->Release();
 
 	// convert world
diff --git a/source/gameengine/Ketsji/KX_GameObject.cpp b/source/gameengine/Ketsji/KX_GameObject.cpp
index 71af263bf46..c8fc64f4679 100644
--- a/source/gameengine/Ketsji/KX_GameObject.cpp
+++ b/source/gameengine/Ketsji/KX_GameObject.cpp
@@ -292,6 +292,21 @@ void KX_GameObject::SetDupliGroupObject(KX_GameObject* obj)
 	m_pDupliGroupObject = obj;
 }
 
+void KX_GameObject::AddConstraint(bRigidBodyJointConstraint *cons)
+{
+	m_constraints.push_back(cons);
+}
+
+std::vector<bRigidBodyJointConstraint*> KX_GameObject::GetConstraints()
+{
+	return m_constraints;
+}
+
+void KX_GameObject::ClearConstraints()
+{
+	m_constraints.clear();
+}
+
 KX_GameObject* KX_GameObject::GetParent()
 {
 	KX_GameObject* result = NULL;
diff --git a/source/gameengine/Ketsji/KX_GameObject.h b/source/gameengine/Ketsji/KX_GameObject.h
index 7d1a934bcec..132e72c45e2 100644
--- a/source/gameengine/Ketsji/KX_GameObject.h
+++ b/source/gameengine/Ketsji/KX_GameObject.h
@@ -49,6 +49,7 @@
 #include "CTR_HashedPtr.h"
 #include "KX_Scene.h"
 #include "KX_KetsjiEngine.h" /* for m_anim_framerate */
+#include "DNA_constraint_types.h" /* for constraint replication */
 #include "DNA_object_types.h"
 #include "SCA_LogicManager.h" /* for ConvertPythonToGameObject to search object names */
 
@@ -117,6 +118,7 @@ protected:
 	SG_Node*							m_pSGNode;
 
 	MT_CmMatrix4x4						m_OpenGL_4x4Matrix;
+	std::vector<bRigidBodyJointConstraint*>	m_constraints;
 
 	KX_ObstacleSimulation*				m_pObstacleSimulation;
 
@@ -193,6 +195,14 @@ public:
 		void
 	UpdateBlenderObjectMatrix(Object* blendobj=NULL);
 
+	/**
+	 * Used for constraint replication for group instances.
+	 * The list of constraints is filled during data conversion.
+	 */
+	void AddConstraint(bRigidBodyJointConstraint *cons);
+	std::vector<bRigidBodyJointConstraint*> GetConstraints();
+	void ClearConstraints();
+
 	/** 
 	 * Get a pointer to the game object that is the parent of 
 	 * this object. Or NULL if there is no parent. The returned
diff --git a/source/gameengine/Ketsji/KX_Scene.cpp b/source/gameengine/Ketsji/KX_Scene.cpp
index d1b10098237..47510baa436 100644
--- a/source/gameengine/Ketsji/KX_Scene.cpp
+++ b/source/gameengine/Ketsji/KX_Scene.cpp
@@ -848,6 +848,12 @@ void KX_Scene::DupliGroupRecurse(CValue* obj, int level)
 	// now look if object in the hierarchy have dupli group and recurse
 	for (git = m_logicHierarchicalGameObjects.begin();!(git==m_logicHierarchicalGameObjects.end());++git)
 	{
+		/* Replicate all constraints. */
+		if ((*git)->GetPhysicsController()) {
+			(*git)->GetPhysicsController()->ReplicateConstraints((*git), m_logicHierarchicalGameObjects);
+			(*git)->ClearConstraints();
+		}
+
 		if ((*git) != groupobj && (*git)->IsDupliGroup())
 			// can't instantiate group immediately as it destroys m_logicHierarchicalGameObjects
 			duplilist.push_back((*git));
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")