172 lines
6.2 KiB
C#
172 lines
6.2 KiB
C#
using UnityEngine;
|
|
|
|
namespace IK
|
|
{
|
|
public class TwoBoneIK
|
|
{
|
|
private class JointInfo
|
|
{
|
|
public Transform transform;
|
|
|
|
public float distance;
|
|
}
|
|
|
|
public enum EffectorRotationType
|
|
{
|
|
None,
|
|
World,
|
|
Local
|
|
}
|
|
|
|
private const int JointNum = 3;
|
|
|
|
private const int Root = 0;
|
|
|
|
private const int Center = 1;
|
|
|
|
private const int EndEffector = 2;
|
|
|
|
private JointInfo[] _joint;
|
|
|
|
public Vector3 TargetPosition { get; set; }
|
|
|
|
public Vector3 JointTargetPosition { get; set; }
|
|
|
|
public Transform EndEffectorTransform => _joint[EndEffector].transform;
|
|
|
|
public float BlendAlpha { get; set; }
|
|
|
|
public Quaternion EndEffectorRotation { get; set; }
|
|
|
|
public EffectorRotationType EndEffectorRotationType { get; set; }
|
|
|
|
public void Initialize(Transform endEffector)
|
|
{
|
|
Transform transform = endEffector;
|
|
_joint = new JointInfo[JointNum];
|
|
|
|
_joint[EndEffector] = new JointInfo();
|
|
_joint[EndEffector].transform = transform;
|
|
transform = transform.parent;
|
|
|
|
_joint[Center] = new JointInfo();
|
|
_joint[Center].transform = transform;
|
|
transform = transform.parent;
|
|
|
|
_joint[Root] = new JointInfo();
|
|
_joint[Root].transform = transform;
|
|
for (int i = 0; i < 2; i++)
|
|
{
|
|
_joint[i].distance = Vector3.Distance(_joint[i].transform.position, _joint[i + 1].transform.position);
|
|
}
|
|
}
|
|
|
|
public void Solve()
|
|
{
|
|
float blendAlpha = BlendAlpha;
|
|
if (blendAlpha <= 0f)
|
|
{
|
|
return;
|
|
}
|
|
Vector3 positionRoot = _joint[Root].transform.position;
|
|
Vector3 positionCenter = _joint[Center].transform.position;
|
|
Vector3 positionEndEffector = _joint[EndEffector].transform.position;
|
|
float distanceRoot = _joint[Root].distance;
|
|
float distanceCenter = _joint[Center].distance;
|
|
|
|
Vector3 vector = TargetPosition - positionRoot;
|
|
float num = vector.magnitude;
|
|
if (num < 1E-06f)
|
|
{
|
|
num = 1E-06f;
|
|
vector = Helper.AxisUp;
|
|
}
|
|
else
|
|
{
|
|
vector /= num;
|
|
}
|
|
Vector3 vector2 = JointTargetPosition - positionRoot;
|
|
Vector3 axis;
|
|
if (vector2.sqrMagnitude < 1E-12f)
|
|
{
|
|
axis = Helper.AxisUp;
|
|
}
|
|
else
|
|
{
|
|
Vector3 axis2 = Vector3.Cross(vector, vector2);
|
|
if (axis2.sqrMagnitude < 1E-12f)
|
|
{
|
|
Helper.FindBestAxisVectors(axis2, out axis2, out axis);
|
|
}
|
|
else
|
|
{
|
|
axis = Vector3.Normalize(vector2 - vector * Vector3.Dot(vector2, vector));
|
|
}
|
|
}
|
|
Vector3 vector3 = TargetPosition;
|
|
Vector3 vector4 = positionCenter;
|
|
float num2 = distanceRoot + distanceCenter;
|
|
if (num > num2)
|
|
{
|
|
vector3 = positionRoot + num2 * vector;
|
|
vector4 = positionRoot + distanceRoot * vector;
|
|
}
|
|
else
|
|
{
|
|
float num3 = 2f * distanceRoot * num;
|
|
float num4 = ((num3 != 0f) ? ((distanceRoot * distanceRoot + num * num - distanceCenter * distanceCenter) / num3) : 0f);
|
|
if (num4 > 1f || num4 < -1f)
|
|
{
|
|
if (distanceRoot > distanceCenter)
|
|
{
|
|
vector4 = positionRoot + distanceRoot * vector;
|
|
vector3 = vector4 - distanceCenter * vector;
|
|
}
|
|
else
|
|
{
|
|
vector4 = positionRoot - distanceRoot * vector;
|
|
vector3 = vector4 + distanceCenter * vector;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
float f = Mathf.Acos(num4);
|
|
bool num5 = num4 < 0f;
|
|
float num6 = distanceRoot * Mathf.Sin(f);
|
|
float num7 = distanceRoot * distanceRoot - num6 * num6;
|
|
float num8 = ((num7 > 0f) ? Mathf.Sqrt(num7) : 0f);
|
|
if (num5)
|
|
{
|
|
num8 *= -1f;
|
|
}
|
|
vector4 = positionRoot + num8 * vector + num6 * axis;
|
|
}
|
|
}
|
|
Vector3 vector5 = Vector3.Normalize(vector4 - positionRoot);
|
|
Vector3 vector6 = Vector3.Normalize(positionCenter - positionRoot);
|
|
if (Vector3.Dot(vector6, vector5) < 0.999999f)
|
|
{
|
|
Quaternion quaternion = Quaternion.Lerp(Helper.QuaternionIdentity, Quaternion.FromToRotation(vector6, vector5), blendAlpha);
|
|
_joint[Root].transform.rotation = quaternion * _joint[Root].transform.rotation;
|
|
positionCenter = _joint[Center].transform.position;
|
|
positionEndEffector = _joint[EndEffector].transform.position;
|
|
}
|
|
Vector3 vector7 = Vector3.Normalize(vector3 - positionCenter);
|
|
Vector3 vector8 = Vector3.Normalize(positionEndEffector - positionCenter);
|
|
if (Vector3.Dot(vector8, vector7) < 0.999999f)
|
|
{
|
|
Quaternion quaternion2 = Quaternion.Lerp(Helper.QuaternionIdentity, Quaternion.FromToRotation(vector8, vector7), blendAlpha);
|
|
_joint[Center].transform.rotation = quaternion2 * _joint[Center].transform.rotation;
|
|
}
|
|
switch (EndEffectorRotationType)
|
|
{
|
|
case EffectorRotationType.Local:
|
|
_joint[EndEffector].transform.localRotation = Quaternion.Lerp(_joint[EndEffector].transform.localRotation, EndEffectorRotation, blendAlpha);
|
|
break;
|
|
case EffectorRotationType.World:
|
|
_joint[EndEffector].transform.rotation = Quaternion.Lerp(_joint[EndEffector].transform.rotation, EndEffectorRotation, blendAlpha);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
} |