X-Git-Url: https://harrygodden.com/git/?p=vg.git;a=blobdiff_plain;f=vg_rigidbody_collision.h;fp=vg_rigidbody_collision.h;h=e0cf3e378e6a7b0f78c597a3dbc50132c5f81e0c;hp=c08baa536a5e55714d98fd7a39393d8e2098ed7f;hb=3b14f3dcd5bf9dd3c85144f2123d667bfa4bb63f;hpb=fce86711735b15bff37de0f70716808410fcf269 diff --git a/vg_rigidbody_collision.h b/vg_rigidbody_collision.h index c08baa5..e0cf3e3 100644 --- a/vg_rigidbody_collision.h +++ b/vg_rigidbody_collision.h @@ -1,12 +1,11 @@ #pragma once -#include "vg_rigidbody.h" -#ifndef VG_MAX_CONTACTS +/* TODO: Get rid of this! */ #define VG_MAX_CONTACTS 256 -#endif typedef struct rb_ct rb_ct; -static struct rb_ct{ +struct rb_ct +{ rigidbody *rba, *rbb; v3f co, n; v3f t[2]; @@ -17,877 +16,55 @@ static struct rb_ct{ enum contact_type type; } -rb_contact_buffer[VG_MAX_CONTACTS]; -static int rb_contact_count = 0; - -/* - * 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 - */ - -/* - * By collecting the minimum(time) and maximum(time) pairs of points, we - * build a reduced and stable exact manifold. - * - * tx: time at point - * rx: minimum distance of these points - * dx: the delta between the two points - * - * pairs will only ammend these if they are creating a collision - */ -typedef struct capsule_manifold capsule_manifold; -struct capsule_manifold{ - f32 t0, t1; - f32 r0, r1; - v3f d0, d1; -}; - -/* - * Expand a line manifold with a new pair. t value is the time along segment - * on the oriented object which created this pair. - */ -static void rb_capsule_manifold( v3f pa, v3f pb, f32 t, f32 r, - capsule_manifold *manifold ){ - v3f delta; - v3_sub( pa, pb, delta ); - - if( v3_length2(delta) < r*r ){ - if( t < manifold->t0 ){ - v3_copy( delta, manifold->d0 ); - manifold->t0 = t; - manifold->r0 = r; - } - - if( t > manifold->t1 ){ - v3_copy( delta, manifold->d1 ); - manifold->t1 = t; - manifold->r1 = r; - } - } -} - -static void rb_capsule_manifold_init( capsule_manifold *manifold ){ - manifold->t0 = INFINITY; - manifold->t1 = -INFINITY; -} - -static int rb_capsule__manifold_done( m4x3f mtx, rb_capsule *c, - capsule_manifold *manifold, - rb_ct *buf ){ - v3f p0, p1; - v3_muladds( mtx[3], mtx[1], -c->h*0.5f+c->r, p0 ); - v3_muladds( mtx[3], mtx[1], c->h*0.5f-c->r, p1 ); - - int count = 0; - if( manifold->t0 <= 1.0f ){ - rb_ct *ct = buf; - - v3f pa; - v3_muls( p0, 1.0f-manifold->t0, pa ); - v3_muladds( pa, p1, manifold->t0, pa ); - - f32 d = v3_length( manifold->d0 ); - v3_muls( manifold->d0, 1.0f/d, ct->n ); - v3_muladds( pa, ct->n, -c->r, ct->co ); - - ct->p = manifold->r0 - d; - ct->type = k_contact_type_default; - count ++; - } - - if( (manifold->t1 >= 0.0f) && (manifold->t0 != manifold->t1) ){ - rb_ct *ct = buf+count; - - v3f pa; - v3_muls( p0, 1.0f-manifold->t1, pa ); - v3_muladds( pa, p1, manifold->t1, pa ); - - f32 d = v3_length( manifold->d1 ); - v3_muls( manifold->d1, 1.0f/d, ct->n ); - v3_muladds( pa, ct->n, -c->r, ct->co ); - - ct->p = manifold->r1 - d; - ct->type = k_contact_type_default; - - count ++; - } - - /* - * Debugging - */ - -#if 0 - if( count == 2 ) - vg_line( buf[0].co, buf[1].co, 0xff0000ff ); -#endif - - return count; -} - -static int rb_capsule__sphere( m4x3f mtxA, rb_capsule *ca, - v3f coB, f32 rb, rb_ct *buf ){ - f32 ha = ca->h, - ra = ca->r, - r = ra + rb; - - v3f p0, p1; - v3_muladds( mtxA[3], mtxA[1], -ha*0.5f+ra, p0 ); - v3_muladds( mtxA[3], mtxA[1], ha*0.5f-ra, p1 ); - - v3f c, delta; - closest_point_segment( p0, p1, coB, c ); - v3_sub( c, coB, delta ); - f32 d2 = v3_length2(delta); - - if( d2 < r*r ){ - f32 d = sqrtf(d2); - - rb_ct *ct = buf; - v3_muls( delta, 1.0f/d, ct->n ); - ct->p = r-d; - - v3f p0, p1; - v3_muladds( c, ct->n, -ra, p0 ); - v3_muladds( coB, ct->n, rb, p1 ); - v3_add( p0, p1, ct->co ); - v3_muls( ct->co, 0.5f, ct->co ); - ct->type = k_contact_type_default; - return 1; - } - else return 0; -} - -static int rb_capsule__capsule( m4x3f mtxA, rb_capsule *ca, - m4x3f mtxB, rb_capsule *cb, rb_ct *buf ){ - f32 ha = ca->h, - hb = cb->h, - ra = ca->r, - rb = cb->r, - r = ra+rb; - - v3f p0, p1, p2, p3; - v3_muladds( mtxA[3], mtxA[1], -ha*0.5f+ra, p0 ); - v3_muladds( mtxA[3], mtxA[1], ha*0.5f-ra, p1 ); - v3_muladds( mtxB[3], mtxB[1], -hb*0.5f+rb, p2 ); - v3_muladds( mtxB[3], mtxB[1], hb*0.5f-rb, p3 ); - - capsule_manifold manifold; - rb_capsule_manifold_init( &manifold ); - - v3f pa, pb; - f32 ta, tb; - closest_segment_segment( p0, p1, p2, p3, &ta, &tb, pa, pb ); - rb_capsule_manifold( pa, pb, ta, r, &manifold ); - - ta = closest_point_segment( p0, p1, p2, pa ); - tb = closest_point_segment( p0, p1, p3, pb ); - rb_capsule_manifold( pa, p2, ta, r, &manifold ); - rb_capsule_manifold( pb, p3, tb, r, &manifold ); - - closest_point_segment( p2, p3, p0, pa ); - closest_point_segment( p2, p3, p1, pb ); - rb_capsule_manifold( p0, pa, 0.0f, r, &manifold ); - rb_capsule_manifold( p1, pb, 1.0f, r, &manifold ); - - return rb_capsule__manifold_done( mtxA, ca, &manifold, buf ); -} - -/* - * Generates up to two contacts; optimised for the most stable manifold - */ -static int rb_capsule__box( m4x3f mtxA, rb_capsule *ca, - m4x3f mtxB, m4x3f mtxB_inverse, boxf box, - rb_ct *buf ){ - f32 h = ca->h, r = ca->r; - - /* - * Solving this in symetric local space of the cube saves us some time and a - * couple branches when it comes to the quad stage. - */ - v3f centroid; - v3_add( box[0], box[1], centroid ); - v3_muls( centroid, 0.5f, centroid ); - - boxf bbx; - v3_sub( box[0], centroid, bbx[0] ); - v3_sub( box[1], centroid, bbx[1] ); - - v3f pc, p0w, p1w, p0, p1; - v3_muladds( mtxA[3], mtxA[1], -h*0.5f+r, p0w ); - v3_muladds( mtxA[3], mtxA[1], h*0.5f-r, p1w ); - - m4x3_mulv( mtxB_inverse, p0w, p0 ); - m4x3_mulv( mtxB_inverse, p1w, p1 ); - v3_sub( p0, centroid, p0 ); - v3_sub( p1, centroid, p1 ); - v3_add( p0, p1, pc ); - v3_muls( pc, 0.5f, pc ); - - /* - * Finding an appropriate quad to collide lines with - */ - v3f region; - v3_div( pc, bbx[1], region ); - - v3f quad[4]; - if( (fabsf(region[0]) > fabsf(region[1])) && - (fabsf(region[0]) > fabsf(region[2])) ) - { - f32 px = vg_signf(region[0]) * bbx[1][0]; - v3_copy( (v3f){ px, bbx[0][1], bbx[0][2] }, quad[0] ); - v3_copy( (v3f){ px, bbx[1][1], bbx[0][2] }, quad[1] ); - v3_copy( (v3f){ px, bbx[1][1], bbx[1][2] }, quad[2] ); - v3_copy( (v3f){ px, bbx[0][1], bbx[1][2] }, quad[3] ); - } - else if( fabsf(region[1]) > fabsf(region[2]) ) - { - f32 py = vg_signf(region[1]) * bbx[1][1]; - v3_copy( (v3f){ bbx[0][0], py, bbx[0][2] }, quad[0] ); - v3_copy( (v3f){ bbx[1][0], py, bbx[0][2] }, quad[1] ); - v3_copy( (v3f){ bbx[1][0], py, bbx[1][2] }, quad[2] ); - v3_copy( (v3f){ bbx[0][0], py, bbx[1][2] }, quad[3] ); - } - else - { - f32 pz = vg_signf(region[2]) * bbx[1][2]; - v3_copy( (v3f){ bbx[0][0], bbx[0][1], pz }, quad[0] ); - v3_copy( (v3f){ bbx[1][0], bbx[0][1], pz }, quad[1] ); - v3_copy( (v3f){ bbx[1][0], bbx[1][1], pz }, quad[2] ); - v3_copy( (v3f){ bbx[0][0], bbx[1][1], pz }, quad[3] ); - } - - capsule_manifold manifold; - rb_capsule_manifold_init( &manifold ); - - v3f c0, c1; - closest_point_aabb( p0, bbx, c0 ); - closest_point_aabb( p1, bbx, c1 ); - - v3f d0, d1, da; - v3_sub( c0, p0, d0 ); - v3_sub( c1, p1, d1 ); - v3_sub( p1, p0, da ); - - /* TODO: ? */ - v3_normalize(d0); - v3_normalize(d1); - v3_normalize(da); - - if( v3_dot( da, d0 ) <= 0.01f ) - rb_capsule_manifold( p0, c0, 0.0f, r, &manifold ); - - if( v3_dot( da, d1 ) >= -0.01f ) - rb_capsule_manifold( p1, c1, 1.0f, r, &manifold ); - - for( i32 i=0; i<4; i++ ){ - i32 i0 = i, - i1 = (i+1)%4; - - v3f ca, cb; - f32 ta, tb; - closest_segment_segment( p0, p1, quad[i0], quad[i1], &ta, &tb, ca, cb ); - rb_capsule_manifold( ca, cb, ta, r, &manifold ); - } - - /* - * Create final contacts based on line manifold - */ - m3x3_mulv( mtxB, manifold.d0, manifold.d0 ); - m3x3_mulv( mtxB, manifold.d1, manifold.d1 ); - return rb_capsule__manifold_done( mtxA, ca, &manifold, buf ); -} - -static int rb_sphere__box( v3f coA, f32 ra, - m4x3f mtxB, m4x3f mtxB_inverse, boxf box, - rb_ct *buf ){ - v3f co, delta; - closest_point_obb( coA, box, mtxB, mtxB_inverse, co ); - v3_sub( coA, co, delta ); - - f32 d2 = v3_length2(delta); - - if( d2 <= ra*ra ){ - f32 d; - - rb_ct *ct = buf; - if( d2 <= 0.0001f ){ - v3f e, coB; - v3_sub( box[1], box[0], e ); - v3_muls( e, 0.5f, e ); - v3_add( box[0], e, coB ); - v3_sub( coA, coB, delta ); - - /* - * some extra testing is required to find the best axis to push the - * object back outside the box. Since there isnt a clear seperating - * vector already, especially on really high aspect boxes. - */ - f32 lx = v3_dot( mtxB[0], delta ), - ly = v3_dot( mtxB[1], delta ), - lz = v3_dot( mtxB[2], delta ), - px = e[0] - fabsf(lx), - py = e[1] - fabsf(ly), - pz = e[2] - fabsf(lz); - - if( px < py && px < pz ) v3_muls( mtxB[0], vg_signf(lx), ct->n ); - else if( py < pz ) v3_muls( mtxB[1], vg_signf(ly), ct->n ); - else v3_muls( mtxB[2], vg_signf(lz), ct->n ); - - v3_muladds( coA, ct->n, -ra, ct->co ); - ct->p = ra; - } - else{ - d = sqrtf(d2); - v3_muls( delta, 1.0f/d, ct->n ); - ct->p = ra-d; - v3_copy( co, ct->co ); - } - - ct->type = k_contact_type_default; - return 1; - } - else return 0; -} - -static int rb_sphere__sphere( v3f coA, f32 ra, - v3f coB, f32 rb, rb_ct *buf ){ - v3f delta; - v3_sub( coA, coB, delta ); - - f32 d2 = v3_length2(delta), - r = ra+rb; - - if( d2 < r*r ){ - f32 d = sqrtf(d2); - - rb_ct *ct = buf; - v3_muls( delta, 1.0f/d, ct->n ); - - v3f p0, p1; - v3_muladds( coA, ct->n,-ra, p0 ); - v3_muladds( coB, ct->n, rb, p1 ); - v3_add( p0, p1, ct->co ); - v3_muls( ct->co, 0.5f, ct->co ); - ct->type = k_contact_type_default; - ct->p = r-d; - return 1; - } - else return 0; -} - -static int rb_sphere__triangle( m4x3f mtxA, f32 r, - v3f tri[3], rb_ct *buf ){ - v3f delta, co; - enum contact_type type = closest_on_triangle_1( mtxA[3], tri, co ); - v3_sub( mtxA[3], co, delta ); - f32 d2 = v3_length2( delta ); - - if( d2 <= r*r ){ - rb_ct *ct = buf; - - v3f ab, ac, tn; - v3_sub( tri[2], tri[0], ab ); - v3_sub( tri[1], tri[0], ac ); - v3_cross( ac, ab, tn ); - 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; - } - - v3_normalize( ct->n ); - - f32 d = sqrtf(d2); - - v3_copy( co, ct->co ); - ct->type = type; - ct->p = r-d; - return 1; - } - - return 0; -} - -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->h*0.5f+c->r, p0w ); - v3_muladds( mtxA[3], mtxA[1], c->h*0.5f-c->r, p1w ); - - capsule_manifold manifold; - rb_capsule_manifold_init( &manifold ); - - v3f v0, v1, n; - 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 ); - -#if 1 - /* deep penetration recovery. for when we clip through the triangles. so its - * not very 'correct' */ - f32 dist; - if( ray_tri( tri, p0w, mtxA[1], &dist, 1 ) ){ - f32 l = c->h - c->r*2.0f; - if( (dist >= 0.0f) && (dist < l) ){ - v3f co; - v3_muladds( p0w, mtxA[1], dist, co ); - vg_line_point( co, 0.02f, 0xffffff00 ); - - v3f d0, d1; - v3_sub( p0w, co, d0 ); - v3_sub( p1w, co, d1 ); - - f32 p = vg_minf( v3_dot( n, d0 ), v3_dot( n, d1 ) ) - c->r; - - rb_ct *ct = buf; - ct->p = -p; - ct->type = k_contact_type_default; - v3_copy( n, ct->n ); - v3_muladds( co, n, p, ct->co ); - - return 1; - } - } -#endif - - v3f c0, c1; - closest_on_triangle_1( p0w, tri, c0 ); - closest_on_triangle_1( p1w, tri, c1 ); - - v3f d0, d1, da; - v3_sub( c0, p0w, d0 ); - v3_sub( c1, p1w, d1 ); - v3_sub( p1w, p0w, da ); - - v3_normalize(d0); - v3_normalize(d1); - v3_normalize(da); - - /* the two balls at the ends */ - if( v3_dot( da, d0 ) <= 0.01f ) - rb_capsule_manifold( p0w, c0, 0.0f, c->r, &manifold ); - if( v3_dot( da, d1 ) >= -0.01f ) - rb_capsule_manifold( p1w, c1, 1.0f, c->r, &manifold ); - - /* the edges to edges */ - for( int i=0; i<3; i++ ){ - int i0 = i, - i1 = (i+1)%3; - - v3f ca, cb; - f32 ta, tb; - closest_segment_segment( p0w, p1w, tri[i0], tri[i1], &ta, &tb, ca, cb ); - rb_capsule_manifold( ca, cb, ta, c->r, &manifold ); - } - - int count = rb_capsule__manifold_done( mtxA, c, &manifold, buf ); - for( int i=0; i vg_list_size(rb_contact_buffer) ) - return 0; - - return 1; -} - -static rb_ct *rb_global_buffer( void ){ - return &rb_contact_buffer[ rb_contact_count ]; -} - -/* - * ----------------------------------------------------------------------------- - * Boolean shape overlap functions - * ----------------------------------------------------------------------------- - */ - -/* - * Project AABB, and triangle interval onto axis to check if they overlap - */ -static int rb_box_triangle_interval( v3f extent, v3f axis, v3f tri[3] ){ - float - - r = extent[0] * fabsf(axis[0]) + - extent[1] * fabsf(axis[1]) + - extent[2] * fabsf(axis[2]), - - p0 = v3_dot( axis, tri[0] ), - p1 = v3_dot( axis, tri[1] ), - p2 = v3_dot( axis, tri[2] ), - - e = vg_maxf(-vg_maxf(p0,vg_maxf(p1,p2)), vg_minf(p0,vg_minf(p1,p2))); - - if( e > r ) return 0; - else return 1; -} - -/* - * Seperating axis test box vs triangle - */ -static int rb_box_triangle_sat( v3f extent, v3f center, - m4x3f to_local, v3f tri_src[3] ){ - v3f tri[3]; - - for( int i=0; i<3; i++ ){ - m4x3_mulv( to_local, tri_src[i], tri[i] ); - 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; - - /* normal */ - v3_cross( f0, f1, n ); - if(!rb_box_triangle_interval( extent, n, tri )) return 0; - - return 1; -} - -/* - * ----------------------------------------------------------------------------- - * Manifold - * ----------------------------------------------------------------------------- - */ - -static int rb_manifold_apply_filtered( rb_ct *man, int len ){ - int k = 0; - - for( int i=0; itype == k_contact_type_disabled ) - continue; - - man[k ++] = man[i]; - } - - return k; -} +extern rb_contact_buffer[VG_MAX_CONTACTS]; +extern int rb_contact_count; + +int rb_capsule__sphere( m4x3f mtxA, rb_capsule *ca, + v3f coB, f32 rb, rb_ct *buf ); +int rb_capsule__capsule( m4x3f mtxA, rb_capsule *ca, + m4x3f mtxB, rb_capsule *cb, rb_ct *buf ); +int rb_capsule__box( m4x3f mtxA, rb_capsule *ca, + m4x3f mtxB, m4x3f mtxB_inverse, boxf box, + rb_ct *buf ); +int rb_sphere__box( v3f coA, f32 ra, + m4x3f mtxB, m4x3f mtxB_inverse, boxf box, + rb_ct *buf ); +int rb_sphere__sphere( v3f coA, f32 ra, v3f coB, f32 rb, rb_ct *buf ); +int rb_sphere__triangle( m4x3f mtxA, f32 r, v3f tri[3], rb_ct *buf ); +int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c, v3f tri[3], rb_ct *buf ); +int rb_global_has_space( void ); +rb_ct *rb_global_buffer( void ); +int rb_manifold_apply_filtered( rb_ct *man, int len ); + +int rb_box_triangle_sat( v3f extent, v3f center, + m4x3f to_local, v3f tri_src[3] ); /* * Merge two contacts if they are within radius(r) of eachother */ -static void rb_manifold_contact_weld( rb_ct *ci, rb_ct *cj, float r ){ - if( v3_dist2( ci->co, cj->co ) < r*r ){ - cj->type = k_contact_type_disabled; - ci->p = (ci->p + cj->p) * 0.5f; - - v3_add( ci->co, cj->co, ci->co ); - v3_muls( ci->co, 0.5f, ci->co ); - - v3f delta; - v3_sub( ci->rba->co, ci->co, delta ); - - float c0 = v3_dot( ci->n, delta ), - c1 = v3_dot( cj->n, delta ); - - if( c0 < 0.0f || c1 < 0.0f ){ - /* error */ - ci->type = k_contact_type_disabled; - } - else{ - v3f n; - v3_muls( ci->n, c0, n ); - v3_muladds( n, cj->n, c1, n ); - v3_normalize( n ); - v3_copy( n, ci->n ); - } - } -} - -/* - * - */ -static void rb_manifold_filter_joint_edges( rb_ct *man, int len, float r ){ - for( int i=0; itype != k_contact_type_edge ) - continue; - - for( int j=i+1; jtype != k_contact_type_edge ) - continue; - - rb_manifold_contact_weld( ci, cj, r ); - } - } -} +void rb_manifold_contact_weld( rb_ct *ci, rb_ct *cj, float r ); +void rb_manifold_filter_joint_edges( rb_ct *man, int len, float r ); /* * Resolve overlapping pairs */ -static void rb_manifold_filter_pairs( rb_ct *man, int len, float r ){ - for( int i=0; itype == k_contact_type_disabled ) continue; - - for( int j=i+1; jtype == k_contact_type_disabled ) continue; - - if( v3_dist2( ci->co, cj->co ) < r*r ){ - cj->type = k_contact_type_disabled; - v3_add( cj->n, ci->n, ci->n ); - ci->p += cj->p; - similar ++; - } - } - - if( similar ){ - float n = 1.0f/((float)similar+1.0f); - v3_muls( ci->n, n, ci->n ); - ci->p *= n; - - if( v3_length2(ci->n) < 0.1f*0.1f ) - ci->type = k_contact_type_disabled; - else - v3_normalize( ci->n ); - } - } -} +void rb_manifold_filter_pairs( rb_ct *man, int len, float r ); /* * Remove contacts that are facing away from A */ -static void rb_manifold_filter_backface( rb_ct *man, int len ){ - for( int i=0; itype == k_contact_type_disabled ) - continue; - - v3f delta; - v3_sub( ct->co, ct->rba->co, delta ); - - if( v3_dot( delta, ct->n ) > -0.001f ) - ct->type = k_contact_type_disabled; - } -} +void rb_manifold_filter_backface( rb_ct *man, int len ); /* * Filter out duplicate coplanar results. Good for spheres. */ -static void rb_manifold_filter_coplanar( rb_ct *man, int len, float w ){ - for( int i=0; itype == k_contact_type_disabled || - ci->type == k_contact_type_edge ) - continue; - - float d1 = v3_dot( ci->co, ci->n ); - - for( int j=0; jtype == k_contact_type_disabled ) - continue; - - float d2 = v3_dot( cj->co, ci->n ), - d = d2-d1; - - if( fabsf( d ) <= w ){ - cj->type = k_contact_type_disabled; - } - } - } -} - -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 ){ - vg_line_point( ct->co, 0.0125f, 0xff0000ff ); - vg_line( ct->co, p1, 0xffffffff ); - } - else if( ct->type == k_contact_type_edge ){ - vg_line_point( ct->co, 0.0125f, 0xff00ffc0 ); - vg_line( ct->co, p1, 0xffffffff ); - } -} - -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 void rb_prepare_contact( rb_ct *ct ){ - ct->bias = -k_phys_baumgarte * (vg.time_fixed_delta*3600.0f) - * vg_minf( 0.0f, -ct->p+k_penetration_slop ); - - v3_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; -} - -/* - * calculate total move to depenetrate object from contacts. - * manifold should belong to ONE object only - */ -static void rb_depenetrate( rb_ct *manifold, int len, v3f dt ){ - v3_zero( dt ); - - for( int j=0; j<7; j++ ){ - for( int i=0; in, dt ), - remaining = (ct->p-k_penetration_slop) - resolved_amt, - apply = vg_maxf( remaining, 0.0f ) * 0.4f; - - v3_muladds( dt, ct->n, apply, dt ); - } - } -} - -/* - * Initializing things like tangent vectors - */ -static void rb_presolve_contacts( rb_ct *buffer, int len ){ - for( int i=0; ico, ct->rba->co, ra ); - v3_sub( ct->co, ct->rbb->co, rb ); - v3_cross( ra, ct->n, raCn ); - v3_cross( rb, ct->n, rbCn ); - - /* orient inverse inertia tensors */ - v3f raCnI, rbCnI; - m3x3_mulv( ct->rba->iIw, raCn, raCnI ); - m3x3_mulv( ct->rbb->iIw, rbCn, rbCnI ); - - ct->normal_mass = ct->rba->inv_mass + ct->rbb->inv_mass; - ct->normal_mass += v3_dot( raCn, raCnI ); - ct->normal_mass += v3_dot( rbCn, rbCnI ); - ct->normal_mass = 1.0f/ct->normal_mass; - - for( int j=0; j<2; j++ ){ - v3f raCtI, rbCtI; - v3_cross( ct->t[j], ra, raCt ); - v3_cross( ct->t[j], rb, rbCt ); - m3x3_mulv( ct->rba->iIw, raCt, raCtI ); - m3x3_mulv( ct->rbb->iIw, rbCt, rbCtI ); - - ct->tangent_mass[j] = ct->rba->inv_mass + ct->rbb->inv_mass; - ct->tangent_mass[j] += v3_dot( raCt, raCtI ); - ct->tangent_mass[j] += v3_dot( rbCt, rbCtI ); - ct->tangent_mass[j] = 1.0f/ct->tangent_mass[j]; - } - } -} - -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 ); - rb_rcv( ct->rba, ct->rbb, ra, rb, rv ); - - float v = v3_dot( rv, ct->n ); - - if( v < -1.0f ){ - ct->bias += -cr * v; - } -} - -/* - * One iteration to solve the contact constraint - */ -static void rb_solve_contacts( rb_ct *buf, int len ){ - for( int i=0; ico, ct->rba->co, ra ); - v3_sub( ct->co, ct->rbb->co, rb ); - rb_rcv( ct->rba, ct->rbb, ra, rb, rv ); - - /* Friction */ - for( int j=0; j<2; j++ ){ - float f = k_friction * ct->norm_impulse, - vt = v3_dot( rv, ct->t[j] ), - lambda = ct->tangent_mass[j] * -vt; - - float temp = ct->tangent_impulse[j]; - ct->tangent_impulse[j] = vg_clampf( temp + lambda, -f, f ); - lambda = ct->tangent_impulse[j] - temp; - - v3f impulse; - v3_muls( ct->t[j], lambda, impulse ); - rb_linear_impulse( ct->rba, ra, impulse ); - - v3_muls( ct->t[j], -lambda, impulse ); - rb_linear_impulse( ct->rbb, rb, impulse ); - } - - /* Normal */ - rb_rcv( ct->rba, ct->rbb, ra, rb, rv ); - float vn = v3_dot( rv, ct->n ), - lambda = ct->normal_mass * (-vn + ct->bias); - - float temp = ct->norm_impulse; - ct->norm_impulse = vg_maxf( temp + lambda, 0.0f ); - lambda = ct->norm_impulse - temp; - - v3f impulse; - v3_muls( ct->n, lambda, impulse ); - rb_linear_impulse( ct->rba, ra, impulse ); - - v3_muls( ct->n, -lambda, impulse ); - rb_linear_impulse( ct->rbb, rb, impulse ); - } -} +void rb_manifold_filter_coplanar( rb_ct *man, int len, float w ); +void rb_debug_contact( rb_ct *ct ); +void rb_solver_reset(void); +rb_ct *rb_global_ct(void); +void rb_prepare_contact( rb_ct *ct, f32 dt ); +void rb_depenetrate( rb_ct *manifold, int len, v3f dt ); +void rb_presolve_contacts( rb_ct *buffer, f32 dt, int len ); +void rb_contact_restitution( rb_ct *ct, float cr ); +void rb_solve_contacts( rb_ct *buf, int len );