46 lines
1.7 KiB
C#
46 lines
1.7 KiB
C#
|
using UnityEngine.Scripting.APIUpdating;
|
||
|
|
|
||
|
|
namespace UnityEngine.U2D.IK
|
||
|
|
{
|
||
|
|
/// <summary>
|
||
|
|
/// Utility for 2D Limb IK Solver.
|
||
|
|
/// </summary>
|
||
|
|
[MovedFrom("UnityEngine.Experimental.U2D.IK")]
|
||
|
|
public static class Limb
|
||
|
|
{
|
||
|
|
/// <summary>
|
||
|
|
/// Solve based on Limb IK
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="targetPosition">Target position.</param>
|
||
|
|
/// <param name="lengths">Length of the chains.</param>
|
||
|
|
/// <param name="positions">Chain positions.</param>
|
||
|
|
/// <param name="outAngles">Output angles for the chain's position.</param>
|
||
|
|
/// <returns>Always returns true.</returns>
|
||
|
|
public static bool Solve(Vector3 targetPosition, float[] lengths, Vector3[] positions, ref float[] outAngles)
|
||
|
|
{
|
||
|
|
outAngles[0] = 0f;
|
||
|
|
outAngles[1] = 0f;
|
||
|
|
|
||
|
|
if (lengths[0] == 0f || lengths[1] == 0f)
|
||
|
|
return false;
|
||
|
|
|
||
|
|
var startToEnd = targetPosition - positions[0];
|
||
|
|
var distanceMagnitude = startToEnd.magnitude;
|
||
|
|
var sqrDistance = startToEnd.sqrMagnitude;
|
||
|
|
|
||
|
|
var sqrParentLength = (lengths[0] * lengths[0]);
|
||
|
|
var sqrTargetLength = (lengths[1] * lengths[1]);
|
||
|
|
|
||
|
|
var angle0Cos = (sqrDistance + sqrParentLength - sqrTargetLength) / (2f * lengths[0] * distanceMagnitude);
|
||
|
|
var angle1Cos = (sqrDistance - sqrParentLength - sqrTargetLength) / (2f * lengths[0] * lengths[1]);
|
||
|
|
|
||
|
|
if (angle0Cos >= -1f && angle0Cos <= 1f && angle1Cos >= -1f && angle1Cos <= 1f)
|
||
|
|
{
|
||
|
|
outAngles[0] = Mathf.Acos(angle0Cos) * Mathf.Rad2Deg;
|
||
|
|
outAngles[1] = Mathf.Acos(angle1Cos) * Mathf.Rad2Deg;
|
||
|
|
}
|
||
|
|
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|