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)); }