1 files changed, 110 insertions, 26 deletions

diff --git a/2d/softbody/softbody_1/damped.cpp b/2d/softbody/softbody_1/damped.cpp
index 0344bd0..f56f346 100644
--- a/2d/softbody/softbody_1/damped.cpp
+++ b/2d/softbody/softbody_1/damped.cpp
@@ -52,13 +52,17 @@ namespace Damped {
         float32 gamma = 6.2;
         float32 discriminant = 0.f;
         float32 omega1       = 0.f;
+		float32 c1 = 0.f;
+		float32 c2 = 0.f;
+		float32 r1 = 0.f;
+		float32 r2 = 0.f;
 
         // Update variables
         float32 displacement = 0.f;
         float32 timeElapsed  = 0.f;
         
 
-        void load(Renderer2d* renderer, DampedSpringWeight* inWieight, float32 length, float32 loopRadius, float32 initialDisplacement);
+        void load(Renderer2d* renderer, DampedSpringWeight* inWieight, float32 length, float32 loopRadius, float32 initialDisplacement, float32 inK, float32 inC);
         void update(float32 dtSeconds);
         void render(Renderer2d* renderer);
         void unload();
@@ -92,7 +96,7 @@ namespace Damped {
         renderer.load(context);
 
         weight.load(&renderer, initVariables.mass, Vector4 { 55.f, 235.f, 35.f, 255.f }, Vector4 { 235.f, 5.f, 235.f, 255.f });
-        spring.load(&renderer, &weight, initVariables.springLength, 16.f, initVariables.initialDisplacement);
+        spring.load(&renderer, &weight, initVariables.springLength, 16.f, initVariables.initialDisplacement, initVariables.k, initVariables.c);
 
         mainLoop.run(update);
     }
@@ -164,30 +168,105 @@ namespace Damped {
 
     const float32 epsilon = 0.0001f;
 
-    void DampedSpring::load(Renderer2d* renderer, DampedSpringWeight* inWeight, float32 length, float32 loopRadius, float32 initialDisplacement) {
-		initialDisplacement = initialDisplacement;
+    void DampedSpring::load(Renderer2d* renderer, DampedSpringWeight* inWeight, float32 length, float32 loopRadius, float32 inInitialDisplacement, float32 inK, float32 inC) {
+		initialDisplacement = inInitialDisplacement;
         weight = inWeight;
-        discriminant = c * c - (4 * weight->mass * k);
+		k = inK;
+		c = inC;
 
-        if (discriminant < epsilon && discriminant > -epsilon) { // Real repeated root (~ zero): Overdamped motion
-			printf("Overdamped motion.\n");
-            type = DampedSpringType::Overdamped;
-        }
-        else if (discriminant > 0) { // Two real roots (greater than zero): Critically damped motion
+		// Set the quadratic terms to something that we're familiar with.
+		float32 A = weight->mass;
+		float32 B = c;
+		float32 C = k;
+		
+
+		discriminant = B * B - (4 * A * C);
+		timeElapsed = 0;
+		displacement = 0;
+
+        if (discriminant < epsilon && discriminant > -epsilon) {
+			// Real repeated root (~ zero): Critically damped motion
 			printf("Critically damped motion.\n");
-            type = DampedSpringType::Critical;
+			
+			type = DampedSpringType::Critical;
+
+			r1 = -B / (2 * A);
+			r2 = r1;
+
+			// Solve for the initial displacement.
+			c2 = (initialDisplacement - r1) / (r2);
+			c2 = r2 * c2;
+			c1 = r1;
+			printf("Equation: %f e^(%f t) + %f t e^(%f t)\n", c1, r1, c2, r2);
         }
-        else { // Complex pair (less than zero): Underdamped motion
+        else if (discriminant > 0) {
+			// Two real roots (greater than zero): Overdamped motion
+			printf("Overdamped motion.\n");
+			
+			type = DampedSpringType::Overdamped;
+
+			// Discover the two treal roots. Both of these WILL be negative.
+			float32 sqrtDis = sqrtf(discriminant);
+			r1 = (-B + sqrtDis) / (2 * A);
+			r2 = (-B - sqrtDis) / (2 * A);
+
+			// Now, solve for c1 and c2 given the initial displacement. The equation is:
+			// x(0) = r1 * c1 * e^(r1 * t) + r2 * c2 * e^(r2 * t)   ->
+			// x(0) = r1 * c1 + r2 * c2                             ->
+			// initialDisplacement  = r1 * c1 + r2 * c2;            -> if abs(r1) > abs(r2) => c1 = 1, else c2 = 1
+			if (r1 < r2) {
+				c2 = (initialDisplacement - r1) / (r2);
+				c2 = r2 * c2;
+				c1 = r1;
+			}
+			else {
+				c1 = (initialDisplacement - r2) / (r1);
+				c1 = r1 * c1;
+				c2 = r2;
+			}
+
+			printf("Equation: %f e^(%f t) + %f e^(%f t)\n", c1, r1, c2, r2);
+        }
+        else {
+			// Complex pair (less than zero): Underdamped motion
 			printf("Underdamped motion.\n");
 			
 			type = DampedSpringType::Underdamped;
-            omega1 = sqrtf(-discriminant) / (2.f * weight->mass); // Note that we negate the discriminant to get the REAL positive part.
+			float32 sqrtDis = sqrtf(-discriminant);
+            omega1 = sqrtf(sqrtDis) / (2.f * A); // Note that we negate the discriminant to get the REAL positive part.
+
+			r1 = (-B + sqrtDis) / (2 * A);
+			r2 = (-B - sqrtDis) / (2 * A);
+
+			// Now, solve for c1 and c2 given the initial displacement. The equation is:
+			// x(0) = r1 * c1 * e^(r1 * t) + r2 * c2 * e^(r2 * t)   ->
+			// x(0) = r1 * c1 + r2 * c2                             ->
+			// initialDisplacement  = r1 * c1 + r2 * c2;            -> if abs(r1) > abs(r2) => c1 = 1, else c2 = 1
+			if (r1 < r2) {
+				c2 = (initialDisplacement - r1) / (r2);
+				c2 = r2 * c2;
+				c1 = r1;
+			}
+			else {
+				c1 = (initialDisplacement - r2) / (r1);
+				c1 = r1 * c1;
+				c2 = r2;
+			}
+
+			// At this point, we have:
+			// x(t) = c1 cos(omega1 t) + c2 cos(omega1 t)     ->
+			// x(t) = A cos(omega1 t - Φ)                     ->
+			// Now we need to solve for A and Φ for our initial displacement, so:
+			// initialDisplacement = c1
+			R = sqrtf(c1 * c1 + c2 * c2);
+			if (initialDisplacement < 0) {
+				R = -R;
+			}
+			gamma = atanf(c2 / c1);
+
+			printf("Equation: %f e^(-(%f / %f) t) * cos(%f t - %f)\n", R, c, weight->mass, omega1, gamma);
         }
 
-		
-
-        timeElapsed = 0.f;
-
         const int32 verticesPerSegment = 6;
         numSegments = 256;
         numVertices = numSegments * verticesPerSegment;
@@ -226,16 +305,21 @@ namespace Damped {
 
 		float32 lastDisplacement = displacement;
 
-        if (discriminant < epsilon && discriminant > -epsilon) { // Real repeated root: Overdamped motion
-            
-        }
-        else if (discriminant > 0) { // Two real roots: Critically damped motion
-            
-        }
-        else { // Complex pair (less than zero): Underdamped motion
-            float32 exponent = (-c / (2 * weight->mass)) * timeElapsed;
+		switch (type) {
+		case DampedSpringType::Critical:
+			displacement = c1 * pow(E, r1 * timeElapsed) + c2 * timeElapsed * pow(E, r2 * timeElapsed);
+			break;
+		case DampedSpringType::Overdamped:
+			displacement = c1 * pow(E, r1 * timeElapsed) + c2 * pow(E, r2 * timeElapsed);
+			break;
+		case DampedSpringType::Underdamped:  {
+			float32 exponent = -(c / (2 * weight->mass)) * timeElapsed;
             displacement = R * pow(E, exponent) * (cosf(omega1 * timeElapsed - gamma));
-        }
+			break;
+		}
+		default:
+			break;
+		}
 
 		float32 dx = displacement - lastDisplacement;
         int32 vidx = 0;