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/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/physics/particle_contact.cpp

@@ -58,7 +58,7 @@ namespace physics {
     if (particle[1]) {
       particle[1]->velocity
         = particle[1]->velocity
-        + impulse_per_inverse_mass * -particle[0]->inverse_mass;
+        + impulse_per_inverse_mass * -particle[1]->inverse_mass;
     }
   }
 
@@ -70,11 +70,11 @@ namespace physics {
 
     if (total_inverse_mass <= 0) return;
 
-    vec3 move_per_inverse_mass = contact_normal * (-penetration / total_inverse_mass);
+    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;
+      particle[1]->position += move_per_inverse_mass * -particle[1]->inverse_mass;
     }
   }
 

src/physics/particle_contact.hpp

@@ -25,7 +25,7 @@ namespace physics {
     inline float get_total_inverse_mass()
     {
       float total_inverse_mass = particle[0]->inverse_mass;
-      if (particle[0]) total_inverse_mass += particle[1]->inverse_mass;
+      if (particle[1]) total_inverse_mass += particle[1]->inverse_mass;
       return total_inverse_mass;
     }
   };

src/physics/particle_link.cpp

@@ -49,4 +49,28 @@ namespace physics {
 
     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

@@ -26,8 +26,25 @@ namespace physics {
   {
     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()
   {
@@ -167,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;
@@ -182,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);
       }
     }
   }