阶段性完成

Assets/OtherPlugins/FImpossible Creations/Plugins - Shared/IK/Limb/FimpIK_Limb.Features.cs

@@ -0,0 +1,208 @@
+using System;
+using UnityEngine;
+
+namespace FIMSpace.FTools
+{
+    public partial class FimpIK_Limb : FIK_ProcessorBase
+    {
+        // Foot/End Bone rotation helper with root reference
+        public Quaternion EndBoneMapping { get; protected set; }
+        public IKBone FeetIKBone { get { return IKBones[3]; } }
+
+
+        /// <summary> Assigning helpful reference to main root transform of body to help IK rotations </summary>
+        public virtual void SetRootReference(Transform mainParentTransform)
+        {
+            Root = mainParentTransform;
+            EndBoneMapping = Quaternion.FromToRotation(EndIKBone.right, Vector3.right);
+            EndBoneMapping *= Quaternion.FromToRotation(EndIKBone.up, Vector3.up);
+            if (mainParentTransform) hasRoot = true;
+        }
+
+
+        /// <summary> Reference scale for computations - active length from start bone to middle knee </summary>
+        public float ScaleReference { get; protected set; }
+
+        public void RefreshLength()
+        {
+            ScaleReference = (StartIKBone.transform.position - MiddleIKBone.transform.position).magnitude;
+        }
+
+        public void RefreshScaleReference()
+        {
+            ScaleReference = (StartIKBone.transform.position - MiddleIKBone.transform.position).magnitude;
+        }
+
+        float GetCurrentLegToAnkleLength()
+        {
+            float fullLength = Mathf.Epsilon;
+            fullLength += (StartIKBone.transform.position - MiddleIKBone.transform.position).magnitude;
+            fullLength += (MiddleIKBone.transform.position - EndIKBone.transform.position).magnitude;
+            return fullLength;
+        }
+
+        /// <summary> Returning >= 1f when max range for IK point is reached </summary>
+        public float GetStretchValue(Vector3 targetPos)
+        {
+            float toGoal = (StartIKBone.transform.position - targetPos).magnitude;
+            return toGoal / GetCurrentLegToAnkleLength();
+        }
+
+        public Vector3 GetNotStretchedPositionTowards(Vector3 targetPos, float maxStretch)
+        {
+            Vector3 toGoal = (targetPos - StartIKBone.transform.position);
+            return StartIKBone.transform.position + toGoal.normalized * (GetCurrentLegToAnkleLength() * maxStretch);
+        }
+
+        public void ApplyMaxStretchingPreprocessing(float maxStretch, float allowIKRotationFadeout = 2f)
+        {
+            if (maxStretch < 1.1f)
+            {
+
+                float toGoal = (StartIKBone.transform.position - IKTargetPosition).magnitude;
+                float limbUnitLength = GetCurrentLegToAnkleLength();
+                float stretch = toGoal / limbUnitLength;
+
+                if (stretch > maxStretch)
+                {
+
+                    if (hasFeet && FeetStretchWeight > 0f)
+                    {
+
+                        #region Feet stretch helper
+
+                        if (maxFeetAngle > 0f)
+                        {
+                            // Feet angle factor helpers
+                            Vector3 thighToTarget = IKTargetPosition - StartIKBone.transform.position;
+                            thighToTarget.Normalize();
+                            Vector3 ankleToFeet = FeetIKBone.transform.position - EndIKBone.transform.position;
+                            ankleToFeet.Normalize();
+
+                            float feetDot = Vector3.Dot(thighToTarget, ankleToFeet);
+                            feetDot = Mathf.Clamp01(feetDot);
+
+                            // Feet bone rotation helpers
+                            float feetLength = (FeetIKBone.transform.position - EndIKBone.transform.position).magnitude;
+                            float stretchDiff = toGoal - limbUnitLength * Mathf.Min(maxStretch, 1f);
+                            stretchDiff /= (feetLength * FeetFadeQuicker);
+                            float stretchDiff2 = stretchDiff;
+                            stretchDiff *= maxFeetAngleFactor * FeetStretchSensitivity;
+                            if (stretchDiff > 1f) stretchDiff = 1f;
+
+                            if (stretchDiff2 < 1f) stretchDiff2 = 1f; else { if (stretchDiff2 > 2f) stretchDiff2 = 2f; stretchDiff2 -= 1f; stretchDiff2 *= stretchDiff2; stretchDiff2 = 1f - stretchDiff2; }
+
+                            // Apply
+                            float heelFactor = Mathf.Min(FeetStretchLimit, (1f - feetDot) * (90f / maxFeetAngle) * stretchDiff * FeetStretchWeight);
+
+                            if (stretch > 1.09f)
+                            {
+                                stretchDiff2 *= 1f - Mathf.InverseLerp(1.09f, 1.23f, stretch);
+                            }
+
+                            if (heelFactor != 0f) OffsetHeel(heelFactor, stretchDiff2);
+
+                            // Recompute
+                            toGoal = (StartIKBone.transform.position - IKTargetPosition).magnitude;
+                            stretch = toGoal / limbUnitLength;
+                        }
+
+                        #endregion
+
+                        if (stretch > maxStretch)
+                        {
+                            float len = (maxStretch * limbUnitLength);
+                            IKTargetPosition = StartIKBone.transform.position + (IKTargetPosition - StartIKBone.transform.position).normalized * len;
+                        }
+
+                    }
+                    else
+                    {
+                        float len = (maxStretch * limbUnitLength);
+                        IKTargetPosition = StartIKBone.transform.position + (IKTargetPosition - StartIKBone.transform.position).normalized * len;
+                    }
+
+                    if (allowIKRotationFadeout > 0f)
+                    {
+                        float stretchDiff = stretch - maxStretch;
+                        stretchDiff = Mathf.Clamp01(stretchDiff * allowIKRotationFadeout);
+                        internalRotationWeightMul = (1f - stretchDiff);
+                    }
+                }
+                else
+                {
+                    internalRotationWeightMul = 1f;
+                }
+            }
+        }
+
+
+        #region Prepare Feet
+
+        [NonSerialized] public float FeetStretchWeight = 1f;
+        [NonSerialized] public float FeetStretchSensitivity = 1f;
+        [NonSerialized] public float FeetStretchLimit = 1f;
+        [NonSerialized] public float FeetFadeQuicker = 1f;
+        [NonSerialized] public bool disableFeet = false;
+        float maxFeetAngle = 0f;
+        float maxFeetAngleFactor = 0f;
+        Vector3 ankleToFeet;
+        void PrepareFeet()
+        {
+            Vector3 kneeToAnkle = EndIKBone.transform.position - MiddleIKBone.transform.position;
+            kneeToAnkle.Normalize();
+
+            ankleToFeet = FeetIKBone.transform.position - EndIKBone.transform.position;
+            ankleToFeet.Normalize();
+
+            maxFeetAngle = Vector3.Angle(ankleToFeet, kneeToAnkle);
+            maxFeetAngleFactor = 90f / maxFeetAngle;
+
+            //if ( Root == null)
+            //{
+            //    UnityEngine.Debug.Log("[IK] Feet requires Root Transform defined!");
+            //    hasFeet = false;
+            //}
+        }
+
+        #endregion
+
+
+        internal void OffsetHeel(float heelRot, float feetCompensate = 1f)
+        {
+            if (hasFeet == false) return;
+            if (disableFeet) return;
+
+            Quaternion preAnkleRot = IKTargetRotation;
+            Vector3 toFeet = (FeetIKBone.transform.position - EndIKBone.transform.position);
+
+            Vector3 rotatedOffset = Quaternion.Inverse(preAnkleRot) * (toFeet);
+
+            Vector3 rightAxis;
+            if (UseEndBoneMapping) rightAxis = IKTargetRotation * Vector3.right;
+            else rightAxis = IKTargetRotation * (EndIKBone.right);// Root.right;
+
+            Quaternion rotationOffset = Quaternion.AngleAxis(heelRot * maxFeetAngle, rightAxis);
+            Quaternion newAnkleRot = rotationOffset * preAnkleRot;
+
+            IKTargetRotation = newAnkleRot;
+
+            Vector3 newOffset = newAnkleRot * rotatedOffset;
+            rotatedOffset = newOffset - toFeet;
+
+            if (feetCompensate > 0f)
+            {
+                Quaternion newFeetRot = Quaternion.Inverse(rotationOffset) * (FeetIKBone.transform.rotation);
+
+                if (feetCompensate >= 1f)
+                    FeetIKBone.transform.rotation = newFeetRot;
+                else
+                    FeetIKBone.transform.rotation = Quaternion.Lerp(FeetIKBone.transform.rotation, newFeetRot, feetCompensate);
+            }
+
+            IKTargetPosition -= rotatedOffset;
+        }
+
+
+    }
+}