[vg.git] / vg_rigidbody_collision.h

#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;
   v3f co, n;
   v3f t[2];
   float p, bias, norm_impulse, tangent_impulse[2],
         normal_mass, tangent_mass[2];

   u32 element_id;

   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<count; i++ )
      v3_copy( n, buf[i].n );

   return count;
}

static int rb_global_has_space( void ){
   if( rb_contact_count + 16 > 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; 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
 */
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; 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
 */
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
 */
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.
 */
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;
         }
      }
   }
}

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; i<len; i++ ){
         rb_ct *ct = &manifold[i];

         float resolved_amt = v3_dot( ct->n, 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; i<len; i++ ){
      rb_ct *ct = &buffer[i];
      rb_prepare_contact( ct );

      v3f ra, rb, raCn, rbCn, raCt, rbCt;
      v3_sub( ct->co, 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; i<len; i++ ){
      rb_ct *ct = &buf[i];

      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 );
      
      /* 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 );
   }
}