collision2d-impulse-solver

.:
CMakeLists.txt
examples
include
src
tests

./examples:
head_on.collision

./include:
collision2d

./include/collision2d:
io.hpp
solver.hpp
types.hpp

./src:
io.cpp
main.cpp
solver.cpp
types.cpp

./tests:
CMakeLists.txt
test_smoke.cpp
test_util.hpp

[stdout]
.:
CMakeLists.txt
examples
include
src
tests

./examples:
head_on.collision

./include:
collision2d

./include/collision2d:
io.hpp
solver.hpp
types.hpp

./src:
io.cpp
main.cpp
solver.cpp
types.cpp

./tests:
CMakeLists.txt
test_smoke.cpp
test_util.hpp

1	#ifndef COLLISION2D_TYPES_HPP
2	#define COLLISION2D_TYPES_HPP
3	
4	#include <cmath>
5	#include <cstddef>
6	#include <vector>
7	
8	namespace collision2d {
9	
10	/// A vector / point in the 2D plane with the usual linear-algebra operations.
11	struct Vec2 {
12	    double x{0.0};
13	    double y{0.0};
14	};
15	
16	inline Vec2 operator+(const Vec2& a, const Vec2& b) { return Vec2{a.x + b.x, a.y + b.y}; }
17	inline Vec2 operator-(const Vec2& a, const Vec2& b) { return Vec2{a.x - b.x, a.y - b.y}; }
18	inline Vec2 operator*(double s, const Vec2& a) { return Vec2{s * a.x, s * a.y}; }
19	inline Vec2 operator*(const Vec2& a, double s) { return s * a; }
20	
21	/// Euclidean dot product a . b.
22	inline double dot(const Vec2& a, const Vec2& b) { return a.x * b.x + a.y * b.y; }
23	
24	/// Scalar 2D cross product a x b = a.x*b.y - a.y*b.x (signed out-of-plane z).
25	inline double cross(const Vec2& a, const Vec2& b) { return a.x * b.y - a.y * b.x; }
26	
27	/// Cross of a scalar (out-of-plane) w with an in-plane vector v: (-w*v.y, w*v.x).
28	inline Vec2 cross(double w, const Vec2& v) { return Vec2{-w * v.y, w * v.x}; }
29	
30	/// Squared Euclidean length.
31	inline double length_sq(const Vec2& a) { return a.x * a.x + a.y * a.y; }
32	
33	/// Rotate a vector by the angle whose cosine is `c` and sine is `s`.
34	inline Vec2 rotate(const Vec2& v, double c, double s) {
35	    return Vec2{c * v.x - s * v.y, s * v.x + c * v.y};
36	}
37	
38	/// The geometric shape of a rigid body.
39	enum class Shape { Disk, Polygon };
40	
41	/// A rigid body in the plane: a disk OR a convex polygon, each with mass,
42	/// centre-of-mass position, linear velocity, and angular velocity (spin). A
43	/// polygon additionally has an orientation and shape-dependent rotational
44	/// inertia. All SI. The class is a fully implemented data container plus
45	/// kinematic helpers; the contact resolution lives in ContactSolver.
46	///
47	///   mass     m   [kg]    (> 0)
48	///   position p   [m]     centre of mass
49	///   velocity v   [m/s]   linear velocity of the centre of mass
50	///   omega    w   [rad/s] angular velocity (counter-clockwise +)
51	///   theta        [rad]   orientation (polygons only; inert for disks)
52	///
53	/// A uniform solid disk has inertia I = (1/2) m R^2 about its centre. A uniform
54	/// convex polygon has the shape-dependent moment of inertia about its centroid.
55	class Body {
56	public:
57	    /// Disk body of radius R [m] (> 0).
58	    Body(double mass, double radius, Vec2 position, Vec2 velocity, double omega = 0.0);
59	
60	    /// Convex polygon body. `local_vertices` are the polygon's corners in the
61	    /// body-local frame, counter-clockwise; they are recentred on their centroid
62	    /// so `position` is the centre of mass. `theta` is the orientation [rad].
63	    /// Throws std::invalid_argument if fewer than 3 vertices or non-positive area.
64	    Body(double mass, std::vector<Vec2> local_vertices, Vec2 position,
65	         Vec2 velocity, double theta = 0.0, double omega = 0.0);
66	
67	    double mass() const { return mass_; }
68	    /// Disk radius; for a polygon, the bounding radius (max centroid-to-vertex).
69	    double radius() const { return radius_; }
70	    const Vec2& position() const { return position_; }
71	    const Vec2& velocity() const { return velocity_; }
72	    double omega() const { return omega_; }
73	
74	    void set_velocity(const Vec2& v) { velocity_ = v; }
75	    void set_omega(double w) { omega_ = w; }
76	    void set_position(const Vec2& p) { position_ = p; }
77	
78	    Shape shape() const { return shape_; }
79	    double theta() const { return theta_; }
80	    void set_theta(double t) { theta_ = t; }
81	
82	    std::size_t vertex_count() const { return local_verts_.size(); }
83	    const std::vector<Vec2>& local_vertices() const { return local_verts_; }
84	
85	    /// World-space position of polygon vertex `i` = position + R(theta)*local_i.
86	    Vec2 world_vertex(std::size_t i) const {
87	        const double c = std::cos(theta_), s = std::sin(theta_);
88	        return position_ + rotate(local_verts_[i], c, s);
89	    }
90	    /// World-space outward unit normal of polygon face `i` (edge i -> i+1, CCW).
91	    Vec2 world_face_normal(std::size_t i) const;
92	
93	    double inv_mass() const { return 1.0 / mass_; }
94	    /// Moment of inertia about the centre of mass [kg.m^2] (shape-correct).
95	    double inertia() const { return inertia_; }
96	    double inv_inertia() const { return 1.0 / inertia_; }
97	
98	    /// Velocity of the material point at world position `point`,
99	    /// v_point = v + w x (point - centre).
100	    Vec2 velocity_at(const Vec2& point) const {
101	        return velocity_ + cross(omega_, point - position_);
102	    }
103	
104	    /// Translational + rotational kinetic energy [J].
105	    double kinetic_energy() const;
106	    /// Linear momentum m v [kg.m/s].
107	    Vec2 momentum() const;
108	    /// Angular momentum about the world origin, L = I w + m (p x v) [kg.m^2/s].
109	    double angular_momentum() const;
110	
111	private:
112	    double mass_{0.0};
113	    double radius_{0.0};
114	    Vec2 position_{0.0, 0.0};
115	    Vec2 velocity_{0.0, 0.0};
116	    double omega_{0.0};
117	    Shape shape_{Shape::Disk};
118	    double theta_{0.0};
119	    double inertia_{0.0};
120	    std::vector<Vec2> local_verts_;  // CCW, centroid at local origin (polygons)
121	};
122	
123	} // namespace collision2d
124	
125	#endif // COLLISION2D_TYPES_HPP
126

