HumanoidControl_Free/Assets/Passer/Humanoid/Scripts/Movements/TorsoMovements.cs

using UnityEngine;

namespace Passer.Humanoid {

    public partial class HipsTarget {
        public float hipsBaseHeight;
    }

    public class TorsoMovements {
        private HipsTarget hipsTarget;

        private Breathing breathing = new Breathing();

        #region Start
        public void Start(HumanoidControl _humanoid, HipsTarget _target) {
            hipsTarget = _target;

            if (hipsTarget.hips.bone.transform == null)
                return;
        }
        #endregion

        #region Update

        public static void Update(HipsTarget hipsTarget) {
            if (hipsTarget.hips.bone.transform == null || !hipsTarget.humanoid.calculateBodyPose)
                return;

            if (Mathf.Abs(hipsTarget.hipsBaseHeight) < 0.00001F)
                hipsTarget.hipsBaseHeight = hipsTarget.hips.bone.transform.position.y - hipsTarget.hips.target.transform.position.y;

            HeadTarget headTarget = hipsTarget.humanoid.headTarget;
            HumanoidTarget.BoneTransform neckBone = (headTarget.neck.bone.transform != null) ? headTarget.neck.bone : headTarget.head.bone;
            Vector3 neckPosition = neckBone.transform.position;
            Quaternion neckRotation = neckBone.transform.rotation;
            Quaternion neckTargetRotation = neckRotation * neckBone.toTargetRotation * Quaternion.Inverse(headTarget.neck.bone.baseRotation);

            if (Application.isPlaying &&
                hipsTarget.humanoid.targetsRig.runtimeAnimatorController != null &&
                hipsTarget.hips.target.confidence.position == 0 &&
                hipsTarget.humanoid.headTarget.head.target.confidence.position == 0) {

                hipsTarget.torsoMovements.CharacterNoIK(hipsTarget, neckPosition);
            }
            else {
                if (hipsTarget.hips.target.confidence.rotation > 0.5F && hipsTarget.spine.target.confidence.rotation > 0.5F && hipsTarget.chest.target.confidence.rotation > 0.5F)
                    hipsTarget.torsoMovements.CharacterNoIK(hipsTarget, neckPosition);
                else if (!hipsTarget.newSpineIK)
                    hipsTarget.torsoMovements.NewIK(hipsTarget, neckPosition, neckTargetRotation);
                else
                    hipsTarget.torsoMovements.SimpleIK(hipsTarget);

                if (Application.isPlaying)
                    hipsTarget.torsoMovements.breathing.Update();

                neckBone.transform.position = neckPosition;
                neckBone.transform.rotation = neckRotation;
            }
        }

        public void CharacterNoIK(HipsTarget hipsTarget, Vector3 neckBonePosition) {
            hipsTarget.hips.SetBoneRotation(hipsTarget.hips.target.transform.rotation);
            hipsTarget.spine.SetBoneRotation(hipsTarget.spine.target.transform.rotation);
            hipsTarget.chest.SetBoneRotation(hipsTarget.chest.target.transform.rotation);

            if (hipsTarget.humanoid.headTarget.head.target.confidence.position > 0) {
                if (hipsTarget.chest.bone.transform != null) {
                    Vector3 chestTopPosition = hipsTarget.chest.bone.transform.position + hipsTarget.chest.bone.targetRotation * Vector3.up * hipsTarget.chest.bone.length;
                    Vector3 spineVector = chestTopPosition - hipsTarget.hips.bone.transform.position;
                    hipsTarget.hips.SetBonePosition(neckBonePosition - spineVector);
                }
                else if (hipsTarget.spine.bone.transform != null) {
                    Vector3 spineTopPosition = hipsTarget.spine.bone.transform.position + hipsTarget.spine.bone.targetRotation * Vector3.up * hipsTarget.spine.bone.length;
                    Vector3 spineVector = spineTopPosition - hipsTarget.hips.bone.transform.position;
                    hipsTarget.hips.SetBonePosition(neckBonePosition - spineVector);
                }
            }
            else {
                Vector3 hipsPosition = hipsTarget.hips.target.transform.position + hipsTarget.hipsBaseHeight * Vector3.up;
                hipsTarget.hips.SetBonePosition(hipsPosition);
            }
        }

        private void SimpleIK(HipsTarget hipsTarget) {
            // This implementation ignores the spine and chest bones
            // Which results in a stiff spine
            Vector3 neckTargetPosition = hipsTarget.humanoid.headTarget.neck.target.transform.position;
            Quaternion neckRotation = hipsTarget.humanoid.headTarget.neck.bone.transform.rotation;

            Vector3 hipsTargetPosition = hipsTarget.hips.target.transform.position;

            Quaternion hipsTargetRotation = hipsTarget.hips.target.transform.rotation;

            Vector3 hipsBack = hipsTargetRotation * Vector3.back;
            Vector3 hipsUp = neckTargetPosition - hipsTargetPosition;

            if (hipsUp.sqrMagnitude <= 0.0001F)
                hipsUp = hipsTarget.humanoid.up;

            Quaternion spineRotation = Quaternion.LookRotation(hipsUp, hipsBack) * Quaternion.Euler(90, 0, 0);
            Quaternion hipsRotation = spineRotation * hipsTarget.spine2HipsRotation;
            hipsTarget.hips.SetBoneRotation(hipsRotation);

            Vector3 spineVector = spineRotation * Vector3.up * hipsTarget.torsoLength;
            hipsTarget.hips.SetBonePosition(hipsTarget.humanoid.headTarget.neck.target.transform.position - spineVector);

            hipsTarget.humanoid.headTarget.neck.bone.transform.rotation = neckRotation;
        }

