From 84a7ae83a25966e0004a1a4b409dbb3d49fae286 Mon Sep 17 00:00:00 2001 From: hgn Date: Wed, 20 Jul 2022 00:00:46 +0100 Subject: [PATCH 1/1] physics revision --- main.c | 98 ++++++++++++----- physics_test.h | 128 ++++++++++++++++++++++ player.h | 9 +- rigidbody.h | 286 ++++++++++++++++++++++++++++++++++++++++++++----- 4 files changed, 463 insertions(+), 58 deletions(-) create mode 100644 physics_test.h diff --git a/main.c b/main.c index 0219a66..8b2f561 100644 --- a/main.c +++ b/main.c @@ -17,6 +17,7 @@ vg_tex2d tex_water = { .path = "textures/water.qoi" }; static int debugview = 0; static int sv_debugcam = 0; static int lightedit = 0; +static int sv_scene = 1; /* Components */ #include "road.h" @@ -37,6 +38,10 @@ static int lightedit = 0; #include "shaders/standard.h" #include "shaders/unlit.h" +#ifndef VG_RELEASE +#include "physics_test.h" +#endif + void vg_register(void) { shader_blit_register(); @@ -72,7 +77,7 @@ vg_tex2d *texture_list[] = int main( int argc, char *argv[] ) { - vg_init( argc, argv, "Voyager Game Engine" ); + vg_init( argc, argv, "CARBE" ); } static int playermodel( int argc, char const *argv[] ) @@ -89,6 +94,14 @@ static int playermodel( int argc, char const *argv[] ) void vg_start(void) { + vg_convar_push( (struct vg_convar){ + .name = "scene", + .data = &sv_scene, + .data_type = k_convar_dtype_i32, + .opt_i32 = { .min=0, .max=1, .clamp=1 }, + .persistent = 1 + }); + vg_convar_push( (struct vg_convar){ .name = "fc", .data = &freecam, @@ -154,13 +167,26 @@ void vg_start(void) init_other(); - character_load( &player.mdl, "ch_default" ); - character_init_ragdoll( &player.mdl ); + /* + * If we're in physics test mode we dont need to load anything else, this + * parameter is dev only. TODO: dev only cvars that don't ship with the game + * when building in release mode. + */ + + if( sv_scene == 0 ) + { + character_load( &player.mdl, "ch_default" ); + character_init_ragdoll( &player.mdl ); - world_load(); + world_load(); - reset_player( 1, (const char *[]){ "start" } ); - rb_init( &player.rb ); + reset_player( 1, (const char *[]){ "start" } ); + rb_init( &player.rb ); + } + else + { + physics_test_start(); + } } void vg_free(void) @@ -170,14 +196,21 @@ void vg_free(void) void vg_update(void) { - player_update(); - world_update(); - //traffic_visualize( world.traffic, world.traffic_count ); - // - /* TEMP */ - if( glfwGetKey( vg_window, GLFW_KEY_J )) + if( sv_scene == 0 ) + { + player_update(); + world_update(); + //traffic_visualize( world.traffic, world.traffic_count ); + // + /* TEMP */ + if( glfwGetKey( vg_window, GLFW_KEY_J )) + { + v3_copy( player.camera_pos, world.mr_ball.co ); + } + } + else if( sv_scene == 1 ) { - v3_copy( player.camera_pos, world.mr_ball.co ); + physics_test_update(); } } @@ -188,15 +221,15 @@ static void vg_framebuffer_resize( int w, int h ) water_fb_resize(); } -void vg_render(void) +static void draw_origin_axis(void) { - glBindFramebuffer( GL_FRAMEBUFFER, 0 ); - glViewport( 0,0, vg_window_x, vg_window_y ); - - glDisable( GL_DEPTH_TEST ); - glClearColor( 0.11f, 0.35f, 0.37f, 1.0f ); - glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT ); + vg_line( (v3f){ 0.0f, 0.0f, 0.0f }, (v3f){ 1.0f, 0.0f, 0.0f }, 0xffff0000 ); + vg_line( (v3f){ 0.0f, 0.0f, 0.0f }, (v3f){ 0.0f, 1.0f, 0.0f }, 0xff00ff00 ); + vg_line( (v3f){ 0.0f, 0.0f, 0.0f }, (v3f){ 0.0f, 0.0f, 1.0f }, 0xff0000ff ); +} +static void render_main_game(void) +{ float speed = freecam? 0.0f: v3_length( player.rb.v ); v3f shake = { vg_randf()-0.5f, vg_randf()-0.5f, vg_randf()-0.5f }; v3_muls( shake, speed*0.01f, shake ); @@ -211,10 +244,6 @@ void vg_render(void) m4x4_mul( vg_pv, world_4x4, vg_pv ); - vg_line( (v3f){ 0.0f, 0.0f, 0.0f }, (v3f){ 1.0f, 0.0f, 0.0f }, 0xffff0000 ); - vg_line( (v3f){ 0.0f, 0.0f, 0.0f }, (v3f){ 0.0f, 1.0f, 0.0f }, 0xff00ff00 ); - vg_line( (v3f){ 0.0f, 0.0f, 0.0f }, (v3f){ 0.0f, 0.0f, 1.0f }, 0xff0000ff ); - glEnable( GL_DEPTH_TEST ); /* @@ -285,6 +314,27 @@ void vg_render(void) glViewport( 0,0, vg_window_x, vg_window_y ); } +void vg_render(void) +{ + glBindFramebuffer( GL_FRAMEBUFFER, 0 ); + glViewport( 0,0, vg_window_x, vg_window_y ); + + glDisable( GL_DEPTH_TEST ); + glClearColor( 0.11f, 0.35f, 0.37f, 1.0f ); + glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT ); + + draw_origin_axis(); + + if( sv_scene == 0 ) + { + render_main_game(); + } + else if( sv_scene == 1 ) + { + physics_test_render(); + } +} + static void run_light_widget( struct light_widget *lw ) { struct ui_checkbox c1 = { .data=&lw->enabled }; diff --git a/physics_test.h b/physics_test.h new file mode 100644 index 0000000..8187b91 --- /dev/null +++ b/physics_test.h @@ -0,0 +1,128 @@ +#ifndef PHYSICS_TEST_H +#define PHYSICS_TEST_H + +#include "rigidbody.h" +#include "player.h" + +rigidbody ground = { .type = k_rb_shape_box, + .bbx = {{-100.0f,-1.0f,-100.0f},{100.0f,0.0f,100.0f}}, + .co = {0.0f, 0.0f, 0.0f}, + .q = {0.0f,0.0f,0.0f,1.0f}, + .is_world = 1 }; +rigidbody funnel[4] = { + { + .type = k_rb_shape_box, + .bbx = {{-20.0f,-1.0f,-20.0f},{20.0f,1.0f,20.0f}}, + .co = {-10.0f,5.0f,0.0f}, + .is_world = 1 + }, + { + .type = k_rb_shape_box, + .bbx = {{-20.0f,-1.0f,-20.0f},{20.0f,1.0f,20.0f}}, + .co = { 10.0f,5.0f,0.0f}, + .is_world = 1 + }, + { + .type = k_rb_shape_box, + .bbx = {{-20.0f,-1.0f,-20.0f},{20.0f,1.0f,20.0f}}, + .co = { 0.0f,5.0f,10.0f}, + .is_world = 1 + }, + { + .type = k_rb_shape_box, + .bbx = {{-20.0f,-1.0f,-20.0f},{20.0f,1.0f,20.0f}}, + .co = {0.0f,5.0f,-10.0f}, + .is_world = 1 + } +}; + + +rigidbody ball = { .type = k_rb_shape_sphere, + .inf.sphere = { .radius = 2.0f }, + .co = {0.0f,6.0f,0.0f}, + .q = {0.0f,0.0f,0.0f,1.0f}}, + + ball1= { .type = k_rb_shape_sphere, + .inf.sphere = { .radius = 1.0f }, + .co = {0.1f,9.0f,0.2f}, + .q = {0.0f,0.0f,0.0f,1.0f}}; + +static void physics_test_start(void) +{ + q_axis_angle( funnel[0].q, (v3f){1.0f,0.0f,0.0f}, 0.3f ); + q_axis_angle( funnel[1].q, (v3f){1.0f,0.0f,0.0f}, -0.3f ); + q_axis_angle( funnel[2].q, (v3f){0.0f,0.0f,1.0f}, 0.3f ); + q_axis_angle( funnel[3].q, (v3f){0.0f,0.0f,1.0f}, -0.3f ); + + for( int i=0; i<4; i++ ) + rb_init( &funnel[i] ); + + rb_init( &ground ); + rb_init( &ball ); + rb_init( &ball1 ); +} + +static void physics_test_update(void) +{ + player_freecam(); + player_camera_update(); + + { + + rb_iter( &ball ); + rb_iter( &ball1 ); + + rb_solver_reset(); + + for( int i=0; i<4; i++ ) + { + rb_contact_count += rb_sphere_vs_box( &ball, &funnel[i], rb_global_ct()); + rb_contact_count += rb_sphere_vs_box( &ball1, &funnel[i], rb_global_ct()); + } + + rb_contact_count += rb_sphere_vs_box( &ball, &ground, rb_global_ct() ); + rb_contact_count += rb_sphere_vs_box( &ball1, &ground, rb_global_ct() ); + rb_contact_count += rb_sphere_vs_sphere( &ball, &ball1, rb_global_ct() ); + + rb_presolve_contacts(); + + for( int i=0; i<5; i++ ) + rb_solve_contacts(); + + rb_update_transform( &ball ); + rb_update_transform( &ball1 ); + + } + + if(glfwGetKey( vg_window, GLFW_KEY_L )) + { + v3_copy( player.camera_pos, ball.co ); + v3_zero( ball.v ); + v3_zero( ball.w ); + } + + for( int i=0; i<4; i++ ) + rb_debug( &funnel[i], 0xff0060e0 ); + rb_debug( &ground, 0xff00ff00 ); + rb_debug( &ball, 0xffe00040 ); + rb_debug( &ball1, 0xff00e050 ); +} + +static void physics_test_render(void) +{ + m4x4f world_4x4; + m4x3_expand( player.camera_inverse, world_4x4 ); + + gpipeline.fov = freecam? 60.0f: 135.0f; /* 120 */ + m4x4_projection( vg_pv, gpipeline.fov, + (float)vg_window_x / (float)vg_window_y, + 0.1f, 2100.0f ); + + m4x4_mul( vg_pv, world_4x4, vg_pv ); + glEnable( GL_DEPTH_TEST ); + + glDisable( GL_DEPTH_TEST ); + vg_lines_drawall( (float *)vg_pv ); +} + +#endif /* PHYSICS_TEST_H */ diff --git a/player.h b/player.h index 9540d9e..a0ac858 100644 --- a/player.h +++ b/player.h @@ -457,7 +457,6 @@ static void player_physics(void) vg_line( poles[j], p1, 0xffffffff ); struct contact *ct = &manifold[manifold_count ++]; - v3_sub( co, player.rb.co, ct->delta ); v3_copy( co, ct->co ); v3_copy( norm, ct->n ); @@ -491,8 +490,9 @@ static void player_physics(void) { struct contact *ct = &manifold[i]; - v3f dv; - v3_cross( player.rb.w, ct->delta, dv ); + v3f dv, delta; + v3_sub( ct->co, player.rb.co, delta ); + v3_cross( player.rb.w, delta, dv ); v3_add( player.rb.v, dv, dv ); float vn = -v3_dot( dv, ct->n ); @@ -503,7 +503,6 @@ static void player_physics(void) vn = ct->norm_impulse - temp; v3f impulse; - v3_muls( ct->n, vn, impulse ); if( fabsf(v3_dot( impulse, player.rb.forward )) > 10.0f || @@ -515,7 +514,7 @@ static void player_physics(void) } v3_add( impulse, player.rb.v, player.rb.v ); - v3_cross( ct->delta, impulse, impulse ); + v3_cross( delta, impulse, impulse ); /* * W Impulses are limited to the Y and X axises, we don't really want diff --git a/rigidbody.h b/rigidbody.h index d911688..c00b0f0 100644 --- a/rigidbody.h +++ b/rigidbody.h @@ -17,6 +17,8 @@ static bh_system bh_system_rigidbodies; #define k_rb_delta (1.0f/k_rb_rate) typedef struct rigidbody rigidbody; +typedef struct contact rb_ct; + struct rigidbody { v3f co, v, w; @@ -48,6 +50,8 @@ struct rigidbody v3f right, up, forward; + int is_world; + boxf bbx, bbx_world; float inv_mass; @@ -55,14 +59,24 @@ struct rigidbody m4x3f to_world, to_local; }; +static rigidbody rb_static_null = +{ + .co={0.0f,0.0f,0.0f}, + .q={0.0f,0.0f,0.0f,1.0f}, + .v={0.0f,0.0f,0.0f}, + .w={0.0f,0.0f,0.0f}, + .is_world = 1, + .inv_mass = 0.0f +}; + static void rb_debug( rigidbody *rb, u32 colour ); static struct contact { - rigidbody *rba; - v3f co, n, delta; + rigidbody *rba, *rbb; + v3f co, n; v3f t[2]; - float bias, norm_impulse, tangent_impulse[2]; + float mass_total, p, bias, norm_impulse, tangent_impulse[2]; } rb_contact_buffer[256]; static int rb_contact_count = 0; @@ -117,7 +131,14 @@ static void rb_init( rigidbody *rb ) rb->bbx[1][1] = h; } - rb->inv_mass = 1.0f/(8.0f*volume); + if( rb->is_world ) + { + rb->inv_mass = 0.0f; + } + else + { + rb->inv_mass = 1.0f/(8.0f*volume); + } v3_zero( rb->v ); v3_zero( rb->w ); @@ -178,6 +199,10 @@ static void rb_build_manifold_terrain( rigidbody *rb ); static void rb_build_manifold_terrain_sphere( rigidbody *rb ); static void rb_solve_contacts(void); +/* + * These closest point tests were learned from Real-Time Collision Detection by + * Christer Ericson + */ static float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2, float *s, float *t, v3f c1, v3f c2) { @@ -255,6 +280,20 @@ static float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2, return v3_length2( v0 ); } +static void closest_point_aabb( v3f p, boxf box, v3f dest ) +{ + v3_maxv( p, box[0], dest ); + v3_minv( dest, box[1], dest ); +} + +static void closest_point_obb( v3f p, rigidbody *rb, v3f dest ) +{ + v3f local; + m4x3_mulv( rb->to_local, p, local ); + closest_point_aabb( local, rb->bbx, local ); + m4x3_mulv( rb->to_world, local, dest ); +} + static void closest_point_segment( v3f a, v3f b, v3f point, v3f dest ) { v3f v0, v1; @@ -265,7 +304,6 @@ static void closest_point_segment( v3f a, v3f b, v3f point, v3f dest ) v3_muladds( a, v0, vg_clampf(t,0.0f,1.0f), dest ); } -/* Real-Time Collision Detection */ static void closest_on_triangle( v3f p, v3f tri[3], v3f dest ) { v3f ab, ac, ap; @@ -355,6 +393,143 @@ static void closest_on_triangle( v3f p, v3f tri[3], v3f dest ) v3_muladds( dest, ac, w, dest ); } +/* + * Contact generators + * + * These do not automatically allocate contacts, an appropriately sized + * buffer must be supplied. The function returns the size of the manifold + * which was generated. + * + * The values set on the contacts are: n, co, p, rba, rbb + */ + +static void rb_debug_contact( rb_ct *ct ) +{ + v3f p1; + v3_muladds( ct->co, ct->n, 0.2f, p1 ); + vg_line_pt3( ct->co, 0.1f, 0xff0000ff ); + vg_line( ct->co, p1, 0xffffffff ); +} + +static int rb_sphere_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf ) +{ + v3f co, delta; + + closest_point_obb( rba->co, rbb, co ); + v3_sub( rba->co, co, delta ); + + float d2 = v3_length2(delta), + r = rba->inf.sphere.radius; + + if( d2 <= r*r ) + { + float d; + if( d2 <= 0.0001f ) + { + v3_sub( rbb->co, rba->co, delta ); + d2 = v3_length2(delta); + } + + d = sqrtf(d2); + + rb_ct *ct = buf; + v3_muls( delta, 1.0f/d, ct->n ); + v3_copy( co, ct->co ); + ct->p = r-d; + ct->rba = rba; + ct->rbb = rbb; + return 1; + } + + return 0; +} + +static int rb_sphere_vs_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf ) +{ + v3f delta; + v3_sub( rba->co, rbb->co, delta ); + + float d2 = v3_length2(delta), + r = rba->inf.sphere.radius + rbb->inf.sphere.radius; + + if( d2 < r*r ) + { + float d = sqrtf(d2); + + rb_ct *ct = buf; + v3_muls( delta, -1.0f/d, ct->n ); + + v3f p0, p1; + v3_muladds( rba->co, ct->n, rba->inf.sphere.radius, p0 ); + v3_muladds( rbb->co, ct->n,-rbb->inf.sphere.radius, p1 ); + v3_add( p0, p1, ct->co ); + v3_muls( ct->co, 0.5f, ct->co ); + ct->p = r-d; + ct->rba = rba; + ct->rbb = rbb; + return 1; + } + + return 0; +} + +static int rb_box_vs_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf ) +{ + return rb_sphere_vs_box( rbb, rba, buf ); +} + +static int rb_box_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf ) +{ + /* TODO: Generating a stable quad manifold, lots of clipping */ + return 0; +} + +/* + * This function does not accept triangle as a dynamic object, it is assumed + * to always be static. + * + * The triangle is also assumed to be one sided for better detection + */ +static int rb_sphere_vs_triangle( rigidbody *rba, v3f tri[3], rb_ct *buf ) +{ + v3f delta, co; + + closest_on_triangle( rba->co, tri, co ); + v3_sub( rba->co, co, delta ); + + float d2 = v3_length2( delta ), + r = rba->inf.sphere.radius; + + if( d2 < r*r ) + { + v3f ab, ac, tn; + v3_sub( tri[1], tri[0], ab ); + v3_sub( tri[2], tri[0], ac ); + v3_cross( ac, ab, tn ); + + if( v3_dot( delta, tn ) > 0.0f ) + v3_muls( delta, -1.0f, delta ); + + float d = sqrtf(d2); + + rb_ct *ct = buf; + v3_muls( delta, 1.0f/d, ct->n ); + v3_copy( co, ct->co ); + ct->p = r-d; + ct->rba = rba; + ct->rbb = &rb_static_null; + return 1; + } + + return 0; +} + + +/* + * Generic functions + */ + +RB_DEPR static int sphere_vs_triangle( v3f c, float r, v3f tri[3], v3f co, v3f norm, float *p ) { @@ -394,6 +569,11 @@ static void rb_solver_reset(void) rb_contact_count = 0; } +static rb_ct *rb_global_ct(void) +{ + return rb_contact_buffer + rb_contact_count; +} + static struct contact *rb_start_contact(void) { if( rb_contact_count == vg_list_size(rb_contact_buffer) ) @@ -454,8 +634,6 @@ static void rb_build_manifold_terrain_sphere( rigidbody *rb ) ct->rba = rb; v3_copy( co, ct->co ); v3_copy( norm, ct->n ); - - v3_sub( co, rb->co, ct->delta ); rb_commit_contact( ct, p ); } } @@ -520,7 +698,6 @@ static void rb_build_manifold_terrain( rigidbody *rb ) v3_copy( hit.normal, ct->n ); v3_add( point, surface, ct->co ); v3_muls( ct->co, 0.5f, ct->co ); - v3_sub( ct->co, rb->co, ct->delta ); rb_commit_contact( ct, p ); count ++; @@ -530,37 +707,91 @@ static void rb_build_manifold_terrain( rigidbody *rb ) } } +/* + * Initializing things like tangent vectors + */ +static void rb_presolve_contacts(void) +{ + for( int i=0; ibias = -0.2f * k_rb_rate * vg_minf(0.0f,-ct->p+0.04f); + rb_tangent_basis( ct->n, ct->t[0], ct->t[1] ); + + ct->norm_impulse = 0.0f; + ct->tangent_impulse[0] = 0.0f; + ct->tangent_impulse[1] = 0.0f; + ct->mass_total = 1.0f/(ct->rba->inv_mass + ct->rbb->inv_mass); + + rb_debug_contact( ct ); + } +} + +/* + * Creates relative contact velocity vector, and offsets between each body */ +static void rb_rcv( rb_ct *ct, v3f rv, v3f da, v3f db ) +{ + rigidbody *rba = ct->rba, + *rbb = ct->rbb; + + v3_sub( rba->co, ct->co, da ); + v3_sub( rbb->co, ct->co, db ); + + v3f rva, rvb; + v3_cross( rba->w, da, rva ); + v3_add( rba->v, rva, rva ); + + v3_cross( rbb->w, db, rvb ); + v3_add( rbb->v, rvb, rvb ); + v3_add( rva, rvb, rv ); +} + +static void rb_standard_impulse( rb_ct *ct, v3f da, v3f db, v3f impulse ) +{ + rigidbody *rba = ct->rba, + *rbb = ct->rbb; + + /* response */ + v3_muladds( rba->v, impulse, ct->mass_total * rba->inv_mass, rba->v ); + v3_muladds( rbb->v, impulse, ct->mass_total * rbb->inv_mass, rbb->v ); + + /* Angular velocity */ + v3f wa, wb; + v3_cross( da, impulse, wa ); + v3_cross( db, impulse, wb ); + v3_muladds( rba->w, wa, ct->mass_total * rba->inv_mass, rba->w ); + v3_muladds( rbb->w, wb, ct->mass_total * rbb->inv_mass, rbb->w ); +} + static void rb_solve_contacts(void) { float k_friction = 0.1f; + /* TODO: second object + * Static objects route to static element */ + /* Friction Impulse */ for( int i=0; irba; - v3f dv; - v3_cross( rb->w, ct->delta, dv ); - v3_add( rb->v, dv, dv ); + v3f rv, da, db; + rb_rcv( ct, rv, da, db ); for( int j=0; j<2; j++ ) { - float vt = vg_clampf( -v3_dot( dv, ct->t[j] ), - -k_friction, k_friction ); - - vt = -v3_dot( dv, ct->t[j] ); + float f = k_friction * ct->norm_impulse, + vt = -v3_dot( rv, ct->t[j] ); float temp = ct->tangent_impulse[j]; - ct->tangent_impulse[j] = vg_clampf( temp+vt, -k_friction, k_friction ); + ct->tangent_impulse[j] = vg_clampf( temp+vt, -f, f ); vt = ct->tangent_impulse[j] - temp; v3f impulse; - v3_muls( ct->t[j], vt, impulse ); - v3_add( impulse, rb->v, rb->v ); - v3_cross( ct->delta, impulse, impulse ); - v3_add( impulse, rb->w, rb->w ); + rb_standard_impulse( ct, da, db, impulse ); } } @@ -568,13 +799,13 @@ static void rb_solve_contacts(void) for( int i=0; irba; + rigidbody *rba = ct->rba, + *rbb = ct->rbb; - v3f dv; - v3_cross( rb->w, ct->delta, dv ); - v3_add( rb->v, dv, dv ); + v3f rv, da, db; + rb_rcv( ct, rv, da, db ); - float vn = -v3_dot( dv, ct->n ); + float vn = -v3_dot( rv, ct->n ); vn += ct->bias; float temp = ct->norm_impulse; @@ -582,11 +813,8 @@ static void rb_solve_contacts(void) vn = ct->norm_impulse - temp; v3f impulse; - v3_muls( ct->n, vn, impulse ); - v3_add( impulse, rb->v, rb->v ); - v3_cross( ct->delta, impulse, impulse ); - v3_add( impulse, rb->w, rb->w ); + rb_standard_impulse( ct, da, db, impulse ); } } -- 2.25.1