build octree async and reuse tree from last frame (disabled as it breaks physics)
This commit is contained in:
@ -1,67 +1,77 @@
|
||||
#include "mass_spring_system.hpp"
|
||||
#include "config.hpp"
|
||||
#include "util.hpp"
|
||||
|
||||
#include <cfloat>
|
||||
#include <cstring>
|
||||
|
||||
#ifdef TRACY
|
||||
#include <tracy/Tracy.hpp>
|
||||
#endif
|
||||
|
||||
auto mass_spring_system::mass::clear_force() -> void
|
||||
auto mass_spring_system::calculate_spring_force(const size_t s) -> void
|
||||
{
|
||||
force = Vector3Zero();
|
||||
const spring _s = springs[s];
|
||||
const Vector3 a_pos = positions[_s.a];
|
||||
const Vector3 b_pos = positions[_s.b];
|
||||
const Vector3 a_vel = velocities[_s.a];
|
||||
const Vector3 b_vel = velocities[_s.b];
|
||||
|
||||
const Vector3 delta_pos = a_pos - b_pos;
|
||||
const Vector3 delta_vel = a_vel - b_vel;
|
||||
|
||||
const float sq_len = Vector3DotProduct(delta_pos, delta_pos);
|
||||
const float inv_len = rsqrt(sq_len);
|
||||
const float len = sq_len * inv_len;
|
||||
|
||||
const float hooke = SPRING_CONSTANT * (len - REST_LENGTH);
|
||||
const float dampening = DAMPENING_CONSTANT * Vector3DotProduct(delta_vel, delta_pos) * inv_len;
|
||||
|
||||
const Vector3 a_force = Vector3Scale(delta_pos, -(hooke + dampening) * inv_len);
|
||||
const Vector3 b_force = a_force * -1.0f;
|
||||
|
||||
forces[_s.a] += a_force;
|
||||
forces[_s.b] += b_force;
|
||||
}
|
||||
|
||||
auto mass_spring_system::mass::calculate_velocity(const float delta_time) -> void
|
||||
auto mass_spring_system::integrate_velocity(const size_t m, const float dt) -> void
|
||||
{
|
||||
const Vector3 acceleration = Vector3Scale(force, 1.0 / MASS);
|
||||
const Vector3 temp = Vector3Scale(acceleration, delta_time);
|
||||
velocity = Vector3Add(velocity, temp);
|
||||
const Vector3 acc = forces[m] / MASS;
|
||||
velocities[m] += acc * dt;
|
||||
}
|
||||
|
||||
auto mass_spring_system::mass::calculate_position(const float delta_time) -> void
|
||||
auto mass_spring_system::integrate_position(const size_t m, const float dt) -> void
|
||||
{
|
||||
previous_position = position;
|
||||
|
||||
const Vector3 temp = Vector3Scale(velocity, delta_time);
|
||||
position = Vector3Add(position, temp);
|
||||
previous_positions[m] = positions[m];
|
||||
positions[m] += velocities[m] * dt;
|
||||
}
|
||||
|
||||
auto mass_spring_system::mass::verlet_update(const float delta_time) -> void
|
||||
auto mass_spring_system::verlet_update(const size_t m, const float dt) -> void
|
||||
{
|
||||
const Vector3 acceleration = Vector3Scale(force, 1.0 / MASS);
|
||||
const Vector3 temp_position = position;
|
||||
const Vector3 acc = (forces[m] / MASS) * dt * dt;
|
||||
const Vector3 pos = positions[m];
|
||||
|
||||
Vector3 displacement = Vector3Subtract(position, previous_position);
|
||||
const Vector3 accel_term = Vector3Scale(acceleration, delta_time * delta_time);
|
||||
Vector3 delta_pos = pos - previous_positions[m];
|
||||
delta_pos *= 1.0 - VERLET_DAMPENING; // Minimal dampening
|
||||
|
||||
// Minimal dampening
|
||||
displacement = Vector3Scale(displacement, 1.0 - VERLET_DAMPENING);
|
||||
|
||||
position = Vector3Add(Vector3Add(position, displacement), accel_term);
|
||||
previous_position = temp_position;
|
||||
positions[m] += delta_pos + acc;
|
||||
previous_positions[m] = pos;
|
||||
}
|
||||
|
||||
auto mass_spring_system::spring::calculate_spring_force(mass& _a, mass& _b) -> void
|
||||
#ifdef THREADPOOL
|
||||
auto mass_spring_system::set_mass_springs_pool_thread_name(size_t idx) -> void
|
||||
{
|
||||
// TODO: Use a bungee force here instead of springs, since we already have global repulsion?
|
||||
const Vector3 delta_position = Vector3Subtract(_a.position, _b.position);
|
||||
const float current_length = Vector3Length(delta_position);
|
||||
const Vector3 delta_velocity = Vector3Subtract(_a.velocity, _b.velocity);
|
||||
|
||||
const float hooke = SPRING_CONSTANT * (current_length - REST_LENGTH);
|
||||
const float dampening = DAMPENING_CONSTANT * Vector3DotProduct(delta_velocity, delta_position) / current_length;
|
||||
|
||||
const Vector3 force_a = Vector3Scale(delta_position, -(hooke + dampening) / current_length);
|
||||
const Vector3 force_b = Vector3Scale(force_a, -1.0);
|
||||
|
||||
_a.force = Vector3Add(_a.force, force_a);
|
||||
_b.force = Vector3Add(_b.force, force_b);
|
||||
BS::this_thread::set_os_thread_name(std::format("repulsion-{}", idx));
|
||||
traceln("Using thread \"{}\"", BS::this_thread::get_os_thread_name().value_or("INVALID NAME"));
|
||||
}
|
||||
#endif
|
||||
|
||||
auto mass_spring_system::clear() -> void
|
||||
{
|
||||
masses.clear();
|
||||
positions.clear();
|
||||
previous_positions.clear();
|
||||
velocities.clear();
|
||||
forces.clear();
|
||||
springs.clear();
|
||||
tree.nodes.clear();
|
||||
}
|
||||
@ -77,14 +87,17 @@ auto mass_spring_system::add_mass() -> void
|
||||
// };
|
||||
// position = Vector3Scale(Vector3Normalize(position), REST_LENGTH * 2.0);
|
||||
|
||||
masses.emplace_back(Vector3Zero());
|
||||
positions.emplace_back(Vector3Zero());
|
||||
previous_positions.emplace_back(Vector3Zero());
|
||||
velocities.emplace_back(Vector3Zero());
|
||||
forces.emplace_back(Vector3Zero());
|
||||
}
|
||||
|
||||
auto mass_spring_system::add_spring(size_t a, size_t b) -> void
|
||||
{
|
||||
// Update masses to be located along a random walk when adding the springs
|
||||
const mass& mass_a = masses.at(a);
|
||||
mass& mass_b = masses.at(b);
|
||||
const Vector3& mass_a = positions[a];
|
||||
const Vector3& mass_b = positions[b];
|
||||
|
||||
Vector3 offset{
|
||||
static_cast<float>(GetRandomValue(-100, 100)), static_cast<float>(GetRandomValue(-100, 100)),
|
||||
@ -94,7 +107,7 @@ auto mass_spring_system::add_spring(size_t a, size_t b) -> void
|
||||
|
||||
// If the offset moves the mass closer to the current center of mass, flip it
|
||||
if (!tree.nodes.empty()) {
|
||||
const Vector3 mass_center_direction = Vector3Subtract(mass_a.position, tree.nodes.at(0).mass_center);
|
||||
const Vector3 mass_center_direction = Vector3Subtract(positions[a], tree.nodes[0].mass_center);
|
||||
const float mass_center_distance = Vector3Length(mass_center_direction);
|
||||
|
||||
if (mass_center_distance > 0 && Vector3DotProduct(offset, mass_center_direction) < 0.0f) {
|
||||
@ -102,8 +115,8 @@ auto mass_spring_system::add_spring(size_t a, size_t b) -> void
|
||||
}
|
||||
}
|
||||
|
||||
mass_b.position = mass_a.position + offset;
|
||||
mass_b.previous_position = mass_b.position;
|
||||
positions[b] = mass_a + offset;
|
||||
previous_positions[b] = mass_b;
|
||||
|
||||
// infoln("Adding spring: ({}, {}, {})->({}, {}, {})", mass_a.position.x, mass_a.position.y,
|
||||
// mass_a.position.z,
|
||||
@ -118,9 +131,7 @@ auto mass_spring_system::clear_forces() -> void
|
||||
ZoneScoped;
|
||||
#endif
|
||||
|
||||
for (auto& m : masses) {
|
||||
m.clear_force();
|
||||
}
|
||||
memset(forces.data(), 0, forces.size() * sizeof(Vector3));
|
||||
}
|
||||
|
||||
auto mass_spring_system::calculate_spring_forces() -> void
|
||||
@ -129,56 +140,18 @@ auto mass_spring_system::calculate_spring_forces() -> void
|
||||
ZoneScoped;
|
||||
#endif
|
||||
|
||||
for (const auto s : springs) {
|
||||
mass& a = masses.at(s.a);
|
||||
mass& b = masses.at(s.b);
|
||||
spring::calculate_spring_force(a, b);
|
||||
const auto solve_spring_force = [&](const int i)
|
||||
{
|
||||
calculate_spring_force(i);
|
||||
};
|
||||
|
||||
#ifdef THREADPOOL
|
||||
threads.submit_loop(0, springs.size(), solve_spring_force, SMALL_TASK_BLOCK_SIZE).wait();
|
||||
#else
|
||||
for (size_t i = 0; i < springs.size(); ++i) {
|
||||
solve_spring_force(i);
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef THREADPOOL
|
||||
auto mass_spring_system::set_thread_name(size_t idx) -> void
|
||||
{
|
||||
BS::this_thread::set_os_thread_name(std::format("bh-worker-{}", idx));
|
||||
}
|
||||
#endif
|
||||
|
||||
auto mass_spring_system::build_octree() -> void
|
||||
{
|
||||
#ifdef TRACY
|
||||
ZoneScoped;
|
||||
#endif
|
||||
|
||||
tree.nodes.clear();
|
||||
tree.nodes.reserve(masses.size() * 2);
|
||||
|
||||
// Compute bounding box around all masses
|
||||
Vector3 min{FLT_MAX, FLT_MAX, FLT_MAX};
|
||||
Vector3 max{-FLT_MAX, -FLT_MAX, -FLT_MAX};
|
||||
for (const auto& m : masses) {
|
||||
min.x = std::min(min.x, m.position.x);
|
||||
max.x = std::max(max.x, m.position.x);
|
||||
min.y = std::min(min.y, m.position.y);
|
||||
max.y = std::max(max.y, m.position.y);
|
||||
min.z = std::min(min.z, m.position.z);
|
||||
max.z = std::max(max.z, m.position.z);
|
||||
}
|
||||
|
||||
// Pad the bounding box
|
||||
constexpr float pad = 1.0;
|
||||
min = Vector3Subtract(min, Vector3Scale(Vector3One(), pad));
|
||||
max = Vector3Add(max, Vector3Scale(Vector3One(), pad));
|
||||
|
||||
// Make it cubic (so subdivisions are balanced)
|
||||
const float max_extent = std::max({max.x - min.x, max.y - min.y, max.z - min.z});
|
||||
max = Vector3Add(min, Vector3Scale(Vector3One(), max_extent));
|
||||
|
||||
// Root node spans the entire area
|
||||
const int root = tree.create_empty_leaf(min, max);
|
||||
|
||||
for (size_t i = 0; i < masses.size(); ++i) {
|
||||
tree.insert(root, static_cast<int>(i), masses[i].position, MASS, 0);
|
||||
}
|
||||
}
|
||||
|
||||
auto mass_spring_system::calculate_repulsion_forces() -> void
|
||||
@ -187,45 +160,60 @@ auto mass_spring_system::calculate_repulsion_forces() -> void
|
||||
ZoneScoped;
|
||||
#endif
|
||||
|
||||
build_octree();
|
||||
|
||||
auto solve_octree = [&](const int i)
|
||||
const auto solve_octree = [&](const int i)
|
||||
{
|
||||
const Vector3 force = tree.calculate_force(0, masses[i].position);
|
||||
masses[i].force = Vector3Add(masses[i].force, force);
|
||||
const Vector3 force = tree.calculate_force(0, positions[i]);
|
||||
forces[i] += force;
|
||||
};
|
||||
|
||||
// Calculate forces using Barnes-Hut
|
||||
#ifdef THREADPOOL
|
||||
const BS::multi_future<void> loop_future = threads.submit_loop(0, masses.size(), solve_octree, 256);
|
||||
loop_future.wait();
|
||||
threads.submit_loop(0, positions.size(), solve_octree, LARGE_TASK_BLOCK_SIZE).wait();
|
||||
#else
|
||||
for (size_t i = 0; i < masses.size(); ++i) {
|
||||
for (size_t i = 0; i < positions.size(); ++i) {
|
||||
solve_octree(i);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
auto mass_spring_system::verlet_update(const float delta_time) -> void
|
||||
auto mass_spring_system::update(const float dt) -> void
|
||||
{
|
||||
#ifdef TRACY
|
||||
ZoneScoped;
|
||||
#endif
|
||||
|
||||
for (auto& m : masses) {
|
||||
m.verlet_update(delta_time);
|
||||
const auto update = [&](const int i)
|
||||
{
|
||||
verlet_update(i, dt);
|
||||
};
|
||||
|
||||
#ifdef THREADPOOL
|
||||
threads.submit_loop(0, positions.size(), update, SMALL_TASK_BLOCK_SIZE).wait();
|
||||
#else
|
||||
for (size_t i = 0; i < positions.size(); ++i) {
|
||||
update(i);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
auto mass_spring_system::center_masses() -> void
|
||||
{
|
||||
Vector3 mean = Vector3Zero();
|
||||
for (const auto& m : masses) {
|
||||
mean += m.position;
|
||||
for (const Vector3& pos : positions) {
|
||||
mean += pos;
|
||||
}
|
||||
mean /= static_cast<float>(masses.size());
|
||||
mean /= static_cast<float>(positions.size());
|
||||
|
||||
for (auto& m : masses) {
|
||||
m.position -= mean;
|
||||
const auto center_mass = [&](const int i)
|
||||
{
|
||||
positions[i] -= mean;
|
||||
};
|
||||
|
||||
#ifdef THREADPOOL
|
||||
threads.submit_loop(0, positions.size(), center_mass, SMALL_TASK_BLOCK_SIZE).wait();
|
||||
#else
|
||||
for (size_t i = 0; i < positions.size(); ++i) {
|
||||
center_mass(i);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
Reference in New Issue
Block a user