f

[carveJwlIkooP6JGAAIwe30JlM.git] / rigidbody.h

diff --git a/rigidbody.h b/rigidbody.h
index 83d32c7eccda75eb841193bd59b9307375589ba8..aa8d4f8bc7a2b526766a6c57b5f1884f1d6623f1 100644 (file)
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -12,6 +12,8 @@
 #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;
 
@@ -446,9 +448,9 @@ VG_STATIC void rb_init( rigidbody *rb )
 
 VG_STATIC void rb_iter( rigidbody *rb )
 {
-   if( isnanf( rb->v[0] ) ||
-       isnanf( rb->v[1] ) ||
-       isnanf( rb->v[2] ) )
+   if( !vg_validf( rb->v[0] ) ||
+       !vg_validf( rb->v[1] ) ||
+       !vg_validf( rb->v[2] ) )
    {
       vg_fatal_exit_loop( "NaN velocity" );
    }
@@ -584,6 +586,44 @@ VG_STATIC int rb_manifold_apply_filtered( rb_ct *man, int len )
    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++ )
@@ -597,41 +637,16 @@ VG_STATIC void rb_manifold_filter_joint_edges( rb_ct *man, int len, float r )
          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 )
 {
@@ -1171,6 +1186,13 @@ VG_STATIC int rb_sphere_triangle( rigidbody *rba, rigidbody *rbb,
       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);