chipmunk 5.3.4.5 → 6.1.3.0.rc1

data/ext/chipmunk/constraints/cpRatchetJoint.h

@@ -19,8 +19,12 @@
  * SOFTWARE.
  */
 
-const cpConstraintClass *cpRatchetJointGetClass();
+/// @defgroup cpRatchetJoint cpRatchetJoint
+/// @{
 
+const cpConstraintClass *cpRatchetJointGetClass(void);
+
+/// @private
 typedef struct cpRatchetJoint {
 	cpConstraint constraint;
 	cpFloat angle, phase, ratchet;
@@ -28,13 +32,18 @@ typedef struct cpRatchetJoint {
 	cpFloat iSum;
 		
 	cpFloat bias;
-	cpFloat jAcc, jMax;
+	cpFloat jAcc;
 } cpRatchetJoint;
 
-cpRatchetJoint *cpRatchetJointAlloc(void);
-cpRatchetJoint *cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet);
-cpConstraint *cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet);
+/// Allocate a ratchet joint.
+cpRatchetJoint* cpRatchetJointAlloc(void);
+/// Initialize a ratched joint.
+cpRatchetJoint* cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet);
+/// Allocate and initialize a ratchet joint.
+cpConstraint* cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet);
+
+CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, angle, Angle)
+CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, phase, Phase)
+CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, ratchet, Ratchet)
 
-CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, angle, Angle);
-CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, phase, Phase);
-CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, ratchet, Ratchet);
+/// @}

data/ext/chipmunk/constraints/cpRotaryLimitJoint.h

@@ -19,8 +19,12 @@
  * SOFTWARE.
  */
 
-const cpConstraintClass *cpRotaryLimitJointGetClass();
+/// @defgroup cpRotaryLimitJoint cpRotaryLimitJoint
+/// @{
 
+const cpConstraintClass *cpRotaryLimitJointGetClass(void);
+
+/// @private
 typedef struct cpRotaryLimitJoint {
 	cpConstraint constraint;
 	cpFloat min, max;
@@ -28,12 +32,17 @@ typedef struct cpRotaryLimitJoint {
 	cpFloat iSum;
 		
 	cpFloat bias;
-	cpFloat jAcc, jMax;
+	cpFloat jAcc;
 } cpRotaryLimitJoint;
 
-cpRotaryLimitJoint *cpRotaryLimitJointAlloc(void);
-cpRotaryLimitJoint *cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max);
-cpConstraint *cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max);
+/// Allocate a damped rotary limit joint.
+cpRotaryLimitJoint* cpRotaryLimitJointAlloc(void);
+/// Initialize a damped rotary limit joint.
+cpRotaryLimitJoint* cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max);
+/// Allocate and initialize a damped rotary limit joint.
+cpConstraint* cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max);
+
+CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, min, Min)
+CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, max, Max)
 
-CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, min, Min);
-CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, max, Max);
+/// @}

data/ext/chipmunk/{cpSimpleMotor.h → constraints/cpSimpleMotor.h}

@@ -19,19 +19,28 @@
  * SOFTWARE.
  */
 
-const cpConstraintClass *cpSimpleMotorGetClass();
+/// @defgroup cpSimpleMotor cpSimpleMotor
+/// @{
 
+const cpConstraintClass *cpSimpleMotorGetClass(void);
+
+/// @private
 typedef struct cpSimpleMotor {
 	cpConstraint constraint;
 	cpFloat rate;
 	
 	cpFloat iSum;
 		
-	cpFloat jAcc, jMax;
+	cpFloat jAcc;
 } cpSimpleMotor;
 
-cpSimpleMotor *cpSimpleMotorAlloc(void);
-cpSimpleMotor *cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate);
-cpConstraint *cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate);
+/// Allocate a simple motor.
+cpSimpleMotor* cpSimpleMotorAlloc(void);
+/// initialize a simple motor.
+cpSimpleMotor* cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate);
+/// Allocate and initialize a simple motor.
+cpConstraint* cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate);
+
+CP_DefineConstraintProperty(cpSimpleMotor, cpFloat, rate, Rate)
 
-CP_DefineConstraintProperty(cpSimpleMotor, cpFloat, rate, Rate);
+/// @}

data/ext/chipmunk/constraints/cpSlideJoint.h

@@ -19,8 +19,12 @@
  * SOFTWARE.
  */
 
