demo: add bridge

demo.mk

@@ -19,7 +19,10 @@ DEMO_OBJ = \
 	src/platform/texture.o \
 	src/platform/font.o \
 	src/demo/ballistics.o \
+	src/demo/bridge.o \
 	src/physics/particle.o \
+	src/physics/particle_link.o \
+	src/physics/particle_contact.o
 
 demo.elf: LDSCRIPT = $(LIB)/main.lds
 demo.elf: $(START_OBJ) $(DEMO_OBJ) $(FONT_OBJ) $(LIBGCC)

src/demo/ballistics.cpp

@@ -16,13 +16,21 @@ namespace demo {
     draw_cube(writer, trans * translate(particle.position) * scale(1.f), color);
   }
 
-  void ballistics::init()
+  mat4x4 ballistics::init()
   {
     current_projectile_type = projectile_type::ARTILLERY;
 
     for (int i = 0; i < max_projectiles; i++) {
       projectiles[i].type = projectile_type::NONE;
     }
+
+    mat4x4 view_trans
+      = translate(vec3(-0.5, 0.5, 0.75))
+      * rotate_x(pi / 4)
+      * rotate_y(pi / 4)
+      * scale(vec3(-1, -1, 1));
+
+    return view_trans;
   }
 
   void ballistics::a()

src/demo/ballistics.hpp

