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