From f924f70e76c454d4c51d67ecb61036bfeb6eec52 Mon Sep 17 00:00:00 2001
From: Irlan <Irlan@DESKTOP-Q1RSAUM>
Date: Tue, 4 Apr 2017 20:56:10 -0300
Subject: [PATCH] add pendulum test (reduced coordinates), remove redundant
 assert

---
 examples/testbed/framework/test_entries.cpp   |   2 +
 examples/testbed/tests/pendulum.h             | 115 ++++++++++++++++++
 src/bounce/dynamics/joints/revolute_joint.cpp |   2 -
 3 files changed, 117 insertions(+), 2 deletions(-)
 create mode 100644 examples/testbed/tests/pendulum.h

diff --git a/examples/testbed/framework/test_entries.cpp b/examples/testbed/framework/test_entries.cpp
index e34c580..bd7f554 100644
--- a/examples/testbed/framework/test_entries.cpp
+++ b/examples/testbed/framework/test_entries.cpp
@@ -57,6 +57,7 @@
 #include <testbed/tests/varying_restitution.h>
 #include <testbed/tests/cloth_test.h>
 #include <testbed/tests/tumbler.h>
+#include <testbed/tests/pendulum.h>
 
 TestEntry g_tests[] =
 {
@@ -99,5 +100,6 @@ TestEntry g_tests[] =
 	{ "Cloth", &Cloth::Create },
 	{ "Tumbler", &Tumbler::Create },
 	{ "Initial Overlap", &InitialOverlap::Create },
+	{ "Pendulum", &Pendulum::Create },
 	{ NULL, NULL }
 };
\ No newline at end of file
diff --git a/examples/testbed/tests/pendulum.h b/examples/testbed/tests/pendulum.h
new file mode 100644
index 0000000..2420c2d
--- /dev/null
+++ b/examples/testbed/tests/pendulum.h
@@ -0,0 +1,115 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef PENDULUM_H
+#define PENDULUM_H
+
+extern Settings g_settings;
+extern DebugDraw* g_debugDraw;
+
+class Pendulum : public Test
+{
+public:
+	Pendulum()
+	{
+		m_g = -10.0f;
+
+		m_r = 10.0f;
+		m_m = 1.0f;
+		m_I = m_m * m_r * m_r;
+		
+		// Initial state
+		m_theta = 0.5f * B3_PI;
+		m_omega = 0.0f;
+	}
+
+	void Step()
+	{
+		float32 h = g_settings.hertz > 0.0f ? 1.0f / g_settings.hertz : 0.0f;
+		
+		if (g_settings.pause)
+		{
+			if (g_settings.singleStep)
+			{
+				g_settings.singleStep = false;
+			}
+			else
+			{
+				h = 0.0f;
+			}
+		}
+
+		// Solution (acceleration)
+		float32 omega_dot = -m_g / m_r * sin(m_theta);
+
+		// Integrate acceleration
+		m_omega += h * omega_dot;
+
+		// Integrate velocity
+		m_theta += h * m_omega;
+
+		// Convert from polar coordinates (r, theta) to Cartesian coordinates (x, y)
+		b3Vec3 c;
+		c.x = m_r * sin(m_theta);
+		c.y = m_r * cos(m_theta);
+		c.z = 0.0f;
+		g_debugDraw->DrawSolidSphere(c, 1.0f, b3Color_white);
+
+		b3Vec3 pole;
+		pole.SetZero();
+		g_debugDraw->DrawSegment(pole, c, b3Color_white);
+
+		// Kinetic energy
+		float32 T = 0.5f * m_I * m_omega * m_omega;
+
+		// Potential energy
+		float32 V = -m_m * m_g * m_r * cos(m_theta);
+
+		// Lagrangian
+		float32 L = T - V;
+
+		static char s[256];
+		sprintf(s, "T = %f \nV = %f \nL = %f", T, V, L);
+		g_debugDraw->DrawString(s, b3Color_white);
+	}
+
+	static Test* Create()
+	{
+		return new Pendulum();
+	}
+
+	// Gravity
+	float32 m_g;
+	
+	// Mass, inertia
+	float32 m_m, m_I;
+
+	// Radial coordinate
+	float32 m_r;
+
+	// The allowable generalized coordinate in polar coordinate frame.
+	// Only motions satisfying the constraints can be described 
+	// in this frame. Therefore, all solutions satisfy the constraints.
+	// This is the so called reduced coordinates approach.
+	float32 m_theta;  
+	
+	// Velocity
+	float32 m_omega; 
+};
+
+#endif
\ No newline at end of file
diff --git a/src/bounce/dynamics/joints/revolute_joint.cpp b/src/bounce/dynamics/joints/revolute_joint.cpp
index c0ca8e1..8e0b212 100644
--- a/src/bounce/dynamics/joints/revolute_joint.cpp
+++ b/src/bounce/dynamics/joints/revolute_joint.cpp
@@ -184,8 +184,6 @@ void b3RevoluteJointDef::Initialize(b3Body* bA, b3Body* bB,
 	float32 len = q.Normalize();
 	B3_ASSERT(len > B3_EPSILON);
 
-	B3_ASSERT(b3Length(q) > B3_EPSILON);
-	
 	b3Quat qA = bA->GetOrientation();
 	b3Quat qB = bB->GetOrientation();