@@ -29,7 +29,7 @@ namespace demo {
     projectile_type current_projectile_type;
     int32_t tick;
 
-    void init() override;
+    mat4x4 init() override;
     void a() override;
     void x() override;
     void update() override;

src/demo/bridge.cpp

@@ -0,0 +1,191 @@
+#include "math/float_types.hpp"
+#include "math/transform.hpp"
+
+#include "platform/graphics_primitive.hpp"
+#include "platform/font.hpp"
+
+#include "physics/particle_contact.hpp"
+
+#include "demo/bridge.hpp"
+
+#include "assert.h"
+
+namespace demo {
+
+  mat4x4 bridge::init()
+  {
+    // particles
+    for (int i = 0; i < particles_length; i++) {
+      float x = float(i / 2) * 2.0f - 5.0f;
+      float z = float(i % 2) * 2.0f - 1.0f;
+
+      particles[i].position = vec3(x, 4, z);
+      particles[i].velocity = vec3(0, 0, 0);
+      particles[i].damping = 0.95f;
+      particles[i].acceleration = vec3(0, -9.81, 0);
+      //particles[i].acceleration = vec3(0, -0.5, 0);
+      particles[i].force_accum = vec3(0, 0, 0);
+    }
+
+    // cables
+    for (int i = 0; i < cables_length; i++) {
+      cables[i].particle[0] = &particles[i];
+      cables[i].particle[1] = &particles[i+2];
+      cables[i].max_length = 1.9f;
+      cables[i].restitution = 0.5f;
+    }
+
+    // anchors
+    for (int i = 0; i < anchors_length; i++) {
+      anchors[i].particle = &particles[i];
+      float x = float(i / 2) * 2.2f - 5.5f;
+      float z = float(i % 2) * 1.6f - 0.8f;
+      anchors[i].anchor = vec3(x, 6, z);
+      if (i < 6)
+        anchors[i].max_length = float(i / 2) * 0.5f + 3.0f;
+      else
+        anchors[i].max_length = 5.5f - float(i / 2) * 0.5f;
+      anchors[i].restitution = 0.5f;
+    }
+
+    // rods
+    for (int i = 0; i < rods_length; i++) {
+      rods[i].particle[0] = &particles[i * 2];
+      rods[i].particle[1] = &particles[i * 2 + 1];
+      rods[i].length = 3;
+    }
+
+    mat4x4 view_trans
+      = translate(vec3(0.0, 0.0, 0.5))
+      * rotate_x(pi / 4)
+      * rotate_y(pi / 12)
+      * scale(vec3(-1, -1, 1));
+
+    return view_trans;
+  }
+
+  static int en_cables = 1;
+
+  void bridge::a()
+  //void bridge::update()
+  {
+    en_cables = !en_cables;
+    printf("en cables %d\n", en_cables);
+  }
+
+  void bridge::_update()
+  {
+    for (int i = 0; i < particles_length; i++) {
+      particles[i].force_accum = vec3(0, 0, 0);
+      particles[i].inverse_mass = 1.0f / 1.0f;
+    }
+
+    float duration = 1.f / 120.0f;
+
+    for (int i = 0; i < particles_length; i++) {
+      particles[i].integrate(duration);
+    }
+
+    // contact generators
+    const int max_contacts = 128;
+    static physics::particle_contact contacts[max_contacts];
+    int contact_ix = 0;
+
+    for (int i = 0; i < anchors_length; i++) {
+      int len = anchors[i].fill_contact(&contacts[contact_ix], 1);
+      contact_ix += len;
+      assert(contact_ix <= 128);
+    }
+
+    if (en_cables) {
+      for (int i = 0; i < cables_length; i++) {
+        int len = cables[i].fill_contact(&contacts[contact_ix], 1);
+        contact_ix += len;
+        assert(contact_ix <= 128);
+      }
+
+      for (int i = 0; i < rods_length; i++) {
+        int len = rods[i].fill_contact(&contacts[contact_ix], 1);
+        contact_ix += len;
+        assert(contact_ix <= 128);
+      }
+    }
+
+    static physics::particle_contact_resolver resolver;
+    resolver.max_iterations = contact_ix * 2;
+    resolver.resolve_contacts(contacts, contact_ix, duration);
+  }
+
+  void bridge::update()
+  //void bridge::a()
+  {
+    _update();
+    _update();
+  }
+
+  void bridge::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: bridge", 0xffffffff);
+  }
+
+  void bridge::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 * scale(5.f));
+
+    // draw particles
+    for (int i = 0; i < particles_length; i++) {
+      const vec3 color(1.0, 1.0, 1.0);
+      draw_cube(writer, trans * translate(particles[i].position) * scale(0.1f), color);
+    }
+
+    // draw cables
+    global_polygon_intensity(writer, vec3(1.0, 1.0, 0.0));
+    for (int i = 0; i < cables_length; i++) {
+      vec3 p1 = screen_transform(trans * cables[i].particle[0]->position);
+      vec3 p2 = screen_transform(trans * cables[i].particle[1]->position);
+
+      draw_line(writer,
+                p1,
+                p2);
+    }
+
+    // draw anchors
+    global_polygon_intensity(writer, vec3(0.0, 1.0, 1.0));
+    for (int i = 0; i < anchors_length; i++) {
+      vec3 p1 = screen_transform(trans * anchors[i].particle->position);
+      vec3 p2 = screen_transform(trans * anchors[i].anchor);
+
+      draw_line(writer,
+                p1,
+                p2);
+    }
+
+    // draw rods
+    global_polygon_intensity(writer, vec3(1.0, 0.0, 1.0));
+    for (int i = 0; i < anchors_length; i++) {
+      vec3 p1 = screen_transform(trans * rods[i].particle[0]->position);
+      vec3 p2 = screen_transform(trans * rods[i].particle[1]->position);
+
+      draw_line(writer,
+                p1,
+                p2);
+    }
+
+    writer.append<ta_global_parameter::end_of_list>() =
+      ta_global_parameter::end_of_list(para_control::para_type::end_of_list);
+  }
+}

src/demo/bridge.hpp

