#pragma once
#include "vg_rigidbody.h"
+#ifndef VG_MAX_CONTACTS
+#define VG_MAX_CONTACTS 256
+#endif
+
typedef struct rb_ct rb_ct;
static struct rb_ct{
rigidbody *rba, *rbb;
enum contact_type type;
}
-rb_contact_buffer[256];
+rb_contact_buffer[VG_MAX_CONTACTS];
static int rb_contact_count = 0;
/*
* Debugging
*/
+#if 0
if( count == 2 )
vg_line( buf[0].co, buf[1].co, 0xff0000ff );
+#endif
return count;
}
-#if 0
-static int rb_capsule_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){
- rigidbody *rba = &obja->rb, *rbb = &objb->rb;
- f32 h = obja->inf.capsule.h,
- ra = obja->inf.capsule.r,
- rb = objb->inf.sphere.r;
+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( rba->co, rba->to_world[1], -h*0.5f+ra, p0 );
- v3_muladds( rba->co, rba->to_world[1], h*0.5f-ra, 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, rbb->co, c );
- v3_sub( c, rbb->co, delta );
-
- f32 d2 = v3_length2(delta),
- r = ra + rb;
+ 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);
v3f p0, p1;
v3_muladds( c, ct->n, -ra, p0 );
- v3_muladds( rbb->co, ct->n, rb, p1 );
+ v3_muladds( coB, ct->n, rb, p1 );
v3_add( p0, p1, ct->co );
v3_muls( ct->co, 0.5f, ct->co );
-
- ct->rba = rba;
- ct->rbb = rbb;
ct->type = k_contact_type_default;
-
return 1;
}
-
- return 0;
+ else return 0;
}
-#endif
static int rb_capsule__capsule( m4x3f mtxA, rb_capsule *ca,
m4x3f mtxB, rb_capsule *cb, rb_ct *buf ){
return rb_capsule__manifold_done( mtxA, ca, &manifold, buf );
}
-#if 0
-static int rb_sphere_box( rb_object *obja, rb_object *objb, rb_ct *buf ){
- v3f co, delta;
- rigidbody *rba = &obja->rb, *rbb = &objb->rb;
+/*
+ * 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;
- closest_point_obb( rba->co, rbb->bbx, rbb->to_world, rbb->to_local, co );
- v3_sub( rba->co, co, delta );
+ /*
+ * 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 );
- f32 d2 = v3_length2(delta),
- r = obja->inf.sphere.radius;
+ 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] );
+ }
- if( d2 <= r*r ){
+ 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 ){
- v3_sub( rba->co, rbb->co, delta );
+ 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( rbb->to_world[0], delta ),
- ly = v3_dot( rbb->to_world[1], delta ),
- lz = v3_dot( rbb->to_world[2], delta ),
- px = rbb->bbx[1][0] - fabsf(lx),
- py = rbb->bbx[1][1] - fabsf(ly),
- pz = rbb->bbx[1][2] - fabsf(lz);
-
- if( px < py && px < pz )
- v3_muls( rbb->to_world[0], vg_signf(lx), ct->n );
- else if( py < pz )
- v3_muls( rbb->to_world[1], vg_signf(ly), ct->n );
- else
- v3_muls( rbb->to_world[2], vg_signf(lz), ct->n );
-
- v3_muladds( rba->co, ct->n, -r, ct->co );
- ct->p = r;
+ 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 = r-d;
+ ct->p = ra-d;
v3_copy( co, ct->co );
}
- ct->rba = rba;
- ct->rbb = rbb;
ct->type = k_contact_type_default;
return 1;
}
-
- return 0;
+ else return 0;
}
-#endif
-#if 0
-static int rb_sphere_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){
- rigidbody *rba = &obja->rb, *rbb = &objb->rb;
+static int rb_sphere__sphere( v3f coA, f32 ra,
+ v3f coB, f32 rb, rb_ct *buf ){
v3f delta;
- v3_sub( rba->co, rbb->co, delta );
+ v3_sub( coA, coB, delta );
f32 d2 = v3_length2(delta),
- r = obja->inf.sphere.radius + objb->inf.sphere.radius;
+ r = ra+rb;
if( d2 < r*r ){
f32 d = sqrtf(d2);
v3_muls( delta, 1.0f/d, ct->n );
v3f p0, p1;
- v3_muladds( rba->co, ct->n,-obja->inf.sphere.radius, p0 );
- v3_muladds( rbb->co, ct->n, objb->inf.sphere.radius, 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;
- ct->rba = rba;
- ct->rbb = rbb;
return 1;
}
-
- return 0;
+ else return 0;
}
-#endif
static int rb_sphere__triangle( m4x3f mtxA, f32 r,
v3f tri[3], rb_ct *buf ){
return rb_contact_buffer + rb_contact_count;
}
-static void rb_prepare_contact( rb_ct *ct, float timestep ){
- ct->bias = -k_phys_baumgarte * (timestep*3600.0f)
+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] );
static void rb_presolve_contacts( rb_ct *buffer, int len ){
for( int i=0; i<len; i++ ){
rb_ct *ct = &buffer[i];
- rb_prepare_contact( ct, k_rb_delta );
+ rb_prepare_contact( ct );
v3f ra, rb, raCn, rbCn, raCt, rbCt;
v3_sub( ct->co, ct->rba->co, ra );
ct->tangent_mass[j] += v3_dot( rbCt, rbCtI );
ct->tangent_mass[j] = 1.0f/ct->tangent_mass[j];
}
-
- rb_debug_contact( ct );
}
}
projects / vg.git / blobdiff