X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=rigidbody.h;h=c00b0f04f75817ad21072755ed983ce098094bfb;hb=84a7ae83a25966e0004a1a4b409dbb3d49fae286;hp=d911688beca8a5e6fc7e98399aa0739b029f02eb;hpb=98ecdd9f0377f1563566fe4d3d3b8185a055ec4d;p=carveJwlIkooP6JGAAIwe30JlM.git diff --git a/rigidbody.h b/rigidbody.h index d911688..c00b0f0 100644 --- a/rigidbody.h +++ b/rigidbody.h @@ -17,6 +17,8 @@ static bh_system bh_system_rigidbodies; #define k_rb_delta (1.0f/k_rb_rate) typedef struct rigidbody rigidbody; +typedef struct contact rb_ct; + struct rigidbody { v3f co, v, w; @@ -48,6 +50,8 @@ struct rigidbody v3f right, up, forward; + int is_world; + boxf bbx, bbx_world; float inv_mass; @@ -55,14 +59,24 @@ struct rigidbody m4x3f to_world, to_local; }; +static rigidbody rb_static_null = +{ + .co={0.0f,0.0f,0.0f}, + .q={0.0f,0.0f,0.0f,1.0f}, + .v={0.0f,0.0f,0.0f}, + .w={0.0f,0.0f,0.0f}, + .is_world = 1, + .inv_mass = 0.0f +}; + static void rb_debug( rigidbody *rb, u32 colour ); static struct contact { - rigidbody *rba; - v3f co, n, delta; + rigidbody *rba, *rbb; + v3f co, n; v3f t[2]; - float bias, norm_impulse, tangent_impulse[2]; + float mass_total, p, bias, norm_impulse, tangent_impulse[2]; } rb_contact_buffer[256]; static int rb_contact_count = 0; @@ -117,7 +131,14 @@ static void rb_init( rigidbody *rb ) rb->bbx[1][1] = h; } - rb->inv_mass = 1.0f/(8.0f*volume); + if( rb->is_world ) + { + rb->inv_mass = 0.0f; + } + else + { + rb->inv_mass = 1.0f/(8.0f*volume); + } v3_zero( rb->v ); v3_zero( rb->w ); @@ -178,6 +199,10 @@ static void rb_build_manifold_terrain( rigidbody *rb ); static void rb_build_manifold_terrain_sphere( rigidbody *rb ); static void rb_solve_contacts(void); +/* + * These closest point tests were learned from Real-Time Collision Detection by + * Christer Ericson + */ static float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2, float *s, float *t, v3f c1, v3f c2) { @@ -255,6 +280,20 @@ static float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2, return v3_length2( v0 ); } +static void closest_point_aabb( v3f p, boxf box, v3f dest ) +{ + v3_maxv( p, box[0], dest ); + v3_minv( dest, box[1], dest ); +} + +static void closest_point_obb( v3f p, rigidbody *rb, v3f dest ) +{ + v3f local; + m4x3_mulv( rb->to_local, p, local ); + closest_point_aabb( local, rb->bbx, local ); + m4x3_mulv( rb->to_world, local, dest ); +} + static void closest_point_segment( v3f a, v3f b, v3f point, v3f dest ) { v3f v0, v1; @@ -265,7 +304,6 @@ static void closest_point_segment( v3f a, v3f b, v3f point, v3f dest ) v3_muladds( a, v0, vg_clampf(t,0.0f,1.0f), dest ); } -/* Real-Time Collision Detection */ static void closest_on_triangle( v3f p, v3f tri[3], v3f dest ) { v3f ab, ac, ap; @@ -355,6 +393,143 @@ static void closest_on_triangle( v3f p, v3f tri[3], v3f dest ) v3_muladds( dest, ac, w, dest ); } +/* + * Contact generators + * + * These do not automatically allocate contacts, an appropriately sized + * buffer must be supplied. The function returns the size of the manifold + * which was generated. + * + * The values set on the contacts are: n, co, p, rba, rbb + */ + +static void rb_debug_contact( rb_ct *ct ) +{ + v3f p1; + v3_muladds( ct->co, ct->n, 0.2f, p1 ); + vg_line_pt3( ct->co, 0.1f, 0xff0000ff ); + vg_line( ct->co, p1, 0xffffffff ); +} + +static int rb_sphere_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf ) +{ + v3f co, delta; + + closest_point_obb( rba->co, rbb, co ); + v3_sub( rba->co, co, delta ); + + float d2 = v3_length2(delta), + r = rba->inf.sphere.radius; + + if( d2 <= r*r ) + { + float d; + if( d2 <= 0.0001f ) + { + v3_sub( rbb->co, rba->co, delta ); + d2 = v3_length2(delta); + } + + d = sqrtf(d2); + + rb_ct *ct = buf; + v3_muls( delta, 1.0f/d, ct->n ); + v3_copy( co, ct->co ); + ct->p = r-d; + ct->rba = rba; + ct->rbb = rbb; + return 1; + } + + return 0; +} + +static int rb_sphere_vs_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf ) +{ + v3f delta; + v3_sub( rba->co, rbb->co, delta ); + + float d2 = v3_length2(delta), + r = rba->inf.sphere.radius + rbb->inf.sphere.radius; + + if( d2 < r*r ) + { + float d = sqrtf(d2); + + rb_ct *ct = buf; + v3_muls( delta, -1.0f/d, ct->n ); + + v3f p0, p1; + v3_muladds( rba->co, ct->n, rba->inf.sphere.radius, p0 ); + v3_muladds( rbb->co, ct->n,-rbb->inf.sphere.radius, p1 ); + v3_add( p0, p1, ct->co ); + v3_muls( ct->co, 0.5f, ct->co ); + ct->p = r-d; + ct->rba = rba; + ct->rbb = rbb; + return 1; + } + + return 0; +} + +static int rb_box_vs_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf ) +{ + return rb_sphere_vs_box( rbb, rba, buf ); +} + +static int rb_box_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf ) +{ + /* TODO: Generating a stable quad manifold, lots of clipping */ + return 0; +} + +/* + * This function does not accept triangle as a dynamic object, it is assumed + * to always be static. + * + * The triangle is also assumed to be one sided for better detection + */ +static int rb_sphere_vs_triangle( rigidbody *rba, v3f tri[3], rb_ct *buf ) +{ + v3f delta, co; + + closest_on_triangle( rba->co, tri, co ); + v3_sub( rba->co, co, delta ); + + float d2 = v3_length2( delta ), + r = rba->inf.sphere.radius; + + if( d2 < r*r ) + { + v3f ab, ac, tn; + v3_sub( tri[1], tri[0], ab ); + v3_sub( tri[2], tri[0], ac ); + v3_cross( ac, ab, tn ); + + if( v3_dot( delta, tn ) > 0.0f ) + v3_muls( delta, -1.0f, delta ); + + float d = sqrtf(d2); + + rb_ct *ct = buf; + v3_muls( delta, 1.0f/d, ct->n ); + v3_copy( co, ct->co ); + ct->p = r-d; + ct->rba = rba; + ct->rbb = &rb_static_null; + return 1; + } + + return 0; +} + + +/* + * Generic functions + */ + +RB_DEPR static int sphere_vs_triangle( v3f c, float r, v3f tri[3], v3f co, v3f norm, float *p ) { @@ -394,6 +569,11 @@ static void rb_solver_reset(void) rb_contact_count = 0; } +static rb_ct *rb_global_ct(void) +{ + return rb_contact_buffer + rb_contact_count; +} + static struct contact *rb_start_contact(void) { if( rb_contact_count == vg_list_size(rb_contact_buffer) ) @@ -454,8 +634,6 @@ static void rb_build_manifold_terrain_sphere( rigidbody *rb ) ct->rba = rb; v3_copy( co, ct->co ); v3_copy( norm, ct->n ); - - v3_sub( co, rb->co, ct->delta ); rb_commit_contact( ct, p ); } } @@ -520,7 +698,6 @@ static void rb_build_manifold_terrain( rigidbody *rb ) v3_copy( hit.normal, ct->n ); v3_add( point, surface, ct->co ); v3_muls( ct->co, 0.5f, ct->co ); - v3_sub( ct->co, rb->co, ct->delta ); rb_commit_contact( ct, p ); count ++; @@ -530,37 +707,91 @@ static void rb_build_manifold_terrain( rigidbody *rb ) } } +/* + * Initializing things like tangent vectors + */ +static void rb_presolve_contacts(void) +{ + for( int i=0; ibias = -0.2f * k_rb_rate * vg_minf(0.0f,-ct->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; + ct->mass_total = 1.0f/(ct->rba->inv_mass + ct->rbb->inv_mass); + + rb_debug_contact( ct ); + } +} + +/* + * Creates relative contact velocity vector, and offsets between each body */ +static void rb_rcv( rb_ct *ct, v3f rv, v3f da, v3f db ) +{ + rigidbody *rba = ct->rba, + *rbb = ct->rbb; + + v3_sub( rba->co, ct->co, da ); + v3_sub( rbb->co, ct->co, db ); + + v3f rva, rvb; + v3_cross( rba->w, da, rva ); + v3_add( rba->v, rva, rva ); + + v3_cross( rbb->w, db, rvb ); + v3_add( rbb->v, rvb, rvb ); + v3_add( rva, rvb, rv ); +} + +static void rb_standard_impulse( rb_ct *ct, v3f da, v3f db, v3f impulse ) +{ + rigidbody *rba = ct->rba, + *rbb = ct->rbb; + + /* response */ + v3_muladds( rba->v, impulse, ct->mass_total * rba->inv_mass, rba->v ); + v3_muladds( rbb->v, impulse, ct->mass_total * rbb->inv_mass, rbb->v ); + + /* Angular velocity */ + v3f wa, wb; + v3_cross( da, impulse, wa ); + v3_cross( db, impulse, wb ); + v3_muladds( rba->w, wa, ct->mass_total * rba->inv_mass, rba->w ); + v3_muladds( rbb->w, wb, ct->mass_total * rbb->inv_mass, rbb->w ); +} + static void rb_solve_contacts(void) { float k_friction = 0.1f; + /* TODO: second object + * Static objects route to static element */ + /* Friction Impulse */ for( int i=0; irba; - v3f dv; - v3_cross( rb->w, ct->delta, dv ); - v3_add( rb->v, dv, dv ); + v3f rv, da, db; + rb_rcv( ct, rv, da, db ); for( int j=0; j<2; j++ ) { - float vt = vg_clampf( -v3_dot( dv, ct->t[j] ), - -k_friction, k_friction ); - - vt = -v3_dot( dv, ct->t[j] ); + float f = k_friction * ct->norm_impulse, + vt = -v3_dot( rv, ct->t[j] ); float temp = ct->tangent_impulse[j]; - ct->tangent_impulse[j] = vg_clampf( temp+vt, -k_friction, k_friction ); + ct->tangent_impulse[j] = vg_clampf( temp+vt, -f, f ); vt = ct->tangent_impulse[j] - temp; v3f impulse; - v3_muls( ct->t[j], vt, impulse ); - v3_add( impulse, rb->v, rb->v ); - v3_cross( ct->delta, impulse, impulse ); - v3_add( impulse, rb->w, rb->w ); + rb_standard_impulse( ct, da, db, impulse ); } } @@ -568,13 +799,13 @@ static void rb_solve_contacts(void) for( int i=0; irba; + rigidbody *rba = ct->rba, + *rbb = ct->rbb; - v3f dv; - v3_cross( rb->w, ct->delta, dv ); - v3_add( rb->v, dv, dv ); + v3f rv, da, db; + rb_rcv( ct, rv, da, db ); - float vn = -v3_dot( dv, ct->n ); + float vn = -v3_dot( rv, ct->n ); vn += ct->bias; float temp = ct->norm_impulse; @@ -582,11 +813,8 @@ static void rb_solve_contacts(void) vn = ct->norm_impulse - temp; v3f impulse; - v3_muls( ct->n, vn, impulse ); - v3_add( impulse, rb->v, rb->v ); - v3_cross( ct->delta, impulse, impulse ); - v3_add( impulse, rb->w, rb->w ); + rb_standard_impulse( ct, da, db, impulse ); } }