Constant speed splines

BSVino · Jun 23, 2016 · 9ee0bd3 · 9ee0bd3
1 parent c33f7d4
commit 9ee0bd3
Show file tree

Hide file tree

Showing 3 changed files with 134 additions and 85 deletions.
diff --git a/game/game.cpp b/game/game.cpp
@@ -46,6 +46,8 @@ LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON A
 
 #include "spline.h"
 
+#define SEGMENTS_PER_POINT 60
+
 Vector g_spline_points[SPLINE_POINTS] =
 {
 	Vector(0, 1, 0),
@@ -66,6 +68,9 @@ Vector g_spline_points[SPLINE_POINTS] =
 	Vector(15, 1, -15),
 };
 
+Vector g_spline_segments[(SPLINE_POINTS+1) * SEGMENTS_PER_POINT];
+float g_spline_speed = 1.5f;
+
 CGame::CGame(int argc, char** argv)
 	: CApplication(argc, argv)
 {
@@ -106,6 +111,27 @@ void CGame::Load()
 
 	memcpy(g_spline.m_points, g_spline_points, sizeof(g_spline_points));
 	g_spline.InitializeSpline();
+
+	float total_length = g_spline.GetTotalLength();
+
+
+
+
+
+
+
+
+	for (int k = 0; k < VArraySize(g_spline_segments); k++)
+		g_spline_segments[k] = g_spline.ConstVelocitySplineAtTime(total_length*k/VArraySize(g_spline_segments)/g_spline_speed, g_spline_speed);
+
+
+
+
+
+
+
+
+
 }
 
 void CGame::MakePuff(const Point& p)
@@ -634,35 +660,33 @@ void CGame::Draw()
 
 		c.BeginRenderLines();
 
-		float segments_per_spline = 60.0f;
-		for (int k = 0; k < (SPLINE_POINTS - 1) * segments_per_spline - 1; k++)
+		for (int k = 0; k < VArraySize(g_spline_segments)-1; k++)
 		{
-			float t0 = (float)k/segments_per_spline;
-			float t1 = (float)(k+1)/segments_per_spline;
-
-			Vector p0 = g_spline.SplineAtTime(t0);
-			Vector p1 = g_spline.SplineAtTime(t1);
+			Vector p0 = g_spline_segments[k];
+			Vector p1 = g_spline_segments[k+1];
 
 			c.Vertex(p0);
 			c.Vertex(p1);
 		}
 
 		c.EndRender();
 
+		float total_length = g_spline.GetTotalLength();
+
 		Matrix4x4 m;
-		m.SetTranslation(g_spline.SplineAtTime(fmod(Application()->GetTime()/2, (float)SPLINE_POINTS-1)));
+		m.SetTranslation(g_spline.ConstVelocitySplineAtTime(fmod(Application()->GetTime()*10, total_length / g_spline_speed), g_spline_speed));
 		c.LoadTransform(m);
 		c.RenderBox(Vector(-0.1f, -0.1f, -0.1f), Vector(0.1f, 0.1f, 0.1f));
 
-		m.SetTranslation(g_spline.SplineAtTime(fmod((Application()->GetTime()+0.1f)/2, (float)SPLINE_POINTS-1)));
+		m.SetTranslation(g_spline.ConstVelocitySplineAtTime(fmod((Application()->GetTime()+0.1f)*10, total_length / g_spline_speed), g_spline_speed));
 		c.LoadTransform(m);
 		c.RenderBox(Vector(-0.1f, -0.1f, -0.1f), Vector(0.1f, 0.1f, 0.1f));
 
-		m.SetTranslation(g_spline.SplineAtTime(fmod((Application()->GetTime()+0.2f)/2, (float)SPLINE_POINTS-1)));
+		m.SetTranslation(g_spline.ConstVelocitySplineAtTime(fmod((Application()->GetTime()+0.2f)*10, total_length / g_spline_speed), g_spline_speed));
 		c.LoadTransform(m);
 		c.RenderBox(Vector(-0.1f, -0.1f, -0.1f), Vector(0.1f, 0.1f, 0.1f));
 
