acdream/src/AcDream.App/Rendering/Wb/GeometryUtils.cs

using System.Numerics;

// Phase O-T7: verbatim copy of WorldBuilder.Shared.Lib.GeometryUtils into
// the AcDream.App.Rendering.Wb namespace so the WorldBuilder.Shared project
// reference can be dropped.  Only the float-precision overloads are used by
// ObjectMeshManager (RayIntersectsSphere + RayIntersectsTriangle).  The
// double-precision overloads are kept verbatim for completeness.

namespace AcDream.App.Rendering.Wb;

public static class GeometryUtils {

    public static bool RayIntersectsBox(Vector3 rayOrigin, Vector3 rayDirection, Vector3 min, Vector3 max, out float distance) {
        distance = 0;
        float tmin = 0.0f;
        float tmax = float.MaxValue;

        if (Math.Abs(rayDirection.X) < 1e-7f) {
            if (rayOrigin.X < min.X || rayOrigin.X > max.X) return false;
        }
        else {
            float invD = 1.0f / rayDirection.X;
            float t0 = (min.X - rayOrigin.X) * invD;
            float t1 = (max.X - rayOrigin.X) * invD;
            if (t0 > t1) (t0, t1) = (t1, t0);
            tmin = Math.Max(tmin, t0);
            tmax = Math.Min(tmax, t1);
            if (tmin > tmax) return false;
        }

        if (Math.Abs(rayDirection.Y) < 1e-7f) {
            if (rayOrigin.Y < min.Y || rayOrigin.Y > max.Y) return false;
        }
        else {
            float invD = 1.0f / rayDirection.Y;
            float t0 = (min.Y - rayOrigin.Y) * invD;
            float t1 = (max.Y - rayOrigin.Y) * invD;
            if (t0 > t1) (t0, t1) = (t1, t0);
            tmin = Math.Max(tmin, t0);
            tmax = Math.Min(tmax, t1);
            if (tmin > tmax) return false;
        }

        if (Math.Abs(rayDirection.Z) < 1e-7f) {
            if (rayOrigin.Z < min.Z || rayOrigin.Z > max.Z) return false;
        }
        else {
            float invD = 1.0f / rayDirection.Z;
            float t0 = (min.Z - rayOrigin.Z) * invD;
            float t1 = (max.Z - rayOrigin.Z) * invD;
            if (t0 > t1) (t0, t1) = (t1, t0);
            tmin = Math.Max(tmin, t0);
            tmax = Math.Min(tmax, t1);
            if (tmin > tmax) return false;
        }

        distance = tmin;
        return true;
    }

    public static bool RayIntersectsTriangle(Vector3 origin, Vector3 direction, Vector3 v0, Vector3 v1, Vector3 v2, out float t) {
        t = 0;
        Vector3 edge1 = v1 - v0;
        Vector3 edge2 = v2 - v0;
        Vector3 h = Vector3.Cross(direction, edge2);
        float a = Vector3.Dot(edge1, h);

        if (a > -0.00001f && a < 0.00001f) return false;

        float f = 1.0f / a;
        Vector3 s = origin - v0;
        float u = f * Vector3.Dot(s, h);

        if (u < 0.0f || u > 1.0f) return false;

        Vector3 q = Vector3.Cross(s, edge1);
        float v = f * Vector3.Dot(direction, q);

        if (v < 0.0f || u + v > 1.0f) return false;

        t = f * Vector3.Dot(edge2, q);
        return t > 0.00001f;
    }

    public static bool RayIntersectsSphere(Vector3 rayOrigin, Vector3 rayDirection, Vector3 sphereOrigin, float sphereRadius, out float distance) {
        distance = 0;
        Vector3 l = sphereOrigin - rayOrigin;
        float tca = Vector3.Dot(l, rayDirection);
        if (tca < 0) return false;
        float d2 = Vector3.Dot(l, l) - tca * tca;
        float r2 = sphereRadius * sphereRadius;
        if (d2 > r2) return false;
        float thc = MathF.Sqrt(r2 - d2);
        distance = tca - thc;
        return true;
    }

    public static ushort PackKey(int x, int y) => (ushort)((x << 8) | y);

    /// <summary>
    /// Converts a quaternion to Euler angles (in degrees) using the ZYX convention.
    /// </summary>
    public static Vector3 QuaternionToEuler(Quaternion q) {
        float x = q.X, y = q.Y, z = q.Z, w = q.W;
        float roll, pitch, yaw;

        float sinr_cosp = 2 * (w * x + y * z);
        float cosr_cosp = 1 - 2 * (x * x + y * y);
        roll = (float)Math.Atan2(sinr_cosp, cosr_cosp);

        float sinp = 2 * (w * y - z * x);
        if (Math.Abs(sinp) >= 1)
            pitch = (float)Math.CopySign(Math.PI / 2, sinp);
        else
            pitch = (float)Math.Asin(sinp);

        float siny_cosp = 2 * (w * z + x * y);
        float cosy_cosp = 1 - 2 * (y * y + z * z);
        yaw = (float)Math.Atan2(siny_cosp, cosy_cosp);

        return new Vector3(roll, pitch, yaw) * (180.0f / (float)Math.PI);
    }

    /// <summary>
    /// Converts Euler angles (in degrees) to a quaternion using the ZYX convention.
    /// </summary>
    public static Quaternion EulerToQuaternion(Vector3 euler) {
        Vector3 rads = euler * (MathF.PI / 180.0f);
        float roll = rads.X;
        float pitch = rads.Y;
        float yaw = rads.Z;

        float cr = MathF.Cos(roll * 0.5f);
        float sr = MathF.Sin(roll * 0.5f);
        float cp = MathF.Cos(pitch * 0.5f);
        float sp = MathF.Sin(pitch * 0.5f);
        float cy = MathF.Cos(yaw * 0.5f);
        float sy = MathF.Sin(yaw * 0.5f);

        return new Quaternion(
            sr * cp * cy - cr * sp * sy,
            cr * sp * cy + sr * cp * sy,
            cr * cp * sy - sr * sp * cy,
            cr * cp * cy + sr * sp * sy
        );
    }
}