1	#ifndef COLLISION2D_SOLVER_HPP
2	#define COLLISION2D_SOLVER_HPP
3	
4	#include <cstddef>
5	#include <vector>
6	
7	#include "collision2d/types.hpp"
8	
9	namespace collision2d {
10	
11	/// A single pairwise contact constraint between two overlapping disks.
12	///
13	///   a, b         indices into the body array (a < b)
14	///   normal       UNIT contact normal pointing from body a toward body b
15	///   point        world-space contact point (in the overlap region, on the line
16	///                of centers)
17	///   penetration  overlap depth (>= 0)
18	struct Contact {
19	    std::size_t a{0};
20	    std::size_t b{0};
21	    Vec2 normal{1.0, 0.0};
22	    Vec2 point{0.0, 0.0};
23	    double penetration{0.0};
24	};
25	
26	/// Diagnostics returned by a velocity solve.
27	struct SolveReport {
28	    int iterations{0};         ///< sweeps actually performed
29	    double max_residual{0.0};  ///< convergence residual at exit
30	    double total_normal_impulse{0.0}; ///< summed accumulated normal impulse
31	    bool converged{false};     ///< residual fell below tol before the cap
32	};
33	
34	/// Constraint solver for the simultaneous, possibly multi-contact collision of a
35	/// system of rigid disks with rotation, tangential Coulomb friction, and Newton
36	/// restitution. The continuum model and the contract are specified in
37	/// instruction.md; designing and implementing the contact detection, the
38	/// velocity-level impulse resolution, and the positional de-penetration is the
39	/// work of this class.
40	///
41	/// Physical expectations a correct solver must satisfy: contacts exchange equal
42	/// and opposite impulses (so total linear and angular momentum are conserved);
43	/// kinetic energy never increases (and is conserved only for a perfectly elastic
44	/// frictionless impact); the post-impact normal separation obeys the restitution
45	/// coefficient; tangential response obeys the Coulomb friction cone; and the
46	/// positional correction removes overlap WITHOUT changing velocities or adding
47	/// energy.
48	class ContactSolver {
49	public:
50	    /// Construct with the coefficient of restitution `e` in [0, 1], the Coulomb
51	    /// friction coefficient `mu` >= 0, the maximum number of velocity sweeps
52	    /// `iterations` >= 1, the velocity convergence tolerance `tol` > 0, the
53	    /// positional bias factor `bias_factor` in [0, 1] (fraction of the excess
54	    /// penetration removed by correct_positions), and the penetration `slop`
55	    /// >= 0 (allowed residual overlap left uncorrected). Throws
56	    /// std::invalid_argument on any out-of-range argument.
57	    ContactSolver(double e, double mu, int iterations = 64, double tol = 1e-12,
58	                  double bias_factor = 0.2, double slop = 1e-3);
59	
60	    double restitution() const { return e_; }
61	    double friction() const { return mu_; }
62	    double bias_factor() const { return bias_factor_; }
63	    double slop() const { return slop_; }
64	
65	    /// Find every pairwise contact among the disks: a pair (i, j), i < j, is in
66	    /// contact iff the centers are closer than the sum of radii. The normal
67	    /// points from i toward j. Throws std::runtime_error if two *overlapping*
68	    /// disks share the same center (normal undefined).
69	    static std::vector<Contact> detect_contacts(const std::vector<Body>& bodies);
70	
71	    /// Resolve the given contacts in place by applying contact impulses,
72	    /// mutating the bodies' linear and angular velocities (only). Returns a
73	    /// SolveReport. The bodies vector and the contacts must be consistent.
74	    SolveReport solve(std::vector<Body>& bodies,
75	                      const std::vector<Contact>& contacts) const;
76	
77	    /// Correct positional overlap for the given contacts by moving the bodies'
78	    /// positions apart (only) so that residual penetration is reduced toward the
79	    /// slop. Must not change any velocity and must not add kinetic energy.
80	    void correct_positions(std::vector<Body>& bodies,
81	                           const std::vector<Contact>& contacts) const;
82	
83	    /// Convenience: detect_contacts, then solve, then correct_positions.
84	    SolveReport resolve(std::vector<Body>& bodies) const;
85	
86	    /// Continuous collision: the earliest time t in [0, 1] at which any pair of
87	    /// bodies FIRST comes into contact while translating along
88	    /// pos_i(t) = pos_i + t * vel_i (orientation held fixed for the sweep).
89	    /// Returns 1.0 if no new contact forms within the step, and 0.0 if a pair is
90	    /// already overlapping at t = 0.
91	    double time_of_impact(const std::vector<Body>& bodies) const;
92	
93	    /// One continuous step: advance every body to the time of impact t*
94	    /// (position += t* * velocity), then resolve the contacts there
95	    /// (detect_contacts -> solve -> correct_positions). Returns the solve report.
96	    SolveReport step(std::vector<Body>& bodies) const;
97	
98	private:
99	    double e_;
100	    double mu_;
101	    int iterations_;
102	    double tol_;
103	    double bias_factor_;
104	    double slop_;
105	};
106	
107	} // namespace collision2d
108	
109	#endif // COLLISION2D_SOLVER_HPP
110

