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

6 files changed, 58 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();