From bca3f027970b2fd7f3448636364ba0aeaf347af3 Mon Sep 17 00:00:00 2001
From: Ruthie <s2169121@online.houstonisd.org>
Date: Sat, 30 May 2026 11:39:50 -0500
Subject: [PATCH] Add collision resolution for airborne gamepieces

Implemented pairwise collision resolution for airborne gamepieces, adjusting positions and velocities based on mass and restitution.
---
 core/driver/src/physics_world.cpp | 58 +++++++++++++++++++++++++++++++
 1 file changed, 58 insertions(+)

diff --git a/core/driver/src/physics_world.cpp b/core/driver/src/physics_world.cpp
index 24285621..920faf0f 100644
--- a/core/driver/src/physics_world.cpp
+++ b/core/driver/src/physics_world.cpp
@@ -358,6 +358,64 @@ void PhysicsWorld::step() {
     ball.setState(s);
   }
 
+  // Resolve ball <-> ball collisions (pairwise) so gamepieces interact with each
+  // other. We only consider airborne gamepieces here to match rigid-body
+  // collision handling.
+  for (std::size_t i = 0; i < gamepieces_.size(); ++i) {
+    for (std::size_t j = i + 1; j < gamepieces_.size(); ++j) {
+      auto& a = gamepieces_[i];
+      auto& b = gamepieces_[j];
+
+      if (a.getGamepieceState() != Gamepiece::State::kAirborne ||
+          b.getGamepieceState() != Gamepiece::State::kAirborne) {
+        continue;
+      }
+
+      auto sa = a.state();
+      auto sb = b.state();
+
+      const auto& ap = a.ballProperties();
+      const auto& bp = b.ballProperties();
+      const double ra = std::max(0.0, ap.radius_m);
+      const double rb = std::max(0.0, bp.radius_m);
+      const double ma = std::max(1e-9, ap.mass_kg);
+      const double mb = std::max(1e-9, bp.mass_kg);
+      const double inv_ma = 1.0 / ma;
+      const double inv_mb = 1.0 / mb;
+
+      Vector3 rel = sa.position_m - sb.position_m;
+      const double dist = rel.norm();
+      const double contact_dist = ra + rb;
+      if (dist <= contact_dist) {
+        Vector3 normal = dist > 1e-9 ? rel / dist : Vector3::unitZ();
+
+        // positional correction: separate proportionally to mass
+        const double penetration = contact_dist - dist;
+        const double total_m = ma + mb;
+        if (total_m > 0.0) {
+          sa.position_m += normal * (penetration * (mb / total_m));
+          sb.position_m -= normal * (penetration * (ma / total_m));
+        }
+
+        const Vector3 rel_vel = sa.velocity_mps - sb.velocity_mps;
+        const double vn = rel_vel.dot(normal);
+        if (vn < 0.0) {
+          const double ea = ap.restitution;
+          const double eb = bp.restitution;
+          const double e = std::clamp(0.5 * (ea + eb), 0.0, 1.0);
+          const double j = -(1.0 + e) * vn / (inv_ma + inv_mb);
+          const Vector3 impulse = normal * j;
+
+          sa.velocity_mps += impulse * inv_ma;
+          sb.velocity_mps -= impulse * inv_mb;
+        }
+
+        a.setState(sa);
+        b.setState(sb);
+      }
+    }
+  }
+
   ++step_count_;
   accumulated_sim_time_s_ += dt_s;
 }