diff --git a/libs/phys/source/engine_2d.cpp b/libs/phys/source/engine_2d.cpp index 6733bc44..470deb5b 100644 --- a/libs/phys/source/engine_2d.cpp +++ b/libs/phys/source/engine_2d.cpp @@ -84,6 +84,82 @@ namespace psemek::phys2d return c; } + template + std::optional convex_collision(Points1 const & points1, Normals1 const & normals1, Points2 const & points2, Normals2 const & normals2) + { + static constexpr float inf = std::numeric_limits::infinity(); + + bool invert = false; + geom::point point{0.f, 0.f}; + float penetration = -inf; + geom::vector normal{0.f, 0.f}; + + // Normals of 1st body against points of 2nd body + { + auto p = std::begin(points1); + auto n = std::begin(normals1); + + for (; n != std::end(normals1); ++p, ++n) + { + float min = inf; + geom::point minp{0.f, 0.f}; + for (auto const & p2 : points2) + { + float v = geom::dot(p2 - *p, *n); + if (v < min) + { + min = v; + minp = p2; + } + } + + if (min > 0.f) + return std::nullopt; + + if (min > penetration) + { + point = minp; + penetration = min; + normal = *n; + } + } + } + + // Normals of 2nd body against points of 1st body + { + auto p = std::begin(points2); + auto n = std::begin(normals2); + + for (; n != std::end(normals2); ++p, ++n) + { + float min = inf; + geom::point minp{0.f, 0.f}; + for (auto const & p1 : points1) + { + float v = geom::dot(p1 - *p, *n); + if (v < min) + { + min = v; + minp = p1; + } + } + + if (min > 0.f) + return std::nullopt; + + if (min > penetration) + { + invert = true; + point = minp; + penetration = min; + normal = *n; + } + } + } + + return collision{point - normal * penetration / 2.f, (invert ? 1.f : -1.f) * normal * penetration}; + } + std::optional shape_collision(ball const & b1, static_state const & s1, ball const & b2, static_state const & s2) { auto const d = s2.position - s1.position; @@ -244,60 +320,39 @@ namespace psemek::phys2d float w2 = b2.width / 2.f; float h2 = b2.height / 2.f; - float dxx = geom::dot(ex1, ex2); - float dxy = geom::dot(ex1, ey2); - float dyx = geom::dot(ey1, ex2); - float dyy = geom::dot(ey1, ey2); - - // SAT collision detector, hand-optimized for boxes - - geom::vector normal{0.f, 0.f}; - float penetration = -std::numeric_limits::infinity(); - geom::point point{0.f, 0.f}; - - auto side = [&](geom::vector const & n, geom::point const & a, float vx, float vy, geom::point const & c, geom::vector const & ex, geom::vector const & ey, bool invert) + geom::point points1[4] = { - float vc = geom::dot((c - a), n); - - float v = vc - std::abs(vx) - std::abs(vy); - - if (v > penetration) - { - penetration = v; - point = c - geom::sign(vx) * ex - geom::sign(vy) * ey; - normal = invert ? -n : n; - } + s1.position - ex1 * w1 - ey1 * h1, + s1.position + ex1 * w1 - ey1 * h1, + s1.position + ex1 * w1 + ey1 * h1, + s1.position - ex1 * w1 + ey1 * h1, }; - // dot(n, c +/- ex +/- ey) = dot(n, c) +/- dot(n, ex) +/- dot(n, ey) + geom::vector normals1[4] = + { + -ey1, + ex1, + ey1, + -ex1 + }; - // box2 vs box1 +x - side(ex1, s1.position + ex1 * w1, dxx * w2, dxy * h2, s2.position, ex2 * w2, ey2 * h2, false); - if (penetration > 0.f) return std::nullopt; - // box2 vs box1 -x - side(-ex1, s1.position - ex1 * w1, -dxx * w2, -dxy * h2, s2.position, ex2 * w2, ey2 * h2, false); - if (penetration > 0.f) return std::nullopt; - // box2 vs box1 +y - side(ey1, s1.position + ey1 * h1, dyx * w2, dyy * h2, s2.position, ex2 * w2, ey2 * h2, false); - if (penetration > 0.f) return std::nullopt; - // box2 vs box1 -y - side(-ey1, s1.position - ey1 * h1, -dyx * w2, -dyy * h2, s2.position, ex2 * w2, ey2 * h2, false); - if (penetration > 0.f) return std::nullopt; + geom::point points2[4] = + { + s2.position - ex2 * w2 - ey2 * h2, + s2.position + ex2 * w2 - ey2 * h2, + s2.position + ex2 * w2 + ey2 * h2, + s2.position - ex2 * w2 + ey2 * h2, + }; - // box1 vs box2 +x - side(ex2, s2.position + ex2 * w2, dxx * w1, dyx * h1, s1.position, ex1 * w1, ey1 * h1, true); - if (penetration > 0.f) return std::nullopt; - // box1 vs box2 -x - side(-ex2, s2.position - ex2 * w2, -dxx * w1, -dyx * h1, s1.position, ex1 * w1, ey1 * h1, true); - if (penetration > 0.f) return std::nullopt; - // box1 vs box2 +y - side(ey2, s2.position + ey2 * h2, dxy * w1, dyy * h1, s1.position, ex1 * w1, ey1 * h1, true); - if (penetration > 0.f) return std::nullopt; - // box1 vs box2 -y - side(-ey2, s2.position - ey2 * h2, -dxy * w1, -dyy * h1, s1.position, ex1 * w1, ey1 * h1, true); - if (penetration > 0.f) return std::nullopt; + geom::vector normals2[4] = + { + -ey2, + ex2, + ey2, + -ex2 + }; - return collision{point, -normal * penetration}; + return convex_collision(points1, normals1, points2, normals2); } }