From 5a58dcf0ccaaf954b9134db183b6b8b8cd4b395d Mon Sep 17 00:00:00 2001
From: Irlan <Irlan@DESKTOP-Q1RSAUM>
Date: Sat, 14 Jan 2017 11:10:48 -0200
Subject: [PATCH] correct CFM formulas and spring joint mass, bugfixes

---
 src/bounce/dynamics/joints/mouse_joint.cpp  |  1 -
 src/bounce/dynamics/joints/spring_joint.cpp | 33 +++++++++++----------
 src/testbed/framework/main.cpp              |  5 ++++
 src/testbed/framework/test_entries.cpp      |  1 +
 4 files changed, 24 insertions(+), 16 deletions(-)

diff --git a/src/bounce/dynamics/joints/mouse_joint.cpp b/src/bounce/dynamics/joints/mouse_joint.cpp
index 101cee9..8c50499 100644
--- a/src/bounce/dynamics/joints/mouse_joint.cpp
+++ b/src/bounce/dynamics/joints/mouse_joint.cpp
@@ -84,7 +84,6 @@ void b3MouseJoint::SolveVelocityConstraints(const b3SolverData* data)
 bool b3MouseJoint::SolvePositionConstraints(const b3SolverData* data) 
 {
 	// There is no position correction for this constraint.
-	// todo Implement Buda spring?
 	return true;
 }
 
diff --git a/src/bounce/dynamics/joints/spring_joint.cpp b/src/bounce/dynamics/joints/spring_joint.cpp
index e786c64..062c2c6 100644
--- a/src/bounce/dynamics/joints/spring_joint.cpp
+++ b/src/bounce/dynamics/joints/spring_joint.cpp
@@ -116,21 +116,31 @@ void b3SpringJoint::InitializeConstraints(const b3SolverData* data)
 	b3Vec3 xB = data->positions[m_indexB].x;
 	b3Quat qB = data->positions[m_indexB].q;
 
+	m_rA = b3Mul(qA, m_localAnchorA - m_localCenterA);
+	m_rB = b3Mul(qB, m_localAnchorB - m_localCenterB);
+
 	// Singularity check.
 	m_n = xB + m_rB - xA - m_rA;
 	float32 length = b3Length(m_n);
 	if (length > B3_LINEAR_SLOP)
 	{
-		m_n = 1.0f / length * m_n;
+		m_n /= length;
 	}
 	else
 	{
 		m_n.SetZero();
 	}
+	
+	// Compute the effective mass matrix
+	b3Vec3 rnA = b3Cross(m_rA, m_n);
+	b3Vec3 rnB = b3Cross(m_rB, m_n);
+
+	float32 mass = m_mA + m_mB + b3Dot(m_iA * rnA, rnA) + b3Dot(m_iB * rnB, rnB);
+	
+	m_mass = mass > 0.0f ? 1.0f / mass : 0.0f;
 
 	if (m_frequencyHz > 0.0f)
 	{
-		// Compute spring parameters
 		float32 C = length - m_length;
 		
 		// Angular frequency
@@ -140,29 +150,22 @@ void b3SpringJoint::InitializeConstraints(const b3SolverData* data)
 		float32 d = 2.0f * m_mass * m_dampingRatio * omega;
 
 		// Spring stiffness
-		float32 s = m_mass * omega * omega;
+		float32 k = m_mass * omega * omega;
 
 		// Box2D's Soft Constraints talk
 		float32 h = data->dt;
-		m_gamma = h * (d + h * s);
-		m_bias = h * C * s * m_gamma;
+		m_gamma = h * (d + h * k);
+		m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
+		m_bias = h * C * k * m_gamma;
 
+		mass += m_gamma;
+		m_mass = mass != 0.0f ? 1.0f / mass : 0.0f;
 	}
 	else
 	{
 		m_bias = 0.0f;
 		m_gamma = 0.0f;
 	}
-
-	// Compute effective mass
-	m_rA = b3Mul(qA, m_localAnchorA - m_localCenterA);
-	m_rB = b3Mul(qB, m_localAnchorB - m_localCenterB);
-
-	b3Vec3 rnA = b3Cross(m_rA, m_n);
-	b3Vec3 rnB = b3Cross(m_rB, m_n);
-
-	float32 mass = m_mA + m_mB + b3Dot(m_iA * rnA, rnA) + b3Dot(m_iB * rnB, rnB) + m_gamma;
-	m_mass = mass > 0.0f ? 1.0f / mass : 0.0f;
 }
 
 void b3SpringJoint::WarmStart(const b3SolverData* data) 
diff --git a/src/testbed/framework/main.cpp b/src/testbed/framework/main.cpp
index a601c14..25fa3b5 100644
--- a/src/testbed/framework/main.cpp
+++ b/src/testbed/framework/main.cpp
@@ -402,6 +402,11 @@ void Run()
 
 int main(int argc, char** args)
 {
+#if defined(_WIN32)
+	// Report memory leaks
+	_CrtSetDbgFlag(_CRTDBG_LEAK_CHECK_DF | _CrtSetDbgFlag(_CRTDBG_REPORT_FLAG));
+#endif
+
 	if (glfwInit() == 0)
 	{
 		fprintf(stderr, "Failed to initialize GLFW\n");
diff --git a/src/testbed/framework/test_entries.cpp b/src/testbed/framework/test_entries.cpp
index 4ae1d90..9ec0805 100644
--- a/src/testbed/framework/test_entries.cpp
+++ b/src/testbed/framework/test_entries.cpp
@@ -77,4 +77,5 @@ TestEntry g_tests[] =
 	{ "Body Types", &BodyTypes::Create },
 	{ "Varying Friction", &VaryingFriction::Create },
 	{ "Varying Restitution", &VaryingRestitution::Create },
+	{ NULL, NULL }
 };
\ No newline at end of file