Files
UmaViewer/Assets/Scripts/umamusume/Gallop/DereScript/IK/TwoBoneIK.cs
T
2023-01-20 07:13:54 +08:00

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