demo: add collision

This commit is contained in:
Zack Buhman 2025-07-25 13:32:07 -05:00
parent edeeec34b6
commit e2a005d2b9
11 changed files with 665 additions and 53 deletions

View File

@ -15,17 +15,20 @@ DEMO_OBJ = \
$(LIB)/printf/parse.o \ $(LIB)/printf/parse.o \
src/platform/main.o \ src/platform/main.o \
src/platform/graphics.o \ src/platform/graphics.o \
src/platform/graphics_primitive.o \
src/platform/input.o \ src/platform/input.o \
src/platform/texture.o \ src/platform/texture.o \
src/platform/font.o \ src/platform/font.o \
src/demo/ballistics.o \ src/demo/ballistics.o \
src/demo/bridge.o \ src/demo/bridge.o \
src/demo/sailboat.o \ src/demo/sailboat.o \
src/demo/collision.o \
src/physics/particle.o \ src/physics/particle.o \
src/physics/particle_link.o \ src/physics/particle_link.o \
src/physics/particle_contact.o \ src/physics/particle_contact.o \
src/physics/body.o \ src/physics/body.o \
src/physics/force_generator.o src/physics/force_generator.o \
src/physics/collide.o
demo.elf: LDSCRIPT = $(LIB)/main.lds demo.elf: LDSCRIPT = $(LIB)/main.lds
demo.elf: $(START_OBJ) $(DEMO_OBJ) $(FONT_OBJ) $(LIBGCC) demo.elf: $(START_OBJ) $(DEMO_OBJ) $(FONT_OBJ) $(LIBGCC)

124
src/demo/collision.cpp Normal file
View File

@ -0,0 +1,124 @@
#include "math/float_types.hpp"
#include "math/transform.hpp"
#include "platform/graphics_primitive.hpp"
#include "platform/font.hpp"
#include "demo/collision.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);
}
mat4x4 collision::init()
{
plane.body = &plane_body;
plane.body->position = {0, 0, 0};
plane.body->orientation = {0, 0, 0, 1};
plane.body->calculate_derived_data();
plane.normal = {0, 1, 0};
plane.distance = 0;
plane.offset = mat4x4();
plane.update_transform();
sphere.body = &sphere_body;
sphere.body->inverse_mass = 1.0f / 1.0f;
sphere.body->position = {0, 1, 0};
sphere.body->orientation = {0, 0, 0, 1};
sphere.body->velocity = {0, 0, 0};
sphere.body->rotation = {0, 0, 0};
sphere.body->set_inertia_tensor(inertia_tensor_coeffs(5.0, 5.0, 5.0));
sphere.body->linear_damping = 0.998f;
sphere.body->angular_damping = 0.998f;
sphere.body->acceleration = {0, -9.81, 0};
sphere.body->calculate_derived_data();
sphere.radius = 0.25f;
sphere.offset = mat4x4();
sphere.update_transform();
collision_data.ix = 0;
collision_data.length = 128;
collision_data.restitution = 0.2;
collision_data.friction = 0.9;
mat4x4 view_trans
= translate(vec3(0.0, 0.0, 0.5))
* rotate_x(pi / 4)
* rotate_y(pi / 4)
* scale(vec3(-3, -3, 3));
return view_trans;
}
void collision::update()
{
sphere.body->clear_accumulators();
// generate contacts
collision_data.ix = 0;
int res = physics::collision_detector::sphere_and_halfspace(sphere, plane, collision_data);
printf("res %d\n", res);
// set iterations
// resolve contacts
float duration = 1.0f / 600.0f;
sphere.body->integrate(duration);
sphere.update_transform();
}
void collision::draw_hud(ta_parameter_writer& writer)
{
const int title_length = 15;
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: collision", 0xffffffff);
}
void collision::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_icosphere(writer,
trans * sphere.transform * scale(sphere.radius),
vec3(1, 0, 1));
vec3 n = plane.normal;
vec3 k = plane.normal * plane.distance;
vec3 color(1, 1, 0);
draw_halfspace(writer,
k,
n,
trans,
color);
writer.append<ta_global_parameter::end_of_list>() =
ta_global_parameter::end_of_list(para_control::para_type::end_of_list);
}
}
extern "C"
void memset()
{
}

