implement verlet integration - much more stable

include/config.hpp

@@ -10,7 +10,7 @@ constexpr float VERTEX_SIZE = 5.0;
 constexpr Color VERTEX_COLOR = {27, 188, 104, 255};
 constexpr Color EDGE_COLOR = {20, 133, 38, 255};
 
-constexpr float SIM_SPEED = 2.0;
+constexpr float SIM_SPEED = 4.0;
 constexpr float CAMERA_DISTANCE = 2.2;
 
 constexpr float DEFAULT_SPRING_CONSTANT = 1.5;

include/mass_springs.hpp

@@ -3,27 +3,34 @@
 
 #include <cstddef>
 #include <raylib.h>
+#include <raymath.h>
 #include <vector>
 
 class Mass {
 public:
   float mass;
   Vector3 position;
+  Vector3 previous_position; // for verlet integration
   Vector3 velocity;
   Vector3 force;
   bool fixed;
 
 public:
   Mass(float mass, Vector3 position, bool fixed)
-      : mass(mass), position(position), fixed(fixed) {}
+      : mass(mass), position(position), previous_position(position),
+        velocity(Vector3Zero()), force(Vector3Zero()), fixed(fixed) {}
 
   Mass(const Mass &copy)
-      : mass(copy.mass), position(copy.position), fixed(copy.fixed) {};
+      : mass(copy.mass), position(copy.position),
+        previous_position(copy.previous_position), velocity(copy.velocity),
+        force(copy.force), fixed(copy.fixed) {};
 
   Mass &operator=(const Mass &copy) = delete;
 
   Mass(Mass &&move)
-      : mass(move.mass), position(move.position), fixed(move.fixed) {};
+      : mass(move.mass), position(move.position),
+        previous_position(move.previous_position), velocity(move.velocity),
+        force(move.force), fixed(move.fixed) {};
 
   Mass &operator=(Mass &&move) = delete;
 
@@ -35,6 +42,8 @@ public:
   auto CalculateVelocity(const float delta_time) -> void;
 
   auto CalculatePosition(const float delta_time) -> void;
+
+  auto VerletUpdate(const float delta_time) -> void;
 };
 
 using MassList = std::vector<Mass>;
@@ -43,7 +52,6 @@ class Spring {
 public:
   Mass &massA;
   Mass &massB;
-  int indexB;
   float spring_constant;
   float dampening_constant;
   float rest_length;
@@ -110,6 +118,8 @@ public:
   auto IntegrateVelocities(const float delta_time) -> void;
 
   auto IntegratePositions(const float delta_time) -> void;
+
+  auto VerletUpdate(const float delta_time) -> void;
 };
 
 #endif

src/main.cpp

@@ -1,5 +1,7 @@
 #include "mass_springs.hpp"
 
+#define VERLET_UPDATE
+
 #include <iostream>
 #include <raylib.h>
 #include <raymath.h>
@@ -28,15 +30,11 @@ auto main(int argc, char *argv[]) -> int {
   MassSpringSystem mass_springs;
   mass_springs.AddMass(1.0, Vector3(-0.5, 0.5, 0.0), true);
   mass_springs.AddMass(1.0, Vector3(0.5, 0.5, 0.0), false);
+  mass_springs.AddMass(1.0, Vector3(0.5, 0.0, 0.0), false);
   mass_springs.AddSpring(0, 1, DEFAULT_SPRING_CONSTANT,
                          DEFAULT_DAMPENING_CONSTANT, DEFAULT_REST_LENGTH);
-
-  Mass &massA = mass_springs.masses[0];
-  Mass &massB = mass_springs.masses[1];
-  std::cout << "Position: A: (" << massA.position.x << ", " << massA.position.y
-            << ", " << massA.position.z << ")" << std::endl;
-  std::cout << "Position: B: (" << massB.position.x << ", " << massB.position.y
-            << ", " << massB.position.z << ")" << std::endl;
+  mass_springs.AddSpring(1, 2, DEFAULT_SPRING_CONSTANT,
+                         DEFAULT_DAMPENING_CONSTANT, DEFAULT_REST_LENGTH);
 
   Renderer renderer(WIDTH, HEIGHT);
 
@@ -47,8 +45,12 @@ auto main(int argc, char *argv[]) -> int {
     frametime = GetFrameTime();
     mass_springs.ClearForces();
     mass_springs.CalculateSpringForces();
+#ifdef VERLET_UPDATE
+    mass_springs.VerletUpdate(frametime * SIM_SPEED);
+#else
     mass_springs.IntegrateVelocities(frametime * SIM_SPEED);
     mass_springs.IntegratePositions(frametime * SIM_SPEED);
+#endif
 
     // std::cout << "Calculating Spring Forces: A: (" << massA.force.x << ", "
     //           << massA.force.y << ", " << massA.force.z << ") B: ("

src/mass_springs.cpp

@@ -23,12 +23,32 @@ auto Mass::CalculatePosition(const float delta_time) -> void {
     return;
   }
 
+  previous_position = position;
+
   Vector3 temp;
 
   temp = Vector3Scale(velocity, delta_time);
   position = Vector3Add(position, temp);
 }
 
+auto Mass::VerletUpdate(const float delta_time) -> void {
+  if (fixed) {
+    return;
+  }
+
+  Vector3 acceleration = Vector3Scale(force, 1.0 / mass);
+  Vector3 temp_position = position;
+
+  Vector3 displacement = Vector3Subtract(position, previous_position);
+  Vector3 accel_term = Vector3Scale(acceleration, delta_time * delta_time);
+
+  // Minimal dampening
+  displacement = Vector3Scale(displacement, 0.99);
+
+  position = Vector3Add(Vector3Add(position, displacement), accel_term);
+  previous_position = temp_position;
+}
+
 auto Spring::CalculateSpringForce() -> void {
   Vector3 delta_position;
   float current_length;
@@ -95,3 +115,9 @@ auto MassSpringSystem::IntegratePositions(const float delta_time) -> void {
     mass.CalculatePosition(delta_time);
   }
 }
+
+auto MassSpringSystem::VerletUpdate(const float delta_time) -> void {
+  for (auto &mass : masses) {
+    mass.VerletUpdate(delta_time);
+  }
+}