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(cpRatchetJoint *joint, cpFloat dt) { cpBody *a = joint->constraint.a; cpBody *b = joint->constraint.b; cpFloat angle = joint->angle; cpFloat phase = joint->phase; cpFloat ratchet = joint->ratchet; cpFloat delta = b->a - a->a; cpFloat diff = angle - delta; cpFloat pdist = 0.0f; if(diff*ratchet > 0.0f){ pdist = diff; } else { joint->angle = cpffloor((delta - phase)/ratchet)*ratchet + phase; } // calculate moment of inertia coefficient. joint->iSum = 1.0f/(a->i_inv + b->i_inv); // calculate bias velocity cpFloat maxBias = joint->constraint.maxBias; joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias); // If the bias is 0, the joint is not at a limit. Reset the impulse. if(!joint->bias) joint->jAcc = 0.0f; }
static void preStep(cpRotaryLimitJoint *joint, cpFloat dt) { cpBody *a = joint->constraint.a; cpBody *b = joint->constraint.b; cpFloat dist = b->a - a->a; cpFloat pdist = 0.0f; if(dist > joint->max) { pdist = joint->max - dist; } else if(dist < joint->min) { pdist = joint->min - dist; } // calculate moment of inertia coefficient. joint->iSum = 1.0f/(1.0f/a->i + 1.0f/b->i); // 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->jMax = J_MAX(joint, dt); // If the bias is 0, the joint is not at a limit. Reset the impulse. if(!joint->bias) joint->jAcc = 0.0f; }
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; }
static void preStep(cpGearJoint *joint, cpFloat dt) { cpBody *a = joint->constraint.a; cpBody *b = joint->constraint.b; // calculate moment of inertia coefficient. joint->iSum = 1.0f/(a->i_inv*joint->ratio_inv + joint->ratio*b->i_inv); // calculate bias velocity cpFloat maxBias = joint->constraint.maxBias; joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*(b->a*joint->ratio - a->a - joint->phase)/dt, -maxBias, maxBias); }
static void preStep(cpPivotJoint *joint, cpFloat dt) { cpBody *a = joint->constraint.a; cpBody *b = joint->constraint.b; joint->r1 = cpTransformVect(a->transform, cpvsub(joint->anchorA, a->cog)); joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog)); // Calculate mass tensor joint-> k = k_tensor(a, b, joint->r1, joint->r2); // calculate bias velocity cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias); }
static void preStep(cpPivotJoint *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); // 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, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias); }
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(cpGrooveJoint *joint, cpFloat dt) { 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, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias); }