demo: add sailboat

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)

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>() =
+      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>() =
+      ta_global_parameter::end_of_list(para_control::para_type::end_of_list);
+  }
+}

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;
+  };
+}

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();
+  }
+}

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<float>(inverse_mass);
+      return sqrt_mass * sqrt_mass;
+    }
+  };
+
+}

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);
+  }
+}

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;
+  };
+}

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()
   {