Collision detection and physics in C++

world.hpp

// The world of the collision examples: 10,000 circles that move and bounce, every-pair testing, and a spatial
// hash grid. Shared by broadphase.cpp and grid_tuning.cpp.
#pragma once

#include <algorithm>
#include <cstdint>
#include <unordered_map>
#include <utility>
#include <vector>

#include "shapes.hpp"

namespace sim {

constexpr int count = 10'000;
constexpr float world = 2000.0f;  // a square world, 2000 x 2000 pixels
constexpr float max_r = 6.0f;
constexpr int frames = 10;

// splitmix64: the same numbers with every compiler and standard library (std:: distributions differ between them).
struct Random {
    std::uint64_t state;
    float next() {  // in [0, 1)
        std::uint64_t z = (state += 0x9e3779b97f4a7c15u);
        z = (z ^ (z >> 30)) * 0xbf58476d1ce4e5b9u;
        z = (z ^ (z >> 27)) * 0x94d049bb133111ebu;
        return float((z ^ (z >> 31)) >> 40) / float(1u << 24);
    }
};

struct Body {
    Circle shape;
    Vec2 velocity;  // pixels per second
};

std::vector<Body> make_world(std::uint64_t seed) {
    Random random{seed};
    std::vector<Body> bodies(count);
    for (Body& b : bodies) {
        b.shape = {{random.next() * world, random.next() * world}, 2.0f + random.next() * (max_r - 2.0f)};
        b.velocity = {random.next() * 200.0f - 100.0f, random.next() * 200.0f - 100.0f};
    }
    return bodies;
}

// Moves every body for one step and bounces it off the edges of the world.
void move(std::vector<Body>& bodies, float dt) {
    for (Body& b : bodies) {
        Vec2& p = b.shape.centre;
        p.x += b.velocity.x * dt;
        p.y += b.velocity.y * dt;
        if (p.x < 0.0f || p.x > world) {
            b.velocity.x = -b.velocity.x;
            p.x = std::clamp(p.x, 0.0f, world);
        }
        if (p.y < 0.0f || p.y > world) {
            b.velocity.y = -b.velocity.y;
            p.y = std::clamp(p.y, 0.0f, world);
        }
    }
}

using Pairs = std::vector<std::pair<int, int>>;

// Every pair: n (n - 1) / 2 tests.
Pairs every_pair(const std::vector<Body>& bodies, long long& tests) {
    Pairs found;
    const int n = int(bodies.size());
    for (int i = 0; i < n; ++i)
        for (int j = i + 1; j < n; ++j) {
            ++tests;
            if (overlaps(bodies[i].shape, bodies[j].shape)) found.emplace_back(i, j);
        }
    return found;
}

// A spatial hash grid: square cells, found by hashing the cell's coordinates. A cell at least as wide as the largest
// diameter means that two circles that touch are in the same cell or in neighbouring ones.
struct Grid {
    float cell;
    std::unordered_map<std::uint64_t, std::vector<int>> cells;

    static std::uint64_t key(int cx, int cy) { return std::uint64_t(std::uint32_t(cx)) << 32 | std::uint32_t(cy); }
    int index(float v) const { return int(v / cell); }

    void rebuild(const std::vector<Body>& bodies) {
        for (auto& entry : cells) entry.second.clear();  // keep each cell's memory for the next frame
        for (int i = 0; i < int(bodies.size()); ++i) {
            const Vec2 p = bodies[i].shape.centre;
            cells[key(index(p.x), index(p.y))].push_back(i);
        }
    }

    Pairs pairs(const std::vector<Body>& bodies, long long& tests) const {
        Pairs found;
        for (int i = 0; i < int(bodies.size()); ++i) {
            const int cx = index(bodies[i].shape.centre.x);
            const int cy = index(bodies[i].shape.centre.y);
            for (int dy = -1; dy <= 1; ++dy)
                for (int dx = -1; dx <= 1; ++dx) {
                    const auto it = cells.find(key(cx + dx, cy + dy));
                    if (it == cells.end()) continue;
                    for (int j : it->second) {
                        if (j <= i) continue;  // each pair once
                        ++tests;
                        if (overlaps(bodies[i].shape, bodies[j].shape)) found.emplace_back(i, j);
                    }
                }
        }
        std::sort(found.begin(), found.end());
        return found;
    }
};

}  // namespace sim

broadphase.cpp

// 10,000 moving circles, two ways to find the ones that touch: test every pair, or test only the circles in
// neighbouring cells of a spatial hash grid (world.hpp). Both must find the same pairs in every frame.
#include <cstdio>
#include <ctime>

#include "world.hpp"

using namespace sim;

// Processor time since `start`, in milliseconds: other programs running on the same computer disturb it far less
// than a stopwatch would.
double ms_since(std::clock_t start) { return 1000.0 * double(std::clock() - start) / CLOCKS_PER_SEC; }