-const cpConstraintClass *cpSlideJointGetClass();
+/// @defgroup cpSlideJoint cpSlideJoint
+/// @{
 
+const cpConstraintClass *cpSlideJointGetClass(void);
+
+/// @private
 typedef struct cpSlideJoint {
 	cpConstraint constraint;
 	cpVect anchr1, anchr2;
@@ -30,15 +34,20 @@ typedef struct cpSlideJoint {
 	cpVect n;
 	cpFloat nMass;
 	
-	cpFloat jnAcc, jnMax;
+	cpFloat jnAcc;
 	cpFloat bias;
 } cpSlideJoint;
 
-cpSlideJoint *cpSlideJointAlloc(void);
-cpSlideJoint *cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max);
-cpConstraint *cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max);
+/// Allocate a slide joint.
+cpSlideJoint* cpSlideJointAlloc(void);
+/// Initialize a slide joint.
+cpSlideJoint* cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max);
+/// Allocate and initialize a slide joint.
+cpConstraint* cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max);
+
+CP_DefineConstraintProperty(cpSlideJoint, cpVect, anchr1, Anchr1)
+CP_DefineConstraintProperty(cpSlideJoint, cpVect, anchr2, Anchr2)
+CP_DefineConstraintProperty(cpSlideJoint, cpFloat, min, Min)
+CP_DefineConstraintProperty(cpSlideJoint, cpFloat, max, Max)
 
-CP_DefineConstraintProperty(cpSlideJoint, cpVect, anchr1, Anchr1);
-CP_DefineConstraintProperty(cpSlideJoint, cpVect, anchr2, Anchr2);
-CP_DefineConstraintProperty(cpSlideJoint, cpFloat, min, Min);
-CP_DefineConstraintProperty(cpSlideJoint, cpFloat, max, Max);
+/// @}

data/ext/chipmunk/constraints/util.h

@@ -19,22 +19,13 @@
  * SOFTWARE.
  */
 
-#define CP_DefineClassGetter(t) const cpConstraintClass * t##GetClass(){return (cpConstraintClass *)&klass;}
+// These are utility routines to use when creating custom constraints.
+// I'm not sure if this should be part of the private API or not.
+// I should probably clean up the naming conventions if it is...
 
-void cpConstraintInit(cpConstraint *constraint, const cpConstraintClass *klass, cpBody *a, cpBody *b);
-
-#define J_MAX(constraint, dt) (((cpConstraint *)constraint)->maxForce*(dt))
+#define CP_DefineClassGetter(t) const cpConstraintClass * t##GetClass(void){return (cpConstraintClass *)&klass;}
 
-// Get valid body pointers and exit early if the bodies are idle
-#define CONSTRAINT_BEGIN(constraint, a_var, b_var) \
-cpBody *a_var, *b_var; { \
-	a_var = ((cpConstraint *)constraint)->a; \
-	b_var = ((cpConstraint *)constraint)->b; \
-	if( \
-		(cpBodyIsSleeping(a_var) || cpBodyIsStatic(a_var)) && \
-		(cpBodyIsSleeping(b_var) || cpBodyIsStatic(b_var)) \
-	) return; \
-}
+void cpConstraintInit(cpConstraint *constraint, const cpConstraintClass *klass, cpBody *a, cpBody *b);
 
 static inline cpVect
 relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2){
@@ -49,18 +40,24 @@ normal_relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n){
 	return cpvdot(relative_velocity(a, b, r1, r2), n);
 }
 
+static inline void
+apply_impulse(cpBody *body, cpVect j, cpVect r){
+	body->v = cpvadd(body->v, cpvmult(j, body->m_inv));
+	body->w += body->i_inv*cpvcross(r, j);
+}
+
 static inline void
 apply_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j)
 {
-	cpBodyApplyImpulse(a, cpvneg(j), r1);
-	cpBodyApplyImpulse(b, j, r2);
+	apply_impulse(a, cpvneg(j), r1);
+	apply_impulse(b, j, r2);
 }
 
 static inline void
 apply_bias_impulse(cpBody *body, cpVect j, cpVect r)
 {
-	body->v_bias = cpvadd(body->v_bias, cpvmult(j, body->m_inv));
-	body->w_bias += body->i_inv*cpvcross(r, j);
+	body->CP_PRIVATE(v_bias) = cpvadd(body->CP_PRIVATE(v_bias), cpvmult(j, body->m_inv));
+	body->CP_PRIVATE(w_bias) += body->i_inv*cpvcross(r, j);
 }
 
 static inline void
