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 *cj = &man[j];
if( cj->type != k_contact_type_edge )
continue;
-
- 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 );
- }
- }
+
+ 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 )
{
v3_sub( tri[1], tri[0], ac );
v3_cross( ac, ab, tn );
v3_copy( tn, ct->n );
+
+ if( v3_length2( ct->n ) <= 0.00001f )
+ {
+ vg_error( "Zero area triangle!\n" );
+ return 0;
+ }
+
v3_normalize( ct->n );
float d = sqrtf(d2);