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 void MatchTo(Leg leg) {
            this.femurTarget.position = leg.femur.position;
            this.tibiaTarget.position = leg.tibia.position;
            this.tarsusTarget.position = leg.tarsus.position;
            targetToBoneFemur = TargetRig.TargetToBoneRotation(leg.femur, leg.tibia);
            targetToBoneTibia = TargetRig.TargetToBoneRotation(leg.tibia, leg.tarsus);

            this.target.SetPositionAndRotation(this.tarsusTarget.position, this.tarsusTarget.rotation);
        }

        public virtual void OnDrawGizmosSelected() {
            if (this.enabled == false)
                return;

            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);

            PoseLimb();
        }

        /// <summary>
        /// Pose the target limb
        /// </summary>
        public void PoseLimb() {
            if (target == null)
                return;

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

            femurTarget.rotation = femurOrientation;
            tibiaTarget.rotation = tibiaOrientation;
            tarsusTarget.rotation = tarsusOrientation;
        }

        public void UpdateBones(Leg leg) {
            UpdateFemur(leg.femur);
            UpdateTibia(leg.tibia);
        }

        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;
        }

        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;
        }

        #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;
        }

        #endregion Math
    }

}