1	#include "collision2d/solver.hpp"
2	
3	#include <cmath>
4	#include <stdexcept>
5	
6	#include "collision2d/types.hpp"
7	
8	namespace collision2d {
9	
10	// ============================================================================
11	// TODO(candidate): Implement the N-body rigid-disk contact solver.
12	//
13	// The three methods below are stubs: detect_contacts() reports no contacts,
14	// solve() applies no impulses, and correct_positions() moves nothing, so the
15	// system never reacts to a collision and the grader FAILS. Replace the stub
16	// bodies with correct implementations of the model specified in instruction.md.
17	// Do NOT change the public signatures declared in include/collision2d/
18	// solver.hpp; reuse the Vec2 helpers (dot, cross, perp via cross(w, v)) and the
19	// Body accessors (inv_mass, inv_inertia, velocity_at, set_velocity, set_omega,
20	// set_position). You should only need to edit src/solver.cpp.
21	//
22	// Implement:
23	//   * detect_contacts(): the overlapping pairs, each with a unit normal,
24	//     penetration depth, and a contact point (throwing on coincident centers of
25	//     an overlapping pair);
26	//   * solve(): the velocity-level contact resolution (restitution + Coulomb
27	//     friction + rotation), consistent across all simultaneous contacts;
28	//   * correct_positions(): the positional de-penetration pass.
29	// The required physics, conventions, and contract are described in
30	// instruction.md; grading is performed by a hidden suite (the shipped test is a
31	// smoke check only).
32	// ============================================================================
33	
34	ContactSolver::ContactSolver(double e, double mu, int iterations, double tol,
35	                             double bias_factor, double slop)
36	    : e_(e),
37	      mu_(mu),
38	      iterations_(iterations),
39	      tol_(tol),
40	      bias_factor_(bias_factor),
41	      slop_(slop) {
42	    if (!(e_ >= 0.0 && e_ <= 1.0)) {
43	        throw std::invalid_argument("ContactSolver: restitution must lie in [0, 1]");
44	    }
45	    if (!(mu_ >= 0.0)) {
46	        throw std::invalid_argument(
47	            "ContactSolver: friction coefficient must be non-negative");
48	    }
49	    if (iterations_ < 1) {
50	        throw std::invalid_argument("ContactSolver: iterations must be at least 1");
51	    }
52	    if (!(tol_ > 0.0)) {
53	        throw std::invalid_argument("ContactSolver: tol must be positive");
54	    }
55	    if (!(bias_factor_ >= 0.0 && bias_factor_ <= 1.0)) {
56	        throw std::invalid_argument("ContactSolver: bias_factor must lie in [0, 1]");
57	    }
58	    if (!(slop_ >= 0.0)) {
59	        throw std::invalid_argument("ContactSolver: slop must be non-negative");
60	    }
61	}
62	
63	std::vector<Contact>
64	ContactSolver::detect_contacts(const std::vector<Body>& bodies) {
65	    // TODO(candidate): report every overlapping pair as a contact.
66	    (void)bodies;
67	    return {}; // no contacts found -> wrong
68	}
69	
70	SolveReport ContactSolver::solve(std::vector<Body>& bodies,
71	                                 const std::vector<Contact>& contacts) const {
72	    // TODO(candidate): resolve the contacts by applying contact impulses.
73	    (void)bodies;
74	    (void)contacts;
75	    return SolveReport{}; // no impulses applied -> wrong
76	}
77	
78	void ContactSolver::correct_positions(
79	    std::vector<Body>& bodies, const std::vector<Contact>& contacts) const {
80	    // TODO(candidate): remove positional overlap (positions only, no energy).
81	    (void)bodies;
82	    (void)contacts;
83	}
84	
85	SolveReport ContactSolver::resolve(std::vector<Body>& bodies) const {
86	    const std::vector<Contact> contacts = detect_contacts(bodies);
87	    SolveReport report = solve(bodies, contacts);
88	    correct_positions(bodies, contacts);
89	    return report;
90	}
91	
92	double ContactSolver::time_of_impact(const std::vector<Body>& bodies) const {
93	    // TODO(candidate): earliest contact time under linear translation.
94	    (void)bodies;
95	    return 1.0;  // claims no impact -> wrong
96	}
97	
98	SolveReport ContactSolver::step(std::vector<Body>& bodies) const {
99	    // TODO(candidate): advance to the time of impact, then resolve there.
100	    const double t = time_of_impact(bodies);
101	    for (Body& b : bodies) b.set_position(b.position() + t * b.velocity());
102	    return resolve(bodies);
103	}
104	
105	} // namespace collision2d
106

