diff --git a/demo.mk b/demo.mk index 50b6cf1..8e3564b 100644 --- a/demo.mk +++ b/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) diff --git a/src/demo/bridge.cpp b/src/demo/bridge.cpp new file mode 100644 index 0000000..d6fde89 --- /dev/null +++ b/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(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(para_control::para_type::end_of_list); + } +} diff --git a/src/demo/bridge.hpp b/src/demo/bridge.hpp new file mode 100644 index 0000000..b13b764 --- /dev/null +++ b/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]; + }; +} diff --git a/src/physics/particle_contact.cpp b/src/physics/particle_contact.cpp index 8a7f78d..8aa21b4 100644 --- a/src/physics/particle_contact.cpp +++ b/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; } } diff --git a/src/physics/particle_contact.hpp b/src/physics/particle_contact.hpp index 761a5a3..05fdacf 100644 --- a/src/physics/particle_contact.hpp +++ b/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; } }; diff --git a/src/physics/particle_link.cpp b/src/physics/particle_link.cpp index 1357d2d..ef2961a 100644 --- a/src/physics/particle_link.cpp +++ b/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; + } } diff --git a/src/physics/particle_link.hpp b/src/physics/particle_link.hpp index d6913e1..26ac7f0 100644 --- a/src/physics/particle_link.hpp +++ b/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; }; } diff --git a/src/platform/graphics.cpp b/src/platform/graphics.cpp index 9906f86..e3406b6 100644 --- a/src/platform/graphics.cpp +++ b/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(data.analog_coordinate_axis[2] - 0x80) * 0.001; - //float dy = static_cast(data.analog_coordinate_axis[3] - 0x80) * 0.001; + float dy = static_cast(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); } } }