@@ -70,37 +67,30 @@ apply_bias_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j)
 	apply_bias_impulse(b, j, r2);
 }
 
-static inline cpVect
-clamp_vect(cpVect v, cpFloat len)
+static inline cpFloat
+k_scalar_body(cpBody *body, cpVect r, cpVect n)
 {
-	return cpvclamp(v, len);
-//	return (cpvdot(v,v) > len*len) ? cpvmult(cpvnormalize(v), len) : v;
+	cpFloat rcn = cpvcross(r, n);
+	return body->m_inv + body->i_inv*rcn*rcn;
 }
 
 static inline cpFloat
 k_scalar(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n)
 {
-	cpFloat mass_sum = a->m_inv + b->m_inv;
-	cpFloat r1cn = cpvcross(r1, n);
-	cpFloat r2cn = cpvcross(r2, n);
-	
-	cpFloat value = mass_sum + a->i_inv*r1cn*r1cn + b->i_inv*r2cn*r2cn;
-	cpAssert(value != 0.0, "Unsolvable collision or constraint.");
+	cpFloat value = k_scalar_body(a, r1, n) + k_scalar_body(b, r2, n);
+	cpAssertSoft(value != 0.0, "Unsolvable collision or constraint.");
 	
 	return value;
 }
 
-static inline void
-k_tensor(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect *k1, cpVect *k2)
+static inline cpMat2x2
+k_tensor(cpBody *a, cpBody *b, cpVect r1, cpVect r2)
 {
-	// calculate mass matrix
-	// If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross...
-	cpFloat k11, k12, k21, k22;
 	cpFloat m_sum = a->m_inv + b->m_inv;
 	
-	// start with I*m_sum
-	k11 = m_sum; k12 = 0.0f;
-	k21 = 0.0f;  k22 = m_sum;
+	// start with Identity*m_sum
+	cpFloat k11 = m_sum, k12 = 0.0f;
+	cpFloat k21 = 0.0f,  k22 = m_sum;
 	
 	// add the influence from r1
 	cpFloat a_i_inv = a->i_inv;
@@ -119,16 +109,18 @@ k_tensor(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect *k1, cpVect *k2)
 	k21 += r2nxy; k22 += r2xsq;
 	
 	// invert
-	cpFloat determinant = k11*k22 - k12*k21;
-	cpAssert(determinant != 0.0, "Unsolvable constraint.");
+	cpFloat det = k11*k22 - k12*k21;
+	cpAssertSoft(det != 0.0, "Unsolvable constraint.");
 	
-	cpFloat det_inv = 1.0f/determinant;
-	*k1 = cpv( k22*det_inv, -k12*det_inv);
-	*k2 = cpv(-k21*det_inv,  k11*det_inv);
+	cpFloat det_inv = 1.0f/det;
+	return cpMat2x2New(
+		 k22*det_inv, -k12*det_inv,
+		-k21*det_inv,  k11*det_inv
+ 	);
 }
 
-static inline cpVect
-mult_k(cpVect vr, cpVect k1, cpVect k2)
+static inline cpFloat
+bias_coef(cpFloat errorBias, cpFloat dt)
 {
-	return cpv(cpvdot(vr, k1), cpvdot(vr, k2));
+	return 1.0f - cpfpow(errorBias, dt);
 }

data/ext/chipmunk/cpArbiter.c

@@ -18,15 +18,10 @@
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  * SOFTWARE.
  */
- 
-#include <stdlib.h>
 
 #include "chipmunk_private.h"
 #include "constraints/util.h"
 
-cpFloat cp_bias_coef = 0.1f;
-cpFloat cp_collision_slop = 0.1f;
-
 cpContact*
 cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash)
 {
@@ -43,62 +38,157 @@ cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash
 	return con;
 }
 
