private void HandIK(Limb hand, Vector2 pos, float force = 1.0f)
{
Vector2 shoulderPos = LimbJoints[2].WorldAnchorA;
Limb arm = (hand.type == LimbType.LeftHand) ? GetLimb(LimbType.LeftArm) : GetLimb(LimbType.RightArm);
//hand length
float a = 37.0f;
//arm length
float b = 28.0f;
//distance from shoulder to holdpos
float c = ConvertUnits.ToDisplayUnits(Vector2.Distance(pos, shoulderPos));
c = MathHelper.Clamp(a + b - 1, b - a, c);
float ang2 = MathUtils.VectorToAngle(pos - shoulderPos) + MathHelper.PiOver2;
float armAngle = MathUtils.SolveTriangleSSS(a, b, c);
float handAngle = MathUtils.SolveTriangleSSS(b, a, c);
arm.body.SmoothRotate((ang2 - armAngle * Dir), 20.0f * force);
hand.body.SmoothRotate((ang2 + handAngle * Dir), 100.0f * force);
}