        //float bendingFactor = 1;

        private void NewIK(HipsTarget hipsTarget, Vector3 neckBonePosition, Quaternion neckBoneTargetRotation) {
            Quaternion hipsTargetRotation;
            if (!Application.isPlaying || hipsTarget.humanoid.primaryTarget == HumanoidControl.PrimaryTarget.Head) {
                hipsTargetRotation = CalculateHipsRotation(hipsTarget, neckBonePosition);
                hipsTarget.hips.SetBoneRotation(hipsTargetRotation);

                hipsTargetRotation *= Quaternion.Inverse(hipsTarget.hips.bone.baseRotation);

                Quaternion spineTargetRotation = CalculateSpineRotation(hipsTarget, hipsTarget.spine.bone.baseRotation, hipsTargetRotation, neckBoneTargetRotation);
                hipsTarget.spine.SetBoneRotation(spineTargetRotation);

                Quaternion chestTargetRotation = CalculateChestRotation(hipsTarget, hipsTarget.chest.bone.baseRotation, hipsTargetRotation, neckBoneTargetRotation);
                hipsTarget.chest.SetBoneRotation(chestTargetRotation);

                CalculateHipsPositionFromHead(hipsTarget, neckBonePosition);
            }
            else {
                HeadTarget headTarget = hipsTarget.humanoid.headTarget;
                hipsTargetRotation = CalculateHipsRotation(hipsTarget, headTarget.head.target.transform.position);
                hipsTarget.hips.SetBoneRotation(hipsTargetRotation);

                Quaternion spineTargetRotation = CalculateSpineLookRotation(hipsTarget, hipsTarget.spine.bone.baseRotation, hipsTargetRotation);
                hipsTarget.spine.SetBoneRotation(spineTargetRotation);

                Quaternion chestTargetRotation = CalculateChestLookRotation(hipsTarget, hipsTarget.chest.bone.baseRotation, hipsTargetRotation);
                hipsTarget.chest.SetBoneRotation(chestTargetRotation);

                CalculateHipsPositionFromTarget(hipsTarget);
            }
        }

        private void CalculateHipsPositionFromHead(HipsTarget hipsTarget, Vector3 neckBonePosition) {
            if (hipsTarget.chest.bone.transform != null) {
                Vector3 chestTopPosition = hipsTarget.chest.bone.transform.position + hipsTarget.chest.bone.targetRotation * Vector3.up * hipsTarget.chest.bone.length;
                Vector3 spineVector = chestTopPosition - hipsTarget.hips.bone.transform.position;
                hipsTarget.hips.SetBonePosition(neckBonePosition - spineVector);
            }
            else if (hipsTarget.spine.bone.transform != null) {
                Vector3 spineTopPosition = hipsTarget.spine.bone.transform.position + hipsTarget.spine.bone.targetRotation * Vector3.up * hipsTarget.spine.bone.length;
                Vector3 spineVector = spineTopPosition - hipsTarget.hips.bone.transform.position;
                hipsTarget.hips.SetBonePosition(neckBonePosition - spineVector);
            }
        }

        private void CalculateHipsPositionFromTarget(HipsTarget hipsTarget) {
            hipsTarget.hips.SetBonePosition(hipsTarget.hips.target.transform.position);
        }

        #endregion

        #region Chest

        public Quaternion CalculateChestRotation(HipsTarget hipsTarget, Quaternion baseRotation, Quaternion hipsRotation, Quaternion neckRotation) {
            float factor = hipsTarget.bendingFactor * 0.3F;
            Quaternion chestRotation = Quaternion.Slerp(neckRotation, hipsRotation, factor) * baseRotation;
            return chestRotation;
        }
        public Quaternion CalculateChestLookRotation(HipsTarget hipsTarget, Quaternion baseRotation, Quaternion hipsRotation) {
            Quaternion spineRotation = hipsRotation;
            return spineRotation;

        }

        #endregion

        #region Spine

        public Quaternion CalculateSpineRotation(HipsTarget hipsTarget, Quaternion baseRotation, Quaternion hipsRotation, Quaternion neckRotation) {
            float factor = hipsTarget.chest.bone.transform != null ? 0.4F : 0.3F;
            factor *= hipsTarget.bendingFactor;
            Quaternion spineRotation = Quaternion.Slerp(neckRotation, hipsRotation, factor) * baseRotation;
            return spineRotation;
        }

        public Quaternion CalculateSpineLookRotation(HipsTarget hipsTarget, Quaternion baseRotation, Quaternion hipsRotation) {
            Quaternion spineRotation = hipsRotation;
            return spineRotation;
        }