+// TODO make this generic so I can reuse it for constraints also.
+static inline void
+unthreadHelper(cpArbiter *arb, cpBody *body)
+{
+	struct cpArbiterThread *thread = cpArbiterThreadForBody(arb, body);
+	cpArbiter *prev = thread->prev;
+	cpArbiter *next = thread->next;
+	
+	if(prev){
+		cpArbiterThreadForBody(prev, body)->next = next;
+	} else if(body->arbiterList == arb) {
+		// IFF prev is NULL and body->arbiterList == arb, is arb at the head of the list.
+		// This function may be called for an arbiter that was never in a list.
+		// In that case, we need to protect it from wiping out the body->arbiterList pointer.
+		body->arbiterList = next;
+	}
+	
+	if(next) cpArbiterThreadForBody(next, body)->prev = prev;
+	
+	thread->prev = NULL;
+	thread->next = NULL;
+}
+
+void
+cpArbiterUnthread(cpArbiter *arb)
+{
+	unthreadHelper(arb, arb->body_a);
+	unthreadHelper(arb, arb->body_b);
+}
+
+cpBool cpArbiterIsFirstContact(const cpArbiter *arb)
+{
+	return arb->CP_PRIVATE(state) == cpArbiterStateFirstColl;
+}
+
+int cpArbiterGetCount(const cpArbiter *arb)
+{
+	// Return 0 contacts if we are in a separate callback.
+	return (arb->CP_PRIVATE(state) != cpArbiterStateCached ? arb->CP_PRIVATE(numContacts) : 0);
+}
+
+cpVect
+cpArbiterGetNormal(const cpArbiter *arb, int i)
+{
+	cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter");
+	
+	cpVect n = arb->contacts[i].n;
+	return arb->swappedColl ? cpvneg(n) : n;
+}
+
+cpVect
+cpArbiterGetPoint(const cpArbiter *arb, int i)
+{
+	cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter");
+	
+	return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(p);
+}
+
+cpFloat
+cpArbiterGetDepth(const cpArbiter *arb, int i)
+{
+	cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter");
+	
+	return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(dist);
+}
+
+cpContactPointSet
+cpArbiterGetContactPointSet(const cpArbiter *arb)
+{
+	cpContactPointSet set;
+	set.count = cpArbiterGetCount(arb);
+	
+	int i;
+	for(i=0; i<set.count; i++){
+		set.points[i].point = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(p);
+		set.points[i].normal = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(n);
+		set.points[i].dist = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(dist);
+	}
+	
+	return set;
+}
+
+
 cpVect
