rb_debug( rbb, 0xffffff00 );
rb_ct manifold[64];
- int len = 0;
+ int len_f = 0,
+ len_b = 0;
+
+ len_f = rb_sphere_scene( rbf, &world.rb_geo, manifold );
+ rb_manifold_filter_coplanar( manifold, len_f, 0.05f );
+ rb_manifold_filter_pairs( manifold, len_f, 0.05f );
+ if( len_f > 1 )
+ rb_manifold_filter_backface( manifold, len_f );
+ len_f = rb_manifold_apply_filtered( manifold, len_f );
+
+ rb_ct *man_b = &manifold[len_f];
+ len_b = rb_sphere_scene( rbb, &world.rb_geo, man_b );
+ rb_manifold_filter_coplanar( man_b, len_b, 0.05f );
+ rb_manifold_filter_pairs( man_b, len_b, 0.05f );
+ if( len_b > 1 )
+ rb_manifold_filter_backface( man_b, len_b );
+ len_b = rb_manifold_apply_filtered( man_b, len_b );
- len += rb_sphere_scene( rbf, &world.rb_geo, manifold+len );
- len += rb_sphere_scene( rbb, &world.rb_geo, manifold+len );
+ int len = len_f+len_b;
+#if 0
/*
* Preprocess collision points, and create a surface picture.
* we want contacts that are within our 'capsule's internal line to be
v3_normalize( manifold[i].n );
}
}
+#endif
rb_presolve_contacts( manifold, len );
v3f surface_avg = {0.0f, 0.0f, 0.0f};
normal_mass, tangent_mass[2];
u32 element_id;
- int cluster;
+ int disabled;
}
rb_contact_buffer[256];
VG_STATIC int rb_contact_count = 0;
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 )
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
ct->p = manifold->r0 - d;
ct->rba = rba;
ct->rbb = rbb;
+ ct->disabled = 0;
count ++;
}
ct->p = manifold->r1 - d;
ct->rba = rba;
ct->rbb = rbb;
+ ct->disabled = 0;
count ++;
}
ct->rba = rba;
ct->rbb = rbb;
+ ct->disabled = 0;
return 1;
}
ct->rba = rba;
ct->rbb = rbb;
+ ct->disabled = 0;
return 1;
}
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;
float d = sqrtf(d2);
v3_copy( co, ct->co );
+ ct->disabled = 0;
ct->p = r-d;
ct->rba = rba;
ct->rbb = rbb;
v3_normalize( ct->n );
v3_copy( co, ct->co );
+ ct->disabled = 0;
ct->p = r-d;
ct->rba = rba;
ct->rbb = rbb;
if( ct->p < 0.0f )
continue;
+ ct->disabled = 0;
ct->rba = rba;
ct->rbb = rbb;
count ++;
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;
for( int i=0; i<len; i++ )
{
struct contact *ct = &buf[i];
+
rigidbody *rb = ct->rba;
v3f rv, da, db;