diff --git a/examples/physics.cpp b/examples/physics.cpp new file mode 100644 index 00000000..f9ddb714 --- /dev/null +++ b/examples/physics.cpp @@ -0,0 +1,892 @@ +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +using namespace psemek; + +namespace psemek::util +{ + + template + struct fixed_vector + { + T * begin() { return data_; } + T * end() { return data_ + size_; } + + T const * begin() const { return data_; } + T const * end() const { return data_ + size_; } + + void push_back(T const & value) + { + assert(size_ < Max); + data_[size_++] = value; + } + + private: + T data_[Max]; + std::size_t size_ = 0; + }; + +} + +struct stick_model +{ + std::vector> points; + std::vector> vels; + std::vector movable; + + std::vector> sticks; + std::vector stick_length; + std::vector stick_solid; + + void add_stick(std::size_t i, std::size_t j, bool solid = true); +}; + +void stick_model::add_stick(std::size_t i, std::size_t j, bool solid) +{ + sticks.push_back({i, j}); + stick_length.push_back(geom::distance(points[i], points[j])); + stick_solid.push_back(solid); +} + +static const float ball_radius = 0.1f; + +struct physics_demo_app + : app::app +{ + physics_demo_app(); + ~physics_demo_app(); + + void on_resize(int width, int height) override; + + void on_left_button_down() override; + void on_left_button_up() override; + + void on_right_button_down() override; + + void on_mouse_move(int x, int y, int, int) override; + + void on_key_down(SDL_Keycode key) override; + + void on_mouse_wheel(int delta) override; + + void update() override; + void render() override; + + geom::point screen_to_world(geom::point const & p) const; + geom::point world_to_screen(geom::point const & p) const; + + util::clock> frame_clock; + float update_time_left = 0.f; + + bool paused = false; + + stick_model model; + + gfx::painter painter; + + geom::box view_region; + + std::optional closest_point; + std::optional closest_stick; + std::optional> drag_delta; + + std::optional new_spring_start; + + std::string text; + + util::statistics physics_update_stats; +}; + +physics_demo_app::physics_demo_app() + : app("Physics Demo", 4) +{ + view_region[0] = {0.f, 0.f}; + view_region[1] = {-2.f, 5.f}; + + int preset = 5; + + if (preset == 0) + { + int const N = 10; + + for (int i = 0; i <= N; ++i) + { + model.points.push_back({0.f, i * 0.5f}); + + if (i > 0) + model.add_stick(i - 1, i); + } + } + else if (preset == 1) + { + int N = 10; + + for (int i = 0; i <= N; ++i) + { + float t = 1.f - (i * 1.f) / N; + + model.points.push_back({-(0.25f + 0.f * t), i * 0.5f}); + model.points.push_back({ (0.25f + 0.f * t), i * 0.5f}); + + model.add_stick(2 * i + 0, 2 * i + 1); + + if (i > 0) + { + model.add_stick(2 * i - 2, 2 * i + 0); + model.add_stick(2 * i - 1, 2 * i + 1); + model.add_stick(2 * i - 2, 2 * i + 1); + model.add_stick(2 * i - 1, 2 * i + 0); + } + } + } + else if (preset == 2) + { + int N = 12; + + float y = 1.f; + + model.points.push_back({0.f, y}); + + for (int i = 0; i < N; ++i) + { + float a = (2.f * geom::pi * i) / N; + model.points.push_back({std::cos(a) * 0.5f, y + std::sin(a) * 0.5f}); + } + + for (int i = 0; i < N; ++i) + { + model.add_stick(0, i + 1); + model.add_stick(i + 1, 1 + ((i + 1) % N)); + } + } + else if (preset == 3) + { + model.points.push_back({-5.f, -1.f}); + model.points.push_back({-4.f, -1.f}); + model.points.push_back({-5.f, 3.f}); + model.points.push_back({-4.f, 3.f}); + model.points.push_back({-4.5f, 2.8f}); + + model.add_stick(0, 1); + model.add_stick(0, 2); + model.add_stick(0, 3); + model.add_stick(1, 2); + model.add_stick(1, 3); + model.add_stick(2, 3); + model.add_stick(2, 4); + model.add_stick(3, 4); + } + + model.vels.assign(model.points.size(), geom::vector::zero()); + model.movable.assign(model.points.size(), true); +} + +physics_demo_app::~physics_demo_app() +{ + log::info() << "Update: " << physics_update_stats; +} + +void physics_demo_app::on_resize(int width, int height) +{ + app::on_resize(width, height); + + float aspect_ratio = static_cast(width) / height; + + float xc = view_region[0].center(); + float yl = view_region[1].length(); + + view_region[0].min = xc - yl / 2.f * aspect_ratio; + view_region[0].max = xc + yl / 2.f * aspect_ratio; +} + +void physics_demo_app::on_left_button_down() +{ + app::on_left_button_down(); + + if (mouse() && closest_point) + { + drag_delta = world_to_screen(model.points[*closest_point]) - geom::cast(*mouse()); + model.vels[*closest_point] = geom::vector::zero(); + } +} + +void physics_demo_app::on_left_button_up() +{ + app::on_left_button_up(); + + drag_delta = std::nullopt; +} + +void physics_demo_app::on_right_button_down() +{ + if (new_spring_start) + { + new_spring_start = std::nullopt; + } + else if (closest_point) + { + model.movable[*closest_point] = !model.movable[*closest_point]; + model.vels[*closest_point] = geom::vector::zero(); + } +} + +void physics_demo_app::on_mouse_move(int x, int y, int dx, int dy) +{ + app::on_mouse_move(x, y, dx, dy); + + if (!drag_delta) + { + closest_point = std::nullopt; + closest_stick = std::nullopt; + + if (mouse()) + { + auto const m = geom::cast(*mouse()); + + std::size_t closest = 0; + float distance = std::numeric_limits::infinity(); + + for (std::size_t i = 0; i < model.points.size(); ++i) + { + auto const d = geom::distance(m, world_to_screen(model.points[i])); + if (d < distance) + { + distance = d; + closest = i; + } + } + + if (distance < 20.f) + { + closest_point = closest; + } + + if (!closest_point) + { + std::size_t closest = 0; + float distance = std::numeric_limits::infinity(); + + for (std::size_t i = 0; i < model.sticks.size(); ++i) + { + auto const d = geom::distance(m, geom::simplex{world_to_screen(model.points[model.sticks[i][0]]), world_to_screen(model.points[model.sticks[i][1]])}); + if (d < distance) + { + distance = d; + closest = i; + } + } + + if (distance < 20.f) + { + closest_stick = closest; + } + } + } + } +} + +void physics_demo_app::on_key_down(SDL_Keycode key) +{ + if (key == SDLK_SPACE) + { + paused = !paused; + } + else if (key == SDLK_x) + { + if (closest_point) + { + model.points.erase(model.points.begin() + *closest_point); + model.vels.erase(model.vels.begin() + *closest_point); + model.movable.erase(model.movable.begin() + *closest_point); + + std::vector> new_sticks; + std::vector new_stick_length; + std::vector new_stick_solid; + + for (std::size_t i = 0; i < model.sticks.size(); ++i) + { + if (model.sticks[i].points[0] == *closest_point) continue; + if (model.sticks[i].points[1] == *closest_point) continue; + + auto s = model.sticks[i]; + if (s[0] > *closest_point) --s[0]; + if (s[1] > *closest_point) --s[1]; + + new_sticks.push_back(s); + new_stick_length.push_back(model.stick_length[i]); + new_stick_solid.push_back(model.stick_solid[i]); + } + + model.sticks = std::move(new_sticks); + model.stick_length = std::move(new_stick_length); + model.stick_solid = std::move(new_stick_solid); + + closest_point = std::nullopt; + } + + if (closest_stick) + { + model.sticks.erase(model.sticks.begin() + *closest_stick); + model.stick_length.erase(model.stick_length.begin() + *closest_stick); + model.stick_solid.erase(model.stick_solid.begin() + *closest_stick); + closest_stick = std::nullopt; + } + } + else if (key == SDLK_c) + { + if (new_spring_start && closest_point) + { + if (new_spring_start != closest_point) + { + model.add_stick(*new_spring_start, *closest_point); + new_spring_start = std::nullopt; + } + } + else if (new_spring_start && mouse()) + { + model.points.push_back(screen_to_world(geom::cast(*mouse()))); + model.vels.push_back(geom::vector::zero()); + model.movable.push_back(false); + model.add_stick(*new_spring_start, model.points.size() - 1); + new_spring_start = model.points.size() - 1; + } + else if (closest_point) + { + new_spring_start = *closest_point; + closest_point = std::nullopt; + } + else if (mouse()) + { + model.points.push_back(screen_to_world(geom::cast(*mouse()))); + model.vels.push_back(geom::vector::zero()); + model.movable.push_back(false); + } + } + else if (key == SDLK_n) + { + if (mouse()) + { + model.points.push_back(screen_to_world(geom::cast(*mouse()))); + model.vels.push_back(geom::vector::zero()); + model.movable.push_back(true); + } + } + else if (key == SDLK_f) + { + if (closest_stick) + { + model.stick_solid[*closest_stick] = !model.stick_solid[*closest_stick]; + } + } + else if (key == SDLK_w) + { + if (mouse()) + { + std::uint32_t const base = model.points.size(); + + int N = 12; + + auto o = screen_to_world(geom::cast(*mouse())); + + model.points.push_back(o); + model.vels.push_back(geom::vector::zero()); + model.movable.push_back(true); + + float r = 0.5f; + + for (int i = 0; i < N; ++i) + { + float a = (2.f * geom::pi * i) / N; + model.points.push_back({o[0] + std::cos(a) * r, o[1] + std::sin(a) * r}); + model.vels.push_back(geom::vector::zero()); + model.movable.push_back(true); + } + + for (int i = 0; i < N; ++i) + { + model.add_stick(base, base + i + 1, false); + model.add_stick(base + i + 1, base + 1 + ((i + 1) % N)); + } + } + } + else if (key == SDLK_b) + { + std::uint32_t const base = model.points.size(); + + auto o = screen_to_world(geom::cast(*mouse())); + + float w = 0.25f; + + model.points.push_back({o[0] - w, o[1] - w}); + model.points.push_back({o[0] + w, o[1] - w}); + model.points.push_back({o[0] - w, o[1] + w}); + model.points.push_back({o[0] + w, o[1] + w}); + + model.vels.push_back(geom::vector::zero()); + model.vels.push_back(geom::vector::zero()); + model.vels.push_back(geom::vector::zero()); + model.vels.push_back(geom::vector::zero()); + model.movable.push_back(true); + model.movable.push_back(true); + model.movable.push_back(true); + model.movable.push_back(true); + + model.add_stick(base + 0, base + 1); + model.add_stick(base + 0, base + 2); + model.add_stick(base + 0, base + 3, false); + model.add_stick(base + 1, base + 2, false); + model.add_stick(base + 1, base + 3); + model.add_stick(base + 2, base + 3); + } +} + +void physics_demo_app::on_mouse_wheel(int delta) +{ + if (closest_stick) + { + auto & l = model.stick_length[*closest_stick]; + + l += delta * ball_radius / 2.f; + + l = std::max(3.f * ball_radius, l); + } +} + +void physics_demo_app::update() +{ + if (paused) + { + frame_clock.restart(); + return; + } + + float const dt = 0.001f; + + float const spring_constant = 10000.f; + float const spring_damping_constant = 200.f; + float const ground_friction = 100.f; + float const ground_mu = 0.3f; + float const ground_bounce = 0.5f; + float const ball_friction = 0.f; + float const ball_bounce = 0.5f; + float const stick_friction = 500.f; + float const stick_bounce = 0.9f; + + geom::vector const gravity {0.f, -10.f}; + + update_time_left += frame_clock.restart().count(); + + int update_steps = 0; + + while (update_time_left >= dt) + { + ++update_steps; + if (update_steps == 32) + { + log::warning() << "Can't keep up with physics, skipping " << std::round(update_time_left / dt) << " updates"; + update_time_left = 0.f; + break; + } + + util::clock<> timer; + + update_time_left -= dt; + + float const cell_size = ball_radius * 2.f; + + std::vector, 8>> ball_cells(model.points.size()); + + util::array, 2> cells({std::ceil(view_region[0].length() / cell_size), std::ceil(view_region[1].length() / cell_size)}); + + float dx = view_region[0].length() / cells.width(); + float dy = view_region[1].length() / cells.height(); + + for (std::size_t i = 0; i < model.points.size(); ++i) + { + int x = std::floor(cells.width() * (model.points[i][0] - view_region[0].min) / view_region[0].length()); + x = geom::clamp(x, {0, cells.width() - 1}); + + int y = std::floor(cells.height() * (model.points[i][1] - view_region[1].min) / view_region[1].length()); + y = geom::clamp(y, {0, cells.height() - 1}); + + float cx = view_region[0].min + dx * x; + float cy = view_region[1].min + dy * y; + + if (x > 0 && model.points[i][0] - ball_radius <= cx) + { + if (y > 0 && model.points[i][1] - ball_radius <= cy) + { + cells(x - 1, y - 1).push_back(i); + ball_cells[i].push_back({x - 1, y - 1}); + } + + cells(x - 1, y).push_back(i); + ball_cells[i].push_back({x - 1, y}); + + if (y + 1 < cells.height() && model.points[i][1] + ball_radius >= cy + dy) + { + cells(x - 1, y + 1).push_back(i); + ball_cells[i].push_back({x - 1, y + 1}); + } + } + + if (y > 0 && model.points[i][1] - ball_radius <= cy) + { + cells(x, y - 1).push_back(i); + ball_cells[i].push_back({x, y - 1}); + } + + cells(x, y).push_back(i); + ball_cells[i].push_back({x, y}); + + if (y + 1 < cells.height() && model.points[i][1] + ball_radius >= cy + dy) + { + cells(x, y + 1).push_back(i); + ball_cells[i].push_back({x, y + 1}); + } + + if (x + 1 < cells.width() && model.points[i][0] + ball_radius >= cx + dx) + { + if (y > 0 && model.points[i][1] - ball_radius <= cy) + { + cells(x + 1, y - 1).push_back(i); + ball_cells[i].push_back({x + 1, y - 1}); + } + + cells(x + 1, y).push_back(i); + ball_cells[i].push_back({x + 1, y}); + + if (y + 1 < cells.height() && model.points[i][1] + ball_radius >= cy + dy) + { + cells(x + 1, y + 1).push_back(i); + ball_cells[i].push_back({x + 1, y + 1}); + } + } + } + + std::optional fixed; + + if (closest_point && drag_delta && mouse()) + { + auto target = screen_to_world(geom::cast(*mouse()) + *drag_delta); + auto & point = model.points[*closest_point]; + + auto & vel = model.vels[*closest_point]; + + vel = vel * 0.9f + (target - point) * 0.1f / dt; + + point = target; + + fixed = closest_point; + } + + std::vector> force(model.points.size(), geom::vector::zero()); + + for (std::size_t i = 0; i < model.points.size(); ++i) + { + force[i] += gravity; + } + + for (std::size_t i = 0; i < model.sticks.size(); ++i) + { + auto const & s = model.sticks[i]; + auto d = model.points[s[1]] - model.points[s[0]]; + auto l = geom::length(d); + auto n = d / l; + + auto f = - spring_constant * (l - model.stick_length[i]) * n / 2.f; + + force[s[0]] -= f; + force[s[1]] += f; + } + + for (std::size_t i = 0; i < model.points.size(); ++i) + { + if (fixed && i == *fixed) continue; + + if (!model.movable[i]) continue; + + model.vels[i] += dt * force[i]; + } + + std::vector> impulse(model.points.size(), geom::vector::zero()); + std::vector> offset(model.points.size(), geom::vector::zero()); + + for (std::size_t i = 0; i < model.sticks.size(); ++i) + { + auto const & s = model.sticks[i]; + + auto rel = model.vels[s[1]] - model.vels[s[0]]; + auto n = geom::normalized(model.points[s[1]] - model.points[s[0]]); + + rel = n * geom::dot(rel, n); + + impulse[s[0]] += rel / 2.f * spring_damping_constant * dt; + impulse[s[1]] -= rel / 2.f * spring_damping_constant * dt; + } + + for (std::size_t i = 0; i < model.points.size(); ++i) + { + bool collision = false; + geom::vector n; + float d; + + if (model.points[i][0] < view_region[0].min + ball_radius && model.vels[i][0] < 0.f) + { + collision = true; + d = view_region[0].min + ball_radius - model.points[i][0]; + n = {1.f, 0.f}; + } + + if (model.points[i][0] > view_region[0].max - ball_radius && model.vels[i][0] > 0.f) + { + collision = true; + d = ball_radius + model.points[i][0] - view_region[0].max; + n = {-1.f, 0.f}; + } + + if (model.points[i][1] < view_region[1].min + ball_radius && model.vels[i][1] < 0.f) + { + collision = true; + d = view_region[1].min + ball_radius - model.points[i][1]; + n = {0.f, 1.f}; + } + + if (model.points[i][1] > view_region[1].max - ball_radius && model.vels[i][1] > 0.f) + { + collision = true; + d = ball_radius + model.points[i][1] - view_region[1].max; + n = {0.f, -1.f}; + } + + if (collision) + { + offset[i] += n * d; + (void)d; + + auto nv = n * geom::dot(n, model.vels[i]); + auto tv = model.vels[i] - nv; + + auto jn = - (1.f + ground_bounce) * nv; + + auto jt = - tv * ground_friction * dt; + + unused(ground_mu); + +// if (geom::length(jt) > ground_mu * geom::length(jn)) +// { +// jt = geom::normalized(jt) * ground_mu * geom::length(jn); +// } + + impulse[i] += jn + jt; + } + } + + std::vector checked; + checked.reserve(16); + for (std::size_t i = 0; i < model.points.size(); ++i) + { + checked.clear(); + +// for (std::size_t j = i + 1; j < model.points.size(); ++j) + for (auto c : ball_cells[i]) for (auto j : cells(c[0], c[1])) + { + if (i >= j) continue; + + if (std::find(checked.begin(), checked.end(), j) != checked.end()) continue; + + checked.push_back(j); + + auto r = geom::distance(model.points[i], model.points[j]); + + if (r < ball_radius * 2.f) + { + auto n = (model.points[j] - model.points[i]) / r; + auto rel = model.vels[j] - model.vels[i]; + + auto d = geom::dot(rel, n); + + if (d < 0.f) + { + auto nv = n * d; + auto tv = rel - nv; + + impulse[i] += nv * (1.f + ball_bounce) / 2.f; + impulse[j] -= nv * (1.f + ball_bounce) / 2.f; + + impulse[i] += tv * ball_friction * dt / 2.f; + impulse[j] -= tv * ball_friction * dt / 2.f; + + offset[i] -= n * (2.f * ball_radius - r) / 2.f / 2.f; + offset[j] += n * (2.f * ball_radius - r) / 2.f / 2.f; + } + } + } + } + + for (std::size_t i = 0; i < model.points.size(); ++i) + { + for (std::size_t j = 0; j < model.sticks.size(); ++j) + { + if (!model.stick_solid[j]) continue; + + auto const & s = model.sticks[j]; + + if (i == s[0] || i == s[1]) continue; + + auto d = geom::normalized(model.points[s[1]] - model.points[s[0]]); + + auto t = geom::dot(d, model.points[i] - model.points[s[0]]) / geom::distance(model.points[s[1]], model.points[s[0]]); + + if (t < 0.f || t > 1.f) continue; + + auto v = geom::lerp(model.vels[s[0]], model.vels[s[1]], t); + + auto rel = model.vels[i] - v; + + auto n = geom::ort(d); + + float r = geom::dot(n, model.points[i] - model.points[s[0]]); + + if (geom::dot(rel, n) * r >= 0.f) continue; + + auto nv = n * geom::dot(rel, n); + auto tv = rel - nv; + + unused(stick_friction); + unused(tv); + + if (std::abs(r) >= ball_radius) continue; + + geom::vector off = (r < 0.f ? -1.f : 1.f) * n * (ball_radius - std::abs(r)); + unused(off); + + geom::vector imp = - nv * (1.f + stick_bounce) - tv * stick_friction * dt; + + offset[i] += off * 2.f / 3.f; + offset[s[0]] -= off / 3.f * (1.f - t); + offset[s[1]] -= off / 3.f * t; + + impulse[i] += imp * 2.f / 3.f; + impulse[s[0]] -= imp / 3.f * (1.f - t); + impulse[s[1]] -= imp / 3.f * t; + } + } + + for (std::size_t i = 0; i < model.points.size(); ++i) + { + if (fixed && i == *fixed) continue; + + if (!model.movable[i]) continue; + + model.vels[i] += impulse[i]; + model.points[i] += dt * model.vels[i] + offset[i]; + } + + physics_update_stats.push(timer.count()); + } +} + +void physics_demo_app::render() +{ + gl::Viewport(0, 0, width(), height()); + + gl::ClearColor(0.8f, 0.8f, 0.9f, 0.f); + gl::Clear(gl::COLOR_BUFFER_BIT); + + gl::Enable(gl::BLEND); + gl::BlendFunc(gl::SRC_ALPHA, gl::ONE_MINUS_SRC_ALPHA); + + for (std::size_t i = 0; i < model.sticks.size(); ++i) + { + painter.line(model.points[model.sticks[i][0]], model.points[model.sticks[i][1]], model.stick_solid[i] ? ball_radius / 2.f : ball_radius / 4.f, + model.stick_solid[i] ? gfx::color_rgba{0, 0, 0, 255} : gfx::color_rgba{63, 63, 63, 255}); + } + + if (new_spring_start && mouse()) + { + painter.line(model.points[*new_spring_start], screen_to_world(geom::cast(*mouse())), ball_radius / 2.f, gfx::red); + } + + if (closest_stick) + { + painter.line(model.points[model.sticks[*closest_stick][0]], model.points[model.sticks[*closest_stick][1]], ball_radius / 2.f, gfx::red); + } + + for (std::size_t i = 0; i < model.points.size(); ++i) + { + painter.circle(model.points[i], ball_radius, model.movable[i] ? gfx::black : gfx::blue); + } + + if (closest_point) + { + painter.circle(model.points[*closest_point], ball_radius, gfx::red); + } + + painter.render(geom::orthographic(geom::box{{view_region[0], view_region[1], {-1.f, 1.f}}}).homogeneous_matrix()); + + text = util::to_string("Balls: ", model.points.size(), "\n", text); + + { + gfx::painter::text_options opts; + opts.x = gfx::painter::x_align::left; + opts.y = gfx::painter::y_align::top; + opts.c = gfx::black; + opts.f = gfx::painter::font::font_9x12; + opts.scale = 2.f; + + float y = 10.f; + for (std::size_t i = 0;;) + { + std::size_t j = text.find_first_of('\n', i); + + if (j > i) + { + painter.text({10.f, y}, std::string_view(text).substr(i, j == std::string::npos ? std::string::npos : j - i), opts); + } + + if (j == std::string::npos) break; + + i = j + 1; + y += 24.f; + } + } + + text.clear(); + + painter.render(geom::window_camera{width(), height()}.transform()); +} + +geom::point physics_demo_app::screen_to_world(geom::point const & p) const +{ + return geom::orthographic(view_region).inverse()(geom::point{2.f * p[0] / width() - 1.f, 1.f - 2.f * p[1] / height()}); +} + +geom::point physics_demo_app::world_to_screen(geom::point const & p) const +{ + auto q = geom::orthographic(view_region)(p); + + return {(q[0] + 1.f) / 2.f * width(), (1.f - q[1]) / 2.f * height()}; +} + +int main() +{ + return app::main(); +}