internal override void SolveVelocity(float dt)
{
float rn = normal.Dot(PTransformer.CalcRelativeVelocity(b1, b2,
relAnchor1, relAnchor2));
float impulse = -mass * rn;
norI += impulse;
float forceX = normal.x * impulse;
float forceY = normal.y * impulse;
b1.ApplyImpulse(forceX, forceY, anchor1.x, anchor1.y);
b2.ApplyImpulse(-forceX, -forceY, anchor2.x, anchor2.y);
}
internal override void SolveVelocity(float dt)
{
Vector2f relVel = PTransformer.CalcRelativeVelocity(b1, b2, relAnchor1,
relAnchor2);
Vector2f force = mass.Mul(relVel).Negate();
impulse.AddLocal(force);
b1.ApplyImpulse(force.x, force.y, anchor1.x, anchor1.y);
b2.ApplyImpulse(-force.x, -force.y, anchor2.x, anchor2.y);
if (enableMotor)
{
float angRelVel = b2.angVel - b1.angVel - motorSpeed;
float torque = angM * angRelVel;
float subMotorI = motI;
motI = MathUtils.Clamp(motI + torque, -motorTorque * dt,
motorTorque * dt);
torque = motI - subMotorI;
b1.ApplyTorque(torque);
b2.ApplyTorque(-torque);
}
if (enableLimit && limitState != 0)
{
float angRelVel_0 = b2.angVel - b1.angVel - targetAngleSpeed;
float torque_1 = angM * angRelVel_0;
float subLimitI = limI;
if (limitState == -1)
{
limI = MathUtils.Min(limI + torque_1, 0.0F);
}
else
{
if (limitState == 1)
{
limI = MathUtils.Max(limI + torque_1, 0.0F);
}
}
torque_1 = limI - subLimitI;
b1.ApplyTorque(torque_1);
b2.ApplyTorque(-torque_1);
}
}
internal override void PreSolve(float i_0)
{
relAnchor1 = b1.mAng.Mul(localAnchor1);
relAnchor2 = b2.mAng.Mul(localAnchor2);
anchor1.Set(relAnchor1.x + b1.pos.x, relAnchor1.y + b1.pos.y);
anchor2.Set(relAnchor2.x + b2.pos.x, relAnchor2.y + b2.pos.y);
normal = anchor2.Sub(anchor1);
float over = dist - normal.Length();
normal.Normalize();
mass = PTransformer.CalcEffectiveMass(b1, b2, relAnchor1, relAnchor2,
normal);
float k = mass * 1000F * str;
force = -over * k;
force += PTransformer.CalcRelativeVelocity(b1, b2, relAnchor1,
relAnchor2).Dot(normal)
* damp * -(float)System.Math.Sqrt(k * mass) * 2.0F;
force *= i_0;
b1.ApplyImpulse(normal.x * force, normal.y * force, anchor1.x,
anchor1.y);
b2.ApplyImpulse(normal.x * -force, normal.y * -force, anchor2.x,
anchor2.y);
}