X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=rigidbody.h;h=e374f3fbd6e773cdbc5f33349a5202f576125903;hb=1656d58a7bd17df4a1edcc9677ade4dbafc82229;hp=01734e8f2d74760ab32659165a1a51c633a4785c;hpb=7758c7efec3956c68294bc914e7524045a2b1bd7;p=carveJwlIkooP6JGAAIwe30JlM.git diff --git a/rigidbody.h b/rigidbody.h index 01734e8..e374f3f 100644 --- a/rigidbody.h +++ b/rigidbody.h @@ -3,15 +3,15 @@ * qu3e - Randy Gaul */ -#include "vg/vg.h" +#include "common.h" static void rb_tangent_basis( v3f n, v3f tx, v3f ty ); #ifndef RIGIDBODY_H #define RIGIDBODY_H -#define RB_DEPR +#include "bvh.h" -#include "world.h" +#define RB_DEPR #define k_rb_delta (1.0f/60.0f) @@ -20,7 +20,7 @@ struct rigidbody { v3f co, v, I; v4f q; - boxf bbx; + boxf bbx, bbx_world; float inv_mass; struct contact @@ -29,7 +29,7 @@ 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 */ @@ -43,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 ) @@ -108,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 ) +#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]; @@ -134,7 +144,7 @@ static void rb_build_manifold( rigidbody *rb ) 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++ ) { @@ -171,7 +181,8 @@ static void rb_build_manifold( rigidbody *rb ) ct->tangent_impulse[1] = 0.0f; rb->manifold_count ++; - if( rb->manifold_count == 4 ) + count ++; + if( count == 4 ) break; } } @@ -242,8 +253,7 @@ struct rb_angle_limit float impulse, bias; }; -static int rb_angle_limit_force( - rigidbody *rba, v3f va, +static int rb_angle_limit_force( rigidbody *rba, v3f va, rigidbody *rbb, v3f vb, float max ) { @@ -432,4 +442,150 @@ 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, + + .item_size = sizeof(rigidbody) +}; + #endif /* RIGIDBODY_H */