1	#include "collision2d/types.hpp"
2	
3	#include <algorithm>
4	#include <cmath>
5	#include <stdexcept>
6	
7	namespace collision2d {
8	
9	Body::Body(double mass, double radius, Vec2 position, Vec2 velocity, double omega)
10	    : mass_(mass),
11	      radius_(radius),
12	      position_(position),
13	      velocity_(velocity),
14	      omega_(omega),
15	      shape_(Shape::Disk),
16	      theta_(0.0),
17	      inertia_(0.5 * mass * radius * radius) {
18	    if (!(mass_ > 0.0)) {
19	        throw std::invalid_argument("Body: mass must be positive");
20	    }
21	    if (!(radius_ > 0.0)) {
22	        throw std::invalid_argument("Body: radius must be positive");
23	    }
24	}
25	
26	Body::Body(double mass, std::vector<Vec2> local_vertices, Vec2 position,
27	           Vec2 velocity, double theta, double omega)
28	    : mass_(mass),
29	      position_(position),
30	      velocity_(velocity),
31	      omega_(omega),
32	      shape_(Shape::Polygon),
33	      theta_(theta) {
34	    if (!(mass_ > 0.0)) {
35	        throw std::invalid_argument("Body: mass must be positive");
36	    }
37	    if (local_vertices.size() < 3) {
38	        throw std::invalid_argument("Body: polygon needs at least 3 vertices");
39	    }
40	    const std::size_t n = local_vertices.size();
41	
42	    // Signed area and centroid (shoelace) in the supplied local frame.
43	    double area2 = 0.0;        // 2 * signed area
44	    Vec2 c{0.0, 0.0};
45	    for (std::size_t i = 0; i < n; ++i) {
46	        const Vec2& p = local_vertices[i];
47	        const Vec2& q = local_vertices[(i + 1) % n];
48	        const double cr = cross(p, q);  // x_i y_{i+1} - x_{i+1} y_i
49	        area2 += cr;
50	        c = c + (cr * (p + q));
51	    }
52	    const double area = 0.5 * area2;
53	    if (!(area > 1e-12)) {
54	        throw std::invalid_argument(
55	            "Body: polygon must have positive area (counter-clockwise vertices)");
56	    }
57	    c = (1.0 / (6.0 * area)) * c;  // centroid in local frame
58	
59	    // Recentre vertices on the centroid so `position` is the centre of mass.
60	    local_verts_.resize(n);
61	    for (std::size_t i = 0; i < n; ++i) local_verts_[i] = local_vertices[i] - c;
62	
63	    // Polar second moment of area about the centroid, then scale by density.
64	    double J = 0.0;
65	    for (std::size_t i = 0; i < n; ++i) {
66	        const Vec2& p = local_verts_[i];
67	        const Vec2& q = local_verts_[(i + 1) % n];
68	        const double cr = cross(p, q);
69	        J += cr * (dot(p, p) + dot(p, q) + dot(q, q));
70	    }
71	    J /= 12.0;
72	    inertia_ = (mass_ / area) * J;
73	    if (!(inertia_ > 0.0)) {
74	        throw std::invalid_argument("Body: degenerate polygon inertia");
75	    }
76	
77	    // Bounding radius (broad phase) = max centroid-to-vertex distance.
78	    double r2 = 0.0;
79	    for (const Vec2& v : local_verts_) r2 = std::max(r2, length_sq(v));
80	    radius_ = std::sqrt(r2);
81	}
82	
83	Vec2 Body::world_face_normal(std::size_t i) const {
84	    const std::size_t n = local_verts_.size();
85	    const Vec2 a = world_vertex(i);
86	    const Vec2 b = world_vertex((i + 1) % n);
87	    const Vec2 e = b - a;                 // edge direction (CCW)
88	    Vec2 nrm{e.y, -e.x};                  // outward (right of CCW edge)
89	    const double len = std::sqrt(length_sq(nrm));
90	    if (len > 0.0) nrm = (1.0 / len) * nrm;
91	    return nrm;
92	}
93	
94	double Body::kinetic_energy() const {
95	    return 0.5 * mass_ * dot(velocity_, velocity_) + 0.5 * inertia_ * omega_ * omega_;
96	}
97	
98	Vec2 Body::momentum() const { return mass_ * velocity_; }
99	
100	double Body::angular_momentum() const {
101	    return inertia_ * omega_ + mass_ * cross(position_, velocity_);
102	}
103	
104	} // namespace collision2d
105

