internal override void SolvePosition()
{
if (enableLimit && limitState != 0)
{
float over = b2.ang - b1.ang - localAngle;
if (over < minAngle)
{
over += 0.008F;
over = ((over - minAngle) + b2.correctAngVel)
- b1.correctAngVel;
float torque = over * 0.2F * angM;
float subAngleImpulse = angI;
angI = MathUtils.Min(angI + torque, 0.0F);
torque = angI - subAngleImpulse;
b1.PositionCorrection(torque);
b2.PositionCorrection(-torque);
}
if (over > maxAngle)
{
over -= 0.008F;
over = ((over - maxAngle) + b2.correctAngVel)
- b1.correctAngVel;
float torque_0 = over * 0.2F * angM;
float subAngleImpulse_1 = angI;
angI = MathUtils.Max(angI + torque_0, 0.0F);
torque_0 = angI - subAngleImpulse_1;
b1.PositionCorrection(torque_0);
b2.PositionCorrection(-torque_0);
}
}
Vector2f force = anchor2.Sub(anchor1);
force.SubLocal(PTransformer.CalcRelativeCorrectVelocity(b1, b2,
relAnchor1, relAnchor2));
float length = force.Length();
force.Normalize();
force.MulLocal(System.Math.Max(length * 0.2F - 0.002F, 0.0F));
mass.MulEqual(force);
b1.PositionCorrection(force.x, force.y, anchor1.x, anchor1.y);
b2.PositionCorrection(-force.x, -force.y, anchor2.x, anchor2.y);
}
internal override void SolvePosition()
{
float rvn = normal.Dot(PTransformer.CalcRelativeCorrectVelocity(b1, b2,
relAnchor1, relAnchor2));
float impulse = -mass * ((dist - length) + rvn) * 0.2F;
if (impulse > 0.0F)
{
impulse = Max(impulse - 0.002F, 0.0F);
}
else if (impulse < 0.0F)
{
impulse = Min(impulse + 0.002F, 0.0F);
}
float forceX = normal.x * impulse;
float forceY = normal.y * impulse;
b1.PositionCorrection(forceX, forceY, anchor1.x, anchor1.y);
b2.PositionCorrection(-forceX, -forceY, anchor2.x, anchor2.y);
}