even more collision filtering

[carveJwlIkooP6JGAAIwe30JlM.git] / rigidbody.h

diff --git a/rigidbody.h b/rigidbody.h
index 04d5a9c9830c824d466b7c3fb7cff82364556726..2e66fe68e5fea5800f3fcfc0201abfded3c4adf4 100644 (file)
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -103,7 +103,7 @@ VG_STATIC struct contact
          normal_mass, tangent_mass[2];
 
    u32 element_id;
-   int cluster;
+   int disabled;
 }
 rb_contact_buffer[256];
 VG_STATIC int rb_contact_count = 0;
@@ -148,10 +148,13 @@ VG_STATIC void rb_tangent_basis( v3f n, v3f tx, v3f ty )
 
 VG_STATIC void rb_debug_contact( rb_ct *ct )
 {
-   v3f p1;
-   v3_muladds( ct->co, ct->n, 0.1f, p1 );
-   vg_line_pt3( ct->co, 0.025f, 0xff0000ff );
-   vg_line( ct->co, p1, 0xffffffff );
+   if( !ct->disabled )
+   {
+      v3f p1;
+      v3_muladds( ct->co, ct->n, 0.1f, p1 );
+      vg_line_pt3( ct->co, 0.025f, 0xff0000ff );
+      vg_line( ct->co, p1, 0xffffffff );
+   }
 }
 
 VG_STATIC void debug_sphere( m4x3f m, float radius, u32 colour )
@@ -833,6 +836,114 @@ VG_STATIC int rb_box_triangle_sat( rigidbody *rba, v3f tri_src[3] )
    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->disabled )
+         continue;
+
+      man[k ++] = man[i];
+   }
+
+   return k;
+}
+
+/*
+ * Resolve overlapping pairs
+ */
+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->disabled ) continue;
+
+      for( int j=i+1; j<len; j++ )
+      {
+         rb_ct *cj = &man[j];
+
+         if( cj->disabled ) continue;
+
+         if( v3_dist2( ci->co, cj->co ) < r*r )
+         {
+            cj->disabled = 1;
+            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->disabled = 1;
+         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->disabled ) continue;
+
+      v3f delta;
+      v3_sub( ct->co, ct->rba->co, delta );
+      
+      if( v3_dot( delta, ct->n ) > -0.001f )
+         ct->disabled = 1;
+   }
+}
+
+/*
+ * 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-1; i++ )
+   {
+      rb_ct *ci = &man[i];
+      if( ci->disabled ) continue;
+
+      float d1 = v3_dot( ci->co, ci->n );
+
+      for( int j=i+1; j<len; j++ )
+      {
+         rb_ct *cj = &man[j];
+         if( cj->disabled ) continue;
+         
+         float d2 = v3_dot( cj->co, ci->n ),
+               d  = d2-d1;
+
+         if( fabsf( d ) <= w )
+            cj->disabled = 1;
+      }
+   }
+}
+
 /*
  * -----------------------------------------------------------------------------
  *                            Collision matrix
@@ -927,6 +1038,7 @@ VG_STATIC int rb_capsule_manifold_done( rigidbody *rba, rigidbody *rbb,
       ct->p = manifold->r0 - d;
       ct->rba = rba;
       ct->rbb = rbb;
+      ct->disabled = 0;
 
       count ++;
    }
@@ -946,6 +1058,7 @@ VG_STATIC int rb_capsule_manifold_done( rigidbody *rba, rigidbody *rbb,
       ct->p = manifold->r1 - d;
       ct->rba = rba;
       ct->rbb = rbb;
+      ct->disabled = 0;
 
       count ++;
    }
@@ -993,6 +1106,7 @@ VG_STATIC int rb_capsule_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
 
       ct->rba = rba;
       ct->rbb = rbb;
+      ct->disabled = 0;
 
       return 1;
    }
@@ -1213,6 +1327,7 @@ VG_STATIC int rb_sphere_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
 
       ct->rba = rba;
       ct->rbb = rbb;
+      ct->disabled = 0;
       return 1;
    }
 
@@ -1239,6 +1354,7 @@ VG_STATIC int rb_sphere_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
       v3_muladds( rbb->co, ct->n, rbb->inf.sphere.radius, p1 );
       v3_add( p0, p1, ct->co );
       v3_muls( ct->co, 0.5f, ct->co );
+      ct->disabled = 0;
       ct->p = r-d;
       ct->rba = rba;
       ct->rbb = rbb;
@@ -1283,6 +1399,7 @@ VG_STATIC int rb_sphere_triangle( rigidbody *rba, rigidbody *rbb,
       float d = sqrtf(d2);
 
       v3_copy( co, ct->co );
+      ct->disabled = 0;
       ct->p = r-d;
       ct->rba = rba;
       ct->rbb = rbb;
@@ -1427,12 +1544,13 @@ next_vert:;
       vg_line( tri[1],tri[2],0x10ffffff );
       vg_line( tri[2],tri[0],0x10ffffff );
 
-      int hits = rb_sphere_triangle( rba, rbb, tri, buf+count );
+      int contact = rb_sphere_triangle( rba, rbb, tri, buf+count );
+
 #ifdef RIGIDBODY_DYNAMIC_MESH_EDGES
-      if( hits )
+      if( contact )
          inf->collided = 1;
 #endif
-      count += hits;
+      count += contact;
 
       if( count == 12 )
       {
@@ -1475,12 +1593,13 @@ next_vert:;
             continue;
 
          rb_ct *ct = buf+count;
-            
+
          v3_muls( inf_i->normal, c0, ct->n );
          v3_muladds( ct->n, inf_j->normal, c1, ct->n );
          v3_normalize( ct->n );
 
          v3_copy( co, ct->co );
+         ct->disabled = 0;
          ct->p = r-d;
          ct->rba = rba;
          ct->rbb = rbb;
@@ -1629,6 +1748,7 @@ VG_STATIC int rb_box_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
          if( ct->p < 0.0f )
             continue;
 
+         ct->disabled = 0;
          ct->rba = rba;
          ct->rbb = rbb;
          count ++;
@@ -1730,9 +1850,11 @@ VG_STATIC void rb_presolve_contacts( rb_ct *buffer, int len )
    for( int i=0; i<len; i++ )
    {
       rb_ct *ct = &buffer[i];
+
       ct->bias = -0.2f * k_rb_rate * vg_minf( 0.0f, -ct->p+k_penetration_slop );
       rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
 
+      ct->disabled = 0;
       ct->norm_impulse = 0.0f;
       ct->tangent_impulse[0] = 0.0f;
       ct->tangent_impulse[1] = 0.0f;
@@ -1815,6 +1937,7 @@ VG_STATIC void rb_solve_contacts( rb_ct *buf, int len )
    for( int i=0; i<len; i++ )
    {
       struct contact *ct = &buf[i];
+
       rigidbody *rb = ct->rba;
 
       v3f rv, da, db;