1	// Smoke test: a minimal, non-leaky sanity check so the project compiles and a
2	// solver gets quick feedback. Asserts only generic properties (a contact is
3	// found, the bodies react, momentum is conserved) and contains NO closed-form
4	// answer. The authoritative grading is a separate hidden suite.
5	
6	#include <cmath>
7	#include <vector>
8	
9	#include "collision2d/solver.hpp"
10	#include "collision2d/types.hpp"
11	#include "test_util.hpp"
12	
13	using namespace collision2d;
14	
15	COLLISION2D_TEST("smoke_two_disks_react") {
16	    std::vector<Body> b{Body(1.0, 0.5, {0, 0}, {2, 0}), Body(1.0, 0.5, {0.9, 0}, {-1, 0})};
17	    Vec2 p0{0, 0};
18	    for (const Body& x : b) p0 = p0 + x.momentum();
19	
20	    std::vector<Contact> cs = ContactSolver::detect_contacts(b);
21	    collision2d_test::expect(cs.size() == 1, "one contact detected for the overlapping pair");
22	
23	    ContactSolver(1.0, 0.0).solve(b, cs);
24	    Vec2 p1{0, 0};
25	    for (const Body& x : b) p1 = p1 + x.momentum();
26	
27	    for (const Body& x : b) collision2d_test::expect(std::isfinite(x.velocity().x), "finite velocity");
28	    collision2d_test::expect(b[0].velocity().x < 2.0 - 1e-9, "approaching body reacted to the contact");
29	    collision2d_test::expect(std::fabs(p1.x - p0.x) < 1e-9 && std::fabs(p1.y - p0.y) < 1e-9,
30	                             "linear momentum conserved");
31	}
32	
33	COLLISION2D_TEST_MAIN()
34