28
src/demo/collision.hpp Normal file
View File

@ -0,0 +1,28 @@
#pragma once
#include "demo/scene.hpp"
#include "physics/collide.hpp"
namespace demo {
struct collision : scene {
physics::contact contacts[128];
physics::collision_data collision_data;
physics::rigid_body plane_body;
physics::collision_plane plane;
physics::rigid_body sphere_body;
physics::collision_sphere sphere;
collision()
: collision_data(contacts, 128, 0.2, 0.9)
{}
mat4x4 init() override;
void update() override;
void draw_hud(ta_parameter_writer& writer);
void draw(ta_parameter_writer& writer, const mat4x4& trans) override;
};
}

View File

@ -25,7 +25,6 @@ namespace demo {
0.3f * mass * (squares.x + squares.y)); 0.3f * mass * (squares.x + squares.y));
} }
mat4x4 sailboat::init() mat4x4 sailboat::init()
{ {
buoyancy.center_of_buoyancy = vec3(0.0f, 0.5f, 0.0f); buoyancy.center_of_buoyancy = vec3(0.0f, 0.5f, 0.0f);

107
src/physics/collide.cpp Normal file
View File

@ -0,0 +1,107 @@
#include "physics/collide.hpp"
namespace physics {
int collision_detector::sphere_and_sphere(const collision_sphere& a,
const collision_sphere& b,
collision_data& data)
{
if (data.ix >= data.length)
return 0;
vec3 a_position = col(a.transform, 3);
vec3 b_position = col(b.transform, 3);
vec3 mid = a_position - b_position;
float length = magnitude(mid);
if (length >= (a.radius + b.radius)) {
return 0;
}
vec3 normal = mid * (1.0f / length);
contact * contact = data.add_contact();
contact->contact_normal = normal;
contact->contact_point = a_position + mid * 0.5f;
contact->penetration = a.radius + b.radius - length;
contact->body[0] = a.body;
contact->body[1] = b.body;
contact->restitution = data.restitution;
contact->friction = data.friction;
return 1;
}
int collision_detector::sphere_and_halfspace(const collision_sphere& sphere,
const collision_plane& plane,
collision_data& data)
{
if (data.ix >= data.length)
return 0;
vec3 position = col(sphere.transform, 3);
float distance = dot(plane.normal, position) - sphere.radius - plane.distance;
if (distance >= 0)
return 0;
contact * contact = data.add_contact();
contact->contact_normal = plane.normal;
contact->penetration = -distance;
contact->contact_point = position - plane.normal * (distance + sphere.radius);
contact->body[0] = sphere.body;
contact->body[1] = nullptr;
contact->restitution = data.restitution;
contact->friction = data.friction;
return 1;
}
int collision_detector::sphere_and_plane(const collision_sphere& sphere,
const collision_plane& plane,
collision_data& data)
{
if (data.ix >= data.length)
return 0;
vec3 position = col(sphere.transform, 3);
float distance = dot(plane.normal, position) - plane.distance;
if (distance * distance > sphere.radius * sphere.radius) {
return 0;
}
vec3 normal;
float penetration;
if (distance < 0) {
normal = -plane.normal;
penetration = distance;
} else {
normal = plane.normal;
penetration = -distance;
}
penetration += sphere.radius;
contact * contact = data.add_contact();
contact->contact_normal = normal;
contact->penetration = penetration;
contact->contact_point = position - plane.normal * distance;
contact->body[0] = sphere.body;
contact->body[1] = nullptr;
contact->restitution = data.restitution;
contact->friction = data.friction;
return 1;
}
int collision_detector::box_and_halfspace(const collision_box& box,
const collision_plane& plane,
collision_data& data)
{
if (data.ix >= data.length)
return 0;
return 0;
}
}

78
src/physics/collide.hpp Normal file
View File

@ -0,0 +1,78 @@
#include "physics/body.hpp"
#include "physics/contact.hpp"
namespace physics {
struct collision_primitive
{
rigid_body * body;
mat4x4 offset;
mat4x4 transform;
inline void update_transform()
{
transform = body->transform_matrix * offset;
}
};
struct collision_sphere : collision_primitive
{
float radius;
};
struct collision_plane : collision_primitive
{
vec3 normal;
float distance;
};
struct collision_box : collision_primitive
{
vec3 half_size;
};
struct collision_data
{
contact * const contacts;
int ix;
int length; // const
// what...
float restitution; // const
float friction; // const
collision_data(contact * contacts,
int length,
float restitution,
float friction)
: contacts(contacts), ix(0), length(length), restitution(restitution), friction(friction)
{}
inline contact * add_contact()
{
contact * contact = &contacts[ix];
ix += 1;
return contact;
}
};
struct collision_detector
{
static int sphere_and_sphere(const collision_sphere& a,
const collision_sphere& b,
collision_data& data);
static int sphere_and_halfspace(const collision_sphere& sphere,
const collision_plane& plane,
collision_data& data);
static int sphere_and_plane(const collision_sphere& sphere,
const collision_plane& plane,
collision_data& data);
static int box_and_halfspace(const collision_box& box,
const collision_plane& plane,
collision_data& data);
};
}

16
src/physics/contact.hpp Normal file
View File

@ -0,0 +1,16 @@
#include "math/float_types.hpp"
#include "physics/body.hpp"
namespace physics {
struct contact {
rigid_body * body[2];
float restitution;
float friction;
vec3 contact_point;
vec3 contact_normal;
float penetration;
};
}

View File

@ -26,6 +26,7 @@
#include "demo/ballistics.hpp" #include "demo/ballistics.hpp"
#include "demo/bridge.hpp" #include "demo/bridge.hpp"
#include "demo/sailboat.hpp" #include "demo/sailboat.hpp"
#include "demo/collision.hpp"
#include "maple/maple_bus_bits.hpp" #include "maple/maple_bus_bits.hpp"
@ -96,7 +97,8 @@ namespace graphics {
demo::ballistics _ballistics; demo::ballistics _ballistics;
demo::bridge _bridge; demo::bridge _bridge;
demo::sailboat _sailboat; demo::sailboat _sailboat;
demo::scene * current_scene = &_sailboat; demo::collision _collision;
demo::scene * current_scene = &_collision;
void draw() void draw()
{ {
@ -188,7 +190,7 @@ namespace graphics {
current_scene->analog(dx); current_scene->analog(dx);
view_trans = view_trans * rotate_y(dy); view_trans = view_trans * rotate_y(dy);// * rotate_x(dx);
} }
} }
} }

