Operations.cs

using Unity.Mathematics;
using static Unity.Mathematics.math;
using static Unity.Mathematics.quaternion;

namespace Orbiter.Math
{
    public static class Operations
    {
        public static float3 transform_l2w(float4x4 m, float3 v)
        {
            return mul3x4(m, v);
        }

        public static float3 transform_w2l(float4x4 m, float3 v)
        {
            return mul3x4(inverse(m), v);
        }

        public static float3 mul3x4(float4x4 m, float3 v)
        {
            return float3(
                (m.c0.x * v.x + m.c1.x * v.y + m.c2.x * v.z) + m.c3.x,
                (m.c0.y * v.x + m.c1.y * v.y + m.c2.y * v.z) + m.c3.y,
                (m.c0.z * v.x + m.c1.z * v.y + m.c2.z * v.z) + m.c3.z);
        }

        public static float angle(float3 from, float3 to)
        {
            return acos(dot(from, to) / (length(from) * length(to)));
        }

        public static quaternion rotation_from_to(float3 from, float3 to)
        {
            float3 axis = cross(from, to);
            float a = angle(from, to);
            return AxisAngle(normalize(axis), a);
        }

        public static float3 project(float3 vector, float3 onNormal)
        {
            float num = dot(onNormal, onNormal);
            if (num < 1.40129846432482E-45f)
                return 0;
            else
                return onNormal * dot(vector, onNormal) / num;
        }

        public static double3 project(double3 vector, double3 onNormal)
        {
            double num = dot(onNormal, onNormal);
            if (num < 1.40129846432482E-45d)
                return 0;
            else
                return onNormal * dot(vector, onNormal) / num;
        }

        public static float3 project_on_plane(float3 vector, float3 planeNormal)
        {
            return vector - project(vector, planeNormal);
        }

        public static double3 project_on_plane(double3 vector, double3 planeNormal)
        {
            return vector - project(vector, planeNormal);
        }

        //Get the intersection between a line and a plane. 
        //If the line and plane are not parallel, the function outputs true, otherwise false.
        public static bool intersect_plane(out float3 intersection, float3 linePoint, float3 lineVec, float3 planeNormal, float3 planePoint)
        {

            float length;
            float dotNumerator;
            float dotDenominator;
            float3 vector;
            intersection = float3(0, 0, 0);

            //calculate the distance between the linePoint and the line-plane intersection point
            dotNumerator = dot((planePoint - linePoint), planeNormal);
            dotDenominator = dot(lineVec, planeNormal);

            //line and plane are not parallel
            if (dotDenominator != 0.0f)
            {
                length = dotNumerator / dotDenominator;

                //create a vector from the linePoint to the intersection point
                float3 dir = normalize(lineVec);
                vector = lineVec * length;

                //get the coordinates of the line-plane intersection point
                intersection = linePoint + vector;

                return true;
            }
            else
            {
                return false;
            }
        }
    }
}