RoboidControl-csharp/LinearAlgebra/src/Vector3.cs

#if !UNITY_5_3_OR_NEWER
using System;

namespace LinearAlgebra {
    public class Vector3Of<T> {
        public T x;
        public T y;
        public T z;

        public Vector3Of(T x, T y, T z) {
            this.x = x;
            this.y = y;
            this.z = z;
        }

        // public uint magnitude {
        //     get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
        // }
    }

    public class Vector3Int : Vector3Of<int> {
        public Vector3Int(int x, int y, int z) : base(x, y, z) { }
    }
    public class Vector3Float : Vector3Of<float> {
        public Vector3Float(float x, float y, float z) : base(x, y, z) { }

        public float magnitude {
            get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
        }
    }

    /// <summary>
    /// 3-dimensional vectors
    /// </summary>
    /// This uses the right-handed coordinate system.
    public struct Vector3 : IEquatable<Vector3> {

        /// <summary>
        /// The right axis of the vector
        /// </summary>
        public float x; //> left/right
        /// <summary>
        /// The upward axis of the vector
        /// </summary>
        public float y; //> up/down
        /// <summary>
        /// The forward axis of the vector
        /// </summary>
        public float z; //> forward/backward

        /// <summary>
        /// Create a new 3-dimensional vector
        /// </summary>
        /// <param name="x">x axis value</param>
        /// <param name="y">y axis value</param>
        /// <param name="z">z axis value</param>
        public Vector3(float x, float y, float z) {
            this.x = x;
            this.y = y;
            this.z = z;
        }

        /// <summary>
        /// A vector with zero for all axis
        /// </summary>
        public static readonly Vector3 zero = new Vector3(0, 0, 0);
        /// <summary>
        /// A vector with one for all axis
        /// </summary>
        public static readonly Vector3 one = new Vector3(1, 1, 1);
        /// <summary>
        /// A vector3 with values (-1, 0, 0)
        /// </summary>
        public static readonly Vector3 left = new Vector3(-1, 0, 0);
        /// <summary>
        /// A vector with values (1, 0, 0)
        /// </summary>
        public static readonly Vector3 right = new Vector3(1, 0, 0);
        /// <summary>
        /// A vector with values (0, -1, 0)
        /// </summary>
        public static readonly Vector3 down = new Vector3(0, -1, 0);
        /// <summary>
        /// A vector with values (0, 1, 0)
        /// </summary>
        public static readonly Vector3 up = new Vector3(0, 1, 0);
        /// <summary>
        /// A vector with values (0, 0, -1)
        /// </summary>
        public static readonly Vector3 back = new Vector3(0, -1, 0);
        /// <summary>
        /// A vector with values (0, 0, 1)
        /// </summary>
        public static readonly Vector3 forward = new Vector3(0, 1, 0);

        public readonly float magnitude {
            get {
                float d = (float)Math.Sqrt(x * x + y * y + z * z);
                return d;
            }
        }

        public Vector3 normalized {
            get {
                float l = magnitude;
                Vector3 v = zero;
                if (l > Float.epsilon)
                    v = this / l;
                return v;
            }
        }

        public static Vector3 operator +(Vector3 v1, Vector3 v2) {
            Vector3 v = new Vector3(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);
            return v;
        }

        public static Vector3 operator -(Vector3 v1, Vector3 v2) {
            Vector3 v = new Vector3(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
            return v;
        }

        public static Vector3 operator -(Vector3 v1) {
            Vector3 v = new Vector3(-v1.x, -v1.y, -v1.z);
            return v;
        }

        public static Vector3 operator *(Vector3 v1, float d) {
            Vector3 v = new Vector3(v1.x * d, v1.y * d, v1.z * d);
            return v;
        }

        public static Vector3 operator *(float d, Vector3 v1) {
            Vector3 v = new Vector3(d * v1.x, d * v1.y, d * v1.z);
            return v;
        }

        public static Vector3 operator /(Vector3 v1, float d) {
            Vector3 v = new Vector3(v1.x / d, v1.y / d, v1.z / d);
            return v;
        }

        public bool Equals(Vector3 v) => (x == v.x && y == v.y && z == v.z);

        public override bool Equals(object obj) {
            if (!(obj is Vector3 v))
                return false;

            return (x == v.x && y == v.y && z == v.z);
        }

        public static bool operator ==(Vector3 v1, Vector3 v2) {
            return (v1.x == v2.x && v1.y == v2.y && v1.z == v2.z);
        }

        public static bool operator !=(Vector3 v1, Vector3 v2) {
            return (v1.x != v2.x || v1.y != v2.y || v1.z != v2.z);
        }

        public override int GetHashCode() {
            return (x, y, z).GetHashCode();
        }

        public static float Distance(Vector3 v1, Vector3 v2) {
            return (v2 - v1).magnitude;
        }

        public static float Dot(Vector3 v1, Vector3 v2) {
            return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
        }

        public static Vector3 Lerp(Vector3 v1, Vector3 v2, float f) {
            Vector3 v = v1 + (v2 - v1) * f;
            return v;
        }

    }
}
#endif