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 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)); }
static void applyCachedImpulse(cpPivotJoint *joint, cpFloat dt_coef) { cpBody *a = joint->constraint.a; cpBody *b = joint->constraint.b; apply_impulses(a, b, joint->r1, joint->r2, cpvmult(joint->jAcc, dt_coef)); }
static void applyCachedImpulse(cpSlideJoint *joint, cpFloat dt_coef) { cpBody *a = joint->constraint.a; cpBody *b = joint->constraint.b; cpVect j = cpvmult(joint->n, joint->jnAcc*dt_coef); apply_impulses(a, b, joint->r1, joint->r2, j); }
void cpArbiterApplyCachedImpulse(cpArbiter *arb, cpFloat dt_coef) { if(cpArbiterIsFirstContact(arb)) return; cpBody *a = arb->body_a; cpBody *b = arb->body_b; for(int i=0; i<arb->numContacts; i++){ cpContact *con = &arb->contacts[i]; cpVect j = cpvrotate(con->n, cpv(con->jnAcc, con->jtAcc)); apply_impulses(a, b, con->r1, con->r2, cpvmult(j, dt_coef)); } }
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))); } }
void cpArbiterApplyCachedImpulse(cpArbiter *arb) { cpShape *shapea = arb->a; cpShape *shapeb = arb->b; arb->u = shapea->u * shapeb->u; arb->surface_vr = cpvsub(shapeb->surface_v, shapea->surface_v); cpBody *a = shapea->body; cpBody *b = shapeb->body; for(int i=0; i<arb->numContacts; i++){ cpContact *con = &arb->contacts[i]; apply_impulses(a, b, con->r1, con->r2, cpvrotate(con->n, cpv(con->jnAcc, con->jtAcc))); } }
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 preStep(cpGrooveJoint *joint, cpFloat dt, cpFloat dt_inv) { cpBody *a = joint->constraint.a; cpBody *b = joint->constraint.b; // calculate endpoints in worldspace cpVect ta = cpBodyLocal2World(a, joint->grv_a); cpVect tb = cpBodyLocal2World(a, joint->grv_b); // calculate axis cpVect n = cpvrotate(joint->grv_n, a->rot); cpFloat d = cpvdot(ta, n); joint->grv_tn = n; joint->r2 = cpvrotate(joint->anchr2, b->rot); // calculate tangential distance along the axis of r2 cpFloat td = cpvcross(cpvadd(b->p, joint->r2), n); // calculate clamping factor and r2 if(td <= cpvcross(ta, n)){ joint->clamp = 1.0f; joint->r1 = cpvsub(ta, a->p); } else if(td >= cpvcross(tb, n)){ joint->clamp = -1.0f; joint->r1 = cpvsub(tb, a->p); } else { joint->clamp = 0.0f; joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p); } // Calculate mass tensor k_tensor(a, b, joint->r1, joint->r2, &joint->k1, &joint->k2); // compute max impulse joint->jMaxLen = J_MAX(joint, dt); // calculate bias velocity cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias); // apply accumulated impulse apply_impulses(a, b, joint->r1, joint->r2, joint->jAcc); }
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(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(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(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(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 preStep(cpPivotJoint *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); // Calculate mass tensor k_tensor(a, b, joint->r1, joint->r2, &joint->k1, &joint->k2); // compute max impulse joint->jMaxLen = J_MAX(joint, dt); // calculate bias velocity cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias); // apply accumulated impulse apply_impulses(a, b, joint->r1, joint->r2, joint->jAcc); }
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); }
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 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 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(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) { 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 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 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)); }
void RaycastCar::updateVehicle( btScalar step ) { m_currentVehicleSpeedKmHour = btScalar(3.6f) * getRigidBody()->getLinearVelocity().length(); const btTransform & chassisTrans = getChassisWorldTransform(); btVector3 forwardW(chassisTrans.getBasis()[0][m_indexForwardAxis], chassisTrans.getBasis()[1][m_indexForwardAxis], chassisTrans.getBasis()[2][m_indexForwardAxis]); if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.0f)) m_currentVehicleSpeedKmHour *= btScalar(-1.0f); for (int i = 0; i < getNumWheels(); i++) { updateWheelTransform(i, false); btScalar depth; depth = rayCast(m_wheelInfo[i]); } update_suspension(step); update_engine(step); update_forces(step); apply_impulses(step); }