NanoBrain-unitypackage/CreatureControl/Runtime/Scripts/TargetLeg.cs

using UnityEngine;

namespace Passer.CreatureControl {

    [System.Serializable]
    public class TargetLeg : MonoBehaviour {
        public Transform femurTarget; // UpperLeg, Thigh
        public Transform tibiaTarget; // LowerLeg, Shank
        public Transform tarsusTarget; // Foot

        public Transform target; // for the tarsus
        protected LegTarget legTarget;

        public Quaternion targetToBoneFemur;
        public Quaternion targetToBoneTibia;

        public virtual void OnDrawGizmosSelected() {
            if (target != null && legTarget == null) {
                legTarget = target.GetComponent<LegTarget>();
                if (legTarget == null)
                    legTarget = target.gameObject.AddComponent<LegTarget>();
                legTarget.leg = this;
            }

            Gizmos.color = Color.white;
            if (this.femurTarget != null && this.tibiaTarget != null)
                Gizmos.DrawLine(this.femurTarget.position, this.tibiaTarget.position);
            if (tibiaTarget != null && this.tarsusTarget != null)
                Gizmos.DrawLine(this.tibiaTarget.position, this.tarsusTarget.position);

            if (this.enabled)
                PoseLimb();
        }

        public void PoseLimb() {
            if (target == null)
                return;

            Quaternion femurOrientation = FemurRotation(target.position);
            Quaternion tibiaOrientation = TibiaRotation(target.position);
            Quaternion tarsusrientation = TarsusRotation(target.rotation);

            SetFemurOrientation(femurTarget, femurOrientation);
            SetTibiaOrientation(tibiaTarget, tibiaOrientation);
            SetTarsusOrientation(tarsusTarget, tarsusrientation);
        }

        protected Quaternion FemurRotation(Vector3 targetPosition) {
            if (this.femurTarget == null || this.tibiaTarget == null || this.tarsusTarget == null)
                return Quaternion.identity;

            Vector3 toTarget = targetPosition - this.femurTarget.position;
            // Debug.DrawRay(femur.position, toTarget, Color.magenta);
            float targetDistance = toTarget.magnitude;
            float femurLength = Vector3.Distance(this.femurTarget.position, this.tibiaTarget.position);
            float tibiaLength = Vector3.Distance(this.tibiaTarget.position, this.tarsusTarget.position);

            float hipAngle = CosineRule(targetDistance, femurLength, tibiaLength);
            // NaN happens when the distance to the footTarget is longer than the length of the leg
            // We will stretch the leg full then (angle = 0)
            if (float.IsNaN(hipAngle))
                hipAngle = 0;

            Quaternion femurOrientation = Quaternion.LookRotation(toTarget, Vector3.up);
            femurOrientation = Quaternion.AngleAxis(hipAngle, femurOrientation * Vector3.left) * femurOrientation;
            // Debug.DrawRay(femur.position, femurOrientation * Vector3.forward, Color.blue);
            // Debug.DrawRay(femur.position, femurOrientation * Vector3.up, Color.green);

            return femurOrientation;
        }

        protected Quaternion TibiaRotation(Vector3 targetPosition) {
            if (this.tibiaTarget == null)
                return Quaternion.identity;

            Vector3 directionToTarget = targetPosition - this.tibiaTarget.position;

            Quaternion tibiaOrientation = Quaternion.LookRotation(directionToTarget, Vector3.up); // femur.up);
            return tibiaOrientation; // In world space
        }

        protected Quaternion TarsusRotation(Quaternion targetRotation) {
            return targetRotation;
        }


        private void SetFemurOrientation(Transform bone, Quaternion orientation) {
            if (this.femurTarget == null)
                return;
            // Debug.DrawRay(femur.position, orientation * Vector3.forward, Color.blue);
            // Debug.DrawRay(femur.position, orientation * Vector3.up, Color.green);
            bone.rotation = orientation;
        }

        private void SetTibiaOrientation(Transform bone, Quaternion orientation) {
            if (this.tibiaTarget == null)
                return;

            bone.rotation = orientation;
        }

        private void SetTarsusOrientation(Transform bone, Quaternion orientation) {
            if (this.tarsusTarget == null)
                return;

            bone.rotation = orientation;
        }


        // public void MatchToCreature(Transform femurBone, Transform tibiaBone, Transform tarsusBone) {
        //     TargetRig.MoveTargetToBone(femurTarget, femurBone);
        //     TargetRig.MoveTargetToBone(tibiaTarget, tibiaBone);
        //     TargetRig.MoveTargetToBone(tarsusTarget, tarsusBone);
        //     targetToBoneFemur = TargetRig.TargetToBoneRotation(femurBone, tibiaBone);
        //     targetToBoneTibia = TargetRig.TargetToBoneRotation(tibiaBone, tarsusBone);
        // }
        public void MatchToCreature(Leg leg) {
            TargetRig.MoveTargetToBone(this.femurTarget, leg.femur);
            TargetRig.MoveTargetToBone(this.tibiaTarget, leg.tibia);
            TargetRig.MoveTargetToBone(this.tarsusTarget, leg.tarsus);
            targetToBoneFemur = TargetRig.TargetToBoneRotation(leg.femur, leg.tibia);
            targetToBoneTibia = TargetRig.TargetToBoneRotation(leg.tibia, leg.tarsus);
        }

        public void UpdateFemur(Transform femurBone) {
            if (femurBone == null || this.femurTarget == null)
                return;

            femurBone.rotation = this.femurTarget.rotation * targetToBoneFemur;
        }

        public void UpdateTibia(Transform tibiaBone) {
            if (tibiaBone == null || this.tibiaTarget == null)
                return;

            tibiaBone.rotation = this.tibiaTarget.rotation * targetToBoneTibia;
        }

        // public void UpdateBones(Transform femurBone, Transform tibiaBone) {
        //     UpdateFemur(femurBone);
        //     UpdateTibia(tibiaBone);
        // }
        public void UpdateBones(Leg leg) {
            UpdateFemur(leg.femur);
            UpdateTibia(leg.tibia);
        }

        #region Math

        public static float CosineRule(float a, float b, float c) {
            float a2 = a * a;
            float b2 = b * b;
            float c2 = c * c;

            double angle = System.Math.Acos((a2 + b2 - c2) / (2 * a * b)) * Mathf.Rad2Deg;
            if (double.IsNaN(angle))
                angle = 0;
            return (float)angle;
        }

        private static float Square(float x) {
            return x * x;
        }

        #endregion Math
    }

}