void
cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv)
{
cpBody *a = arb->a->body;
cpBody *b = arb->b->body;
for(int i=0; i<arb->numContacts; i++){
cpContact *con = &arb->contacts[i];
// Calculate the offsets.
con->r1 = cpvsub(con->p, a->p);
con->r2 = cpvsub(con->p, b->p);
// Calculate the mass normal and mass tangent.
con->nMass = 1.0f/k_scalar(a, b, con->r1, con->r2, con->n);
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->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;
}
}
void
cpArbiterPreStep(cpArbiter *arb, cpFloat dt, cpFloat slop, cpFloat bias)
{
cpBody *a = arb->body_a;
cpBody *b = arb->body_b;
for(int i=0; i<arb->numContacts; i++){
cpContact *con = &arb->contacts[i];
// Calculate the offsets.
con->r1 = cpvsub(con->p, a->p);
con->r2 = cpvsub(con->p, b->p);
// Calculate the mass normal and mass tangent.
con->nMass = 1.0f/k_scalar(a, b, con->r1, con->r2, con->n);
con->tMass = 1.0f/k_scalar(a, b, con->r1, con->r2, cpvperp(con->n));
// Calculate the target bias velocity.
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;
}
}
static void
applyImpulse(cpPinJoint *joint)
{
CONSTRAINT_BEGIN(joint, a, b);
cpVect n = joint->n;
// compute relative velocity
cpFloat vrn = normal_relative_velocity(a, b, joint->r1, joint->r2, n);
// compute normal impulse
cpFloat jn = (joint->bias - vrn)*joint->nMass;
cpFloat jnOld = joint->jnAcc;
joint->jnAcc = cpfclamp(jnOld + jn, -joint->jnMax, joint->jnMax);
jn = joint->jnAcc - jnOld;
// apply impulse
apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn));
}
static void
applyImpulse(cpDampedSpring *spring)
{
cpBody *a = spring->constraint.a;
cpBody *b = spring->constraint.b;
cpVect n = spring->n;
cpVect r1 = spring->r1;
cpVect r2 = spring->r2;
// compute relative velocity
cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n);
// compute velocity loss from drag
cpFloat v_damp = (spring->target_vrn - vrn)*spring->v_coef;
spring->target_vrn = vrn + v_damp;
apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass));
}
static void
applyImpulse(cpDampedSpring *spring)
{
CONSTRAINT_BEGIN(spring, a, b);
cpVect n = spring->n;
cpVect r1 = spring->r1;
cpVect r2 = spring->r2;
// compute relative velocity
cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn;
// compute velocity loss from drag
// not 100% certain this is derived correctly, though it makes sense
cpFloat v_damp = -vrn*spring->v_coef;
spring->target_vrn = vrn + v_damp;
apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass));
}
static void
applyImpulse(cpDampedSpring *spring)
{
cpBody *a = spring->constraint.a;
cpBody *b = spring->constraint.b;
cpVect n = spring->n;
cpVect r1 = spring->r1;
cpVect r2 = spring->r2;
// compute relative velocity
cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn;
// compute velocity loss from drag
// not 100% certain this is derived correctly, though it makes sense
cpFloat v_damp = -vrn*(1.0f - cpfexp(-spring->damping*spring->dt/spring->nMass));
spring->target_vrn = vrn + v_damp;
apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass));
}
static void
applyImpulse(cpPinJoint *joint, cpFloat dt)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
cpVect n = joint->n;
// compute relative velocity
cpFloat vrn = normal_relative_velocity(a, b, joint->r1, joint->r2, n);
cpFloat jnMax = joint->constraint.maxForce*dt;
// compute normal impulse
cpFloat jn = (joint->bias - vrn)*joint->nMass;
cpFloat jnOld = joint->jnAcc;
joint->jnAcc = cpfclamp(jnOld + jn, -jnMax, jnMax);
jn = joint->jnAcc - jnOld;
// apply impulse
apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn));
}
void
cpArbiterPreStep(cpArbiter *arb, cpFloat dt, cpFloat slop, cpFloat bias)
{
cpBody *a = arb->body_a;
cpBody *b = arb->body_b;
cpVect n = arb->n;
cpVect body_delta = cpvsub(b->p, a->p);
for(int i=0; i<arb->count; i++){
struct cpContact *con = &arb->contacts[i];
// Calculate the mass normal and mass tangent.
con->nMass = 1.0f/k_scalar(a, b, con->r1, con->r2, n);
con->tMass = 1.0f/k_scalar(a, b, con->r1, con->r2, cpvperp(n));
// Calculate the target bias velocity.
cpFloat dist = cpvdot(cpvadd(cpvsub(con->r2, con->r1), body_delta), n);
con->bias = -bias*cpfmin(0.0f, dist + slop)/dt;
con->jBias = 0.0f;
// Calculate the target bounce velocity.
con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, n)*arb->e;
}
}
