#include "scene.h"
#include "distq.h"
+#include <math.h>
+
VG_STATIC void rb_tangent_basis( v3f n, v3f tx, v3f ty );
VG_STATIC bh_system bh_system_rigidbodies;
VG_STATIC void rb_iter( rigidbody *rb )
{
+ if( !vg_validf( rb->v[0] ) ||
+ !vg_validf( rb->v[1] ) ||
+ !vg_validf( rb->v[2] ) )
+ {
+ vg_fatal_exit_loop( "NaN velocity" );
+ }
+
v3f gravity = { 0.0f, -9.8f, 0.0f };
v3_muladds( rb->v, gravity, k_rb_delta, rb->v );
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);