-cpArbiterTotalImpulse(cpArbiter *arb)
+cpArbiterTotalImpulse(const cpArbiter *arb)
 {
 	cpContact *contacts = arb->contacts;
 	cpVect sum = cpvzero;
 	
-	for(int i=0, count=arb->numContacts; i<count; i++){
+	for(int i=0, count=cpArbiterGetCount(arb); i<count; i++){
 		cpContact *con = &contacts[i];
 		sum = cpvadd(sum, cpvmult(con->n, con->jnAcc));
 	}
-		
-	return sum;
+	
+	return (arb->swappedColl ? sum : cpvneg(sum));
 }
 
 cpVect
-cpArbiterTotalImpulseWithFriction(cpArbiter *arb)
+cpArbiterTotalImpulseWithFriction(const cpArbiter *arb)
 {
 	cpContact *contacts = arb->contacts;
 	cpVect sum = cpvzero;
 	
-	for(int i=0, count=arb->numContacts; i<count; i++){
+	for(int i=0, count=cpArbiterGetCount(arb); i<count; i++){
 		cpContact *con = &contacts[i];
 		sum = cpvadd(sum, cpvrotate(con->n, cpv(con->jnAcc, con->jtAcc)));
 	}
 		
-	return sum;
+	return (arb->swappedColl ? sum : cpvneg(sum));
 }
 
 cpFloat
-cpContactsEstimateCrushingImpulse(cpContact *contacts, int numContacts)
+cpArbiterTotalKE(const cpArbiter *arb)
 {
-	cpFloat fsum = 0.0f;
-	cpVect vsum = cpvzero;
+	cpFloat eCoef = (1 - arb->e)/(1 + arb->e);
+	cpFloat sum = 0.0;
 	
-	for(int i=0; i<numContacts; i++){
+	cpContact *contacts = arb->contacts;
+	for(int i=0, count=cpArbiterGetCount(arb); i<count; i++){
 		cpContact *con = &contacts[i];
-		cpVect j = cpvrotate(con->n, cpv(con->jnAcc, con->jtAcc));
+		cpFloat jnAcc = con->jnAcc;
+		cpFloat jtAcc = con->jtAcc;
 		
-		fsum += cpvlength(j);
-		vsum = cpvadd(vsum, j);
+		sum += eCoef*jnAcc*jnAcc/con->nMass + jtAcc*jtAcc/con->tMass;
 	}
 	
-	cpFloat vmag = cpvlength(vsum);
-	return (1.0f - vmag/fsum);
+	return sum;
 }
 
+//cpFloat
+//cpContactsEstimateCrushingImpulse(cpContact *contacts, int numContacts)
+//{
+//	cpFloat fsum = 0.0f;
+//	cpVect vsum = cpvzero;
+//	
+//	for(int i=0; i<numContacts; i++){
+//		cpContact *con = &contacts[i];
+//		cpVect j = cpvrotate(con->n, cpv(con->jnAcc, con->jtAcc));
+//		
+//		fsum += cpvlength(j);
+//		vsum = cpvadd(vsum, j);
+//	}
+//	
+//	cpFloat vmag = cpvlength(vsum);
+//	return (1.0f - vmag/fsum);
+//}
+
 void
 cpArbiterIgnore(cpArbiter *arb)
 {
-	arb->state = cpArbiterStateIgnore;
-}
-
-cpArbiter*
-cpArbiterAlloc(void)
-{
-	return (cpArbiter *)cpcalloc(1, sizeof(cpArbiter));
+	arb->state = cpArbiterStateIgnore;
 }
 
 cpArbiter*
@@ -114,41 +204,27 @@ cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b)
 	arb->numContacts = 0;
 	arb->contacts = NULL;
 	
-	arb->a = a;
-	arb->b = b;
+	arb->a = a; arb->body_a = a->body;
+	arb->b = b; arb->body_b = b->body;
+	
+	arb->thread_a.next = NULL;
+	arb->thread_b.next = NULL;
+	arb->thread_a.prev = NULL;
+	arb->thread_b.prev = NULL;
 	
 	arb->stamp = 0;
 	arb->state = cpArbiterStateFirstColl;
 	
+	arb->data = NULL;
+	
 	return arb;
 }
 
-cpArbiter*
-cpArbiterNew(cpShape *a, cpShape *b)
-{
-	return cpArbiterInit(cpArbiterAlloc(), a, b);
-}
-
-void
-cpArbiterDestroy(cpArbiter *arb)
-{
-//	if(arb->contacts) cpfree(arb->contacts);
-}
-
-void
-cpArbiterFree(cpArbiter *arb)
-{
-	if(arb){
-		cpArbiterDestroy(arb);
-		cpfree(arb);
-	}
-}
-
 void
 cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, cpCollisionHandler *handler, cpShape *a, cpShape *b)
 {
 	// Arbiters without contact data may exist if a collision function rejected the collision.
-	if(arb->contacts){
+	if(arb->numContacts > 0){
 		// Iterate over the possible pairs to look for hash value matches.
 		for(int i=0; i<arb->numContacts; i++){
 			cpContact *old = &arb->contacts[i];
@@ -177,18 +253,18 @@ cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, cpCollisio
 	arb->surface_vr = cpvsub(a->surface_v, b->surface_v);
 	
 	// For collisions between two similar primitive types, the order could have been swapped.
-	arb->a = a;
-	arb->b = b;
+	arb->a = a; arb->body_a = a->body;
+	arb->b = b; arb->body_b = b->body;
 	
 	// mark it as new if it's been cached
 	if(arb->state == cpArbiterStateCached) arb->state = cpArbiterStateFirstColl;
 }
 
 void
-cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv)
+cpArbiterPreStep(cpArbiter *arb, cpFloat dt, cpFloat slop, cpFloat bias)
 {
-	cpBody *a = arb->a->body;
-	cpBody *b = arb->b->body;
+	cpBody *a = arb->body_a;
+	cpBody *b = arb->body_b;
 	
 	for(int i=0; i<arb->numContacts; i++){
 		cpContact *con = &arb->contacts[i];
@@ -202,79 +278,68 @@ cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv)
 		con->tMass = 1.0f/k_scalar(a, b, con->r1, con->r2, cpvperp(con->n));
 				
 		// Calculate the target bias velocity.
-		con->bias = -cp_bias_coef*dt_inv*cpfmin(0.0f, con->dist + cp_collision_slop);
+		con->bias = -bias*cpfmin(0.0f, con->dist + slop)/dt;
 		con->jBias = 0.0f;
 		
 		// Calculate the target bounce velocity.
-		con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, con->n)*arb->e;//cpvdot(con->n, cpvsub(v2, v1))*e;
+		con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, con->n)*arb->e;
 	}
 }
 
 void
