X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=rigidbody.h;h=7d878e9a4976146a08cf09cb9cc2a5519e978408;hb=137d40d96fe923600d8378b8e138e3c276f27ff4;hp=8489dbccf830ed1a527d1b17906e27bbe5a63f6d;hpb=73adac381b2c72f08293416a960942dc40db3c7f;p=carveJwlIkooP6JGAAIwe30JlM.git diff --git a/rigidbody.h b/rigidbody.h index 8489dbc..7d878e9 100644 --- a/rigidbody.h +++ b/rigidbody.h @@ -105,7 +105,7 @@ struct rb_object{ inf; }; -VG_STATIC struct contact{ +static struct contact{ rigidbody *rba, *rbb; v3f co, n; v3f t[2]; @@ -117,7 +117,7 @@ VG_STATIC struct contact{ enum contact_type type; } rb_contact_buffer[256]; -VG_STATIC int rb_contact_count = 0; +static int rb_contact_count = 0; typedef struct rb_constr_pos rb_constr_pos; typedef struct rb_constr_swingtwist rb_constr_swingtwist; @@ -147,7 +147,7 @@ struct rb_constr_swingtwist{ * ----------------------------------------------------------------------------- */ -VG_STATIC void rb_debug_contact( rb_ct *ct ){ +static void rb_debug_contact( rb_ct *ct ){ v3f p1; v3_muladds( ct->co, ct->n, 0.05f, p1 ); @@ -162,7 +162,7 @@ VG_STATIC void rb_debug_contact( rb_ct *ct ){ } -VG_STATIC void rb_object_debug( rb_object *obj, u32 colour ){ +static void rb_object_debug( rb_object *obj, u32 colour ){ if( obj->type == k_rb_shape_box ){ v3f *box = obj->rb.bbx; vg_line_boxf_transformed( obj->rb.to_world, obj->rb.bbx, colour ); @@ -191,16 +191,15 @@ VG_STATIC void rb_object_debug( rb_object *obj, u32 colour ){ /* * Update world space bounding box based on local one */ -VG_STATIC void rb_update_bounds( rigidbody *rb ) -{ - box_copy( rb->bbx, rb->bbx_world ); - m4x3_transform_aabb( rb->to_world, rb->bbx_world ); +static void rb_update_bounds( rigidbody *rb ){ + box_init_inf( rb->bbx_world ); + m4x3_expand_aabb_aabb( rb->to_world, rb->bbx_world, rb->bbx ); } /* * Commit transform to rigidbody. Updates matrices */ -VG_STATIC void rb_update_transform( rigidbody *rb ) +static void rb_update_transform( rigidbody *rb ) { q_normalize( rb->q ); q_m3x3( rb->q, rb->to_world ); @@ -217,7 +216,7 @@ VG_STATIC void rb_update_transform( rigidbody *rb ) * Extrapolate rigidbody into a transform based on vg accumulator. * Useful for rendering */ -VG_STATIC void rb_extrapolate( rigidbody *rb, v3f co, v4f q ) +static void rb_extrapolate( rigidbody *rb, v3f co, v4f q ) { float substep = vg.time_fixed_extrapolate; v3_muladds( rb->co, rb->v, k_rb_delta*substep, co ); @@ -241,7 +240,7 @@ VG_STATIC void rb_extrapolate( rigidbody *rb, v3f co, v4f q ) /* * Initialize rigidbody and calculate masses, inertia */ -VG_STATIC void rb_init_object( rb_object *obj ){ +static void rb_init_object( rb_object *obj ){ float volume = 1.0f; int inert = 0; @@ -303,7 +302,7 @@ VG_STATIC void rb_init_object( rb_object *obj ){ rb_update_transform( &obj->rb ); } -VG_STATIC void rb_iter( rigidbody *rb ){ +static void rb_iter( rigidbody *rb ){ if( !vg_validf( rb->v[0] ) || !vg_validf( rb->v[1] ) || !vg_validf( rb->v[2] ) ) @@ -346,7 +345,7 @@ VG_STATIC void rb_iter( rigidbody *rb ){ /* * Project AABB, and triangle interval onto axis to check if they overlap */ -VG_STATIC int rb_box_triangle_interval( v3f extent, v3f axis, v3f tri[3] ){ +static int rb_box_triangle_interval( v3f extent, v3f axis, v3f tri[3] ){ float r = extent[0] * fabsf(axis[0]) + @@ -366,7 +365,7 @@ VG_STATIC int rb_box_triangle_interval( v3f extent, v3f axis, v3f tri[3] ){ /* * Seperating axis test box vs triangle */ -VG_STATIC int rb_box_triangle_sat( v3f extent, v3f center, +static int rb_box_triangle_sat( v3f extent, v3f center, m4x3f to_local, v3f tri_src[3] ){ v3f tri[3]; @@ -413,7 +412,7 @@ VG_STATIC int rb_box_triangle_sat( v3f extent, v3f center, * ----------------------------------------------------------------------------- */ -VG_STATIC int rb_manifold_apply_filtered( rb_ct *man, int len ){ +static int rb_manifold_apply_filtered( rb_ct *man, int len ){ int k = 0; for( int i=0; ico, cj->co ) < r*r ){ cj->type = k_contact_type_disabled; ci->p = (ci->p + cj->p) * 0.5f; @@ -462,7 +461,7 @@ VG_STATIC void rb_manifold_contact_weld( rb_ct *ci, rb_ct *cj, float r ){ /* * */ -VG_STATIC void rb_manifold_filter_joint_edges( rb_ct *man, int len, float r ){ +static void rb_manifold_filter_joint_edges( rb_ct *man, int len, float r ){ for( int i=0; itype != k_contact_type_edge ) @@ -481,7 +480,7 @@ VG_STATIC void rb_manifold_filter_joint_edges( rb_ct *man, int len, float r ){ /* * Resolve overlapping pairs */ -VG_STATIC void rb_manifold_filter_pairs( rb_ct *man, int len, float r ){ +static void rb_manifold_filter_pairs( rb_ct *man, int len, float r ){ for( int i=0; itype == k_contact_type_disabled ) @@ -534,7 +533,7 @@ VG_STATIC void rb_manifold_filter_backface( rb_ct *man, int len ){ /* * Filter out duplicate coplanar results. Good for spheres. */ -VG_STATIC void rb_manifold_filter_coplanar( rb_ct *man, int len, float w ){ +static void rb_manifold_filter_coplanar( rb_ct *man, int len, float w ){ for( int i=0; itype == k_contact_type_disabled || @@ -598,7 +597,7 @@ struct capsule_manifold{ * Expand a line manifold with a new pair. t value is the time along segment * on the oriented object which created this pair. */ -VG_STATIC void rb_capsule_manifold( v3f pa, v3f pb, float t, float r, +static void rb_capsule_manifold( v3f pa, v3f pb, float t, float r, capsule_manifold *manifold ){ v3f delta; v3_sub( pa, pb, delta ); @@ -618,12 +617,12 @@ VG_STATIC void rb_capsule_manifold( v3f pa, v3f pb, float t, float r, } } -VG_STATIC void rb_capsule_manifold_init( capsule_manifold *manifold ){ +static void rb_capsule_manifold_init( capsule_manifold *manifold ){ manifold->t0 = INFINITY; manifold->t1 = -INFINITY; } -VG_STATIC int rb_capsule__manifold_done( m4x3f mtx, rb_capsule *c, +static int rb_capsule__manifold_done( m4x3f mtx, rb_capsule *c, capsule_manifold *manifold, rb_ct *buf ){ v3f p0, p1; @@ -674,7 +673,7 @@ VG_STATIC int rb_capsule__manifold_done( m4x3f mtx, rb_capsule *c, return count; } -VG_STATIC int rb_capsule_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){ +static int rb_capsule_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){ rigidbody *rba = &obja->rb, *rbb = &objb->rb; float h = obja->inf.capsule.height, ra = obja->inf.capsule.radius, @@ -714,7 +713,7 @@ VG_STATIC int rb_capsule_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){ return 0; } -VG_STATIC int rb_capsule__capsule( m4x3f mtxA, rb_capsule *ca, +static int rb_capsule__capsule( m4x3f mtxA, rb_capsule *ca, m4x3f mtxB, rb_capsule *cb, rb_ct *buf ){ float ha = ca->height, hb = cb->height, @@ -749,7 +748,7 @@ VG_STATIC int rb_capsule__capsule( m4x3f mtxA, rb_capsule *ca, return rb_capsule__manifold_done( mtxA, ca, &manifold, buf ); } -VG_STATIC int rb_sphere_box( rb_object *obja, rb_object *objb, rb_ct *buf ){ +static int rb_sphere_box( rb_object *obja, rb_object *objb, rb_ct *buf ){ v3f co, delta; rigidbody *rba = &obja->rb, *rbb = &objb->rb; @@ -804,7 +803,7 @@ VG_STATIC int rb_sphere_box( rb_object *obja, rb_object *objb, rb_ct *buf ){ return 0; } -VG_STATIC int rb_sphere_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){ +static int rb_sphere_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){ rigidbody *rba = &obja->rb, *rbb = &objb->rb; v3f delta; v3_sub( rba->co, rbb->co, delta ); @@ -833,7 +832,7 @@ VG_STATIC int rb_sphere_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){ return 0; } -VG_STATIC int rb_sphere__triangle( m4x3f mtxA, rb_sphere *b, +static int rb_sphere__triangle( m4x3f mtxA, rb_sphere *b, v3f tri[3], rb_ct *buf ){ v3f delta, co; enum contact_type type = closest_on_triangle_1( mtxA[3], tri, co ); @@ -853,7 +852,9 @@ VG_STATIC int rb_sphere__triangle( m4x3f mtxA, rb_sphere *b, v3_copy( tn, ct->n ); if( v3_length2( ct->n ) <= 0.00001f ){ +#ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING vg_error( "Zero area triangle!\n" ); +#endif return 0; } @@ -870,8 +871,9 @@ VG_STATIC int rb_sphere__triangle( m4x3f mtxA, rb_sphere *b, return 0; } -VG_STATIC int rb_sphere__scene( m4x3f mtxA, rb_sphere *b, - m4x3f mtxB, rb_scene *s, rb_ct *buf ){ +static int rb_sphere__scene( m4x3f mtxA, rb_sphere *b, + m4x3f mtxB, rb_scene *s, rb_ct *buf, + u16 ignore ){ scene_context *sc = s->bh_scene->user; int count = 0; @@ -889,6 +891,8 @@ VG_STATIC int rb_sphere__scene( m4x3f mtxA, rb_sphere *b, u32 *ptri = &sc->arrindices[ idx*3 ]; v3f tri[3]; + if( sc->arrvertices[ptri[0]].flags & ignore ) continue; + for( int j=0; j<3; j++ ) v3_copy( sc->arrvertices[ptri[j]].co, tri[j] ); @@ -910,8 +914,8 @@ VG_STATIC int rb_sphere__scene( m4x3f mtxA, rb_sphere *b, return count; } -VG_STATIC int rb_box__scene( m4x3f mtxA, boxf bbx, - m4x3f mtxB, rb_scene *s, rb_ct *buf ){ +static int rb_box__scene( m4x3f mtxA, boxf bbx, + m4x3f mtxB, rb_scene *s, rb_ct *buf, u16 ignore ){ scene_context *sc = s->bh_scene->user; v3f tri[3]; @@ -941,6 +945,7 @@ VG_STATIC int rb_box__scene( m4x3f mtxA, boxf bbx, while( bh_next( s->bh_scene, &it, &idx ) ){ u32 *ptri = &sc->arrindices[ idx*3 ]; + if( sc->arrvertices[ptri[0]].flags & ignore ) continue; for( int j=0; j<3; j++ ) v3_copy( sc->arrvertices[ptri[j]].co, tri[j] ); @@ -961,6 +966,14 @@ VG_STATIC int rb_box__scene( m4x3f mtxA, boxf bbx, v3_sub( tri[1], tri[0], v0 ); v3_sub( tri[2], tri[0], v1 ); v3_cross( v0, v1, n ); + + if( v3_length2( n ) <= 0.00001f ){ +#ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING + vg_error( "Zero area triangle!\n" ); +#endif + return 0; + } + v3_normalize( n ); /* find best feature */ @@ -1055,7 +1068,7 @@ VG_STATIC int rb_box__scene( m4x3f mtxA, boxf bbx, return count; } -VG_STATIC int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c, +static int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c, v3f tri[3], rb_ct *buf ){ v3f pc, p0w, p1w; v3_muladds( mtxA[3], mtxA[1], -c->height*0.5f+c->radius, p0w ); @@ -1099,7 +1112,9 @@ VG_STATIC int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c, v3_cross( v0, v1, n ); if( v3_length2( n ) <= 0.00001f ){ +#ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING vg_error( "Zero area triangle!\n" ); +#endif return 0; } @@ -1113,9 +1128,9 @@ VG_STATIC int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c, } /* mtxB is defined only for tradition; it is not used currently */ -VG_STATIC int rb_capsule__scene( m4x3f mtxA, rb_capsule *c, +static int rb_capsule__scene( m4x3f mtxA, rb_capsule *c, m4x3f mtxB, rb_scene *s, - rb_ct *buf ){ + rb_ct *buf, u16 ignore ){ int count = 0; boxf bbx; @@ -1129,8 +1144,9 @@ VG_STATIC int rb_capsule__scene( m4x3f mtxA, rb_capsule *c, i32 idx; while( bh_next( s->bh_scene, &it, &idx ) ){ u32 *ptri = &sc->arrindices[ idx*3 ]; - v3f tri[3]; + if( sc->arrvertices[ptri[0]].flags & ignore ) continue; + v3f tri[3]; for( int j=0; j<3; j++ ) v3_copy( sc->arrvertices[ptri[j]].co, tri[j] ); @@ -1148,14 +1164,14 @@ VG_STATIC int rb_capsule__scene( m4x3f mtxA, rb_capsule *c, return count; } -VG_STATIC int rb_global_has_space( void ){ +static int rb_global_has_space( void ){ if( rb_contact_count + 16 > vg_list_size(rb_contact_buffer) ) return 0; return 1; } -VG_STATIC rb_ct *rb_global_buffer( void ){ +static rb_ct *rb_global_buffer( void ){ return &rb_contact_buffer[ rb_contact_count ]; } @@ -1165,15 +1181,15 @@ VG_STATIC rb_ct *rb_global_buffer( void ){ * ----------------------------------------------------------------------------- */ -VG_STATIC void rb_solver_reset(void){ +static void rb_solver_reset(void){ rb_contact_count = 0; } -VG_STATIC rb_ct *rb_global_ct(void){ +static rb_ct *rb_global_ct(void){ return rb_contact_buffer + rb_contact_count; } -VG_STATIC void rb_prepare_contact( rb_ct *ct, float timestep ){ +static void rb_prepare_contact( rb_ct *ct, float timestep ){ ct->bias = -0.2f * (timestep*3600.0f) * vg_minf( 0.0f, -ct->p+k_penetration_slop ); @@ -1184,7 +1200,7 @@ VG_STATIC void rb_prepare_contact( rb_ct *ct, float timestep ){ } /* calculate total move. manifold should belong to ONE object only */ -VG_STATIC void rb_depenetrate( rb_ct *manifold, int len, v3f dt ){ +static void rb_depenetrate( rb_ct *manifold, int len, v3f dt ){ v3_zero( dt ); for( int j=0; j<7; j++ ) @@ -1205,7 +1221,7 @@ VG_STATIC void rb_depenetrate( rb_ct *manifold, int len, v3f dt ){ /* * Initializing things like tangent vectors */ -VG_STATIC void rb_presolve_contacts( rb_ct *buffer, int len ){ +static void rb_presolve_contacts( rb_ct *buffer, int len ){ for( int i=0; iw, ra, rva ); v3_add( rba->v, rva, rva ); @@ -1256,7 +1272,7 @@ VG_STATIC void rb_rcv( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb, v3f rv ){ v3_sub( rva, rvb, rv ); } -VG_STATIC void rb_contact_restitution( rb_ct *ct, float cr ){ +static void rb_contact_restitution( rb_ct *ct, float cr ){ v3f rv, ra, rb; v3_sub( ct->co, ct->rba->co, ra ); v3_sub( ct->co, ct->rbb->co, rb ); @@ -1272,7 +1288,7 @@ VG_STATIC void rb_contact_restitution( rb_ct *ct, float cr ){ /* * Apply impulse to object */ -VG_STATIC void rb_linear_impulse( rigidbody *rb, v3f delta, v3f impulse ){ +static void rb_linear_impulse( rigidbody *rb, v3f delta, v3f impulse ){ /* linear */ v3_muladds( rb->v, impulse, rb->inv_mass, rb->v ); @@ -1287,7 +1303,7 @@ VG_STATIC void rb_linear_impulse( rigidbody *rb, v3f delta, v3f impulse ){ /* * One iteration to solve the contact constraint */ -VG_STATIC void rb_solve_contacts( rb_ct *buf, int len ){ +static void rb_solve_contacts( rb_ct *buf, int len ){ for( int i=0; irba, *rbb = constr->rbb; @@ -1356,7 +1372,7 @@ VG_STATIC void rb_debug_position_constraints( rb_constr_pos *buffer, int len ){ } } -VG_STATIC void rb_presolve_swingtwist_constraints( rb_constr_swingtwist *buf, +static void rb_presolve_swingtwist_constraints( rb_constr_swingtwist *buf, int len ){ float size = 0.12f; @@ -1447,7 +1463,7 @@ VG_STATIC void rb_presolve_swingtwist_constraints( rb_constr_swingtwist *buf, } } -VG_STATIC void rb_debug_swingtwist_constraints( rb_constr_swingtwist *buf, +static void rb_debug_swingtwist_constraints( rb_constr_swingtwist *buf, int len ){ float size = 0.12f; @@ -1543,7 +1559,7 @@ VG_STATIC void rb_debug_swingtwist_constraints( rb_constr_swingtwist *buf, /* * Solve a list of positional constraints */ -VG_STATIC void rb_solve_position_constraints( rb_constr_pos *buf, int len ){ +static void rb_solve_position_constraints( rb_constr_pos *buf, int len ){ for( int i=0; irba, *rbb = constr->rbb; @@ -1607,7 +1623,7 @@ VG_STATIC void rb_solve_position_constraints( rb_constr_pos *buf, int len ){ } } -VG_STATIC void rb_solve_swingtwist_constraints( rb_constr_swingtwist *buf, +static void rb_solve_swingtwist_constraints( rb_constr_swingtwist *buf, int len ){ float size = 0.12f; @@ -1662,7 +1678,7 @@ VG_STATIC void rb_solve_swingtwist_constraints( rb_constr_swingtwist *buf, } } -VG_STATIC void rb_solve_constr_angle( rigidbody *rba, rigidbody *rbb, +static void rb_solve_constr_angle( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb ){ m3x3f ssra, ssrb, ssrat, ssrbt; m3x3f cma, cmb; @@ -1704,7 +1720,7 @@ VG_STATIC void rb_solve_constr_angle( rigidbody *rba, rigidbody *rbb, * Correct position constraint drift errors * [ 0.0 <= amt <= 1.0 ]: the correction amount */ -VG_STATIC void rb_correct_position_constraints( rb_constr_pos *buf, int len, +static void rb_correct_position_constraints( rb_constr_pos *buf, int len, float amt ){ for( int i=0; irba, @@ -1787,7 +1803,7 @@ VG_STATIC void rb_correct_contact_constraints( rb_ct *buf, int len, float amt ){ * Effectors */ -VG_STATIC void rb_effect_simple_bouyency( rigidbody *ra, v4f plane, +static void rb_effect_simple_bouyency( rigidbody *ra, v4f plane, float amt, float drag ){ /* float */ float depth = v3_dot( plane, ra->co ) - plane[3], @@ -1802,7 +1818,7 @@ VG_STATIC void rb_effect_simple_bouyency( rigidbody *ra, v4f plane, /* apply a spring&dampener force to match ra(worldspace) on rigidbody, to * rt(worldspace) */ -VG_STATIC void rb_effect_spring_target_vector( rigidbody *rba, v3f ra, v3f rt, +static void rb_effect_spring_target_vector( rigidbody *rba, v3f ra, v3f rt, float spring, float dampening, float timestep ){ float d = v3_dot( rt, ra );