Add physics example
This commit is contained in:
parent
42561ccadb
commit
b861cc60ff
1 changed files with 892 additions and 0 deletions
892
examples/physics.cpp
Normal file
892
examples/physics.cpp
Normal file
|
|
@ -0,0 +1,892 @@
|
|||
#include <psemek/app/app.hpp>
|
||||
#include <psemek/app/main.hpp>
|
||||
|
||||
#include <psemek/gfx/gl.hpp>
|
||||
#include <psemek/gfx/painter.hpp>
|
||||
|
||||
#include <psemek/geom/point.hpp>
|
||||
#include <psemek/geom/simplex.hpp>
|
||||
#include <psemek/geom/orthographic.hpp>
|
||||
#include <psemek/geom/rotation.hpp>
|
||||
#include <psemek/geom/camera.hpp>
|
||||
#include <psemek/geom/math.hpp>
|
||||
|
||||
#include <psemek/util/clock.hpp>
|
||||
#include <psemek/util/unused.hpp>
|
||||
#include <psemek/util/to_string.hpp>
|
||||
#include <psemek/util/statistics.hpp>
|
||||
|
||||
#include <vector>
|
||||
|
||||
using namespace psemek;
|
||||
|
||||
namespace psemek::util
|
||||
{
|
||||
|
||||
template <typename T, std::size_t Max>
|
||||
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<geom::point<float, 2>> points;
|
||||
std::vector<geom::vector<float, 2>> vels;
|
||||
std::vector<bool> movable;
|
||||
|
||||
std::vector<geom::segment<std::uint32_t>> sticks;
|
||||
std::vector<float> stick_length;
|
||||
std::vector<bool> 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<float, 2> screen_to_world(geom::point<float, 2> const & p) const;
|
||||
geom::point<float, 2> world_to_screen(geom::point<float, 2> const & p) const;
|
||||
|
||||
util::clock<std::chrono::duration<float>> frame_clock;
|
||||
float update_time_left = 0.f;
|
||||
|
||||
bool paused = false;
|
||||
|
||||
stick_model model;
|
||||
|
||||
gfx::painter painter;
|
||||
|
||||
geom::box<float, 2> view_region;
|
||||
|
||||
std::optional<std::size_t> closest_point;
|
||||
std::optional<std::size_t> closest_stick;
|
||||
std::optional<geom::vector<float, 2>> drag_delta;
|
||||
|
||||
std::optional<std::size_t> new_spring_start;
|
||||
|
||||
std::string text;
|
||||
|
||||
util::statistics<float> 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<float, 2>::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<float>(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<float>(*mouse());
|
||||
model.vels[*closest_point] = geom::vector<float, 2>::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<float, 2>::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<float>(*mouse());
|
||||
|
||||
std::size_t closest = 0;
|
||||
float distance = std::numeric_limits<float>::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<float>::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<geom::segment<std::uint32_t>> new_sticks;
|
||||
std::vector<float> new_stick_length;
|
||||
std::vector<bool> 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<float>(*mouse())));
|
||||
model.vels.push_back(geom::vector<float, 2>::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<float>(*mouse())));
|
||||
model.vels.push_back(geom::vector<float, 2>::zero());
|
||||
model.movable.push_back(false);
|
||||
}
|
||||
}
|
||||
else if (key == SDLK_n)
|
||||
{
|
||||
if (mouse())
|
||||
{
|
||||
model.points.push_back(screen_to_world(geom::cast<float>(*mouse())));
|
||||
model.vels.push_back(geom::vector<float, 2>::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<float>(*mouse()));
|
||||
|
||||
model.points.push_back(o);
|
||||
model.vels.push_back(geom::vector<float, 2>::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<float, 2>::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<float>(*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<float, 2>::zero());
|
||||
model.vels.push_back(geom::vector<float, 2>::zero());
|
||||
model.vels.push_back(geom::vector<float, 2>::zero());
|
||||
model.vels.push_back(geom::vector<float, 2>::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<float, 2> 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<util::fixed_vector<std::array<int, 2>, 8>> ball_cells(model.points.size());
|
||||
|
||||
util::array<util::fixed_vector<std::uint32_t, 16>, 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<int>(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<int>(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<std::size_t> fixed;
|
||||
|
||||
if (closest_point && drag_delta && mouse())
|
||||
{
|
||||
auto target = screen_to_world(geom::cast<float>(*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<geom::vector<float, 2>> force(model.points.size(), geom::vector<float, 2>::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<geom::vector<float, 2>> impulse(model.points.size(), geom::vector<float, 2>::zero());
|
||||
std::vector<geom::vector<float, 2>> offset(model.points.size(), geom::vector<float, 2>::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<float, 2> 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<std::uint32_t> 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<float, 2> off = (r < 0.f ? -1.f : 1.f) * n * (ball_radius - std::abs(r));
|
||||
unused(off);
|
||||
|
||||
geom::vector<float, 2> 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<float>(*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<float, 3>(geom::box<float, 3>{{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<float, 2> physics_demo_app::screen_to_world(geom::point<float, 2> const & p) const
|
||||
{
|
||||
return geom::orthographic<float, 2>(view_region).inverse()(geom::point{2.f * p[0] / width() - 1.f, 1.f - 2.f * p[1] / height()});
|
||||
}
|
||||
|
||||
geom::point<float, 2> physics_demo_app::world_to_screen(geom::point<float, 2> const & p) const
|
||||
{
|
||||
auto q = geom::orthographic<float, 2>(view_region)(p);
|
||||
|
||||
return {(q[0] + 1.f) / 2.f * width(), (1.f - q[1]) / 2.f * height()};
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
return app::main<physics_demo_app>();
|
||||
}
|
||||
Loading…
Add table
Reference in a new issue