-		m.SetTranslation(g_spline.SplineAtTime(fmod((Application()->GetTime()+0.3f)/2, (float)SPLINE_POINTS-1)));
+		m.SetTranslation(g_spline.ConstVelocitySplineAtTime(fmod((Application()->GetTime()+0.3f)*10, total_length / g_spline_speed), g_spline_speed));
 		c.LoadTransform(m);
 		c.RenderBox(Vector(-0.1f, -0.1f, -0.1f), Vector(0.1f, 0.1f, 0.1f));
 	}

diff --git a/game/main.cpp b/game/main.cpp
@@ -39,57 +39,9 @@ LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON A
 #define VTB_IMPLEMENTATION
 #include "vtb.h"
 
-using std::vector;
-
-
-// This is actually midpoint rule
-double BoringRectangles(double t0, double t1, int n)
-{
-	double h = (t1-t0) / n;
-	double sum = 0;
-
-	for (int k = 0; k < n; k++)
-	{
-		double t = t0 + k*h + h/2;
-		double rectangle_area = h * exp(t);
-		sum += rectangle_area;
-	}
-
-	return sum;
-}
-
-double CompositeSimpsonsRule(double t0, double t1, int n)
-{
-	double h = (t1-t0) / n;
-	double endpoints = exp(t0) + exp(t1);
-	double X4 = 0;
-	double X2 = 0;
-
-	for (int k = 1; k < n; k += 2)
-	{
-		double t = t0 + k*h;
-		X4 += exp(t);
-	}
-
-	for (int k = 2; k < n; k += 2)
-	{
-		double t = t0 + k*h;
-		X2 += exp(t);
-	}
-
-	return (h / 3) * (endpoints + 4 * X4 + 2 * X2);
-}
-
-
 
 int main(int argc, char* argv[])
 {
-	printf("Actual value:      %.16f\n", exp(1) - 1);
-	printf("Boring rectangles: %.16f\n", BoringRectangles(0, 1, 64));
-	printf("Composite Simpson: %.16f\n", CompositeSimpsonsRule(0, 1, 64));
-
-	mtsrand(0);
-
 	// Create a game
 	CGame game(argc, argv);
 

diff --git a/math/spline.h b/math/spline.h
@@ -44,7 +44,7 @@ struct CubicSpline
 		}
 
 		for (int k = 0; k < SPLINE_POINTS - 1; k++)
-			m_lengths[k] = Integrate(k, 1);
+			m_lengths[k] = SimpsonsRuleSingleSpline(k, 1);
 	}
 
 	vec3 SplineAtTime(float t)
@@ -68,8 +68,11 @@ struct CubicSpline
 		return result;
 	}
 
-	float ArcLengthIntegrand(int spline, float t)
+	float ArcLengthIntegrandSingleSpline(int spline, float t)
 	{
+		VAssert(t >= -1);
+		VAssert(t <= 2);
+
 		float tt = t*t;
 
 		vec3 dv = m_coeffs[spline][1] + 2 * m_coeffs[spline][2] * t + 3 * m_coeffs[spline][3] * tt;
@@ -80,48 +83,118 @@ struct CubicSpline
 		return sqrt(xx + yy + zz);
 	}
 
+	float ArcLengthIntegrand(float t)
+	{
+		if (t < 0)
+			return ArcLengthIntegrandSingleSpline(0, t);
+
+		if (t >= SPLINE_POINTS-1)
+			return ArcLengthIntegrandSingleSpline(SPLINE_POINTS-1, fmod(t, 1));
+
+		return ArcLengthIntegrandSingleSpline((int)t, t - (int)t);
+	}
+
 	// Composite Simpson's Rule, Burden & Faires - Numerical Analysis 9th, algorithm 4.1