1	#ifndef COLLISION2D_TEST_UTIL_HPP
2	#define COLLISION2D_TEST_UTIL_HPP
3	
4	// Minimal dependency-free test harness for the collision2d suite.
5	//
6	// Each test executable defines test functions and registers them with
7	// COLLISION2D_TEST(name) { ... }. main() (provided by COLLISION2D_TEST_MAIN)
8	// runs every registered test and returns non-zero if any assertion fails, which
9	// CTest interprets as a failure.
10	
11	#include <cmath>
12	#include <cstddef>
13	#include <functional>
14	#include <iostream>
15	#include <string>
16	#include <vector>
17	
18	namespace collision2d_test {
19	
20	struct Case {
21	    std::string name;
22	    std::function<void()> fn;
23	};
24	
25	inline std::vector<Case>& registry() {
26	    static std::vector<Case> cases;
27	    return cases;
28	}
29	
30	struct Registrar {
31	    Registrar(const std::string& name, std::function<void()> fn) {
32	        registry().push_back(Case{name, std::move(fn)});
33	    }
34	};
35	
36	// Thrown by an assertion failure to abort the current test case.
37	struct AssertionError {
38	    std::string message;
39	};
40	
41	inline void expect(bool cond, const std::string& what) {
42	    if (!cond) {
43	        throw AssertionError{what};
44	    }
45	}
46	
47	inline void expect_near(double got, double want, double tol, const std::string& what) {
48	    if (std::fabs(got - want) > tol) {
49	        throw AssertionError{what + " (got " + std::to_string(got) +
50	                             ", want " + std::to_string(want) +
51	                             ", tol " + std::to_string(tol) + ")"};
52	    }
53	}
54	
55	// Relative-error check (with an absolute floor) for quantities whose scale
56	// varies widely across cases.
57	inline void expect_rel(double got, double want, double rel_tol,
58	                       const std::string& what) {
59	    const double denom = std::fabs(want) > 1e-300 ? std::fabs(want) : 1.0;
60	    if (std::fabs(got - want) / denom > rel_tol) {
61	        throw AssertionError{what + " (got " + std::to_string(got) +
62	                             ", want " + std::to_string(want) +
63	                             ", rel_tol " + std::to_string(rel_tol) + ")"};
64	    }
65	}
66	
67	inline int run_all() {
68	    int failures = 0;
69	    for (const auto& c : registry()) {
70	        try {
71	            c.fn();
72	            std::cout << "[ PASS ] " << c.name << "\n";
73	        } catch (const AssertionError& e) {
74	            std::cout << "[ FAIL ] " << c.name << ": " << e.message << "\n";
75	            ++failures;
76	        } catch (const std::exception& e) {
77	            std::cout << "[ FAIL ] " << c.name
78	                      << ": unexpected exception: " << e.what() << "\n";
79	            ++failures;
80	        } catch (...) {
81	            std::cout << "[ FAIL ] " << c.name << ": unknown exception\n";
82	            ++failures;
83	        }
84	    }
85	    std::cout << "----\n"
86	              << (registry().size() - failures) << "/" << registry().size()
87	              << " tests passed\n";
88	    return failures == 0 ? 0 : 1;
89	}
90	
91	} // namespace collision2d_test
92	
93	#define COLLISION2D_CONCAT_INNER(a, b) a##b
94	#define COLLISION2D_CONCAT(a, b) COLLISION2D_CONCAT_INNER(a, b)
95	
96	#define COLLISION2D_TEST(NAME)                                                 \
97	    static void COLLISION2D_CONCAT(collision2d_test_fn_, __LINE__)();          \
98	    static ::collision2d_test::Registrar COLLISION2D_CONCAT(                   \
99	        collision2d_test_reg_, __LINE__){                                      \
100	        NAME, &COLLISION2D_CONCAT(collision2d_test_fn_, __LINE__)};            \
101	    static void COLLISION2D_CONCAT(collision2d_test_fn_, __LINE__)()
102	
103	#define COLLISION2D_TEST_MAIN()                                                \
104	    int main() { return ::collision2d_test::run_all(); }
105	
106	#endif // COLLISION2D_TEST_UTIL_HPP
107

