+#endif
+
+ m3x3_mul( rb->iI, rb->to_local, rb->iIw );
+ m3x3_mul( rb->to_world, rb->iIw, rb->iIw );
+
+ rb_update_bounds( rb );
+}
+
+/*
+ * Extrapolate rigidbody into a transform based on vg accumulator.
+ * Useful for rendering
+ */
+VG_STATIC void rb_extrapolate( rigidbody *rb, v3f co, v4f q )
+{
+ float substep = vg_clampf( vg.accumulator / k_rb_delta, 0.0f, 1.0f );
+
+ v3_muladds( rb->co, rb->v, k_rb_delta*substep, co );
+
+ if( v3_length2( rb->w ) > 0.0f ){
+ v4f rotation;
+ v3f axis;
+ v3_copy( rb->w, axis );
+
+ float mag = v3_length( axis );
+ v3_divs( axis, mag, axis );
+ q_axis_angle( rotation, axis, mag*k_rb_delta*substep );
+ q_mul( rotation, rb->q, q );
+ q_normalize( q );
+ }
+ else{
+ v4_copy( rb->q, q );
+ }
+}
+
+/*
+ * Initialize rigidbody and calculate masses, inertia
+ */
+VG_STATIC void rb_init_object( rb_object *obj )
+{
+ float volume = 1.0f;
+ int inert = 0;
+
+ if( obj->type == k_rb_shape_box ){
+ v3f dims;
+ v3_sub( obj->rb.bbx[1], obj->rb.bbx[0], dims );
+ volume = dims[0]*dims[1]*dims[2];
+ }
+ else if( obj->type == k_rb_shape_sphere ){
+ volume = sphere_volume( obj->inf.sphere.radius );
+ v3_fill( obj->rb.bbx[0], -obj->inf.sphere.radius );
+ v3_fill( obj->rb.bbx[1], obj->inf.sphere.radius );
+ }
+ else if( obj->type == k_rb_shape_capsule ){
+ float r = obj->inf.capsule.radius,
+ h = obj->inf.capsule.height;
+ volume = sphere_volume( r ) + VG_PIf * r*r * (h - r*2.0f);
+
+ v3_fill( obj->rb.bbx[0], -r );
+ v3_fill( obj->rb.bbx[1], r );
+ obj->rb.bbx[0][1] = -h;
+ obj->rb.bbx[1][1] = h;
+ }
+ else if( obj->type == k_rb_shape_scene ){
+ inert = 1;
+ box_copy( obj->inf.scene.bh_scene->nodes[0].bbx, obj->rb.bbx );
+ }
+
+ if( inert ){
+ obj->rb.inv_mass = 0.0f;
+ v3_zero( obj->rb.I );
+ m3x3_zero( obj->rb.iI );
+ }
+ else{
+ float mass = 2.0f*volume;
+ obj->rb.inv_mass = 1.0f/mass;
+
+ v3f extent;
+ v3_sub( obj->rb.bbx[1], obj->rb.bbx[0], extent );
+ v3_muls( extent, 0.5f, extent );
+
+ /* local intertia tensor */
+ float scale = k_inertia_scale;
+ float ex2 = scale*extent[0]*extent[0],
+ ey2 = scale*extent[1]*extent[1],
+ ez2 = scale*extent[2]*extent[2];
+
+ obj->rb.I[0] = ((1.0f/12.0f) * mass * (ey2+ez2));
+ obj->rb.I[1] = ((1.0f/12.0f) * mass * (ex2+ez2));
+ obj->rb.I[2] = ((1.0f/12.0f) * mass * (ex2+ey2));
+
+ m3x3_identity( obj->rb.iI );
+ obj->rb.iI[0][0] = obj->rb.I[0];
+ obj->rb.iI[1][1] = obj->rb.I[1];
+ obj->rb.iI[2][2] = obj->rb.I[2];
+ m3x3_inv( obj->rb.iI, obj->rb.iI );
+ }
+
+ rb_update_transform( &obj->rb );
+}
+
+VG_STATIC void rb_iter( rigidbody *rb )
+{
+ if( !vg_validf( rb->v[0] ) ||
+ !vg_validf( rb->v[1] ) ||
+ !vg_validf( rb->v[2] ) )
+ {
+ vg_fatal_exit_loop( "NaN velocity" );
+ }
+
+ v3f gravity = { 0.0f, -9.8f, 0.0f };
+ v3_muladds( rb->v, gravity, k_rb_delta, rb->v );
+
+ /* intergrate velocity */
+ v3_muladds( rb->co, rb->v, k_rb_delta, rb->co );
+ v3_lerp( rb->w, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->w );
+
+ /* inegrate inertia */
+ if( v3_length2( rb->w ) > 0.0f )
+ {
+ v4f rotation;
+ v3f axis;
+ v3_copy( rb->w, axis );
+
+ float mag = v3_length( axis );
+ v3_divs( axis, mag, axis );
+ q_axis_angle( rotation, axis, mag*k_rb_delta );
+ q_mul( rotation, rb->q, rb->q );
+ }
+
+ /* damping */
+ v3_muls( rb->v, 1.0f/(1.0f+k_rb_delta*k_damp_linear), rb->v );
+ v3_muls( rb->w, 1.0f/(1.0f+k_rb_delta*k_damp_angular), rb->w );
+}
+
+
+/*
+ * -----------------------------------------------------------------------------
+ * Boolean shape overlap functions
+ * -----------------------------------------------------------------------------
+ */
+
+/*
+ * Project AABB, and triangle interval onto axis to check if they overlap
+ */
+VG_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
+ */
+VG_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] );
+ }
+
+ /* 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;
+
+ v3f v0,v1,v2,n, e0,e1,e2;
+ v3_sub( tri[1], tri[0], v0 );
+ v3_sub( tri[2], tri[0], v1 );
+ v3_sub( tri[2], tri[1], v2 );
+ v3_normalize( v0 );
+ v3_normalize( v1 );
+ v3_normalize( v2 );
+ v3_cross( v0, v1, n );
+ v3_cross( v0, n, e0 );
+ v3_cross( n, v1, e1 );
+ v3_cross( v2, n, e2 );
+
+ /* normal */
+ if(!rb_box_triangle_interval( extent, n, tri )) return 0;
+
+ v3f axis[9];
+ v3_cross( e0, (v3f){1.0f,0.0f,0.0f}, axis[0] );
+ v3_cross( e0, (v3f){0.0f,1.0f,0.0f}, axis[1] );
+ v3_cross( e0, (v3f){0.0f,0.0f,1.0f}, axis[2] );
+ v3_cross( e1, (v3f){1.0f,0.0f,0.0f}, axis[3] );
+ v3_cross( e1, (v3f){0.0f,1.0f,0.0f}, axis[4] );
+ v3_cross( e1, (v3f){0.0f,0.0f,1.0f}, axis[5] );
+ v3_cross( e2, (v3f){1.0f,0.0f,0.0f}, axis[6] );
+ v3_cross( e2, (v3f){0.0f,1.0f,0.0f}, axis[7] );
+ v3_cross( e2, (v3f){0.0f,0.0f,1.0f}, axis[8] );
+
+ for( int i=0; i<9; i++ )
+ if(!rb_box_triangle_interval( extent, axis[i], tri )) return 0;
+
+ return 1;
+}
+
+/*
+ * -----------------------------------------------------------------------------
+ * Manifold
+ * -----------------------------------------------------------------------------
+ */
+
+VG_STATIC int rb_manifold_apply_filtered( rb_ct *man, int len )
+{
+ int k = 0;
+
+ for( int i=0; i<len; i++ ){
+ rb_ct *ct = &man[i];
+
+ if( ct->type == k_contact_type_disabled )
+ continue;
+
+ man[k ++] = man[i];
+ }
+
+ return k;
+}
+
+/*
+ * Merge two contacts if they are within radius(r) of eachother
+ */
+VG_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 );
+ }
+ }
+}
+
+/*
+ *
+ */
+VG_STATIC void rb_manifold_filter_joint_edges( rb_ct *man, int len, float r )
+{
+ for( int i=0; i<len-1; i++ ){
+ rb_ct *ci = &man[i];
+ if( ci->type != k_contact_type_edge )
+ continue;
+
+ for( int j=i+1; j<len; j++ ){
+ rb_ct *cj = &man[j];
+ if( cj->type != k_contact_type_edge )
+ continue;
+
+ rb_manifold_contact_weld( ci, cj, r );
+ }
+ }
+}
+
+/*
+ * Resolve overlapping pairs
+ *
+ * TODO: Remove?
+ */
+VG_STATIC void rb_manifold_filter_pairs( rb_ct *man, int len, float r )
+{
+ for( int i=0; i<len-1; i++ ){
+ rb_ct *ci = &man[i];
+ int similar = 0;
+
+ if( ci->type == k_contact_type_disabled ) continue;
+
+ for( int j=i+1; j<len; j++ ){
+ rb_ct *cj = &man[j];
+
+ if( cj->type == 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 );
+ }
+ }
+}
+
+/*
+ * Remove contacts that are facing away from A
+ */
+VG_STATIC void rb_manifold_filter_backface( rb_ct *man, int len )
+{
+ for( int i=0; i<len; i++ ){
+ rb_ct *ct = &man[i];
+ if( ct->type == 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;
+ }
+}
+
+/*
+ * Filter out duplicate coplanar results. Good for spheres.
+ */
+VG_STATIC void rb_manifold_filter_coplanar( rb_ct *man, int len, float w )
+{
+ for( int i=0; i<len; i++ ){
+ rb_ct *ci = &man[i];
+ if( ci->type == k_contact_type_disabled ||
+ ci->type == k_contact_type_edge )
+ continue;
+
+ float d1 = v3_dot( ci->co, ci->n );
+
+ for( int j=0; j<len; j++ ){
+ if( j == i )
+ continue;
+
+ rb_ct *cj = &man[j];
+ if( cj->type == 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;
+ }
+ }
+ }
+}
+
+/*
+ * -----------------------------------------------------------------------------
+ * Collision matrix
+ * -----------------------------------------------------------------------------
+ */
+
+/*
+ * 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
+{
+ float t0, t1;
+ float 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.
+ */
+VG_STATIC void rb_capsule_manifold( v3f pa, v3f pb, float t, float 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;
+ }
+ }