322 lines
8.9 KiB
C#
322 lines
8.9 KiB
C#
using System;
|
|
using UnityEngine;
|
|
|
|
namespace RootMotion.FinalIK
|
|
{
|
|
[Serializable]
|
|
public class IKSolverTrigonometric : IKSolver
|
|
{
|
|
[Serializable]
|
|
public class TrigonometricBone : Bone
|
|
{
|
|
private Quaternion targetToLocalSpace;
|
|
|
|
private Vector3 defaultLocalBendNormal;
|
|
|
|
public void Initiate(Vector3 childPosition, Vector3 bendNormal)
|
|
{
|
|
Quaternion rotation = Quaternion.LookRotation(childPosition - transform.position, bendNormal);
|
|
targetToLocalSpace = QuaTools.RotationToLocalSpace(transform.rotation, rotation);
|
|
defaultLocalBendNormal = Quaternion.Inverse(transform.rotation) * bendNormal;
|
|
}
|
|
|
|
public Quaternion GetRotation(Vector3 direction, Vector3 bendNormal)
|
|
{
|
|
return Quaternion.LookRotation(direction, bendNormal) * targetToLocalSpace;
|
|
}
|
|
|
|
public Vector3 GetBendNormalFromCurrentRotation()
|
|
{
|
|
return transform.rotation * defaultLocalBendNormal;
|
|
}
|
|
}
|
|
|
|
public Transform target;
|
|
|
|
[Range(0f, 1f)]
|
|
public float IKRotationWeight = 1f;
|
|
|
|
public Quaternion IKRotation = Quaternion.identity;
|
|
|
|
public Vector3 bendNormal = Vector3.right;
|
|
|
|
public TrigonometricBone bone1 = new TrigonometricBone();
|
|
|
|
public TrigonometricBone bone2 = new TrigonometricBone();
|
|
|
|
public TrigonometricBone bone3 = new TrigonometricBone();
|
|
|
|
protected Vector3 weightIKPosition;
|
|
|
|
protected bool directHierarchy = true;
|
|
|
|
public void SetBendGoalPosition(Vector3 goalPosition, float weight)
|
|
{
|
|
if (!base.initiated || weight <= 0f)
|
|
{
|
|
return;
|
|
}
|
|
Vector3 vector = Vector3.Cross(goalPosition - bone1.transform.position, IKPosition - bone1.transform.position);
|
|
if (vector != Vector3.zero)
|
|
{
|
|
if (weight >= 1f)
|
|
{
|
|
bendNormal = vector;
|
|
}
|
|
else
|
|
{
|
|
bendNormal = Vector3.Lerp(bendNormal, vector, weight);
|
|
}
|
|
}
|
|
}
|
|
|
|
public void SetBendPlaneToCurrent()
|
|
{
|
|
if (base.initiated)
|
|
{
|
|
Vector3 vector = Vector3.Cross(bone2.transform.position - bone1.transform.position, bone3.transform.position - bone2.transform.position);
|
|
if (vector != Vector3.zero)
|
|
{
|
|
bendNormal = vector;
|
|
}
|
|
}
|
|
}
|
|
|
|
public void SetIKRotation(Quaternion rotation)
|
|
{
|
|
IKRotation = rotation;
|
|
}
|
|
|
|
public void SetIKRotationWeight(float weight)
|
|
{
|
|
IKRotationWeight = Mathf.Clamp(weight, 0f, 1f);
|
|
}
|
|
|
|
public Quaternion GetIKRotation()
|
|
{
|
|
return IKRotation;
|
|
}
|
|
|
|
public float GetIKRotationWeight()
|
|
{
|
|
return IKRotationWeight;
|
|
}
|
|
|
|
public override Point[] GetPoints()
|
|
{
|
|
return new Point[3] { bone1, bone2, bone3 };
|
|
}
|
|
|
|
public override Point GetPoint(Transform transform)
|
|
{
|
|
if (bone1.transform == transform)
|
|
{
|
|
return bone1;
|
|
}
|
|
if (bone2.transform == transform)
|
|
{
|
|
return bone2;
|
|
}
|
|
if (bone3.transform == transform)
|
|
{
|
|
return bone3;
|
|
}
|
|
return null;
|
|
}
|
|
|
|
public override void StoreDefaultLocalState()
|
|
{
|
|
bone1.StoreDefaultLocalState();
|
|
bone2.StoreDefaultLocalState();
|
|
bone3.StoreDefaultLocalState();
|
|
}
|
|
|
|
public override void FixTransforms()
|
|
{
|
|
if (base.initiated)
|
|
{
|
|
bone1.FixTransform();
|
|
bone2.FixTransform();
|
|
bone3.FixTransform();
|
|
}
|
|
}
|
|
|
|
public override bool IsValid(ref string message)
|
|
{
|
|
if (bone1.transform == null || bone2.transform == null || bone3.transform == null)
|
|
{
|
|
message = "Please assign all Bones to the IK solver.";
|
|
return false;
|
|
}
|
|
UnityEngine.Object[] objects = new Transform[3] { bone1.transform, bone2.transform, bone3.transform };
|
|
Transform transform = (Transform)Hierarchy.ContainsDuplicate(objects);
|
|
if (transform != null)
|
|
{
|
|
message = transform.name + " is represented multiple times in the Bones.";
|
|
return false;
|
|
}
|
|
if (bone1.transform.position == bone2.transform.position)
|
|
{
|
|
message = "first bone position is the same as second bone position.";
|
|
return false;
|
|
}
|
|
if (bone2.transform.position == bone3.transform.position)
|
|
{
|
|
message = "second bone position is the same as third bone position.";
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
public bool SetChain(Transform bone1, Transform bone2, Transform bone3, Transform root)
|
|
{
|
|
this.bone1.transform = bone1;
|
|
this.bone2.transform = bone2;
|
|
this.bone3.transform = bone3;
|
|
Initiate(root);
|
|
return base.initiated;
|
|
}
|
|
|
|
public static void Solve(Transform bone1, Transform bone2, Transform bone3, Vector3 targetPosition, Vector3 bendNormal, float weight)
|
|
{
|
|
if (weight <= 0f)
|
|
{
|
|
return;
|
|
}
|
|
targetPosition = Vector3.Lerp(bone3.position, targetPosition, weight);
|
|
Vector3 vector = targetPosition - bone1.position;
|
|
float magnitude = vector.magnitude;
|
|
if (magnitude != 0f)
|
|
{
|
|
float sqrMagnitude = (bone2.position - bone1.position).sqrMagnitude;
|
|
float sqrMagnitude2 = (bone3.position - bone2.position).sqrMagnitude;
|
|
Vector3 bendDirection = Vector3.Cross(vector, bendNormal);
|
|
Vector3 directionToBendPoint = GetDirectionToBendPoint(vector, magnitude, bendDirection, sqrMagnitude, sqrMagnitude2);
|
|
Quaternion quaternion = Quaternion.FromToRotation(bone2.position - bone1.position, directionToBendPoint);
|
|
if (weight < 1f)
|
|
{
|
|
quaternion = Quaternion.Lerp(Quaternion.identity, quaternion, weight);
|
|
}
|
|
bone1.rotation = quaternion * bone1.rotation;
|
|
Quaternion quaternion2 = Quaternion.FromToRotation(bone3.position - bone2.position, targetPosition - bone2.position);
|
|
if (weight < 1f)
|
|
{
|
|
quaternion2 = Quaternion.Lerp(Quaternion.identity, quaternion2, weight);
|
|
}
|
|
bone2.rotation = quaternion2 * bone2.rotation;
|
|
}
|
|
}
|
|
|
|
private static Vector3 GetDirectionToBendPoint(Vector3 direction, float directionMag, Vector3 bendDirection, float sqrMag1, float sqrMag2)
|
|
{
|
|
float num = (directionMag * directionMag + (sqrMag1 - sqrMag2)) / 2f / directionMag;
|
|
float y = (float)Math.Sqrt(Mathf.Clamp(sqrMag1 - num * num, 0f, float.PositiveInfinity));
|
|
if (direction == Vector3.zero)
|
|
{
|
|
return Vector3.zero;
|
|
}
|
|
return Quaternion.LookRotation(direction, bendDirection) * new Vector3(0f, y, num);
|
|
}
|
|
|
|
protected override void OnInitiate()
|
|
{
|
|
if (bendNormal == Vector3.zero)
|
|
{
|
|
bendNormal = Vector3.right;
|
|
}
|
|
OnInitiateVirtual();
|
|
IKPosition = bone3.transform.position;
|
|
IKRotation = bone3.transform.rotation;
|
|
InitiateBones();
|
|
directHierarchy = IsDirectHierarchy();
|
|
}
|
|
|
|
private bool IsDirectHierarchy()
|
|
{
|
|
if (bone3.transform.parent != bone2.transform)
|
|
{
|
|
return false;
|
|
}
|
|
if (bone2.transform.parent != bone1.transform)
|
|
{
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
public void InitiateBones()
|
|
{
|
|
bone1.Initiate(bone2.transform.position, bendNormal);
|
|
bone2.Initiate(bone3.transform.position, bendNormal);
|
|
SetBendPlaneToCurrent();
|
|
}
|
|
|
|
protected override void OnUpdate()
|
|
{
|
|
IKPositionWeight = Mathf.Clamp(IKPositionWeight, 0f, 1f);
|
|
IKRotationWeight = Mathf.Clamp(IKRotationWeight, 0f, 1f);
|
|
if (target != null)
|
|
{
|
|
IKPosition = target.position;
|
|
IKRotation = target.rotation;
|
|
}
|
|
OnUpdateVirtual();
|
|
if (IKPositionWeight > 0f)
|
|
{
|
|
if (!directHierarchy)
|
|
{
|
|
bone1.Initiate(bone2.transform.position, bendNormal);
|
|
bone2.Initiate(bone3.transform.position, bendNormal);
|
|
}
|
|
bone1.sqrMag = (bone2.transform.position - bone1.transform.position).sqrMagnitude;
|
|
bone2.sqrMag = (bone3.transform.position - bone2.transform.position).sqrMagnitude;
|
|
if (bendNormal == Vector3.zero && !Warning.logged)
|
|
{
|
|
LogWarning("IKSolverTrigonometric Bend Normal is Vector3.zero.");
|
|
}
|
|
weightIKPosition = Vector3.Lerp(bone3.transform.position, IKPosition, IKPositionWeight);
|
|
Vector3 vector = Vector3.Lerp(bone1.GetBendNormalFromCurrentRotation(), bendNormal, IKPositionWeight);
|
|
Vector3 vector2 = Vector3.Lerp(bone2.transform.position - bone1.transform.position, GetBendDirection(weightIKPosition, vector), IKPositionWeight);
|
|
if (vector2 == Vector3.zero)
|
|
{
|
|
vector2 = bone2.transform.position - bone1.transform.position;
|
|
}
|
|
bone1.transform.rotation = bone1.GetRotation(vector2, vector);
|
|
bone2.transform.rotation = bone2.GetRotation(weightIKPosition - bone2.transform.position, bone2.GetBendNormalFromCurrentRotation());
|
|
}
|
|
if (IKRotationWeight > 0f)
|
|
{
|
|
bone3.transform.rotation = Quaternion.Slerp(bone3.transform.rotation, IKRotation, IKRotationWeight);
|
|
}
|
|
OnPostSolveVirtual();
|
|
}
|
|
|
|
protected virtual void OnInitiateVirtual()
|
|
{
|
|
}
|
|
|
|
protected virtual void OnUpdateVirtual()
|
|
{
|
|
}
|
|
|
|
protected virtual void OnPostSolveVirtual()
|
|
{
|
|
}
|
|
|
|
protected Vector3 GetBendDirection(Vector3 IKPosition, Vector3 bendNormal)
|
|
{
|
|
Vector3 vector = IKPosition - bone1.transform.position;
|
|
if (vector == Vector3.zero)
|
|
{
|
|
return Vector3.zero;
|
|
}
|
|
float sqrMagnitude = vector.sqrMagnitude;
|
|
float num = (float)Math.Sqrt(sqrMagnitude);
|
|
float num2 = (sqrMagnitude + bone1.sqrMag - bone2.sqrMag) / 2f / num;
|
|
float y = (float)Math.Sqrt(Mathf.Clamp(bone1.sqrMag - num2 * num2, 0f, float.PositiveInfinity));
|
|
Vector3 upwards = Vector3.Cross(vector / num, bendNormal);
|
|
return Quaternion.LookRotation(vector, upwards) * new Vector3(0f, y, num2);
|
|
}
|
|
}
|
|
}
|