        public static Vector3 CalculateSpinePosition(Vector3 chestPosition, Quaternion spineRotation, float spineLength) {
            Vector3 spinePosition = chestPosition - spineRotation * Vector3.up * spineLength;
            return spinePosition;
        }

        #endregion

        #region Hips

        private Quaternion CalculateHipsRotation(HipsTarget hipsTarget, Vector3 neckBonePosition) {
            if (hipsTarget.hips.target.transform == null)
                return Quaternion.identity;

            Vector3 torsoUp = neckBonePosition - hipsTarget.hips.target.transform.position;
            Quaternion spine2HipsRotation = Rotations.Rotate(hipsTarget.spine2HipsRotation, hipsTarget.hips.target.transform.rotation);
            Vector3 hipsUp = spine2HipsRotation * torsoUp;

            Vector3 hipsForward = hipsTarget.hips.target.transform.forward;

            if (hipsUp == Vector3.zero)
                return Quaternion.identity;

            Quaternion hipsRotation = Quaternion.LookRotation(hipsUp, -hipsForward) * Quaternion.AngleAxis(90, Vector3.right);

            return hipsRotation;
        }

        #endregion


        public static Vector3 CalculateNeckPosition(Vector3 chestPosition, Quaternion chestRotation, Vector3 chest2neck) {
            Vector3 neckPosition = chestPosition + chestRotation * chest2neck;
            return neckPosition;
        }

        public static Quaternion CalculateHipsRotation(Vector3 hipsPosition, Quaternion hipsRotation, Quaternion leftHandRotation, Quaternion rightHandRotation, Transform leftFoot, Transform rightFoot, Quaternion neckRotation, Vector3 neckPosition) {
            float angleX = hipsRotation.eulerAngles.x;
            float angleY = hipsRotation.eulerAngles.y;
            float angleZ = hipsRotation.eulerAngles.z;

            float dOrientation = 0;

            Vector3 leftHandForward = leftHandRotation * Vector3.left;
            Vector3 rightHandForward = rightHandRotation * Vector3.right;
            Vector3 hipsForward = hipsRotation * Vector3.forward;

            Vector2 leftHandForward2 = new Vector2(leftHandForward.x, leftHandForward.z);
            Vector2 rightHandForward2 = new Vector2(rightHandForward.x, rightHandForward.z);
            Vector2 hipsRotation2 = new Vector2(hipsForward.x, hipsForward.z);

            // Check for hands not pointing up/down too much
            float dOrientationL = leftHandForward2.sqrMagnitude > 0.1F ? UnityAngles.SignedAngle(hipsRotation2, leftHandForward2) : 0;
            float dOrientationR = rightHandForward2.sqrMagnitude > 0.1F ? UnityAngles.SignedAngle(hipsRotation2, rightHandForward2) : 0;
            if (Mathf.Sign(dOrientationL) == Mathf.Sign(dOrientationR)) {
                if (Mathf.Abs(dOrientationL) < Mathf.Abs(dOrientationR))
                    dOrientation = dOrientationL;
                else
                    dOrientation = dOrientationR;
            }

            float neckOrientation = UnityAngles.Difference(neckRotation.eulerAngles.y, angleY + dOrientation);
            if (neckOrientation < 90 && neckOrientation > -90) { // head cannot turn more than 90 degrees
                angleY += dOrientation;
            }

            Quaternion newHipsRotation = Quaternion.Euler(angleX, angleY, angleZ);
            return newHipsRotation;
        }

        public static Vector3 CalculateHipsPosition(Vector3 spinePosition, Quaternion hipsRotation, float hipsLength, Vector3 leftFootPosition, Vector3 rightFootPosition) {
            Vector3 hipsPosition = spinePosition - hipsRotation * Vector3.up * hipsLength;

            // stationary
            /*
            float footHeightDifference = leftFootPosition.y - rightFootPosition.y;

            Vector3 centerFoot;
            if (footHeightDifference > maxFeetdifference) {
                //standing on right foot
                centerFoot = rightFootPosition;
            } else if (footHeightDifference < -maxFeetdifference) {
                //standing on left foot
                centerFoot = leftFootPosition;
            } else {
                //standing on both feet
                float fraction = footHeightDifference * (0.5F / maxFeetdifference) + 0.5F;
                centerFoot = leftFootPosition * (1 - fraction) + rightFootPosition * fraction;
            }
            hipsPosition = new Vector3(centerFoot.x, hipsPosition.y, centerFoot.z);
            */
            return hipsPosition;
        }

    }

    public class Breathing {
        public float speed = 4;
        public float intensity = 1;

        private float f;
        public float v;

        public void Update() {
            f = CalculateF();
            v = Mathf.Sin(f * (Mathf.PI + Mathf.PI)) * intensity;
        }

        private float lastBreathStart;
        private float CalculateF() {
            float f = (Time.realtimeSinceStartup - lastBreathStart) / speed;
            if (f > 1) {
                lastBreathStart = Time.realtimeSinceStartup;
                f = 0;
            }

            return f;
        }
    }

}