debug lines / register the applied impulse for constraint (for breaking)

10 files changed, 106 insertions, 5 deletions

diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp
index 12e02f2837a..0e5c7cf5715 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp
@@ -47,6 +47,8 @@ m_angularOnly(false)
 
 void	HingeConstraint::BuildJacobian()
 {
+	m_appliedImpulse = 0.f;
+
 	SimdVector3	normal(0,0,0);
 
 	if (!m_angularOnly)
@@ -120,7 +122,7 @@ void	HingeConstraint::SolveConstraint(SimdScalar	timeStep)
 			//positional error (zeroth order error)
 			SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
 			SimdScalar impulse = depth*tau/timeStep  * jacDiagABInv -  damping * rel_vel * jacDiagABInv * damping;
-
+			m_appliedImpulse += impulse;
 			SimdVector3 impulse_vector = normal * impulse;
 			m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
 			m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp
index 64b470f4391..b3e25e0fb45 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp
@@ -40,6 +40,8 @@ Point2PointConstraint::Point2PointConstraint(RigidBody& rbA,const SimdVector3& p
 
 void	Point2PointConstraint::BuildJacobian()
 {
+	m_appliedImpulse = 0.f;
+
 	SimdVector3	normal(0,0,0);
 
 	for (int i=0;i<3;i++)
@@ -98,7 +100,7 @@ void	Point2PointConstraint::SolveConstraint(SimdScalar	timeStep)
 		SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
 		
 		SimdScalar impulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
-
+		m_appliedImpulse+=impulse;
 		SimdVector3 impulse_vector = normal * impulse;
 		m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
 		m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/TypedConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/TypedConstraint.cpp
index d1605b7a030..61ea8ace7d6 100644
--- a/extern/bullet/BulletDynamics/ConstraintSolver/TypedConstraint.cpp
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/TypedConstraint.cpp
@@ -24,7 +24,8 @@ TypedConstraint::TypedConstraint()
 : m_userConstraintType(-1),
 m_userConstraintId(-1),
 m_rbA(s_fixed),
-m_rbB(s_fixed)
+m_rbB(s_fixed),
+m_appliedImpulse(0.f)
 {
 	s_fixed.setMassProps(0.f,SimdVector3(0.f,0.f,0.f));
 }
@@ -32,7 +33,8 @@ TypedConstraint::TypedConstraint(RigidBody& rbA)
 : m_userConstraintType(-1),
 m_userConstraintId(-1),
 m_rbA(rbA),
-m_rbB(s_fixed)
+m_rbB(s_fixed),
+m_appliedImpulse(0.f)
 {
 		s_fixed.setMassProps(0.f,SimdVector3(0.f,0.f,0.f));
 
@@ -43,7 +45,8 @@ TypedConstraint::TypedConstraint(RigidBody& rbA,RigidBody& rbB)
 : m_userConstraintType(-1),
 m_userConstraintId(-1),
 m_rbA(rbA),
-m_rbB(rbB)
+m_rbB(rbB),
+m_appliedImpulse(0.f)
 {
 		s_fixed.setMassProps(0.f,SimdVector3(0.f,0.f,0.f));
 
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/TypedConstraint.h b/extern/bullet/BulletDynamics/ConstraintSolver/TypedConstraint.h
index 0bc18075d19..bc90814de44 100755
--- a/extern/bullet/BulletDynamics/ConstraintSolver/TypedConstraint.h
+++ b/extern/bullet/BulletDynamics/ConstraintSolver/TypedConstraint.h
@@ -24,10 +24,13 @@ class TypedConstraint
 {
 	int	m_userConstraintType;
 	int	m_userConstraintId;
+	
 
 protected:
 	RigidBody&	m_rbA;
 	RigidBody&	m_rbB;
+	float	m_appliedImpulse;
+
 
 public:
 
@@ -78,6 +81,10 @@ public:
 	{
 		return m_userConstraintId;
 	}
+	float	GetAppliedImpulse()
+	{
+		return m_appliedImpulse;
+	}
 };
 
 #endif //TYPED_CONSTRAINT_H
diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
index d5fd194dd99..2fee6d06963 100644
--- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
+++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
@@ -1271,6 +1271,21 @@ int			CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
 
 }
 
+float		CcdPhysicsEnvironment::getAppliedImpulse(int	constraintid)
+{
+	std::vector<TypedConstraint*>::iterator i;
+
+	for (i=m_constraints.begin();
+		!(i==m_constraints.end()); i++)
+	{
+		TypedConstraint* constraint = (*i);
+		if (constraint->GetUserConstraintId() == constraintid)
+		{
+			return constraint->GetAppliedImpulse();
+		}
+	}
+	return 0.f;
+}
 void		CcdPhysicsEnvironment::removeConstraint(int	constraintId)
 {
 	std::vector<TypedConstraint*>::iterator i;
@@ -1337,6 +1352,13 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i
 	SimdVector3 rayFrom(fromX,fromY,fromZ);
 	SimdVector3 rayTo(toX,toY,toZ);
 
+	
+	if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)	
+	{
+		SimdVector3 color (1,0,0);
+		m_debugDrawer->DrawLine(rayFrom,rayTo,color);
+	}
+
 	SimdVector3	hitPointWorld,normalWorld;
 
 	//Either Ray Cast with or without filtering
@@ -1354,10 +1376,26 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i
 		hitX = 	rayCallback.m_hitPointWorld.getX();
 		hitY = 	rayCallback.m_hitPointWorld.getY();
 		hitZ = 	rayCallback.m_hitPointWorld.getZ();
+		if (rayCallback.m_hitNormalWorld.length2() > SIMD_EPSILON)
+		{
+			rayCallback.m_hitNormalWorld.normalize();
+		}
 
 		normalX = rayCallback.m_hitNormalWorld.getX();
 		normalY = rayCallback.m_hitNormalWorld.getY();
 		normalZ = rayCallback.m_hitNormalWorld.getZ();
+		
+		if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)	
+		{
+			SimdVector3 colorNormal(0,0,1);
+			m_debugDrawer->DrawLine(rayCallback.m_hitPointWorld,rayCallback.m_hitPointWorld+rayCallback.m_hitNormalWorld,colorNormal);
+		
+			SimdVector3 color (0,1,0);
+			m_debugDrawer->DrawLine(rayFrom,rayCallback.m_hitPointWorld,color);
+			
+		
+		}
+
 
 	}	
 
diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h
index 9de7053d271..73bd9f8c95e 100644
--- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h
+++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h
@@ -113,6 +113,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
 			float pivotX,float pivotY,float pivotZ,
 			float axisX,float axisY,float axisZ);
 	    virtual void		removeConstraint(int	constraintid);
+		virtual float		getAppliedImpulse(int	constraintid);
 
 
 		virtual void	CallbackTriggers();
diff --git a/source/gameengine/Ketsji/KX_PyConstraintBinding.cpp b/source/gameengine/Ketsji/KX_PyConstraintBinding.cpp
index 28138195a5e..6fbf2888bd9 100644
--- a/source/gameengine/Ketsji/KX_PyConstraintBinding.cpp
+++ b/source/gameengine/Ketsji/KX_PyConstraintBinding.cpp
@@ -73,6 +73,8 @@ static char gPySetSolverType__doc__[] = "setSolverType(int solverType) Very expe
 static char gPyCreateConstraint__doc__[] = "createConstraint(ob1,ob2,float restLength,float restitution,float damping)";
 static char gPyGetVehicleConstraint__doc__[] = "getVehicleConstraint(int constraintId)";
 static char gPyRemoveConstraint__doc__[] = "removeConstraint(int constraintId)";
+static char gPyGetAppliedImpulse__doc__[] = "getAppliedImpulse(int constraintId)";
+
 
 
 
@@ -401,6 +403,32 @@ static PyObject* gPyCreateConstraint(PyObject* self,
 }
 
 
+
+
+static PyObject* gPyGetAppliedImpulse(PyObject* self,
+										 PyObject* args, 
+										 PyObject* kwds)
+{
+	float	appliedImpulse = 0.f;
+
+#if defined(_WIN64)
+	__int64 constraintid;
+	if (PyArg_ParseTuple(args,"L",&constraintid))
+#else
+	long constraintid;
+	if (PyArg_ParseTuple(args,"l",&constraintid))
+#endif
+	{
+		if (PHY_GetActiveEnvironment())
+		{
+			appliedImpulse = PHY_GetActiveEnvironment()->getAppliedImpulse(constraintid);
+		}
+	}
+
+	return PyFloat_FromDouble(appliedImpulse);
+}
+
+
 static PyObject* gPyRemoveConstraint(PyObject* self,
 										 PyObject* args, 
 										 PyObject* kwds)
@@ -470,6 +498,9 @@ static struct PyMethodDef physicsconstraints_methods[] = {
 
   {"removeConstraint",(PyCFunction) gPyRemoveConstraint,
    METH_VARARGS, gPyRemoveConstraint__doc__},
+	{"getAppliedImpulse",(PyCFunction) gPyGetAppliedImpulse,
+   METH_VARARGS, gPyGetAppliedImpulse__doc__},
+
 
    //sentinel
   { NULL, (PyCFunction) NULL, 0, NULL }
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
index 6ff471c4bd5..2fee6d06963 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
@@ -1271,6 +1271,21 @@ int			CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
 
 }
 
+float		CcdPhysicsEnvironment::getAppliedImpulse(int	constraintid)
+{
+	std::vector<TypedConstraint*>::iterator i;
+
+	for (i=m_constraints.begin();
+		!(i==m_constraints.end()); i++)
+	{
+		TypedConstraint* constraint = (*i);
+		if (constraint->GetUserConstraintId() == constraintid)
+		{
+			return constraint->GetAppliedImpulse();
+		}
+	}
+	return 0.f;
+}
 void		CcdPhysicsEnvironment::removeConstraint(int	constraintId)
 {
 	std::vector<TypedConstraint*>::iterator i;
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
index 9de7053d271..73bd9f8c95e 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
@@ -113,6 +113,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
 			float pivotX,float pivotY,float pivotZ,
 			float axisX,float axisY,float axisZ);
 	    virtual void		removeConstraint(int	constraintid);
+		virtual float		getAppliedImpulse(int	constraintid);
 
 
 		virtual void	CallbackTriggers();
diff --git a/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h b/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h
index cdd67257de2..fe4370fa725 100644
--- a/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h
+++ b/source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h
@@ -88,6 +88,7 @@ class PHY_IPhysicsEnvironment
 			float pivotX,float pivotY,float pivotZ,
 			float axisX,float axisY,float axisZ)=0;
 		virtual void		removeConstraint(int	constraintid)=0;
+		virtual float		getAppliedImpulse(int	constraintid){ return 0.f;}
 
 		//complex constraint for vehicles
 		virtual PHY_IVehicle*	getVehicleConstraint(int constraintId) =0;