View File

@ -0,0 +1,263 @@
#include "math/transform.hpp"
#include "platform/graphics_primitive.hpp"
static inline float _intensity(const mat4x4& trans, const vec3& normal)
{
const vec3 light_vec = {-1, -1, -10};
vec3 n = normal_multiply(trans, normal);
float n_dot_l = dot(n, light_vec);
float intensity = 0.5f;
if (n_dot_l > 0)
intensity += 0.5f * n_dot_l * (inverse_length(n) * inverse_length(light_vec));
return intensity;
}
void draw_cube(ta_parameter_writer& writer,
const mat4x4& trans,
const vec3& color)
{
static const vec3 position[] = {
{-1.0, -1.0, 1.0},
{-1.0, 1.0, 1.0},
{-1.0, -1.0, -1.0},
{-1.0, 1.0, -1.0},
{1.0, -1.0, 1.0},
{1.0, 1.0, 1.0},
{1.0, -1.0, -1.0},
{1.0, 1.0, -1.0},
};
const int position_length = (sizeof (position)) / (sizeof (position[0]));
struct quad {
int a, b, c, d;
};
static const quad quads[] = {
{0, 1, 3, 2},
{2, 3, 7, 6},
{6, 7, 5, 4},
{4, 5, 1, 0},
{2, 6, 4, 0},
{7, 3, 1, 5}
};
const int quads_length = (sizeof (quads)) / (sizeof (quads[0]));
vec3 cache[position_length];
for (int i = 0; i < position_length; i++) {
cache[i] = screen_transform(trans * position[i]);
}
global_polygon_intensity(writer, color);
for (int i = 0; i < quads_length; i++) {
const vec3& ap = cache[quads[i].a];
const vec3& bp = cache[quads[i].b];
const vec3& cp = cache[quads[i].c];
const vec3& dp = cache[quads[i].d];
quad_type_2(writer,
ap,
bp,
cp,
dp,
1.0f);
}
}
void draw_icosphere(ta_parameter_writer& writer,
const mat4x4& trans,
const vec3& color)
{
static const vec3 position[] = {
{0.000000, -1.000000, 0.000000},
{0.723600, -0.447215, 0.525720},
{-0.276385, -0.447215, 0.850640},
{-0.894425, -0.447215, 0.000000},
{-0.276385, -0.447215, -0.850640},
{0.723600, -0.447215, -0.525720},
{0.276385, 0.447215, 0.850640},
{-0.723600, 0.447215, 0.525720},
{-0.723600, 0.447215, -0.525720},
{0.276385, 0.447215, -0.850640},
{0.894425, 0.447215, 0.000000},
{0.000000, 1.000000, 0.000000},
};
const int position_length = (sizeof (position)) / (sizeof (position[0]));
static const vec3 normal[] = {
{0.1876, -0.7947, 0.5774},
{0.6071, -0.7947, -0.0000},
{-0.4911, -0.7947, 0.3568},
{-0.4911, -0.7947, -0.3568},
{0.1876, -0.7947, -0.5774},
{0.9822, -0.1876, -0.0000},
{0.3035, -0.1876, 0.9342},
{-0.7946, -0.1876, 0.5774},
{-0.7946, -0.1876, -0.5774},
{0.3035, -0.1876, -0.9342},
{0.7946, 0.1876, 0.5774},
{-0.3035, 0.1876, 0.9342},
{-0.9822, 0.1876, -0.0000},
{-0.3035, 0.1876, -0.9342},
{0.7946, 0.1876, -0.5774},
{0.4911, 0.7947, 0.3568},
{-0.1876, 0.7947, 0.5774},
{-0.6071, 0.7947, -0.0000},
{-0.1876, 0.7947, -0.5774},
{0.4911, 0.7947, -0.3568},
};
struct pn {
int p, n;
};
struct tri {
int a;
int b;
int c;
int n;
};
static const tri tris[] = {
{0, 1, 2, 0},
{1, 0, 5, 1},
{0, 2, 3, 2},
{0, 3, 4, 3},
{0, 4, 5, 4},
{1, 5, 10, 5},
{2, 1, 6, 6},
{3, 2, 7, 7},
{4, 3, 8, 8},
{5, 4, 9, 9},
{1, 10, 6, 10},
{2, 6, 7, 11},
{3, 7, 8, 12},
{4, 8, 9, 13},
{5, 9, 10, 14},
{6, 10, 11, 15},
{7, 6, 11, 16},
{8, 7, 11, 17},
{9, 8, 11, 18},
{10, 9, 11, 19},
};
const int tris_length = (sizeof (tris)) / (sizeof (tris[0]));
static vec3 position_cache[position_length];
for (int i = 0; i < position_length; i++) {
position_cache[i] = screen_transform(trans * position[i]);
}
global_polygon_intensity(writer, color);
for (int i = 0; i < tris_length; i++) {
const vec3& ap = position_cache[tris[i].a];
const vec3& bp = position_cache[tris[i].b];
const vec3& cp = position_cache[tris[i].c];
const float& intensity = _intensity(trans, normal[tris[i].n]);
tri_type_2(writer,
ap,
bp,
cp,
intensity);
}
}
void draw_plane(ta_parameter_writer& writer,
const vec3& k,
const vec3& n,
const mat4x4& trans,
const vec3& color)
{
static const vec3 position[] = {
{-1.000000, 0.000000, 1.000000},
{ 1.000000, 0.000000, 1.000000},
{-1.000000, 0.000000, -1.000000},
{ 1.000000, 0.000000, -1.000000},
};
//const vec3 normal = {0.0000 1.0000 0.0000};
vec3 u = {0, 1, 0};
if (dot(u, n) > 0.7071f) u = {0, 0, 1};
vec3 r = normalize(cross(u, n));
u = normalize(cross(n, r));
mat4x4 m = {
r.x, n.x, u.x, k.x,
r.y, n.y, u.y, k.y,
r.z, n.z, u.z, k.z,
0, 0, 0, 1
};
mat4x4 t = trans * m;
vec3 ap = screen_transform(t * position[0]);
vec3 bp = screen_transform(t * position[1]);
vec3 cp = screen_transform(t * position[3]);
vec3 dp = screen_transform(t * position[2]);
global_polygon_intensity(writer, color);
quad_type_2(writer,
ap,
bp,
cp,
dp,
1.0f);
}
void draw_halfspace(ta_parameter_writer& writer,
const vec3& k,
const vec3& n,
const mat4x4& trans,
const vec3& color)
{
static const vec3 position[] = {
{-1.000000, 0.000000, 1.000000},
{ 1.000000, 0.000000, 1.000000},
{-1.000000, 0.000000, -1.000000},
{ 1.000000, 0.000000, -1.000000},
};
const vec3 red = {0.906, 0.369, 0.373};
//const vec3 normal = {0.0000 1.0000 0.0000};
vec3 u = {0, 1, 0};
if (dot(u, n) > 0.7071f) u = {0, 0, 1};
vec3 r = normalize(cross(u, n));
u = normalize(cross(n, r));
mat4x4 m = {
r.x, n.x, u.x, k.x,
r.y, n.y, u.y, k.y,
r.z, n.z, u.z, k.z,
0, 0, 0, 1
};
mat4x4 t = trans * m;
vec3 ap = screen_transform(t * position[0]);
vec3 bp = screen_transform(t * position[1]);
vec3 cp = screen_transform(t * position[3]);
vec3 dp = screen_transform(t * position[2]);
global_polygon_intensity(writer,
color,
isp_tsp_instruction_word::culling_mode::cull_if_negative);
quad_type_2(writer,
ap,
bp,
cp,
dp,
1.0f);
global_polygon_intensity(writer,
red,
isp_tsp_instruction_word::culling_mode::cull_if_positive);
quad_type_2(writer,
ap,
bp,
cp,
dp,
1.0f);
}

