X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=rigidbody.h;h=d39cf1b6012926941f63860c8e441927f160bc4b;hb=0124cd309a7db70cdd74b5661f2df8b862ca2f2f;hp=e8dae45227d1045b91a755b49214dc0dac170f27;hpb=c52e639cdcf6d21f69caa9625238afded7513ca4;p=carveJwlIkooP6JGAAIwe30JlM.git

diff --git a/rigidbody.h b/rigidbody.h
index e8dae45..d39cf1b 100644
--- 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