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

using UnityEngine;

namespace CreatureControl {

    /// <summary>
    /// A leg in a TargetRig which is used to control a leg in a model
    /// </summary>
    [System.Serializable]
    public class TargetLeg : MonoBehaviour {
        public Leg bones;

        public Transform target; // for the tarsus

        public Quaternion targetToBoneFemur;
        public Quaternion targetToBoneTibia;

        public void MatchTo(Leg leg) {
            if (this.bones.femur == null || this.bones.tibia == null || this.bones.tarsus == null)
                return;
            if (leg.femur == null || leg.tibia == null || leg.tarsus == null)
                return;

            this.bones.femur.position = leg.femur.position;
            this.bones.tibia.position = leg.tibia.position;
            this.bones.tarsus.position = leg.tarsus.position;

            targetToBoneFemur = TargetRig.TargetToBoneRotation(leg.femur, leg.tibia);
            targetToBoneTibia = TargetRig.TargetToBoneRotation(leg.tibia, leg.tarsus);

            float modelLegLength = leg.length;
            float targetLegLength = this.bones.length;
            Debug.Log($"model: {modelLegLength} rig: {targetLegLength}");

            // Put the end-effector target for IK in a sensible place
            Vector3 legDirection = (this.bones.tarsus.position - this.bones.femur.position).normalized;
            Vector3 targetPosition = this.bones.femur.position + 0.7f * this.bones.length * legDirection.normalized;
            Quaternion targetRotation = Quaternion.LookRotation(legDirection);
            this.target.SetPositionAndRotation(targetPosition, targetRotation);
            this.target.localPosition = new(this.target.localPosition.x, 0, this.target.localPosition.z);
        }

        /// <summary>
        /// Pose the target limb
        /// </summary>
        public void PoseLimb() {
            if (target == null)
                return;
            if (bones.femur == null || bones.tibia == null || bones.tarsus == null)
                return;

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

            bones.femur.rotation = femurOrientation;
            bones.tibia.rotation = tibiaOrientation;
            bones.tarsus.rotation = tarsusOrientation;
        }

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

        protected Quaternion FemurRotation(Vector3 targetPosition) {
            if (this.bones.femur == null || this.bones.tibia == null || this.bones.tarsus == null)
                return Quaternion.identity;

            Vector3 toTarget = targetPosition - this.bones.femur.position;
            // Debug.DrawRay(femur.position, toTarget, Color.magenta);
            float targetDistance = toTarget.magnitude;
            float femurLength = Vector3.Distance(this.bones.femur.position, this.bones.tibia.position);
            float tibiaLength = Vector3.Distance(this.bones.tibia.position, this.bones.tarsus.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.bones.tibia == null)
                return Quaternion.identity;

            Vector3 directionToTarget = targetPosition - this.bones.tibia.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.bones.femur == null)
                return;

            femurBone.rotation = this.bones.femur.rotation * targetToBoneFemur;
        }

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

            tibiaBone.rotation = this.bones.tibia.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

        #region Scene

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

            Gizmos.color = Color.white;
            if (this.bones.femur != null && this.bones.tibia != null)
                Gizmos.DrawLine(this.bones.femur.position, this.bones.tibia.position);
            if (bones.tibia != null && this.bones.tarsus != null)
                Gizmos.DrawLine(this.bones.tibia.position, this.bones.tarsus.position);

            PoseLimb();
        }

        #endregion Scene
    }

}