499 lines
15 KiB
C++
499 lines
15 KiB
C++
#include <psemek/app/application_base.hpp>
|
|
#include <psemek/app/default_application_factory.hpp>
|
|
|
|
#include <psemek/phys/engine_2d.hpp>
|
|
|
|
#include <psemek/gfx/painter.hpp>
|
|
|
|
#include <psemek/geom/box.hpp>
|
|
#include <psemek/geom/camera.hpp>
|
|
#include <psemek/geom/rotation.hpp>
|
|
|
|
#include <psemek/util/clock.hpp>
|
|
#include <psemek/util/recursive.hpp>
|
|
#include <psemek/prof/profiler.hpp>
|
|
|
|
#include <psemek/async/event_loop.hpp>
|
|
|
|
#include <psemek/random/generator.hpp>
|
|
#include <psemek/random/uniform.hpp>
|
|
|
|
#include <psemek/log/log.hpp>
|
|
|
|
#include <cstdint>
|
|
#include <vector>
|
|
|
|
using namespace psemek;
|
|
|
|
struct physics_2d_app
|
|
: app::application_base
|
|
{
|
|
phys2d::engine physics;
|
|
|
|
geom::box<float, 2> view_box;
|
|
geom::box<float, 2> simulation_box;
|
|
|
|
gfx::painter painter;
|
|
|
|
util::clock<std::chrono::duration<float>> frame_clock;
|
|
|
|
float physics_lag = 0.f;
|
|
|
|
phys2d::engine::material_handle material;
|
|
phys2d::engine::shape_handle ball_shape, large_ball_shape, box_shape, small_box_shape, wide_box_shape;
|
|
phys2d::engine::group_handle ball_group, box_group;
|
|
|
|
float const world_width = 5.f;
|
|
float const world_height = 5.f;
|
|
|
|
float const line_width = 0.05f;
|
|
|
|
float const ball_radius = 0.25f;
|
|
float const box_width = 0.5f;
|
|
float const box_height = 0.25f;
|
|
|
|
std::vector<float> radiuses;
|
|
|
|
std::optional<geom::point<float, 2>> mouse;
|
|
|
|
std::optional<std::size_t> selected_ball;
|
|
|
|
std::optional<geom::vector<float, 2>> drag_delta;
|
|
|
|
std::size_t motor_id;
|
|
|
|
float motor = 0.f;
|
|
|
|
async::event_loop loop;
|
|
|
|
random::generator gen;
|
|
|
|
physics_2d_app(options const &, context const &)
|
|
{
|
|
simulation_box = {{{-world_width, world_width}, {-world_height, world_height}}};
|
|
|
|
view_box = expand(simulation_box, 1.f);
|
|
|
|
physics.set_gravity({0.f, -9.8f});
|
|
|
|
float const inf = std::numeric_limits<float>::infinity();
|
|
|
|
material = physics.add_material({1.f, 0.5f, 0.5f});
|
|
|
|
ball_shape = physics.add_shape(phys2d::ball{ball_radius});
|
|
ball_group = physics.create_group();
|
|
|
|
large_ball_shape = physics.add_shape(phys2d::ball{8.f * ball_radius});
|
|
|
|
box_shape = physics.add_shape(phys2d::box{box_width, box_height});
|
|
box_group = physics.create_group();
|
|
|
|
small_box_shape = physics.add_shape(phys2d::box{box_width / 2.f, box_height});
|
|
|
|
wide_box_shape = physics.add_shape(phys2d::box{box_width * 8.f, box_height});
|
|
|
|
int nx = std::ceil(simulation_box[0].length() / box_width);
|
|
int ny = std::ceil(simulation_box[1].length() / box_height);
|
|
|
|
// piramid
|
|
if (false)
|
|
{
|
|
int ky = 10;
|
|
|
|
for (int y = 0; y < ky; ++y)
|
|
{
|
|
for (int x = nx / 2 - ky / 2; x < nx / 2 - ky / 2 + (ky - y); ++x)
|
|
{
|
|
geom::point<float, 2> pos{simulation_box.corner((x + 0.5f * y + 0.5f) / nx, (y + 0.5f) / ny)};
|
|
physics.add_object(box_group, box_shape, material, {pos, 0.f}, {{0.f, 0.f}, 0.01f});
|
|
}
|
|
}
|
|
}
|
|
|
|
// pavement
|
|
// if (false)
|
|
{
|
|
int chess = 1;
|
|
for (int y = 0; y < 4; ++y)
|
|
{
|
|
if (chess && (y % 2))
|
|
{
|
|
geom::point<float, 2> pos{simulation_box.corner((0.25f) / nx, (y + 0.5f) / ny)};
|
|
physics.add_object(box_group, small_box_shape, material, {pos, 0.f}, {{0.f, 0.f}, 0.01f});
|
|
}
|
|
|
|
for (int x = 0; x < nx - chess * (y % 2); ++x)
|
|
{
|
|
geom::point<float, 2> pos{simulation_box.corner((x + 0.5f + chess * 0.5f * (y % 2)) / nx, (y + 0.5f) / ny)};
|
|
physics.add_object(box_group, box_shape, material, {pos, 0.f}, {{0.f, 0.f}, 0.01f});
|
|
}
|
|
|
|
if (chess && (y % 2))
|
|
{
|
|
geom::point<float, 2> pos{simulation_box.corner((nx - 0.25f) / nx, (y + 0.5f) / ny)};
|
|
physics.add_object(box_group, small_box_shape, material, {pos, 0.f}, {{0.f, 0.f}, 0.01f});
|
|
}
|
|
}
|
|
}
|
|
|
|
motor_id = physics.group_size(box_group);
|
|
|
|
// physics.add_object(box_group, box_shape, material, {{- box_width * 1.2f, 0.f}, 0.f}, {});
|
|
// physics.add_object(box_group, box_shape, material, {{ box_width * 1.2f, 0.f}, 0.f}, {});
|
|
|
|
{
|
|
|
|
if(false)
|
|
for (int i = 0; i < 15; ++i)
|
|
{
|
|
float a = geom::rad(360 * i / 15.f);
|
|
float r = ball_radius * 15.f / geom::pi;
|
|
geom::point<float, 2> pos{r * std::cos(a), r * std::sin(a)};
|
|
physics.add_object(ball_group, ball_shape, material, {pos, 0.f}, {});
|
|
}
|
|
}
|
|
|
|
auto wall_group = physics.create_group();
|
|
auto wall_material = physics.add_material({inf, 0.5f, 1.5f});
|
|
|
|
auto wall_0_shape = physics.add_shape(phys2d::half_space{{1.f, 0.f}, simulation_box[0].min});
|
|
physics.add_object(wall_group, wall_0_shape, wall_material, {}, {});
|
|
|
|
auto wall_1_shape = physics.add_shape(phys2d::half_space{{-1.f, 0.f}, -simulation_box[0].max});
|
|
physics.add_object(wall_group, wall_1_shape, wall_material, {}, {});
|
|
|
|
auto wall_2_shape = physics.add_shape(phys2d::half_space{{0.f, 1.f}, simulation_box[1].min});
|
|
physics.add_object(wall_group, wall_2_shape, wall_material, {}, {});
|
|
|
|
auto wall_3_shape = physics.add_shape(phys2d::half_space{{0.f, -1.f}, -simulation_box[1].max});
|
|
physics.add_object(wall_group, wall_3_shape, wall_material, {}, {});
|
|
|
|
auto task = util::recursive([this, dx = box_width * 0.4f](auto && self) mutable -> void {
|
|
physics.add_object(box_group, box_shape, material, {simulation_box.corner(0.5f, 0.9f) + geom::vector{dx, 0.f}, 0.f}, {});
|
|
// physics.add_object(ball_group, ball_shape, material, {simulation_box.corner(0.5f, 0.9f) + geom::vector{dx, 0.f}, 0.f}, {});
|
|
|
|
// float r = random::uniform_distribution<float>{0.05f, 0.5f}(gen);
|
|
// auto new_shape = physics.add_shape(phys2d::ball{r});
|
|
// physics.add_object(ball_group, new_shape, material, {simulation_box.corner(0.5f, 0.9f) + geom::vector{dx, 0.f}, 0.f}, {});
|
|
// radiuses.push_back(r);
|
|
|
|
dx *= -1.f;
|
|
if (physics.group_size(box_group) < 100)
|
|
loop.dispatch_at(std::chrono::system_clock::now() + std::chrono::milliseconds{100}, self);
|
|
});
|
|
(void)task;
|
|
// task();
|
|
|
|
// loop.dispatch_at(async::executor::clock::now() + std::chrono::seconds{1}, [this]{
|
|
// physics.add_object(ball_group, ball_shape, material, {simulation_box.corner(0.5f, 0.9f), 0.f}, {});
|
|
// });
|
|
|
|
loop.dispatch_at(async::clock::now() + std::chrono::seconds{1}, [this]{
|
|
physics.explode(simulation_box.corner(0.5f, -0.2f), 1000.f, 100.f);
|
|
});
|
|
|
|
// loop.dispatch_at(async::executor::clock::now() + std::chrono::seconds{5}, [this]{
|
|
// stop();
|
|
// });
|
|
}
|
|
|
|
~physics_2d_app()
|
|
{
|
|
prof::dump();
|
|
}
|
|
|
|
void on_event(app::resize_event const & event) override
|
|
{
|
|
app::application_base::on_event(event);
|
|
|
|
float const ratio = static_cast<float>(event.size[0]) / event.size[1];
|
|
|
|
float const c = view_box[0].center();
|
|
float const l = view_box[1].length() * ratio;
|
|
|
|
view_box[0] = {c - l / 2.f, c + l / 2.f};
|
|
}
|
|
|
|
void on_event(app::mouse_button_event const & event) override
|
|
{
|
|
app::application_base::on_event(event);
|
|
|
|
if (event.button == app::mouse_button::left && event.down)
|
|
{
|
|
if (selected_ball && mouse)
|
|
{
|
|
auto const & s = physics.group_static_state(ball_group)[*selected_ball];
|
|
auto r = s.position - *mouse;
|
|
r = geom::plane_rotation<float, 2>(0, 1, -s.rotation)(r);
|
|
drag_delta = r;
|
|
}
|
|
else if (mouse)
|
|
{
|
|
// physics.add_object(ball_group, ball_shape, material, {*mouse, 0.f}, {{0.f, 0.f}, 0.f});
|
|
// physics.add_object(ball_group, large_ball_shape, material, {*mouse, 0.f}, {});
|
|
physics.add_object(box_group, box_shape, material, {*mouse, 0.f}, {});
|
|
// physics.add_object(box_group, wide_box_shape, material, {*mouse, 0.f}, {});
|
|
}
|
|
}
|
|
|
|
if (event.button == app::mouse_button::left && !event.down)
|
|
{
|
|
drag_delta = std::nullopt;
|
|
}
|
|
|
|
if (event.button == app::mouse_button::right && event.down)
|
|
{
|
|
if (mouse)
|
|
{
|
|
physics.add_object(ball_group, ball_shape, material, {*mouse, 0.f}, {{0.f, 0.f}, 0.f});
|
|
// physics.add_object(box_group, box_shape, material, {*mouse, 0.f}, {});
|
|
// physics.add_object(box_group, wide_box_shape, material, {*mouse, 0.f}, {});
|
|
// physics.explode(*mouse, 10.f, 100.f);
|
|
}
|
|
}
|
|
}
|
|
|
|
void on_event(app::mouse_move_event const & event) override
|
|
{
|
|
app::application_base::on_event(event);
|
|
|
|
float mx = event.position[0] * 1.f / state().size[0];
|
|
float my = 1.f - event.position[1] * 1.f / state().size[1];
|
|
|
|
mouse = view_box.corner(mx, my);
|
|
}
|
|
|
|
void update() override
|
|
{
|
|
prof::profiler prof("update");
|
|
|
|
float MOTOR = 15.f;
|
|
|
|
float m = 0.f;
|
|
if (state().key_down.contains(app::keycode::LEFT))
|
|
m += MOTOR;
|
|
if (state().key_down.contains(app::keycode::RIGHT))
|
|
m += -MOTOR;
|
|
m = 15.f;
|
|
motor = m;
|
|
|
|
auto apply_constraints = [this](float dt, float lambda){
|
|
if (drag_delta && selected_ball)
|
|
{
|
|
auto & s = physics.group_static_state(ball_group)[*selected_ball];
|
|
auto & d = physics.group_dynamic_state(ball_group)[*selected_ball];
|
|
// auto const & info = physics.info(ball_group, *selected_ball);
|
|
{
|
|
auto r = -*drag_delta;
|
|
|
|
auto R = geom::plane_rotation<float, 2>(0, 1, s.rotation);
|
|
|
|
geom::vector<float, 2> f;
|
|
f[0] = s.position[0] + R(r)[0] - (*mouse)[0];
|
|
f[1] = s.position[1] + R(r)[1] - (*mouse)[1];
|
|
|
|
geom::matrix<float, 2, 3> J;
|
|
J[0][0] = 1.f;
|
|
J[0][1] = 0.f;
|
|
J[0][2] = - std::sin(s.rotation) * r[0] + std::cos(s.rotation) * r[1];
|
|
J[1][0] = 0.f;
|
|
J[1][1] = 1.f;
|
|
J[1][2] = std::cos(s.rotation) * r[0] - std::sin(s.rotation) * r[1];
|
|
|
|
geom::vector<float, 3> v;
|
|
v[0] = d.velocity[0];
|
|
v[1] = d.velocity[1];
|
|
v[2] = d.angular_velocity;
|
|
|
|
auto dv = geom::least_squares(J, -(1.f / 8.f) * f / dt - (J * v));
|
|
if (dv)
|
|
{
|
|
d.velocity[0] += (*dv)[0] * lambda;
|
|
d.velocity[1] += (*dv)[1] * lambda;
|
|
d.angular_velocity += (*dv)[2] * lambda;
|
|
}
|
|
}
|
|
}
|
|
|
|
auto solve_distance = [this, dt, lambda](auto group, std::size_t i, std::size_t j, float target)
|
|
{
|
|
auto & s0 = physics.group_static_state(group)[i];
|
|
auto & s1 = physics.group_static_state(group)[j];
|
|
auto & d0 = physics.group_dynamic_state(group)[i];
|
|
auto & d1 = physics.group_dynamic_state(group)[j];
|
|
|
|
auto r = s1.position - s0.position;
|
|
float l = geom::length(r);
|
|
|
|
geom::vector<float, 1> f;
|
|
f[0] = l - target;
|
|
|
|
geom::matrix<float, 1, 4> J;
|
|
J[0][0] = -r[0] / l;
|
|
J[0][1] = -r[1] / l;
|
|
J[0][2] = r[0] / l;
|
|
J[0][3] = r[1] / l;
|
|
|
|
geom::vector<float, 4> v;
|
|
v[0] = d0.velocity[0];
|
|
v[1] = d0.velocity[1];
|
|
v[2] = d1.velocity[0];
|
|
v[3] = d1.velocity[1];
|
|
|
|
auto dv = geom::least_squares(J, -(1.f / 8.f) * f / dt - (J * v));
|
|
if (dv)
|
|
{
|
|
d0.velocity[0] += (*dv)[0] * lambda;
|
|
d0.velocity[1] += (*dv)[1] * lambda;
|
|
d1.velocity[0] += (*dv)[2] * lambda;
|
|
d1.velocity[1] += (*dv)[3] * lambda;
|
|
}
|
|
};
|
|
|
|
std::size_t const n = 15;//physics.group_size(ball_group);
|
|
|
|
if (physics.group_size(ball_group) >= n)
|
|
for (std::size_t i = 0; i + 1 < n; ++i)
|
|
{
|
|
// solve_distance(ball_group, i, (i + 1) % n, 2.f * ball_radius);
|
|
}
|
|
|
|
if (false && physics.group_size(box_group) >= motor_id + 2)
|
|
{
|
|
solve_distance(box_group, motor_id, motor_id + 1, 5.75f * ball_radius);
|
|
if (motor != 0.f)
|
|
{
|
|
physics.group_dynamic_state(box_group)[motor_id + 0].angular_velocity = motor;
|
|
physics.group_dynamic_state(box_group)[motor_id + 1].angular_velocity = motor;
|
|
}
|
|
}
|
|
};
|
|
|
|
(void)apply_constraints;
|
|
|
|
loop.pump();
|
|
|
|
float const frame_dt = frame_clock.restart().count();
|
|
physics_lag += frame_dt;
|
|
|
|
static int frame_count = 0;
|
|
|
|
float const physics_dt = 0.001f;
|
|
float const max_physics_time = 0.01f;
|
|
if (false)
|
|
while (physics_lag > physics_dt)
|
|
{
|
|
physics_lag -= physics_dt;
|
|
physics.update(physics_dt);
|
|
for (int i = 0; i < 8; ++i)
|
|
apply_constraints(physics_dt, 1.f);
|
|
|
|
if (false && frame_clock.count() > max_physics_time)
|
|
{
|
|
log::warning() << "Can't keep up with physics, running " << physics_lag << "s behind";
|
|
break;
|
|
}
|
|
}
|
|
|
|
for (int i = 0; i < 16; ++i)
|
|
physics.update(physics_dt);
|
|
|
|
++frame_count;
|
|
if (frame_count == 300)
|
|
stop();
|
|
|
|
if (!drag_delta)
|
|
{
|
|
selected_ball = std::nullopt;
|
|
|
|
if (mouse)
|
|
{
|
|
float closest = std::numeric_limits<float>::infinity();
|
|
|
|
for (std::size_t i = 0; i < physics.group_size(ball_group); ++i)
|
|
{
|
|
auto p = physics.group_static_state(ball_group)[i].position;
|
|
float d = geom::distance(p, *mouse);
|
|
if (d <= std::min(closest, ball_radius))
|
|
{
|
|
closest = d;
|
|
selected_ball = i;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void present() override
|
|
{
|
|
gl::ClearColor(0.8f, 0.8f, 0.8f, 0.f);
|
|
gl::Clear(gl::COLOR_BUFFER_BIT);
|
|
|
|
for (std::size_t i = 0; i < physics.group_size(ball_group); ++i)
|
|
{
|
|
bool selected = selected_ball ? (*selected_ball == i) : false;
|
|
|
|
auto p = physics.group_static_state(ball_group)[i].position;
|
|
auto a = physics.group_static_state(ball_group)[i].rotation;
|
|
|
|
float r = std::get<phys2d::ball const *>(physics.get_shape(physics.get_object(ball_group, i).shape))->radius;
|
|
|
|
painter.circle(p, r, gfx::black);
|
|
painter.circle(p, r - line_width, gfx::light(gfx::blue, selected ? 0.8f : 1.f).as_color_rgba());
|
|
|
|
geom::vector const n{std::cos(a), std::sin(a)};
|
|
geom::vector const m = geom::ort(n);
|
|
|
|
painter.line(p - n * ball_radius / 2.f, p + n * ball_radius / 2.f, line_width, gfx::black, false);
|
|
painter.line(p - m * ball_radius / 2.f, p + m * ball_radius / 2.f, line_width, gfx::black, false);
|
|
}
|
|
|
|
for (std::size_t i = 0; i < physics.group_size(box_group); ++i)
|
|
{
|
|
// bool selected = false;
|
|
|
|
auto p = physics.group_static_state(box_group)[i].position;
|
|
auto a = physics.group_static_state(box_group)[i].rotation;
|
|
|
|
float w = std::get<phys2d::box const *>(physics.get_shape(physics.get_object(box_group, i).shape))->width / 2.f;
|
|
float h = std::get<phys2d::box const *>(physics.get_shape(physics.get_object(box_group, i).shape))->height / 2.f;
|
|
|
|
geom::vector<float, 2> q[4];
|
|
q[0] = {-w, -h};
|
|
q[1] = { w, -h};
|
|
q[2] = { w, h};
|
|
q[3] = {-w, h};
|
|
|
|
auto m = geom::plane_rotation<float, 2>(0, 1, a).linear_matrix();
|
|
|
|
for (std::size_t j = 0; j < 4; ++j)
|
|
q[j] = m * q[j];
|
|
|
|
painter.triangle(p + q[0], p + q[1], p + q[3], gfx::white);
|
|
painter.triangle(p + q[3], p + q[1], p + q[2], gfx::white);
|
|
|
|
painter.line(p + q[0], p + q[1], line_width, gfx::black, false);
|
|
painter.line(p + q[1], p + q[2], line_width, gfx::black, false);
|
|
painter.line(p + q[2], p + q[3], line_width, gfx::black, false);
|
|
painter.line(p + q[3], p + q[0], line_width, gfx::black, false);
|
|
}
|
|
|
|
painter.line(simulation_box.corner(0, 0), simulation_box.corner(1, 0), line_width, gfx::black, false);
|
|
painter.line(simulation_box.corner(1, 0), simulation_box.corner(1, 1), line_width, gfx::black, false);
|
|
painter.line(simulation_box.corner(1, 1), simulation_box.corner(0, 1), line_width, gfx::black, false);
|
|
painter.line(simulation_box.corner(0, 1), simulation_box.corner(0, 0), line_width, gfx::black, false);
|
|
|
|
painter.render(geom::orthographic_camera{view_box}.transform());
|
|
}
|
|
|
|
};
|
|
|
|
namespace psemek::app
|
|
{
|
|
|
|
std::unique_ptr<application::factory> make_application_factory()
|
|
{
|
|
return default_application_factory<physics_2d_app>({.name = "Physics example", .multisampling = 4});
|
|
}
|
|
|
|
}
|