fix

[carveJwlIkooP6JGAAIwe30JlM.git] / rigidbody.h

diff --git a/rigidbody.h b/rigidbody.h
index 5ca3e1e4e4e8e7d7909a1535004619189db8b374..746c684920e98add3572a8c9e1fba7d8590c6f13 100644 (file)
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -214,13 +214,19 @@ VG_STATIC void rb_tangent_basis( v3f n, v3f tx, v3f ty )
 
 VG_STATIC void rb_debug_contact( rb_ct *ct )
 {
-   if( ct->type != k_contact_type_disabled )
+   v3f p1;
+   v3_muladds( ct->co, ct->n, 0.05f, p1 );
+
+   if( ct->type == k_contact_type_default )
    {
-      v3f p1;
-      v3_muladds( ct->co, ct->n, 0.05f, p1 );
       vg_line_pt3( ct->co, 0.0125f, 0xff0000ff );
       vg_line( ct->co, p1, 0xffffffff );
    }
+   else if( ct->type == k_contact_type_edge )
+   {
+      vg_line_pt3( ct->co, 0.0125f, 0xff00ffc0 );
+      vg_line( ct->co, p1, 0xffffffff );
+   }
 }
 
 VG_STATIC void debug_sphere( m4x3f m, float radius, u32 colour )
@@ -2004,6 +2010,26 @@ VG_STATIC void rb_prepare_contact( rb_ct *ct, float timestep )
    ct->tangent_impulse[1] = 0.0f;
 }
 
+/* calculate total move. manifold should belong to ONE object only */
+VG_STATIC void rb_depenetrate( rb_ct *manifold, int len, v3f dt )
+{
+   v3_zero( dt );
+
+   for( int j=0; j<7; j++ )
+   {
+      for( int i=0; i<len; i++ )
+      {
+         struct contact *ct = &manifold[i];
+
+         float resolved_amt = v3_dot( ct->n, dt ),
+               remaining    = (ct->p-k_penetration_slop) - resolved_amt,
+               apply        = vg_maxf( remaining, 0.0f ) * 0.4f;
+
+         v3_muladds( dt, ct->n, apply, dt );
+      }
+   }
+}
+
 /*
  * Initializing things like tangent vectors
  */