3D physics engine wip
This commit is contained in:
parent
141c707562
commit
a2323884af
3 changed files with 1207 additions and 0 deletions
364
examples/physics_3d.cpp
Normal file
364
examples/physics_3d.cpp
Normal file
|
|
@ -0,0 +1,364 @@
|
|||
#include <psemek/app/app.hpp>
|
||||
#include <psemek/app/main.hpp>
|
||||
|
||||
#include <psemek/phys/engine_3d.hpp>
|
||||
|
||||
#include <psemek/gfx/program.hpp>
|
||||
#include <psemek/gfx/mesh.hpp>
|
||||
|
||||
#include <psemek/cg/body/icosahedron.hpp>
|
||||
#include <psemek/cg/body/box.hpp>
|
||||
|
||||
#include <psemek/geom/camera.hpp>
|
||||
#include <psemek/geom/mesh.hpp>
|
||||
#include <psemek/geom/rotation.hpp>
|
||||
#include <psemek/geom/translation.hpp>
|
||||
#include <psemek/geom/scale.hpp>
|
||||
|
||||
#include <psemek/random/device.hpp>
|
||||
#include <psemek/random/generator.hpp>
|
||||
#include <psemek/random/uniform.hpp>
|
||||
#include <psemek/random/uniform_sphere.hpp>
|
||||
#include <psemek/random/uniform_ball.hpp>
|
||||
|
||||
#include <psemek/util/clock.hpp>
|
||||
|
||||
using namespace psemek;
|
||||
|
||||
static char const program_vs[] =
|
||||
R"(#version 330
|
||||
|
||||
uniform mat4 u_camera_transform;
|
||||
uniform mat4 u_object_transform;
|
||||
|
||||
layout (location = 0) in vec4 in_position;
|
||||
layout (location = 1) in vec3 in_normal;
|
||||
|
||||
out vec3 position;
|
||||
out vec3 normal;
|
||||
out vec3 test_position;
|
||||
|
||||
void main()
|
||||
{
|
||||
test_position = in_position.xyz;
|
||||
|
||||
vec4 p = u_object_transform * in_position;
|
||||
position = p.xyz;
|
||||
gl_Position = u_camera_transform * p;
|
||||
|
||||
normal = (u_object_transform * vec4(in_normal, 0.0)).xyz;
|
||||
}
|
||||
)";
|
||||
|
||||
static char const program_fs[] =
|
||||
R"(#version 330
|
||||
|
||||
uniform vec3 u_light_direction;
|
||||
uniform vec3 u_light_color;
|
||||
|
||||
uniform vec3 u_object_color;
|
||||
uniform int u_grid;
|
||||
|
||||
layout (location = 0) out vec4 out_color;
|
||||
|
||||
in vec3 position;
|
||||
in vec3 normal;
|
||||
in vec3 test_position;
|
||||
|
||||
float mmin(vec3 v)
|
||||
{
|
||||
return min(v.x, min(v.y, v.z));
|
||||
}
|
||||
|
||||
void main()
|
||||
{
|
||||
float lit = dot(normalize(normal), u_light_direction) * 0.5 + 0.5;
|
||||
|
||||
vec3 object_color = u_object_color;
|
||||
if (u_grid == 1)
|
||||
object_color = mix(vec3(1.0), u_object_color, smoothstep(0.0, 0.05, mmin(abs(test_position))));
|
||||
|
||||
vec3 color = u_light_color * object_color * lit;
|
||||
color = pow(color, vec3(1.0 / 2.2));
|
||||
out_color = vec4(color, 1.0);
|
||||
}
|
||||
)";
|
||||
|
||||
struct physics_3d_app
|
||||
: app::app
|
||||
{
|
||||
physics_3d_app()
|
||||
: app::app("Physics 3D", 4)
|
||||
, program_(program_vs, program_fs)
|
||||
, rng_{random::device{}}
|
||||
{
|
||||
camera_.near_clip = 0.1f;
|
||||
camera_.far_clip = 1000.f;
|
||||
camera_.fov_y = geom::rad(90.f);
|
||||
camera_.fov_x = camera_.fov_y;
|
||||
camera_.target = {0.f, 0.f, 0.f};
|
||||
camera_.distance = 10.f;
|
||||
camera_.elevation_angle = geom::rad(45.f);
|
||||
camera_.azimuthal_angle = geom::rad(30.f);
|
||||
|
||||
camera_distance_tgt_ = camera_.distance;
|
||||
camera_azimuthal_angle_tgt_ = camera_.azimuthal_angle;
|
||||
camera_elevation_angle_tgt_ = camera_.elevation_angle;
|
||||
|
||||
struct vertex
|
||||
{
|
||||
geom::point<float, 3> position;
|
||||
geom::vector<float, 3> normal;
|
||||
|
||||
static void setup(gfx::mesh & m)
|
||||
{
|
||||
m.setup<geom::point<float, 3>, geom::vector<float, 3>>();
|
||||
}
|
||||
};
|
||||
|
||||
{
|
||||
std::vector<vertex> vertices;
|
||||
vertices.push_back({{-10.f, -10.f, 0.f}, {0.f, 0.f, 1.f}});
|
||||
vertices.push_back({{ 10.f, -10.f, 0.f}, {0.f, 0.f, 1.f}});
|
||||
vertices.push_back({{-10.f, 10.f, 0.f}, {0.f, 0.f, 1.f}});
|
||||
vertices.push_back({{-10.f, 10.f, 0.f}, {0.f, 0.f, 1.f}});
|
||||
vertices.push_back({{ 10.f, -10.f, 0.f}, {0.f, 0.f, 1.f}});
|
||||
vertices.push_back({{ 10.f, 10.f, 0.f}, {0.f, 0.f, 1.f}});
|
||||
|
||||
vertex::setup(plane_mesh_);
|
||||
plane_mesh_.load(vertices, gl::TRIANGLES, gl::STATIC_DRAW);
|
||||
}
|
||||
|
||||
{
|
||||
cg::icosahedron<float> body{{0.f, 0.f, 0.f}, 1.f};
|
||||
|
||||
auto const & body_vertices = cg::vertices(body);
|
||||
auto const & body_triangles = cg::triangles(body);
|
||||
|
||||
std::vector<geom::point<float, 3>> positions;
|
||||
std::copy(body_vertices.begin(), body_vertices.end(), std::back_inserter(positions));
|
||||
|
||||
std::vector<geom::triangle<std::uint32_t>> triangles;
|
||||
for (auto const & t : body_triangles)
|
||||
triangles.push_back({t[0], t[1], t[2]});
|
||||
|
||||
for (int iterations = 0; iterations < 2; ++iterations)
|
||||
{
|
||||
geom::subdivide(positions, triangles);
|
||||
for (auto & p : positions)
|
||||
p = p.zero() + geom::normalized(p - p.zero());
|
||||
}
|
||||
|
||||
auto normals = geom::smooth_normals(positions, triangles);
|
||||
|
||||
|
||||
std::vector<vertex> vertices;
|
||||
for (std::size_t i = 0; i < positions.size(); ++i)
|
||||
vertices.push_back({positions[i], normals[i]});
|
||||
|
||||
vertex::setup(sphere_mesh_);
|
||||
sphere_mesh_.load(vertices, triangles, gl::STATIC_DRAW);
|
||||
}
|
||||
|
||||
{
|
||||
cg::box<float, 3> body{{{{-1.f, 1.f}, {-1.f, 1.f}, {-1.f, 1.f}}}};
|
||||
|
||||
auto const & body_vertices = cg::vertices(body);
|
||||
auto const & body_triangles = cg::triangles(body);
|
||||
|
||||
std::vector<geom::point<float, 3>> positions;
|
||||
std::copy(body_vertices.begin(), body_vertices.end(), std::back_inserter(positions));
|
||||
|
||||
std::vector<geom::triangle<std::uint32_t>> triangles;
|
||||
for (auto const & t : body_triangles)
|
||||
triangles.push_back({t[0], t[1], t[2]});
|
||||
|
||||
auto flat_positions = geom::deindex(positions, triangles);
|
||||
|
||||
std::vector<vertex> vertices;
|
||||
for (auto const & t : flat_positions)
|
||||
{
|
||||
auto n = geom::normal(t[0], t[1], t[2]);
|
||||
vertices.push_back({t[0], n});
|
||||
vertices.push_back({t[1], n});
|
||||
vertices.push_back({t[2], n});
|
||||
}
|
||||
|
||||
vertex::setup(box_mesh_);
|
||||
box_mesh_.load(vertices, gl::TRIANGLES, gl::STATIC_DRAW);
|
||||
}
|
||||
|
||||
engine_.set_gravity({0.f, 0.f, -10.f});
|
||||
// auto h0 = engine_.add_object(engine_.add_shape(phys3d::ball{1.f}), engine_.add_material({1.f, 1.f, 10.f}), {{-4.f, 0.25f, 2.f}});
|
||||
// auto h1 = engine_.add_object(engine_.add_shape(phys3d::ball{1.f}), engine_.add_material({1.f, 1.f, 10.f}), {{ 4.f, -0.25f, 2.f}});
|
||||
// engine_.get_object_state(h0).velocity = { 3.f, 0.f, 0.f};
|
||||
// engine_.get_object_state(h1).velocity = {-3.f, 0.f, 0.f};
|
||||
// (void)h0;
|
||||
// (void)h1;
|
||||
|
||||
engine_.add_object(engine_.add_shape(phys3d::half_space{{0.f, 0.f, 1.f}, 0.f}), engine_.add_material({1.f, 0.f, 10.f}), {{0.f, 0.f, 0.f}});
|
||||
|
||||
// engine_.add_object(engine_.add_shape(phys3d::half_space{{ 1.f, 0.f, 0.f}, -10.f}), engine_.add_material({1.f, 1.f, 50.f}), {{0.f, 0.f, 0.f}});
|
||||
// engine_.add_object(engine_.add_shape(phys3d::half_space{{-1.f, 0.f, 0.f}, -10.f}), engine_.add_material({1.f, 1.f, 50.f}), {{0.f, 0.f, 0.f}});
|
||||
// engine_.add_object(engine_.add_shape(phys3d::half_space{{ 0.f, 1.f, 0.f}, -10.f}), engine_.add_material({1.f, 1.f, 50.f}), {{0.f, 0.f, 0.f}});
|
||||
// engine_.add_object(engine_.add_shape(phys3d::half_space{{ 0.f, -1.f, 0.f}, -10.f}), engine_.add_material({1.f, 1.f, 50.f}), {{0.f, 0.f, 0.f}});
|
||||
}
|
||||
|
||||
void on_resize(int width, int height) override
|
||||
{
|
||||
app::app::on_resize(width, height);
|
||||
|
||||
camera_.set_fov(camera_.fov_y, (1.f * width) / height);
|
||||
}
|
||||
|
||||
void on_mouse_move(int x, int y, int dx, int dy) override
|
||||
{
|
||||
app::app::on_mouse_move(x, y, dx, dy);
|
||||
|
||||
if (is_right_button_down())
|
||||
{
|
||||
camera_elevation_angle_tgt_ += dy * 0.003f;
|
||||
camera_azimuthal_angle_tgt_ -= dx * 0.003f;
|
||||
}
|
||||
}
|
||||
|
||||
void on_mouse_wheel(int delta) override
|
||||
{
|
||||
app::app::on_mouse_wheel(delta);
|
||||
|
||||
camera_distance_tgt_ *= std::pow(0.8f, delta);
|
||||
}
|
||||
|
||||
void on_key_down(SDL_Keycode key) override
|
||||
{
|
||||
if (key == SDLK_SPACE)
|
||||
{
|
||||
// float r = 7.f;
|
||||
// engine_.add_object(engine_.add_shape(phys3d::ball{3.f}), engine_.add_material({1.f, 0.125f, 50.f}), {{random::uniform(rng_, -r, r), random::uniform(rng_, -r, r), 20.f}});
|
||||
|
||||
|
||||
// engine_.add_object(engine_.add_shape(phys3d::ball{1.f}), engine_.add_material({1.f, 0.5f, 50.f}), {{0.f, 0.f, 20.f}});
|
||||
|
||||
phys3d::object_state state;
|
||||
state.rotation.coords = random::uniform_sphere_vector_distribution<float, 4>()(rng_);
|
||||
if (!engine_.is_object(1))
|
||||
state.position = {0.f, 0.f, 4.f};
|
||||
else
|
||||
state.position = {0.01f, 0.01f, 4.f};
|
||||
// state.angular_velocity = random::uniform_ball_vector_distribution<float, 3>(25.f)(rng_);
|
||||
engine_.add_object(engine_.add_shape(phys3d::box{{1.5f, 1.5f, 1.5f}}), engine_.add_material({1.f, 0.f, 10.f}), state);
|
||||
// engine_.add_object(engine_.add_shape(phys3d::ball{1.f}), engine_.add_material({1.f, 0.5f, 100.f}), state);
|
||||
}
|
||||
else if (key == SDLK_f)
|
||||
{
|
||||
float const f = 1.f;
|
||||
for (std::uint32_t h = 1; h < engine_.object_count(); ++h)
|
||||
if (engine_.is_object(h))
|
||||
engine_.add_impulse(h, {random::uniform(rng_, -f, f), random::uniform(rng_, -f, f), random::uniform(rng_, 0.f, 0.f)});
|
||||
}
|
||||
}
|
||||
|
||||
void update() override
|
||||
{
|
||||
float const dt = clock_.restart().count();
|
||||
|
||||
physics_lag_ += dt;
|
||||
|
||||
float const ph_dt = 0.004f;
|
||||
|
||||
while (physics_lag_ >= ph_dt)
|
||||
{
|
||||
physics_lag_ -= ph_dt;
|
||||
engine_.update(ph_dt);
|
||||
|
||||
float E = 0.f;
|
||||
for (phys3d::engine::object_handle h = 1; h < engine_.object_count(); ++h)
|
||||
{
|
||||
if (!engine_.is_object(h)) continue;
|
||||
|
||||
auto m = engine_.get_object_mass(h);
|
||||
auto I = engine_.get_object_inertia(h);
|
||||
auto v = engine_.get_object_state(h).velocity;
|
||||
auto w = engine_.get_object_state(h).angular_velocity;
|
||||
auto H = engine_.get_object_state(h).position[2];
|
||||
|
||||
E += 0.5f * geom::length_sqr(v) * m + 0.5f * geom::dot(w, I * w) + m * 10.f * H;
|
||||
}
|
||||
log::info() << "Energy: " << E;
|
||||
}
|
||||
|
||||
camera_.distance += (camera_distance_tgt_ - camera_.distance) * (1.f - std::exp(- 20.f * dt));
|
||||
camera_.azimuthal_angle += (camera_azimuthal_angle_tgt_ - camera_.azimuthal_angle) * (1.f - std::exp(- 20.f * dt));
|
||||
camera_.elevation_angle += (camera_elevation_angle_tgt_ - camera_.elevation_angle) * (1.f - std::exp(- 20.f * dt));
|
||||
}
|
||||
|
||||
void present() override
|
||||
{
|
||||
gl::ClearColor(0.8f, 0.8f, 1.f, 1.f);
|
||||
gl::Clear(gl::COLOR_BUFFER_BIT | gl::DEPTH_BUFFER_BIT);
|
||||
|
||||
gl::Enable(gl::DEPTH_TEST);
|
||||
gl::DepthFunc(gl::LESS);
|
||||
|
||||
program_.bind();
|
||||
program_["u_camera_transform"] = camera_.transform();
|
||||
|
||||
program_["u_light_direction"] = geom::normalized(geom::vector{1.f, 1.f, 1.f});
|
||||
program_["u_light_color"] = geom::vector{1.f, 1.f, 1.f};
|
||||
|
||||
program_["u_object_transform"] = geom::matrix<float, 4, 4>::identity();
|
||||
program_["u_object_color"] = geom::vector{0.5f, 0.5f, 0.5f};
|
||||
program_["u_grid"] = 0;
|
||||
plane_mesh_.draw();
|
||||
|
||||
program_["u_object_color"] = geom::vector{0.f, 0.f, 1.f};
|
||||
program_["u_grid"] = 1;
|
||||
for (phys3d::engine::object_handle h = 0; h < engine_.object_count(); ++h)
|
||||
{
|
||||
if (!engine_.is_object(h)) continue;
|
||||
|
||||
auto sh = engine_.get_shape(engine_.get_object_shape(h));
|
||||
|
||||
if (auto s = std::get_if<phys3d::ball const *>(&sh))
|
||||
{
|
||||
program_["u_object_transform"] =
|
||||
geom::translation<float, 3>(engine_.get_object_state(h).position - geom::point<float, 3>::zero()).homogeneous_matrix() *
|
||||
geom::quaternion_rotation<float>(engine_.get_object_state(h).rotation).homogeneous_matrix() *
|
||||
geom::scale<float, 3>((*s)->radius).homogeneous_matrix();
|
||||
sphere_mesh_.draw();
|
||||
}
|
||||
else if (auto s = std::get_if<phys3d::box const *>(&sh))
|
||||
{
|
||||
program_["u_object_transform"] =
|
||||
geom::translation<float, 3>(engine_.get_object_state(h).position - geom::point<float, 3>::zero()).homogeneous_matrix() *
|
||||
geom::quaternion_rotation<float>(engine_.get_object_state(h).rotation).homogeneous_matrix() *
|
||||
geom::scale<float, 3>((*s)->dimensions / 2.f).homogeneous_matrix();
|
||||
box_mesh_.draw();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
gfx::program program_;
|
||||
|
||||
geom::spherical_camera camera_;
|
||||
float camera_distance_tgt_;
|
||||
float camera_azimuthal_angle_tgt_;
|
||||
float camera_elevation_angle_tgt_;
|
||||
|
||||
gfx::mesh plane_mesh_;
|
||||
gfx::mesh sphere_mesh_;
|
||||
gfx::mesh box_mesh_;
|
||||
|
||||
util::clock<std::chrono::duration<float>, std::chrono::high_resolution_clock> clock_;
|
||||
|
||||
phys3d::engine engine_;
|
||||
float physics_lag_ = 0.f;
|
||||
|
||||
random::generator rng_;
|
||||
};
|
||||
|
||||
int main()
|
||||
{
|
||||
return app::main<physics_3d_app>();
|
||||
}
|
||||
114
libs/phys/include/psemek/phys/engine_3d.hpp
Normal file
114
libs/phys/include/psemek/phys/engine_3d.hpp
Normal file
|
|
@ -0,0 +1,114 @@
|
|||
#pragma once
|
||||
|
||||
#include <psemek/geom/vector.hpp>
|
||||
#include <psemek/geom/point.hpp>
|
||||
#include <psemek/geom/quaternion.hpp>
|
||||
#include <psemek/geom/matrix.hpp>
|
||||
|
||||
#include <psemek/util/pimpl.hpp>
|
||||
|
||||
#include <variant>
|
||||
|
||||
namespace psemek::phys3d
|
||||
{
|
||||
|
||||
struct ball
|
||||
{
|
||||
float radius;
|
||||
};
|
||||
|
||||
struct box
|
||||
{
|
||||
geom::vector<float, 3> dimensions;
|
||||
};
|
||||
|
||||
struct half_space
|
||||
{
|
||||
// p * normal >= value
|
||||
geom::vector<float, 3> normal;
|
||||
float value;
|
||||
};
|
||||
|
||||
struct material
|
||||
{
|
||||
float density;
|
||||
float elasticity;
|
||||
float friction;
|
||||
};
|
||||
|
||||
struct object_state
|
||||
{
|
||||
geom::point<float, 3> position;
|
||||
geom::quaternion<float> rotation = geom::quaternion<float>::identity();
|
||||
|
||||
geom::vector<float, 3> velocity = {0.f, 0.f, 0.f};
|
||||
geom::vector<float, 3> angular_velocity = {0.f, 0.f, 0.f};
|
||||
};
|
||||
|
||||
struct engine
|
||||
{
|
||||
engine();
|
||||
~engine();
|
||||
|
||||
static constexpr std::uint32_t null = static_cast<std::uint32_t>(-1);
|
||||
|
||||
// Shape management
|
||||
|
||||
using shape_handle = std::uint32_t;
|
||||
|
||||
shape_handle add_shape(ball const & s);
|
||||
shape_handle add_shape(box const & s);
|
||||
shape_handle add_shape(half_space const & s);
|
||||
|
||||
using any_shape = std::variant<ball, box, half_space>;
|
||||
using any_shape_ptr = std::variant<ball const *, box const *, half_space const *>;
|
||||
|
||||
any_shape_ptr get_shape(shape_handle h) const;
|
||||
|
||||
void remove_shape(shape_handle h);
|
||||
|
||||
// Material management
|
||||
|
||||
using material_handle = std::uint32_t;
|
||||
|
||||
material_handle add_material(material const & m);
|
||||
|
||||
material const & get_material(material_handle h) const;
|
||||
|
||||
void remove_material(material_handle h);
|
||||
|
||||
// Object management
|
||||
|
||||
using object_handle = std::uint32_t;
|
||||
|
||||
object_handle add_object(shape_handle shape, material_handle material, object_state const & s);
|
||||
|
||||
shape_handle get_object_shape(object_handle h) const;
|
||||
material_handle get_object_material(object_handle h) const;
|
||||
|
||||
object_state const & get_object_state(object_handle h) const;
|
||||
object_state & get_object_state(object_handle h);
|
||||
|
||||
float get_object_mass(object_handle h) const;
|
||||
geom::matrix<float, 3, 3> get_object_inertia(object_handle h) const;
|
||||
|
||||
void remove_object(object_handle h);
|
||||
|
||||
object_handle object_count() const;
|
||||
bool is_object(object_handle h) const;
|
||||
|
||||
void add_impulse(object_handle h, geom::vector<float, 3> const & impulse);
|
||||
|
||||
// Force management
|
||||
|
||||
void set_gravity(geom::vector<float, 3> const & g);
|
||||
|
||||
// Update loop
|
||||
|
||||
void update(float dt);
|
||||
|
||||
private:
|
||||
psemek_declare_pimpl
|
||||
};
|
||||
|
||||
}
|
||||
729
libs/phys/source/engine_3d.cpp
Normal file
729
libs/phys/source/engine_3d.cpp
Normal file
|
|
@ -0,0 +1,729 @@
|
|||
#include <psemek/phys/engine_3d.hpp>
|
||||
|
||||
#include <psemek/geom/gauss.hpp>
|
||||
#include <psemek/geom/rotation.hpp>
|
||||
#include <psemek/geom/box.hpp>
|
||||
|
||||
#include <psemek/util/flat_list.hpp>
|
||||
#include <psemek/util/function.hpp>
|
||||
#include <psemek/util/not_implemented.hpp>
|
||||
#include <psemek/util/unused.hpp>
|
||||
|
||||
namespace psemek::phys3d
|
||||
{
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
constexpr float inf = std::numeric_limits<float>::infinity();
|
||||
|
||||
struct shape
|
||||
{
|
||||
engine::any_shape shape;
|
||||
float volume;
|
||||
geom::vector<float, 3> inertia;
|
||||
};
|
||||
|
||||
struct object
|
||||
{
|
||||
engine::shape_handle shape;
|
||||
engine::material_handle material;
|
||||
object_state state;
|
||||
|
||||
float inv_mass;
|
||||
geom::vector<float, 3> inertia;
|
||||
|
||||
geom::matrix<float, 3, 3> global_inv_inertia = {};
|
||||
};
|
||||
|
||||
float volume(ball const & s)
|
||||
{
|
||||
return 4.f / 3.f * geom::pi * s.radius * s.radius * s.radius;
|
||||
}
|
||||
|
||||
float volume(box const & s)
|
||||
{
|
||||
return s.dimensions[0] * s.dimensions[1] * s.dimensions[2];
|
||||
}
|
||||
|
||||
float volume(half_space const &)
|
||||
{
|
||||
return inf;
|
||||
}
|
||||
|
||||
geom::vector<float, 3> inertia(ball const & s)
|
||||
{
|
||||
float const I = 0.4f * s.radius * s.radius;
|
||||
return {I, I, I};
|
||||
}
|
||||
|
||||
geom::vector<float, 3> inertia(box const & s)
|
||||
{
|
||||
float const x = s.dimensions[0] * s.dimensions[0] / 12.f;
|
||||
float const y = s.dimensions[1] * s.dimensions[1] / 12.f;
|
||||
float const z = s.dimensions[2] * s.dimensions[2] / 12.f;
|
||||
|
||||
return {y + z, x + z, x + y};
|
||||
}
|
||||
|
||||
geom::vector<float, 3> inertia(half_space const &)
|
||||
{
|
||||
return {inf, inf, inf};
|
||||
}
|
||||
|
||||
struct collision_data
|
||||
{
|
||||
geom::vector<float, 3> point1, point2;
|
||||
geom::vector<float, 3> normal;
|
||||
// depth should be equal to dot(position 2 + point 2 - position1 - point1, normal)
|
||||
float depth;
|
||||
};
|
||||
|
||||
collision_data reversed(collision_data d)
|
||||
{
|
||||
std::swap(d.point1, d.point2);
|
||||
d.normal = -d.normal;
|
||||
return d;
|
||||
}
|
||||
|
||||
std::optional<collision_data> reversed(std::optional<collision_data> const & d)
|
||||
{
|
||||
if (d)
|
||||
return reversed(*d);
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
std::optional<collision_data> collision(object_state const & state1, ball const & shape1, object_state const & state2, ball const & shape2)
|
||||
{
|
||||
auto r = state2.position - state1.position;
|
||||
auto l = geom::length(r);
|
||||
if (l >= shape1.radius + shape2.radius)
|
||||
return std::nullopt;
|
||||
|
||||
auto n = r / l;
|
||||
auto p1 = n * shape1.radius;
|
||||
auto p2 = -n * shape2.radius;
|
||||
|
||||
return collision_data{p1, p2, n, l - shape1.radius - shape2.radius};
|
||||
}
|
||||
|
||||
std::optional<collision_data> collision(object_state const & state1, ball const & shape1, object_state const & state2, box const & shape2)
|
||||
{
|
||||
unused(state1);
|
||||
unused(shape1);
|
||||
unused(state2);
|
||||
unused(shape2);
|
||||
util::not_implemented();
|
||||
}
|
||||
|
||||
std::optional<collision_data> collision(object_state const & state1, ball const & shape1, object_state const & /* state2 */, half_space const & shape2)
|
||||
{
|
||||
auto v = geom::dot(shape2.normal, state1.position - state1.position.zero()) - shape2.value;
|
||||
|
||||
if (v >= shape1.radius)
|
||||
return std::nullopt;
|
||||
|
||||
return collision_data{- shape2.normal * shape1.radius, {0.f, 0.f, 0.f}, -shape2.normal, v - shape1.radius};
|
||||
}
|
||||
|
||||
std::optional<collision_data> collision(object_state const & state1, box const & shape1, object_state const & state2, ball const & shape2)
|
||||
{
|
||||
return reversed(collision(state2, shape2, state1, shape1));
|
||||
}
|
||||
|
||||
std::optional<collision_data> collision(object_state const & state1, box const & shape1, object_state const & state2, box const & shape2)
|
||||
{
|
||||
float const C = 1000.f;
|
||||
float const slop = -0.001f;
|
||||
(void)C;
|
||||
|
||||
geom::vector<float, 3> normals[15];
|
||||
|
||||
normals[0] = geom::rotate(state1.rotation, {1.f, 0.f, 0.f});
|
||||
normals[1] = geom::rotate(state1.rotation, {0.f, 1.f, 0.f});
|
||||
normals[2] = geom::rotate(state1.rotation, {0.f, 0.f, 1.f});
|
||||
|
||||
normals[3] = geom::rotate(state2.rotation, {1.f, 0.f, 0.f});
|
||||
normals[4] = geom::rotate(state2.rotation, {0.f, 1.f, 0.f});
|
||||
normals[5] = geom::rotate(state2.rotation, {0.f, 0.f, 1.f});
|
||||
|
||||
normals[ 6] = geom::cross(normals[0], normals[3]);
|
||||
normals[ 7] = geom::cross(normals[0], normals[4]);
|
||||
normals[ 8] = geom::cross(normals[0], normals[5]);
|
||||
normals[ 9] = geom::cross(normals[1], normals[3]);
|
||||
normals[10] = geom::cross(normals[1], normals[4]);
|
||||
normals[11] = geom::cross(normals[1], normals[5]);
|
||||
normals[12] = geom::cross(normals[2], normals[3]);
|
||||
normals[13] = geom::cross(normals[2], normals[4]);
|
||||
normals[14] = geom::cross(normals[2], normals[5]);
|
||||
|
||||
auto tx1 = normals[0] * shape1.dimensions[0] / 2.f;
|
||||
auto ty1 = normals[1] * shape1.dimensions[1] / 2.f;
|
||||
auto tz1 = normals[2] * shape1.dimensions[2] / 2.f;
|
||||
|
||||
auto tx2 = normals[3] * shape2.dimensions[0] / 2.f;
|
||||
auto ty2 = normals[4] * shape2.dimensions[1] / 2.f;
|
||||
auto tz2 = normals[5] * shape2.dimensions[2] / 2.f;
|
||||
|
||||
geom::vector<float, 3> max_n{0.f, 0.f, 0.f};
|
||||
float max_v = -inf;
|
||||
geom::vector<float, 3> max_p1{0.f, 0.f, 0.f};
|
||||
geom::vector<float, 3> max_p2{0.f, 0.f, 0.f};
|
||||
|
||||
static const auto zero = geom::point<float, 3>::zero();
|
||||
|
||||
for (auto & n : normals)
|
||||
{
|
||||
auto nl = geom::length(n);
|
||||
if (nl == 0.f) continue;
|
||||
n /= nl;
|
||||
|
||||
geom::interval<float> i1, i2;
|
||||
|
||||
geom::vector<float, 3> min_pp1, max_pp1;
|
||||
geom::vector<float, 3> min_pp2, max_pp2;
|
||||
|
||||
for (float x : {-1.f, 1.f})
|
||||
{
|
||||
for (float y : {-1.f, 1.f})
|
||||
{
|
||||
for (float z : {-1.f, 1.f})
|
||||
{
|
||||
auto p = tx1 * x + ty1 * y + tz1 * z;
|
||||
float v = geom::dot(n, state1.position + p - zero);
|
||||
if (v < i1.min)
|
||||
{
|
||||
i1.min = v;
|
||||
min_pp1 = p;
|
||||
}
|
||||
if (v > i1.max)
|
||||
{
|
||||
i1.max = v;
|
||||
max_pp1 = p;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (float x : {-1.f, 1.f})
|
||||
{
|
||||
for (float y : {-1.f, 1.f})
|
||||
{
|
||||
for (float z : {-1.f, 1.f})
|
||||
{
|
||||
auto p = tx2 * x + ty2 * y + tz2 * z;
|
||||
float v = geom::dot(n, state2.position + p - zero);
|
||||
if (v < i2.min)
|
||||
{
|
||||
i2.min = v;
|
||||
min_pp2 = p;
|
||||
}
|
||||
if (v > i2.max)
|
||||
{
|
||||
i2.max = v;
|
||||
max_pp2 = p;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
auto i = (i1 & i2);
|
||||
|
||||
float v = -i.length();
|
||||
|
||||
if (v > max_v)
|
||||
{
|
||||
max_v = v;
|
||||
|
||||
if (i1.center() < i2.center())
|
||||
{
|
||||
max_n = n;
|
||||
max_p1 = max_pp1;
|
||||
max_p2 = min_pp2;
|
||||
}
|
||||
else
|
||||
{
|
||||
max_n = -n;
|
||||
max_p1 = min_pp1;
|
||||
max_p2 = max_pp2;
|
||||
}
|
||||
}
|
||||
|
||||
if (max_v >= slop)
|
||||
break;
|
||||
}
|
||||
|
||||
if (max_v >= slop)
|
||||
return std::nullopt;
|
||||
|
||||
return collision_data{max_p1, max_p2, max_n, max_v - slop};
|
||||
}
|
||||
|
||||
std::optional<collision_data> collision(object_state const & state1, box const & shape1, object_state const & /* state2 */, half_space const & shape2)
|
||||
{
|
||||
float min_v = inf;
|
||||
|
||||
for (float tx : {-1.f, 1.f})
|
||||
{
|
||||
for (float ty : {-1.f, 1.f})
|
||||
{
|
||||
for (float tz : {-1.f, 1.f})
|
||||
{
|
||||
geom::vector<float, 3> t{tx, ty, tz};
|
||||
t = geom::rotate(state1.rotation, geom::pointwise_mult(t, shape1.dimensions / 2.f));
|
||||
|
||||
auto p = state1.position + t;
|
||||
|
||||
auto v = geom::dot(shape2.normal, p - p.zero()) - shape2.value;
|
||||
|
||||
if (v < min_v)
|
||||
min_v = v;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float const C = 1000.f;
|
||||
float const slop = -0.001f;
|
||||
|
||||
geom::vector<float, 3> sum_r{0.f, 0.f, 0.f};
|
||||
float sum_w = 0.f;
|
||||
|
||||
for (float tx : {-1.f, 1.f})
|
||||
{
|
||||
for (float ty : {-1.f, 1.f})
|
||||
{
|
||||
for (float tz : {-1.f, 1.f})
|
||||
{
|
||||
geom::vector<float, 3> t{tx, ty, tz};
|
||||
t = geom::rotate(state1.rotation, geom::pointwise_mult(t, shape1.dimensions / 2.f));
|
||||
|
||||
auto p = state1.position + t;
|
||||
|
||||
auto v = geom::dot(shape2.normal, p - p.zero()) - shape2.value;
|
||||
|
||||
float w = std::exp(- C * (v - min_v));
|
||||
|
||||
sum_w += w;
|
||||
sum_r += w * t;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (min_v >= slop)
|
||||
return std::nullopt;
|
||||
|
||||
return collision_data{sum_r / sum_w, {0.f, 0.f, 0.f}, -shape2.normal, min_v - slop};
|
||||
}
|
||||
|
||||
std::optional<collision_data> collision(object_state const & state1, half_space const & shape1, object_state const & state2, ball const & shape2)
|
||||
{
|
||||
return reversed(collision(state2, shape2, state1, shape1));
|
||||
}
|
||||
|
||||
std::optional<collision_data> collision(object_state const & state1, half_space const & shape1, object_state const & state2, box const & shape2)
|
||||
{
|
||||
return reversed(collision(state2, shape2, state1, shape1));
|
||||
}
|
||||
|
||||
std::optional<collision_data> collision(object_state const & /* state1 */, half_space const & /* shape1 */, object_state const & /* state2 */, half_space const & /* shape2 */)
|
||||
{
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
std::optional<collision_data> collision(object_state const & state1, engine::any_shape const & shape1, object_state const & state2, engine::any_shape const & shape2)
|
||||
{
|
||||
return std::visit([&](auto const & s1, auto const & s2){ return collision(state1, s1, state2, s2); }, shape1, shape2);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
struct engine::impl
|
||||
{
|
||||
util::flat_list<shape, shape_handle> shapes;
|
||||
util::flat_list<material, material_handle> materials;
|
||||
util::flat_list<object, object_handle> objects;
|
||||
std::vector<bool> object_mask;
|
||||
|
||||
geom::vector<float, 3> gravity{0.f, 0.f, 0.f};
|
||||
float collision_bias = 0.25f;
|
||||
|
||||
template <typename Shape>
|
||||
shape_handle add_shape(Shape const & s);
|
||||
|
||||
bool is_object(object_handle h) const;
|
||||
|
||||
using solver = util::function<void()>;
|
||||
|
||||
template <typename Value, typename Jacobian, typename Limits, std::size_t N>
|
||||
solver make_solver(Value && value_fn, Jacobian && jacobian_fn, Limits && limits_fn, std::array<object_handle, N> const & objects);
|
||||
|
||||
solver make_collision_solver(object_handle h1, object_handle h2, collision_data const & c, float dt);
|
||||
|
||||
void update(float dt);
|
||||
};
|
||||
|
||||
template <typename Shape>
|
||||
engine::shape_handle engine::impl::add_shape(Shape const & s)
|
||||
{
|
||||
return shapes.insert({s, volume(s), inertia(s)});
|
||||
}
|
||||
|
||||
bool engine::impl::is_object(object_handle h) const
|
||||
{
|
||||
return h < objects.capacity() && object_mask[h];
|
||||
}
|
||||
|
||||
template <typename Value, typename Jacobian, typename Limits, std::size_t N>
|
||||
engine::impl::solver engine::impl::make_solver(Value && value_fn, Jacobian && jacobian_fn, Limits && limits_fn, std::array<object_handle, N> const & handles)
|
||||
{
|
||||
return [this, value_fn = std::move(value_fn), jacobian_fn = std::move(jacobian_fn), limits_fn = std::move(limits_fn), handles]{
|
||||
|
||||
auto f = value_fn();
|
||||
auto J = jacobian_fn();
|
||||
auto limits = limits_fn();
|
||||
|
||||
auto M_inv_J_T = geom::transpose(J);
|
||||
|
||||
geom::vector<float, 6 * N> v;
|
||||
for (std::size_t i = 0; i < N; ++i)
|
||||
{
|
||||
object const & obj = objects[handles[i]];
|
||||
v[6 * i + 0] = obj.state.velocity[0];
|
||||
v[6 * i + 1] = obj.state.velocity[1];
|
||||
v[6 * i + 2] = obj.state.velocity[2];
|
||||
v[6 * i + 3] = obj.state.angular_velocity[0];
|
||||
v[6 * i + 4] = obj.state.angular_velocity[1];
|
||||
v[6 * i + 5] = obj.state.angular_velocity[2];
|
||||
|
||||
for (std::size_t j = 0; j < M_inv_J_T.columns(); ++j)
|
||||
{
|
||||
M_inv_J_T[6 * i + 0][j] *= obj.inv_mass;
|
||||
M_inv_J_T[6 * i + 1][j] *= obj.inv_mass;
|
||||
M_inv_J_T[6 * i + 2][j] *= obj.inv_mass;
|
||||
|
||||
geom::vector<float, 3> r;
|
||||
r[0] = M_inv_J_T[6 * i + 3][j];
|
||||
r[1] = M_inv_J_T[6 * i + 4][j];
|
||||
r[2] = M_inv_J_T[6 * i + 5][j];
|
||||
|
||||
r = obj.global_inv_inertia * r;
|
||||
|
||||
M_inv_J_T[6 * i + 3][j] = r[0];
|
||||
M_inv_J_T[6 * i + 4][j] = r[1];
|
||||
M_inv_J_T[6 * i + 5][j] = r[2];
|
||||
}
|
||||
}
|
||||
|
||||
auto lambda = geom::solve(J * M_inv_J_T, - J * v - f);
|
||||
|
||||
if (!lambda)
|
||||
return;
|
||||
|
||||
for (std::size_t i = 0; i < lambda->dimension(); ++i)
|
||||
(*lambda)[i] = geom::clamp((*lambda)[i], limits[i]);
|
||||
|
||||
auto dv = M_inv_J_T * (*lambda);
|
||||
|
||||
for (std::size_t i = 0; i < N; ++i)
|
||||
{
|
||||
object & obj = objects[handles[i]];
|
||||
obj.state.velocity[0] += dv[6 * i + 0];
|
||||
obj.state.velocity[1] += dv[6 * i + 1];
|
||||
obj.state.velocity[2] += dv[6 * i + 2];
|
||||
obj.state.angular_velocity[0] += dv[6 * i + 3];
|
||||
obj.state.angular_velocity[1] += dv[6 * i + 4];
|
||||
obj.state.angular_velocity[2] += dv[6 * i + 5];
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
engine::impl::solver engine::impl::make_collision_solver(object_handle h1, object_handle h2, collision_data const & c, float dt)
|
||||
{
|
||||
auto tx = geom::ort(c.normal);
|
||||
auto ty = geom::cross(c.normal, tx);
|
||||
|
||||
auto value_fn = [this, c, dt, h1, h2]{
|
||||
|
||||
auto const & s1 = objects[h1].state;
|
||||
auto const & s2 = objects[h2].state;
|
||||
|
||||
auto v1 = s1.velocity + geom::cross(s1.angular_velocity, c.point1);
|
||||
auto v2 = s2.velocity + geom::cross(s2.angular_velocity, c.point2);
|
||||
|
||||
auto dv = v2 - v1;
|
||||
|
||||
auto const & m1 = materials[objects[h1].material];
|
||||
auto const & m2 = materials[objects[h2].material];
|
||||
|
||||
float b = (m1.elasticity + m2.elasticity) / 2.f;
|
||||
|
||||
// float f = std::sqrt(m1.friction * m2.friction);
|
||||
// float fc = std::exp(- f * dt);
|
||||
|
||||
return geom::vector{
|
||||
std::min(c.depth * collision_bias / dt, geom::dot(c.normal, dv) * b),
|
||||
0.f,
|
||||
0.f,
|
||||
// - fc * geom::dot(tx, dv),
|
||||
// - fc * geom::dot(ty, dv),
|
||||
};
|
||||
};
|
||||
|
||||
auto jacobian_fn = [c, tx, ty]{
|
||||
// f = dot(n, p2 - p1)
|
||||
// df = dot(n, v2 + cross(omega2, r2) - v1 - cross(omega1, r1))
|
||||
|
||||
auto t1 = geom::cross(c.point1, c.normal);
|
||||
auto t2 = geom::cross(c.point2, c.normal);
|
||||
|
||||
auto tx1 = geom::cross(c.point1, tx);
|
||||
auto tx2 = geom::cross(c.point2, tx);
|
||||
|
||||
auto ty1 = geom::cross(c.point1, ty);
|
||||
auto ty2 = geom::cross(c.point2, ty);
|
||||
|
||||
geom::matrix<float, 3, 12> J;
|
||||
|
||||
J[0][0] = -c.normal[0];
|
||||
J[0][1] = -c.normal[1];
|
||||
J[0][2] = -c.normal[2];
|
||||
|
||||
J[0][3] = -t1[0];
|
||||
J[0][4] = -t1[1];
|
||||
J[0][5] = -t1[2];
|
||||
|
||||
J[0][6] = c.normal[0];
|
||||
J[0][7] = c.normal[1];
|
||||
J[0][8] = c.normal[2];
|
||||
|
||||
J[0][9] = t2[0];
|
||||
J[0][10] = t2[1];
|
||||
J[0][11] = t2[2];
|
||||
|
||||
J[1][0] = -tx[0];
|
||||
J[1][1] = -tx[1];
|
||||
J[1][2] = -tx[2];
|
||||
|
||||
J[1][3] = -tx1[0];
|
||||
J[1][4] = -tx1[1];
|
||||
J[1][5] = -tx1[2];
|
||||
|
||||
J[1][6] = tx[0];
|
||||
J[1][7] = tx[1];
|
||||
J[1][8] = tx[2];
|
||||
|
||||
J[1][9] = tx2[0];
|
||||
J[1][10] = tx2[1];
|
||||
J[1][11] = tx2[2];
|
||||
|
||||
J[2][0] = -ty[0];
|
||||
J[2][1] = -ty[1];
|
||||
J[2][2] = -ty[2];
|
||||
|
||||
J[2][3] = -ty1[0];
|
||||
J[2][4] = -ty1[1];
|
||||
J[2][5] = -ty1[2];
|
||||
|
||||
J[2][6] = ty[0];
|
||||
J[2][7] = ty[1];
|
||||
J[2][8] = ty[2];
|
||||
|
||||
J[2][9] = ty2[0];
|
||||
J[2][10] = ty2[1];
|
||||
J[2][11] = ty2[2];
|
||||
|
||||
return J;
|
||||
};
|
||||
|
||||
auto limits_fn = [this, h1, h2, dt]{
|
||||
|
||||
auto const & m1 = materials[objects[h1].material];
|
||||
auto const & m2 = materials[objects[h2].material];
|
||||
|
||||
float f = std::sqrt(m1.friction * m2.friction) * dt * 0;
|
||||
// float fc = std::exp(- f * dt);
|
||||
|
||||
return geom::box<float, 3>{{
|
||||
{0.f, inf},
|
||||
// {-inf, inf},
|
||||
// {-inf, inf},
|
||||
{-f, f},
|
||||
{-f, f},
|
||||
// {0.f, 0.f},
|
||||
// {0.f, 0.f},
|
||||
}};
|
||||
};
|
||||
|
||||
return make_solver(value_fn, jacobian_fn, limits_fn, std::array{h1, h2});
|
||||
}
|
||||
|
||||
void engine::impl::update(float dt)
|
||||
{
|
||||
// Update inertia
|
||||
for (object_handle h = 0; h < objects.capacity(); ++h)
|
||||
{
|
||||
if (!is_object(h)) continue;
|
||||
|
||||
auto & o = objects[h];
|
||||
auto R = geom::quaternion_rotation<float>(o.state.rotation).linear_matrix();
|
||||
if (auto i = geom::inverse(R * geom::matrix<float, 3, 3>::diagonal(o.inertia) * geom::transpose(R)); i && o.inv_mass != 0.f)
|
||||
o.global_inv_inertia = *i;
|
||||
else
|
||||
o.global_inv_inertia = o.global_inv_inertia.zero();
|
||||
}
|
||||
|
||||
// Apply forces
|
||||
for (auto & o : objects)
|
||||
{
|
||||
if (o.inv_mass != 0.f)
|
||||
{
|
||||
o.state.velocity += gravity * dt;
|
||||
}
|
||||
}
|
||||
|
||||
// Gather constraints
|
||||
std::vector<util::function<void()>> solvers;
|
||||
for (object_handle h1 = 0; h1 < objects.capacity(); ++h1)
|
||||
{
|
||||
if (!is_object(h1)) continue;
|
||||
for (object_handle h2 = h1 + 1; h2 < objects.capacity(); ++h2)
|
||||
{
|
||||
if (!is_object(h2)) continue;
|
||||
|
||||
if (auto c = collision(objects[h1].state, shapes[objects[h1].shape].shape, objects[h2].state, shapes[objects[h2].shape].shape))
|
||||
solvers.push_back(make_collision_solver(h1, h2, *c, dt));
|
||||
}
|
||||
}
|
||||
|
||||
// Apply constraints
|
||||
for (auto & s : solvers)
|
||||
s();
|
||||
|
||||
// Integrate velocity
|
||||
for (auto & o : objects)
|
||||
{
|
||||
o.state.position += o.state.velocity * dt;
|
||||
o.state.rotation = geom::normalized(geom::exp(geom::quaternion<float>::vector(o.state.angular_velocity * dt)) * o.state.rotation);
|
||||
}
|
||||
}
|
||||
|
||||
engine::engine()
|
||||
: pimpl_(make_impl())
|
||||
{}
|
||||
|
||||
engine::~engine() = default;
|
||||
|
||||
engine::shape_handle engine::add_shape(ball const & s)
|
||||
{
|
||||
return impl().add_shape(s);
|
||||
}
|
||||
|
||||
engine::shape_handle engine::add_shape(box const & s)
|
||||
{
|
||||
return impl().add_shape(s);
|
||||
}
|
||||
|
||||
engine::shape_handle engine::add_shape(half_space const & s)
|
||||
{
|
||||
return impl().add_shape(s);
|
||||
}
|
||||
|
||||
engine::any_shape_ptr engine::get_shape(shape_handle h) const
|
||||
{
|
||||
return std::visit([](auto const & x) -> any_shape_ptr { return &x; }, impl().shapes[h].shape);
|
||||
}
|
||||
|
||||
void engine::remove_shape(shape_handle h)
|
||||
{
|
||||
impl().shapes.erase(h);
|
||||
}
|
||||
|
||||
engine::material_handle engine::add_material(material const & m)
|
||||
{
|
||||
return impl().materials.insert(m);
|
||||
}
|
||||
|
||||
material const & engine::get_material(material_handle h) const
|
||||
{
|
||||
return impl().materials[h];
|
||||
}
|
||||
|
||||
void engine::remove_material(material_handle h)
|
||||
{
|
||||
impl().materials.erase(h);
|
||||
}
|
||||
|
||||
engine::object_handle engine::add_object(shape_handle shape, material_handle material, object_state const & s)
|
||||
{
|
||||
float mass = impl().shapes[shape].volume * impl().materials[material].density;
|
||||
auto inertia = impl().shapes[shape].inertia * mass;
|
||||
|
||||
auto h = impl().objects.insert(object{shape, material, s, 1.f / mass, inertia});
|
||||
if (impl().object_mask.size() <= h)
|
||||
impl().object_mask.resize(impl().objects.capacity(), false);
|
||||
impl().object_mask[h] = true;
|
||||
return h;
|
||||
}
|
||||
|
||||
engine::shape_handle engine::get_object_shape(object_handle h) const
|
||||
{
|
||||
return impl().objects[h].shape;
|
||||
}
|
||||
|
||||
engine::material_handle engine::get_object_material(object_handle h) const
|
||||
{
|
||||
return impl().objects[h].material;
|
||||
}
|
||||
|
||||
object_state const & engine::get_object_state(object_handle h) const
|
||||
{
|
||||
return impl().objects[h].state;
|
||||
}
|
||||
|
||||
object_state & engine::get_object_state(object_handle h)
|
||||
{
|
||||
return impl().objects[h].state;
|
||||
}
|
||||
|
||||
float engine::get_object_mass(object_handle h) const
|
||||
{
|
||||
return 1.f / impl().objects[h].inv_mass;
|
||||
}
|
||||
|
||||
geom::matrix<float, 3, 3> engine::get_object_inertia(object_handle h) const
|
||||
{
|
||||
return *geom::inverse(impl().objects[h].global_inv_inertia);
|
||||
}
|
||||
|
||||
void engine::remove_object(object_handle h)
|
||||
{
|
||||
impl().object_mask[h] = false;
|
||||
impl().objects.erase(h);
|
||||
}
|
||||
|
||||
engine::object_handle engine::object_count() const
|
||||
{
|
||||
return impl().objects.capacity();
|
||||
}
|
||||
|
||||
bool engine::is_object(object_handle h) const
|
||||
{
|
||||
return impl().is_object(h);
|
||||
}
|
||||
|
||||
void engine::add_impulse(object_handle h, geom::vector<float, 3> const & impulse)
|
||||
{
|
||||
impl().objects[h].state.velocity += impulse * impl().objects[h].inv_mass;
|
||||
}
|
||||
|
||||
void engine::set_gravity(geom::vector<float, 3> const & g)
|
||||
{
|
||||
impl().gravity = g;
|
||||
}
|
||||
|
||||
void engine::update(float dt)
|
||||
{
|
||||
impl().update(dt);
|
||||
}
|
||||
|
||||
}
|
||||
Loading…
Add table
Reference in a new issue