preStep(cpSlideJoint *joint, double 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 = cpVect.Sub(cpVect.Add(b.p, joint.r2), cpVect.Add(a.p, joint.r1));
double dist = cpvlength(delta);
double 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;
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
double maxBias = joint.constraint.maxBias;
joint.bias = cpfclamp(-bias_coef(joint.constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias);
}
applyCachedImpulse(cpSlideJoint *joint, double dt_coef)
{
cpBody a = joint.constraint.a;
cpBody b = joint.constraint.b;
cpVect j = cpVect.Multiply(joint.n, joint.jnAcc*dt_coef);
apply_impulses(a, b, joint.r1, joint.r2, j);
}
public void Draw(cpSlideJoint constraint)
{
cpVect a = cpTransform.Point(constraint.a.transform, constraint.GetAnchorA());
cpVect b = cpTransform.Point(constraint.b.transform, constraint.GetAnchorB());
DrawSegment(a, b, 1, cpColor.Grey);
DrawDot(a, 5, CONSTRAINT_COLOR);
DrawDot(b, 5, CONSTRAINT_COLOR);
}
protected bool Init(CCPhysicsBody a, CCPhysicsBody b, CCPoint anchr1, CCPoint anchr2, float min, float max)
{
if (!base.Init(a, b))
{
return(false);
}
cpConstraint joint = new cpSlideJoint(GetBodyInfo(a).Body, GetBodyInfo(b).Body,
PhysicsHelper.CCPointToCpVect(anchr1),
PhysicsHelper.CCPointToCpVect(anchr2),
min,
max);
if (joint == null)
{
return(false);
}
_info.Add(joint);
return(true);
}
applyImpulse(cpSlideJoint *joint, double dt)
{
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);
double vrn = cpVect.Dot(vr, n);
// compute normal impulse
double jn = (joint.bias - vrn)*joint.nMass;
double jnOld = joint.jnAcc;
joint.jnAcc = cpfclamp(jnOld + jn, -joint.constraint.maxForce*dt, 0.0f);
jn = joint.jnAcc - jnOld;
// apply impulse
apply_impulses(a, b, joint.r1, joint.r2, cpVect.Multiply(n, jn));
}
cpSlideJointInit(cpSlideJoint *joint, cpBody a, cpBody b, cpVect anchr1, cpVect anchr2, double min, double max)
{
cpConstraintInit((cpConstraint )joint, &klass, a, b);
joint.anchr1 = anchr1;
joint.anchr2 = anchr2;
joint.min = min;
joint.max = max;
joint.jnAcc = 0.0f;
return joint;
}
CP_DefineClassGetter(cpSlideJoint) cpSlideJoint *