lisyarus/psemek
1
0
Fork 1
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions
psemek/examples/gravity.cpp

1279 lines
34 KiB
C++
Raw Blame History

#include <psemek/app/application_base.hpp>
#include <psemek/app/default_application_factory.hpp>
#include <psemek/gfx/painter.hpp>
#include <psemek/gfx/gl.hpp>
#include <psemek/geom/scale.hpp>
#include <psemek/geom/camera.hpp>
#include <psemek/geom/constants.hpp>
#include <psemek/util/clock.hpp>
#include <psemek/util/to_string.hpp>
#include <psemek/prof/profiler.hpp>
#include <psemek/util/moving_average.hpp>
#include <psemek/log/log.hpp>
#include <random>
#include <set>
/*
No optimizations, 125:
[2020 Aug 29 09:54:27.348][ main][ info] Avg forces time: 0.000641577
[2020 Aug 29 09:54:27.348][ main][ info] Avg collision time: 0.000789326
No optimizations, 250:
[2020 Aug 29 09:49:48.476][ main][ info] Avg forces time: 0.00257929
[2020 Aug 29 09:49:48.476][ main][ info] Avg collision time: 0.00314664
No optimizations, 500:
[2020 Aug 29 09:44:02.200][ main][ info] Avg forces time: 0.00299429
[2020 Aug 29 09:44:02.200][ main][ info] Avg collision time: 0.00362124
No optimizations, 1000:
[2020 Aug 29 09:42:34.143][ main][ info] Avg forces time: 0.0072538
[2020 Aug 29 09:42:34.143][ main][ info] Avg collision time: 0.00873274
No optimizations, 2000:
[2020 Aug 29 09:44:35.334][ main][ info] Avg forces time: 0.029108
[2020 Aug 29 09:44:35.334][ main][ info] Avg collision time: 0.0345713
No optimizations, 4000:
[2020 Aug 29 09:50:15.647][ main][ info] Avg forces time: 0.118145
[2020 Aug 29 09:50:15.647][ main][ info] Avg collision time: 0.1402
*/
using namespace psemek;
struct particle
{
geom::point<float, 2> pos;
geom::vector<float, 2> vel;
float angle;
float angle_vel;
float radius;
float mass;
float density;
geom::vector<float, 2> delta_pos{0.f, 0.f};
geom::vector<float, 2> delta_vel{0.f, 0.f};
geom::point<float, 2> old_pos{0.f, 0.f};
geom::vector<float, 2> acc{0.f, 0.f};
float T = 0.f;
};
float const G = 50.f;
float const GG = 0*1000.f;
float const GC = 0*100000.f;
float const K = 10000.f;
float const FR = 0.99f;
float const dt = 0.01f;
float const world_size = 10000000.f;
geom::point world_center{0.f, 0.f};
int const SOLVE_ITERATIONS = 16;
float const BIAS = 1.f / 8.f;
struct myapp
: app::application_base
{
myapp(options const &, context const & context)
: rng_{std::random_device{}()}
{
gl::ClearColor(1.f, 1.f, 1.f, 1.f);
context.vsync(true);
std::uniform_real_distribution<float> d{-50.f, 50.f};
std::uniform_real_distribution<float> rr{0.5f, 2.f};
std::uniform_real_distribution<float> rden{0.25f, 1.f};
std::uniform_real_distribution<float> ra{0.f, 2.f * geom::pi};
float min_R = 0.f;
float max_R = 100.f;
std::uniform_real_distribution<float> rR{0.f, 1.f};
bool star = false;
if (star)
particles_.push_back({{0.f, 0.f}, {0.f, 0.f}, 0.f, 0.f, 10.f, geom::pi * 10000.f, 1.f});
// particles_.push_back({{-2.f, 0.f}, {0.f, 0.f}, 0.f, 0.f, 1.f, 1.f, 1.f});
// particles_.push_back({{ 2.f, 0.f}, {0.f, 0.f}, 0.f, 0.f, 1.f, 1.f, 1.f});
// particles_.push_back({{ 0.f, 3.f}, {0.f, 0.f}, 0.f, 0.f, 1.f, 1.f, 1.f});
// particles_.push_back({{ 0.f, -3.f}, {0.f, 0.f}, 0.f, 0.f, 1.f, 1.f, 1.f});
// float planet_R[] = {200.f, 300.f, 400.f};
// float planet_a[] = {0.f, geom::rad(120.f), geom::rad(240.f)};
// if(false)
for (int i = 0; i < 500; ++i)
{
geom::vector v{0.f, 0.f};
geom::point<float, 2> p;
float m;
float r;
float den;
while (true)
{
// p = {((i % 2) ? -200.f : 200.f) + d(rng_), d(rng_)};
r = rr(rng_);
den = rden(rng_);
m = geom::pi * r * r * den;
auto a = ra(rng_);
float R = std::sqrt(rR(rng_)) * (max_R - min_R) + min_R;
// if (std::uniform_int_distribution<int>(0, 1)(rng_) == 0)
// R += 200.f;
// auto pl = std::uniform_int_distribution<int>(0, std::size(planet_R) - 1)(rng_);
// a = planet_a[pl];
// R = planet_R[pl];
// R += std::uniform_real_distribution<float>(-10.f, 10.f)(rng_);
// a += geom::rad(std::uniform_real_distribution<float>(-2.f, 2.f)(rng_));
p =
{
R * std::cos(a),
R * std::sin(a),
};
// p += geom::vector{((i % 2) ? -1000.f : 1000.f), 0.f};
// v[0] = {(i % 2) ? 100.f : -100.f};
if (std::all_of(particles_.begin(), particles_.end(), [&](particle const & q){ return geom::distance(q.pos, p) > q.radius + r; }))
break;
}
particles_.push_back({p, v, 0.f, 0.f, r, m, den});
}
float total_M = 0.f;
if (star)
total_M += particles_[0].mass;
for (std::size_t i = star ? 1 : 0; i < particles_.size(); ++i)
total_M += particles_[i].mass / 2.f;
if(false)
for (std::size_t i = star ? 1 : 0; i < particles_.size(); ++i)
{
auto r = particles_[i].pos - geom::point{0.f, 0.f};
auto R = geom::length(r);
float V = std::sqrt(G * total_M / R);
particles_[i].vel = geom::ort(r / R) * V;
(void)V;
}
for (auto & p : particles_)
p.old_pos = p.pos;
}
void on_event(app::resize_event const & event) override
{
app::application_base::on_event(event);
gl::Viewport(0, 0, event.size[0], event.size[1]);
camera_ratio_ = static_cast<float>(event.size[0]) / event.size[1];
}
void on_event(app::mouse_wheel_event const & event) override
{
app::application_base::on_event(event);
camera_size_ *= std::pow(0.8f, event.delta);
}
void on_event(app::mouse_button_event const & event) override
{
if (event.button == app::mouse_button::left && event.down)
{
geom::scale<float, 2> const flip_y({1.f, -1.f});
float const scale = camera_size_ / state().size[1];
geom::point<int, 2> const screen_center { state().size[0] / 2,state().size[1] / 2 };
auto target = camera_center_ + flip_y(geom::cast<float>(state().mouse - screen_center) * scale);
force_target_ = target;
// geom::point<float, 2> pos{100.f, 0.f};
// geom::vector<float, 2> vel = geom::normalized(target - pos) * 40.f;
// particles_.push_back({pos, vel, 1.f, 1.f});
// for (auto & p : particles_)
// {
// auto r = p.pos - target;
// p.vel += 4000.f * r / geom::length_sqr(r) / p.mass;
// }
}
if (event.button == app::mouse_button::left && !event.down)
{
force_target_ = std::nullopt;
}
if (event.button == app::mouse_button::right && event.down)
{
camera_drag_ = state().mouse;
}
if (event.button == app::mouse_button::right && !event.down)
{
camera_drag_ = std::nullopt;
}
}
void on_event(app::mouse_move_event const & event) override
{
app::application_base::on_event(event);
if (camera_drag_)
{
geom::scale<float, 2> const flip_y({1.f, -1.f});
float const scale = camera_size_ / state().size[1];
camera_center_ += flip_y(geom::cast<float>(*camera_drag_ - event.position) * scale);
camera_drag_ = event.position;
}
if (force_target_)
{
geom::scale<float, 2> const flip_y({1.f, -1.f});
float const scale = camera_size_ / state().size[1];
geom::point<int, 2> const screen_center { state().size[0] / 2, state().size[1] / 2 };
auto target = camera_center_ + flip_y(geom::cast<float>(state().mouse - screen_center) * scale);
force_target_ = target;
}
}
void on_event(app::key_event const & event) override
{
app::application_base::on_event(event);
if (event.key == app::keycode::SPACE && event.down)
{
paused_ = !paused_;
}
if (event.key == app::keycode::C && event.down)
{
particles_.push_back({{200.f, 0.f}, {-5000.f, 0.f}, 0.f, 0.f, 1.f, 100.f, 1.f});
particles_.back().old_pos = particles_.back().pos;
}
}
void update() override
{
if (paused_)
return;
for (std::size_t step = 0; step < 1; ++step)
{
{
util::clock<> clock;
for (auto & p : particles_)
p.acc = {0.f, 0.f};
for (auto & p : particles_)
p.acc += geom::vector{0.f, -GG};
for (auto & p : particles_)
{
auto r = (p.pos - p.pos.zero());
p.acc -= GC * r / std::pow(1.f + geom::length(r), 3.f);
}
// for (std::size_t i = 0; i < particles_.size(); ++i)
// {
// log::info() << "Start: #" << i << " pos = " << std::setprecision(10) << particles_[i].pos << ", vel = " << particles_[i].vel
// << ", |vel| = " << geom::length(particles_[i].vel);
// }
for (std::size_t i = 0; i < particles_.size(); ++i)
{
for (std::size_t j = i + 1; j < particles_.size(); ++j)
{
auto const r = particles_[i].pos - particles_[j].pos;
// float const l = std::max(particles_[i].radius + particles_[j].radius, length(r));
float const l = length(r);
auto const f = G * particles_[i].mass * particles_[j].mass * r / std::pow(l, 3.f);
// log::info() << "Force: #" << i << "," << j << " = " << std::setprecision(10) << f;
particles_[i].acc -= f / particles_[i].mass;
particles_[j].acc += f / particles_[j].mass;
}
}
// Force-based collisions
// if (false)
for (std::size_t i = 0; i < particles_.size(); ++i)
{
for (std::size_t j = i + 1; j < particles_.size(); ++j)
{
auto const r = particles_[i].pos - particles_[j].pos;
float const l = length(r);
float R = particles_[i].radius + particles_[j].radius;
auto n = r / l;
if (l < R)
{
auto f = K * n * (R - l);
(void)n;
// auto const f = -2.f * G * particles_[i].mass * particles_[j].mass * r / std::pow(l, 3.f);
// f -= 2.f * G * particles_[i].mass * particles_[j].mass * r / std::pow(l, 3.f);
particles_[i].acc += f / particles_[i].mass;
particles_[j].acc -= f / particles_[j].mass;
}
// auto vij = particles_[i].vel - particles_[j].vel;
// auto vn = geom::dot(vij, n) * n;
// particles_[i].acc -= vn * particles_[j].mass / (particles_[i].mass + particles_[j].mass);
// particles_[j].acc += vn * particles_[i].mass / (particles_[i].mass + particles_[j].mass);
}
}
total_forces_ += clock.count();
}
// for (std::size_t i = 0; i < particles_.size(); ++i)
// {
// log::info() << "Force: #" << i << " = " << std::setprecision(10) << particles_[i].acc;
// }
if (state().key_down.contains(app::keycode::M))
{
for (auto & p : particles_)
p.vel *= std::exp(-100.f*dt);
}
if (force_target_ && false)
{
for (auto & p : particles_)
{
auto r = p.pos - *force_target_;
p.acc += 100000.f * r / geom::length_sqr(r) / p.mass;
}
}
// for (auto & p : particles_)
// {
// auto old = p.pos;
// p.pos += (p.pos - p.old_pos) + p.acc * dt * dt;
// p.old_pos = old;
// }
// for (std::size_t i = 0; i < particles_.size(); ++i)
// {
// log::info() << "Integrate: #" << i << " new pos = " << std::setprecision(10) << particles_[i].pos;
// }
if (false)
{
float s = 100.f;
world_center[0] += dt * std::uniform_real_distribution<float>(-s, s)(rng_);
world_center[1] += dt * std::uniform_real_distribution<float>(-s, s)(rng_);
}
if (force_target_)
world_center = *force_target_;
// if (false)
for (auto & p : particles_)
{
p.vel += p.acc * dt;
p.vel *= FR;
p.pos += p.vel * dt;
}
// Verlet
if (false)
for (auto & p : particles_)
{
auto old = p.pos;
p.pos += (p.pos - p.old_pos) + p.acc * dt * dt;
p.old_pos = old;
}
// New iterative algorithm
if (false)
{
std::vector<geom::point<float, 2>> old_pos(particles_.size());
for (std::size_t i = 0; i < particles_.size(); ++i)
old_pos[i] = particles_[i].pos;
struct collision
{
std::size_t i, j;
geom::vector<float, 2> n; // i -> j
};
std::vector<collision> collisions;
for (std::size_t i = 0; i < particles_.size(); ++i)
{
for (std::size_t j = i + 1; j < particles_.size(); ++j)
{
auto const r = particles_[i].pos - particles_[j].pos;
float const R = particles_[i].radius + particles_[j].radius;
float const l = length(r);
if (l < R)
collisions.push_back({i, j, - r / l});
}
}
for (std::size_t iteration = 0; iteration < 16; ++iteration)
{
for (auto const & c : collisions)
{
auto const r = particles_[c.j].pos - particles_[c.i].pos;
auto const R = particles_[c.j].radius + particles_[c.i].radius;
auto const l = geom::dot(r, c.n);
if (l < R)
{
auto const M = particles_[c.j].mass + particles_[c.i].mass;
auto const d = (l - R) * c.n / M;
particles_[c.i].pos += d * particles_[c.j].mass;
particles_[c.j].pos -= d * particles_[c.i].mass;
}
}
}
for (std::size_t i = 0; i < particles_.size(); ++i)
particles_[i].vel += (particles_[i].pos - old_pos[i]) / dt;
}
// First-event algorithm
if (false)
{
// std::vector<float> particle_time(particles_.size(), 0.f);
for (std::size_t i = 0; i < particles_.size(); ++i)
{
bool collided = false;
for (std::size_t j = 0; j < particles_.size(); ++j)
{
if (i == j) continue;
// auto dp = (particles_[i].pos - particles_[i].vel * particle_time[i]) - (particles_[j].pos - particles_[j].vel * particle_time[j]);
auto dp = particles_[i].pos - particles_[j].pos;
auto dv = particles_[i].vel - particles_[j].vel;
auto R = particles_[i].radius + particles_[j].radius;
float A = geom::dot(dv, dv);
float B = geom::dot(dp, dv);
float C = geom::dot(dp, dp) - geom::sqr(R);
if (B >= 0.f) return;
float collision_time;
if (C <= 0.f)
{
collision_time = 0.f;
}
else
{
float D = B * B - A * C;
if (D <= 0.f)
continue;
collision_time = C / (- B + std::sqrt(D));
}
if (collision_time >= dt)
continue;
collided = true;
particles_[i].pos += particles_[i].vel * collision_time;
// particles_[i].pos += particles_[i].vel * (e.t - particle_time[i]);
// particles_[j].pos += particles_[j].vel * (e.t - particle_time[j]);
// particle_time[i] = e.t;
// particle_time[j] = e.t;
auto n = geom::normalized(particles_[i].pos - particles_[j].pos);
float S = 0.5f * (1.f / particles_[i].mass + 1.f / particles_[j].mass);
float T = dot(n, particles_[i].vel - particles_[j].vel);
auto np = n * (- T / S);
np *= std::exp(-10.f * dt);
particles_[i].vel += np / particles_[i].mass;
particles_[j].vel -= np / particles_[j].mass;
break;
}
if (!collided)
particles_[i].pos += particles_[i].vel * dt;
}
}
// Event-based algorithm
if (false)
{
// log::info() << "===== ITERATION =====";
collisions_ = 0;
struct collision_event
{
std::uint32_t i0, i1;
float t;
bool erased = false;
};
struct comparator
{
bool operator()(collision_event const & e1, collision_event const & e2) const
{
return std::tie(e1.t, e1.i0, e1.i1) < std::tie(e2.t, e2.i0, e2.i1);
}
};
using events_container = std::set<collision_event, comparator>;
events_container events;
std::vector<events_container::node_type> erased_events;
std::vector<std::vector<events_container::iterator>> event_list(particles_.size());
std::vector<float> particle_time(particles_.size(), 0.f);
std::unordered_map<geom::vector<int, 2>, std::vector<std::size_t>> cells;
float time = 0.f;
auto check_collision = [&](std::size_t i, std::size_t j)
{
if (i > j)
std::swap(i, j);
// (pi + vi * (t - ti) - pj - vj * (t - tj))^2 = (ri+rj)^2
auto dp = (particles_[i].pos - particles_[i].vel * particle_time[i]) - (particles_[j].pos - particles_[j].vel * particle_time[j]);
auto dv = particles_[i].vel - particles_[j].vel;
auto R = particles_[i].radius + particles_[j].radius;
float A = geom::dot(dv, dv);
float B = geom::dot(dp, dv);
float C = geom::dot(dp, dp) - geom::sqr(R);
// log::info() << "DV = " << dv;
// log::info() << "DP = " << dp;
// log::info() << "B = " << B;
// if (C < 0.f)
// log::info() << "DEFINITELY A COLLISION!";
// if (B > 0.f) return;
// auto result = geom::solve_quadratic(A, B, C);
// if (!result) return;
// log::info() << result->first << " " << result->second;
// float min_time = 0.f;
// min_time = std::max(particle_time[i], particle_time[j]);
// std::optional<float> collision_time;
// if (result->first >= min_time && result->first <= dt)
// collision_time = result->first;
// else if (result->second >= min_time && result->second <= dt)
// collision_time = result->second;
// if (result->first >= 0.f && result->first <= dt)
// if (result->first <= dt)
// collision_time = result->first;
// collision_time = std::max(min_time, result->first);
// (void)min_time;
// if (!collision_time) return;
if (B >= -0.01f * geom::length(dv) * geom::length(dp)) return;
float collision_time;
if (C <= 0.f)
{
collision_time = time;
}
else
{
float D = B * B - A * C;
if (D <= 0.f)
return;
collision_time = C / (- B + std::sqrt(D));
}
if (collision_time >= dt) return;
auto dpn = (particles_[i].pos + particles_[i].vel * (collision_time - particle_time[i])) - (particles_[j].pos + particles_[j].vel * (collision_time - particle_time[j]));
if (geom::dot(dv, dpn) >= -0.01f * geom::length(dv) * geom::length(dpn)) return;
auto insert_result = events.insert(collision_event{i, j, collision_time});
if (insert_result.second)
{
event_list[i].push_back(insert_result.first);
event_list[j].push_back(insert_result.first);
}
};
auto erase = [&](events_container::iterator it)
{
if (!it->erased)
{
auto n = events.extract(it);
n.value().erased = true;
erased_events.push_back(std::move(n));
}
};
auto clear_collisions = [&](std::size_t i)
{
for (auto it : event_list[i])
erase(it);
event_list[i].clear();
};
auto foreach_cells = [&](std::size_t i, auto && callback)
{
geom::box<float, 2> bbox;
bbox |= particles_[i].pos - geom::vector{1.f, 1.f} * particles_[i].radius;
bbox |= particles_[i].pos + geom::vector{1.f, 1.f} * particles_[i].radius;
auto npos = particles_[i].pos + particles_[i].vel * dt;//(dt - particle_time[i]);
bbox |= npos - geom::vector{1.f, 1.f} * particles_[i].radius;
bbox |= npos + geom::vector{1.f, 1.f} * particles_[i].radius;
int xmin = std::floor(bbox[0].min);
int xmax = std::floor(bbox[0].max);
int ymin = std::floor(bbox[1].min);
int ymax = std::floor(bbox[1].max);
for (int x = xmin; x <= xmax; ++x)
{
for (int y = ymin; y <= ymax; ++y)
{
callback(cells[{x, y}]);
}
}
};
auto clear_cells = [&](std::size_t i)
{
foreach_cells(i, [i](auto & cell){
auto it = std::find(cell.begin(), cell.end(), i);
if (it != cell.end())
cell.erase(it);
});
};
auto fill_cells = [&](std::size_t i)
{
foreach_cells(i, [i](auto & cell){ cell.push_back(i); });
};
auto find_collisions = [&](std::size_t i)
{
foreach_cells(i, [&, i](auto & cell){
for (auto j : cell)
if (i != j) check_collision(i, j);
});
};
auto handle_collision = [&](collision_event const & e)
{
clear_cells(e.i0);
clear_cells(e.i1);
particles_[e.i0].pos += particles_[e.i0].vel * (e.t - particle_time[e.i0]);
particles_[e.i1].pos += particles_[e.i1].vel * (e.t - particle_time[e.i1]);
// log::info() << "Collision " << e.i0 << " " << e.i1 << " " << geom::distance(particles_[e.i0].pos, particles_[e.i1].pos);
particle_time[e.i0] = e.t;
particle_time[e.i1] = e.t;
auto n = geom::normalized(particles_[e.i0].pos - particles_[e.i1].pos);
float A = 0.5f * (1.f / particles_[e.i0].mass + 1.f / particles_[e.i1].mass);
float B = dot(n, particles_[e.i0].vel - particles_[e.i1].vel);
auto BB = B / geom::length(particles_[e.i0].vel - particles_[e.i1].vel);
(void)BB;
auto np = n * (- B / A);
np *= std::exp(-10.f * dt);
particles_[e.i0].vel += np / particles_[e.i0].mass;
particles_[e.i1].vel -= np / particles_[e.i1].mass;
fill_cells(e.i0);
fill_cells(e.i1);
};
// 0 - naive
// 1 - optimized
int algorithm = 1;
if (algorithm == 0)
{
collisions_ = 0;
while (time < dt)
{
particle_time.assign(particles_.size(), 0.f);
collision_event next{0, 0, std::numeric_limits<float>::infinity()};
for (std::size_t i = 0; i < particles_.size(); ++i)
{
for (std::size_t j = i + 1; j < particles_.size(); ++j)
{
auto rij = particles_[i].pos - particles_[j].pos;
auto vij = particles_[i].vel - particles_[j].vel;
auto R = particles_[i].radius + particles_[j].radius;
float B = geom::dot(rij, vij);
if (B >= 0.f) continue;
std::optional<collision_event> e;
float C = geom::dot(rij, rij) - geom::sqr(R);
if (C <= 0.f)
{
e = collision_event{i, j, 0.f};
}
else
{
float A = geom::dot(vij, vij);
float D = B * B - A * C;
if (D <= 0.f)
continue;
float t = C / (- B + std::sqrt(D));
e = collision_event{i, j, t};
}
if (e && e->t < next.t)
next = *e;
}
}
if (time + next.t < dt)
{
handle_collision(next);
++collisions_;
for (std::size_t i = 0; i < particles_.size(); ++i)
particles_[i].pos += particles_[i].vel * (next.t - particle_time[i]);
time += next.t;
}
else
break;
}
for (std::size_t i = 0; i < particles_.size(); ++i)
particles_[i].pos += particles_[i].vel * (dt - time);
}
else if (algorithm == 1)
{
for (std::size_t i = 0; i < particles_.size(); ++i)
fill_cells(i);
for (std::size_t i = 0; i < particles_.size(); ++i)
find_collisions(i);
while (!events.empty())
{
collision_event e = *events.begin();
time = e.t;
// if (e.t < time)
// {
// erase(events.begin());
// continue;
// }
// time = e.t;
// if (e.t < particle_time[e.i0])
// log::info() << e.t << " < " << particle_time[e.i0];
// if (e.t < particle_time[e.i1])
// log::info() << e.t << " < " << particle_time[e.i1];
handle_collision(e);
clear_collisions(e.i0);
clear_collisions(e.i1);
find_collisions(e.i0);
find_collisions(e.i1);
++collisions_;
}
for (std::size_t i = 0; i < particles_.size(); ++i)
{
particles_[i].pos += particles_[i].vel * (dt - particle_time[i]);
}
}
}
if (false)
{
util::clock<> clock;
for (std::size_t iteration = 0; iteration < 1; ++iteration)
{
for (auto & p : particles_)
{
auto r = p.pos - world_center;
auto l = geom::length(r);
if (l + p.radius > world_size)
{
auto n = r / l;
auto t = geom::ort(n);
auto vn = n * geom::dot(p.vel, n);
auto vt = t * geom::dot(p.vel + t * p.angle_vel * p.radius, t);
auto pt = - vt * (1.f - std::exp(- 10.f * dt));
float I = 0.5f * p.mass * geom::sqr(p.radius);
p.pos -= n * (l + p.radius - world_size);
p.vel -= 1.75f * vn;
p.vel += pt / p.mass;
p.angle_vel += geom::det(n * p.radius, pt) / I;
}
}
for (std::size_t i = 0; i < particles_.size(); ++i)
{
for (std::size_t j = i + 1; j < particles_.size(); ++j)
{
auto const r = particles_[i].pos - particles_[j].pos;
float const l = length(r);
float const R = particles_[i].radius + particles_[j].radius;
auto const n = r / l;
/*
if (l < R)
{
auto f = E * n * std::pow(l / R, -2.f);
particles_[i].vel += f * dt / particles_[i].mass;
particles_[j].vel -= f * dt / particles_[j].mass;
auto const vij = particles_[i].vel - particles_[j].vel;
auto dp = - K * dt * n * dot(n, vij) * 2.f / (1.f / particles_[i].mass + 1.f / particles_[j].mass);
float Ei0 = geom::length_sqr(particles_[i].vel) * particles_[i].mass * 0.5f;
particles_[i].vel += dp / particles_[i].mass;
float Ei1 = geom::length_sqr(particles_[i].vel) * particles_[i].mass * 0.5f;
particles_[i].T += Ei0 - Ei1;
float Ej0 = geom::length_sqr(particles_[j].vel) * particles_[j].mass * 0.5f;
particles_[j].vel -= dp / particles_[j].mass;
float Ej1 = geom::length_sqr(particles_[j].vel) * particles_[j].mass * 0.5f;
particles_[j].T += Ej0 - Ej1;
float dT = particles_[i].T - particles_[j].T;
particles_[i].T -= C * dT * dt;
particles_[j].T += C * dT * dt;
}
*/
auto const vij = particles_[i].vel - particles_[j].vel;
// merging collision
if (l < R && false)
{
particle p;
auto M = (particles_[i].mass + particles_[j].mass);
p.pos = particles_[i].pos + (particles_[j].pos - particles_[i].pos) * particles_[j].mass / M;
p.vel = (particles_[i].vel * particles_[i].mass + particles_[j].vel * particles_[j].mass) / M;
p.mass = M;
p.radius = std::sqrt(geom::sqr(particles_[i].radius) + geom::sqr(particles_[j].radius));
p.density = p.mass / (geom::pi * geom::sqr(p.radius));
particles_[i] = p;
particles_.erase(particles_.begin() + j);
break;
}
// inelastic collision
// if (l < R && dot(vij, n) < 0.f)
if (l < R && false)
{
float D = R - l;
float A = 0.5f * (1.f / particles_[i].mass + 1.f / particles_[j].mass);
float B = dot(n, vij);
auto np = n * (- B / A);
// np *= 0.5f + 0.5f * std::exp(- 0.f * dt);
auto p = particles_[j].pos + (particles_[j].radius - D / 2.f) * n;
auto ri = p - particles_[i].pos;
auto rj = p - particles_[j].pos;
auto t = geom::ort(n);
auto vri = geom::dot(t, particles_[i].vel + geom::ort(ri) * particles_[i].angle_vel);
auto vrj = geom::dot(t, particles_[j].vel + geom::ort(rj) * particles_[j].angle_vel);
auto vrij = vri - vrj;
auto tp = - t * vrij * (1.f - std::exp(- 10.f * dt));;
particles_[i].vel += (np + tp) / particles_[i].mass;
particles_[j].vel -= (np + tp) / particles_[j].mass;
float Ii = 0.5f * particles_[i].mass * geom::sqr(particles_[i].radius);
float Ij = 0.5f * particles_[j].mass * geom::sqr(particles_[j].radius);
auto ti = geom::det(ri, tp);
auto tj = geom::det(rj, -tp);
particles_[i].angle_vel += ti / Ii;
particles_[j].angle_vel += tj / Ij;
float ki = D * particles_[j].mass / (particles_[i].mass + particles_[j].mass);
float kj = - D * particles_[i].mass / (particles_[i].mass + particles_[j].mass);
particles_[i].pos += ki * n;
particles_[j].pos += kj * n;
}
// inelastic collision without rotation
if (l < R && dot(vij, n) < 0.f && false)
// if (l < R)
{
float D = R - l;
float elasticity = 0.25f;
float A = (1.f / particles_[i].mass + 1.f / particles_[j].mass);
float B = dot(n, vij);
auto np = n * (- B / A) * (1.f + elasticity);
particles_[i].vel += np / particles_[i].mass;
particles_[j].vel -= np / particles_[j].mass;
float ki = D * particles_[j].mass / (particles_[i].mass + particles_[j].mass);
float kj = - D * particles_[i].mass / (particles_[i].mass + particles_[j].mass);
particles_[i].pos += ki * n;
particles_[j].pos += kj * n;
}
// collision replaced by force
if (l < R && false)
{
auto f = 10000.f * n * (R - l);
particles_[i].vel += dt * f / particles_[i].mass;
particles_[j].vel -= dt * f / particles_[j].mass;
auto vn = geom::dot(vij, n) * n;
// vn *= 1.f - std::exp(- (1.f - l / R));
// vn *= 0.5f;
particles_[i].vel -= vn * particles_[j].mass / (particles_[i].mass + particles_[j].mass);
particles_[j].vel += vn * particles_[i].mass / (particles_[i].mass + particles_[j].mass);
}
// Verlet collision
if (l < R)
{
float D = (R - l) * 1.f;
float ki = D * particles_[j].mass / (particles_[i].mass + particles_[j].mass);
float kj = - D * particles_[i].mass / (particles_[i].mass + particles_[j].mass);
auto di = ki * n;
auto dj = kj * n;
particles_[i].pos += di;
particles_[j].pos += dj;
}
// impulse-based collision
if (l < R && dot(n, particles_[i].vel - particles_[j].vel) < 0.f && false)
{
float bias = 0.5f;
float constraint = l - R;
float reduced_mass = 1.f / (1.f / particles_[i].mass + 1.f / particles_[j].mass);
float lambda = (- geom::dot(n, particles_[i].vel - particles_[j].vel) - bias / dt * constraint) * reduced_mass;
auto impulse = lambda * n;
particles_[i].vel += impulse / particles_[i].mass;
particles_[j].vel -= impulse / particles_[j].mass;
}
}
}
for (auto & p : particles_)
{
p.vel += p.delta_vel;
p.pos += p.delta_pos;
p.delta_pos = {0.f, 0.f};
p.delta_vel = {0.f, 0.f};
}
}
total_collisions_ += clock.count();
}
// Verlet finalization
if(false)
for (auto & p : particles_)
p.vel = (p.pos - p.old_pos) / dt;
// Iterative impulse-based collision
if (false)
{
struct collision
{
std::size_t i, j;
geom::vector<float, 2> normal;
float value;
float impulse = 0.f;
};
std::vector<collision> collisions;
for (std::size_t i = 0; i < particles_.size(); ++i)
{
for (std::size_t j = i + 1; j < particles_.size(); ++j)
{
auto const r = particles_[i].pos - particles_[j].pos;
float const l = length(r);
float const R = particles_[i].radius + particles_[j].radius;
auto const n = r / l;
if (l < R)
{
collisions.push_back({i, j, n, l - R});
}
}
}
for (std::size_t iteration = 0; iteration < SOLVE_ITERATIONS; ++iteration)
{
for (auto & c : collisions)
{
float bias = BIAS;
float reduced_mass = 1.f / (1.f / particles_[c.i].mass + 1.f / particles_[c.j].mass);
auto dv = geom::dot(c.normal, particles_[c.i].vel - particles_[c.j].vel);
float elasticity = 0.75f;
float lambda = (- dv - bias / dt * c.value - dv * elasticity) * reduced_mass;
float new_impulse = std::max(0.f, c.impulse + lambda);
auto delta = new_impulse - c.impulse;
c.impulse = new_impulse;
particles_[c.i].vel += delta * c.normal / particles_[c.i].mass;
particles_[c.j].vel -= delta * c.normal / particles_[c.j].mass;
}
}
}
if (false)
for (auto & p : particles_)
{
p.pos += p.vel * dt;
}
float Ep = 0.f;
float Ek = 0.f;
for (std::size_t i = 0; i < particles_.size(); ++i)
{
Ek += geom::length_sqr(particles_[i].vel) * particles_[i].mass / 2.f;
for (std::size_t j = i + 1; j < particles_.size(); ++j)
{
Ep -= G * particles_[i].mass * particles_[j].mass / distance(particles_[i].pos, particles_[j].pos);
}
}
float omega = 0.f;
for (std::size_t i = 0; i < particles_.size(); ++i)
{
omega += geom::det(particles_[i].mass * particles_[i].vel, particles_[i].pos - geom::point{0.f, 0.f});
}
energy_ = Ek + Ep;
// log::info() << "Angular velocity: " << omega;
// log::info() << "Energy: " << Ek << " - " << (-Ep) << " = " << (Ek + Ep);
++frame_count_;
// std::reverse(particles_.begin(), particles_.end());
}
}
void present() override
{
gl::ClearColor(0.f, 0.f, 0.1f, 0.f);
gl::Clear(gl::COLOR_BUFFER_BIT);
painter_.circle(world_center, world_size, {255, 255, 255, 255}, 72);
// for (auto const & c : cells_)
// {
// float a = 0.25f;
// if (!c.second.empty())
// a = 0.5f;
// painter_.rect({{{c.first[0], c.first[0] + 1.f}, {c.first[1], c.first[1] + 1.f}}}, gfx::to_coloru8(gfx::color_4f{1.f, 0.f, 0.f, a}));
// }
for (auto & p : particles_)
{
// float c = 2.f / (std::exp(-p.T / 100000.f) + 1.f) - 1.f;
float c = 1.f - p.density;
auto x = static_cast<std::uint8_t>(c * 255.f);
float s = state().size[1] / camera_size_;
float r = std::max(p.radius * s, 1.5f) / s;
painter_.circle(p.pos, r, {x, x, x, 191});
// painter_.line(p.pos, p.pos + r * geom::direction(p.angle), r / 16.f, {255, 0, 0, 255}, false);
// painter_.circle(p.pos, r * 0.75f, {255, 255, 255, 255});
}
geom::orthographic_camera camera;
camera.box[0].min = camera_center_[0] - camera_size_ * camera_ratio_ / 2.f;
camera.box[0].max = camera_center_[0] + camera_size_ * camera_ratio_ / 2.f;
camera.box[1].min = camera_center_[1] - camera_size_ / 2.f;
camera.box[1].max = camera_center_[1] + camera_size_ / 2.f;
camera.box[2].min = -1.f;
camera.box[2].max = 1.f;
painter_.render(camera.transform());
float rotation = 0.f;
for (auto const & p : particles_)
rotation += geom::det(p.pos - p.pos.zero(), p.vel) * p.mass;
rotation_.push(rotation);
{
gfx::painter::text_options opts;
opts.scale = 3.f;
opts.c = {0, 0, 0, 255};
opts.x = gfx::painter::x_align::center;
opts.y = gfx::painter::y_align::top;
auto put = [&](geom::point<float, 2> const & pos, std::string const & str)
{
return;
painter_.text(pos, str, opts);
painter_.text(pos + geom::vector{1.f, 0.f}, str, opts);
};
put({state().size[0] / 2.f, 30.f}, util::to_string("ITERATIONS: ", SOLVE_ITERATIONS));
put({state().size[0] / 2.f, 60.f}, BIAS == 1.f ? "BIAS: 1" : util::to_string("BIAS: 1/", std::round(1.f / BIAS)));
// painter_.text({10.f, 10.f}, util::to_string("Angular velocity: ", rotation_.average()), opts);
// painter_.text({10.f, 30.f}, util::to_string("Collisions: ", collisions_, " = ", (collisions_ * 1.f / particles_.size() / particles_.size()), " N^2"), opts);
// painter_.text({10.f, 50.f}, util::to_string("Energy: ", energy_), opts);
}
painter_.render(geom::window_camera{state().size[0], state().size[1]}.transform());
}
~myapp()
{
log::info() << "Avg forces time: " << (total_forces_ / frame_count_);
log::info() << "Avg collision time: " << (total_collisions_ / frame_count_);
prof::dump();
}
private:
std::default_random_engine rng_;
std::optional<geom::point<float, 2>> force_target_;
geom::point<float, 2> camera_center_ { 0.f, 0.f };
float camera_size_ = 150.f;
float camera_ratio_ = 1.f;
std::optional<geom::point<int, 2>> camera_drag_;
gfx::painter painter_;
std::vector<particle> particles_;
int frame_count_ = 0;
float total_forces_ = 0.f;
float total_collisions_ = 0.f;
util::moving_average<float> rotation_{300};
int collisions_ = 0;
float energy_ = 0.f;
bool paused_ = true;
};
namespace psemek::app
{
std::unique_ptr<application::factory> make_application_factory()
{
return default_application_factory<myapp>({.name = "Gravity example"});
}
}
Reference in a new issue View git blame Copy permalink