@@ -0,0 +1,31 @@
+#pragma once
+
+#include "demo/scene.hpp"
+#include "physics/particle.hpp"
+#include "physics/particle_link.hpp"
+
+namespace demo {
+
+  struct bridge : scene {
+    mat4x4 init() override;
+
+    void _update();
+    void update() override;
+
+    void draw_hud(ta_parameter_writer& writer);
+    void draw(ta_parameter_writer& writer, const mat4x4& trans) override;
+    void a() override;
+
+    static const inline int particles_length = 12;
+    physics::particle particles[particles_length];
+
+    static const inline int cables_length = 10;
+    physics::particle_cable cables[cables_length];
+
+    static const inline int anchors_length = 12;
+    physics::particle_cable_constraint anchors[anchors_length];
+
+    static const inline int rods_length = 6;
+    physics::particle_rod rods[anchors_length];
+  };
+}

src/demo/scene.hpp

@@ -6,7 +6,7 @@
 namespace demo {
 
   struct scene {
-    virtual void init() = 0;
+    virtual mat4x4 init() = 0;
     virtual void a() {}
     virtual void b() {}
     virtual void x() {}

src/physics/particle_contact.cpp

@@ -0,0 +1,105 @@
+#include "physics/particle_contact.hpp"
+
+namespace physics {
+
+  void particle_contact::resolve(float duration)
+  {
+    resolve_velocity(duration);
+    resolve_interpenetration(duration);
+  }
+
+  float particle_contact::calculate_separating_velocity() const
+  {
+    vec3 relative_velocity = particle[0]->velocity;
+    if (particle[1])
+      relative_velocity -= particle[1]->velocity;
+
+    return dot(relative_velocity, contact_normal);
+  }
+
+  void particle_contact::resolve_velocity(float duration)
+  {
+    float separating_velocity = calculate_separating_velocity();
+
+    if (separating_velocity > 0) {
+      // no impulse required
+      return;
+    }
+
+    float new_separating_velocity = -separating_velocity * restitution;
+
+    // velocity build-up due to acceleration only
+    vec3 acceleration_caused_velocity = particle[0]->acceleration;
+    if (particle[1]) acceleration_caused_velocity -= particle[1]->acceleration;
+    float acceleration_caused_separating_velocity = dot(acceleration_caused_velocity, contact_normal) * duration;
+
+    // remove acceleration velocity from separating velocity
+    if (acceleration_caused_separating_velocity < 0) {
+      new_separating_velocity += restitution * acceleration_caused_separating_velocity;
+
+      if (new_separating_velocity < 0) new_separating_velocity = 0;
+    }
+
+    float delta_velocity = new_separating_velocity - separating_velocity;
+
+    float total_inverse_mass = get_total_inverse_mass();
+
+    if (total_inverse_mass <= 0)
+      return;
+
+    float impulse = delta_velocity / total_inverse_mass;
+
+    vec3 impulse_per_inverse_mass = contact_normal * impulse;
+
+    particle[0]->velocity
+      = particle[0]->velocity
+      + impulse_per_inverse_mass * particle[0]->inverse_mass;
+
+    if (particle[1]) {
+      particle[1]->velocity
+        = particle[1]->velocity
+        + impulse_per_inverse_mass * -particle[1]->inverse_mass;
+    }
+  }
+
+  void particle_contact::resolve_interpenetration(float duration)
+  {
+    if (penetration <= 0) return;
+
+    float total_inverse_mass = get_total_inverse_mass();
+
+    if (total_inverse_mass <= 0) return;
+
+    vec3 move_per_inverse_mass = contact_normal * (penetration / total_inverse_mass);
+
+    particle[0]->position += move_per_inverse_mass * particle[0]->inverse_mass;
+    if (particle[1]) {
+      particle[1]->position += move_per_inverse_mass * -particle[1]->inverse_mass;
+    }
+  }
+
+  void particle_contact_resolver::resolve_contacts(particle_contact * contacts,
+                                                   int contacts_length,
+                                                   float duration)
+  {
+    iterations = 0;
+
+    while (iterations < max_iterations) {
+      float max = 0;
+      int max_index = -1;
+      for (int i = 0; i < contacts_length; i++) {
+        float separating_velocity = contacts[i].calculate_separating_velocity();
+        if (separating_velocity < max) {
+          max = separating_velocity;
+          max_index = i;
+        }
+      }
+
+      if (max_index == -1) break;
+
+      contacts[max_index].resolve(duration);
+
+      iterations += 1;
+    }
+  }
+}

src/physics/particle_contact.hpp

@@ -0,0 +1,43 @@
+#pragma once
+
+#include "physics/particle.hpp"
+
+namespace physics {
+
+  struct particle_contact
+  {
+    physics::particle * particle[2];
+
+    float restitution;
+
+    vec3 contact_normal;
+
+    float penetration;
+
+    void resolve(float duration);
+
+    float calculate_separating_velocity() const;
+
+    void resolve_velocity(float duration);
+
+    void resolve_interpenetration(float duration);
+
+    inline float get_total_inverse_mass()
+    {
+      float total_inverse_mass = particle[0]->inverse_mass;
+      if (particle[1]) total_inverse_mass += particle[1]->inverse_mass;
+      return total_inverse_mass;
+    }
+  };
+
+  struct particle_contact_resolver
+  {
+    int max_iterations;
+    int iterations;
+
+    void resolve_contacts(particle_contact * contacts,
+                          int contacts_length,
+                          float duration);
+  };
+
+}

src/physics/particle_link.cpp

@@ -0,0 +1,76 @@
+#include "particle_link.hpp"
+
+namespace physics {
+
+  float particle_link::current_length() const
+  {
+    vec3 relative_position = particle[0]->position - particle[1]->position;
+
+    return magnitude(relative_position);
+  }
+
+  int particle_cable::fill_contact(particle_contact * contact, int contact_length) const
+  {
+    float cur_length = current_length();
+    if (cur_length < max_length)
+      return 0;
+
+    contact->particle[0] = particle[0];
+    contact->particle[1] = particle[1];
+
+    vec3 normal = normalize(particle[1]->position - particle[0]->position);
+    contact->contact_normal = normal;
+    contact->penetration = cur_length - max_length;
+    contact->restitution = restitution;
+
+    return 1;
+  }
+
+  int particle_rod::fill_contact(particle_contact * contact, int contact_length) const
+  {
+    float cur_length = current_length();
+    if (cur_length == length)
+      return 0;
+
+    contact->particle[0] = particle[0];
+    contact->particle[1] = particle[1];
+
+    vec3 normal = normalize(particle[1]->position - particle[0]->position);
+
+    if (cur_length > length) {
+      contact->contact_normal = normal;
+      contact->penetration = cur_length - length;
+    } {
+      contact->contact_normal = -normal;
+      contact->penetration = length - cur_length;
+    }
+
+    contact->restitution = 0;
+
+    return 1;
+  }
+
+  float particle_constraint::current_length() const
+  {
+    vec3 relative_position = particle->position - anchor;
+
+    return magnitude(relative_position);
+  }
+
+  int particle_cable_constraint::fill_contact(particle_contact * contact, int contact_length) const
+  {
+    float cur_length = current_length();
+    if (cur_length < max_length)
+      return 0;
+
+    contact->particle[0] = particle;
+    contact->particle[1] = nullptr;
+
+    vec3 normal = normalize(anchor - particle->position);
+    contact->contact_normal = normal;
+    contact->penetration = cur_length - max_length;
+    contact->restitution = restitution;
+
+    return 1;
+  }
+}

src/physics/particle_link.hpp

@@ -0,0 +1,50 @@
+#pragma once
+
+#include "particle.hpp"
+#include "particle_contact.hpp"
+
+namespace physics {
+
+  struct particle_link
+  {
+    physics::particle * particle[2];
+
+    float current_length() const;
+
+    virtual int fill_contact(particle_contact * contact, int contact_length) const = 0;
+  };
+
+  struct particle_cable : particle_link
+  {
+    float max_length;
+    float restitution;
+
+    int fill_contact(particle_contact * contact, int contact_length) const override;
+  };
+
+  struct particle_rod : particle_link
+  {
+    float length;
+
+    int fill_contact(particle_contact * contact, int contact_length) const override;
+  };
+
+  struct particle_constraint
+  {
+    physics::particle * particle;
+
+    vec3 anchor;
+
+    float current_length() const;
+
+    virtual int fill_contact(particle_contact * contact, int contact_length) const = 0;
+  };
+
+  struct particle_cable_constraint : particle_constraint
+  {
+    float max_length;
+    float restitution;
+
+    int fill_contact(particle_contact * contact, int contact_length) const override;
+  };
+}

src/platform/graphics.cpp

@@ -24,6 +24,7 @@
 #include "platform/font.hpp"
 
 #include "demo/ballistics.hpp"
+#include "demo/bridge.hpp"
 
 #include "maple/maple_bus_bits.hpp"
 
@@ -92,7 +93,8 @@ namespace graphics {
   }
 
   demo::ballistics _ballistics;
-  demo::scene * current_scene = &_ballistics;
+  demo::bridge _bridge;
+  demo::scene * current_scene = &_bridge;
 
   void draw()
   {
@@ -104,7 +106,7 @@ namespace graphics {
     core_init();
     framebuffer::scaler_init();
 
-    current_scene->init();
+    view_trans = current_scene->init();
 
     // read
     while (spg_status::vsync(holly.SPG_STATUS));
@@ -144,17 +146,6 @@ namespace graphics {
                            1,
                            texture_memory_alloc.region_array.start,
                            texture_memory_alloc.object_list.start);
-
-    //vec3 t = {framebuffer::framebuffer.px_width / 2.f, framebuffer::framebuffer.px_height / 2.f, 0};
-    float s = framebuffer::framebuffer.px_height / 3.f;
-
-    view_trans
-      = scale(vec3(s, s, 1))
-      * translate(vec3(0, 0, 10))
-      * rotate_x(pi / 4)
-      * rotate_y(pi / 4)
-      * scale(vec3(-1, -1, 1));
-    view_trans = view_trans * translate(vec3(0, 0, -0.9));
   }
 
   void input_events()
@@ -178,7 +169,7 @@ namespace graphics {
         auto& data = data_fields.data;
 
         //float dx = static_cast<float>(data.analog_coordinate_axis[2] - 0x80) * 0.001;
-        //float dy = static_cast<float>(data.analog_coordinate_axis[3] - 0x80) * 0.001;
+        float dy = static_cast<float>(data.analog_coordinate_axis[3] - 0x80) * 0.001;
 
         bool a = ft0::data_transfer::digital_button::a(data.digital_button) == 0;
         bool b = ft0::data_transfer::digital_button::b(data.digital_button) == 0;
@@ -193,7 +184,7 @@ namespace graphics {
         last[port_ix].x = x;
         last[port_ix].y = y;
 
-        //view_trans = view_trans * rotate_x(dx) * rotate_y(dy);
+        view_trans = view_trans * rotate_y(dy);
       }
     }
   }

src/platform/graphics_primitive.hpp

@@ -33,10 +33,11 @@ static inline vec3 screen_transform(const vec3& v)
 {
   float tx = framebuffer::framebuffer.px_width / 2.f;
   float ty = framebuffer::framebuffer.px_height / 2.f;
+  float s = framebuffer::framebuffer.px_height / 2.f;
 
-  float z = _fsrra(v.z - 9);
+  float z = _fsrra(v.z);
   float z2 = z * z;
-  return {v.x * z2 + tx , v.y * z2 + ty, z2};
+  return {(v.x) * s * z2 + tx , (v.y) * s * z2 + ty , z2};
 }
 
 static inline void draw_grid(ta_parameter_writer& writer,