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;
}
}
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;
}
}
static void
preStep(cpSlideJoint *joint, cpFloat dt)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
joint->r1 = cpvrotate(joint->anchr1, a->rot);
joint->r2 = cpvrotate(joint->anchr2, b->rot);
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
cpFloat dist = cpvlength(delta);
cpFloat pdist = 0.0f;
if(dist > joint->max) {
pdist = dist - joint->max;
joint->n = cpvnormalize_safe(delta);
} else if(dist < joint->min) {
pdist = joint->min - dist;
dist = -dist;
joint->n = cpvneg(cpvnormalize_safe(delta));
} else {
joint->n = cpvzero;
joint->jnAcc = 0.0f;
}
// calculate mass normal
joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n);
// calculate bias velocity
cpFloat maxBias = joint->constraint.maxBias;
joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias);
// compute max impulse
joint->jnMax = J_MAX(joint, dt);
}
static void
preStep(cpPinJoint *joint, cpFloat dt, cpFloat dt_inv)
{
CONSTRAINT_BEGIN(joint, a, b);
joint->r1 = cpvrotate(joint->anchr1, a->rot);
joint->r2 = cpvrotate(joint->anchr2, b->rot);
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
cpFloat dist = cpvlength(delta);
joint->n = cpvmult(delta, 1.0f/(dist ? dist : (cpFloat)INFINITY));
// calculate mass normal
joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n);
// calculate bias velocity
cpFloat maxBias = joint->constraint.maxBias;
joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(dist - joint->dist), -maxBias, maxBias);
// compute max impulse
joint->jnMax = J_MAX(joint, dt);
// apply accumulated impulse
cpVect j = cpvmult(joint->n, joint->jnAcc);
apply_impulses(a, b, joint->r1, joint->r2, j);
}
static void
preStep(cpSlideJoint *joint, cpFloat dt)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
joint->r1 = cpvrotate(joint->anchr1, a->rot);
joint->r2 = cpvrotate(joint->anchr2, b->rot);
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
cpFloat dist = cpvlength(delta);
cpFloat pdist = 0.0f;
if(dist > joint->max) {
pdist = dist - joint->max;
} else if(dist < joint->min) {
pdist = joint->min - dist;
dist = -dist;
}
joint->n = cpvmult(delta, 1.0f/(dist ? dist : (cpFloat)INFINITY));
// calculate mass normal
joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n);
// calculate bias velocity
cpFloat maxBias = joint->constraint.maxBias;
joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias);
// compute max impulse
joint->jnMax = J_MAX(joint, dt);
// if bias is 0, then the joint is not at a limit. Reset cached impulse.
if(!joint->bias) joint->jnAcc = 0.0f;
}
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;
}
}
static void
preStep(cpPinJoint *joint, cpFloat dt)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
joint->r1 = cpvrotate(joint->anchr1, a->rot);
joint->r2 = cpvrotate(joint->anchr2, b->rot);
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
cpFloat dist = cpvlength(delta);
joint->n = cpvmult(delta, 1.0f/(dist ? dist : (cpFloat)INFINITY));
// calculate mass normal
joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n);
// calculate bias velocity
cpFloat maxBias = joint->constraint.maxBias;
joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*(dist - joint->dist)/dt, -maxBias, maxBias);
}
static void
preStep(cpSlideJoint *joint, cpFloat dt, cpFloat dt_inv)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
joint->r1 = cpvrotate(joint->anchr1, a->rot);
joint->r2 = cpvrotate(joint->anchr2, b->rot);
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
cpFloat dist = cpvlength(delta);
cpFloat pdist = 0.0f;
if(dist > joint->max) {
pdist = dist - joint->max;
} else if(dist < joint->min) {
pdist = joint->min - dist;
dist = -dist;
}
joint->n = cpvmult(delta, 1.0f/(dist ? dist : (cpFloat)INFINITY));
// calculate mass normal
joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n);
// calculate bias velocity
cpFloat maxBias = joint->constraint.maxBias;
joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(pdist), -maxBias, maxBias);
// compute max impulse
joint->jnMax = J_MAX(joint, dt);
// apply accumulated impulse
if(!joint->bias) //{
// if bias is 0, then the joint is not at a limit.
joint->jnAcc = 0.0f;
// } else {
cpVect j = cpvmult(joint->n, joint->jnAcc);
apply_impulses(a, b, joint->r1, joint->r2, j);
// }
}
static void
preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv)
{
CONSTRAINT_BEGIN(spring, a, b);
spring->r1 = cpvrotate(spring->anchr1, a->rot);
spring->r2 = cpvrotate(spring->anchr2, b->rot);
cpVect delta = cpvsub(cpvadd(b->p, spring->r2), cpvadd(a->p, spring->r1));
cpFloat dist = cpvlength(delta);
spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY));
cpFloat k = k_scalar(a, b, spring->r1, spring->r2, spring->n);
spring->nMass = 1.0f/k;
spring->target_vrn = 0.0f;
spring->v_coef = 1.0f - cpfexp(-spring->damping*dt*k);
// apply spring force
cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist);
apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, f_spring*dt));
}
static void
preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv)
{
cpBody *a = spring->constraint.a;
cpBody *b = spring->constraint.b;
spring->r1 = cpvrotate(spring->anchr1, a->rot);
spring->r2 = cpvrotate(spring->anchr2, b->rot);
cpVect delta = cpvsub(cpvadd(b->p, spring->r2), cpvadd(a->p, spring->r1));
cpFloat dist = cpvlength(delta);
spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY));
// calculate mass normal
spring->nMass = 1.0f/k_scalar(a, b, spring->r1, spring->r2, spring->n);
spring->dt = dt;
spring->target_vrn = 0.0f;
// apply spring force
cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist);
apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, f_spring*dt));
}
static void
preStep(cpDampedSpring *spring, cpFloat dt)
{
cpBody *a = spring->constraint.a;
cpBody *b = spring->constraint.b;
spring->r1 = cpvrotate(spring->anchr1, a->rot);
spring->r2 = cpvrotate(spring->anchr2, b->rot);
cpVect delta = cpvsub(cpvadd(b->p, spring->r2), cpvadd(a->p, spring->r1));
cpFloat dist = cpvlength(delta);
spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY));
cpFloat k = k_scalar(a, b, spring->r1, spring->r2, spring->n);
cpAssertSoft(k != 0.0, "Unsolvable spring.");
spring->nMass = 1.0f/k;
spring->target_vrn = 0.0f;
spring->v_coef = 1.0f - cpfexp(-spring->damping*dt*k);
// apply spring force
cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist);
apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, f_spring*dt));
}
