X-Git-Url: https://harrygodden.com/git/?p=vg.git;a=blobdiff_plain;f=vg_rigidbody_collision.c;fp=vg_rigidbody_collision.c;h=f70a39b03c00dd55755ac9b0dadada566ef9c636;hp=0000000000000000000000000000000000000000;hb=3b14f3dcd5bf9dd3c85144f2123d667bfa4bb63f;hpb=fce86711735b15bff37de0f70716808410fcf269

diff --git a/vg_rigidbody_collision.c b/vg_rigidbody_collision.c
new file mode 100644
index 0000000..f70a39b
--- /dev/null
+++ b/vg_rigidbody_collision.c
@@ -0,0 +1,887 @@
+#include "vg_rigidbody.h"
+#include "vg_rigidbody_collision.h"
+#include "vg_m.h"
+#include "vg_lines.h"
+#include "vg_platform.h"
+
+int rb_contact_count = 0;
+struct rb_ct rb_contact_buffer[VG_MAX_CONTACTS];
+
+/*
+ * 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;
+}
+
+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;
+}
+
+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
+ */
+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 );
+}
+
+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;
+}
+
+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;
+}
+
+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;
+}
+
+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;
+}
+
+int rb_global_has_space( void )
+{
+   if( rb_contact_count + 16 > vg_list_size(rb_contact_buffer) )
+      return 0;
+
+   return 1;
+}
+
+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
+ */
+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
+ * -----------------------------------------------------------------------------
+ */
+
+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;
+}
+
+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 );
+      }
+   }
+}
+
+/*
+ * 
+ */
+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 );
+      }
+   }
+}
+
+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 );
+      }
+   }
+}
+
+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;
+   }
+}
+
+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;
+         }
+      }
+   }
+}
+
+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 );
+   }
+}
+
+void rb_solver_reset(void)
+{
+   rb_contact_count = 0;
+}
+
+rb_ct *rb_global_ct(void)
+{
+   return rb_contact_buffer + rb_contact_count;
+}
+
+void rb_prepare_contact( rb_ct *ct, f32 dt )
+{
+   ct->bias = -k_phys_baumgarte * (dt*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 
+ */
+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
+ */
+void rb_presolve_contacts( rb_ct *buffer, f32 dt, int len )
+{
+   for( int i=0; i<len; i++ ){
+      rb_ct *ct = &buffer[i];
+      rb_prepare_contact( ct, dt );
+
+      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];
+      }
+   }
+}
+
+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
+ */
+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 );
+   }
+}