-cpArbiterApplyCachedImpulse(cpArbiter *arb)
+cpArbiterApplyCachedImpulse(cpArbiter *arb, cpFloat dt_coef)
 {
-	cpShape *shapea = arb->a;
-	cpShape *shapeb = arb->b;
-		
-	arb->u = shapea->u * shapeb->u;
-	arb->surface_vr = cpvsub(shapeb->surface_v, shapea->surface_v);
-
-	cpBody *a = shapea->body;
-	cpBody *b = shapeb->body;
+	if(cpArbiterIsFirstContact(arb)) return;
+	
+	cpBody *a = arb->body_a;
+	cpBody *b = arb->body_b;
 	
 	for(int i=0; i<arb->numContacts; i++){
 		cpContact *con = &arb->contacts[i];
-		apply_impulses(a, b, con->r1, con->r2, cpvrotate(con->n, cpv(con->jnAcc, con->jtAcc)));
+		cpVect j = cpvrotate(con->n, cpv(con->jnAcc, con->jtAcc));
+		apply_impulses(a, b, con->r1, con->r2, cpvmult(j, dt_coef));
 	}
 }
 
+// TODO is it worth splitting velocity/position correction?
+
 void
-cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef)
+cpArbiterApplyImpulse(cpArbiter *arb)
 {
-	cpBody *a = arb->a->body;
-	cpBody *b = arb->b->body;
+	cpBody *a = arb->body_a;
+	cpBody *b = arb->body_b;
+	cpVect surface_vr = arb->surface_vr;
+	cpFloat friction = arb->u;
 
 	for(int i=0; i<arb->numContacts; i++){
 		cpContact *con = &arb->contacts[i];
+		cpFloat nMass = con->nMass;
 		cpVect n = con->n;
 		cpVect r1 = con->r1;
 		cpVect r2 = con->r2;
 		
-		// Calculate the relative bias velocities.
 		cpVect vb1 = cpvadd(a->v_bias, cpvmult(cpvperp(r1), a->w_bias));
 		cpVect vb2 = cpvadd(b->v_bias, cpvmult(cpvperp(r2), b->w_bias));
+		cpVect vr = relative_velocity(a, b, r1, r2);
+		
 		cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n);
+		cpFloat vrn = cpvdot(vr, n);
+		cpFloat vrt = cpvdot(cpvadd(vr, surface_vr), cpvperp(n));
 		
-		// Calculate and clamp the bias impulse.
-		cpFloat jbn = (con->bias - vbn)*con->nMass;
+		cpFloat jbn = (con->bias - vbn)*nMass;
 		cpFloat jbnOld = con->jBias;
 		con->jBias = cpfmax(jbnOld + jbn, 0.0f);
-		jbn = con->jBias - jbnOld;
 		
-		// Apply the bias impulse.
-		apply_bias_impulses(a, b, r1, r2, cpvmult(n, jbn));
-
-		// Calculate the relative velocity.
-		cpVect vr = relative_velocity(a, b, r1, r2);
-		cpFloat vrn = cpvdot(vr, n);
-		
-		// Calculate and clamp the normal impulse.
-		cpFloat jn = -(con->bounce*eCoef + vrn)*con->nMass;
+		cpFloat jn = -(con->bounce + vrn)*nMass;
 		cpFloat jnOld = con->jnAcc;
 		con->jnAcc = cpfmax(jnOld + jn, 0.0f);
-		jn = con->jnAcc - jnOld;
-		
-		// Calculate the relative tangent velocity.
-		cpFloat vrt = cpvdot(cpvadd(vr, arb->surface_vr), cpvperp(n));
 		
-		// Calculate and clamp the friction impulse.
-		cpFloat jtMax = arb->u*con->jnAcc;
+		cpFloat jtMax = friction*con->jnAcc;
 		cpFloat jt = -vrt*con->tMass;
 		cpFloat jtOld = con->jtAcc;
 		con->jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax);
-		jt = con->jtAcc - jtOld;
 		
-		// Apply the final impulse.
-		apply_impulses(a, b, r1, r2, cpvrotate(n, cpv(jn, jt)));
+		apply_bias_impulses(a, b, r1, r2, cpvmult(n, con->jBias - jbnOld));
+		apply_impulses(a, b, r1, r2, cpvrotate(n, cpv(con->jnAcc - jnOld, con->jtAcc - jtOld)));
 	}
 }