static void
applyImpulse(cpSlideJoint *joint)
{
if(cpveql(joint->n, cpvzero)) return; // early exit
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
cpVect n = joint->n;
cpVect r1 = joint->r1;
cpVect r2 = joint->r2;
// compute relative velocity
cpVect vr = relative_velocity(a, b, r1, r2);
cpFloat vrn = cpvdot(vr, n);
// compute normal impulse
cpFloat jn = (joint->bias - vrn)*joint->nMass;
cpFloat jnOld = joint->jnAcc;
joint->jnAcc = cpfclamp(jnOld + jn, -joint->jnMax, 0.0f);
jn = joint->jnAcc - jnOld;
// apply impulse
apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn));
}
void
cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef)
{
cpBody *a = arb->a->body;
cpBody *b = arb->b->body;
for(int i=0; i<arb->numContacts; i++){
cpContact *con = &arb->contacts[i];
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));
cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n);
// Calculate and clamp the bias impulse.
cpFloat jbn = (con->bias - vbn)*con->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 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 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)));
}
}
static void
applyImpulse(cpGrooveJoint *joint)
{
CONSTRAINT_BEGIN(joint, a, b);
cpVect r1 = joint->r1;
cpVect r2 = joint->r2;
// compute impulse
cpVect vr = relative_velocity(a, b, r1, r2);
cpVect j = mult_k(cpvsub(joint->bias, vr), joint->k1, joint->k2);
cpVect jOld = joint->jAcc;
joint->jAcc = grooveConstrain(joint, cpvadd(jOld, j));
j = cpvsub(joint->jAcc, jOld);
// apply impulse
apply_impulses(a, b, joint->r1, joint->r2, j);
}
static void
applyImpulse(cpGrooveJoint *joint, cpFloat dt)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
cpVect r1 = joint->r1;
cpVect r2 = joint->r2;
// compute impulse
cpVect vr = relative_velocity(a, b, r1, r2);
cpVect j = cpMat2x2Transform(joint->k, cpvsub(joint->bias, vr));
cpVect jOld = joint->jAcc;
joint->jAcc = grooveConstrain(joint, cpvadd(jOld, j), dt);
j = cpvsub(joint->jAcc, jOld);
// apply impulse
apply_impulses(a, b, joint->r1, joint->r2, j);
}
static void
applyImpulse(cpPivotJoint *joint)
{
CONSTRAINT_BEGIN(joint, a, b);
cpVect r1 = joint->r1;
cpVect r2 = joint->r2;
// compute relative velocity
cpVect vr = relative_velocity(a, b, r1, r2);
// compute normal impulse
cpVect j = mult_k(cpvsub(joint->bias, vr), joint->k1, joint->k2);
cpVect jOld = joint->jAcc;
joint->jAcc = cpvclamp(cpvadd(joint->jAcc, j), joint->jMaxLen);
j = cpvsub(joint->jAcc, jOld);
// apply impulse
apply_impulses(a, b, joint->r1, joint->r2, j);
}
static void
applyImpulse(cpPivotJoint *joint, cpFloat dt)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
cpVect r1 = joint->r1;
cpVect r2 = joint->r2;
// compute relative velocity
cpVect vr = relative_velocity(a, b, r1, r2);
// compute normal impulse
cpVect j = cpMat2x2Transform(joint->k, cpvsub(joint->bias, vr));
cpVect jOld = joint->jAcc;
joint->jAcc = cpvclamp(cpvadd(joint->jAcc, j), joint->constraint.maxForce*dt);
j = cpvsub(joint->jAcc, jOld);
// apply impulse
apply_impulses(a, b, joint->r1, joint->r2, j);
}
void
cpArbiterApplyImpulse(cpArbiter *arb)
{
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;
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));
cpFloat jbn = (con->bias - vbn)*nMass;
cpFloat jbnOld = con->jBias;
con->jBias = cpfmax(jbnOld + jbn, 0.0f);
cpFloat jn = -(con->bounce + vrn)*nMass;
cpFloat jnOld = con->jnAcc;
con->jnAcc = cpfmax(jnOld + jn, 0.0f);
cpFloat jtMax = friction*con->jnAcc;
cpFloat jt = -vrt*con->tMass;
cpFloat jtOld = con->jtAcc;
con->jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax);
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)));
}
}
static void
applyImpulse(cpSlideJoint *joint)
{
if(!joint->bias) return; // early exit
CONSTRAINT_BEGIN(joint, a, b);
cpVect n = joint->n;
cpVect r1 = joint->r1;
cpVect r2 = joint->r2;
// compute relative velocity
cpVect vr = relative_velocity(a, b, r1, r2);
cpFloat vrn = cpvdot(vr, n);
// compute normal impulse
cpFloat jn = (joint->bias - vrn)*joint->nMass;
cpFloat jnOld = joint->jnAcc;
joint->jnAcc = cpfclamp(jnOld + jn, -joint->jnMax, 0.0f);
jn = joint->jnAcc - jnOld;
// apply impulse
apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn));
}
static inline real_t
normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vector2 n){
return relative_velocity(a, b, rA, rB).dot(n);
}