X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=rigidbody.h;h=aa74a8477436296d4932a02ae91691cb2096cdfc;hb=a6e1ee0f51aa5570b20aad658365dec896f8c9b8;hp=cf258b8282ddcefe10839d69a10f895c906b31ae;hpb=d2fa06a21b490b38352195005c3134683679f9c0;p=carveJwlIkooP6JGAAIwe30JlM.git diff --git a/rigidbody.h b/rigidbody.h index cf258b8..aa74a84 100644 --- a/rigidbody.h +++ b/rigidbody.h @@ -1,10 +1,17 @@ -/* SHite fphysics */ +/* + * Resources: Box2D - Erin Catto + * qu3e - Randy Gaul + */ + +#include "common.h" +static void rb_tangent_basis( v3f n, v3f tx, v3f ty ); #ifndef RIGIDBODY_H #define RIGIDBODY_H -#include "vg/vg.h" -#include "scene.h" +#include "bvh.h" + +#define RB_DEPR #define k_rb_delta (1.0f/60.0f) @@ -13,7 +20,8 @@ struct rigidbody { v3f co, v, I; v4f q; - boxf bbx; + boxf bbx, bbx_world; + float inv_mass; struct contact { @@ -21,9 +29,10 @@ struct rigidbody v3f t[2]; float bias, norm_impulse, tangent_impulse[2]; } - manifold[4]; + manifold[12]; int manifold_count; + v3f delta; /* where is the origin of this in relation to a parent body */ m4x3f to_world, to_local; }; @@ -34,6 +43,9 @@ static void rb_update_transform( rigidbody *rb ) v3_copy( rb->co, rb->to_world[3] ); m4x3_invert_affine( rb->to_world, rb->to_local ); + + box_copy( rb->bbx, rb->bbx_world ); + m4x3_transform_aabb( rb->to_world, rb->bbx_world ); } static void rb_init( rigidbody *rb ) @@ -42,6 +54,11 @@ static void rb_init( rigidbody *rb ) v3_zero( rb->v ); v3_zero( rb->I ); + v3f dims; + v3_sub( rb->bbx[1], rb->bbx[0], dims ); + + rb->inv_mass = 1.0f/(8.0f*dims[0]*dims[1]*dims[2]); + rb_update_transform( rb ); } @@ -94,7 +111,14 @@ static void rb_tangent_basis( v3f n, v3f tx, v3f ty ) v3_cross( n, tx, ty ); } -static void rb_build_manifold( rigidbody *rb, scene *sc ) +#include "world.h" + +static void rb_manifold_reset( rigidbody *rb ) +{ + rb->manifold_count = 0; +} + +static void rb_build_manifold_terrain( rigidbody *rb ) { v3f *box = rb->bbx; v3f pts[8]; @@ -120,7 +144,7 @@ static void rb_build_manifold( rigidbody *rb, scene *sc ) m4x3_mulv( rb->to_world, p110, p110 ); m4x3_mulv( rb->to_world, p111, p111 ); - rb->manifold_count = 0; + int count = 0; for( int i=0; i<8; i++ ) { @@ -128,9 +152,16 @@ static void rb_build_manifold( rigidbody *rb, scene *sc ) struct contact *ct = &rb->manifold[rb->manifold_count]; v3f surface; - v3_copy( point, surface ); - bvh_scene_sample( sc, surface, ct->n ); + surface[1] += 4.0f; + + ray_hit hit; + hit.dist = INFINITY; + if( !ray_world( surface, (v3f){0.0f,-1.0f,0.0f}, &hit )) + continue; + + v3_copy( hit.normal, ct->n ); + v3_copy( hit.pos, surface ); float p = vg_minf( surface[1] - point[1], 1.0f ); @@ -150,7 +181,8 @@ static void rb_build_manifold( rigidbody *rb, scene *sc ) ct->tangent_impulse[1] = 0.0f; rb->manifold_count ++; - if( rb->manifold_count == 4 ) + count ++; + if( count == 4 ) break; } } @@ -158,7 +190,7 @@ static void rb_build_manifold( rigidbody *rb, scene *sc ) static void rb_constraint_manifold( rigidbody *rb ) { - float k_friction = 0.07f; + float k_friction = 0.1f; /* Friction Impulse */ for( int i=0; imanifold_count; i++ ) @@ -214,6 +246,51 @@ static void rb_constraint_manifold( rigidbody *rb ) } } +struct rb_angle_limit +{ + rigidbody *rba, *rbb; + v3f axis; + float impulse, bias; +}; + +static int rb_angle_limit_force( rigidbody *rba, v3f va, + rigidbody *rbb, v3f vb, + float max ) +{ + v3f wva, wvb; + m3x3_mulv( rba->to_world, va, wva ); + m3x3_mulv( rbb->to_world, vb, wvb ); + + float dt = v3_dot(wva,wvb)*0.999f, + ang = fabsf(dt); + ang = acosf( dt ); + if( ang > max ) + { + float correction = max-ang; + + v3f axis; + v3_cross( wva, wvb, axis ); + + v4f rotation; + q_axis_angle( rotation, axis, -correction*0.25f ); + q_mul( rotation, rba->q, rba->q ); + + q_axis_angle( rotation, axis, correction*0.25f ); + q_mul( rotation, rbb->q, rbb->q ); + + return 1; + } + + return 0; +} + +static void rb_constraint_angle_limit( struct rb_angle_limit *limit ) +{ + +} + + +RB_DEPR static void rb_constraint_angle( rigidbody *rba, v3f va, rigidbody *rbb, v3f vb, float max, float spring ) @@ -248,11 +325,26 @@ static void rb_constraint_angle( rigidbody *rba, v3f va, } } +static void rb_relative_velocity( rigidbody *ra, v3f lca, + rigidbody *rb, v3f lcb, v3f rcv ) +{ + v3f wca, wcb; + m3x3_mulv( ra->to_world, lca, wca ); + m3x3_mulv( rb->to_world, lcb, wcb ); + + v3_sub( ra->v, rb->v, rcv ); + + v3f rcv_Ra, rcv_Rb; + v3_cross( ra->I, wca, rcv_Ra ); + v3_cross( rb->I, wcb, rcv_Rb ); + v3_add( rcv_Ra, rcv, rcv ); + v3_sub( rcv, rcv_Rb, rcv ); +} + static void rb_constraint_position( rigidbody *ra, v3f lca, rigidbody *rb, v3f lcb ) { /* C = (COa + Ra*LCa) - (COb + Rb*LCb) = 0 */ - v3f wca, wcb; m3x3_mulv( ra->to_world, lca, wca ); m3x3_mulv( rb->to_world, lcb, wcb ); @@ -273,19 +365,24 @@ static void rb_constraint_position( rigidbody *ra, v3f lca, v3_cross( rb->I, wcb, rcv_Rb ); v3_add( rcv_Ra, rcv, rcv ); v3_sub( rcv, rcv_Rb, rcv ); + + float nm = 0.5f/(rb->inv_mass + ra->inv_mass); + + float mass_a = 1.0f/ra->inv_mass, + mass_b = 1.0f/rb->inv_mass, + total_mass = mass_a+mass_b; v3f impulse; - v3_muls( rcv, 0.5f, impulse ); - v3_add( impulse, rb->v, rb->v ); + v3_muls( rcv, 1.0f, impulse ); + v3_muladds( rb->v, impulse, mass_b/total_mass, rb->v ); v3_cross( wcb, impulse, impulse ); v3_add( impulse, rb->I, rb->I ); - v3_muls( rcv, -0.5f, impulse ); - v3_add( impulse, ra->v, ra->v ); + v3_muls( rcv, -1.0f, impulse ); + v3_muladds( ra->v, impulse, mass_a/total_mass, ra->v ); v3_cross( wca, impulse, impulse ); v3_add( impulse, ra->I, ra->I ); - #if 0 v3f impulse; v3_muls( delta, 0.5f*spring, impulse ); @@ -345,4 +442,148 @@ static void rb_debug( rigidbody *rb, u32 colour ) vg_line( p100, p010, colour ); } +/* + * out penetration distance, normal + */ +static int rb_point_in_body( rigidbody *rb, v3f pos, float *pen, v3f normal ) +{ + v3f local; + m4x3_mulv( rb->to_local, pos, local ); + + if( local[0] > rb->bbx[0][0] && local[0] < rb->bbx[1][0] && + local[1] > rb->bbx[0][1] && local[1] < rb->bbx[1][1] && + local[2] > rb->bbx[0][2] && local[2] < rb->bbx[1][2] ) + { + v3f area, com, comrel; + v3_add( rb->bbx[0], rb->bbx[1], com ); + v3_muls( com, 0.5f, com ); + + v3_sub( rb->bbx[1], rb->bbx[0], area ); + v3_sub( local, com, comrel ); + v3_div( comrel, area, comrel ); + + int axis = 0; + float max_mag = fabsf(comrel[0]); + + if( fabsf(comrel[1]) > max_mag ) + { + axis = 1; + max_mag = fabsf(comrel[1]); + } + if( fabsf(comrel[2]) > max_mag ) + { + axis = 2; + max_mag = fabsf(comrel[2]); + } + + v3_zero( normal ); + normal[axis] = vg_signf(comrel[axis]); + + if( normal[axis] < 0.0f ) + *pen = local[axis] - rb->bbx[0][axis]; + else + *pen = rb->bbx[1][axis] - local[axis]; + + m3x3_mulv( rb->to_world, normal, normal ); + return 1; + } + + return 0; +} + +static void rb_build_manifold_rb_static( rigidbody *ra, rigidbody *rb_static ) +{ + v3f verts[8]; + + v3f a, b; + v3_copy( ra->bbx[0], a ); + v3_copy( ra->bbx[1], b ); + + m4x3_mulv( ra->to_world, (v3f){ a[0], a[1], a[2] }, verts[0] ); + m4x3_mulv( ra->to_world, (v3f){ a[0], b[1], a[2] }, verts[1] ); + m4x3_mulv( ra->to_world, (v3f){ b[0], b[1], a[2] }, verts[2] ); + m4x3_mulv( ra->to_world, (v3f){ b[0], a[1], a[2] }, verts[3] ); + m4x3_mulv( ra->to_world, (v3f){ a[0], a[1], b[2] }, verts[4] ); + m4x3_mulv( ra->to_world, (v3f){ a[0], b[1], b[2] }, verts[5] ); + m4x3_mulv( ra->to_world, (v3f){ b[0], b[1], b[2] }, verts[6] ); + m4x3_mulv( ra->to_world, (v3f){ b[0], a[1], b[2] }, verts[7] ); + + int count = 0; + + for( int i=0; i<8; i++ ) + { + if( ra->manifold_count == vg_list_size(ra->manifold) ) + return; + + struct contact *ct = &ra->manifold[ ra->manifold_count ]; + + float p; + v3f normal; + + if( rb_point_in_body( rb_static, verts[i], &p, normal )) + { + v3_copy( normal, ct->n ); + v3_muladds( verts[i], ct->n, p*0.5f, ct->co ); + v3_sub( ct->co, ra->co, ct->delta ); + + vg_line_pt3( ct->co, 0.0125f, 0xffff00ff ); + + ct->bias = -0.2f * (1.0f/k_rb_delta) * vg_minf( 0.0f, -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; + + ra->manifold_count ++; + count ++; + if( count == 4 ) + return; + } + } +} + +/* + * BVH implementation, this is ONLY for static rigidbodies, its to slow for + * realtime use. + */ + +static void rb_bh_expand_bound( void *user, boxf bound, u32 item_index ) +{ + rigidbody *rb = &((rigidbody *)user)[ item_index ]; + box_concat( bound, rb->bbx_world ); +} + +static float rb_bh_centroid( void *user, u32 item_index, int axis ) +{ + rigidbody *rb = &((rigidbody *)user)[ item_index ]; + return (rb->bbx_world[axis][0] + rb->bbx_world[1][axis]) * 0.5f; +} + +static void rb_bh_swap( void *user, u32 ia, u32 ib ) +{ + rigidbody temp, *rba, *rbb; + rba = &((rigidbody *)user)[ ia ]; + rbb = &((rigidbody *)user)[ ib ]; + + temp = *rba; + *rba = *rbb; + *rbb = temp; +} + +static void rb_bh_debug( void *user, u32 item_index ) +{ + rigidbody *rb = &((rigidbody *)user)[ item_index ]; + rb_debug( rb, 0xff00ffff ); +} + +static bh_system bh_system_rigidbodies = +{ + .expand_bound = rb_bh_expand_bound, + .item_centroid = rb_bh_centroid, + .item_swap = rb_bh_swap, + .item_debug = rb_bh_debug, + .cast_ray = NULL +}; + #endif /* RIGIDBODY_H */