latest

[carveJwlIkooP6JGAAIwe30JlM.git] / rigidbody.h

diff --git a/rigidbody.h b/rigidbody.h
index e8dae45227d1045b91a755b49214dc0dac170f27..d39cf1b6012926941f63860c8e441927f160bc4b 100644 (file)
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -2350,6 +2350,21 @@ VG_STATIC void rb_correct_swingtwist_constraints( rb_constr_swingtwist *buf,
    }
 }
 
+VG_STATIC void rb_correct_contact_constraints( rb_ct *buf, int len, float amt )
+{
+   for( int i=0; i<len; i++ )
+   {
+      rb_ct *ct = &buf[i];
+      rigidbody *rba = ct->rba,
+                *rbb = ct->rbb;
+
+      float mass_total = 1.0f / (rba->inv_mass + rbb->inv_mass);
+
+      v3_muladds( rba->co, ct->n, -mass_total * rba->inv_mass, rba->co );
+      v3_muladds( rbb->co, ct->n,  mass_total * rbb->inv_mass, rbb->co );
+   }
+}
+
 
 /* 
  * Effectors