1005 lines
25 KiB
C++
1005 lines
25 KiB
C++
#include <psemek/phys/engine_2d.hpp>
|
|
|
|
#include <psemek/util/heterogeneous_container.hpp>
|
|
|
|
#include <psemek/geom/constants.hpp>
|
|
#include <psemek/geom/matrix.hpp>
|
|
#include <psemek/geom/gauss.hpp>
|
|
#include <psemek/geom/rotation.hpp>
|
|
#include <psemek/geom/math.hpp>
|
|
|
|
#include <psemek/util/profiler.hpp>
|
|
|
|
#include <psemek/log/log.hpp>
|
|
|
|
#include <optional>
|
|
#include <set>
|
|
|
|
namespace psemek::phys2d
|
|
{
|
|
|
|
namespace
|
|
{
|
|
|
|
float shape_area(ball const & b)
|
|
{
|
|
return geom::pi * b.radius * b.radius;
|
|
}
|
|
|
|
float shape_inertia(ball const & b)
|
|
{
|
|
return shape_area(b) * b.radius * b.radius / 2.f;
|
|
}
|
|
|
|
float shape_area(half_space const &)
|
|
{
|
|
return std::numeric_limits<float>::infinity();
|
|
}
|
|
|
|
float shape_inertia(half_space const &)
|
|
{
|
|
return std::numeric_limits<float>::infinity();
|
|
}
|
|
|
|
float shape_area(box const & b)
|
|
{
|
|
return b.width * b.height;
|
|
}
|
|
|
|
float shape_inertia(box const & b)
|
|
{
|
|
return shape_area(b) * (b.width * b.width + b.height * b.height) / 12.f;
|
|
}
|
|
|
|
template <typename Shape>
|
|
struct wrapped_shape
|
|
{
|
|
Shape shape;
|
|
float area;
|
|
float inertia;
|
|
|
|
wrapped_shape(Shape const & shape)
|
|
: shape{shape}
|
|
, area(shape_area(shape))
|
|
, inertia(shape_inertia(shape))
|
|
{}
|
|
};
|
|
|
|
template <typename Shape>
|
|
wrapped_shape<Shape> wrap(Shape const & shape)
|
|
{
|
|
return {shape};
|
|
}
|
|
|
|
// penetration points from object 1 to object 2
|
|
struct collision
|
|
{
|
|
geom::point<float, 2> position;
|
|
geom::vector<float, 2> penetration;
|
|
};
|
|
|
|
std::optional<collision> invert(std::optional<collision> c)
|
|
{
|
|
if (c)
|
|
c->penetration *= -1.f;
|
|
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)
|
|
{
|
|
auto const d = s2.position - s1.position;
|
|
float const l = geom::length(d);
|
|
if (l > b1.radius + b2.radius)
|
|
return std::nullopt;
|
|
|
|
auto const p = geom::lerp(s1.position, s2.position, (b1.radius + l - b2.radius) / 2.f / l);
|
|
return collision{p, d * (b1.radius + b2.radius - l) / l};
|
|
}
|
|
|
|
std::optional<collision> shape_collision(ball const & b1, static_state const & s1, half_space const & b2, static_state const &)
|
|
{
|
|
float v = b2.normal[0] * s1.position[0] + b2.normal[1] * s1.position[1] - b2.value - b1.radius;
|
|
if (v > 0.f)
|
|
return std::nullopt;
|
|
|
|
return collision{s1.position - b2.normal * b1.radius, v * b2.normal};
|
|
}
|
|
|
|
std::optional<collision> shape_collision(ball const & b1, static_state const & s1, box const & b2, static_state const & s2)
|
|
{
|
|
geom::vector ex{std::cos(s2.rotation), std::sin(s2.rotation)};
|
|
geom::vector ey{-std::sin(s2.rotation), std::cos(s2.rotation)};
|
|
|
|
float w = b2.width / 2.f;
|
|
float h = b2.height / 2.f;
|
|
|
|
auto r = s1.position - s2.position;
|
|
|
|
float x = geom::dot(r, ex);
|
|
float y = geom::dot(r, ey);
|
|
|
|
if (std::abs(x) < w && std::abs(y) < h)
|
|
{
|
|
// ball center is inside the box
|
|
|
|
float vx = w - std::abs(x);
|
|
float vy = h - std::abs(y);
|
|
|
|
if (vx < vy)
|
|
{
|
|
if (x > 0.f)
|
|
{
|
|
return collision{s1.position, geom::vector{-vx - b1.radius, 0.f}};
|
|
}
|
|
else
|
|
{
|
|
return collision{s1.position, geom::vector{vx + b1.radius, 0.f}};
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (y > 0.f)
|
|
{
|
|
return collision{s1.position, geom::vector{0.f, -vy - b1.radius}};
|
|
}
|
|
else
|
|
{
|
|
return collision{s1.position, geom::vector{0.f, vy + b1.radius}};
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
float cx = geom::clamp(x, {-w, w});
|
|
float cy = geom::clamp(y, {-h, h});
|
|
|
|
geom::vector n = ex * (cx - x) + ey * (cy - y);
|
|
|
|
float l = geom::length(n);
|
|
|
|
if (l < b1.radius)
|
|
{
|
|
float v = b1.radius - l;
|
|
n = n / l;
|
|
return collision{s1.position + n * (b1.radius - v / 2.f), n * v};
|
|
}
|
|
}
|
|
|
|
|
|
return std::nullopt;
|
|
}
|
|
|
|
std::optional<collision> shape_collision(half_space const & b1, static_state const & s1, ball const & b2, static_state const & s2)
|
|
{
|
|
return invert(shape_collision(b2, s2, b1, s1));
|
|
}
|
|
|
|
std::optional<collision> shape_collision(half_space const &, static_state const &, half_space const &, static_state const &)
|
|
{
|
|
return std::nullopt;
|
|
}
|
|
|
|
std::optional<collision> shape_collision(half_space const & b1, static_state const &, box const & b2, static_state const & s2)
|
|
{
|
|
geom::vector a{ b2.width / 2.f, b2.height / 2.f};
|
|
geom::vector b{-b2.width / 2.f, b2.height / 2.f};
|
|
|
|
auto const rot = geom::plane_rotation<float, 2>(0, 1, s2.rotation);
|
|
a = rot(a);
|
|
b = rot(b);
|
|
|
|
float v = b1.normal[0] * s2.position[0] + b1.normal[1] * s2.position[1] - b1.value;
|
|
|
|
geom::vector<float, 2> d;
|
|
|
|
float va = geom::dot(a, b1.normal);
|
|
float vb = geom::dot(b, b1.normal);
|
|
v -= std::max(std::abs(va), std::abs(vb));
|
|
if (std::abs(va) > std::abs(vb))
|
|
{
|
|
if (va > 0.f)
|
|
d = -a;
|
|
else
|
|
d = a;
|
|
}
|
|
else if (std::abs(vb) > std::abs(va))
|
|
{
|
|
if (vb > 0.f)
|
|
d = -b;
|
|
else
|
|
d = b;
|
|
}
|
|
else
|
|
{
|
|
// multiple points of contact
|
|
auto da = (va > 0.f) ? -a : a;
|
|
auto db = (vb > 0.f) ? -b : b;
|
|
d = da * 0.5f + db * 0.5f;
|
|
}
|
|
|
|
if (v > 0.f)
|
|
return std::nullopt;
|
|
return collision{s2.position + d, - v * b1.normal};
|
|
}
|
|
|
|
std::optional<collision> shape_collision(box const & b1, static_state const & s1, ball const & b2, static_state const & s2)
|
|
{
|
|
return invert(shape_collision(b2, s2, b1, s1));
|
|
}
|
|
|
|
std::optional<collision> shape_collision(box const & b1, static_state const & s1, half_space const & b2, static_state const & s2)
|
|
{
|
|
return invert(shape_collision(b2, s2, b1, s1));
|
|
}
|
|
|
|
std::optional<collision> shape_collision(box const & b1, static_state const & s1, box const & b2, static_state const & s2)
|
|
{
|
|
geom::vector<float, 2> ex1{ std::cos(s1.rotation), std::sin(s1.rotation)};
|
|
geom::vector<float, 2> ey1{-std::sin(s1.rotation), std::cos(s1.rotation)};
|
|
|
|
geom::vector<float, 2> ex2{ std::cos(s2.rotation), std::sin(s2.rotation)};
|
|
geom::vector<float, 2> ey2{-std::sin(s2.rotation), std::cos(s2.rotation)};
|
|
|
|
float w1 = b1.width / 2.f;
|
|
float h1 = b1.height / 2.f;
|
|
float w2 = b2.width / 2.f;
|
|
float h2 = b2.height / 2.f;
|
|
|
|
geom::point<float, 2> points1[4] =
|
|
{
|
|
s1.position - ex1 * w1 - ey1 * h1,
|
|
s1.position + ex1 * w1 - ey1 * h1,
|
|
s1.position + ex1 * w1 + ey1 * h1,
|
|
s1.position - ex1 * w1 + ey1 * h1,
|
|
};
|
|
|
|
geom::vector<float, 2> normals1[4] =
|
|
{
|
|
-ey1,
|
|
ex1,
|
|
ey1,
|
|
-ex1
|
|
};
|
|
|
|
geom::point<float, 2> 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,
|
|
};
|
|
|
|
geom::vector<float, 2> normals2[4] =
|
|
{
|
|
-ey2,
|
|
ex2,
|
|
ey2,
|
|
-ex2
|
|
};
|
|
|
|
return convex_collision(points1, normals1, points2, normals2);
|
|
}
|
|
|
|
struct contact
|
|
{
|
|
std::size_t gi, i, gj, j;
|
|
geom::vector<float, 2> ri, rj;
|
|
geom::vector<float, 2> penetration;
|
|
geom::vector<float, 2> normal;
|
|
float penetration_depth;
|
|
geom::vector<float, 2> velocity;
|
|
float velocity_projection;
|
|
};
|
|
|
|
struct velocity_comparator
|
|
{
|
|
bool operator()(contact const & c1, contact const & c2) const
|
|
{
|
|
if (c1.velocity_projection == c2.velocity_projection)
|
|
return std::tie(c1.gi, c1.gj, c1.i, c1.j) < std::tie(c2.gi, c2.gj, c2.i, c2.j);
|
|
return c1.velocity_projection < c2.velocity_projection;
|
|
}
|
|
};
|
|
|
|
struct penetration_comparator
|
|
{
|
|
bool operator()(contact const & c1, contact const & c2) const
|
|
{
|
|
if (c1.penetration_depth == c2.penetration_depth)
|
|
return std::tie(c1.gi, c1.gj, c1.i, c1.j) < std::tie(c2.gi, c2.gj, c2.i, c2.j);
|
|
return c1.penetration_depth > c2.penetration_depth;
|
|
}
|
|
};
|
|
|
|
}
|
|
|
|
struct engine::impl
|
|
{
|
|
util::heterogeneous_container<engine::shape_handle
|
|
, wrapped_shape<ball>
|
|
, wrapped_shape<half_space>
|
|
, wrapped_shape<box>
|
|
// , box
|
|
// , convex_polygon
|
|
> shapes;
|
|
|
|
util::flat_list<material, engine::material_handle> materials;
|
|
|
|
struct group
|
|
{
|
|
std::vector<object_info> infos;
|
|
std::vector<static_state> static_states;
|
|
std::vector<dynamic_state> dynamic_states;
|
|
|
|
std::vector<std::vector<std::set<contact, velocity_comparator>::iterator>> contacts_by_velocity;
|
|
std::vector<std::vector<std::set<contact, penetration_comparator>::iterator>> contacts_by_penetration;
|
|
};
|
|
|
|
std::vector<group> groups;
|
|
|
|
std::optional<geom::vector<float, 2>> gravity;
|
|
|
|
void explode(geom::point<float, 2> const & center, float strength, float attenuation);
|
|
|
|
void update(float dt);
|
|
|
|
void apply_gravity(float dt);
|
|
void integrate(float dt);
|
|
void resolve_collisions();
|
|
|
|
void log_energy();
|
|
float energy();
|
|
};
|
|
|
|
void engine::impl::explode(geom::point<float, 2> const & center, float strength, float attenuation)
|
|
{
|
|
for (auto & g : groups)
|
|
{
|
|
for (std::size_t i = 0; i < g.infos.size(); ++i)
|
|
{
|
|
auto r = g.static_states[i].position - center;
|
|
float l = geom::length(r);
|
|
|
|
auto J = (r / l) * strength / (1.f + l * l * attenuation);
|
|
g.dynamic_states[i].velocity += J * g.infos[i].inv_mass;
|
|
}
|
|
}
|
|
}
|
|
|
|
void engine::impl::update(float dt)
|
|
{
|
|
apply_gravity(dt);
|
|
integrate(dt);
|
|
resolve_collisions();
|
|
// log_energy();
|
|
}
|
|
|
|
void engine::impl::apply_gravity(float dt)
|
|
{
|
|
if (gravity)
|
|
{
|
|
for (auto & g : groups)
|
|
{
|
|
for (std::size_t i = 0; i < g.infos.size(); ++i)
|
|
{
|
|
if (g.infos[i].inv_mass == 0.f) continue;
|
|
|
|
g.dynamic_states[i].velocity += *gravity * dt;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void engine::impl::integrate(float dt)
|
|
{
|
|
for (auto & g : groups)
|
|
{
|
|
for (std::size_t i = 0; i < g.infos.size(); ++i)
|
|
{
|
|
g.static_states[i].position += dt * g.dynamic_states[i].velocity;
|
|
g.static_states[i].rotation += dt * g.dynamic_states[i].angular_velocity;
|
|
}
|
|
}
|
|
}
|
|
|
|
void engine::impl::resolve_collisions()
|
|
{
|
|
for (std::size_t gi = 0; gi < groups.size(); ++gi)
|
|
{
|
|
groups[gi].contacts_by_velocity.resize(groups[gi].infos.size());
|
|
groups[gi].contacts_by_penetration.resize(groups[gi].infos.size());
|
|
for (std::size_t i = 0; i < groups[gi].infos.size(); ++i)
|
|
{
|
|
groups[gi].contacts_by_velocity[i].clear();
|
|
groups[gi].contacts_by_penetration[i].clear();
|
|
}
|
|
}
|
|
|
|
std::set<contact, velocity_comparator> contacts_by_velocity;
|
|
|
|
for (std::size_t gi = 0; gi < groups.size(); ++gi)
|
|
{
|
|
for (std::size_t gj = gi; gj < groups.size(); ++gj)
|
|
{
|
|
for (std::size_t i = 0; i < groups[gi].infos.size(); ++i)
|
|
{
|
|
for (std::size_t j = (gi == gj) ? i + 1 : 0; j < groups[gj].infos.size(); ++j)
|
|
{
|
|
auto shi = groups[gi].infos[i].shape;
|
|
auto shj = groups[gj].infos[j].shape;
|
|
|
|
auto & sti = groups[gi].static_states[i];
|
|
auto & stj = groups[gj].static_states[j];
|
|
|
|
auto c = shapes.visit([&](auto const & si, auto const & sj){
|
|
return shape_collision(si.shape, sti, sj.shape, stj);
|
|
}, shi, shj);
|
|
|
|
if (!c) continue;
|
|
|
|
if (c->penetration == geom::vector<float, 2>::zero()) continue;
|
|
|
|
auto ri = c->position - sti.position;
|
|
auto rj = c->position - stj.position;
|
|
|
|
auto & dsi = groups[gi].dynamic_states[i];
|
|
auto & dsj = groups[gj].dynamic_states[j];
|
|
|
|
auto ui = dsi.velocity + geom::ort(ri) * dsi.angular_velocity;
|
|
auto uj = dsj.velocity + geom::ort(rj) * dsj.angular_velocity;
|
|
|
|
auto const u = uj - ui;
|
|
|
|
contact ct;
|
|
ct.gi = gi;
|
|
ct.gj = gj;
|
|
ct.i = i;
|
|
ct.j = j;
|
|
ct.ri = ri;
|
|
ct.rj = rj;
|
|
ct.penetration = c->penetration;
|
|
ct.normal = geom::normalized(ct.penetration);
|
|
ct.penetration_depth = geom::length(ct.penetration);
|
|
ct.velocity = u;
|
|
ct.velocity_projection = geom::dot(u, ct.normal);
|
|
|
|
auto res = contacts_by_velocity.insert(ct);
|
|
assert(res.second);
|
|
groups[gi].contacts_by_velocity[i].push_back(res.first);
|
|
groups[gj].contacts_by_velocity[j].push_back(res.first);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
std::size_t const contact_count = contacts_by_velocity.size();
|
|
std::size_t const impulse_iterations = contact_count * 2;
|
|
std::size_t const penetraion_iterations = contact_count * 2;
|
|
|
|
for (std::size_t iteration = 0; iteration < impulse_iterations; ++iteration)
|
|
{
|
|
contact const & c = *contacts_by_velocity.begin();
|
|
|
|
if (c.velocity_projection > 0.f)
|
|
{
|
|
break;
|
|
}
|
|
|
|
if (c.gi == 1 && c.gj == 1)
|
|
{
|
|
int fuck = 42;
|
|
(void)fuck;
|
|
}
|
|
|
|
auto const & infoi = groups[c.gi].infos[c.i];
|
|
auto const & infoj = groups[c.gj].infos[c.j];
|
|
|
|
auto const & mati = materials[infoi.material];
|
|
auto const & matj = materials[infoj.material];
|
|
|
|
geom::matrix<float, 2, 2> K;
|
|
K[0][0] = infoi.inv_mass + infoj.inv_mass;
|
|
K[1][1] = K[0][0];
|
|
K[0][1] = 0.f;
|
|
K[1][0] = 0.f;
|
|
|
|
K[0][0] += c.ri[1] * c.ri[1] * infoi.inv_inertia;
|
|
K[1][1] += c.ri[0] * c.ri[0] * infoi.inv_inertia;
|
|
K[0][1] -= c.ri[0] * c.ri[1] * infoi.inv_inertia;
|
|
|
|
K[0][0] += c.rj[1] * c.rj[1] * infoj.inv_inertia;
|
|
K[1][1] += c.rj[0] * c.rj[0] * infoj.inv_inertia;
|
|
K[0][1] -= c.rj[0] * c.rj[1] * infoj.inv_inertia;
|
|
|
|
K[1][0] = K[0][1];
|
|
|
|
auto const & n = c.normal;
|
|
|
|
// Plastic sliding impulse
|
|
// Normal relative velocity -> 0
|
|
// Tangential relative velocity -> unchanged
|
|
auto const J1 = - n * c.velocity_projection / geom::dot(n, K * n);
|
|
// Plastic sticking impulse
|
|
// Normal relative velocity -> 0
|
|
// Tangential relative velocity -> 0
|
|
auto const J2 = - *geom::solve(K, c.velocity);
|
|
|
|
float const e = std::sqrt(mati.elasticity * matj.elasticity);
|
|
float const f = std::sqrt(mati.friction * matj.friction);
|
|
float const mu = f;
|
|
|
|
auto J = (1.f + e) * J1 + f * (J2 - J1);
|
|
|
|
// Test for friction cone
|
|
float q = geom::length(J - n * geom::dot(J, n));
|
|
if (q > mu * geom::dot(J, n))
|
|
{
|
|
float k = (mu * (1.f + e) * geom::dot(J1, n)) / (geom::length(J2 - n * geom::dot(J2, n)) - mu * geom::dot(n, J2 - J1));
|
|
J = (1.f + e) * J1 + k * (J2 - J1);
|
|
}
|
|
|
|
auto dvi = -J * infoi.inv_mass;
|
|
auto dvj = J * infoj.inv_mass;
|
|
|
|
auto dai = -geom::det(c.ri, J) * infoi.inv_inertia;
|
|
auto daj = geom::det(c.rj, J) * infoj.inv_inertia;
|
|
|
|
groups[c.gi].dynamic_states[c.i].velocity += dvi;
|
|
groups[c.gj].dynamic_states[c.j].velocity += dvj;
|
|
|
|
groups[c.gi].dynamic_states[c.i].angular_velocity += dai;
|
|
groups[c.gj].dynamic_states[c.j].angular_velocity += daj;
|
|
|
|
auto ct = contacts_by_velocity.begin();
|
|
|
|
std::size_t cti = -1, ctj = -1;
|
|
|
|
auto update_contact = [&](auto old_it, std::size_t G, std::size_t I, std::size_t i, geom::vector<float, 2> const & dv, float da)
|
|
{
|
|
contact const & c = *old_it;
|
|
|
|
std::size_t OG, OI;
|
|
bool flip;
|
|
|
|
if (c.gi == G && c.i == I)
|
|
{
|
|
OG = c.gj;
|
|
OI = c.j;
|
|
flip = false;
|
|
}
|
|
else
|
|
{
|
|
OG = c.gi;
|
|
OI = c.i;
|
|
flip = true;
|
|
}
|
|
|
|
std::size_t oi;
|
|
for (oi = 0; oi < groups[OG].contacts_by_velocity[OI].size(); ++oi)
|
|
{
|
|
if (groups[OG].contacts_by_velocity[OI][oi] == old_it)
|
|
break;
|
|
}
|
|
|
|
auto node = contacts_by_velocity.extract(old_it);
|
|
contact & cc = node.value();
|
|
cc.velocity += (flip ? 1.f : -1.f) * (dv + geom::ort(flip ? cc.rj : cc.ri) * da);
|
|
cc.velocity_projection = geom::dot(cc.velocity, cc.normal);
|
|
|
|
auto res = contacts_by_velocity.insert(std::move(node));
|
|
assert(res.inserted);
|
|
auto new_it = res.position;
|
|
|
|
groups[G].contacts_by_velocity[I][i] = new_it;
|
|
groups[OG].contacts_by_velocity[OI][oi] = new_it;
|
|
};
|
|
|
|
for (std::size_t i = 0; i < groups[c.gi].contacts_by_velocity[c.i].size(); ++i)
|
|
{
|
|
auto old_it = groups[c.gi].contacts_by_velocity[c.i][i];
|
|
if (old_it == ct)
|
|
{
|
|
cti = i;
|
|
continue;
|
|
}
|
|
|
|
update_contact(old_it, c.gi, c.i, i, dvi, dai);
|
|
}
|
|
|
|
for (std::size_t i = 0; i < groups[c.gj].contacts_by_velocity[c.j].size(); ++i)
|
|
{
|
|
auto old_it = groups[c.gj].contacts_by_velocity[c.j][i];
|
|
if (old_it == ct)
|
|
{
|
|
ctj = i;
|
|
continue;
|
|
}
|
|
|
|
update_contact(old_it, c.gj, c.j, i, dvj, daj);
|
|
}
|
|
|
|
{
|
|
auto node = contacts_by_velocity.extract(ct);
|
|
contact & cc = node.value();
|
|
cc.velocity -= dvi + geom::ort(cc.ri) * dai;
|
|
cc.velocity += dvj + geom::ort(cc.rj) * daj;
|
|
cc.velocity_projection = geom::dot(cc.velocity, cc.normal);
|
|
|
|
auto res = contacts_by_velocity.insert(std::move(node));
|
|
assert(res.inserted);
|
|
auto new_it = res.position;
|
|
|
|
auto & ccc = *new_it;
|
|
|
|
groups[ccc.gi].contacts_by_velocity[ccc.i][cti] = new_it;
|
|
groups[ccc.gj].contacts_by_velocity[ccc.j][ctj] = new_it;
|
|
}
|
|
}
|
|
|
|
std::set<contact, penetration_comparator> contacts_by_penetration;
|
|
|
|
for (auto it = contacts_by_velocity.begin(); it != contacts_by_velocity.end();)
|
|
{
|
|
auto jt = it++;
|
|
auto node = contacts_by_velocity.extract(jt);
|
|
auto res = contacts_by_penetration.insert(std::move(node));
|
|
assert(res.inserted);
|
|
auto new_it = res.position;
|
|
contact const & c = *new_it;
|
|
groups[c.gi].contacts_by_penetration[c.i].push_back(new_it);
|
|
groups[c.gj].contacts_by_penetration[c.j].push_back(new_it);
|
|
}
|
|
|
|
assert(contacts_by_penetration.size() == contact_count);
|
|
|
|
for (std::size_t iteration = 0; iteration < penetraion_iterations; ++iteration)
|
|
{
|
|
contact const & c = *contacts_by_penetration.begin();
|
|
|
|
if (c.penetration_depth < 0.f)
|
|
break;
|
|
|
|
auto const & infoi = groups[c.gi].infos[c.i];
|
|
auto const & infoj = groups[c.gj].infos[c.j];
|
|
|
|
auto di = - c.penetration * infoi.inv_mass / (infoi.inv_mass + infoj.inv_mass);
|
|
auto dj = c.penetration * infoj.inv_mass / (infoi.inv_mass + infoj.inv_mass);
|
|
|
|
groups[c.gi].static_states[c.i].position += di;
|
|
groups[c.gj].static_states[c.j].position += dj;
|
|
|
|
auto ct = contacts_by_penetration.begin();
|
|
|
|
std::size_t cti = -1, ctj = -1;
|
|
|
|
auto update_contact = [&](auto old_it, std::size_t G, std::size_t I, std::size_t i, geom::vector<float, 2> const & d)
|
|
{
|
|
contact const & c = *old_it;
|
|
|
|
std::size_t OG, OI;
|
|
bool flip;
|
|
|
|
if (c.gi == G && c.i == I)
|
|
{
|
|
OG = c.gj;
|
|
OI = c.j;
|
|
flip = false;
|
|
}
|
|
else
|
|
{
|
|
OG = c.gi;
|
|
OI = c.i;
|
|
flip = true;
|
|
}
|
|
|
|
std::size_t oi;
|
|
for (oi = 0; oi < groups[OG].contacts_by_penetration[OI].size(); ++oi)
|
|
{
|
|
if (groups[OG].contacts_by_penetration[OI][oi] == old_it)
|
|
break;
|
|
}
|
|
|
|
auto node = contacts_by_penetration.extract(old_it);
|
|
contact & cc = node.value();
|
|
cc.penetration += (flip ? -1.f : 1.f) * d;
|
|
cc.penetration_depth = geom::dot(cc.penetration, cc.normal);
|
|
|
|
auto res = contacts_by_penetration.insert(std::move(node));
|
|
assert(res.inserted);
|
|
auto new_it = res.position;
|
|
|
|
groups[G].contacts_by_penetration[I][i] = new_it;
|
|
groups[OG].contacts_by_penetration[OI][oi] = new_it;
|
|
};
|
|
|
|
for (std::size_t i = 0; i < groups[c.gi].contacts_by_penetration[c.i].size(); ++i)
|
|
{
|
|
auto old_it = groups[c.gi].contacts_by_penetration[c.i][i];
|
|
if (old_it == ct)
|
|
{
|
|
cti = i;
|
|
continue;
|
|
}
|
|
|
|
update_contact(old_it, c.gi, c.i, i, di);
|
|
}
|
|
|
|
for (std::size_t i = 0; i < groups[c.gj].contacts_by_penetration[c.j].size(); ++i)
|
|
{
|
|
auto old_it = groups[c.gj].contacts_by_penetration[c.j][i];
|
|
if (old_it == ct)
|
|
{
|
|
ctj = i;
|
|
continue;
|
|
}
|
|
|
|
update_contact(old_it, c.gj, c.j, i, dj);
|
|
}
|
|
|
|
{
|
|
auto node = contacts_by_penetration.extract(ct);
|
|
contact & cc = node.value();
|
|
cc.penetration += di;
|
|
cc.penetration -= dj;
|
|
cc.penetration_depth = geom::dot(cc.penetration, cc.normal);
|
|
|
|
auto res = contacts_by_penetration.insert(std::move(node));
|
|
assert(res.inserted);
|
|
auto new_it = res.position;
|
|
|
|
auto & ccc = *new_it;
|
|
|
|
groups[ccc.gi].contacts_by_penetration[ccc.i][cti] = new_it;
|
|
groups[ccc.gj].contacts_by_penetration[ccc.j][ctj] = new_it;
|
|
}
|
|
}
|
|
}
|
|
|
|
float engine::impl::energy()
|
|
{
|
|
float Eg = 0.f;
|
|
float Er = 0.f;
|
|
float Ek = 0.f;
|
|
|
|
for (auto & g : groups)
|
|
{
|
|
for (std::size_t i = 0; i < g.infos.size(); ++i)
|
|
{
|
|
if (g.infos[i].inv_mass == 0.f) continue;
|
|
|
|
if (gravity)
|
|
Eg -= geom::dot(*gravity, g.static_states[i].position - geom::point<float, 2>::zero()) / g.infos[i].inv_mass;
|
|
|
|
Ek += geom::length_sqr(g.dynamic_states[i].velocity) / g.infos[i].inv_mass / 2.f;
|
|
Er += geom::sqr(g.dynamic_states[i].angular_velocity) / g.infos[i].inv_inertia / 2.f;
|
|
}
|
|
}
|
|
|
|
return Eg + Ek + Er;
|
|
}
|
|
|
|
void engine::impl::log_energy()
|
|
{
|
|
float Eg = 0.f;
|
|
float Er = 0.f;
|
|
float Ek = 0.f;
|
|
|
|
for (auto & g : groups)
|
|
{
|
|
for (std::size_t i = 0; i < g.infos.size(); ++i)
|
|
{
|
|
if (g.infos[i].inv_mass == 0.f) continue;
|
|
|
|
if (gravity)
|
|
Eg -= geom::dot(*gravity, g.static_states[i].position - geom::point<float, 2>::zero()) / g.infos[i].inv_mass;
|
|
|
|
Ek += geom::length_sqr(g.dynamic_states[i].velocity) / g.infos[i].inv_mass / 2.f;
|
|
Er += geom::sqr(g.dynamic_states[i].angular_velocity) / g.infos[i].inv_inertia / 2.f;
|
|
}
|
|
}
|
|
|
|
log::info() << "G " << Eg << " K " << Ek << " R " << Er;
|
|
}
|
|
|
|
engine::engine()
|
|
: pimpl_{std::make_unique<struct impl>()}
|
|
{}
|
|
|
|
engine::~engine() = default;
|
|
|
|
engine::shape_handle engine::add_shape(ball const & b)
|
|
{
|
|
return impl().shapes.insert(wrap(b));
|
|
}
|
|
|
|
engine::shape_handle engine::add_shape(half_space const & s)
|
|
{
|
|
return impl().shapes.insert(wrap(s));
|
|
}
|
|
|
|
engine::shape_handle engine::add_shape(box const & b)
|
|
{
|
|
return impl().shapes.insert(wrap(b));
|
|
}
|
|
|
|
void engine::remove_shape(shape_handle handle)
|
|
{
|
|
impl().shapes.erase(handle);
|
|
}
|
|
|
|
engine::material_handle engine::add_material(material const & m)
|
|
{
|
|
return impl().materials.insert(m);
|
|
}
|
|
|
|
void engine::remove_material(material_handle handle)
|
|
{
|
|
impl().materials.erase(handle);
|
|
}
|
|
|
|
engine::group_handle engine::create_group()
|
|
{
|
|
auto h = static_cast<group_handle>(impl().groups.size());
|
|
impl().groups.emplace_back();
|
|
return h;
|
|
}
|
|
|
|
void engine::remove_group(group_handle handle)
|
|
{
|
|
auto & g = impl().groups[handle];
|
|
g.infos.clear();
|
|
g.static_states.clear();
|
|
g.dynamic_states.clear();
|
|
}
|
|
|
|
std::size_t engine::group_size(group_handle handle) const
|
|
{
|
|
return impl().groups[handle].infos.size();
|
|
}
|
|
|
|
static_state const * engine::group_static_state(group_handle handle) const
|
|
{
|
|
return impl().groups[handle].static_states.data();
|
|
}
|
|
|
|
dynamic_state const * engine::group_dynamic_state(group_handle handle) const
|
|
{
|
|
return impl().groups[handle].dynamic_states.data();
|
|
}
|
|
|
|
static_state * engine::group_static_state(group_handle handle)
|
|
{
|
|
return impl().groups[handle].static_states.data();
|
|
}
|
|
|
|
dynamic_state * engine::group_dynamic_state(group_handle handle)
|
|
{
|
|
return impl().groups[handle].dynamic_states.data();
|
|
}
|
|
|
|
std::size_t engine::add_object(group_handle group, shape_handle shape, material_handle material, static_state const & ss, dynamic_state const & ds)
|
|
{
|
|
float const density = impl().materials[material].density;
|
|
float const area = impl().shapes.visit([](auto const & s){ return s.area; }, shape);
|
|
float const inertia = impl().shapes.visit([](auto const & s){ return s.inertia; }, shape);
|
|
|
|
auto & g = impl().groups[group];
|
|
g.infos.push_back({shape, material, 1.f / (area * density), 1.f / (inertia * density)});
|
|
g.static_states.push_back(ss);
|
|
g.dynamic_states.push_back(ds);
|
|
|
|
return g.infos.size() - 1;
|
|
}
|
|
|
|
void engine::remove_object(group_handle group, std::size_t index)
|
|
{
|
|
auto & g = impl().groups[group];
|
|
g.infos.erase(g.infos.begin() + index);
|
|
g.static_states.erase(g.static_states.begin() + index);
|
|
g.dynamic_states.erase(g.dynamic_states.begin() + index);
|
|
}
|
|
|
|
engine::object_info const & engine::info(group_handle group, std::size_t index)
|
|
{
|
|
return impl().groups[group].infos[index];
|
|
}
|
|
|
|
std::variant<ball, half_space, box> engine::shape(group_handle group, std::size_t index)
|
|
{
|
|
using result = std::variant<ball, half_space, box>;
|
|
auto sh = impl().groups[group].infos[index].shape;
|
|
return impl().shapes.visit([](auto const & s) -> result { return s.shape; }, sh);
|
|
}
|
|
|
|
void engine::set_gravity(geom::vector<float, 2> const & g)
|
|
{
|
|
impl().gravity = g;
|
|
}
|
|
|
|
void engine::explode(geom::point<float, 2> const & center, float strength, float attenuation)
|
|
{
|
|
impl().explode(center, strength, attenuation);
|
|
}
|
|
|
|
void engine::update(float dt)
|
|
{
|
|
impl().update(dt);
|
|
}
|
|
|
|
}
|