tests/test.sh exit code (0 → resolved); the classification below is post-hoc and cannot change it.List project structure
ls -R --ignore=build --ignore=.git 2>/dev/null | head -60
.: 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
/opt/collision2d/include/collision2d/types.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
/opt/collision2d/include/collision2d/solver.hpp
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
/opt/collision2d/src/solver.cpp
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
/opt/collision2d/src/types.cpp
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
/opt/collision2d/include/collision2d/io.hpp
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
/opt/collision2d/tests/test_smoke.cpp
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
/opt/collision2d/tests/test_util.hpp
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
/opt/collision2d/src/main.cpp
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
/opt/collision2d/src/io.cpp
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
Inspect build config and example
cat CMakeLists.txt && echo "---EXAMPLE---" && cat examples/head_on.collision && echo "---" && cmake --version | head -1
cmake_minimum_required(VERSION 3.16)
project(collision2d LANGUAGES CXX)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release)
endif()
add_compile_options(-Wall -Wextra)
# --- Core library -----------------------------------------------------------
add_library(collision2d
src/types.cpp
src/solver.cpp
src/io.cpp
)
target_include_directories(collision2d PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
# --- Demo CLI ---------------------------------------------------------------
add_executable(collision2d_cli src/main.cpp)
target_link_libraries(collision2d_cli PRIVATE collision2d)
# --- Tests ------------------------------------------------------------------
enable_testing()
add_subdirectory(tests)
---EXAMPLE---
# A three-disk scenario resolved by the N-body sequential-impulse solver.
#
# Disk 0 (2 kg, R=0.5) slides right and is about to strike a stationary line of
# two touching disks (Newton's-cradle style); disk 2 also carries some spin so
# the tangential Coulomb friction does work. Restitution 0.9, friction 0.3.
#
# format: body m R px py vx vy [omega]
body 2.0 0.5 0.0 0.0 3.0 0.0
body 1.0 0.5 1.0 0.0 0.0 0.0
body 1.0 0.5 2.0 0.0 0.0 0.0 5.0
restitution 0.9
friction 0.3
---
cmake version 3.22.1
[stdout]
cmake_minimum_required(VERSION 3.16)
project(collision2d LANGUAGES CXX)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release)
endif()
add_compile_options(-Wall -Wextra)
# --- Core library -----------------------------------------------------------
add_library(collision2d
src/types.cpp
src/solver.cpp
src/io.cpp
)
target_include_directories(collision2d PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
# --- Demo CLI ---------------------------------------------------------------
add_executable(collision2d_cli src/main.cpp)
target_link_libraries(collision2d_cli PRIVATE collision2d)
# --- Tests ------------------------------------------------------------------
enable_testing()
add_subdirectory(tests)
---EXAMPLE---
# A three-disk scenario resolved by the N-body sequential-impulse solver.
#
# Disk 0 (2 kg, R=0.5) slides right and is about to strike a stationary line of
# two touching disks (Newton's-cradle style); disk 2 also carries some spin so
# the tangential Coulomb friction does work. Restitution 0.9, friction 0.3.
#
# format: body m R px py vx vy [omega]
body 2.0 0.5 0.0 0.0 3.0 0.0
body 1.0 0.5 1.0 0.0 0.0 0.0
body 1.0 0.5 2.0 0.0 0.0 0.0 5.0
restitution 0.9
friction 0.3
---
cmake version 3.22.1Baseline build
cmake -S /opt/collision2d -B /opt/collision2d/build -DCMAKE_BUILD_TYPE=Release >/dev/null 2>&1 && cmake --build /opt/collision2d/build --parallel 2>&1 | tail -3 && echo "BUILD OK"
[ 87%] Built target collision2d_cli [100%] Linking CXX executable test_smoke [100%] Built target test_smoke BUILD OK [stdout] [ 87%] Built target collision2d_cli [100%] Linking CXX executable test_smoke [100%] Built target test_smoke BUILD OK
#include "collision2d/solver.hpp"
#include <algorithm>
#include <cmath>
#include <limits>
#include <stdexcept>
#include "collision2d/types.hpp"
namespace collision2d {
// ============================================================================
// N-body rigid-shape (disk / convex polygon) contact solver.
//
// * detect_contacts(): narrow-phase contact generation for disk-disk,
// disk-polygon, and polygon-polygon pairs, producing a unit normal (from
// the lower- toward the higher-indexed body), penetration depth, and one or
// two contact points (face manifolds are expressed as two Contact records).
// * solve(): a sequential-impulse velocity solve with accumulated impulses,
// per-contact Newton restitution (targets frozen at the pre-solve relative
// normal velocity), and a Coulomb friction cone, accounting for rotational
// inertia and contact lever arms at every body and contact point.
// * correct_positions(): a positional de-penetration pass (positions only).
// * time_of_impact()/step(): continuous collision by conservative advancement
// using exact convex separation distances.
// ============================================================================
namespace {
constexpr double kGeomEps = 1e-12;
// --- small geometry helpers -------------------------------------------------
// Closest point on segment [a,b] to p.
Vec2 closest_on_segment(const Vec2& p, const Vec2& a, const Vec2& b) {
const Vec2 e = b - a;
const double ee = dot(e, e);
if (ee <= kGeomEps) return a;
double t = dot(p - a, e) / ee;
t = std::max(0.0, std::min(1.0, t));
return a + t * e;
}
// Nearest boundary point of a convex polygon body to p, plus an `inside` flag.
struct PolyClosest {
Vec2 point;
double dist;
bool inside;
};
PolyClosest closest_on_polygon(const Body& poly, const Vec2& p) {
const std::size_t n = poly.vertex_count();
PolyClosest best{Vec2{}, std::numeric_limits<double>::infinity(), true};
for (std::size_t i = 0; i < n; ++i) {
const Vec2 a = poly.world_vertex(i);
const Vec2 b = poly.world_vertex((i + 1) % n);
if (dot(poly.world_face_normal(i), p - a) > 0.0) best.inside = false;
const Vec2 cp = closest_on_segment(p, a, b);
const double d2 = length_sq(p - cp);
if (d2 < best.dist) {
best.dist = d2;
best.point = cp;
}
}
best.dist = std::sqrt(best.dist);
return best;
}
// Outward (from `ref`) face normal of `ref` and the deepest penetration of
// `inc` along it: the support of `inc` opposite the face normal.
struct FaceQuery {
double separation;
std::size_t face;
};
FaceQuery max_separation(const Body& ref, const Body& inc) {
FaceQuery q{-std::numeric_limits<double>::infinity(), 0};
const std::size_t nr = ref.vertex_count();
const std::size_t ni = inc.vertex_count();
for (std::size_t i = 0; i < nr; ++i) {
const Vec2 n = ref.world_face_normal(i);
const Vec2 v = ref.world_vertex(i);
// Support of inc in direction -n (the vertex minimizing dot(n, .)).
double smin = std::numeric_limits<double>::infinity();
for (std::size_t j = 0; j < ni; ++j) {
const double d = dot(n, inc.world_vertex(j));
if (d < smin) smin = d;
}
const double sep = smin - dot(n, v);
if (sep > q.separation) {
q.separation = sep;
q.face = i;
}
}
return q;
}
// Clip segment vIn[0..1] to the half-plane { v : dot(normal, v) <= offset }.
int clip_segment(Vec2 out[2], const Vec2 in[2], const Vec2& normal, double offset) {
int n = 0;
const double d0 = dot(normal, in[0]) - offset;
const double d1 = dot(normal, in[1]) - offset;
if (d0 <= 0.0) out[n++] = in[0];
if (d1 <= 0.0) out[n++] = in[1];
if (d0 * d1 < 0.0 && n < 2) {
const double t = d0 / (d0 - d1);
out[n++] = in[0] + t * (in[1] - in[0]);
}
return n;
}
// --- narrow-phase contact generation ---------------------------------------
struct ManifoldPoint {
Vec2 point;
double penetration;
};
struct Manifold {
Vec2 normal{1.0, 0.0}; // from body a toward body b
ManifoldPoint pts[2];
int count{0};
};
// Disk a vs disk b. Throws on coincident centers of an overlapping pair.
bool collide_disk_disk(const Body& A, const Body& B, Manifold& m) {
const Vec2 d = B.position() - A.position();
const double dist2 = length_sq(d);
const double r = A.radius() + B.radius();
if (dist2 >= r * r) return false;
const double dist = std::sqrt(dist2);
if (dist <= kGeomEps) {
throw std::runtime_error(
"detect_contacts: overlapping disks share a center (normal undefined)");
}
m.normal = (1.0 / dist) * d;
m.count = 1;
m.pts[0].penetration = r - dist;
// Midpoint of the overlap region on the line of centers.
m.pts[0].point =
A.position() + (A.radius() - 0.5 * m.pts[0].penetration) * m.normal;
return true;
}
// Disk vs polygon. `disk_is_a` orients the normal from a to b.
bool collide_disk_poly(const Body& disk, const Body& poly, bool disk_is_a,
Manifold& m) {
const Vec2 c = disk.position();
const double r = disk.radius();
const PolyClosest pc = closest_on_polygon(poly, c);
Vec2 n_pd; // points from polygon surface toward the disk
double penetration;
if (pc.inside) {
// Disk center inside the polygon: separate through the nearest face.
const std::size_t nv = poly.vertex_count();
double best = std::numeric_limits<double>::infinity();
Vec2 face_n{1.0, 0.0};
for (std::size_t i = 0; i < nv; ++i) {
const Vec2 fn = poly.world_face_normal(i);
const double depth = -dot(fn, c - poly.world_vertex(i)); // >= 0 inside
if (depth < best) {
best = depth;
face_n = fn;
}
}
n_pd = face_n;
penetration = r + best;
} else {
const Vec2 delta = c - pc.point;
const double dist = std::sqrt(length_sq(delta));
if (dist >= r) return false;
if (dist <= kGeomEps) {
throw std::runtime_error(
"detect_contacts: disk center on polygon boundary (normal undefined)");
}
n_pd = (1.0 / dist) * delta;
penetration = r - dist;
}
// Normal from a to b: if the disk is a, flip (poly is toward -n_pd).
m.normal = disk_is_a ? (-1.0 * n_pd) : n_pd;
m.count = 1;
m.pts[0].point = pc.point;
m.pts[0].penetration = penetration;
return true;
}
// Polygon a vs polygon b (SAT + reference/incident face clipping).
bool collide_poly_poly(const Body& A, const Body& B, Manifold& m) {
const FaceQuery qa = max_separation(A, B);
if (qa.separation > 0.0) return false;
const FaceQuery qb = max_separation(B, A);
if (qb.separation > 0.0) return false;
// Pick the reference face; bias toward A for coherence.
const Body* ref;
const Body* inc;
std::size_t ref_face;
bool ref_is_b;
if (qb.separation > qa.separation + 1e-10) {
ref = &B;
inc = &A;
ref_face = qb.face;
ref_is_b = true;
} else {
ref = &A;
inc = &B;
ref_face = qa.face;
ref_is_b = false;
}
const std::size_t nr = ref->vertex_count();
const std::size_t nin = inc->vertex_count();
const Vec2 ref_normal = ref->world_face_normal(ref_face);
const Vec2 rv1 = ref->world_vertex(ref_face);
const Vec2 rv2 = ref->world_vertex((ref_face + 1) % nr);
const Vec2 tangent = (1.0 / std::sqrt(length_sq(rv2 - rv1))) * (rv2 - rv1);
// Incident face: the inc face whose normal is most anti-parallel to ref.
std::size_t inc_face = 0;
double min_dot = std::numeric_limits<double>::infinity();
for (std::size_t j = 0; j < nin; ++j) {
const double d = dot(inc->world_face_normal(j), ref_normal);
if (d < min_dot) {
min_dot = d;
inc_face = j;
}
}
Vec2 seg[2] = {inc->world_vertex(inc_face),
inc->world_vertex((inc_face + 1) % nin)};
// Clip the incident edge to the reference face's side planes.
Vec2 tmp[2];
if (clip_segment(tmp, seg, -1.0 * tangent, -dot(tangent, rv1)) < 2)
return false;
Vec2 clipped[2];
if (clip_segment(clipped, tmp, tangent, dot(tangent, rv2)) < 2) return false;
// Normal from a (=A) to b (=B).
m.normal = ref_is_b ? (-1.0 * ref_normal) : ref_normal;
m.count = 0;
for (int k = 0; k < 2; ++k) {
const double sep = dot(ref_normal, clipped[k] - rv1);
if (sep <= 1e-9) {
m.pts[m.count].point = clipped[k];
m.pts[m.count].penetration = std::max(0.0, -sep);
++m.count;
}
}
return m.count > 0;
}
bool collide(const Body& A, const Body& B, std::size_t ia, std::size_t ib,
Manifold& m) {
// Broad phase on bounding radii.
const double rsum = A.radius() + B.radius();
if (length_sq(B.position() - A.position()) >= rsum * rsum) return false;
const bool ad = A.shape() == Shape::Disk;
const bool bd = B.shape() == Shape::Disk;
(void)ia;
(void)ib;
if (ad && bd) return collide_disk_disk(A, B, m);
if (ad && !bd) return collide_disk_poly(A, B, /*disk_is_a=*/true, m);
if (!ad && bd) return collide_disk_poly(B, A, /*disk_is_a=*/false, m);
return collide_poly_poly(A, B, m);
}
// --- separation distance for continuous collision --------------------------
// Signed separation between two bodies (negative when overlapping) and the unit
// outward normal `n` from A toward B. Returns false (and a non-positive sep)
// when the pair already overlaps.
double pair_separation(const Body& A, const Body& B, Vec2& n) {
const bool ad = A.shape() == Shape::Disk;
const bool bd = B.shape() == Shape::Disk;
if (ad && bd) {
const Vec2 d = B.position() - A.position();
const double dist = std::sqrt(length_sq(d));
if (dist <= kGeomEps) {
n = Vec2{1.0, 0.0};
return -(A.radius() + B.radius());
}
n = (1.0 / dist) * d;
return dist - A.radius() - B.radius();
}
if (ad != bd) {
const Body& disk = ad ? A : B;
const Body& poly = ad ? B : A;
const PolyClosest pc = closest_on_polygon(poly, disk.position());
const Vec2 delta = disk.position() - pc.point; // poly -> disk
const double dist = std::sqrt(length_sq(delta));
double sep;
Vec2 nd; // from disk toward poly
if (pc.inside || dist <= kGeomEps) {
nd = Vec2{1.0, 0.0};
sep = -(disk.radius() + dist);
} else {
nd = (-1.0 / dist) * delta; // disk -> poly
sep = dist - disk.radius();
}
// Orient from A to B.
n = ad ? nd : (-1.0 * nd);
return sep;
}
// polygon-polygon: SAT gives exact separation along a face axis when
// disjoint; if both queries are non-positive the pair overlaps.
const FaceQuery qa = max_separation(A, B);
const FaceQuery qb = max_separation(B, A);
if (qa.separation <= 0.0 && qb.separation <= 0.0) {
n = A.world_face_normal(qa.face);
return std::max(qa.separation, qb.separation); // negative
}
if (qa.separation >= qb.separation) {
n = A.world_face_normal(qa.face); // outward from A == A -> B
return qa.separation;
}
n = -1.0 * B.world_face_normal(qb.face); // outward from B negated == A -> B
return qb.separation;
}
// Earliest impact time in [0,1] for one translating pair (orientation fixed).
double pair_time_of_impact(Body A, Body B) {
const Vec2 va = A.velocity();
const Vec2 vb = B.velocity();
const double kTouch = 1e-10;
double t = 0.0;
for (int iter = 0; iter < 64; ++iter) {
A.set_position(A.position()); // positions advanced below
Vec2 n;
const double sep = pair_separation(A, B, n);
if (sep < kTouch) return t; // touching or overlapping at this time
const double closing = dot(n, va - vb); // > 0 when approaching
if (closing <= kTouch) return 1.0; // never closes within step
const double dt = sep / closing;
t += dt;
if (t >= 1.0) return 1.0;
// Advance both bodies to the new time.
A.set_position(A.position() + dt * va);
B.set_position(B.position() + dt * vb);
}
return t;
}
} // namespace
// ----------------------------------------------------------------------------
ContactSolver::ContactSolver(double e, double mu, int iterations, double tol,
double bias_factor, double slop)
: e_(e),
mu_(mu),
iterations_(iterations),
tol_(tol),
bias_factor_(bias_factor),
slop_(slop) {
if (!(e_ >= 0.0 && e_ <= 1.0)) {
throw std::invalid_argument("ContactSolver: restitution must lie in [0, 1]");
}
if (!(mu_ >= 0.0)) {
throw std::invalid_argument(
"ContactSolver: friction coefficient must be non-negative");
}
if (iterations_ < 1) {
throw std::invalid_argument("ContactSolver: iterations must be at least 1");
}
if (!(tol_ > 0.0)) {
throw std::invalid_argument("ContactSolver: tol must be positive");
}
if (!(bias_factor_ >= 0.0 && bias_factor_ <= 1.0)) {
throw std::invalid_argument("ContactSolver: bias_factor must lie in [0, 1]");
}
if (!(slop_ >= 0.0)) {
throw std::invalid_argument("ContactSolver: slop must be non-negative");
}
}
std::vector<Contact>
ContactSolver::detect_contacts(const std::vector<Body>& bodies) {
std::vector<Contact> contacts;
const std::size_t n = bodies.size();
for (std::size_t i = 0; i < n; ++i) {
for (std::size_t j = i + 1; j < n; ++j) {
Manifold m;
if (!collide(bodies[i], bodies[j], i, j, m)) continue;
if (length_sq(m.normal) <= kGeomEps) {
throw std::runtime_error(
"detect_contacts: degenerate contact normal");
}
for (int k = 0; k < m.count; ++k) {
Contact c;
c.a = i;
c.b = j;
c.normal = m.normal;
c.point = m.pts[k].point;
c.penetration = m.pts[k].penetration;
contacts.push_back(c);
}
}
}
return contacts;
}
SolveReport ContactSolver::solve(std::vector<Body>& bodies,
const std::vector<Contact>& contacts) const {
SolveReport report;
const std::size_t nc = contacts.size();
if (nc == 0) {
report.converged = true;
return report;
}
// Precomputed per-contact constraint data (positions are fixed here).
struct C {
std::size_t a, b;
Vec2 n, t; // normal (a->b) and tangent
Vec2 ra, rb; // contact lever arms from each centre
double mass_n; // normal effective mass
double mass_t; // tangential effective mass
double target; // Newton restitution target for the normal velocity
double pn; // accumulated normal impulse (>= 0)
double pt; // accumulated tangential impulse
};
std::vector<C> cs(nc);
for (std::size_t k = 0; k < nc; ++k) {
const Contact& ct = contacts[k];
const Body& A = bodies[ct.a];
const Body& B = bodies[ct.b];
C& c = cs[k];
c.a = ct.a;
c.b = ct.b;
c.n = ct.normal;
c.t = Vec2{-c.n.y, c.n.x};
c.ra = ct.point - A.position();
c.rb = ct.point - B.position();
const double ran = cross(c.ra, c.n);
const double rbn = cross(c.rb, c.n);
const double kn = A.inv_mass() + B.inv_mass() +
A.inv_inertia() * ran * ran +
B.inv_inertia() * rbn * rbn;
c.mass_n = kn > 0.0 ? 1.0 / kn : 0.0;
const double rat = cross(c.ra, c.t);
const double rbt = cross(c.rb, c.t);
const double kt = A.inv_mass() + B.inv_mass() +
A.inv_inertia() * rat * rat +
B.inv_inertia() * rbt * rbt;
c.mass_t = kt > 0.0 ? 1.0 / kt : 0.0;
// Frozen Newton restitution target from the pre-solve approach speed.
const Vec2 vrel = B.velocity_at(ct.point) - A.velocity_at(ct.point);
const double vn = dot(vrel, c.n);
c.target = vn < 0.0 ? -e_ * vn : 0.0;
c.pn = 0.0;
c.pt = 0.0;
}
auto apply = [&](C& c, const Vec2& J) {
Body& A = bodies[c.a];
Body& B = bodies[c.b];
A.set_velocity(A.velocity() - A.inv_mass() * J);
A.set_omega(A.omega() - A.inv_inertia() * cross(c.ra, J));
B.set_velocity(B.velocity() + B.inv_mass() * J);
B.set_omega(B.omega() + B.inv_inertia() * cross(c.rb, J));
};
int sweep = 0;
double residual = 0.0;
for (; sweep < iterations_; ++sweep) {
residual = 0.0;
for (std::size_t k = 0; k < nc; ++k) {
C& c = cs[k];
const Body& A = bodies[c.a];
const Body& B = bodies[c.b];
// Normal impulse toward the frozen restitution target.
Vec2 vrel = B.velocity_at(B.position() + c.rb) -
A.velocity_at(A.position() + c.ra);
double vn = dot(vrel, c.n);
double dpn = (c.target - vn) * c.mass_n;
const double new_pn = std::max(0.0, c.pn + dpn);
dpn = new_pn - c.pn;
c.pn = new_pn;
apply(c, dpn * c.n);
residual = std::max(residual, std::fabs(dpn) / c.mass_n > 0.0
? std::fabs(dpn)
: 0.0);
// Tangential (friction) impulse within the Coulomb cone.
vrel = B.velocity_at(B.position() + c.rb) -
A.velocity_at(A.position() + c.ra);
const double vt = dot(vrel, c.t);
double dpt = -vt * c.mass_t;
const double max_pt = mu_ * c.pn;
const double new_pt = std::max(-max_pt, std::min(max_pt, c.pt + dpt));
dpt = new_pt - c.pt;
c.pt = new_pt;
apply(c, dpt * c.t);
// Velocity-level residual (correction actually realized).
residual = std::max(residual, std::fabs(dpn) / std::max(c.mass_n, 1e-300));
residual = std::max(residual, std::fabs(dpt) / std::max(c.mass_t, 1e-300));
}
if (residual < tol_) {
++sweep;
break;
}
}
double total_pn = 0.0;
for (const C& c : cs) total_pn += c.pn;
report.iterations = sweep;
report.max_residual = residual;
report.total_normal_impulse = total_pn;
report.converged = residual < tol_;
return report;
}
void ContactSolver::correct_positions(
std::vector<Body>& bodies, const std::vector<Contact>& contacts) const {
for (const Contact& c : contacts) {
Body& A = bodies[c.a];
Body& B = bodies[c.b];
const double excess = c.penetration - slop_;
if (excess <= 0.0) continue;
const double inv_a = A.inv_mass();
const double inv_b = B.inv_mass();
const double inv_sum = inv_a + inv_b;
if (inv_sum <= 0.0) continue;
const Vec2 correction = (bias_factor_ * excess / inv_sum) * c.normal;
A.set_position(A.position() - inv_a * correction);
B.set_position(B.position() + inv_b * correction);
}
}
SolveReport ContactSolver::resolve(std::vector<Body>& bodies) const {
const std::vector<Contact> contacts = detect_contacts(bodies);
SolveReport report = solve(bodies, contacts);
correct_positions(bodies, contacts);
return report;
}
double ContactSolver::time_of_impact(const std::vector<Body>& bodies) const {
double best = 1.0;
const std::size_t n = bodies.size();
for (std::size_t i = 0; i < n; ++i) {
for (std::size_t j = i + 1; j < n; ++j) {
const double t = pair_time_of_impact(bodies[i], bodies[j]);
best = std::min(best, t);
if (best <= 0.0) return 0.0;
}
}
return best;
}
SolveReport ContactSolver::step(std::vector<Body>& bodies) const {
const double t = time_of_impact(bodies);
for (Body& b : bodies) b.set_position(b.position() + t * b.velocity());
return resolve(bodies);
}
} // namespace collision2d
/opt/collision2d/src/solver.cpp
struct C {
std::size_t a, b;
Vec2 n, t; // normal (a->b) and tangent
Vec2 ra, rb; // contact lever arms from each centre
double mass_n; // normal effective mass
double mass_t; // tangential effective mass
double target; // Newton restitution target for the normal velocity
double pn; // accumulated normal impulse (>= 0)
double pt; // accumulated tangential impulse
}; struct C {
std::size_t a, b;
Vec2 n, t; // normal (a->b) and tangent
Vec2 point; // world contact point
Vec2 ra, rb; // contact lever arms from each centre
double mass_n; // normal effective mass
double mass_t; // tangential effective mass
double target; // Newton restitution target for the normal velocity
double pn; // accumulated normal impulse (>= 0)
double pt; // accumulated tangential impulse
};/opt/collision2d/src/solver.cpp
c.n = ct.normal;
c.t = Vec2{-c.n.y, c.n.x};
c.ra = ct.point - A.position();
c.rb = ct.point - B.position(); c.n = ct.normal;
c.t = Vec2{-c.n.y, c.n.x};
c.point = ct.point;
c.ra = ct.point - A.position();
c.rb = ct.point - B.position();/opt/collision2d/src/solver.cpp
// Normal impulse toward the frozen restitution target.
Vec2 vrel = B.velocity_at(B.position() + c.rb) -
A.velocity_at(A.position() + c.ra);
double vn = dot(vrel, c.n);
double dpn = (c.target - vn) * c.mass_n;
const double new_pn = std::max(0.0, c.pn + dpn);
dpn = new_pn - c.pn;
c.pn = new_pn;
apply(c, dpn * c.n);
residual = std::max(residual, std::fabs(dpn) / c.mass_n > 0.0
? std::fabs(dpn)
: 0.0);
// Tangential (friction) impulse within the Coulomb cone.
vrel = B.velocity_at(B.position() + c.rb) -
A.velocity_at(A.position() + c.ra);
const double vt = dot(vrel, c.t);
double dpt = -vt * c.mass_t;
const double max_pt = mu_ * c.pn;
const double new_pt = std::max(-max_pt, std::min(max_pt, c.pt + dpt));
dpt = new_pt - c.pt;
c.pt = new_pt;
apply(c, dpt * c.t);
// Velocity-level residual (correction actually realized).
residual = std::max(residual, std::fabs(dpn) / std::max(c.mass_n, 1e-300));
residual = std::max(residual, std::fabs(dpt) / std::max(c.mass_t, 1e-300)); // Normal impulse toward the frozen restitution target.
Vec2 vrel = B.velocity_at(c.point) - A.velocity_at(c.point);
const double vn = dot(vrel, c.n);
double dpn = (c.target - vn) * c.mass_n;
const double new_pn = std::max(0.0, c.pn + dpn);
dpn = new_pn - c.pn;
c.pn = new_pn;
apply(c, dpn * c.n);
// Tangential (friction) impulse within the Coulomb cone.
vrel = B.velocity_at(c.point) - A.velocity_at(c.point);
const double vt = dot(vrel, c.t);
double dpt = -vt * c.mass_t;
const double max_pt = mu_ * c.pn;
const double new_pt = std::max(-max_pt, std::min(max_pt, c.pt + dpt));
dpt = new_pt - c.pt;
c.pt = new_pt;
apply(c, dpt * c.t);
// Velocity-level residual: the relative-velocity correction realized
// this contact (impulse change times effective inverse mass).
if (c.mass_n > 0.0)
residual = std::max(residual, std::fabs(dpn) / c.mass_n);
if (c.mass_t > 0.0)
residual = std::max(residual, std::fabs(dpt) / c.mass_t);/opt/collision2d/src/solver.cpp
double t = 0.0;
for (int iter = 0; iter < 64; ++iter) {
A.set_position(A.position()); // positions advanced below
Vec2 n; double t = 0.0;
for (int iter = 0; iter < 64; ++iter) {
Vec2 n;Rebuild with new solver
cmake --build /opt/collision2d/build --parallel 2>&1 | grep -E "error|warning|Built target collision2d" | head -30; echo "EXIT ${PIPESTATUS[0]}"[ 50%] Built target collision2d [100%] Built target collision2d_cli EXIT 0 [stdout] [ 50%] Built target collision2d [100%] Built target collision2d_cli EXIT 0
Run smoke test
cd /opt/collision2d/build && ctest --output-on-failure 2>&1 | tail -15
Test project /opt/collision2d/build
Start 1: smoke
1/1 Test #1: smoke ............................ Passed 0.00 sec
100% tests passed, 0 tests failed out of 1
Total Test time (real) = 0.00 sec
[stdout]
Test project /opt/collision2d/build
Start 1: smoke
1/1 Test #1: smoke ............................ Passed 0.00 sec
100% tests passed, 0 tests failed out of 1
Total Test time (real) = 0.00 sec// Standalone physical-invariant checks (not part of CTest by default).
#include <cmath>
#include <cstdio>
#include <random>
#include <vector>
#include "collision2d/solver.hpp"
#include "collision2d/types.hpp"
using namespace collision2d;
static int g_fail = 0;
static void check(bool c, const char* msg) {
if (!c) { std::printf(" FAIL: %s\n", msg); ++g_fail; }
}
static void near(double g, double w, double tol, const char* msg) {
if (std::fabs(g - w) > tol) {
std::printf(" FAIL: %s (got %.12g want %.12g)\n", msg, g, w);
++g_fail;
}
}
static Vec2 tot_p(const std::vector<Body>& b) {
Vec2 p{0, 0}; for (auto& x : b) p = p + x.momentum(); return p;
}
static double tot_l(const std::vector<Body>& b) {
double l = 0; for (auto& x : b) l += x.angular_momentum(); return l;
}
static double tot_ke(const std::vector<Body>& b) {
double k = 0; for (auto& x : b) k += x.kinetic_energy(); return k;
}
// 1. Elastic head-on equal disks: velocities exchange exactly.
static void test_elastic_headon() {
std::printf("test_elastic_headon\n");
std::vector<Body> b{Body(1, 0.5, {0,0}, {2,0}), Body(1, 0.5, {0.9,0}, {-1,0})};
auto cs = ContactSolver::detect_contacts(b);
check(cs.size() == 1, "1 contact");
ContactSolver(1.0, 0.0).solve(b, cs);
near(b[0].velocity().x, -1.0, 1e-9, "body0 vx exchange");
near(b[1].velocity().x, 2.0, 1e-9, "body1 vx exchange");
}
// 2. Restitution law for a single normal contact.
static void test_restitution() {
std::printf("test_restitution\n");
for (double e : {0.0, 0.3, 0.7, 1.0}) {
std::vector<Body> b{Body(1, 0.5, {0,0}, {2,0}), Body(2, 0.5, {0.9,0}, {-1,0})};
Vec2 n{1,0};
double approach = dot(b[1].velocity_at({0.45,0}) - b[0].velocity_at({0.45,0}), n);
auto cs = ContactSolver::detect_contacts(b);
ContactSolver(e, 0.0).solve(b, cs);
double sep = dot(b[1].velocity_at(cs[0].point) - b[0].velocity_at(cs[0].point), cs[0].normal);
near(sep, -e * approach, 1e-9, "newton restitution");
}
}
// 3. Conservation across random disk scenarios.
static void test_conservation_disks() {
std::printf("test_conservation_disks\n");
std::mt19937 rng(12345);
std::uniform_real_distribution<double> U(-1, 1), M(0.5, 3), R(0.3, 0.7);
for (int trial = 0; trial < 400; ++trial) {
int n = 2 + (rng() % 4);
std::vector<Body> b;
for (int i = 0; i < n; ++i)
b.emplace_back(M(rng), R(rng), Vec2{U(rng)*2, U(rng)*2},
Vec2{U(rng)*3, U(rng)*3}, U(rng)*4);
double e = std::fabs(U(rng)), mu = std::fabs(U(rng));
auto cs = ContactSolver::detect_contacts(b);
if (cs.empty()) continue;
Vec2 p0 = tot_p(b); double l0 = tot_l(b), k0 = tot_ke(b);
ContactSolver(e, mu, 256, 1e-14).solve(b, cs);
Vec2 p1 = tot_p(b); double l1 = tot_l(b), k1 = tot_ke(b);
near(p1.x, p0.x, 1e-7, "px conserved");
near(p1.y, p0.y, 1e-7, "py conserved");
near(l1, l0, 1e-7, "L conserved");
check(k1 <= k0 + 1e-7, "KE not increased");
}
}
// 4. Coulomb cone respected (tangential impulse <= mu * normal impulse).
// Verified indirectly: with mu=0, tangential velocity unchanged.
static void test_friction_cone() {
std::printf("test_friction_cone\n");
// Spinning disk striking a stationary one, mu=0 -> no tangential transfer.
std::vector<Body> b{Body(1, 0.5, {0,0}, {1,0}, 10.0), Body(1, 0.5, {0.9,0}, {0,0})};
auto cs = ContactSolver::detect_contacts(b);
auto b2 = b;
ContactSolver(0.5, 0.0).solve(b2, cs);
near(b2[0].velocity().y, 0.0, 1e-9, "no tangential vel mu=0 body0");
near(b2[1].velocity().y, 0.0, 1e-9, "no tangential vel mu=0 body1");
near(b2[0].omega(), 10.0, 1e-9, "spin unchanged mu=0");
// With friction, the spin must do tangential work and change y velocities
// oppositely (momentum still conserved).
auto b3 = b;
Vec2 p0 = tot_p(b3); double l0 = tot_l(b3), k0 = tot_ke(b3);
ContactSolver(0.5, 0.5, 256, 1e-14).solve(b3, cs);
Vec2 p1 = tot_p(b3); double l1 = tot_l(b3), k1 = tot_ke(b3);
check(std::fabs(b3[0].omega()) < 10.0, "spin reduced by friction");
near(p1.x, p0.x, 1e-7, "fric px conserved");
near(p1.y, p0.y, 1e-7, "fric py conserved");
near(l1, l0, 1e-7, "fric L conserved");
check(k1 <= k0 + 1e-7, "fric KE not increased");
}
// 5. Position correction: reduces overlap by bias*(pen-slop), no velocity change.
static void test_position_correction() {
std::printf("test_position_correction\n");
double bias = 0.2, slop = 1e-3;
std::vector<Body> b{Body(1, 0.5, {0,0}, {0,0}), Body(3, 0.5, {0.8,0}, {0,0})};
auto cs = ContactSolver::detect_contacts(b);
double pen0 = cs[0].penetration; // 0.2
Vec2 v0a = b[0].velocity(), v0b = b[1].velocity();
ContactSolver solver(1.0, 0.0, 64, 1e-12, bias, slop);
solver.correct_positions(b, cs);
// velocities unchanged
near(b[0].velocity().x, v0a.x, 0, "vel unchanged a");
near(b[1].velocity().x, v0b.x, 0, "vel unchanged b");
// overlap reduced by bias*(pen-slop), heavier (b, 3kg) moves less.
double newdist = b[1].position().x - b[0].position().x;
double newpen = (0.5 + 0.5) - newdist;
near(newpen, pen0 - bias * (pen0 - slop), 1e-12, "penetration reduced");
// heavier body moved less
double moveA = std::fabs(b[0].position().x - 0.0);
double moveB = std::fabs(b[1].position().x - 0.8);
check(moveB < moveA, "heavier body moves less");
near(moveA / moveB, 3.0, 1e-9, "move ratio = mass ratio");
}
// 6. Polygon: normal impulse imparts spin (off-centroid contact).
static void test_polygon_spin() {
std::printf("test_polygon_spin\n");
// A box hit off-center by a disk should start spinning.
std::vector<Vec2> sq{{-0.5,-0.5},{0.5,-0.5},{0.5,0.5},{-0.5,0.5}};
// disk approaching the box's right face but offset in y so lever arm != 0.
std::vector<Body> b{
Body(1.0, 0.2, {1.55,0.3}, {-2,0}), // disk
Body(1.0, sq, {0.0,0.0}, {0,0}, 0.0, 0.0)}; // box
auto cs = ContactSolver::detect_contacts(b);
check(!cs.empty(), "disk-box contact found");
Vec2 p0 = tot_p(b); double l0 = tot_l(b), k0 = tot_ke(b);
ContactSolver(0.5, 0.0, 256, 1e-14).solve(b, cs);
Vec2 p1 = tot_p(b); double l1 = tot_l(b), k1 = tot_ke(b);
check(std::fabs(b[1].omega()) > 1e-6, "box gained spin from off-center normal impulse");
near(p1.x, p0.x, 1e-7, "poly px conserved");
near(p1.y, p0.y, 1e-7, "poly py conserved");
near(l1, l0, 1e-7, "poly L conserved");
check(k1 <= k0 + 1e-7, "poly KE not increased");
}
// 7. Two stacked/overlapping boxes -> face manifold has 2 contact points.
static void test_box_manifold() {
std::printf("test_box_manifold\n");
std::vector<Vec2> sq{{-0.5,-0.5},{0.5,-0.5},{0.5,0.5},{-0.5,0.5}};
std::vector<Body> b{
Body(1.0, sq, {0.0,0.0}, {0,1.0}, 0.0, 0.0), // moving up
Body(1.0, sq, {0.0,0.95}, {0,-1.0}, 0.0, 0.0)}; // moving down, overlapping
auto cs = ContactSolver::detect_contacts(b);
check(cs.size() == 2, "box-box face manifold => 2 contacts");
Vec2 p0 = tot_p(b); double l0 = tot_l(b), k0 = tot_ke(b);
ContactSolver(1.0, 0.0, 256, 1e-14).solve(b, cs);
Vec2 p1 = tot_p(b); double l1 = tot_l(b), k1 = tot_ke(b);
// Symmetric elastic flat impact: velocities reverse, no spin.
near(b[0].velocity().y, -1.0, 1e-7, "box0 vy reversed");
near(b[1].velocity().y, 1.0, 1e-7, "box1 vy reversed");
near(b[0].omega(), 0.0, 1e-9, "no spurious spin 0");
near(b[1].omega(), 0.0, 1e-9, "no spurious spin 1");
near(p1.y, p0.y, 1e-7, "manifold py conserved");
near(l1, l0, 1e-7, "manifold L conserved");
near(k1, k0, 1e-7, "manifold KE conserved (elastic)");
}
// 8. Time of impact: exact analytic checks.
static void test_toi() {
std::printf("test_toi\n");
ContactSolver s(1.0, 0.0);
{ // two disks R=0.5 at x=0 and x=3, closing at relative speed 3 (1 and -2).
std::vector<Body> b{Body(1,0.5,{0,0},{1,0}), Body(1,0.5,{3,0},{-2,0})};
// gap = 3 - 1 = 2 ; closing speed = 3 ; toi = 2/3.
near(s.time_of_impact(b), 2.0/3.0, 1e-9, "disk toi");
}
{ // already overlapping -> 0.
std::vector<Body> b{Body(1,0.5,{0,0},{0,0}), Body(1,0.5,{0.5,0},{0,0})};
near(s.time_of_impact(b), 0.0, 1e-12, "overlap toi 0");
}
{ // never touching within step -> 1.
std::vector<Body> b{Body(1,0.5,{0,0},{0,0}), Body(1,0.5,{5,0},{-1,0})};
near(s.time_of_impact(b), 1.0, 1e-12, "no impact toi 1");
}
{ // box-box closing vertically: box half-height 0.5; centers y=0 and y=3,
// closing speed 3 -> gap 2 -> toi 2/3.
std::vector<Vec2> sq{{-0.5,-0.5},{0.5,-0.5},{0.5,0.5},{-0.5,0.5}};
std::vector<Body> b{Body(1,sq,{0,0},{0,1},0,0), Body(1,sq,{0,3},{0,-2},0,0)};
near(s.time_of_impact(b), 2.0/3.0, 1e-7, "box toi");
}
{ // tunneling: fast small disk passes a thin gap; ensure exact, not stepped.
std::vector<Body> b{Body(1,0.1,{0,0},{100,0}), Body(1,0.1,{10,0},{0,0})};
// gap = 10 - 0.2 = 9.8 ; speed 100 ; toi = 0.098
near(s.time_of_impact(b), 9.8/100.0, 1e-9, "fast toi exact");
}
}
// 9. step(): advances to impact then resolves.
static void test_step() {
std::printf("test_step\n");
std::vector<Body> b{Body(1,0.5,{0,0},{1,0}), Body(1,0.5,{3,0},{-2,0})};
Vec2 p0 = tot_p(b);
ContactSolver(1.0,0.0).step(b);
// After advancing to toi the disks just touch then exchange (equal mass).
near(b[0].velocity().x, -2.0, 1e-7, "step body0 vx");
near(b[1].velocity().x, 1.0, 1e-7, "step body1 vx");
Vec2 p1 = tot_p(b);
near(p1.x, p0.x, 1e-7, "step momentum conserved");
}
// 10. Random polygon mix conservation + energy.
static void test_conservation_mixed() {
std::printf("test_conservation_mixed\n");
std::mt19937 rng(999);
std::uniform_real_distribution<double> U(-1,1), M(0.5,3);
std::vector<Vec2> sq{{-0.4,-0.4},{0.4,-0.4},{0.4,0.4},{-0.4,0.4}};
std::vector<Vec2> tri{{-0.5,-0.3},{0.5,-0.3},{0.0,0.5}};
int tested = 0;
for (int trial = 0; trial < 600 && tested < 300; ++trial) {
std::vector<Body> b;
int n = 2 + (rng() % 3);
for (int i = 0; i < n; ++i) {
int kind = rng() % 3;
Vec2 pos{U(rng)*1.5, U(rng)*1.5}, vel{U(rng)*3, U(rng)*3};
double w = U(rng)*4;
if (kind == 0) b.emplace_back(M(rng), 0.4, pos, vel, w);
else if (kind == 1) b.emplace_back(M(rng), sq, pos, vel, U(rng)*3, w);
else b.emplace_back(M(rng), tri, pos, vel, U(rng)*3, w);
}
std::vector<Contact> cs;
try { cs = ContactSolver::detect_contacts(b); }
catch (...) { continue; }
if (cs.empty()) continue;
++tested;
double e = std::fabs(U(rng)), mu = std::fabs(U(rng))*1.5;
Vec2 p0 = tot_p(b); double l0 = tot_l(b), k0 = tot_ke(b);
ContactSolver(e, mu, 512, 1e-14).solve(b, cs);
Vec2 p1 = tot_p(b); double l1 = tot_l(b), k1 = tot_ke(b);
near(p1.x, p0.x, 1e-6, "mixed px conserved");
near(p1.y, p0.y, 1e-6, "mixed py conserved");
near(l1, l0, 1e-6, "mixed L conserved");
check(k1 <= k0 + 1e-6, "mixed KE not increased");
}
std::printf(" (%d overlapping scenarios tested)\n", tested);
}
int main() {
test_elastic_headon();
test_restitution();
test_conservation_disks();
test_friction_cone();
test_position_correction();
test_polygon_spin();
test_box_manifold();
test_toi();
test_step();
test_conservation_mixed();
std::printf("----\n%s (%d failures)\n", g_fail ? "FAILURES" : "ALL PASS", g_fail);
return g_fail ? 1 : 0;
}
Compile and run verification harness
cd /opt/collision2d && g++ -std=c++17 -O2 -Iinclude tests/verify.cpp src/types.cpp src/solver.cpp -o build/verify 2>&1 | head -20 && ./build/verify
Exit code 1 test_elastic_headon test_restitution test_conservation_disks FAIL: KE not increased test_friction_cone test_position_correction test_polygon_spin FAIL: disk-box contact found FAIL: box gained spin from off-center normal impulse test_box_manifold test_toi test_step FAIL: step body0 vx (got 1 want -2) FAIL: step body1 vx (got -2 want 1) test_conservation_mixed (300 overlapping scenarios tested) ---- FAILURES (5 failures) [error] tool reported failure
/opt/collision2d/src/solver.cpp
// Earliest impact time in [0,1] for one translating pair (orientation fixed).
double pair_time_of_impact(Body A, Body B) {
const Vec2 va = A.velocity();
const Vec2 vb = B.velocity();
const double kTouch = 1e-10;
double t = 0.0;
for (int iter = 0; iter < 64; ++iter) {
Vec2 n;
const double sep = pair_separation(A, B, n);
if (sep < kTouch) return t; // touching or overlapping at this time
const double closing = dot(n, va - vb); // > 0 when approaching
if (closing <= kTouch) return 1.0; // never closes within step
const double dt = sep / closing;
t += dt;
if (t >= 1.0) return 1.0;
// Advance both bodies to the new time.
A.set_position(A.position() + dt * va);
B.set_position(B.position() + dt * vb);
}
return t;
}// Earliest impact time in [0,1] for one translating pair (orientation fixed),
// found by conservative advancement on the exact convex separation. The
// returned time seats a negligible penetration (~1e-10 in distance) at a
// newly-formed contact so that the subsequent detect/solve in step() engages;
// a pre-existing overlap returns exactly 0.
double pair_time_of_impact(Body A, Body B) {
const Vec2 va = A.velocity();
const Vec2 vb = B.velocity();
constexpr double kEps = 1e-12;
constexpr double kSeat = 1e-10; // tiny overlap to seat a fresh contact
double t = 0.0;
for (int iter = 0; iter < 128; ++iter) {
Vec2 n;
const double sep = pair_separation(A, B, n);
const double closing = dot(n, va - vb); // > 0 when approaching
if (sep <= kEps) { // in contact at this time
if (t <= 0.0) return 0.0; // pre-existing overlap
if (closing <= kEps) return std::min(t, 1.0);
return std::min(t + kSeat / closing, 1.0);
}
if (closing <= kEps) return 1.0; // separating / parallel: never closes
const double dt = sep / closing;
if (t + dt >= 1.0) return 1.0; // first contact lies beyond the step
t += dt;
A.set_position(A.position() + dt * va);
B.set_position(B.position() + dt * vb);
}
return std::min(t, 1.0);
}/opt/collision2d/tests/verify.cpp
std::vector<Body> b{
Body(1.0, 0.2, {1.55,0.3}, {-2,0}), // disk
Body(1.0, sq, {0.0,0.0}, {0,0}, 0.0, 0.0)}; // box std::vector<Body> b{
Body(1.0, 0.2, {0.62,0.3}, {-2,0}), // disk (overlaps right face)
Body(1.0, sq, {0.0,0.0}, {0,0}, 0.0, 0.0)}; // box/opt/collision2d/tests/verify.cpp
Vec2 p1 = tot_p(b); double l1 = tot_l(b), k1 = tot_ke(b);
near(p1.x, p0.x, 1e-7, "px conserved");
near(p1.y, p0.y, 1e-7, "py conserved");
near(l1, l0, 1e-7, "L conserved");
check(k1 <= k0 + 1e-7, "KE not increased");
}
} Vec2 p1 = tot_p(b); double l1 = tot_l(b), k1 = tot_ke(b);
near(p1.x, p0.x, 1e-7, "px conserved");
near(p1.y, p0.y, 1e-7, "py conserved");
near(l1, l0, 1e-7, "L conserved");
if (k1 > k0 + 1e-7) {
std::printf(" KE up by %.3e (k0=%.6g) e=%.3g mu=%.3g n=%d contacts=%zu\n",
k1 - k0, k0, e, mu, n, cs.size());
++g_fail;
}
}
}Rebuild and rerun verify
cd /opt/collision2d && cmake --build build --parallel 2>&1 | grep -E "error|warning" | head; g++ -std=c++17 -O2 -Iinclude tests/verify.cpp src/types.cpp src/solver.cpp -o build/verify 2>&1 | head && ./build/verify
Exit code 1
test_elastic_headon
test_restitution
test_conservation_disks
KE up by 2.564e+00 (k0=33.0416) e=0.994 mu=0.042 n=5 contacts=4
test_friction_cone
test_position_correction
test_polygon_spin
test_box_manifold
test_toi
test_step
test_conservation_mixed
(300 overlapping scenarios tested)
----
FAILURES (1 failures)
[error] tool reported failureProbe energy injection vs penetration depth
cd /opt/collision2d && cat > /tmp/probe.cpp <<'EOF'
#include <cstdio>
#include <random>
#include <vector>
#include "collision2d/solver.hpp"
#include "collision2d/types.hpp"
using namespace collision2d;
static double ke(const std::vector<Body>&b){double k=0;for(auto&x:b)k+=x.kinetic_energy();return k;}
int main(){
std::mt19937 rng(7);
std::uniform_real_distribution<double> U(-1,1);
double worst_shallow=0, worst_deep=0; int ns=0,nd=0;
for(int trial=0;trial<200000;++trial){
int n=2+(rng()%4);
std::vector<Body> b;
// place on a loose grid so overlaps are shallow, velocities random toward center
for(int i=0;i<n;++i){
double R=0.4+0.2*U(rng);
Vec2 pos{ (i%3)*0.7 + 0.02*U(rng), (i/3)*0.7 + 0.02*U(rng)};
Vec2 vel{U(rng)*3,U(rng)*3};
b.emplace_back(0.5+1.5*((U(rng)+1)/2), R, pos, vel, U(rng)*4);
}
std::vector<Contact> cs;
try{cs=ContactSolver::detect_contacts(b);}catch(...){continue;}
if(cs.empty())continue;
double maxpen=0; for(auto&c:cs) maxpen=std::max(maxpen,c.penetration);
double e=(U(rng)+1)/2, mu=(U(rng)+1)/2;
double k0=ke(b);
ContactSolver(e,mu,400,1e-14).solve(b,cs);
double dk=ke(b)-k0;
if(maxpen<0.05){ if(dk>worst_shallow){worst_shallow=dk;} ++ns;}
else { if(dk>worst_deep){worst_deep=dk;} ++nd;}
}
std::printf("shallow(<0.05 pen): n=%d worst KE increase=%.3e\n",ns,worst_shallow);
std::printf("deep : n=%d worst KE increase=%.3e\n",nd,worst_deep);
}
EOF
g++ -std=c++17 -O2 -Iinclude /tmp/probe.cpp src/types.cpp src/solver.cpp -o /tmp/probe && /tmp/probeshallow(<0.05 pen): n=13458 worst KE increase=2.725e+00 deep : n=161000 worst KE increase=1.336e+01 [stdout] shallow(<0.05 pen): n=13458 worst KE increase=2.725e+00 deep : n=161000 worst KE increase=1.336e+01
/opt/collision2d/src/solver.cpp
SolveReport ContactSolver::solve(std::vector<Body>& bodies,
const std::vector<Contact>& contacts) const {
SolveReport report;
const std::size_t nc = contacts.size();
if (nc == 0) {
report.converged = true;
return report;
}
// Precomputed per-contact constraint data (positions are fixed here).
struct C {
std::size_t a, b;
Vec2 n, t; // normal (a->b) and tangent
Vec2 point; // world contact point
Vec2 ra, rb; // contact lever arms from each centre
double mass_n; // normal effective mass
double mass_t; // tangential effective mass
double target; // Newton restitution target for the normal velocity
double pn; // accumulated normal impulse (>= 0)
double pt; // accumulated tangential impulse
};
std::vector<C> cs(nc);
for (std::size_t k = 0; k < nc; ++k) {
const Contact& ct = contacts[k];
const Body& A = bodies[ct.a];
const Body& B = bodies[ct.b];
C& c = cs[k];
c.a = ct.a;
c.b = ct.b;
c.n = ct.normal;
c.t = Vec2{-c.n.y, c.n.x};
c.point = ct.point;
c.ra = ct.point - A.position();
c.rb = ct.point - B.position();
const double ran = cross(c.ra, c.n);
const double rbn = cross(c.rb, c.n);
const double kn = A.inv_mass() + B.inv_mass() +
A.inv_inertia() * ran * ran +
B.inv_inertia() * rbn * rbn;
c.mass_n = kn > 0.0 ? 1.0 / kn : 0.0;
const double rat = cross(c.ra, c.t);
const double rbt = cross(c.rb, c.t);
const double kt = A.inv_mass() + B.inv_mass() +
A.inv_inertia() * rat * rat +
B.inv_inertia() * rbt * rbt;
c.mass_t = kt > 0.0 ? 1.0 / kt : 0.0;
// Frozen Newton restitution target from the pre-solve approach speed.
const Vec2 vrel = B.velocity_at(ct.point) - A.velocity_at(ct.point);
const double vn = dot(vrel, c.n);
c.target = vn < 0.0 ? -e_ * vn : 0.0;
c.pn = 0.0;
c.pt = 0.0;
}
auto apply = [&](C& c, const Vec2& J) {
Body& A = bodies[c.a];
Body& B = bodies[c.b];
A.set_velocity(A.velocity() - A.inv_mass() * J);
A.set_omega(A.omega() - A.inv_inertia() * cross(c.ra, J));
B.set_velocity(B.velocity() + B.inv_mass() * J);
B.set_omega(B.omega() + B.inv_inertia() * cross(c.rb, J));
};
int sweep = 0;
double residual = 0.0;
for (; sweep < iterations_; ++sweep) {
residual = 0.0;
for (std::size_t k = 0; k < nc; ++k) {
C& c = cs[k];
const Body& A = bodies[c.a];
const Body& B = bodies[c.b];
// Normal impulse toward the frozen restitution target.
Vec2 vrel = B.velocity_at(c.point) - A.velocity_at(c.point);
const double vn = dot(vrel, c.n);
double dpn = (c.target - vn) * c.mass_n;
const double new_pn = std::max(0.0, c.pn + dpn);
dpn = new_pn - c.pn;
c.pn = new_pn;
apply(c, dpn * c.n);
// Tangential (friction) impulse within the Coulomb cone.
vrel = B.velocity_at(c.point) - A.velocity_at(c.point);
const double vt = dot(vrel, c.t);
double dpt = -vt * c.mass_t;
const double max_pt = mu_ * c.pn;
const double new_pt = std::max(-max_pt, std::min(max_pt, c.pt + dpt));
dpt = new_pt - c.pt;
c.pt = new_pt;
apply(c, dpt * c.t);
// Velocity-level residual: the relative-velocity correction realized
// this contact (impulse change times effective inverse mass).
if (c.mass_n > 0.0)
residual = std::max(residual, std::fabs(dpn) / c.mass_n);
if (c.mass_t > 0.0)
residual = std::max(residual, std::fabs(dpt) / c.mass_t);
}
if (residual < tol_) {
++sweep;
break;
}
}
double total_pn = 0.0;
for (const C& c : cs) total_pn += c.pn;
report.iterations = sweep;
report.max_residual = residual;
report.total_normal_impulse = total_pn;
report.converged = residual < tol_;
return report;
}SolveReport ContactSolver::solve(std::vector<Body>& bodies,
const std::vector<Contact>& contacts) const {
SolveReport report;
const std::size_t nc = contacts.size();
if (nc == 0) {
report.converged = true;
return report;
}
// Precomputed per-contact constraint data (positions are fixed here).
struct C {
std::size_t a, b;
Vec2 n, t; // normal (a->b) and tangent
Vec2 point; // world contact point
Vec2 ra, rb; // contact lever arms from each centre
double mass_n; // normal effective mass
double mass_t; // tangential effective mass
double rest; // frozen Newton restitution target (>= 0)
double pn; // accumulated normal impulse (>= 0)
double pt; // accumulated tangential impulse
};
std::vector<C> cs(nc);
for (std::size_t k = 0; k < nc; ++k) {
const Contact& ct = contacts[k];
const Body& A = bodies[ct.a];
const Body& B = bodies[ct.b];
C& c = cs[k];
c.a = ct.a;
c.b = ct.b;
c.n = ct.normal;
c.t = Vec2{-c.n.y, c.n.x};
c.point = ct.point;
c.ra = ct.point - A.position();
c.rb = ct.point - B.position();
const double ran = cross(c.ra, c.n);
const double rbn = cross(c.rb, c.n);
const double kn = A.inv_mass() + B.inv_mass() +
A.inv_inertia() * ran * ran +
B.inv_inertia() * rbn * rbn;
c.mass_n = kn > 0.0 ? 1.0 / kn : 0.0;
const double rat = cross(c.ra, c.t);
const double rbt = cross(c.rb, c.t);
const double kt = A.inv_mass() + B.inv_mass() +
A.inv_inertia() * rat * rat +
B.inv_inertia() * rbt * rbt;
c.mass_t = kt > 0.0 ? 1.0 / kt : 0.0;
// Frozen Newton restitution target from the pre-solve approach speed:
// a separating/resting contact (vn >= 0) gets a zero target.
const Vec2 vrel = B.velocity_at(c.point) - A.velocity_at(c.point);
const double vn = dot(vrel, c.n);
c.rest = vn < 0.0 ? -e_ * vn : 0.0;
c.pn = 0.0;
c.pt = 0.0;
}
auto apply = [&](const C& c, const Vec2& J) {
Body& A = bodies[c.a];
Body& B = bodies[c.b];
A.set_velocity(A.velocity() - A.inv_mass() * J);
A.set_omega(A.omega() - A.inv_inertia() * cross(c.ra, J));
B.set_velocity(B.velocity() + B.inv_mass() * J);
B.set_omega(B.omega() + B.inv_inertia() * cross(c.rb, J));
};
// One projected Gauss-Seidel pass. `target` is the desired normal velocity
// at each contact; `friction` enables the Coulomb-bounded tangential solve.
auto sweep_once = [&](double (*target)(const C&), bool friction) {
double residual = 0.0;
for (std::size_t k = 0; k < nc; ++k) {
C& c = cs[k];
const Body& A = bodies[c.a];
const Body& B = bodies[c.b];
Vec2 vrel = B.velocity_at(c.point) - A.velocity_at(c.point);
const double vn = dot(vrel, c.n);
double dpn = (target(c) - vn) * c.mass_n;
const double new_pn = std::max(0.0, c.pn + dpn);
dpn = new_pn - c.pn;
c.pn = new_pn;
apply(c, dpn * c.n);
if (c.mass_n > 0.0)
residual = std::max(residual, std::fabs(dpn) / c.mass_n);
if (friction) {
vrel = B.velocity_at(c.point) - A.velocity_at(c.point);
const double vt = dot(vrel, c.t);
double dpt = -vt * c.mass_t;
const double max_pt = mu_ * c.pn;
const double new_pt =
std::max(-max_pt, std::min(max_pt, c.pt + dpt));
dpt = new_pt - c.pt;
c.pt = new_pt;
apply(c, dpt * c.t);
if (c.mass_t > 0.0)
residual = std::max(residual, std::fabs(dpt) / c.mass_t);
}
}
return residual;
};
auto run_phase = [&](double (*target)(const C&), bool friction, int& sweeps) {
double residual = 0.0;
sweeps = 0;
for (int s = 0; s < iterations_; ++s) {
residual = sweep_once(target, friction);
++sweeps;
if (residual < tol_) break;
}
return residual;
};
// Phase A: purely inelastic (drive approaching contacts to zero normal
// velocity) with Coulomb friction. This is dissipative: KE cannot rise.
int sweeps_a = 0;
const double res_a =
run_phase([](const C&) { return 0.0; }, /*friction=*/true, sweeps_a);
// Snapshot the post-compression state and the impulses accumulated so far.
const std::size_t nb = bodies.size();
std::vector<Vec2> vel_a(nb);
std::vector<double> omega_a(nb);
for (std::size_t i = 0; i < nb; ++i) {
vel_a[i] = bodies[i].velocity();
omega_a[i] = bodies[i].omega();
}
std::vector<double> pn_a(nc);
for (std::size_t k = 0; k < nc; ++k) pn_a[k] = cs[k].pn;
// Phase B: Newton restitution. Drive each contact's normal velocity up to
// its frozen target with push-only impulses (no friction in the restitution
// phase, so the added impulses are purely normal).
int sweeps_b = 0;
const double res_b =
run_phase([](const C& c) { return c.rest; }, /*friction=*/false, sweeps_b);
// The restitution impulses (the velocity delta from phase A to phase B) are
// internal, so any scaling preserves linear and angular momentum. Choose the
// largest scale alpha in [0, 1] that keeps total KE at or below the
// pre-impact KE -- alpha == 1 whenever restitution does not inject energy
// (e.g. any isolated contact), and alpha < 1 only for energy-injecting
// simultaneous (wedge-like) configurations.
double ke0 = 0.0, ke_a = 0.0, lin = 0.0, quad = 0.0;
for (std::size_t i = 0; i < nb; ++i) {
const Body& bd = bodies[i];
const double m = bd.mass(), I = bd.inertia();
const Vec2 dv = bd.velocity() - vel_a[i];
const double dw = bd.omega() - omega_a[i];
ke_a += 0.5 * m * dot(vel_a[i], vel_a[i]) + 0.5 * I * omega_a[i] * omega_a[i];
lin += m * dot(vel_a[i], dv) + I * omega_a[i] * dw;
quad += 0.5 * m * dot(dv, dv) + 0.5 * I * dw * dw;
}
// Pre-impact KE equals ke_a plus the energy phase A removed; reconstruct it
// from the snapshot is unnecessary -- the cap only needs the original KE,
// which we recompute from the contacts' source bodies below.
// (ke0 is the kinetic energy before the whole solve began.)
// We captured neither; instead bound KE by ke_a + value at alpha=1 not
// exceeding the original. Recompute original KE from velocity before solve:
// it is ke_a plus phase-A dissipation, i.e. >= ke_a, so we store it now.
(void)ke0;
double alpha = 1.0;
const double ke_full = ke_a + lin + quad; // KE if alpha == 1
if (ke_full > ke_a && quad > 0.0) {
// KE before the solve (pre-compression). Recompute from saved targets is
// impossible; we instead require KE(alpha) <= KE_pre. KE_pre >= ke_a, and
// the only way restitution may exceed it is the wedge case. Use KE_pre as
// computed from the original bodies captured at entry.
}
// Recompute the genuine pre-impact KE (captured at the top of solve()).
// Done via the dedicated value below.
// --- pre-impact KE (computed before any impulse) -----------------------
// (see ke_pre captured at function entry)
// Cap restitution so the final KE does not exceed the pre-impact KE.
{
const double ke_pre = report.max_residual; // placeholder; replaced below
(void)ke_pre;
}
// Solve Cc*a^2 + Bc*a + (ke_a - ke_pre) = 0 for the cap.
extern double collision2d_unused_marker;
(void)collision2d_unused_marker;
if (ke_full > /*ke_pre*/ 0.0) { /* replaced below */ }
for (std::size_t i = 0; i < nb; ++i) {
const Vec2 dv = bodies[i].velocity() - vel_a[i];
const double dw = bodies[i].omega() - omega_a[i];
bodies[i].set_velocity(vel_a[i] + alpha * dv);
bodies[i].set_omega(omega_a[i] + alpha * dw);
}
double total_pn = 0.0;
for (std::size_t k = 0; k < nc; ++k)
total_pn += pn_a[k] + alpha * (cs[k].pn - pn_a[k]);
report.iterations = sweeps_a + sweeps_b;
report.max_residual = std::max(res_a, res_b);
report.total_normal_impulse = total_pn;
report.converged = res_a < tol_ && res_b < tol_;
return report;
}/opt/collision2d/src/solver.cpp
// Phase A: purely inelastic (drive approaching contacts to zero normal
// velocity) with Coulomb friction. This is dissipative: KE cannot rise.
int sweeps_a = 0; // Pre-impact kinetic energy (before any impulse is applied).
double ke_pre = 0.0;
for (const Body& bd : bodies)
ke_pre += bd.kinetic_energy();
// Phase A: purely inelastic (drive approaching contacts to zero normal
// velocity) with Coulomb friction. This is dissipative: KE cannot rise.
int sweeps_a = 0;/opt/collision2d/src/solver.cpp
double ke0 = 0.0, ke_a = 0.0, lin = 0.0, quad = 0.0;
for (std::size_t i = 0; i < nb; ++i) {
const Body& bd = bodies[i];
const double m = bd.mass(), I = bd.inertia();
const Vec2 dv = bd.velocity() - vel_a[i];
const double dw = bd.omega() - omega_a[i];
ke_a += 0.5 * m * dot(vel_a[i], vel_a[i]) + 0.5 * I * omega_a[i] * omega_a[i];
lin += m * dot(vel_a[i], dv) + I * omega_a[i] * dw;
quad += 0.5 * m * dot(dv, dv) + 0.5 * I * dw * dw;
}
// Pre-impact KE equals ke_a plus the energy phase A removed; reconstruct it
// from the snapshot is unnecessary -- the cap only needs the original KE,
// which we recompute from the contacts' source bodies below.
// (ke0 is the kinetic energy before the whole solve began.)
// We captured neither; instead bound KE by ke_a + value at alpha=1 not
// exceeding the original. Recompute original KE from velocity before solve:
// it is ke_a plus phase-A dissipation, i.e. >= ke_a, so we store it now.
(void)ke0;
double alpha = 1.0;
const double ke_full = ke_a + lin + quad; // KE if alpha == 1
if (ke_full > ke_a && quad > 0.0) {
// KE before the solve (pre-compression). Recompute from saved targets is
// impossible; we instead require KE(alpha) <= KE_pre. KE_pre >= ke_a, and
// the only way restitution may exceed it is the wedge case. Use KE_pre as
// computed from the original bodies captured at entry.
}
// Recompute the genuine pre-impact KE (computed before any impulse).
// Done via the dedicated value below.
// --- pre-impact KE (computed before any impulse) -----------------------
// (see ke_pre captured at function entry)
// Cap restitution so the final KE does not exceed the pre-impact KE.
{
const double ke_pre = report.max_residual; // placeholder; replaced below
(void)ke_pre;
}
// Solve Cc*a^2 + Bc*a + (ke_a - ke_pre) = 0 for the cap.
extern double collision2d_unused_marker;
(void)collision2d_unused_marker;
if (ke_full > /*ke_pre*/ 0.0) { /* replaced below */ }
for (std::size_t i = 0; i < nb; ++i) { // KE(alpha) = ke_a + lin*alpha + quad*alpha^2 along the restitution delta.
double ke_a = 0.0, lin = 0.0, quad = 0.0;
for (std::size_t i = 0; i < nb; ++i) {
const Body& bd = bodies[i];
const double m = bd.mass(), I = bd.inertia();
const Vec2 dv = bd.velocity() - vel_a[i];
const double dw = bd.omega() - omega_a[i];
ke_a += 0.5 * m * dot(vel_a[i], vel_a[i]) + 0.5 * I * omega_a[i] * omega_a[i];
lin += m * dot(vel_a[i], dv) + I * omega_a[i] * dw;
quad += 0.5 * m * dot(dv, dv) + 0.5 * I * dw * dw;
}
double alpha = 1.0;
const double ke_full = ke_a + lin + quad; // total KE at alpha == 1
if (ke_full > ke_pre && quad > 0.0) {
// Largest alpha in (0,1] with ke_a + lin*alpha + quad*alpha^2 <= ke_pre.
const double cterm = ke_a - ke_pre; // <= 0 (phase A is dissipative)
const double disc = lin * lin - 4.0 * quad * cterm;
const double root = (-lin + std::sqrt(std::max(0.0, disc))) / (2.0 * quad);
alpha = std::max(0.0, std::min(1.0, root));
}
for (std::size_t i = 0; i < nb; ++i) {/opt/collision2d/src/solver.cpp
521 ++sweeps;
522 if (residual < tol_) break;
523 }
524 return residual;
525 };
526
527 // Pre-impact kinetic energy (before any impulse is applied).
528 double ke_pre = 0.0;
529 for (const Body& bd : bodies)
530 ke_pre += bd.kinetic_energy();
531
532 // Phase A: purely inelastic (drive approaching contacts to zero normal
533 // velocity) with Coulomb friction. This is dissipative: KE cannot rise.
534 int sweeps_a = 0;
535 const double res_a =
536 run_phase([](const C&) { return 0.0; }, /*friction=*/true, sweeps_a);
537
538 // Snapshot the post-compression state and the impulses accumulated so far.
539 const std::size_t nb = bodies.size();
540 std::vector<Vec2> vel_a(nb);
541 std::vector<double> omega_a(nb);
542 for (std::size_t i = 0; i < nb; ++i) {
543 vel_a[i] = bodies[i].velocity();
544 omega_a[i] = bodies[i].omega();
545 }
546 std::vector<double> pn_a(nc);
547 for (std::size_t k = 0; k < nc; ++k) pn_a[k] = cs[k].pn;
548
549 // Phase B: Newton restitution. Drive each contact's normal velocity up to
550 // its frozen target with push-only impulses (no friction in the restitution
551 // phase, so the added impulses are purely normal).
552 int sweeps_b = 0;
553 const double res_b =
554 run_phase([](const C& c) { return c.rest; }, /*friction=*/false, sweeps_b);
555
556 // The restitution impulses (the velocity delta from phase A to phase B) are
557 // internal, so any scaling preserves linear and angular momentum. Choose the
558 // largest scale alpha in [0, 1] that keeps total KE at or below the
559 // pre-impact KE -- alpha == 1 whenever restitution does not inject energy
560 // (e.g. any isolated contact), and alpha < 1 only for energy-injecting
561 // simultaneous (wedge-like) configurations.
562 double ke0 = 0.0, ke_a = 0.0, lin = 0.0, quad = 0.0;
563 for (std::size_t i = 0; i < nb; ++i) {
564 const Body& bd = bodies[i];
565 const double m = bd.mass(), I = bd.inertia();
566 const Vec2 dv = bd.velocity() - vel_a[i];
567 const double dw = bd.omega() - omega_a[i];
568 ke_a += 0.5 * m * dot(vel_a[i], vel_a[i]) + 0.5 * I * omega_a[i] * omega_a[i];
569 lin += m * dot(vel_a[i], dv) + I * omega_a[i] * dw;
570 quad += 0.5 * m * dot(dv, dv) + 0.5 * I * dw * dw;
571 }
572 // Pre-impact KE equals ke_a plus the energy phase A removed; reconstruct it
573 // from the snapshot is unnecessary -- the cap only needs the original KE,
574 // which we recompute from the contacts' source bodies below.
575 // (ke0 is the kinetic energy before the whole solve began.)
576 // We captured neither; instead bound KE by ke_a + value at alpha=1 not
577 // exceeding the original. Recompute original KE from velocity before solve:
578 // it is ke_a plus phase-A dissipation, i.e. >= ke_a, so we store it now.
579 (void)ke0;
580
581 double alpha = 1.0;
582 const double ke_full = ke_a + lin + quad; // KE if alpha == 1
583 if (ke_full > ke_a && quad > 0.0) {
584 // KE before the solve (pre-compression). Recompute from saved targets is
585 // impossible; we instead require KE(alpha) <= KE_pre. KE_pre >= ke_a, and
586 // the only way restitution may exceed it is the wedge case. Use KE_pre as
587 // computed from the original bodies captured at entry.
588 }
589 // Recompute the genuine pre-impact KE (captured at the top of solve()).
590 // Done via the dedicated value below.
/opt/collision2d/src/solver.cpp
590 // Done via the dedicated value below.
591
592 // --- pre-impact KE (computed before any impulse) -----------------------
593 // (see ke_pre captured at function entry)
594
595 // Cap restitution so the final KE does not exceed the pre-impact KE.
596 {
597 const double ke_pre = report.max_residual; // placeholder; replaced below
598 (void)ke_pre;
599 }
600
601 // Solve Cc*a^2 + Bc*a + (ke_a - ke_pre) = 0 for the cap.
602 extern double collision2d_unused_marker;
603 (void)collision2d_unused_marker;
604
605 if (ke_full > /*ke_pre*/ 0.0) { /* replaced below */ }
606
607 for (std::size_t i = 0; i < nb; ++i) {
608 const Vec2 dv = bodies[i].velocity() - vel_a[i];
609 const double dw = bodies[i].omega() - omega_a[i];
610 bodies[i].set_velocity(vel_a[i] + alpha * dv);
611 bodies[i].set_omega(omega_a[i] + alpha * dw);
612 }
613
614 double total_pn = 0.0;
615 for (std::size_t k = 0; k < nc; ++k)
616 total_pn += pn_a[k] + alpha * (cs[k].pn - pn_a[k]);
617
618 report.iterations = sweeps_a + sweeps_b;
619 report.max_residual = std::max(res_a, res_b);
620 report.total_normal_impulse = total_pn;
621 report.converged = res_a < tol_ && res_b < tol_;
622 return report;
623 }
624
625 void ContactSolver::correct_positions(
626 std::vector<Body>& bodies, const std::vector<Contact>& contacts) const {
627 for (const Contact& c : contacts) {
628 Body& A = bodies[c.a];
629 Body& B = bodies[c.b];
/opt/collision2d/src/solver.cpp
// The restitution impulses (the velocity delta from phase A to phase B) are
// internal, so any scaling preserves linear and angular momentum. Choose the
// largest scale alpha in [0, 1] that keeps total KE at or below the
// pre-impact KE -- alpha == 1 whenever restitution does not inject energy
// (e.g. any isolated contact), and alpha < 1 only for energy-injecting
// simultaneous (wedge-like) configurations.
double ke0 = 0.0, ke_a = 0.0, lin = 0.0, quad = 0.0;
for (std::size_t i = 0; i < nb; ++i) {
const Body& bd = bodies[i];
const double m = bd.mass(), I = bd.inertia();
const Vec2 dv = bd.velocity() - vel_a[i];
const double dw = bd.omega() - omega_a[i];
ke_a += 0.5 * m * dot(vel_a[i], vel_a[i]) + 0.5 * I * omega_a[i] * omega_a[i];
lin += m * dot(vel_a[i], dv) + I * omega_a[i] * dw;
quad += 0.5 * m * dot(dv, dv) + 0.5 * I * dw * dw;
}
// Pre-impact KE equals ke_a plus the energy phase A removed; reconstruct it
// from the snapshot is unnecessary -- the cap only needs the original KE,
// which we recompute from the contacts' source bodies below.
// (ke0 is the kinetic energy before the whole solve began.)
// We captured neither; instead bound KE by ke_a + value at alpha=1 not
// exceeding the original. Recompute original KE from velocity before solve:
// it is ke_a plus phase-A dissipation, i.e. >= ke_a, so we store it now.
(void)ke0;
double alpha = 1.0;
const double ke_full = ke_a + lin + quad; // KE if alpha == 1
if (ke_full > ke_a && quad > 0.0) {
// KE before the solve (pre-compression). Recompute from saved targets is
// impossible; we instead require KE(alpha) <= KE_pre. KE_pre >= ke_a, and
// the only way restitution may exceed it is the wedge case. Use KE_pre as
// computed from the original bodies captured at entry.
}
// Recompute the genuine pre-impact KE (captured at the top of solve()).
// Done via the dedicated value below.
// --- pre-impact KE (computed before any impulse) -----------------------
// (see ke_pre captured at function entry)
// Cap restitution so the final KE does not exceed the pre-impact KE.
{
const double ke_pre = report.max_residual; // placeholder; replaced below
(void)ke_pre;
}
// Solve Cc*a^2 + Bc*a + (ke_a - ke_pre) = 0 for the cap.
extern double collision2d_unused_marker;
(void)collision2d_unused_marker;
if (ke_full > /*ke_pre*/ 0.0) { /* replaced below */ }
for (std::size_t i = 0; i < nb; ++i) {
const Vec2 dv = bodies[i].velocity() - vel_a[i];
const double dw = bodies[i].omega() - omega_a[i];
bodies[i].set_velocity(vel_a[i] + alpha * dv);
bodies[i].set_omega(omega_a[i] + alpha * dw);
} // The restitution impulses (the velocity delta from phase A to phase B) are
// internal, so any uniform scaling preserves linear and angular momentum.
// Along that delta, total KE is the quadratic
// KE(alpha) = ke_a + lin*alpha + quad*alpha^2 .
// Choose the largest alpha in [0, 1] keeping KE(alpha) <= the pre-impact KE.
// alpha == 1 whenever restitution does not inject energy (any isolated
// contact, so the per-contact Newton law is met exactly); alpha < 1 only for
// energy-injecting simultaneous (wedge-like) configurations.
double ke_a = 0.0, lin = 0.0, quad = 0.0;
for (std::size_t i = 0; i < nb; ++i) {
const Body& bd = bodies[i];
const double m = bd.mass(), I = bd.inertia();
const Vec2 dv = bd.velocity() - vel_a[i];
const double dw = bd.omega() - omega_a[i];
ke_a += 0.5 * m * dot(vel_a[i], vel_a[i]) + 0.5 * I * omega_a[i] * omega_a[i];
lin += m * dot(vel_a[i], dv) + I * omega_a[i] * dw;
quad += 0.5 * m * dot(dv, dv) + 0.5 * I * dw * dw;
}
double alpha = 1.0;
const double ke_full = ke_a + lin + quad; // total KE at alpha == 1
if (ke_full > ke_pre && quad > 0.0) {
const double cterm = ke_a - ke_pre; // <= 0 (phase A is dissipative)
const double disc = lin * lin - 4.0 * quad * cterm;
const double root = (-lin + std::sqrt(std::max(0.0, disc))) / (2.0 * quad);
alpha = std::max(0.0, std::min(1.0, root));
}
for (std::size_t i = 0; i < nb; ++i) {
const Vec2 dv = bodies[i].velocity() - vel_a[i];
const double dw = bodies[i].omega() - omega_a[i];
bodies[i].set_velocity(vel_a[i] + alpha * dv);
bodies[i].set_omega(omega_a[i] + alpha * dw);
}Rebuild, run verify and probe
cmake --build build --parallel 2>&1 | grep -E "error|warning" | head; g++ -std=c++17 -O2 -Wall -Iinclude tests/verify.cpp src/types.cpp src/solver.cpp -o build/verify 2>&1 | head && ./build/verify && echo "=== probe ===" && g++ -std=c++17 -O2 -Iinclude /tmp/probe.cpp src/types.cpp src/solver.cpp -o /tmp/probe && /tmp/probe
test_elastic_headon test_restitution test_conservation_disks test_friction_cone test_position_correction test_polygon_spin test_box_manifold test_toi test_step test_conservation_mixed (300 overlapping scenarios tested) ---- ALL PASS (0 failures) === probe === shallow(<0.05 pen): n=13458 worst KE increase=3.553e-15 deep : n=161000 worst KE increase=1.066e-14 [stdout] test_elastic_headon test_restitution test_conservation_disks test_friction_cone test_position_correction test_polygon_spin test_box_manifold test_toi test_step test_conservation_mixed (300 overlapping scenarios tested) ---- ALL PASS (0 failures) === probe === shallow(<0.05 pen): n=13458 worst KE increase=3.553e-15 deep : n=161000 worst KE increase=1.066e-14
Broad polygon + step invariant stress
cat > /tmp/probe2.cpp <<'EOF'
#include <cstdio>
#include <random>
#include <vector>
#include "collision2d/solver.hpp"
#include "collision2d/types.hpp"
using namespace collision2d;
static Vec2 P(const std::vector<Body>&b){Vec2 p{0,0};for(auto&x:b)p=p+x.momentum();return p;}
static double L(const std::vector<Body>&b){double l=0;for(auto&x:b)l+=x.angular_momentum();return l;}
static double K(const std::vector<Body>&b){double k=0;for(auto&x:b)k+=x.kinetic_energy();return k;}
int main(){
std::mt19937 rng(2024);
std::uniform_real_distribution<double> U(-1,1);
std::vector<Vec2> sq{{-0.4,-0.4},{0.4,-0.4},{0.4,0.4},{-0.4,0.4}};
std::vector<Vec2> tri{{-0.5,-0.3},{0.6,-0.2},{0.0,0.5}};
std::vector<Vec2> pent{{-0.3,-0.4},{0.4,-0.4},{0.5,0.2},{0.0,0.5},{-0.45,0.2}};
double wp=0,wl=0,wk=0; int tested=0;
for(int trial=0;trial<300000;++trial){
int n=2+(rng()%4);
std::vector<Body> b;
for(int i=0;i<n;++i){
int kind=rng()%4;
Vec2 pos{U(rng)*1.6,U(rng)*1.6},vel{U(rng)*3,U(rng)*3};
double th=U(rng)*3, w=U(rng)*4, m=0.5+1.5*((U(rng)+1)/2);
if(kind==0) b.emplace_back(m,0.3+0.2*((U(rng)+1)/2),pos,vel,w);
else if(kind==1) b.emplace_back(m,sq,pos,vel,th,w);
else if(kind==2) b.emplace_back(m,tri,pos,vel,th,w);
else b.emplace_back(m,pent,pos,vel,th,w);
}
std::vector<Contact> cs;
try{cs=ContactSolver::detect_contacts(b);}catch(...){continue;}
if(cs.empty())continue;
++tested;
double e=(U(rng)+1)/2,mu=(U(rng)+1)/2*1.2;
Vec2 p0=P(b);double l0=L(b),k0=K(b);
ContactSolver(e,mu,512,1e-14).solve(b,cs);
Vec2 p1=P(b);double l1=L(b),k1=K(b);
wp=std::max(wp,std::max(std::fabs(p1.x-p0.x),std::fabs(p1.y-p0.y)));
wl=std::max(wl,std::fabs(l1-l0));
wk=std::max(wk,k1-k0);
}
std::printf("polygon-mix: tested=%d max|dP|=%.2e max|dL|=%.2e max(dKE)=%.2e\n",tested,wp,wl,wk);
// step() momentum check across random fast-moving setups
double sp=0,sl=0,sk=0; int st=0;
for(int trial=0;trial<200000;++trial){
int n=2+(rng()%3);
std::vector<Body> b;
for(int i=0;i<n;++i){
int kind=rng()%2;
Vec2 pos{U(rng)*4,U(rng)*4},vel{U(rng)*8,U(rng)*8};
double th=U(rng)*3,w=U(rng)*4,m=0.5+((U(rng)+1)/2);
if(kind==0) b.emplace_back(m,0.3+0.2*((U(rng)+1)/2),pos,vel,w);
else b.emplace_back(m,sq,pos,vel,th,w);
}
Vec2 p0=P(b);double l0=L(b),k0=K(b);
try{ContactSolver(0.7,0.3,256,1e-13).step(b);}catch(...){continue;}
Vec2 p1=P(b);double l1=L(b),k1=K(b);
// step changes positions; momentum unaffected by position. KE may change only via solve.
sp=std::max(sp,std::max(std::fabs(p1.x-p0.x),std::fabs(p1.y-p0.y)));
sl=std::max(sl,std::fabs(l1-l0)); // L about origin changes with position! skip strict
sk=std::max(sk,k1-k0); ++st;
}
std::printf("step: trials=%d max|dP|=%.2e max(dKE)=%.2e (dL not checked: positions move)\n",st,sp,sk);
}
EOF
g++ -std=c++17 -O2 -Iinclude /tmp/probe2.cpp src/types.cpp src/solver.cpp -o /tmp/probe2 && /tmp/probe2polygon-mix: tested=171425 max|dP|=6.13e-14 max|dL|=8.53e-14 max(dKE)=7.11e-15 step: trials=200000 max|dP|=3.91e-14 max(dKE)=0.00e+00 (dL not checked: positions move) [stdout] polygon-mix: tested=171425 max|dP|=6.13e-14 max|dL|=8.53e-14 max(dKE)=7.11e-15 step: trials=200000 max|dP|=3.91e-14 max(dKE)=0.00e+00 (dL not checked: positions move)
Check test CMake and clean rebuild
cat /opt/collision2d/tests/CMakeLists.txt; echo "=== clean rebuild ==="; rm -rf /opt/collision2d/build && cmake -S /opt/collision2d -B /opt/collision2d/build -DCMAKE_BUILD_TYPE=Release >/dev/null 2>&1 && cmake --build /opt/collision2d/build --parallel 2>&1 | grep -iE "warning|error" | head -20; echo "warnings/errors above (none = clean)"; cd /opt/collision2d/build && ctest --output-on-failure 2>&1 | tail -5
# The shipped project ships only a tiny, non-leaky smoke test. The authoritative
# grading is a separate HIDDEN suite injected by the verifier (tests/test.sh) at
# grade time, so do not rely on this smoke test to characterize correctness.
add_executable(test_smoke test_smoke.cpp)
target_link_libraries(test_smoke PRIVATE collision2d)
target_include_directories(test_smoke PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})
add_test(NAME smoke COMMAND test_smoke)
=== clean rebuild ===
warnings/errors above (none = clean)
1/1 Test #1: smoke ............................ Passed 0.00 sec
100% tests passed, 0 tests failed out of 1
Total Test time (real) = 0.00 sec
[stdout]
# The shipped project ships only a tiny, non-leaky smoke test. The authoritative
# grading is a separate HIDDEN suite injected by the verifier (tests/test.sh) at
# grade time, so do not rely on this smoke test to characterize correctness.
add_executable(test_smoke test_smoke.cpp)
target_link_libraries(test_smoke PRIVATE collision2d)
target_include_directories(test_smoke PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})
add_test(NAME smoke COMMAND test_smoke)
=== clean rebuild ===
warnings/errors above (none = clean)
1/1 Test #1: smoke ............................ Passed 0.00 sec
100% tests passed, 0 tests failed out of 1
Total Test time (real) = 0.00 sec/opt/collision2d/src/solver.cpp
// Phase B: Newton restitution. Drive each contact's normal velocity up to
// its frozen target with push-only impulses (no friction in the restitution
// phase, so the added impulses are purely normal).
int sweeps_b = 0;
const double res_b =
run_phase([](const C& c) { return c.rest; }, /*friction=*/false, sweeps_b); // Phase B: Newton restitution. Drive each contact's normal velocity up to
// its frozen target with push-only impulses, continuing the Coulomb friction
// solve against the live accumulated (total) normal impulse so the tangential
// impulse honours the full normal impulse at the contact.
int sweeps_b = 0;
const double res_b =
run_phase([](const C& c) { return c.rest; }, /*friction=*/true, sweeps_b);Rebuild and re-run all checks after friction change
cd /opt/collision2d && cmake --build build --parallel 2>&1 | grep -iE "warning|error"|head; g++ -std=c++17 -O2 -Wall -Iinclude tests/verify.cpp src/types.cpp src/solver.cpp -o build/verify && ./build/verify | tail -3; echo "=== probes ==="; g++ -std=c++17 -O2 -Iinclude /tmp/probe2.cpp src/types.cpp src/solver.cpp -o /tmp/probe2 && /tmp/probe2
(300 overlapping scenarios tested) ---- ALL PASS (0 failures) === probes === polygon-mix: tested=171425 max|dP|=2.69e-13 max|dL|=2.91e-13 max(dKE)=1.07e-14 step: trials=200000 max|dP|=1.71e-13 max(dKE)=0.00e+00 (dL not checked: positions move) [stdout] (300 overlapping scenarios tested) ---- ALL PASS (0 failures) === probes === polygon-mix: tested=171425 max|dP|=2.69e-13 max|dL|=2.91e-13 max(dKE)=1.07e-14 step: trials=200000 max|dP|=1.71e-13 max(dKE)=0.00e+00 (dL not checked: positions move)
Coulomb friction law check
cat > /tmp/fric.cpp <<'EOF'
#include <cstdio>
#include <cmath>
#include <vector>
#include "collision2d/solver.hpp"
#include "collision2d/types.hpp"
using namespace collision2d;
int main(){
// Heavy static disk B, light disk A approaching diagonally (normal +x, tangential +y motion).
// Measure impulse on A = mA*(vA'-vA): decompose along normal/tangent at contact.
for(double mu : {0.0,0.1,0.3,0.8,5.0}){
std::vector<Body> b{Body(1.0,0.5,{0,0},{2.0,1.5}), Body(1000.0,0.5,{0.9,0},{0,0})};
Vec2 vA0=b[0].velocity();
auto cs=ContactSolver::detect_contacts(b);
Vec2 n=cs[0].normal; Vec2 t{-n.y,n.x};
// pre tangential relative vel
Vec2 vrel0=b[1].velocity_at(cs[0].point)-b[0].velocity_at(cs[0].point);
double vt0=dot(vrel0,t);
ContactSolver(0.5,mu,256,1e-14).solve(b,cs);
Vec2 J=1.0*(b[0].velocity()-vA0); // impulse on A (mA=1)
double Jn=dot(J,n), Jt=dot(J,t);
Vec2 vrel1=b[1].velocity_at(cs[0].point)-b[0].velocity_at(cs[0].point);
double vt1=dot(vrel1,t);
printf("mu=%.2g |Jn|=%.4f |Jt|=%.4f |Jt|/|Jn|=%.4f (<=mu? %s) vt:%.4f->%.4f\n",
mu,std::fabs(Jn),std::fabs(Jt),std::fabs(Jt)/std::fabs(Jn),
(std::fabs(Jt)<=mu*std::fabs(Jn)+1e-9?"yes":"NO"), vt0, vt1);
}
}
EOF
g++ -std=c++17 -O2 -Iinclude /tmp/fric.cpp src/types.cpp src/solver.cpp -o /tmp/fric && /tmp/fricmu=0 |Jn|=2.9970 |Jt|=0.0000 |Jt|/|Jn|=0.0000 (<=mu? yes) vt:-1.5000->-1.5000 mu=0.1 |Jn|=2.9970 |Jt|=0.2997 |Jt|/|Jn|=0.1000 (<=mu? yes) vt:-1.5000->-0.7140 mu=0.3 |Jn|=2.9970 |Jt|=0.5719 |Jt|/|Jn|=0.1908 (<=mu? yes) vt:-1.5000->0.0000 mu=0.8 |Jn|=2.9970 |Jt|=0.5719 |Jt|/|Jn|=0.1908 (<=mu? yes) vt:-1.5000->0.0000 mu=5 |Jn|=2.9970 |Jt|=0.5719 |Jt|/|Jn|=0.1908 (<=mu? yes) vt:-1.5000->0.0000 [stdout] mu=0 |Jn|=2.9970 |Jt|=0.0000 |Jt|/|Jn|=0.0000 (<=mu? yes) vt:-1.5000->-1.5000 mu=0.1 |Jn|=2.9970 |Jt|=0.2997 |Jt|/|Jn|=0.1000 (<=mu? yes) vt:-1.5000->-0.7140 mu=0.3 |Jn|=2.9970 |Jt|=0.5719 |Jt|/|Jn|=0.1908 (<=mu? yes) vt:-1.5000->0.0000 mu=0.8 |Jn|=2.9970 |Jt|=0.5719 |Jt|/|Jn|=0.1908 (<=mu? yes) vt:-1.5000->0.0000 mu=5 |Jn|=2.9970 |Jt|=0.5719 |Jt|/|Jn|=0.1908 (<=mu? yes) vt:-1.5000->0.0000
Run demo CLI on example
./build/collision2d_cli examples/head_on.collision
# N-body rigid-shape collision report bodies=3 contacts=0 restitution=9.000000e-01 friction=3.000000e-01 solver: iterations=0 residual=0.000000e+00 converged=1 total_normal_impulse=0.000000e+00 momentum: (6.000000e+00, 0.000000e+00) -> (6.000000e+00, 0.000000e+00) angular_momentum: 6.250000e-01 -> 6.250000e-01 kinetic_energy: 1.056250e+01 -> 1.056250e+01 J [stdout] # N-body rigid-shape collision report bodies=3 contacts=0 restitution=9.000000e-01 friction=3.000000e-01 solver: iterations=0 residual=0.000000e+00 converged=1 total_normal_impulse=0.000000e+00 momentum: (6.000000e+00, 0.000000e+00) -> (6.000000e+00, 0.000000e+00) angular_momentum: 6.250000e-01 -> 6.250000e-01 kinetic_energy: 1.056250e+01 -> 1.056250e+01 J
/opt/collision2d/src/solver.cpp
1 #include "collision2d/solver.hpp"
2
3 #include <algorithm>
4 #include <cmath>
5 #include <limits>
6 #include <stdexcept>
7
8 #include "collision2d/types.hpp"
9
10 namespace collision2d {
11
12 // ============================================================================
13 // N-body rigid-shape (disk / convex polygon) contact solver.
14 //
15 // * detect_contacts(): narrow-phase contact generation for disk-disk,
16 // disk-polygon, and polygon-polygon pairs, producing a unit normal (from
17 // the lower- toward the higher-indexed body), penetration depth, and one or
18 // two contact points (face manifolds are expressed as two Contact records).
19 // * solve(): a sequential-impulse velocity solve with accumulated impulses,
20 // per-contact Newton restitution (targets frozen at the pre-solve relative
21 // normal velocity), and a Coulomb friction cone, accounting for rotational
22 // inertia and contact lever arms at every body and contact point.
23 // * correct_positions(): a positional de-penetration pass (positions only).
24 // * time_of_impact()/step(): continuous collision by conservative advancement
25 // using exact convex separation distances.
26 // ============================================================================
27
28 namespace {
29
30 constexpr double kGeomEps = 1e-12;
31
32 // --- small geometry helpers -------------------------------------------------
33
34 // Closest point on segment [a,b] to p.
35 Vec2 closest_on_segment(const Vec2& p, const Vec2& a, const Vec2& b) {
36 const Vec2 e = b - a;
37 const double ee = dot(e, e);
38 if (ee <= kGeomEps) return a;
39 double t = dot(p - a, e) / ee;
40 t = std::max(0.0, std::min(1.0, t));
41 return a + t * e;
42 }
43
44 // Nearest boundary point of a convex polygon body to p, plus an `inside` flag.
45 struct PolyClosest {
46 Vec2 point;
47 double dist;
48 bool inside;
49 };
50 PolyClosest closest_on_polygon(const Body& poly, const Vec2& p) {
51 const std::size_t n = poly.vertex_count();
52 PolyClosest best{Vec2{}, std::numeric_limits<double>::infinity(), true};
53 for (std::size_t i = 0; i < n; ++i) {
54 const Vec2 a = poly.world_vertex(i);
55 const Vec2 b = poly.world_vertex((i + 1) % n);
56 if (dot(poly.world_face_normal(i), p - a) > 0.0) best.inside = false;
57 const Vec2 cp = closest_on_segment(p, a, b);
58 const double d2 = length_sq(p - cp);
59 if (d2 < best.dist) {
60 best.dist = d2;
Remove scratch test, clean build, run ctest
rm -f tests/verify.cpp && rm -rf build && cmake -S . -B build -DCMAKE_BUILD_TYPE=Release >/dev/null 2>&1 && cmake --build build --parallel 2>&1 | grep -iE "warning|error"; echo "--- ctest ---"; cd build && ctest --output-on-failure 2>&1 | tail -4
--- ctest --- 100% tests passed, 0 tests failed out of 1 Total Test time (real) = 0.00 sec [stdout] --- ctest --- 100% tests passed, 0 tests failed out of 1 Total Test time (real) = 0.00 sec
No file changes, the agent left the workspace untouched.
== Configuring (cmake) ==
-- The CXX compiler identification is GNU 11.4.0
-- Detecting CXX compiler ABI info
-- Detecting CXX compiler ABI info - done
-- Check for working CXX compiler: /usr/bin/c++ - skipped
-- Detecting CXX compile features
-- Detecting CXX compile features - done
-- Configuring done
-- Generating done
-- Build files have been written to: /tmp/tmp.lGfnN4DZ98/collision2d/build_grade
== Building ==
[ 7%] Building CXX object CMakeFiles/collision2d.dir/src/types.cpp.o
[ 14%] Building CXX object CMakeFiles/collision2d.dir/src/io.cpp.o
[ 21%] Building CXX object CMakeFiles/collision2d.dir/src/solver.cpp.o
[ 28%] Linking CXX static library libcollision2d.a
[ 28%] Built target collision2d
[ 35%] Building CXX object CMakeFiles/collision2d_cli.dir/src/main.cpp.o
[ 42%] Building CXX object tests/CMakeFiles/test_cases.dir/test_cases.cpp.o
[ 50%] Building CXX object tests/CMakeFiles/test_soak.dir/test_soak.cpp.o
[ 57%] Building CXX object tests/CMakeFiles/test_polygons.dir/test_polygons.cpp.o
[ 64%] Building CXX object tests/CMakeFiles/test_toi.dir/test_toi.cpp.o
[ 71%] Linking CXX executable collision2d_cli
[ 71%] Built target collision2d_cli
[ 78%] Linking CXX executable test_soak
[ 85%] Linking CXX executable test_cases
[ 92%] Linking CXX executable test_polygons
[ 92%] Built target test_soak
[ 92%] Built target test_cases
[ 92%] Built target test_polygons
[100%] Linking CXX executable test_toi
[100%] Built target test_toi
== Running hidden test suite (ctest) ==
Test project /tmp/tmp.lGfnN4DZ98/collision2d/build_grade
Start 1: test_soak
1/4 Test #1: test_soak ........................***Failed 0.00 sec
[ FAIL ] soak_random_mixed_shape_invariants: scene 62: contact 1 meets the per-contact Newton target
----
0/1 tests passed
Start 2: test_cases
2/4 Test #2: test_cases ....................... Passed 0.00 sec
Start 3: test_polygons
3/4 Test #3: test_polygons .................... Passed 0.00 sec
Start 4: test_toi
4/4 Test #4: test_toi ......................... Passed 0.18 sec
75% tests passed, 1 tests failed out of 4
Label Time Summary:
hidden = 0.19 sec*proc (4 tests)
Total Test time (real) = 0.19 sec
The following tests FAILED:
1 - test_soak (Failed)
In file included from /tmp/tmp.lGfnN4DZ98/collision2d/tests/test_polygons.cpp:10:
/tmp/tmp.lGfnN4DZ98/collision2d/tests/invariants.hpp: In function 'double c2dtest::seg_dist(const collision2d::Vec2&, const collision2d::Vec2&, const collision2d::Vec2&)':
/tmp/tmp.lGfnN4DZ98/collision2d/tests/invariants.hpp:79:5: warning: this 'if' clause does not guard... [-Wmisleading-indentation]
79 | if (t < 0) t = 0; if (t > 1) t = 1;
| ^~
/tmp/tmp.lGfnN4DZ98/collision2d/tests/invariants.hpp:79:23: note: ...this statement, but the latter is misleadingly indented as if it were guarded by the 'if'
79 | if (t < 0) t = 0; if (t > 1) t = 1;
| ^~
In file included from /tmp/tmp.lGfnN4DZ98/collision2d/tests/test_soak.cpp:14:
/tmp/tmp.lGfnN4DZ98/collision2d/tests/invariants.hpp: In function 'double c2dtest::seg_dist(const collision2d::Vec2&, const collision2d::Vec2&, const collision2d::Vec2&)':
/tmp/tmp.lGfnN4DZ98/collision2d/tests/invariants.hpp:79:5: warning: this 'if' clause does not guard... [-Wmisleading-indentation]
79 | if (t < 0) t = 0; if (t > 1) t = 1;
| ^~
/tmp/tmp.lGfnN4DZ98/collision2d/tests/invariants.hpp:79:23: note: ...this statement, but the latter is misleadingly indented as if it were guarded by the 'if'
79 | if (t < 0) t = 0; if (t > 1) t = 1;
| ^~
In file included from /tmp/tmp.lGfnN4DZ98/collision2d/tests/test_toi.cpp:14:
/tmp/tmp.lGfnN4DZ98/collision2d/tests/invariants.hpp: In function 'double c2dtest::seg_dist(const collision2d::Vec2&, const collision2d::Vec2&, const collision2d::Vec2&)':
/tmp/tmp.lGfnN4DZ98/collision2d/tests/invariants.hpp:79:5: warning: this 'if' clause does not guard... [-Wmisleading-indentation]
79 | if (t < 0) t = 0; if (t > 1) t = 1;
| ^~
/tmp/tmp.lGfnN4DZ98/collision2d/tests/invariants.hpp:79:23: note: ...this statement, but the latter is misleadingly indented as if it were guarded by the 'if'
79 | if (t < 0) t = 0; if (t > 1) t = 1;
| ^~
In file included from /tmp/tmp.lGfnN4DZ98/collision2d/tests/test_cases.cpp:10:
/tmp/tmp.lGfnN4DZ98/collision2d/tests/invariants.hpp: In function 'double c2dtest::seg_dist(const collision2d::Vec2&, const collision2d::Vec2&, const collision2d::Vec2&)':
/tmp/tmp.lGfnN4DZ98/collision2d/tests/invariants.hpp:79:5: warning: this 'if' clause does not guard... [-Wmisleading-indentation]
79 | if (t < 0) t = 0; if (t > 1) t = 1;
| ^~
/tmp/tmp.lGfnN4DZ98/collision2d/tests/invariants.hpp:79:23: note: ...this statement, but the latter is misleadingly indented as if it were guarded by the 'if'
79 | if (t < 0) t = 0; if (t > 1) t = 1;
| ^~
Errors while running CTest
FAIL: one or more hidden tests failedReproduce this trial: git checkout 2f94510 && PYTHONPATH=src python3 scripts/build_site.py , then open trial/trial_6dc3a11e4c2043e5. Re-running the agent live requires EVAL_PLATFORM_ENABLE_OAUTH_SMOKE=1 and is non-deterministic.
Trial trial_6dc3a11e4c2043e5 · verifier authoritative; classifier explanatory.