510 lines
21 KiB
C#
510 lines
21 KiB
C#
using UnityEngine;
|
|
|
|
namespace FIMSpace.FTools
|
|
{
|
|
/// <summary>
|
|
/// FC: Class for processing IK logics for 3-bones or multiple bones inverse kinematics
|
|
/// </summary>
|
|
[System.Serializable]
|
|
public partial class FTools_IKProcessorek
|
|
{
|
|
public Vector3 IKTargetPosition;
|
|
public Quaternion IKTargetRotation;
|
|
public Vector3 IKElbowTargetPosition = Vector3.zero;
|
|
|
|
public FTools_IKProcessorBone[] IKBones;// { get; private set; }
|
|
|
|
public bool Initialized = false;
|
|
public bool CCDIK { get; private set; }
|
|
|
|
|
|
#region Limb / CCD IK Variables
|
|
|
|
// Global
|
|
[Range(0f, 1f)] public float IKWeight = 1f;
|
|
|
|
// Limb
|
|
public FTools_IKProcessorBone StartBone { get { return IKBones[0]; } }
|
|
public FTools_IKProcessorBone ElbowBone { get { return IKBones[1]; } }
|
|
public FTools_IKProcessorBone EndBone { get { if (!CCDIK) return IKBones[2]; else return IKBones[IKBones.Length - 1]; } }
|
|
|
|
public Vector3 targetElbowNormal = Vector3.right;
|
|
|
|
public bool LHand = false;
|
|
public FIK_ElbowMode ElbowMode = FIK_ElbowMode.Target;
|
|
public enum FIK_ElbowMode { None, Animation, Target, Parent, /*Arm,*/ }
|
|
public Quaternion frameEndBoneRotation;
|
|
|
|
/// <summary> Length of whole bones chain (squared) </summary>
|
|
private float fullLength;
|
|
|
|
// CCD
|
|
[Range(1, 12)]
|
|
public int CCD_ReactionQuality = 4;
|
|
[Range(0f, 1f)]
|
|
public float CCD_Smoothing = 0f;
|
|
[Range(0f, 181f)]
|
|
public float CCD_LimitAngle = 60f;
|
|
public bool AutoWeight = true;
|
|
|
|
public Vector3 LastLocalDirection;
|
|
public Vector3 LocalDirection;
|
|
|
|
#endregion
|
|
|
|
|
|
#region Initiation methods
|
|
|
|
/// <summary> Assigning bones for IK processor with limb IK logics (3-bones) </summary>
|
|
public void SetLimb(Transform startBone, Transform elbowBone, Transform endBone)
|
|
{
|
|
CCDIK = false;
|
|
|
|
IKBones = new FTools_IKProcessorBone[3];
|
|
IKBones[0] = new FTools_IKProcessorBone() { transform = startBone };
|
|
IKBones[1] = new FTools_IKProcessorBone() { transform = elbowBone };
|
|
IKBones[2] = new FTools_IKProcessorBone() { transform = endBone };
|
|
|
|
IKTargetPosition = endBone.position; IKTargetRotation = endBone.rotation;
|
|
}
|
|
|
|
|
|
/// <summary> Assigning bones for IK processor with CCD IK logics (unlimited bone count) </summary>
|
|
public void SetCCD(Transform[] bonesChain)
|
|
{
|
|
CCDIK = true;
|
|
|
|
IKBones = new FTools_IKProcessorBone[bonesChain.Length];
|
|
for (int i = 0; i < bonesChain.Length; i++)
|
|
IKBones[i] = new FTools_IKProcessorBone() { transform = bonesChain[i] };
|
|
|
|
IKTargetPosition = EndBone.transform.position; IKTargetRotation = EndBone.transform.rotation;
|
|
}
|
|
|
|
#endregion
|
|
|
|
Quaternion initWorldRootRotation;
|
|
public void Initialize(Transform root)
|
|
{
|
|
if (Initialized) return;
|
|
|
|
initWorldRootRotation = root.rotation;
|
|
Vector3 normal = Vector3.Cross(ElbowBone.transform.position - StartBone.transform.position, EndBone.transform.position - ElbowBone.transform.position);
|
|
if (normal != Vector3.zero) targetElbowNormal = normal;
|
|
|
|
if (StartBone.transform.parent != null) startParentWorldRotation = Quaternion.Inverse(initWorldRootRotation) * StartBone.transform.parent.rotation;
|
|
|
|
fullLength = 0f;
|
|
|
|
if (!CCDIK) // Initializing Limb IK Bones
|
|
{
|
|
StartBone.Init(ElbowBone.transform.position, targetElbowNormal);
|
|
ElbowBone.Init(EndBone.transform.position, targetElbowNormal);
|
|
EndBone.Init(EndBone.transform.position + (EndBone.transform.position - ElbowBone.transform.position), targetElbowNormal);
|
|
|
|
fullLength = IKBones[0].BoneLength + IKBones[1].BoneLength;
|
|
|
|
RefreshOrientationNormal(); // ?
|
|
}
|
|
else // Initializing CCD IK Bones
|
|
{
|
|
float step = 1f / (float)(IKBones.Length * 1.3f);
|
|
|
|
for (int i = 0; i < IKBones.Length; i++)
|
|
{
|
|
FTools_IKProcessorBone b = IKBones[i];
|
|
|
|
if (i < IKBones.Length - 1)
|
|
{
|
|
b.Init(IKBones[i + 1].transform.position, targetElbowNormal);
|
|
fullLength += b.BoneLength;
|
|
b.Axis = Quaternion.Inverse(b.transform.rotation) * (IKBones[i + 1].transform.position - b.transform.position);
|
|
}
|
|
else
|
|
b.Axis = Quaternion.Inverse(b.transform.rotation) * (IKBones[IKBones.Length - 1].transform.position - IKBones[0].transform.position);
|
|
|
|
if (AutoWeight) b.MotionWeight = 1f - step * i;
|
|
}
|
|
}
|
|
|
|
if (CCD_LimitAngle < 180)
|
|
for (int i = 0; i < IKBones.Length; i++)
|
|
{
|
|
IKBones[i].angleLimit = CCD_LimitAngle;
|
|
IKBones[i].twistAngleLimit = Mathf.Min(80f, CCD_LimitAngle);
|
|
}
|
|
|
|
Initialized = true;
|
|
}
|
|
|
|
|
|
/// <summary> Updates Limb or CCD IK depends which setup is initialized </summary>
|
|
public void Update()
|
|
{
|
|
//for (int i = 0; i < IKBones.Length; i++)
|
|
// IKBones[i].transform.localRotation = IKBones[i].initLocalRotation;
|
|
|
|
if (CCDIK) UpdateCCDIK(); else UpdateLimbIK();
|
|
}
|
|
|
|
|
|
#region Limb IK Methods
|
|
|
|
/// <summary> Updating processor with 3-bones oriented inverse kinematics </summary>
|
|
public void UpdateLimbIK()
|
|
{
|
|
if (!Initialized) return;
|
|
|
|
frameEndBoneRotation = EndBone.transform.rotation;
|
|
|
|
StartBone.BoneLength = (ElbowBone.transform.position - StartBone.transform.position).sqrMagnitude;
|
|
ElbowBone.BoneLength = (EndBone.transform.position - ElbowBone.transform.position).sqrMagnitude;
|
|
|
|
targetElbowNormal = GetOrientationNormal();
|
|
|
|
Vector3 orientationDirection = GetOrientationDirection(IKTargetPosition, targetElbowNormal);
|
|
if (orientationDirection == Vector3.zero) orientationDirection = ElbowBone.transform.position - StartBone.transform.position;
|
|
|
|
StartBone.transform.rotation = StartBone.GetRotation(orientationDirection, targetElbowNormal);
|
|
ElbowBone.transform.rotation = ElbowBone.GetRotation(IKTargetPosition - ElbowBone.transform.position, ElbowBone.GetCurrentOrientationNormal());
|
|
}
|
|
|
|
|
|
/// <summary> Returning >= 1f when max range for IK point is reached </summary>
|
|
public float GetStretchValue(Vector3 targetPos)
|
|
{
|
|
if (!CCDIK)
|
|
{
|
|
float fullLength = Mathf.Epsilon;
|
|
fullLength += (StartBone.transform.position - ElbowBone.transform.position).magnitude;
|
|
fullLength += (ElbowBone.transform.position - EndBone.transform.position).magnitude;
|
|
|
|
float toGoal = (StartBone.transform.position - targetPos).magnitude;
|
|
|
|
return toGoal / fullLength;
|
|
}
|
|
else
|
|
{
|
|
float fullLength = Mathf.Epsilon;
|
|
|
|
for (int i = 0; i < IKBones.Length - 1; i++)
|
|
fullLength += (IKBones[i].transform.position - IKBones[i + 1].transform.position).magnitude;
|
|
|
|
float toGoal = (StartBone.transform.position - targetPos).magnitude;
|
|
|
|
return toGoal / fullLength;
|
|
}
|
|
}
|
|
|
|
private Vector3 GetOrientationNormal()
|
|
{
|
|
if (IKElbowTargetPosition.sqrMagnitude != 0)
|
|
return CalculateElbowNormalToPosition(IKElbowTargetPosition);
|
|
else
|
|
return GetAutomaticElbowNormal();
|
|
|
|
// weight
|
|
}
|
|
|
|
public Vector3 CalculateElbowNormalToPosition(Vector3 targetElbowPos)
|
|
{
|
|
return Vector3.Cross(targetElbowPos - StartBone.transform.position, EndBone.transform.position - StartBone.transform.position);
|
|
}
|
|
|
|
public void RefreshOrientationNormal()
|
|
{
|
|
Vector3 normal = Vector3.Cross(ElbowBone.transform.position - StartBone.transform.position, EndBone.transform.position - ElbowBone.transform.position);
|
|
if (normal != Vector3.zero) targetElbowNormal = normal;
|
|
}
|
|
|
|
private Vector3 GetOrientationDirection(Vector3 ikPosition, Vector3 orientationNormal)
|
|
{
|
|
Vector3 direction = ikPosition - StartBone.transform.position;
|
|
if (direction == Vector3.zero) return Vector3.zero;
|
|
|
|
float directionLength = direction.sqrMagnitude;
|
|
float forward = (directionLength + StartBone.BoneLength - ElbowBone.BoneLength) / 2f / Mathf.Sqrt(directionLength);
|
|
float up = Mathf.Sqrt(StartBone.BoneLength - forward * forward);
|
|
if (float.IsNaN(up)) up = 0f;
|
|
|
|
Vector3 perpendicularUp = Vector3.Cross(direction, orientationNormal);
|
|
|
|
return Quaternion.LookRotation(direction, perpendicularUp) * new Vector3(0f, up, forward);
|
|
}
|
|
|
|
bool maintained = false;
|
|
[Range(0f, 1f)]
|
|
public float weight = 1f;
|
|
Quaternion startParentWorldRotation;
|
|
private Vector3 GetAutomaticElbowNormal()
|
|
{
|
|
Vector3 bendNormal = StartBone.GetCurrentOrientationNormal();
|
|
|
|
switch (ElbowMode)
|
|
{
|
|
case FIK_ElbowMode.Animation:
|
|
if (!maintained) targetElbowNormal = StartBone.GetCurrentOrientationNormal(); maintained = false;
|
|
return Vector3.Lerp(bendNormal, targetElbowNormal, weight);
|
|
|
|
case FIK_ElbowMode.Parent:
|
|
Quaternion parentRotation = StartBone.transform.parent.rotation * Quaternion.Inverse(startParentWorldRotation);
|
|
return Quaternion.Slerp(Quaternion.identity, parentRotation * Quaternion.Inverse(initWorldRootRotation), weight) * bendNormal;
|
|
|
|
case FIK_ElbowMode.Target:
|
|
Quaternion targetRotation = IKTargetRotation * Quaternion.Inverse(EndBone.initLocalRotation);
|
|
return Quaternion.Slerp(Quaternion.identity, targetRotation, weight) * bendNormal;
|
|
|
|
{
|
|
//case FIK_ElbowMode.Arm:
|
|
|
|
// if (StartBone.transform.parent == null) return bendNormal;
|
|
// Vector3 direction = (IKTargetPosition - StartBone.transform.position).normalized;
|
|
// direction = Quaternion.Inverse(StartBone.transform.parent.rotation * Quaternion.Inverse(startParentWorldRotation)) * direction;
|
|
// if (LHand) direction.x = -direction.x;
|
|
|
|
// for (int i = 1; i < axisDirections.Length; i++)
|
|
// {
|
|
// axisDirections[i].dot = Mathf.Clamp(Vector3.Dot(axisDirections[i].direction, direction), 0f, 1f);
|
|
// axisDirections[i].dot = EaseInOutQuint(0f, 1f, axisDirections[i].dot);
|
|
// }
|
|
|
|
// Vector3 sum = axisDirections[0].axis;
|
|
// for (int i = 1; i < axisDirections.Length; i++) sum = Vector3.Slerp(sum, axisDirections[i].axis, axisDirections[i].dot);
|
|
// if (LHand) { sum.x = -sum.x; sum = -sum; }
|
|
|
|
// Vector3 armBendNormal = StartBone.transform.parent.rotation * Quaternion.Inverse(startParentWorldRotation) * sum;
|
|
// if (weight >= 1) return armBendNormal;
|
|
// return Vector3.Lerp(bendNormal, armBendNormal, weight);
|
|
}
|
|
|
|
}
|
|
|
|
return bendNormal;
|
|
}
|
|
|
|
//float EaseInOutQuint(float start, float end, float value)
|
|
//{
|
|
// value /= .5f; end -= start;
|
|
// if (value < 1) return end * 0.5f * value * value * value * value * value + start; value -= 2;
|
|
// return end * 0.5f * (value * value * value * value * value + 2) + start;
|
|
//}
|
|
|
|
#endregion
|
|
|
|
|
|
#region CCD IK Methods
|
|
|
|
/// <summary> Updating processor with n-bones oriented inverse kinematics </summary>
|
|
public void UpdateCCDIK()
|
|
{
|
|
if (!Initialized) return;
|
|
|
|
if (CCD_ReactionQuality < 0) CCD_ReactionQuality = 1;
|
|
Vector3 goalPivotOffset = Vector3.zero;
|
|
if (CCD_ReactionQuality > 1) goalPivotOffset = GetGoalPivotOffset();
|
|
|
|
for (int itr = 0; itr < CCD_ReactionQuality; itr++)
|
|
{
|
|
// Restrictions for multiple interations
|
|
if (itr >= 1)
|
|
if (goalPivotOffset.sqrMagnitude == 0)
|
|
if (CCD_Smoothing > 0)
|
|
if (GetVelocityDifference() < CCD_Smoothing * CCD_Smoothing) break;
|
|
|
|
LastLocalDirection = RefreshLocalDirection();
|
|
|
|
Vector3 ikGoal = IKTargetPosition + goalPivotOffset;
|
|
|
|
// Solving CCD IK
|
|
for (int b = IKBones.Length - 2; b > -1; b--)
|
|
{
|
|
float weight = IKBones[b].MotionWeight * IKWeight;
|
|
|
|
if (weight > 0f)
|
|
{
|
|
Vector3 toEnd = IKBones[IKBones.Length - 1].transform.position - IKBones[b].transform.position;
|
|
Vector3 toTarget = ikGoal - IKBones[b].transform.position;
|
|
|
|
Quaternion targetRotation = Quaternion.FromToRotation(toEnd, toTarget) * IKBones[b].transform.rotation;
|
|
|
|
if (weight < 1) IKBones[b].transform.rotation = Quaternion.Lerp(IKBones[b].transform.rotation, targetRotation, weight);
|
|
else IKBones[b].transform.rotation = targetRotation;
|
|
}
|
|
|
|
IKBones[b].AngleLimiting();
|
|
}
|
|
}
|
|
|
|
LastLocalDirection = RefreshLocalDirection();
|
|
}
|
|
|
|
|
|
protected Vector3 GetGoalPivotOffset()
|
|
{
|
|
if (!GoalPivotOffsetDetected()) return Vector3.zero;
|
|
|
|
Vector3 IKDirection = (IKTargetPosition - IKBones[0].transform.position).normalized;
|
|
Vector3 secondaryDirection = new Vector3(IKDirection.y, IKDirection.z, IKDirection.x);
|
|
|
|
if (CCD_LimitAngle > 0f)
|
|
if (IKBones[IKBones.Length - 2].angleLimit < 180 || IKBones[IKBones.Length - 2].twistAngleLimit < 180)
|
|
secondaryDirection = IKBones[IKBones.Length - 2].transform.rotation * IKBones[IKBones.Length - 2].Axis;
|
|
|
|
return Vector3.Cross(IKDirection, secondaryDirection) * IKBones[IKBones.Length - 2].BoneLength * 0.5f;
|
|
}
|
|
|
|
private bool GoalPivotOffsetDetected()
|
|
{
|
|
if (!Initialized) return false;
|
|
|
|
Vector3 toLastDirection = IKBones[IKBones.Length - 1].transform.position - IKBones[0].transform.position;
|
|
Vector3 toGoalDirection = IKTargetPosition - IKBones[0].transform.position;
|
|
|
|
float toLastMagn = toLastDirection.magnitude;
|
|
float toGoalMagn = toGoalDirection.magnitude;
|
|
|
|
if (toGoalMagn == 0) return false;
|
|
if (toLastMagn == 0) return false;
|
|
if (toLastMagn < toGoalMagn) return false;
|
|
if (toLastMagn < fullLength - (IKBones[IKBones.Length - 2].BoneLength * 0.1f)) return false;
|
|
if (toGoalMagn > toLastMagn) return false;
|
|
|
|
float dot = Vector3.Dot(toLastDirection / toLastMagn, toGoalDirection / toGoalMagn);
|
|
if (dot < 0.999f) return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
Vector3 RefreshLocalDirection()
|
|
{
|
|
LocalDirection = IKBones[0].transform.InverseTransformDirection(IKBones[IKBones.Length - 1].transform.position - IKBones[0].transform.position);
|
|
return LocalDirection;
|
|
}
|
|
|
|
float GetVelocityDifference()
|
|
{ return Vector3.SqrMagnitude(LocalDirection - LastLocalDirection); }
|
|
|
|
#endregion
|
|
|
|
|
|
[System.Serializable]
|
|
public class FTools_IKProcessorBone
|
|
{
|
|
|
|
public Transform transform;
|
|
public float BoneLength;
|
|
public Vector3 Axis;
|
|
public float MotionWeight = 1f;
|
|
|
|
[SerializeField] private Quaternion targetToLocalSpace;
|
|
[SerializeField] private Vector3 defaultLocalPoleNormal;
|
|
public Quaternion initWorldRotation;
|
|
|
|
#region CCD IK Variables
|
|
|
|
[Range(0f, 180f)] public float angleLimit = 45;
|
|
[Range(0f, 180f)] public float twistAngleLimit = 180;
|
|
public Vector2 hingeLimits = Vector2.zero;
|
|
|
|
public Quaternion initLocalRotation;
|
|
public Quaternion previousHingeRotation;
|
|
public float previousHingeAngle;
|
|
|
|
#endregion
|
|
|
|
public void Init(Vector3 childPosition, Vector3 orientationNormal)
|
|
{
|
|
Quaternion defaultTargetRotation = Quaternion.LookRotation(childPosition - transform.position, orientationNormal);
|
|
|
|
targetToLocalSpace = RotationToLocal(transform.rotation, defaultTargetRotation);
|
|
defaultLocalPoleNormal = Quaternion.Inverse(transform.rotation) * orientationNormal;
|
|
BoneLength = (childPosition - transform.position).sqrMagnitude;
|
|
initLocalRotation = transform.localRotation;
|
|
initWorldRotation = transform.rotation;
|
|
}
|
|
|
|
#region Limb IK methods
|
|
|
|
public static Quaternion RotationToLocal(Quaternion parent, Quaternion rotation)
|
|
{ return Quaternion.Inverse(Quaternion.Inverse(parent) * rotation); }
|
|
|
|
public Quaternion GetRotation(Vector3 direction, Vector3 orientationNormal)
|
|
{ return Quaternion.LookRotation(direction, orientationNormal) * targetToLocalSpace; }
|
|
|
|
public Vector3 GetCurrentOrientationNormal()
|
|
{ return transform.rotation * (defaultLocalPoleNormal); }
|
|
|
|
#endregion
|
|
|
|
|
|
#region CCD IK Methods
|
|
|
|
public void AngleLimiting()
|
|
{
|
|
Quaternion localRotation = Quaternion.Inverse(initLocalRotation) * transform.localRotation;
|
|
Quaternion limitedRotation = localRotation;
|
|
|
|
if (hingeLimits.sqrMagnitude == 0)
|
|
{
|
|
if (angleLimit < 180) limitedRotation = LimitPY(limitedRotation);
|
|
if (twistAngleLimit < 180) limitedRotation = LimitRoll(limitedRotation);
|
|
}
|
|
else limitedRotation = LimitHinge(limitedRotation);
|
|
|
|
if (Equals(limitedRotation, localRotation)) return;
|
|
|
|
transform.localRotation = initLocalRotation * limitedRotation;
|
|
}
|
|
|
|
private Quaternion LimitPY(Quaternion rotation)
|
|
{
|
|
if (Equals(rotation, Quaternion.identity)) return rotation;
|
|
|
|
Vector3 pyAxis = rotation * Axis;
|
|
Quaternion angleRotation = Quaternion.FromToRotation(Axis, pyAxis);
|
|
Quaternion limitAngle = Quaternion.RotateTowards(Quaternion.identity, angleRotation, angleLimit);
|
|
Quaternion limit = Quaternion.FromToRotation(pyAxis, limitAngle * Axis);
|
|
|
|
return limit * rotation;
|
|
}
|
|
|
|
private Quaternion LimitRoll(Quaternion currentRotation)
|
|
{
|
|
Vector3 orthogonalAxis = new Vector3(Axis.y, Axis.z, Axis.x);
|
|
Vector3 normal = currentRotation * Axis;
|
|
Vector3 tangent = orthogonalAxis;
|
|
Vector3.OrthoNormalize(ref normal, ref tangent);
|
|
|
|
Vector3 tangentRotation = currentRotation * orthogonalAxis;
|
|
Vector3.OrthoNormalize(ref normal, ref tangentRotation);
|
|
|
|
Quaternion limitRot = Quaternion.FromToRotation(tangentRotation, tangent) * currentRotation;
|
|
if (twistAngleLimit <= 0) return limitRot;
|
|
|
|
return Quaternion.RotateTowards(limitRot, currentRotation, twistAngleLimit);
|
|
}
|
|
|
|
private Quaternion LimitHinge(Quaternion rotation)
|
|
{
|
|
Quaternion freeDegree = Quaternion.FromToRotation(rotation * Axis, Axis) * rotation;
|
|
Quaternion addRotation = freeDegree * Quaternion.Inverse(previousHingeRotation);
|
|
|
|
float addAngle = Quaternion.Angle(Quaternion.identity, addRotation);
|
|
|
|
Vector3 orthogonalAxis = new Vector3(Axis.z, Axis.x, Axis.y);
|
|
Vector3 cross = Vector3.Cross(orthogonalAxis, Axis);
|
|
if (Vector3.Dot(addRotation * orthogonalAxis, cross) > 0f) addAngle = -addAngle;
|
|
|
|
previousHingeAngle = Mathf.Clamp(previousHingeAngle + addAngle, hingeLimits.x, hingeLimits.y);
|
|
|
|
previousHingeRotation = Quaternion.AngleAxis(previousHingeAngle, Axis);
|
|
return previousHingeRotation;
|
|
}
|
|
|
|
#endregion
|
|
|
|
}
|
|
|
|
}
|
|
}
|