int main() {
    std::vector<Body> bodies = make_world(42);
    Grid grid{2 * max_r, {}};
    long long brute_tests = 0, grid_tests = 0, pairs = 0;
    double brute_ms = 0, grid_ms = 0;
    bool same = true;
    for (int frame = 0; frame < frames; ++frame) {
        move(bodies, 1.0f / 60);
        std::clock_t t0 = std::clock();
        const Pairs a = every_pair(bodies, brute_tests);
        brute_ms += ms_since(t0);
        t0 = std::clock();
        grid.rebuild(bodies);
        const Pairs b = grid.pairs(bodies, grid_tests);
        grid_ms += ms_since(t0);
        same = same && a == b;
        pairs += static_cast<long long>(a.size());
    }
    std::printf("%d circles, radius 2 to 6, in a %.0f x %.0f world, %d frames\n", count, world, world, frames);
    std::printf("every pair:   %9lld tests a frame, %lld touching pairs a frame, %6.2f ms a frame\n", brute_tests / frames,
                pairs / frames, brute_ms / frames);
    std::printf("grid of 12:   %9lld tests a frame, %lld touching pairs a frame, %6.2f ms a frame\n", grid_tests / frames,
                pairs / frames, grid_ms / frames);
    std::printf("same pairs in every frame: %s\n", same ? "yes" : "NO");
}

C++ header · exercise/near_box.hpp

// The exercise: a circle-against-box test, and a grid that finds the circles near a box.
#pragma once

#include <cmath>
#include <cstdint>
#include <unordered_map>
#include <vector>

#include "shapes.hpp"

// TODO 1: true when the circle touches or overlaps the box (a circle inside the box counts).
constexpr bool overlaps(const Circle& c, const Box& b) {
    (void)c;
    (void)b;
    return false;
}

// A grid of square cells. Each circle is stored in the one cell that holds its centre.
struct CircleGrid {
    float cell;
    float max_r;  // the largest radius of any circle stored
    std::unordered_map<std::uint64_t, std::vector<int>> cells;

    static std::uint64_t key(int cx, int cy) { return std::uint64_t(std::uint32_t(cx)) << 32 | std::uint32_t(cy); }
    int index(float v) const { return int(std::floor(v / cell)); }

    void insert(int id, const Circle& c) { cells[key(index(c.centre.x), index(c.centre.y))].push_back(id); }

    // TODO 2: the ids of every circle that could touch `area`, each id once, in increasing order. Remember that a
    // circle can touch the box while its centre lies in a cell outside it.
    std::vector<int> near(const Box& area) const {
        (void)area;
        return {};
    }
};

// The circles that touch `area`: candidates from the grid, then the exact test.
inline std::vector<int> touching(const std::vector<Circle>& circles, const CircleGrid& grid, const Box& area) {
    std::vector<int> found;
    for (int id : grid.near(area))
        if (overlaps(circles[std::size_t(id)], area)) found.push_back(id);
    return found;
}

C++ · exercise/near_box_tests.cpp

// Sample tests of the exercise. All of them pass once both TODOs of near_box.hpp are done.
#include <algorithm>
#include <cstdint>
#include <cstdio>
#include <string_view>
#include <vector>

#include "near_box.hpp"

namespace {
int checks = 0;
int failures = 0;
void check(bool ok, std::string_view what) {
    ++checks;
    if (!ok) {
        ++failures;
        std::printf("FAIL %.*s\n", int(what.size()), what.data());
    }
}
// A small generator with the same numbers everywhere: the next value in [0, 1).
float next(std::uint32_t& s) {
    s = s * 1664525u + 1013904223u;
    return float(s >> 8) / float(1u << 24);
}
}  // namespace

int main() {
    const Box box{{100, 0}, {110, 200}};
    check(overlaps(Circle{{115, 50}, 5}, box), "a circle that touches the box's right side");
    check(overlaps(Circle{{105, 100}, 2}, box), "a circle inside the box");
    check(!overlaps(Circle{{113.5f, -3.5f}, 4.5f}, box), "a circle by the corner, inside the box's reach on both axes, but not touching");
    check(overlaps(Circle{{113, -3}, 4.5f}, box), "a circle that just reaches the corner");

    // 300 circles in a 400 x 400 square, and 20 boxes. Testing every circle against every box with a correct
    // overlaps() finds 36 touching pairs of a circle and a box, whose circle ids add up to 5395.
    std::uint32_t seed = 7;
    std::vector<Circle> circles;
    CircleGrid grid{16, 8, {}};
    for (int i = 0; i < 300; ++i) {
        circles.push_back({{next(seed) * 400, next(seed) * 400}, 1 + next(seed) * 7});
        grid.insert(i, circles.back());
    }
    int found = 0;
    long long id_sum = 0;
    bool once = true;
    for (int k = 0; k < 20; ++k) {
        const float x = next(seed) * 360, y = next(seed) * 360;
        const Box area{{x, y}, {x + 5 + next(seed) * 35, y + 5 + next(seed) * 35}};
        const std::vector<int> candidates = grid.near(area);
        once = once && std::is_sorted(candidates.begin(), candidates.end()) &&
               std::adjacent_find(candidates.begin(), candidates.end()) == candidates.end();
        for (int id : touching(circles, grid, area)) {
            ++found;
            id_sum += id;
        }
    }
    check(found == 36 && id_sum == 5395, "the grid finds all 36 contacts between a circle and one of the 20 boxes");
    check(once, "near() lists each circle once, in increasing order");

    CircleGrid edge{16, 8, {}};
    const std::vector<Circle> one{{{38, 10}, 7}};  // its centre is in cell (2, 0); it reaches x = 31
    edge.insert(0, one[0]);
    check(touching(one, edge, Box{{20, 0}, {31, 15}}) == std::vector<int>{0}, "a circle whose centre lies in a cell outside the box");

    std::printf("%d of %d checks passed\n", checks - failures, checks);
    return failures == 0 ? 0 : 1;
}