diff --git a/demo.mk b/demo.mk index 8e3564b..5d4667d 100644 --- a/demo.mk +++ b/demo.mk @@ -20,9 +20,12 @@ DEMO_OBJ = \ src/platform/font.o \ src/demo/ballistics.o \ src/demo/bridge.o \ + src/demo/sailboat.o \ src/physics/particle.o \ src/physics/particle_link.o \ - src/physics/particle_contact.o + src/physics/particle_contact.o \ + src/physics/body.o \ + src/physics/force_generator.o demo.elf: LDSCRIPT = $(LIB)/main.lds demo.elf: $(START_OBJ) $(DEMO_OBJ) $(FONT_OBJ) $(LIBGCC) diff --git a/src/demo/sailboat.cpp b/src/demo/sailboat.cpp new file mode 100644 index 0000000..faf948f --- /dev/null +++ b/src/demo/sailboat.cpp @@ -0,0 +1,134 @@ +#include "math/float_types.hpp" +#include "math/transform.hpp" + +#include "platform/graphics_primitive.hpp" +#include "platform/font.hpp" + +#include "demo/sailboat.hpp" + +#include "assert.h" + +namespace demo { + static inline mat3x3 inertia_tensor_coeffs(float ix, float iy, float iz, + float ixy = 0, float ixz = 0, float iyz = 0) + { + return mat3x3( ix, -ixy, -ixz, + -ixy, iy, -iyz, + -ixz, -iyz, iz); + } + + static inline mat3x3 block_inertia_tensor(const vec3& half_sizes, float mass) + { + vec3 squares = half_sizes * half_sizes; + return inertia_tensor_coeffs(0.3f * mass * (squares.y + squares.z), + 0.3f * mass * (squares.x + squares.z), + 0.3f * mass * (squares.x + squares.y)); + } + + + mat4x4 sailboat::init() + { + buoyancy.center_of_buoyancy = vec3(0.0f, 0.5f, 0.0f); + buoyancy.max_depth = 1.0f; + buoyancy.volume = 3.0f; + buoyancy.water_height = 1.6f; + buoyancy.liquid_density = 1000.0f; + + body.position = vec3(0, 1.6f, 0); + body.orientation = vec4(0, 0, 0, 1); + body.velocity = vec3(0, 0, 0); + body.rotation = vec3(0, 0, 0); + body.inverse_mass = 1.0f / 200.0f; + + mat3x3 inertia_tensor = block_inertia_tensor(vec3(2, 1, 1), 100.0f); + body.set_inertia_tensor(inertia_tensor); + + body.linear_damping = 0.9963f; + body.angular_damping = 0.9963f; + body.acceleration = vec3(0, -9.81, 0); + body.calculate_derived_data(); + + mat4x4 view_trans + = translate(vec3(0.0, 0.1, 0.5)) + * rotate_x(pi / 4) + * rotate_y(pi / 4) + * scale(vec3(-2, -2, 2)); + + return view_trans; + } + + void sailboat::update() + { + body.clear_accumulators(); + + float duration = 1.0f / 60.0f; + + buoyancy.update_force(&body, duration); + + body.integrate(duration); + } + + void sailboat::draw_hud(ta_parameter_writer& writer) + { + const int title_length = 16; + const int title_width_2 = (font::ter_u12n.hori_advance * title_length) >> 1; + const int framebuffer_width_2 = framebuffer::framebuffer.px_width >> 1; + const int x = framebuffer_width_2 - title_width_2; + vec3 center_p = vec3(x, 5, 10); + + font::ter_u12n.global(writer); + font::ter_u12n.draw_string(writer, center_p, "demo: sailboat", 0xffffffff); + } + + void sailboat::draw_boat(ta_parameter_writer& writer, const mat4x4& trans) + { + vec3 color = vec3(1.0, 0.5, 0.0); + + // left hull + const mat4x4 tl + = translate(vec3(0, 0, -1.0f)) + * scale(vec3(2.f, 0.4f, 0.4f)) + * scale(0.5f); + draw_cube(writer, trans * tl, color); + + // right hull + const mat4x4 tr + = translate(vec3(0, 0, 1.0f)) + * scale(vec3(2.f, 0.4f, 0.4f)) + * scale(0.5f); + draw_cube(writer, trans * tr, color); + + // deck + vec3 deck_color = vec3(1.0, 0.0, 1.0); + const mat4x4 td + = translate(vec3(0, 0.3f, 0.0f)) + * scale(vec3(1.0f, 0.1f, 2.0f)) + * scale(0.5f); + draw_cube(writer, trans * td, deck_color); + + // mast + vec3 mast_color = vec3(0.0, 1.0, 1.0); + const mat4x4 tm + = translate(vec3(0, 1.8f, 0)) + * scale(vec3(0.1f, 3.0f, 0.1f)) + * scale(0.5f); + draw_cube(writer, trans * tm, mast_color); + } + + void sailboat::draw(ta_parameter_writer& writer, const mat4x4& trans) + { + // punch through + draw_hud(writer); + + writer.append() = + ta_global_parameter::end_of_list(para_control::para_type::end_of_list); + + // opaque + draw_axis(writer, trans); + + draw_boat(writer, trans * body.transform_matrix); + + writer.append() = + ta_global_parameter::end_of_list(para_control::para_type::end_of_list); + } +} diff --git a/src/demo/sailboat.hpp b/src/demo/sailboat.hpp new file mode 100644 index 0000000..6f114b8 --- /dev/null +++ b/src/demo/sailboat.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include "demo/scene.hpp" +#include "physics/force_generator.hpp" + +namespace demo { + + struct sailboat : scene { + physics::buoyancy buoyancy; + physics::rigid_body body; + + mat4x4 init() override; + + void update() override; + + void draw_hud(ta_parameter_writer& writer); + void draw_boat(ta_parameter_writer& writer, const mat4x4& trans); + void draw(ta_parameter_writer& writer, const mat4x4& trans) override; + }; +} diff --git a/src/physics/body.cpp b/src/physics/body.cpp new file mode 100644 index 0000000..1aa91a9 --- /dev/null +++ b/src/physics/body.cpp @@ -0,0 +1,69 @@ +#include "math/float_types.hpp" +#include "math/transform.hpp" + +#include "body.hpp" + +namespace physics { + + void rigid_body::calculate_derived_data() + { + orientation = normalize(orientation); + + transform_matrix = translate(position) * rotate_quaternion(orientation); + + mat3x3 t = submatrix(transform_matrix, 3, 3); + + inverse_inertia_tensor_world = t * inverse_inertia_tensor * transpose(t); + } + + void rigid_body::set_inertia_tensor(const mat3x3& inertia_tensor) + { + inverse_inertia_tensor = inverse(inertia_tensor); + } + + void rigid_body::clear_accumulators() + { + force_accum = vec3(0, 0, 0); + torque_accum = vec3(0, 0, 0); + } + + void rigid_body::add_force_at_local_point(const vec3& force, + const vec3& point) + { + vec3 pt = transform_matrix * point; + add_force_at_point(force, pt); + } + + void rigid_body::add_force_at_point(const vec3& force, + const vec3& point) + { + // relative to center of mass + vec3 pt = point - position; + + force_accum += force; + torque_accum += cross(pt, force); + } + + void rigid_body::integrate(float duration) + { + last_frame_acceleration = acceleration; + last_frame_acceleration += force_accum * inverse_mass; + + vec3 angular_acceleration = inverse_inertia_tensor_world * torque_accum; + + velocity += last_frame_acceleration * duration; + rotation += angular_acceleration * duration; + + // drag + velocity *= linear_damping; + rotation *= angular_damping; + + position += velocity * duration; + + orientation += rotation * duration; + + calculate_derived_data(); + + clear_accumulators(); + } +} diff --git a/src/physics/body.hpp b/src/physics/body.hpp new file mode 100644 index 0000000..f7f5455 --- /dev/null +++ b/src/physics/body.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include "math/float_types.hpp" + +namespace physics { + + struct rigid_body + { + float inverse_mass; + vec3 position; + vec4 orientation; + vec3 velocity; + vec3 rotation; + mat4x4 transform_matrix; + mat3x3 inverse_inertia_tensor; + mat3x3 inverse_inertia_tensor_world; + vec3 force_accum; + vec3 torque_accum; + float linear_damping; + float angular_damping; + vec3 acceleration; + vec3 last_frame_acceleration; + + void calculate_derived_data(); + + void set_inertia_tensor(const mat3x3& inertia_tensor); + + void clear_accumulators(); + + void add_force_at_local_point(const vec3& force, + const vec3& point); + + void add_force_at_point(const vec3& force, + const vec3& point); + + void integrate(float duration); + + inline float get_mass() const + { + float sqrt_mass = 1.0f / sqrt(inverse_mass); + return sqrt_mass * sqrt_mass; + } + }; + +} diff --git a/src/physics/force_generator.cpp b/src/physics/force_generator.cpp new file mode 100644 index 0000000..c82a1f2 --- /dev/null +++ b/src/physics/force_generator.cpp @@ -0,0 +1,42 @@ +#include "force_generator.hpp" + +namespace physics { + + void gravity::update_force(rigid_body * body, float duration) + { + body->force_accum += gravity * body->get_mass(); + } + + void spring::update_force(rigid_body * body, float duration) + { + vec3 pa = body->transform_matrix * connection_point; + vec3 pb = other->transform_matrix * other_connection_point; + + vec3 force = pa - pb; + + float mag = magnitude(force); + float intensity = abs(mag - rest_length) * spring_constant; + + body->add_force_at_point(normalize(force) * -intensity, pa); + } + + void buoyancy::update_force(rigid_body * body, float duration) + { + vec3 center_of_buoyancy_world = body->transform_matrix * center_of_buoyancy; + float depth = center_of_buoyancy_world.y; + + if (depth >= water_height + max_depth) + return; // not inside the water + + vec3 force(0, 0, 0); + + if (depth <= water_height - max_depth) { + // fully submerged + force.y = liquid_density * volume; + } else { + // partially submerged + force.y = liquid_density * volume * (depth - max_depth - water_height) / 2.0f * max_depth; + } + body->add_force_at_point(force, center_of_buoyancy); + } +} diff --git a/src/physics/force_generator.hpp b/src/physics/force_generator.hpp new file mode 100644 index 0000000..2fcefa9 --- /dev/null +++ b/src/physics/force_generator.hpp @@ -0,0 +1,49 @@ +#pragma once + +#include "body.hpp" + +namespace physics { + + struct force_generator + { + virtual void update_force(rigid_body * body, float duration) = 0; + }; + + struct gravity : force_generator + { + vec3 gravity; + + void update_force(rigid_body * body, float duration) override; + }; + + struct spring : force_generator + { + // local coordinates + vec3 connection_point; + + // other local coordinates + vec3 other_connection_point; + + rigid_body * other; + + float spring_constant; + float rest_length; + + void update_force(rigid_body * body, float duration) override; + }; + + struct buoyancy : force_generator + { + float max_depth; + + float volume; + + float water_height; + + float liquid_density; + + vec3 center_of_buoyancy; + + void update_force(rigid_body * body, float duration) override; + }; +} diff --git a/src/platform/graphics.cpp b/src/platform/graphics.cpp index f64737f..3defb5b 100644 --- a/src/platform/graphics.cpp +++ b/src/platform/graphics.cpp @@ -25,6 +25,7 @@ #include "demo/ballistics.hpp" #include "demo/bridge.hpp" +#include "demo/sailboat.hpp" #include "maple/maple_bus_bits.hpp" @@ -94,7 +95,8 @@ namespace graphics { demo::ballistics _ballistics; demo::bridge _bridge; - demo::scene * current_scene = &_bridge; + demo::sailboat _sailboat; + demo::scene * current_scene = &_sailboat; void draw() {