View File

@ -87,53 +87,22 @@ static inline void draw_axis(ta_parameter_writer& writer,
z_axis); z_axis);
} }
static inline void draw_cube(ta_parameter_writer& writer, void draw_cube(ta_parameter_writer& writer,
const mat4x4& trans, const mat4x4& trans,
const vec3& color) const vec3& color);
{
static const vec3 position[] = {
{-1.0, -1.0, 1.0},
{-1.0, 1.0, 1.0},
{-1.0, -1.0, -1.0},
{-1.0, 1.0, -1.0},
{1.0, -1.0, 1.0},
{1.0, 1.0, 1.0},
{1.0, -1.0, -1.0},
{1.0, 1.0, -1.0},
};
const int position_length = (sizeof (position)) / (sizeof (position[0]));
struct quad { void draw_icosphere(ta_parameter_writer& writer,
int a, b, c, d; const mat4x4& trans,
}; const vec3& color);
static const quad quads[] = { void draw_plane(ta_parameter_writer& writer,
{0, 1, 3, 2}, const vec3& k,
{2, 3, 7, 6}, const vec3& n,
{6, 7, 5, 4}, const mat4x4& trans,
{4, 5, 1, 0}, const vec3& color);
{2, 6, 4, 0},
{7, 3, 1, 5}
};
vec3 cache[position_length]; void draw_halfspace(ta_parameter_writer& writer,
for (int i = 0; i < position_length; i++) { const vec3& k,
cache[i] = screen_transform(trans * position[i]); const vec3& n,
} const mat4x4& trans,
const vec3& color);
global_polygon_intensity(writer, color);
for (int i = 0; i < 6; i++) {
const vec3& ap = cache[quads[i].a];
const vec3& bp = cache[quads[i].b];
const vec3& cp = cache[quads[i].c];
const vec3& dp = cache[quads[i].d];
quad_type_2(writer,
ap,
bp,
cp,
dp,
1.0f);
}
}

