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); }