physics: add particle_contact

src/physics/particle_contact.cpp

@@ -0,0 +1,105 @@
+#include "physics/particle_contact.hpp"
+
+namespace physics {
+
+  void particle_contact::resolve(float duration)
+  {
+    resolve_velocity(duration);
+    resolve_interpenetration(duration);
+  }
+
+  float particle_contact::calculate_separating_velocity() const
+  {
+    vec3 relative_velocity = particle[0]->velocity;
+    if (particle[1])
+      relative_velocity -= particle[1]->velocity;
+
+    return dot(relative_velocity, contact_normal);
+  }
+
+  void particle_contact::resolve_velocity(float duration)
+  {
+    float separating_velocity = calculate_separating_velocity();
+
+    if (separating_velocity > 0) {
+      // no impulse required
+      return;
+    }
+
+    float new_separating_velocity = -separating_velocity * restitution;
+
+    // velocity build-up due to acceleration only
+    vec3 acceleration_caused_velocity = particle[0]->acceleration;
+    if (particle[1]) acceleration_caused_velocity -= particle[1]->acceleration;
+    float acceleration_caused_separating_velocity = dot(acceleration_caused_velocity, contact_normal) * duration;
+
+    // remove acceleration velocity from separating velocity
+    if (acceleration_caused_separating_velocity < 0) {
+      new_separating_velocity += restitution * acceleration_caused_separating_velocity;
+
+      if (new_separating_velocity < 0) new_separating_velocity = 0;
+    }
+
+    float delta_velocity = new_separating_velocity - separating_velocity;
+
+    float total_inverse_mass = get_total_inverse_mass();
+
+    if (total_inverse_mass <= 0)
+      return;
+
+    float impulse = delta_velocity / total_inverse_mass;
+
+    vec3 impulse_per_inverse_mass = contact_normal * impulse;
+
+    particle[0]->velocity
+      = particle[0]->velocity
+      + impulse_per_inverse_mass * particle[0]->inverse_mass;
+
+    if (particle[1]) {
+      particle[1]->velocity
+        = particle[1]->velocity
+        + impulse_per_inverse_mass * -particle[0]->inverse_mass;
+    }
+  }
+
+  void particle_contact::resolve_interpenetration(float duration)
+  {
+    if (penetration <= 0) return;
+
+    float total_inverse_mass = get_total_inverse_mass();
+
+    if (total_inverse_mass <= 0) return;
+
+    vec3 move_per_inverse_mass = contact_normal * (-penetration / total_inverse_mass);
+
+    particle[0]->position += move_per_inverse_mass * particle[0]->inverse_mass;
+    if (particle[1]) {
+      particle[1]->position += move_per_inverse_mass * particle[1]->inverse_mass;
+    }
+  }
+
+  void particle_contact_resolver::resolve_contacts(particle_contact * contacts,
+                                                   int contacts_length,
+                                                   float duration)
+  {
+    iterations = 0;
+
+    while (iterations < max_iterations) {
+      float max = 0;
+      int max_index = -1;
+      for (int i = 0; i < contacts_length; i++) {
+        float separating_velocity = contacts[i].calculate_separating_velocity();
+        if (separating_velocity < max) {
+          max = separating_velocity;
+          max_index = i;
+        }
+      }
+
+      if (max_index == -1) break;
+
+      contacts[max_index].resolve(duration);
+
+      iterations += 1;
+    }
+  }
+}

src/physics/particle_contact.hpp

@@ -0,0 +1,43 @@
+#pragma once
+
+#include "physics/particle.hpp"
+
+namespace physics {
+
+  struct particle_contact
+  {
+    physics::particle * particle[2];
+
+    float restitution;
+
+    vec3 contact_normal;
+
+    float penetration;
+
+    void resolve(float duration);
+
+    float calculate_separating_velocity() const;
+
+    void resolve_velocity(float duration);
+
+    void resolve_interpenetration(float duration);
+
+    inline float get_total_inverse_mass()
+    {
+      float total_inverse_mass = particle[0]->inverse_mass;
+      if (particle[0]) total_inverse_mass += particle[1]->inverse_mass;
+      return total_inverse_mass;
+    }
+  };
+
+  struct particle_contact_resolver
+  {
+    int max_iterations;
+    int iterations;
+
+    void resolve_contacts(particle_contact * contacts,
+                          int contacts_length,
+                          float duration);
+  };
+
+}