diff --git a/examples/physics_2d.cpp b/examples/physics_2d.cpp index e046bbe6..0cce76cd 100644 --- a/examples/physics_2d.cpp +++ b/examples/physics_2d.cpp @@ -18,6 +18,9 @@ #include #include +#include +#include + #include #include @@ -38,16 +41,17 @@ struct physics_2d_app float physics_lag = 0.f; phys2d::engine::material_handle material; - phys2d::engine::shape_handle ball_shape, box_shape; + phys2d::engine::shape_handle ball_shape, large_ball_shape, box_shape, small_box_shape; phys2d::engine::group_handle ball_group, box_group; - float const world_size = 5.f; + 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 = 2.f * ball_radius; - float const box_height = 2.f * ball_radius; + float const ball_radius = 0.5f; + float const box_width = 0.5f; + float const box_height = 0.25f; std::vector radiuses; @@ -55,6 +59,12 @@ struct physics_2d_app std::optional selected_ball; + std::optional> drag_delta; + + std::size_t motor_id; + + float motor = 0.f; + async::event_loop loop; pcg::generator gen; @@ -62,7 +72,7 @@ struct physics_2d_app physics_2d_app() : app("Physics 2D example", 4) { - simulation_box = {{{-world_size, world_size}, {-world_size, world_size}}}; + simulation_box = {{{-world_width, world_width}, {-world_height, world_height}}}; view_box = expand(simulation_box, 1.f); @@ -70,28 +80,81 @@ struct physics_2d_app float const inf = std::numeric_limits::infinity(); - material = physics.add_material({1.f, 0.5f, 0.5f}); + material = physics.add_material({1.f, 0.5f, 1.f}); 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}); + int nx = std::ceil(simulation_box[0].length() / box_width); int ny = std::ceil(simulation_box[1].length() / box_height); - for (int x = 0; x < nx; ++x) + // piramid + if (false) { - for (int y = 0; y < ny / 3; ++y) + int ky = 10; + + for (int y = 0; y < ky; ++y) { - geom::point pos{simulation_box.corner((x + 0.5f) / nx, (y + 0.5f) / ny)}; - physics.add_object(box_group, box_shape, material, {pos, 0.f}, {}); + for (int x = nx / 2 - ky / 2; x < nx / 2 - ky / 2 + (ky - y); ++x) + { + geom::point 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 < 5; ++y) + { + if (chess && (y % 2)) + { + geom::point 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 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 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 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, 0.5f}); + auto wall_material = physics.add_material({inf, 0.5f, 1.f}); 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, {}, {}); @@ -120,6 +183,14 @@ struct physics_2d_app }); (void)task; // task(); + +// loop.dispatch_at(std::chrono::system_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(std::chrono::system_clock::now() + std::chrono::seconds{1}, [this]{ + physics.explode(simulation_box.corner(0.5f, -0.2f), 1000.f, 100.f); + }); } void on_resize(int width, int height) override @@ -138,15 +209,26 @@ struct physics_2d_app { app::on_left_button_down(); - if (mouse) + if (selected_ball && mouse) { - physics.add_object(ball_group, ball_shape, material, {*mouse, 0.f}, {{0.f, 0.f}, 25.f}); + auto const & s = physics.group_static_state(ball_group)[*selected_ball]; + auto r = s.position - *mouse; + r = geom::plane_rotation(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(box_group, box_shape, material, {*mouse, 0.f}, {}); +// physics.add_object(ball_group, large_ball_shape, material, {*mouse, 0.f}, {}); } } void on_left_button_up() override { app::on_left_button_up(); + + drag_delta = std::nullopt; } void on_right_button_down() override @@ -155,7 +237,7 @@ struct physics_2d_app if (mouse) { - physics.explode(*mouse, 10.f, 1.f); + physics.explode(*mouse, 1000.f, 100.f); } } @@ -171,35 +253,142 @@ struct physics_2d_app void update() override { - util::profiler prof("update"); + float MOTOR = 15.f; + + float m = 0.f; + if (is_key_down(SDLK_LEFT)) + m += MOTOR; + if (is_key_down(SDLK_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(0, 1, s.rotation); + + geom::vector f; + f[0] = s.position[0] + R(r)[0] - (*mouse)[0]; + f[1] = s.position[1] + R(r)[1] - (*mouse)[1]; + + geom::matrix 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 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 f; + f[0] = l - target; + + geom::matrix 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 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; - float const physics_dt = 0.001f; + float const physics_dt = 0.002f; while (physics_lag > physics_dt) // if (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); } - selected_ball = std::nullopt; - - if (mouse) + if (!drag_delta) { - float closest = std::numeric_limits::infinity(); + selected_ball = std::nullopt; - for (std::size_t i = 0; i < physics.group_size(ball_group); ++i) + if (mouse) { - auto p = physics.group_static_state(ball_group)[i].position; - float d = geom::distance(p, *mouse); - if (d <= std::min(closest, ball_radius)) + float closest = std::numeric_limits::infinity(); + + for (std::size_t i = 0; i < physics.group_size(ball_group); ++i) { - closest = d; - selected_ball = 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; + } } } } @@ -207,8 +396,6 @@ struct physics_2d_app void present() override { - util::profiler prof("present"); - gl::ClearColor(0.8f, 0.8f, 0.8f, 0.f); gl::Clear(gl::COLOR_BUFFER_BIT); @@ -219,8 +406,10 @@ struct physics_2d_app auto p = physics.group_static_state(ball_group)[i].position; auto a = physics.group_static_state(ball_group)[i].rotation; - painter.circle(p, ball_radius, gfx::black); - painter.circle(p, ball_radius - line_width, gfx::light(gfx::blue, selected ? 0.8f : 1.f).as_color_rgba()); + float r = std::get(physics.shape(ball_group, i)).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); @@ -236,11 +425,14 @@ struct physics_2d_app auto p = physics.group_static_state(box_group)[i].position; auto a = physics.group_static_state(box_group)[i].rotation; + float w = std::get(physics.shape(box_group, i)).width / 2.f; + float h = std::get(physics.shape(box_group, i)).height / 2.f; + geom::vector q[4]; - q[0] = {-box_width / 2.f, -box_height / 2.f}; - q[1] = { box_width / 2.f, -box_height / 2.f}; - q[2] = { box_width / 2.f, box_height / 2.f}; - q[3] = {-box_width / 2.f, box_height / 2.f}; + q[0] = {-w, -h}; + q[1] = { w, -h}; + q[2] = { w, h}; + q[3] = {-w, h}; auto m = geom::plane_rotation(0, 1, a).linear_matrix();