internal void Update()
{
float twoPI = MathUtils.TWO_PI;
ang = (ang + twoPI) % twoPI;
mAng.SetRotate(ang);
for (int i_0 = 0; i_0 < numShapes; i_0++)
{
PShape s = shapes[i_0];
s._pos.Set(s._localPos.x, s._localPos.y);
mAng.MulEqual(s._pos);
s._pos.AddLocal(pos);
s._localAng = (s._localAng + twoPI) % twoPI;
s._ang = ang + s._localAng;
s._mAng.SetRotate(s._ang);
s.Update();
s.CalcAABB();
s._sapAABB.Update();
if (i_0 == 0)
{
aabb.Set(s._aabb.minX, s._aabb.minY, s._aabb.maxX, s._aabb.maxY);
}
else
{
aabb.Set(Min(aabb.minX, s._aabb.minX),
Min(aabb.minY, s._aabb.minY),
Max(aabb.maxX, s._aabb.maxX),
Max(aabb.maxY, s._aabb.maxY));
}
}
}
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 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 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);
}