View File

@ -9,7 +9,8 @@
#include "math/float_types.hpp" #include "math/float_types.hpp"
static inline void global_polygon_intensity(ta_parameter_writer& writer, static inline void global_polygon_intensity(ta_parameter_writer& writer,
const vec3& color) const vec3& color,
uint32_t culling_mode = isp_tsp_instruction_word::culling_mode::no_culling)
{ {
const uint32_t parameter_control_word = para_control::para_type::polygon_or_modifier_volume const uint32_t parameter_control_word = para_control::para_type::polygon_or_modifier_volume
| para_control::list_type::opaque | para_control::list_type::opaque
@ -17,7 +18,7 @@ static inline void global_polygon_intensity(ta_parameter_writer& writer,
; ;
const uint32_t isp_tsp_instruction_word = isp_tsp_instruction_word::depth_compare_mode::greater_or_equal const uint32_t isp_tsp_instruction_word = isp_tsp_instruction_word::depth_compare_mode::greater_or_equal
| isp_tsp_instruction_word::culling_mode::no_culling; | culling_mode;
const uint32_t tsp_instruction_word = tsp_instruction_word::texture_shading_instruction::decal const uint32_t tsp_instruction_word = tsp_instruction_word::texture_shading_instruction::decal
| tsp_instruction_word::src_alpha_instr::one | tsp_instruction_word::src_alpha_instr::one
@ -97,6 +98,28 @@ static inline void quad_type_2(ta_parameter_writer& writer,
base_intensity); base_intensity);
} }
static inline void tri_type_2(ta_parameter_writer& writer,
const vec3& ap,
const vec3& bp,
const vec3& cp,
float base_intensity)
{
writer.append<ta_vertex_parameter::polygon_type_2>() =
ta_vertex_parameter::polygon_type_2(polygon_vertex_parameter_control_word(false),
ap.x, ap.y, ap.z,
base_intensity);
writer.append<ta_vertex_parameter::polygon_type_2>() =
ta_vertex_parameter::polygon_type_2(polygon_vertex_parameter_control_word(false),
bp.x, bp.y, bp.z,
base_intensity);
writer.append<ta_vertex_parameter::polygon_type_2>() =
ta_vertex_parameter::polygon_type_2(polygon_vertex_parameter_control_word(true),
cp.x, cp.y, cp.z,
base_intensity);
}
static inline void quad_type_3(ta_parameter_writer& writer, static inline void quad_type_3(ta_parameter_writer& writer,
const vec3& ap, const vec2& at, const vec3& ap, const vec2& at,
const vec3& bp, const vec2& bt, const vec3& bp, const vec2& bt,