X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=rigidbody.h;h=b1dc834a76ab710d046ec99f729de9da1703d955;hb=edd08a99f72cced7626654b04591d5a54a8b540b;hp=60fc88ce59a18275f59c6b36aba67ed88f9fbb89;hpb=610907ae753bdda202236d52a6fcf77d14d63193;p=carveJwlIkooP6JGAAIwe30JlM.git diff --git a/rigidbody.h b/rigidbody.h index 60fc88c..b1dc834 100644 --- a/rigidbody.h +++ b/rigidbody.h @@ -44,25 +44,10 @@ VG_STATIC float VG_STATIC void rb_register_cvar(void) { - vg_var_push( (struct vg_var){ - .name = "k_limit_bias", .data = &k_limit_bias, - .data_type = k_var_dtype_f32, .opt_f32 = {.clamp = 0}, .persistent = 1 - }); - - vg_var_push( (struct vg_var){ - .name = "k_joint_bias", .data = &k_joint_bias, - .data_type = k_var_dtype_f32, .opt_f32 = {.clamp = 0}, .persistent = 1 - }); - - vg_var_push( (struct vg_var){ - .name = "k_joint_correction", .data = &k_joint_correction, - .data_type = k_var_dtype_f32, .opt_f32 = {.clamp = 0}, .persistent = 1 - }); - - vg_var_push( (struct vg_var){ - .name = "k_joint_impulse", .data = &k_joint_impulse, - .data_type = k_var_dtype_f32, .opt_f32 = {.clamp = 0}, .persistent = 1 - }); + VG_VAR_F32( k_limit_bias, flags=VG_VAR_CHEAT ); + VG_VAR_F32( k_joint_bias, flags=VG_VAR_CHEAT ); + VG_VAR_F32( k_joint_correction, flags=VG_VAR_CHEAT ); + VG_VAR_F32( k_joint_impulse, flags=VG_VAR_CHEAT ); } /* @@ -78,23 +63,19 @@ typedef struct rb_sphere rb_sphere; typedef struct rb_capsule rb_capsule; typedef struct rb_scene rb_scene; -struct rb_sphere -{ +struct rb_sphere{ float radius; }; -struct rb_capsule -{ +struct rb_capsule{ float height, radius; }; -struct rb_scene -{ +struct rb_scene{ bh_tree *bh_scene; }; -struct rigidbody -{ +struct rigidbody{ v3f co, v, w; v4f q; @@ -108,8 +89,7 @@ struct rigidbody }; /* simple objects */ -struct rb_object -{ +struct rb_object{ rigidbody rb; enum rb_shape{ k_rb_shape_box = 0, @@ -119,8 +99,7 @@ struct rb_object } type; - union - { + union{ struct rb_sphere sphere; struct rb_capsule capsule; struct rb_scene scene; @@ -128,8 +107,7 @@ struct rb_object inf; }; -VG_STATIC struct contact -{ +VG_STATIC struct contact{ rigidbody *rba, *rbb; v3f co, n; v3f t[2]; @@ -146,14 +124,12 @@ VG_STATIC int rb_contact_count = 0; typedef struct rb_constr_pos rb_constr_pos; typedef struct rb_constr_swingtwist rb_constr_swingtwist; -struct rb_constr_pos -{ +struct rb_constr_pos{ rigidbody *rba, *rbb; v3f lca, lcb; }; -struct rb_constr_swingtwist -{ +struct rb_constr_swingtwist{ rigidbody *rba, *rbb; v4f conevx, conevy; /* relative to rba */ @@ -167,8 +143,7 @@ struct rb_constr_swingtwist float tangent_mass, axis_mass; }; -struct rb_constr_spring -{ +struct rb_constr_spring{ int nothing; }; @@ -187,14 +162,12 @@ VG_STATIC float sphere_volume( float radius ) VG_STATIC void rb_tangent_basis( v3f n, v3f tx, v3f ty ) { /* Compute tangent basis (box2d) */ - if( fabsf( n[0] ) >= 0.57735027f ) - { + if( fabsf( n[0] ) >= 0.57735027f ){ tx[0] = n[1]; tx[1] = -n[0]; tx[2] = 0.0f; } - else - { + else{ tx[0] = 0.0f; tx[1] = n[2]; tx[2] = -n[1]; @@ -215,13 +188,11 @@ VG_STATIC void rb_debug_contact( rb_ct *ct ) v3f p1; v3_muladds( ct->co, ct->n, 0.05f, p1 ); - if( ct->type == k_contact_type_default ) - { + if( ct->type == k_contact_type_default ){ vg_line_pt3( ct->co, 0.0125f, 0xff0000ff ); vg_line( ct->co, p1, 0xffffffff ); } - else if( ct->type == k_contact_type_edge ) - { + else if( ct->type == k_contact_type_edge ){ vg_line_pt3( ct->co, 0.0125f, 0xff00ffc0 ); vg_line( ct->co, p1, 0xffffffff ); } @@ -233,8 +204,7 @@ VG_STATIC void debug_sphere( m4x3f m, float radius, u32 colour ) lx = { 0.0f, radius, 0.0f }, lz = { 0.0f, 0.0f, radius }; - for( int i=0; i<16; i++ ) - { + for( int i=0; i<16; i++ ){ float t = ((float)(i+1) * (1.0f/16.0f)) * VG_PIf * 2.0f, s = sinf(t), c = cosf(t); @@ -292,8 +262,7 @@ VG_STATIC void debug_capsule( m4x3f m, float radius, float h, u32 colour ) vg_line( a0, a1, colour ); vg_line( b0, b1, colour ); - for( int i=0; i<16; i++ ) - { + for( int i=0; i<16; i++ ){ float t = ((float)(i+1) * (1.0f/16.0f)) * VG_PIf * 2.0f, s1 = sinf(t)*radius, c1 = cosf(t)*radius; @@ -320,15 +289,13 @@ VG_STATIC void debug_capsule( m4x3f m, float radius, float h, u32 colour ) vg_line( a0, a1, colour ); vg_line( b0, b1, colour ); - if( c0 < 0.0f ) - { + if( c0 < 0.0f ){ v3_add( p0, e2, a0 ); v3_add( p0, e3, a1 ); v3_add( p0, e4, b0 ); v3_add( p0, e5, b1 ); } - else - { + else{ v3_add( p1, e2, a0 ); v3_add( p1, e3, a1 ); v3_add( p1, e4, b0 ); @@ -408,8 +375,7 @@ VG_STATIC void rb_update_transform( rigidbody *rb ) */ VG_STATIC void rb_extrapolate( rigidbody *rb, v3f co, v4f q ) { - float substep = vg_clampf( vg.accumulator / k_rb_delta, 0.0f, 1.0f ); - + float substep = vg.time_fixed_extrapolate; v3_muladds( rb->co, rb->v, k_rb_delta*substep, co ); if( v3_length2( rb->w ) > 0.0f ){ @@ -500,7 +466,7 @@ VG_STATIC void rb_iter( rigidbody *rb ) !vg_validf( rb->v[1] ) || !vg_validf( rb->v[2] ) ) { - vg_fatal_exit_loop( "NaN velocity" ); + vg_fatal_error( "NaN velocity" ); } v3f gravity = { 0.0f, -9.8f, 0.0f }; @@ -569,40 +535,35 @@ VG_STATIC int rb_box_triangle_sat( v3f extent, v3f center, v3_sub( tri[i], center, tri[i] ); } + v3f f0,f1,f2,n; + v3_sub( tri[1], tri[0], f0 ); + v3_sub( tri[2], tri[1], f1 ); + v3_sub( tri[0], tri[2], f2 ); + + + v3f axis[9]; + v3_cross( (v3f){1.0f,0.0f,0.0f}, f0, axis[0] ); + v3_cross( (v3f){1.0f,0.0f,0.0f}, f1, axis[1] ); + v3_cross( (v3f){1.0f,0.0f,0.0f}, f2, axis[2] ); + v3_cross( (v3f){0.0f,1.0f,0.0f}, f0, axis[3] ); + v3_cross( (v3f){0.0f,1.0f,0.0f}, f1, axis[4] ); + v3_cross( (v3f){0.0f,1.0f,0.0f}, f2, axis[5] ); + v3_cross( (v3f){0.0f,0.0f,1.0f}, f0, axis[6] ); + v3_cross( (v3f){0.0f,0.0f,1.0f}, f1, axis[7] ); + v3_cross( (v3f){0.0f,0.0f,1.0f}, f2, axis[8] ); + + for( int i=0; i<9; i++ ) + if(!rb_box_triangle_interval( extent, axis[i], tri )) return 0; + /* u0, u1, u2 */ if(!rb_box_triangle_interval( extent, (v3f){1.0f,0.0f,0.0f}, tri )) return 0; if(!rb_box_triangle_interval( extent, (v3f){0.0f,1.0f,0.0f}, tri )) return 0; if(!rb_box_triangle_interval( extent, (v3f){0.0f,0.0f,1.0f}, tri )) return 0; - v3f v0,v1,v2,n, e0,e1,e2; - v3_sub( tri[1], tri[0], v0 ); - v3_sub( tri[2], tri[0], v1 ); - v3_sub( tri[2], tri[1], v2 ); - v3_normalize( v0 ); - v3_normalize( v1 ); - v3_normalize( v2 ); - v3_cross( v0, v1, n ); - v3_cross( v0, n, e0 ); - v3_cross( n, v1, e1 ); - v3_cross( v2, n, e2 ); - /* normal */ + v3_cross( f0, f1, n ); if(!rb_box_triangle_interval( extent, n, tri )) return 0; - v3f axis[9]; - v3_cross( e0, (v3f){1.0f,0.0f,0.0f}, axis[0] ); - v3_cross( e0, (v3f){0.0f,1.0f,0.0f}, axis[1] ); - v3_cross( e0, (v3f){0.0f,0.0f,1.0f}, axis[2] ); - v3_cross( e1, (v3f){1.0f,0.0f,0.0f}, axis[3] ); - v3_cross( e1, (v3f){0.0f,1.0f,0.0f}, axis[4] ); - v3_cross( e1, (v3f){0.0f,0.0f,1.0f}, axis[5] ); - v3_cross( e2, (v3f){1.0f,0.0f,0.0f}, axis[6] ); - v3_cross( e2, (v3f){0.0f,1.0f,0.0f}, axis[7] ); - v3_cross( e2, (v3f){0.0f,0.0f,1.0f}, axis[8] ); - - for( int i=0; i<9; i++ ) - if(!rb_box_triangle_interval( extent, axis[i], tri )) return 0; - return 1; } @@ -1270,11 +1231,7 @@ VG_STATIC int rb_sphere__triangle( m4x3f mtxA, rb_sphere *b, VG_STATIC int rb_sphere__scene( m4x3f mtxA, rb_sphere *b, m4x3f mtxB, rb_scene *s, rb_ct *buf ) { - scene *sc = s->bh_scene->user; - - bh_iter it; - bh_iter_init( 0, &it ); - int idx; + scene_context *sc = s->bh_scene->user; int count = 0; @@ -1282,8 +1239,12 @@ VG_STATIC int rb_sphere__scene( m4x3f mtxA, rb_sphere *b, boxf box; v3_sub( mtxA[3], (v3f){ r,r,r }, box[0] ); v3_add( mtxA[3], (v3f){ r,r,r }, box[1] ); + + bh_iter it; + i32 idx; + bh_iter_init_box( 0, &it, box ); - while( bh_next( s->bh_scene, &it, box, &idx ) ){ + while( bh_next( s->bh_scene, &it, &idx ) ){ u32 *ptri = &sc->arrindices[ idx*3 ]; v3f tri[3]; @@ -1311,7 +1272,8 @@ VG_STATIC int rb_sphere__scene( m4x3f mtxA, rb_sphere *b, VG_STATIC int rb_box__scene( m4x3f mtxA, boxf bbx, m4x3f mtxB, rb_scene *s, rb_ct *buf ) { - scene *sc = s->bh_scene->user; +#if 1 + scene_context *sc = s->bh_scene->user; v3f tri[3]; v3f extent, center; @@ -1332,13 +1294,13 @@ VG_STATIC int rb_box__scene( m4x3f mtxA, boxf bbx, m4x3_invert_affine( mtxA, to_local ); bh_iter it; - bh_iter_init( 0, &it ); + bh_iter_init_box( 0, &it, world_bbx ); int idx; int count = 0; vg_line_boxf( world_bbx, VG__RED ); - while( bh_next( s->bh_scene, &it, world_bbx, &idx ) ){ + while( bh_next( s->bh_scene, &it, &idx ) ){ u32 *ptri = &sc->arrindices[ idx*3 ]; for( int j=0; j<3; j++ ) @@ -1452,6 +1414,50 @@ VG_STATIC int rb_box__scene( m4x3f mtxA, boxf bbx, } } return count; +#else + + scene *sc = s->bh_scene->user; + v3f tri[3]; + + v3f extent, center; + v3_sub( bbx[1], bbx[0], extent ); + v3_muls( extent, 0.5f, extent ); + v3_add( bbx[0], extent, center ); + + float r = v3_length(extent); + boxf world_bbx; + v3_fill( world_bbx[0], -r ); + v3_fill( world_bbx[1], r ); + for( int i=0; i<2; i++ ){ + v3_add( center, world_bbx[i], world_bbx[i] ); + v3_add( mtxA[3], world_bbx[i], world_bbx[i] ); + } + + m4x3f to_local; + m4x3_invert_affine( mtxA, to_local ); + + bh_iter it; + bh_iter_init( 0, &it ); + int idx; + int count = 0; + + vg_line_boxf( world_bbx, VG__RED ); + + while( bh_next( s->bh_scene, &it, world_bbx, &idx ) ){ + u32 *ptri = &sc->arrindices[ idx*3 ]; + + for( int j=0; j<3; j++ ) + v3_copy( sc->arrvertices[ptri[j]].co, tri[j] ); + + vg_line( tri[0],tri[1],VG__BLACK ); + vg_line( tri[1],tri[2],VG__BLACK ); + vg_line( tri[2],tri[0],VG__BLACK ); + + v3f clip[2][8]; + u32 clip_length = 0; + } + +#endif } VG_STATIC int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c, @@ -1511,18 +1517,18 @@ VG_STATIC int rb_capsule__scene( m4x3f mtxA, rb_capsule *c, m4x3f mtxB, rb_scene *s, rb_ct *buf ) { - bh_iter it; - bh_iter_init( 0, &it ); - int idx; int count = 0; boxf bbx; v3_sub( mtxA[3], (v3f){ c->height, c->height, c->height }, bbx[0] ); v3_add( mtxA[3], (v3f){ c->height, c->height, c->height }, bbx[1] ); - scene *sc = s->bh_scene->user; + scene_context *sc = s->bh_scene->user; - while( bh_next( s->bh_scene, &it, bbx, &idx ) ){ + bh_iter it; + bh_iter_init_box( 0, &it, bbx ); + i32 idx; + while( bh_next( s->bh_scene, &it, &idx ) ){ u32 *ptri = &sc->arrindices[ idx*3 ]; v3f tri[3]; @@ -2223,7 +2229,7 @@ VG_STATIC void rb_effect_spring_target_vector( rigidbody *rba, v3f ra, v3f rt, float timestep ) { float d = v3_dot( rt, ra ); - float a = vg_signf( d ) * acosf( vg_clampf( d, -1.0f, 1.0f ) ); + float a = acosf( vg_clampf( d, -1.0f, 1.0f ) ); v3f axis; v3_cross( rt, ra, axis );