internal override void PreSolve(float dt)
{
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);
length = normal.Length();
normal.Normalize();
mass = PTransformer.CalcEffectiveMass(b1, b2, relAnchor1, relAnchor2,
normal);
b1.ApplyImpulse(normal.x * norI, normal.y * norI, anchor1.x, anchor1.y);
b2.ApplyImpulse(normal.x * -norI, normal.y * -norI, anchor2.x,
anchor2.y);
}
internal override void PreSolve(float dt)
{
relAnchor = b.mAng.Mul(localAnchor);
anchor.Set(relAnchor.x + b.pos.x, relAnchor.y + b.pos.y);
mass = PTransformer.CalcEffectiveMass(b, relAnchor);
Vector2f f = anchor.Sub(dragPoint);
float k = b.m;
f.MulLocal(-k * 20F);
Vector2f relVel = b.vel.Clone();
relVel.x += -b.angVel * relAnchor.y;
relVel.y += b.angVel * relAnchor.x;
relVel.MulLocal((float)System.Math.Sqrt(k * 20F * k));
f.SubLocal(relVel);
f.MulLocal(dt);
mass.MulEqual(f);
b.ApplyImpulse(f.x, f.y, anchor.x, anchor.y);
}
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);
}
internal override void PreSolve(float dt)
{
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);
mass = PTransformer.CalcEffectiveMass(b1, b2, relAnchor1, relAnchor2);
angM = 1.0F / (b1.invI + b2.invI);
float ang = b2.ang - b1.ang - localAngle;
if (!enableMotor)
{
motI = 0.0F;
}
if (enableLimit)
{
if (ang < minAngle)
{
if (limitState != -1)
{
limI = 0.0F;
}
limitState = -1;
if (b2.angVel - b1.angVel < 0.0F)
{
targetAngleSpeed = (b2.angVel - b1.angVel) * -rest;
}
else
{
targetAngleSpeed = 0.0F;
}
}
else if (ang > maxAngle)
{
if (limitState != 1)
{
limI = 0.0F;
}
limitState = 1;
if (b2.angVel - b1.angVel > 0.0F)
{
targetAngleSpeed = (b2.angVel - b1.angVel) * -rest;
}
else
{
targetAngleSpeed = 0.0F;
}
}
else
{
limI = limitState = 0;
}
}
else
{
limI = limitState = 0;
}
angI = 0.0F;
b1.ApplyImpulse(impulse.x, impulse.y, anchor1.x, anchor1.y);
b2.ApplyImpulse(-impulse.x, -impulse.y, anchor2.x, anchor2.y);
b1.ApplyTorque(motI + limI);
b2.ApplyTorque(-motI - limI);
}