-	float Integrate(int spline, float t)
+	float SimpsonsRuleSingleSpline(int spline, float t)
 	{
+		VAssert(t >= -1);
+		VAssert(t <= 2);
+
 		int n = 16;
 		float h = t / n;
-		float XI0 = ArcLengthIntegrand(spline, t);
+		float XI0 = ArcLengthIntegrandSingleSpline(spline, t);
 		float XI1 = 0;
 		float XI2 = 0;
 
-		for (int i = 0; i < n; i++)
-		{
-			float X = i*h;
-			if (i % 2 == 0)
-				XI2 += ArcLengthIntegrand(spline, X);
-			else
-				XI1 += ArcLengthIntegrand(spline, X);
-		}
+		for (int i = 0; i < n; i += 2)
+			XI2 += ArcLengthIntegrandSingleSpline(spline, i*h);
+
+		for (int i = 1; i < n; i += 2)
+			XI1 += ArcLengthIntegrandSingleSpline(spline, i*h);
 
 		float XI = h * (XI0 + 2 * XI2 + 4 * XI1) * (1.0f / 3);
 		return XI;
 	}
 
-	vec3 ConstVelocitySplineAtTime(float t)
+	float SimpsonsRule(float t0, float t1)
 	{
-		int spline = 0;
-		while (t > m_lengths[spline])
+		float multiplier = 1;
+		if (t0 > t1)
 		{
-			t -= m_lengths[spline];
-			spline += 1;
+			std::swap(t0, t1);
+			multiplier = -1;
 		}
 
-		float s = t / m_lengths[spline]; // Here's our initial guess.
+		int first_spline = (int)t0;
+		if (first_spline < 0)
+			first_spline = 0;
+
+		int last_spline = (int)t1;
+		if (last_spline >= SPLINE_POINTS-1)
+			last_spline = SPLINE_POINTS-2;
+
+		if (first_spline == last_spline)
+			return (SimpsonsRuleSingleSpline(last_spline, t1 - last_spline) - SimpsonsRuleSingleSpline(first_spline, t0 - first_spline)) * multiplier;
+
+		float sum = m_lengths[first_spline] - SimpsonsRuleSingleSpline(first_spline, t0);
+
+		for (int k = first_spline+1; k < last_spline; k++)
+			sum += m_lengths[k];
+
+		sum += SimpsonsRuleSingleSpline(last_spline, t1 - last_spline);
+
+		return sum * multiplier;
+	}
+
+	vec3 ConstVelocitySplineAtTime(float t, float speed)
+	{
+		float total_length = GetTotalLength();
+		float desired_distance = fmod(t * speed, total_length);
+
+		float t_last = SPLINE_POINTS * desired_distance / total_length;
+		float t_next = t_last;
+
+		auto g = [this, desired_distance](float t) -> float {
+			return SimpsonsRule(0, t) - desired_distance;
+		};
+
+		auto L = [this](float t) -> float {
+			return ArcLengthIntegrand(t);
+		};
+
+		float t_max = SPLINE_POINTS;
+		float t_min = -0.1f;
+
+		while (t_max - t_min > 0.5f)
+		{
+			float t_mid = (t_max + t_min)/2;
+			if (g(t_min) * g(t_mid) < 0)
+				t_max = t_mid;
+			else
+				t_min = t_mid;
+		}
+
+		t_next = (t_max + t_min)/2;
+
+		do {
+			t_last = t_next;
+			t_next = t_last - g(t_last) / L(t_last);
+		} while(fabs(t_last - t_next) > 0.001f);
+
+		// Because of root finding it may be slightly negative sometimes.
+		VAssert(t_next >= -0.1f && t_next <= 999999);
+
+		return SplineAtTime(t_next);
+	}
+
+	float GetTotalLength()
+	{
+		float sum = 0;
 
-		// Do some Newton-Rhapsons.
-		s = s - (Integrate(spline, s) - t) / ArcLengthIntegrand(spline, s);
-		s = s - (Integrate(spline, s) - t) / ArcLengthIntegrand(spline, s);
-		s = s - (Integrate(spline, s) - t) / ArcLengthIntegrand(spline, s);
-		s = s - (Integrate(spline, s) - t) / ArcLengthIntegrand(spline, s);
-		s = s - (Integrate(spline, s) - t) / ArcLengthIntegrand(spline, s);
-		s = s - (Integrate(spline, s) - t) / ArcLengthIntegrand(spline, s);
+		for (int k = 0; k < VArraySize(m_lengths); k++)
+			sum += m_lengths[k];
 
-		return SplineAtTime(spline + s);
+		return sum;
 	}
 };