streamingle-unity-utilities/Runtime/Gaze/GazeDirectionSolver.cs

using System.Collections.Generic;
using UnityEngine;

namespace Streamingle.Gaze
{
    /// <summary>
    /// Pure direction and 3x3 interpolation helpers. Kept independent of the
    /// component so they can also be reused by a future real-time input source.
    /// </summary>
    public static class GazeDirectionSolver
    {
        private const float MinimumAngleRange = 0.001f;

        public static GazeDirectionResult Calculate(
            Quaternion referenceRotation,
            Vector3 worldDirection,
            float minYaw,
            float maxYaw,
            float minPitch,
            float maxPitch)
        {
            if (worldDirection.sqrMagnitude <= Mathf.Epsilon)
                worldDirection = referenceRotation * Vector3.forward;
            else
                worldDirection.Normalize();

            var localDirection = Quaternion.Inverse(referenceRotation) * worldDirection;
            if (localDirection.sqrMagnitude <= Mathf.Epsilon)
                localDirection = Vector3.forward;
            else
                localDirection.Normalize();

            var planarLength = Mathf.Sqrt(
                localDirection.x * localDirection.x + localDirection.z * localDirection.z);
            var rawYaw = Mathf.Atan2(localDirection.x, localDirection.z) * Mathf.Rad2Deg;
            var rawPitch = Mathf.Atan2(localDirection.y, planarLength) * Mathf.Rad2Deg;

            NormalizeRanges(ref minYaw, ref maxYaw, ref minPitch, ref maxPitch);
            var clampedYaw = Mathf.Clamp(rawYaw, minYaw, maxYaw);
            var clampedPitch = Mathf.Clamp(rawPitch, minPitch, maxPitch);

            return new GazeDirectionResult(
                worldDirection,
                localDirection,
                rawYaw,
                rawPitch,
                clampedYaw,
                clampedPitch,
                NormalizeAsymmetric(clampedYaw, minYaw, maxYaw),
                NormalizeAsymmetric(clampedPitch, minPitch, maxPitch));
        }

        public static Vector3 DirectionFromAngles(float yaw, float pitch)
        {
            return Quaternion.Euler(-pitch, yaw, 0f) * Vector3.forward;
        }

        public static float NormalizeAsymmetric(float value, float negativeLimit, float positiveLimit)
        {
            if (value < 0f)
                return Mathf.Clamp(value / Mathf.Max(MinimumAngleRange, -negativeLimit), -1f, 0f);

            return Mathf.Clamp(value / Mathf.Max(MinimumAngleRange, positiveLimit), 0f, 1f);
        }

        public static void Evaluate3x3(
            IReadOnlyList<GazeCalibrationSample> samples,
            int channelCount,
            float normalizedYaw,
            float normalizedPitch,
            float[] output)
        {
            if (output == null || output.Length < channelCount)
                return;

            normalizedYaw = Mathf.Clamp(normalizedYaw, -1f, 1f);
            normalizedPitch = Mathf.Clamp(normalizedPitch, -1f, 1f);

            var x0 = normalizedYaw < 0f ? 0 : 1;
            var x1 = x0 + 1;
            var y0 = normalizedPitch < 0f ? 0 : 1;
            var y1 = y0 + 1;
            var tx = normalizedYaw < 0f ? normalizedYaw + 1f : normalizedYaw;
            var ty = normalizedPitch < 0f ? normalizedPitch + 1f : normalizedPitch;

            var bottomLeft = GetSample(samples, y0 * 3 + x0);
            var bottomRight = GetSample(samples, y0 * 3 + x1);
            var topLeft = GetSample(samples, y1 * 3 + x0);
            var topRight = GetSample(samples, y1 * 3 + x1);

            for (var channel = 0; channel < channelCount; channel++)
            {
                var bottom = Mathf.Lerp(
                    bottomLeft != null ? bottomLeft.GetWeight(channel) : 0f,
                    bottomRight != null ? bottomRight.GetWeight(channel) : 0f,
                    tx);
                var top = Mathf.Lerp(
                    topLeft != null ? topLeft.GetWeight(channel) : 0f,
                    topRight != null ? topRight.GetWeight(channel) : 0f,
                    tx);
                output[channel] = Mathf.Lerp(bottom, top, ty);
            }
        }

        private static GazeCalibrationSample GetSample(
            IReadOnlyList<GazeCalibrationSample> samples,
            int index)
        {
            return samples != null && index >= 0 && index < samples.Count ? samples[index] : null;
        }

        private static void NormalizeRanges(
            ref float minYaw,
            ref float maxYaw,
            ref float minPitch,
            ref float maxPitch)
        {
            minYaw = Mathf.Min(-MinimumAngleRange, minYaw);
            maxYaw = Mathf.Max(MinimumAngleRange, maxYaw);
            minPitch = Mathf.Min(-MinimumAngleRange, minPitch);
            maxPitch = Mathf.Max(MinimumAngleRange, maxPitch);
        }
    }
}