static void
preStep(cpRotaryLimitJoint *joint, cpFloat dt, cpFloat dt_inv)
{
CONSTRAINT_BEGIN(joint, a, 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/(a->i_inv + b->i_inv);
// calculate bias velocity
cpFloat maxBias = joint->constraint.maxBias;
joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(pdist), -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;
// apply joint torque
a->w -= joint->jAcc*a->i_inv;
b->w += joint->jAcc*b->i_inv;
}
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(cpOscillatingMotor *joint, cpFloat dt, cpFloat dt_inv)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
// calculate moment of inertia coefficient.
joint->iSum = 1.0f/(a->i_inv + b->i_inv);
// compute max impulse
joint->jMax = J_MAX(joint, dt);
// apply joint torque
a->w -= joint->jAcc*a->i_inv;
b->w += joint->jAcc*b->i_inv;
// accumulate time
joint->t += dt;
// time modulo (2*pi)/frequency (avoid large t)
if (joint->frequency == 0.0f) {
joint->t = 0.0f;
} else {
cpFloat tLimit = (2.0*M_PI)/joint->frequency;
if (joint->t > tLimit) joint->t -= tLimit;
}
}
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(cpSimpleMotor *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 + b->i_inv);
// compute max impulse
joint->jMax = J_MAX(joint, dt);
}
static void
preStep(cpSimpleMotor *joint, cpFloat dt, cpFloat dt_inv)
{
CONSTRAINT_BEGIN(joint, a, b);
// calculate moment of inertia coefficient.
joint->iSum = 1.0f/(a->i_inv + b->i_inv);
// compute max impulse
joint->jMax = J_MAX(joint, dt);
// apply joint torque
a->w -= joint->jAcc*a->i_inv;
b->w += joint->jAcc*b->i_inv;
}
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
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(cpGearJoint *joint, cpFloat dt, cpFloat dt_inv)
{
CONSTRAINT_BEGIN(joint, a, 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(-joint->constraint.biasCoef*dt_inv*(b->a*joint->ratio - a->a - joint->phase), -maxBias, maxBias);
// compute max impulse
joint->jMax = J_MAX(joint, dt);
// apply joint torque
cpFloat j = joint->jAcc;
a->w -= j*a->i_inv*joint->ratio_inv;
b->w += j*b->i_inv;
}
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
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(cpRatchetJoint *joint, cpFloat dt, cpFloat dt_inv)
{
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(-joint->constraint.biasCoef*dt_inv*pdist, -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;
// apply joint torque
a->w -= joint->jAcc*a->i_inv;
b->w += joint->jAcc*b->i_inv;
}
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);
// compute max impulse
joint->jnMax = J_MAX(joint, dt);
}
