Physics engine 2d: extract generic convex-convex collision detection, use for boxes
This commit is contained in:
parent
980d3e856b
commit
3b141aa84d
1 changed files with 103 additions and 48 deletions
|
|
@ -84,6 +84,82 @@ namespace psemek::phys2d
|
||||||
return c;
|
return c;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
template <typename Points1, typename Normals1, typename Points2, typename Normals2>
|
||||||
|
std::optional<collision> convex_collision(Points1 const & points1, Normals1 const & normals1, Points2 const & points2, Normals2 const & normals2)
|
||||||
|
{
|
||||||
|
static constexpr float inf = std::numeric_limits<float>::infinity();
|
||||||
|
|
||||||
|
bool invert = false;
|
||||||
|
geom::point<float, 2> point{0.f, 0.f};
|
||||||
|
float penetration = -inf;
|
||||||
|
geom::vector<float, 2> 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<float, 2> 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<float, 2> 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<collision> shape_collision(ball const & b1, static_state const & s1, ball const & b2, static_state const & s2)
|
std::optional<collision> shape_collision(ball const & b1, static_state const & s1, ball const & b2, static_state const & s2)
|
||||||
{
|
{
|
||||||
auto const d = s2.position - s1.position;
|
auto const d = s2.position - s1.position;
|
||||||
|
|
@ -244,60 +320,39 @@ namespace psemek::phys2d
|
||||||
float w2 = b2.width / 2.f;
|
float w2 = b2.width / 2.f;
|
||||||
float h2 = b2.height / 2.f;
|
float h2 = b2.height / 2.f;
|
||||||
|
|
||||||
float dxx = geom::dot(ex1, ex2);
|
geom::point<float, 2> points1[4] =
|
||||||
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<float, 2> normal{0.f, 0.f};
|
|
||||||
float penetration = -std::numeric_limits<float>::infinity();
|
|
||||||
geom::point<float, 2> point{0.f, 0.f};
|
|
||||||
|
|
||||||
auto side = [&](geom::vector<float, 2> const & n, geom::point<float, 2> const & a, float vx, float vy, geom::point<float, 2> const & c, geom::vector<float, 2> const & ex, geom::vector<float, 2> const & ey, bool invert)
|
|
||||||
{
|
{
|
||||||
float vc = geom::dot((c - a), n);
|
s1.position - ex1 * w1 - ey1 * h1,
|
||||||
|
s1.position + ex1 * w1 - ey1 * h1,
|
||||||
float v = vc - std::abs(vx) - std::abs(vy);
|
s1.position + ex1 * w1 + ey1 * h1,
|
||||||
|
s1.position - ex1 * w1 + ey1 * h1,
|
||||||
if (v > penetration)
|
|
||||||
{
|
|
||||||
penetration = v;
|
|
||||||
point = c - geom::sign(vx) * ex - geom::sign(vy) * ey;
|
|
||||||
normal = invert ? -n : n;
|
|
||||||
}
|
|
||||||
};
|
};
|
||||||
|
|
||||||
// dot(n, c +/- ex +/- ey) = dot(n, c) +/- dot(n, ex) +/- dot(n, ey)
|
geom::vector<float, 2> normals1[4] =
|
||||||
|
{
|
||||||
|
-ey1,
|
||||||
|
ex1,
|
||||||
|
ey1,
|
||||||
|
-ex1
|
||||||
|
};
|
||||||
|
|
||||||
// box2 vs box1 +x
|
geom::point<float, 2> points2[4] =
|
||||||
side(ex1, s1.position + ex1 * w1, dxx * w2, dxy * h2, s2.position, ex2 * w2, ey2 * h2, false);
|
{
|
||||||
if (penetration > 0.f) return std::nullopt;
|
s2.position - ex2 * w2 - ey2 * h2,
|
||||||
// box2 vs box1 -x
|
s2.position + ex2 * w2 - ey2 * h2,
|
||||||
side(-ex1, s1.position - ex1 * w1, -dxx * w2, -dxy * h2, s2.position, ex2 * w2, ey2 * h2, false);
|
s2.position + ex2 * w2 + ey2 * h2,
|
||||||
if (penetration > 0.f) return std::nullopt;
|
s2.position - ex2 * w2 + ey2 * h2,
|
||||||
// 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;
|
|
||||||
|
|
||||||
// box1 vs box2 +x
|
geom::vector<float, 2> normals2[4] =
|
||||||
side(ex2, s2.position + ex2 * w2, dxx * w1, dyx * h1, s1.position, ex1 * w1, ey1 * h1, true);
|
{
|
||||||
if (penetration > 0.f) return std::nullopt;
|
-ey2,
|
||||||
// box1 vs box2 -x
|
ex2,
|
||||||
side(-ex2, s2.position - ex2 * w2, -dxx * w1, -dyx * h1, s1.position, ex1 * w1, ey1 * h1, true);
|
ey2,
|
||||||
if (penetration > 0.f) return std::nullopt;
|
-ex2
|
||||||
// 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;
|
|
||||||
|
|
||||||
return collision{point, -normal * penetration};
|
return convex_collision(points1, normals1, points2, normals2);
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Add table
Reference in a new issue