X-Git-Url: https://harrygodden.com/git/?p=vg.git;a=blobdiff_plain;f=labs%2Fphysics.c;fp=labs%2Fphysics.c;h=7373660b9fcac4dc9bba1c116b5e693e03badb28;hp=e741f12568184f72924fdf1fd3db8de60f878233;hb=fce86711735b15bff37de0f70716808410fcf269;hpb=1c305409e8eca9cf8449d681df73208956ce14df

diff --git a/labs/physics.c b/labs/physics.c
index e741f12..7373660 100644
--- a/labs/physics.c
+++ b/labs/physics.c
@@ -10,8 +10,93 @@
 
 #define SDL_MAIN_HANDLED
 
+#define VG_MAX_CONTACTS 2048
+
 #include "vg/vg.h"
 #include "vg/vg_camera.h"
+#include "vg/vg_rigidbody.h"
+#include "vg/vg_rigidbody_collision.h"
+#include "vg/vg_profiler.h"
+#include "vg/vg_bvh.h"
+
+#define SHAPE_MAX 256
+static rigidbody shapes[SHAPE_MAX];
+static rb_capsule shapes_inf[SHAPE_MAX];
+static v4f  shapes_colour[SHAPE_MAX];
+static boxf shapes_bbx[SHAPE_MAX];
+
+static boxf floor_box = {{-6.999,-2.001,-6.999},{6.999,-0.999,6.999}};
+
+static rigidbody racket;
+
+static f32 k_iterations = 8.0f,
+           k_view_x = 0.0f,
+           k_view_y = 0.2f,
+           k_view_z = 10.0f,
+           k_shapes = 32.0f,
+           k_racket_d  = 8.0f;
+
+static rb_capsule racket_ca = { .h = 4.3f, .r = 0.6f }, 
+                  racket_cb = { .h = 5.5f, .r = 0.9f };
+static m4x3f racket_a_mdl, racket_b_mdl;
+static m3x3f racket_I;
+
+static v3f k_racket_init_w;
+static i32 k_demo = 0;
+static i32 k_gyro = 0;
+static i32 k_prof_normalize = 0;
+static i32 k_bbx = 1;
+static i32 k_spacial = 0;
+
+static struct vg_profile prof_refit   = { .name = "Refit" },
+                         prof_broad = { .name = "Broad phase", 
+                                        .mode = k_profile_mode_accum }, 
+                         prof_narrow = { .name = "Narrow phase", 
+                                         .mode = k_profile_mode_accum },
+                         prof_solve   = { .name = "Solver" };
+
+static void shape_bvh_expand_bound( void *user, boxf bound, u32 item_index ){
+   box_concat( bound, shapes_bbx[item_index] );
+}
+
+static f32 shape_bvh_centroid( void *user, u32 item_index, int axis ){
+   f32 x = shapes_bbx[item_index][0][axis] + shapes_bbx[item_index][1][axis];
+   return x*0.5f;
+}
+
+static void shape_bvh_closest( void *user, u32 item_index, 
+                               v3f point, v3f closest ){
+   closest_point_aabb( point, shapes_bbx[item_index], closest );
+}
+
+static void shape_bvh_swap( void *user, u32 ia, u32 ib ){
+   rigidbody temp = shapes[ib];
+   shapes[ib] = shapes[ia];
+   shapes[ia] = temp;
+
+   rb_capsule cb = shapes_inf[ib];
+   shapes_inf[ib] = shapes_inf[ia];
+   shapes_inf[ia] = cb;
+
+   v4f colourb;
+   v4_copy( shapes_colour[ib], colourb );
+   v4_copy( shapes_colour[ia], shapes_colour[ib] );
+   v4_copy( colourb, shapes_colour[ia] );
+
+   boxf boxb;
+   box_copy( shapes_bbx[ib], boxb );
+   box_copy( shapes_bbx[ia], shapes_bbx[ib] );
+   box_copy( boxb, shapes_bbx[ia] );
+}
+
+static bh_system shape_bvh = {
+   .expand_bound = shape_bvh_expand_bound,
+   .item_centroid = shape_bvh_centroid,
+   .item_closest = shape_bvh_closest,
+   .item_swap = shape_bvh_swap
+};
+
+static bh_tree *shape_bvh_tree = NULL;
 
 int main( int argc, char *argv[] ){
    vg_mem.use_libc_malloc = 0;
@@ -28,17 +113,278 @@ static void vg_preload(void){
    vg_audio.dsp_enabled = 0;
 }
 
+static void init_random(void){
+   for( u32 i=0; i<(u32)k_shapes; i ++ ){
+      f32 h  = vg_randf64( &vg.rand ) * 2.0f + 1.3f,
+          r  = vg_randf64( &vg.rand ) * 0.5f + 0.125f,
+          pv = vg_capsule_volume( r, h ),
+          k_density = 8.0f,
+          pm = pv * k_density;
+
+      shapes_inf[i].r = r;
+      shapes_inf[i].h = h;
+
+      m3x3f pI;
+      vg_capsule_inertia( r, h, pm, pI );
+      m3x3_inv( pI, shapes[i].iI );
+      shapes[i].inv_mass = 1.0f / pm;
+
+      v3f dir;
+      vg_rand_dir( &vg.rand, dir );
+      q_axis_angle( shapes[i].q, dir, vg_randf64(&vg.rand)*VG_TAUf );
+      vg_rand_sphere( &vg.rand, shapes[i].co );
+      v3_muladds( (v3f){0,4,0}, shapes[i].co, 4.0f, shapes[i].co );
+      v3_zero( shapes[i].v );
+      v3_zero( shapes[i].w );
+      shapes_colour[i][0] = vg_randf64(&vg.rand);
+      shapes_colour[i][1] = vg_randf64(&vg.rand);
+      shapes_colour[i][2] = vg_randf64(&vg.rand);
+      shapes_colour[i][3] = 1.0f;
+      rb_update_matrices( &shapes[i] );
+   }
+}
+
+static void init_racket(void){
+   f32 ma = vg_capsule_volume( racket_ca.r, racket_ca.h ) * k_racket_d,
+       mb = vg_capsule_volume( racket_cb.r, racket_cb.h ) * k_racket_d,
+       mt = ma+mb;
+   m3x3f aI, bI;
+
+   /* tensor for A */
+   vg_capsule_inertia( racket_ca.r, racket_ca.h, ma, aI );
+   m4x3_identity( racket_a_mdl );
+   racket_a_mdl[3][1] = -racket_ca.h*0.5f*(mb/mt);
+   vg_translate_inertia( aI, ma, racket_a_mdl[3] );
+
+   /* tensor for B */
+   vg_capsule_inertia( racket_cb.r, racket_cb.h, mb, bI );
+
+   v4f q;
+   q_axis_angle( q, (v4f){1,0,0}, VG_TAUf*0.25f );
+
+   m4x3_identity( racket_b_mdl );
+   q_m3x3( q, racket_b_mdl );
+   vg_rotate_inertia( bI, racket_b_mdl );
+   racket_b_mdl[3][1] = racket_ca.h*0.5f*(ma/mt);
+   vg_translate_inertia( bI, mb, racket_b_mdl[3] );
+
+   m3x3_add( aI, bI, racket_I );
+   m3x3_inv( racket_I, racket.iI );
+   racket.inv_mass = 1.0f/(mb+ma);
+}
+
+static void reset_racket(void){
+   q_identity( racket.q );
+   v3_zero( racket.co );
+   v3_copy( k_racket_init_w, racket.w );
+   v3_zero( racket.v );
+   rb_update_matrices( &racket );
+}
+
 static void vg_load(void){
    vg_bake_shaders();
+   init_random();
+   shape_bvh_tree = bh_create( NULL, &shape_bvh, NULL, SHAPE_MAX, 1 );
+   init_racket();
+   reset_racket();
 }
 
 static void vg_pre_update(void){
+   vg_console.cheats =  1;
+   vg_lines.render = 1;
+}
+
+static void demo0_refit(void){
+   if( k_spacial == 0 ) return;
+
+   for( u32 i=0; i<(u32)k_shapes; i ++ ){
+      f32 h = shapes_inf[i].h,
+          r = shapes_inf[i].r;
+
+      rigidbody *rb = &shapes[i];
+
+      v3f p0, p1;
+      v3_muladds( rb->to_world[3], rb->to_world[1], -h*0.5f+r, p0 );
+      v3_muladds( rb->to_world[3], rb->to_world[1],  h*0.5f-r, p1 );
+      
+      v3f *bbx = shapes_bbx[i];
+      v3_minv( p0, p1, bbx[0] );
+      v3_maxv( p0, p1, bbx[1] );
+      v3_muladds( bbx[0], (v3f){-1,-1,-1}, r, bbx[0] );
+      v3_muladds( bbx[1], (v3f){ 1, 1, 1}, r, bbx[1] );
+   }
+
+   if( k_spacial == 1 ) return;
+
+   if( k_spacial == 2 )
+      bh_rebuild( shape_bvh_tree, (u32)k_shapes );
+}
+
+static void demo0(void){
+   vg_profile_begin( &prof_refit );
+   demo0_refit();
+   vg_profile_end( &prof_refit );
+
+   static rigidbody _null,
+                    _mover;
+   rb_solver_reset();
+
+   f32 t = vg.time * 0.1f * VG_TAUf;
+   v3f sphere_pos = { sinf(t)*2.0f, -1, cosf(t)*2.0f };
+   _mover.v[0] = (sinf(t+vg.time_fixed_delta)-sinf(t))*2.0f;
+   _mover.v[2] = (cosf(t+vg.time_fixed_delta)-cosf(t))*2.0f;
+
+   for( u32 i=0; i<(u32)k_shapes; i ++ ){
+      rigidbody *rbi = &shapes[i];
+      rb_capsule *infi = &shapes_inf[i];
+      v3f *bbxi = shapes_bbx[i];
+
+      if( rb_global_has_space() ){
+         rb_ct *buf = rb_global_buffer();
+         m4x3f mtx;
+         m4x3_identity( mtx );
+         u32 l = rb_capsule__box( rbi->to_world, infi,
+                                  mtx, mtx, floor_box, buf );
+
+         for( u32 k=0; k<l; k ++ ){
+            buf[k].rba = rbi;
+            buf[k].rbb = &_null;
+         }
+
+         rb_contact_count += l;
+      }
+      else break;
+
+      if( rb_global_has_space() ){
+         rb_ct *buf = rb_global_buffer();
+         u32 l = rb_capsule__sphere( rbi->to_world, infi, sphere_pos, 1, buf );
+
+         for( u32 k=0; k<l; k ++ ){
+            buf[k].rba = rbi;
+            buf[k].rbb = &_mover;
+         }
+
+         rb_contact_count += l;
+      }
+      
+      if( k_spacial == 2 ){
+         bh_iter it;
+         bh_iter_init_box( 0, &it, bbxi );
+         i32 idx;
+
+         while(1){
+            vg_profile_begin( &prof_broad );
+            if( !bh_next( shape_bvh_tree, &it, &idx ) ){
+               vg_profile_end( &prof_broad );
+               break;
+            }
+            vg_profile_end( &prof_broad );
+
+            if( idx <= i ) continue;
+
+            vg_profile_begin( &prof_narrow );
+
+            rigidbody *rbj = &shapes[idx];
+            v3f *bbxj = shapes_bbx[idx];
+            rb_capsule *infj = &shapes_inf[idx];
+
+            if( rb_global_has_space() ){
+               rb_ct *buf = rb_global_buffer();
+               u32 l = rb_capsule__capsule( rbi->to_world, infi,
+                                            rbj->to_world, infj, buf );
+
+               for( u32 k=0; k<l; k ++ ){
+                  buf[k].rba = rbi;
+                  buf[k].rbb = rbj;
+               }
+
+               rb_contact_count += l;
+            }
+
+            vg_profile_end( &prof_narrow );
+         }
+      }
+      else {
+         if( i == ((u32)k_shapes)-1 ){
+            break;
+         }
+         for( u32 j=i+1; j<(u32)k_shapes; j ++ ){
+            rigidbody *rbj = &shapes[j];
+            v3f *bbxj = shapes_bbx[j];
+            rb_capsule *infj = &shapes_inf[j];
+
+            if( k_spacial == 1 ){
+               vg_profile_begin( &prof_broad );
+               if( !box_overlap( bbxi, bbxj ) ){
+                  vg_profile_end( &prof_broad );
+                  continue;
+               }
+            }
+
+            vg_profile_begin( &prof_narrow );
+            if( rb_global_has_space() ){
+               rb_ct *buf = rb_global_buffer();
+               u32 l = rb_capsule__capsule( rbi->to_world, infi,
+                                            rbj->to_world, infj, buf );
+
+               for( u32 k=0; k<l; k ++ ){
+                  buf[k].rba = rbi;
+                  buf[k].rbb = rbj;
+               }
+
+               rb_contact_count += l;
+            }
+            vg_profile_end( &prof_narrow );
+         }
+      }
+   }
+
+   vg_profile_increment( &prof_broad );
+   vg_profile_increment( &prof_narrow );
+
+   vg_profile_begin( &prof_solve );
+   rb_presolve_contacts( rb_contact_buffer, rb_contact_count );
+   for( u32 i=0; i<(u32)k_iterations; i ++ )
+      rb_solve_contacts( rb_contact_buffer, rb_contact_count );
+
+   for( u32 i=0; i<(u32)k_shapes; i ++ ){
+      rigidbody *rbi = &shapes[i];
+      if( k_gyro ){
+         m3x3f I;
+         m3x3_inv( rbi->iI, I );
+         rb_solve_gyroscopic( rbi, I, vg.time_fixed_delta );
+      }
+      rb_iter( rbi );
+      rb_update_matrices( rbi );
+   }
+   vg_profile_end( &prof_solve );
+}
+
+static void demo1(void){
+   vg_profile_begin( &prof_refit );
+   vg_profile_end( &prof_refit );
+
+   vg_profile_increment( &prof_broad );
+   vg_profile_increment( &prof_narrow );
+
+   vg_profile_begin( &prof_solve );
+
+   v3_muladds( racket.v, (v3f){0,9.8f,0}, vg.time_fixed_delta, racket.v );
+   if( k_gyro ) rb_solve_gyroscopic( &racket, racket_I, vg.time_fixed_delta );
+   rb_iter( &racket );
+   rb_update_matrices( &racket );
+
+   vg_profile_end( &prof_solve );
 }
 
 static void vg_fixed_update(void){
+   if( k_demo == 0 ) demo0();
+   else if( k_demo == 1 ) demo1();
 }
 
 static void vg_post_update(void){
+   if( vg_getkey( SDLK_8 ) )
+      init_random();
 }
 
 static void vg_framebuffer_resize( int w, int h ){
@@ -50,6 +396,54 @@ static void draw_origin_axis(void){
    vg_line( (v3f){ 0.0f, 0.0f, 0.0f }, (v3f){ 0.0f, 0.0f, 1.0f }, 0xff0000ff );
 }
 
+static void render0(void){
+   f32 t = vg.time * 0.1f * VG_TAUf;
+   m4x3f mdl;
+   m4x3_identity( mdl );
+   vg_rb_view_box( mdl, floor_box, (v4f){0.8f,0.8f,0.8f,1} );
+
+   mdl[3][0] = sinf(t)*2.0f;
+   mdl[3][1] = -1;
+   mdl[3][2] = cosf(t)*2.0f;
+   vg_rb_view_sphere( mdl, 1, (v4f){0,1,0,1} );
+
+   for( u32 i=0; i<(u32)k_shapes; i ++ ){
+      rigidbody *rbi = &shapes[i];
+      rb_capsule *infi = &shapes_inf[i];
+      f32 *coli = shapes_colour[i];
+
+      v4f q;
+      v3f co;
+      rb_extrapolate( rbi, co, q );
+      q_m3x3( q, mdl );
+      v3_copy( co, mdl[3] );
+      vg_rb_view_capsule( mdl, infi->r, infi->h, coli );
+   }
+
+   if( k_spacial && k_bbx ){
+      for( u32 i=0; i<(u32)k_shapes; i ++ ){
+         vg_line_boxf( shapes_bbx[i], VG__RED );
+      }
+
+      if( k_spacial == 2 ){
+         bh_debug_trace( shape_bvh_tree, 0, (v3f){0,0,0}, VG__GREEN );
+      }
+   }
+}
+
+static void render1(void){
+   m4x3f mdl, mmdl;
+   v4f q;
+   rb_extrapolate( &racket, mdl[3], q );
+   q_m3x3( q, mdl );
+
+   m4x3_mul( mdl, racket_a_mdl, mmdl );
+   vg_rb_view_capsule( mmdl, racket_ca.r, racket_ca.h, (v4f){1,0,0,1} );
+
+   m4x3_mul( mdl, racket_b_mdl, mmdl );
+   vg_rb_view_capsule( mmdl, racket_cb.r, racket_cb.h, (v4f){0,1,0,1} );
+}
+
 static void vg_render(void){
    glBindFramebuffer( GL_FRAMEBUFFER, 0 );
    glViewport( 0,0, vg.window_x, vg.window_y );
@@ -59,19 +453,14 @@ static void vg_render(void){
    glClearColor( 0.05f, 0.05f, 0.05f, 1.0f );
    glClear( GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT );
 
-   f32 x = -((f32)vg.mouse_pos[0] / (f32)vg.window_x) * VG_TAUf,
-       y = (((f32)vg.mouse_pos[1] / (f32)vg.window_y) - 0.5f)*VG_PIf;
-
-   f32 t = vg.time * 0.1f * VG_TAUf;
-
    vg_camera cam = {
-      .angles = { -x, y, 0 },
+      .angles = { -k_view_x, k_view_y, 0 },
       .nearz = 0.01f,
       .farz = 500.0f,
       .fov = 90.0f,
-      .pos = { sinf(x)*10.0f*cosf(y), 
-               sinf(y)*10.0f, 
-               cosf(x)*10.0f*cosf(y) },
+      .pos = { sinf(k_view_x)*k_view_z*cosf(k_view_y), 
+               sinf(k_view_y)*k_view_z, 
+               cosf(k_view_x)*k_view_z*cosf(k_view_y) },
    };
 
    vg_camera_update_transform( &cam );
@@ -80,19 +469,9 @@ static void vg_render(void){
    vg_camera_finalize( &cam );
    m4x4_copy( cam.mtx.pv, vg.pv );
 
-   m4x3f mdl;
-   m4x3_identity( mdl );
-
    vg_rb_view_bind();
-   vg_rb_view_capsule( mdl, (sinf(t*0.2f)*0.5f+0.55f)*2.0f, 
-                            (sinf(t*0.33f)*0.5f+0.55f)*1.0f,
-                            (v4f){1,0,0,1} );
-   vg_rb_view_box( mdl, (boxf){{-4.999,-2.001,-4.999},{4.999,-0.999,4.999}}, 
-                  (v4f){0.8f,0.8f,0.8f,1} );
-
-   mdl[3][0] = sinf(t)*2.0f;
-   mdl[3][2] = cosf(t)*2.0f;
-   vg_rb_view_sphere( mdl, 1, (v4f){0,1,0,1} );
+   if( k_demo == 0 ) render0();
+   else if( k_demo == 1 ) render1();
 
    draw_origin_axis();
    vg_lines_drawall();
@@ -100,6 +479,101 @@ static void vg_render(void){
    glDisable(GL_DEPTH_TEST);
 }
 
+struct ui_enum_opt spacial_mode_ui_enum[] = { 
+   { 0, "None" },
+   { 1, "BBX" },
+   { 2, "BVH - Full rebuild" },
+   { 3, "BVH - Temporal fitting" }
+};
+
+static void gui0( ui_rect panel ){
+   ui_slider( panel, "Shapes", 2, vg_list_size(shapes), &k_shapes, "%.0f" );
+
+   if( ui_button( panel, "randomize" ) == k_ui_button_click ){
+      init_random();
+   }
+}
+
+static void gui1( ui_rect panel ){
+   ui_rect l, r;
+   ui_standard_widget( panel, l, 1 );
+   ui_split_ratio( l, k_ui_axis_v, 0.5f, 4, l, r );
+   ui_slider( l, "aH", 1.0f, 10.0f, &racket_ca.h, "%.1f" );
+   ui_slider( r, "aR", 0.1f, 10.0f, &racket_ca.r, "%.1f" );
+
+   ui_standard_widget( panel, l, 1 );
+   ui_split_ratio( l, k_ui_axis_v, 0.5f, 4, l, r );
+   ui_slider( l, "bH", 1.0f, 10.0f, &racket_cb.h, "%.1f" );
+   ui_slider( r, "bR", 0.1f, 10.0f, &racket_cb.r, "%.1f" );
+
+   for( u32 i=0; i<3; i ++ ){
+      ui_rect v0,v1,v2;
+      ui_standard_widget( panel, v0, 1 );
+      ui_split_ratio( v0, k_ui_axis_v, 2.0f/3.0f, 4, v0, v2 );
+      ui_split_ratio( v0, k_ui_axis_v, 1.0f/2.0f, 4, v0, v1 );
+
+      char buf[16];
+      snprintf( buf, sizeof(buf), "%.3f", racket_I[i][0] );
+      ui_text( v0, buf, 1, k_ui_align_middle_center, 0 );
+      snprintf( buf, sizeof(buf), "%.3f", racket_I[i][1] );
+      ui_text( v1, buf, 1, k_ui_align_middle_center, 0 );
+      snprintf( buf, sizeof(buf), "%.3f", racket_I[i][2] );
+      ui_text( v2, buf, 1, k_ui_align_middle_center, 0 );
+   }
+
+   init_racket();
+
+   ui_info( panel, "init conditions" );
+   ui_rect v0,v1,v2;
+   ui_standard_widget( panel, v0, 1 );
+   ui_split_ratio( v0, k_ui_axis_v, 2.0f/3.0f, 4, v0, v2 );
+   ui_split_ratio( v0, k_ui_axis_v, 1.0f/2.0f, 4, v0, v1 );
+   ui_slider( v0, "X", 0.01f, 30.0f, k_racket_init_w+0, "%.1f" );
+   ui_slider( v1, "Y", 0.01f, 30.0f, k_racket_init_w+1, "%.1f" );
+   ui_slider( v2, "Z", 0.01f, 30.0f, k_racket_init_w+2, "%.1f" );
+
+   if( ui_button( panel, "init" ) == k_ui_button_click ){
+      reset_racket();
+   }
+}
+
 static void vg_gui(void){
    vg_ui.wants_mouse = 1;
+   ui_rect panel = { vg.window_x-300, 0, 300, vg.window_y };
+   ui_rect_pad( panel, (ui_px[2]){ 8, 8 } );
+
+   ui_rect box;
+   ui_split( panel, k_ui_axis_h, VG_PROFILE_SAMPLE_COUNT*2 +4, 8, box, panel );
+   vg_profile_drawn( (struct vg_profile *[]){ &prof_refit, 
+                                              &prof_broad,
+                                              &prof_narrow,
+                                              &prof_solve }, 4,
+                     vg.time_fixed_delta*1000.0, box, 0, k_prof_normalize );
+
+   ui_split( panel, k_ui_axis_h, 14*2+8, 4, box, panel );
+   ui_checkbox( panel, "Normalize", &k_prof_normalize );
+
+   ui_slider( panel, "Iterations", 1.0f, 20.0f, &k_iterations, "%.0f" );
+   ui_enum( panel, "Spacial Type", spacial_mode_ui_enum,
+            vg_list_size(spacial_mode_ui_enum), &k_spacial );
+
+   static f32 rate = 60.0f;
+   ui_slider( panel, "Fixed timestep", 10, 200, &rate, "%.1f" );
+   vg.time_fixed_delta = 1.0f/rate;
+
+   ui_checkbox( panel, "Show BBX", &k_bbx );
+   ui_checkbox( panel, "Gyroscopic Term", &k_gyro );
+
+   ui_tabs( panel, panel, 
+            (const char *[]){ "collision", "racket" }, 2, &k_demo );
+
+   if( k_demo == 0 ) gui0( panel );
+   else if( k_demo == 1 ) gui1( panel );
+
+   ui_rect viewport = { 0,0, vg.window_x-300, vg.window_y };
+
+   if( ui_inside_rect( viewport, vg_ui.mouse ) && ui_clicking(UI_MOUSE_LEFT) ){
+      k_view_x += -((f32)vg.mouse_delta[0] / (f32)vg.window_x) * VG_TAUf,
+      k_view_y +=  ((f32)vg.mouse_delta[1] / (f32)vg.window_y) * VG_PIf;
+   }
 }