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