1	#ifndef COLLISION2D_IO_HPP
2	#define COLLISION2D_IO_HPP
3	
4	#include <iosfwd>
5	#include <string>
6	#include <vector>
7	
8	#include "collision2d/solver.hpp"
9	#include "collision2d/types.hpp"
10	
11	namespace collision2d {
12	
13	/// A fully-parsed multi-body collision scenario: a list of disks plus the
14	/// global coefficient of restitution and Coulomb friction coefficient.
15	struct Scenario {
16	    std::vector<Body> bodies;
17	    double restitution{1.0};
18	    double friction{0.0};
19	};
20	
21	/// Parse a collision scenario from the engine's plain-text `.collision` format.
22	///
23	/// The format is line-oriented. Blank lines and lines beginning with '#' are
24	/// ignored. Recognized records:
25	///   body <m> <R> <px> <py> <vx> <vy> [w]   # one disk (omega optional, default 0)
26	///   poly <m> <px> <py> <theta> <vx> <vy> <w> <k> <x0> <y0> ...  # convex polygon
27	///                                          # (k>=3 local CCW vertices, centroid at origin)
28	///   restitution <e>                        # coefficient of restitution in [0,1]
29	///   friction <mu>                          # Coulomb friction coefficient >= 0
30	///
31	/// At least two `body`/`poly` records and a `restitution` are required;
32	/// `friction` defaults to 0. Throws std::runtime_error on malformed input or a
33	/// missing required record.
34	Scenario parse_scenario(std::istream& in);
35	
36	/// Convenience overload that parses from a file path.
37	Scenario parse_scenario_file(const std::string& path);
38	
39	/// Write a human-readable collision report (number of contacts, the solver
40	/// iterations/residual, and the pre/post total momentum, angular momentum, and
41	/// kinetic energy) to `out`. Fully implemented; used by the demo CLI.
42	void write_report(std::ostream& out, const Scenario& scenario);
43	
44	} // namespace collision2d
45	
46	#endif // COLLISION2D_IO_HPP
47

1	#include <cstdlib>
2	#include <iostream>
3	
4	#include "collision2d/io.hpp"
5	#include "collision2d/solver.hpp"
6	#include "collision2d/types.hpp"
7	
8	// Command-line demo driver: read a .collision scenario, resolve the impulse,
9	// and print the pre/post velocities, momentum, and energy.
10	//   collision2d_cli <scenario.collision>
11	int main(int argc, char** argv) {
12	    if (argc != 2) {
13	        std::cerr << "usage: " << argv[0] << " <scenario.collision>\n";
14	        return 2;
15	    }
16	    try {
17	        collision2d::Scenario scenario =
18	            collision2d::parse_scenario_file(argv[1]);
19	        collision2d::write_report(std::cout, scenario);
20	    } catch (const std::exception& ex) {
21	        std::cerr << "error: " << ex.what() << "\n";
22	        return 1;
23	    }
24	    return 0;
25	}
26

1	# A three-disk scenario resolved by the N-body sequential-impulse solver.
2	#
3	# Disk 0 (2 kg, R=0.5) slides right and is about to strike a stationary line of
4	# two touching disks (Newton's-cradle style); disk 2 also carries some spin so
5	# the tangential Coulomb friction does work. Restitution 0.9, friction 0.3.
6	#
7	# format: body  m  R  px py  vx vy  [omega]
8	body  2.0  0.5   0.0  0.0    3.0  0.0
9	body  1.0  0.5   1.0  0.0    0.0  0.0
10	body  1.0  0.5   2.0  0.0    0.0  0.0   5.0
11	restitution 0.9
12	friction 0.3
13

