Ich bin jetzt hier zum Schluss gekommen. Claude hat mir gut weitergeholfen und sowohl die Performance als auch das Tunneln und Überlappen wurde ansprechend gelöst.
#pragma once
#include <zp/Engine/SDLPixelEngine.h>
#include "DiscTexture.h"
namespace zp::physics
{
inline constexpr f32 EPSILON = 0.0001f;
inline constexpr fvec2 GRAVITY = fvec2(0.0f, 9.81f) * 25.0f;
inline constexpr ui32 ITERATIONS = 2;
inline constexpr ui32 SUBSTEPS = 6;
inline constexpr f32 MAX_STEP_DISPLACEMENT = 3.0f;
inline constexpr f32 MAX_CORRECTION = 2.0f;
// Broadphase: pairs closer than (radiusSum + PAIR_MARGIN) become candidates.
// The margin covers movement caused by the position solver within one substep,
// since the pair list is built once per substep.
inline constexpr f32 PAIR_MARGIN = MAX_CORRECTION * static_cast<f32>(ITERATIONS);
inline constexpr f32 WALL_RESTITUTION = 0.25f;
inline constexpr f32 CIRCLE_RESTITUTION = 0.25f;
inline constexpr f32 CIRCLE_MASS = 10.0f;
inline constexpr f32 CIRCLE_DAMPING = 0.99f; // between [0, 1]
}
namespace zp
{
struct Disc
{
fvec2 Pos, Vel;
f32 Radius = 1;
struct {
RGB Fill = WHITE;
RGB Frame = DARK_GREY;
} Color;
};
struct Wall
{
fvec2 Start, End;
f32 Radius = 1;
RGB Color = BLACK;
};
}
namespace zp
{
using namespace physics;
class CirclePhysics_2
{
public:
CirclePhysics_2() = default;
static_assert(CIRCLE_MASS > 0.0f);
private:
using Circle = std::pair<Disc, DiscTexture>;
std::vector<Circle> m_CircleList;
std::vector<Wall> m_WallList;
// Broadphase buffers (members, so the memory is reused every substep)
std::vector<std::pair<ui32, ui32>> m_Pairs; // candidate pairs, always first < second
std::vector<ui32> m_CellStart; // CSR-style: items of cell c are m_CellItems[m_CellStart[c] .. m_CellStart[c+1])
std::vector<ui32> m_CellItems;
std::vector<ui32> m_CellOf;
std::vector<ui32> m_CellCursor;
std::vector<fvec2> m_PrevPos;
std::vector<f32> m_PreSolveSpeed;
public:
constexpr std::vector<Circle>& circles() { return m_CircleList; }
constexpr const std::vector<Wall>& walls() const { return m_WallList; }
public:
inline void createCircle(SDLPixelEngine& e, const fvec2& pos, const fvec2& vel, const f32 radius, const RGB& fill, const RGB& frame)
{
if (!(radius > 0.0f))
return;
Circle circle;
circle.first.Pos = pos;
circle.first.Vel = vel;
circle.first.Radius = radius;
circle.first.Color = { fill, frame };
circle.second = createCircleTexture(e, circle.first);
m_CircleList.push_back(circle);
}
inline DiscTexture createCircleTexture(SDLPixelEngine& e, const Disc& disc)
{
DiscTexture tex{ e.getRenderer(), disc.Radius, disc.Color.Fill, disc.Color.Frame };
return tex;
}
inline void createWall(const fvec2& start, const fvec2& end, const f32 radius, const RGB& color)
{
if ((end - start).mag2() < EPSILON)
return;
if (!(radius > 0.0f))
return;
Wall wall;
wall.Start = start;
wall.End = end;
wall.Radius = radius;
wall.Color = color;
m_WallList.push_back(wall);
}
inline void reset()
{
m_CircleList.clear();
m_WallList.clear();
m_Pairs.clear();
}
inline void drawCircle(SDLPixelEngine& e, Circle& circle) const
{
// draw as texture
circle.second.position() = circle.first.Pos - circle.second.center();
circle.second.render();
if (circle.first.Radius > 2)
drawVelocityLine(e, circle, circle.first.Color.Frame);
}
constexpr void drawVelocityLine(SDLPixelEngine& e, const Circle& circle, const RGB& color) const
{
const f32 speedSq = circle.first.Vel.mag2();
if (speedSq > EPSILON * EPSILON)
{
const fvec2 dir = circle.first.Vel / std::sqrt(speedSq);
e.draw().line(circle.first.Pos, circle.first.Pos + dir * (circle.first.Radius - 1), color);
}
else
e.draw().point(circle.first.Pos, color);
}
constexpr void drawWalls(SDLPixelEngine& e) const
{
for (const auto& wall : m_WallList)
{
f32 nx = -(wall.End.y - wall.Start.y);
f32 ny = (wall.End.x - wall.Start.x);
const f32 d = std::sqrt(nx * nx + ny * ny);
nx /= d;
ny /= d;
const fvec2 v0{ wall.Start.x + nx * wall.Radius, wall.Start.y + ny * wall.Radius };
const fvec2 v1{ wall.End.x + nx * wall.Radius, wall.End.y + ny * wall.Radius };
const fvec2 v2{ wall.End.x - nx * wall.Radius, wall.End.y - ny * wall.Radius };
const fvec2 v3{ wall.Start.x - nx * wall.Radius, wall.Start.y - ny * wall.Radius };
// draw wall
Box<f32> box = { v0, v1, v2, v3 };
box.color() = wall.Color;
e.draw().fillBox(box);
// draw wall edges
if (wall.Radius > 1)
{
e.draw().fillCircle(wall.Start, wall.Radius, wall.Color);
e.draw().fillCircle(wall.End, wall.Radius, wall.Color);
}
else
{
e.draw().point(wall.Start, wall.Color);
e.draw().point(wall.End, wall.Color);
}
}
}
inline void update(SDLPixelEngine& e)
{
m_PrevPos.resize(m_CircleList.size());
for (size_t k = 0; k < m_CircleList.size(); ++k)
m_PrevPos[k] = m_CircleList[k].first.Pos;
const f32 frameDt = std::min(e.deltaTime(), 1.0f / 30.0f);
const f32 dt = frameDt / static_cast<f32>(SUBSTEPS); // time per substep
for (ui32 step = 0; step < SUBSTEPS; ++step)
{
integrate(dt);
buildPairs();
solveCircleCollisions(dt);
}
removeOffscreen(e);
}
private:
/* --------- */
/* PHYSICS */
/* --------- */
inline void removeOffscreen(SDLPixelEngine& e)
{
const auto screenRect = e.screenRect<f32>();
std::erase_if(m_CircleList, [&](const Circle& c) {
return !utils::isPointInsideBox(screenRect, c.first.Pos, -c.first.Radius);
});
}
inline static std::optional<fvec2> wallContactNormal(const fvec2& delta, const f32 distance, const fvec2& wallVector)
{
// Standard case: The circle center does not lie on the wall segment.
if (distance > EPSILON)
return delta / distance;
// Fallback: The midpoint lies (almost) exactly on the wall line;
// in this case, the perpendicular to the wall is the only sensible direction.
const fvec2 perpendicular(-wallVector.y, wallVector.x);
const f32 length = perpendicular.mag();
if (length < EPSILON)
return std::nullopt;
return perpendicular / length;
}
/* ------------ */
/* BROADPHASE */
/* ------------ */
// Builds m_Pairs: all circle pairs that might collide during this substep.
// Uniform grid + counting sort, O(n) build, then each cell is only
// compared with itself and 4 of its 8 neighbours (so every pair appears once).
inline void buildPairs()
{
m_Pairs.clear();
const size_t n = m_CircleList.size();
if (n < 2)
return;
// --- bounds and largest radius ---
f32 minX = m_CircleList[0].first.Pos.x, maxX = minX;
f32 minY = m_CircleList[0].first.Pos.y, maxY = minY;
f32 maxRadius = 0.0f;
for (const auto& c : m_CircleList)
{
const fvec2& p = c.first.Pos;
minX = std::min(minX, p.x);
maxX = std::max(maxX, p.x);
minY = std::min(minY, p.y);
maxY = std::max(maxY, p.y);
maxRadius = std::max(maxRadius, c.first.Radius);
}
// --- cell size: any colliding pair must lie in neighbouring cells ---
f32 cellSize = 2.0f * maxRadius + PAIR_MARGIN;
const f32 extentX = maxX - minX;
const f32 extentY = maxY - minY;
const size_t maxCells = std::max<size_t>(1024, 4 * n);
size_t w, h;
for (;;)
{
w = static_cast<size_t>(extentX / cellSize) + 1;
h = static_cast<size_t>(extentY / cellSize) + 1;
if (w * h <= maxCells)
break;
cellSize *= 2.0f; // keep the grid small if circles are spread far apart
}
const f32 invCell = 1.0f / cellSize;
const size_t cellCount = w * h;
// --- counting sort of circles into cells ---
m_CellStart.assign(cellCount + 1, 0);
m_CellOf.resize(n);
m_CellItems.resize(n);
for (size_t i = 0; i < n; ++i)
{
const fvec2& p = m_CircleList[i].first.Pos;
const size_t cx = std::min(w - 1, static_cast<size_t>((p.x - minX) * invCell));
const size_t cy = std::min(h - 1, static_cast<size_t>((p.y - minY) * invCell));
const ui32 cell = static_cast<ui32>(cy * w + cx);
m_CellOf[i] = cell;
++m_CellStart[cell + 1];
}
for (size_t c = 0; c < cellCount; ++c)
m_CellStart[c + 1] += m_CellStart[c];
m_CellCursor.assign(m_CellStart.begin(), m_CellStart.begin() + cellCount);
for (size_t i = 0; i < n; ++i)
m_CellItems[m_CellCursor[m_CellOf[i]]++] = static_cast<ui32>(i);
// --- generate candidate pairs ---
auto tryPair = [&](ui32 i, ui32 j)
{
if (i > j)
std::swap(i, j);
const Disc& a = m_CircleList[i].first;
const Disc& b = m_CircleList[j].first;
const f32 reach = a.Radius + b.Radius + PAIR_MARGIN;
if ((b.Pos - a.Pos).mag2() < reach * reach)
m_Pairs.emplace_back(i, j);
};
// only "forward" neighbours, so each cell pair is visited exactly once
static constexpr int OFFSETS[4][2] = { {1, 0}, {-1, 1}, {0, 1}, {1, 1} };
for (size_t cy = 0; cy < h; ++cy)
{
for (size_t cx = 0; cx < w; ++cx)
{
const size_t cell = cy * w + cx;
const ui32 begin = m_CellStart[cell];
const ui32 end = m_CellStart[cell + 1];
if (begin == end)
continue;
for (ui32 a = begin; a < end; ++a)
{
const ui32 i = m_CellItems[a];
// same cell
for (ui32 b = a + 1; b < end; ++b)
tryPair(i, m_CellItems[b]);
// forward neighbour cells
for (const auto& off : OFFSETS)
{
const int nx = static_cast<int>(cx) + off[0];
const int ny = static_cast<int>(cy) + off[1];
if (nx < 0 || ny < 0 || nx >= static_cast<int>(w) || ny >= static_cast<int>(h))
continue;
const size_t neighbour = static_cast<size_t>(ny) * w + static_cast<size_t>(nx);
for (ui32 b = m_CellStart[neighbour]; b < m_CellStart[neighbour + 1]; ++b)
tryPair(i, m_CellItems[b]);
}
}
}
}
}
inline void integrate(const f32 dt)
{
const f32 frameDamping = std::pow(CIRCLE_DAMPING, dt);
const f32 maxSpeed = MAX_STEP_DISPLACEMENT / dt;
for (auto& circle : m_CircleList)
{
circle.first.Vel += GRAVITY * dt;
circle.first.Vel *= frameDamping;
const f32 speed = circle.first.Vel.mag();
if (speed > maxSpeed)
circle.first.Vel *= maxSpeed / speed;
circle.first.Pos += circle.first.Vel * dt;
}
}
inline void solveCircleCollisions(const f32 dt)
{
constexpr f32 SLOP = 0.01f;
constexpr f32 PERCENT = 1.0f;
constexpr f32 InvMass = 1 / CIRCLE_MASS;
constexpr f32 INV_MASS_SUM = InvMass + InvMass;
const f32 restThreshold = 2.0f * GRAVITY.mag() * dt;
m_PreSolveSpeed.resize(m_CircleList.size());
for (size_t k = 0; k < m_CircleList.size(); ++k)
m_PreSolveSpeed[k] = m_CircleList[k].first.Vel.mag();
// -------------------------
// Position solver
// -------------------------
for (ui32 iteration = 0; iteration < ITERATIONS; ++iteration)
{
/* Circle vs circle (candidate pairs from the broadphase) */
for (const auto& [i, j] : m_Pairs)
{
Circle& a = m_CircleList[i];
Circle& b = m_CircleList[j];
const fvec2 delta = b.first.Pos - a.first.Pos;
const f32 radiusSum = a.first.Radius + b.first.Radius;
const f32 distanceSq = delta.mag2();
if (distanceSq >= radiusSum * radiusSum)
continue;
f32 distance = std::sqrt(distanceSq);
fvec2 normal;
if (distance < EPSILON)
{
// deterministic, but different for each pair
const f32 theta = static_cast<f32>((i * 31 + j * 17) % 360) * math::TAU<f32> / 360.0f;
normal = math::toCartesian<f32>(1.0f, theta);
distance = 0.0f;
}
else
normal = delta / distance;
const f32 penetration = radiusSum - distance;
const f32 correction =
std::min(std::max(penetration - SLOP, 0.0f), MAX_CORRECTION) * PERCENT / INV_MASS_SUM;
a.first.Pos -= normal * correction * InvMass;
b.first.Pos += normal * correction * InvMass;
}
/* Circle vs wall */
for (size_t i = 0; i < m_CircleList.size(); ++i)
{
Circle& circle = m_CircleList[i];
for (const auto& wall : m_WallList)
{
const fvec2 wallVector = wall.End - wall.Start;
const f32 wallLengthSq = wallVector.mag2();
if (wallLengthSq < EPSILON)
continue;
const fvec2 toCircle = circle.first.Pos - wall.Start;
f32 t = wallVector.dot(toCircle) / wallLengthSq;
t = std::max(0.0f, std::min(1.0f, t));
const fvec2 closestPoint = wall.Start + wallVector * t;
const fvec2 delta = circle.first.Pos - closestPoint;
const f32 distanceSq = delta.mag2();
const f32 collisionRadius = circle.first.Radius + wall.Radius;
if (distanceSq >= collisionRadius * collisionRadius)
continue;
const f32 distance = std::sqrt(distanceSq);
const auto normal = wallContactNormal(delta, distance, wallVector);
if (!normal)
continue;
const f32 penetration = collisionRadius - distance;
circle.first.Pos += *normal * std::max(penetration - SLOP, 0.0f);
}
}
}
// -------------------------
// Velocity Solver
// -------------------------
for (size_t k = 0; k < m_CircleList.size(); ++k)
{
fvec2 v = (m_CircleList[k].first.Pos - m_PrevPos[k]) / dt;
const f32 s = v.mag();
if (s > m_PreSolveSpeed[k] && s > EPSILON)
v *= m_PreSolveSpeed[k] / s; // verhindert Herausschleudern
m_CircleList[k].first.Vel = v;
}
for (ui32 iteration = 0; iteration < ITERATIONS; ++iteration)
{
/* Circle vs circle (candidate pairs from the broadphase) */
for (const auto& [i, j] : m_Pairs)
{
Circle& a = m_CircleList[i];
Circle& b = m_CircleList[j];
const fvec2 delta = b.first.Pos - a.first.Pos;
const f32 distanceSq = delta.mag2();
const f32 radiusSum = a.first.Radius + b.first.Radius;
// Position solver should already have
// separated these, but allow a small
// tolerance.
if (distanceSq > radiusSum * radiusSum)
continue;
const f32 distance = std::sqrt(distanceSq);
if (distance < EPSILON)
continue;
const fvec2 normal = delta / distance;
const fvec2 relativeVelocity = b.first.Vel - a.first.Vel;
const f32 velocityAlongNormal = relativeVelocity.dot(normal);
// Moving apart.
if (velocityAlongNormal >= 0.0f)
continue;
const f32 e = (-velocityAlongNormal < restThreshold) ? 0.0f : CIRCLE_RESTITUTION;
const f32 impulseMagnitude = -(1 + e) * velocityAlongNormal / INV_MASS_SUM;
const fvec2 impulse = normal * impulseMagnitude;
a.first.Vel -= impulse * InvMass;
b.first.Vel += impulse * InvMass;
}
/* Circle vs wall */
for (size_t i = 0; i < m_CircleList.size(); ++i)
{
Circle& circle = m_CircleList[i];
for (const auto& wall : m_WallList)
{
const fvec2 wallVector = wall.End - wall.Start;
const f32 wallLengthSq = wallVector.mag2();
if (wallLengthSq < EPSILON)
continue;
const fvec2 toCircle = circle.first.Pos - wall.Start;
f32 t = wallVector.dot(toCircle) / wallLengthSq;
t = std::max(0.0f, std::min(1.0f, t));
const fvec2 closestPoint = wall.Start + wallVector * t;
const fvec2 delta = circle.first.Pos - closestPoint;
const f32 distanceSq = delta.mag2();
const f32 collisionRadius = circle.first.Radius + wall.Radius;
if (distanceSq > collisionRadius * collisionRadius)
continue;
const f32 distance = std::sqrt(distanceSq);
const auto normal = wallContactNormal(delta, distance, wallVector);
if (!normal)
continue;
const f32 velocityAlongNormal = circle.first.Vel.dot(*normal);
if (velocityAlongNormal >= 0.0f)
continue; // moving apart
const f32 e = (-velocityAlongNormal < restThreshold) ? 0.0f : WALL_RESTITUTION;
circle.first.Vel -= *normal * ((1 + e) * velocityAlongNormal);
}
}
}
}
};
}
Ich habe noch das Rendern verbessert, in dem ich eine SDLTexture für den Kreis erstellt habe, die nur verschoben und dann kopiert wird (ich glaub, das macht dann sogar die GPU). Das ist deutlich schneller jeden Kreis einzeln zeichnen.
#pragma once
#include <zp/Graphics/SDLPixelTextureObject.h>
namespace zp
{
class DiscTexture : public SDLPixelTextureObject
{
public:
DiscTexture() = default;
DiscTexture(const std::shared_ptr<SDLRenderer>& renderer, const f32 radius, const RGB& fill, const RGB& frame) :
SDLPixelTextureObject{ renderer }
{
init(radius, fill, frame);
}
private:
f32 m_Radius = 0;
RGB m_Fill, m_Frame;
public:
constexpr f32 radius() const { return m_Radius; }
inline void init(const f32 radius, const RGB& fill, const RGB& frame)
{
m_Radius = radius;
m_Fill = fill;
m_Frame = frame;
computeBounds();
this->initPixelArray();
this->updateTexture();
}
private:
virtual void computeBounds() override
{
this->m_Bounds.x = math::toInt<i32>(m_Radius * 2) + 1;
this->m_Bounds.y = math::toInt<i32>(m_Radius * 2) + 1;
}
virtual void renderToPixelArray() override
{
const i32 radius = math::toInt<i32>(m_Radius);
const i32 x = math::toInt<i32>(m_Radius);
const i32 y = math::toInt<i32>(m_Radius);
this->renderFillCircle(x, y, radius, m_Fill);
this->renderCircle(x, y, radius, m_Frame);
}
};
}
Am Labyrinth muss man noch arbeiten. Manchmal kann man das Labyrinth gar nicht lösen, wahrscheinlich, weil ich Seitenwände einsetze. Aber ansonsten war das doch ganz interessant, auch das Arbeiten mit KI. Ich denke, ich habe eine schöne Grundlage für weitere Pysiksimulationen