1	#include "collision2d/io.hpp"
2	
3	#include <cmath>
4	#include <fstream>
5	#include <iomanip>
6	#include <optional>
7	#include <sstream>
8	#include <stdexcept>
9	#include <string>
10	#include <vector>
11	
12	namespace collision2d {
13	
14	namespace {
15	
16	[[noreturn]] void fail(std::size_t line_no, const std::string& msg) {
17	    std::ostringstream oss;
18	    oss << "parse_scenario: line " << line_no << ": " << msg;
19	    throw std::runtime_error(oss.str());
20	}
21	
22	struct BodyData {
23	    bool poly{false};
24	    double m{0}, R{0}, px{0}, py{0}, vx{0}, vy{0}, theta{0}, w{0};
25	    std::vector<Vec2> verts;  // local-frame vertices for a polygon
26	};
27	
28	} // namespace
29	
30	Scenario parse_scenario(std::istream& in) {
31	    std::vector<BodyData> bodies;
32	    std::optional<double> e;
33	    std::optional<double> mu;
34	
35	    std::string line;
36	    std::size_t line_no = 0;
37	    while (std::getline(in, line)) {
38	        ++line_no;
39	        const auto hash = line.find('#');
40	        if (hash != std::string::npos) line.erase(hash);
41	        std::istringstream ls(line);
42	        std::string tag;
43	        if (!(ls >> tag)) continue;
44	
45	        if (tag == "body") {
46	            BodyData d;
47	            if (!(ls >> d.m >> d.R >> d.px >> d.py >> d.vx >> d.vy))
48	                fail(line_no, "body needs m R px py vx vy [w]");
49	            ls >> d.w;  // optional spin
50	            bodies.push_back(d);
51	        } else if (tag == "poly") {
52	            BodyData d;
53	            d.poly = true;
54	            std::size_t k = 0;
55	            if (!(ls >> d.m >> d.px >> d.py >> d.theta >> d.vx >> d.vy >> d.w >> k))
56	                fail(line_no, "poly needs m px py theta vx vy w k <2k vertex coords>");
57	            if (k < 3) fail(line_no, "poly needs at least 3 vertices");
58	            for (std::size_t i = 0; i < k; ++i) {
59	                double x, y;
60	                if (!(ls >> x >> y)) fail(line_no, "poly: not enough vertex coordinates");
61	                d.verts.push_back(Vec2{x, y});
62	            }
63	            bodies.push_back(d);
64	        } else if (tag == "restitution") {
65	            double v;
66	            if (!(ls >> v)) fail(line_no, "restitution needs a value");
67	            e = v;
68	        } else if (tag == "friction") {
69	            double v;
70	            if (!(ls >> v)) fail(line_no, "friction needs a value");
71	            mu = v;
72	        } else {
73	            fail(line_no, "unknown record '" + tag + "'");
74	        }
75	    }
76	
77	    if (bodies.size() < 2) fail(line_no, "need at least two body/poly records");
78	    if (!e) fail(line_no, "missing required record: restitution");
79	
80	    Scenario sc;
81	    sc.restitution = *e;
82	    sc.friction = mu.value_or(0.0);
83	    for (const BodyData& d : bodies) {
84	        try {
85	            if (d.poly)
86	                sc.bodies.emplace_back(d.m, d.verts, Vec2{d.px, d.py}, Vec2{d.vx, d.vy}, d.theta, d.w);
87	            else
88	                sc.bodies.emplace_back(d.m, d.R, Vec2{d.px, d.py}, Vec2{d.vx, d.vy}, d.w);
89	        } catch (const std::exception& ex) {
90	            fail(line_no, ex.what());
91	        }
92	    }
93	    return sc;
94	}
95	
96	Scenario parse_scenario_file(const std::string& path) {
97	    std::ifstream in(path);
98	    if (!in) throw std::runtime_error("parse_scenario_file: cannot open '" + path + "'");
99	    return parse_scenario(in);
100	}
101	
102	namespace {
103	Vec2 total_momentum(const std::vector<Body>& bodies) {
104	    Vec2 p{0.0, 0.0};
105	    for (const Body& b : bodies) p = p + b.momentum();
106	    return p;
107	}
108	double total_angular_momentum(const std::vector<Body>& bodies) {
109	    double l = 0.0;
110	    for (const Body& b : bodies) l += b.angular_momentum();
111	    return l;
112	}
113	double total_ke(const std::vector<Body>& bodies) {
114	    double k = 0.0;
115	    for (const Body& b : bodies) k += b.kinetic_energy();
116	    return k;
117	}
118	} // namespace
119	
120	void write_report(std::ostream& out, const Scenario& scenario) {
121	    std::vector<Body> bodies = scenario.bodies;
122	    ContactSolver solver(scenario.restitution, scenario.friction);
123	    const std::vector<Contact> contacts = ContactSolver::detect_contacts(bodies);
124	
125	    const Vec2 p0 = total_momentum(bodies);
126	    const double l0 = total_angular_momentum(bodies);
127	    const double ke0 = total_ke(bodies);
128	
129	    const SolveReport rep = solver.solve(bodies, contacts);
130	
131	    const Vec2 p1 = total_momentum(bodies);
132	    const double l1 = total_angular_momentum(bodies);
133	    const double ke1 = total_ke(bodies);
134	
135	    out << std::scientific << std::setprecision(6);
136	    out << "# N-body rigid-shape collision report\n";
137	    out << "bodies=" << bodies.size() << " contacts=" << contacts.size()
138	        << " restitution=" << scenario.restitution << " friction=" << scenario.friction << "\n";
139	    out << "solver: iterations=" << rep.iterations << " residual=" << rep.max_residual
140	        << " converged=" << (rep.converged ? 1 : 0)
141	        << " total_normal_impulse=" << rep.total_normal_impulse << "\n";
142	    out << "momentum: (" << p0.x << ", " << p0.y << ") -> (" << p1.x << ", " << p1.y << ")\n";
143	    out << "angular_momentum: " << l0 << " -> " << l1 << "\n";
144	    out << "kinetic_energy: " << ke0 << " -> " << ke1 << " J\n";
145	}
146	
147	} // namespace collision2d
148