Crafter.Math/interfaces/Crafter.Math-Vector.cppm

//SPDX-License-Identifier: LGPL-3.0-only
//SPDX-FileCopyrightText: Copyright (C) 2026 Catcrafts®

export module Crafter.Math:Vector;
import std;

namespace Crafter {
	template <typename T, std::uint32_t Len, std::uint32_t Aligment>
	struct __attribute__((packed)) VectorBase {
		T v[Aligment];
	};
	
	template <typename T, std::uint32_t Aligment>
	struct __attribute__((packed)) VectorBase<T, 1, Aligment> {
		union {
			T v[Aligment];
			T x, r;
		};
	};
	
	template <typename T, std::uint32_t Aligment>
	struct __attribute__((packed)) VectorBase<T, 2, Aligment> {
		union {
			T v[Aligment];
			struct __attribute__((packed)) {
				T x, y;
			};
            struct __attribute__((packed)) {
				T r, g;
			};
		};
		constexpr VectorBase<T, 2, Aligment>(T x, T y): x(x), y(y) {
			
		}
		constexpr VectorBase<T, 2, Aligment>() = default;
	};

	template <typename T, std::uint32_t Aligment>
	struct __attribute__((packed)) VectorBase<T, 3, Aligment> {
		union {
			T v[Aligment];
			struct __attribute__((packed)) {
				T x, y, z;
			};
            struct __attribute__((packed)) {
				T r, g, b;
			};
		};
		constexpr VectorBase<T, 3, Aligment>(T x, T y, T z): x(x), y(y), z(z) {
			
		}
		constexpr VectorBase<T, 3, Aligment>() = default;
	};

	template <typename T, std::uint32_t Aligment>
	struct __attribute__((packed)) VectorBase<T, 4, Aligment> {
		union {
			T v[Aligment];
			struct __attribute__((packed)) {
				T x, y, z, w;
			};
            struct __attribute__((packed)) {
				T r, g, b, a;
			};
		};
		constexpr VectorBase<T, 4, Aligment>(T x, T y, T z, T w): x(x), y(y), z(z), w(w) {
			
		}
		constexpr VectorBase<T, 4, Aligment>() = default;
	};

    template <typename T, std::uint32_t Len>
	struct VectorBase<T, Len, 0> {
		T v[Len];
	};
	
	template <typename T>
	struct VectorBase<T, 1, 0> {
		union {
			T v[1];
			T x, r;
		};
	};
	
	template <typename T>
	struct VectorBase<T, 2, 0> {
		union {
			T v[2];
			struct {
				T x, y;
			};
            struct {
				T r, g;
			};
		};
		constexpr VectorBase<T, 2, 0>(T x, T y): x(x), y(y) {
			
		}
		constexpr VectorBase<T, 2, 0>() = default;
	};

	template <typename T>
	struct VectorBase<T, 3, 0> {
		union {
			T v[3];
			struct {
				T x, y, z;
			};
            struct {
				T r, g, b;
			};
		};
		constexpr VectorBase<T, 3, 0>(T x, T y, T z): x(x), y(y), z(z) {
			
		}
		constexpr VectorBase<T, 3, 0>() = default;
	};

	template <typename T>
	struct VectorBase<T, 4, 0> {
		union {
			T v[4];
			struct {
				T x, y, z, w;
			};
            struct {
				T r, g, b, a;
			};
		};
		constexpr VectorBase<T, 4, 0>(T x, T y, T z, T w): x(x), y(y), z(z), w(w) {
			
		}
		constexpr VectorBase<T, 4, 0>() = default;
	};

	export template <typename T, std::uint32_t Len, std::uint32_t Aligment = 0>
	class Vector : public VectorBase<T, Len, Aligment> {
		public:
		constexpr Vector(T x, T y, T z, T w ) requires(std::same_as<T, T> && Len == 4) : VectorBase<T, Len, Aligment>(x, y, z, w) {

		}
		constexpr Vector(T x, T y, T z) requires(std::same_as<T, T> && Len == 3) : VectorBase<T, Len, Aligment>(x, y, z) {

		}
		constexpr Vector(T x, T y) requires(std::same_as<T, T> && Len == 2) : VectorBase<T, Len, Aligment>(x, y) {

		}

		constexpr Vector() = default;

        template <std::uint32_t BAlignment>
        constexpr operator Vector<T, Len, BAlignment>() const {
            Vector<T, Len, BAlignment> returnVector;
            for(std::uint32_t i = 0; i < Len; i++) {
                returnVector.v[i] = this->v[i];
            }
            return returnVector;
        }

		template <std::uint32_t Blen, std::uint32_t BAlignment>
		constexpr Vector<T, Len, Aligment> operator+(Vector<T, Blen, BAlignment> b) const {
			Vector<T, Len, Aligment> resultVector;
			for(std::uint32_t i = 0; i < std::min(Len, Blen); i++) {
				resultVector.v[i] = this->v[i]+b.v[i];
			}
			return resultVector;
		}
		template <std::uint32_t Blen, std::uint32_t BAlignment>
		constexpr Vector<T, Len, Aligment> operator-(Vector<T, Blen, BAlignment> b) const {
			Vector<T, Len, Aligment> resultVector;
			for(std::uint32_t i = 0; i < std::min(Len, Blen); i++) {
				resultVector.v[i] = this->v[i]-b.v[i];
			}
			return resultVector;
		}
		constexpr Vector<T, Len, Aligment> operator-(){
			Vector<T, Len, Aligment> resultVector;
			for(std::uint32_t i = 0; i < Len; i++) {
				resultVector.v[i] = -this->v[i];
			}
			return resultVector;
		}
		template <std::uint32_t Blen, std::uint32_t BAlignment>
		constexpr Vector<T, Len, Aligment> operator*(Vector<T, Blen, BAlignment> b) const {
			Vector<T, Len, Aligment> resultVector;
			for(std::uint32_t i = 0; i < std::min(Len, Blen); i++) {
				resultVector.v[i] = this->v[i]*b.v[i];
			}
			return resultVector;
		}
		template <std::uint32_t Blen, std::uint32_t BAlignment>
		constexpr Vector<T, Len, Aligment> operator/(Vector<T, Blen, BAlignment> b) const {
			Vector<T, Len, Aligment> resultVector;
			for(std::uint32_t i = 0; i < std::min(Len, Blen); i++) {
				resultVector.v[i] = this->v[i]/b.v[i];
			}
			return resultVector;
		}
        template <std::uint32_t Blen, std::uint32_t BAlignment>
		constexpr void operator+=(Vector<T, Blen, BAlignment> b){
			for(std::uint32_t i = 0; i < std::min(Len, Blen); i++) {
				this->v[i]+=b.v[i];
			}
		}
		template <std::uint32_t Blen, std::uint32_t BAlignment>
		constexpr void operator-=(Vector<T, Blen, BAlignment> b){
			for(std::uint32_t i = 0; i < std::min(Len, Blen); i++) {
				this->v[i]-=b.v[i];
			}
		}
		template <std::uint32_t Blen, std::uint32_t BAlignment>
		constexpr void operator*=(Vector<T, Blen, BAlignment> b){
			for(std::uint32_t i = 0; i < std::min(Len, Blen); i++) {
				this->v[i]*=b.v[i];
			}
		}
		template <std::uint32_t Blen, std::uint32_t BAlignment>
		constexpr void operator/=(Vector<T, Blen, BAlignment> b){
			for(std::uint32_t i = 0; i < std::min(Len, Blen); i++) {
				this->v[i]/=b.v[i];
			}
		}

		template <typename BT>
		constexpr Vector<T, Len, Aligment> operator+(BT b) const requires std::is_arithmetic_v<BT> {
			Vector<T, Len, Aligment> resultVector;
			for(std::uint32_t i = 0; i < Len; i++) {
				resultVector.v[i] = this->v[i] + b;
			}
			return resultVector;
		}
		template <typename BT>
		constexpr Vector<T, Len, Aligment> operator-(BT b) const requires std::is_arithmetic_v<BT> {
			Vector<T, Len, Aligment> resultVector;
			for(std::uint32_t i = 0; i < Len; i++) {
				resultVector.v[i] = this->v[i] - b;
			}
			return resultVector;
		}
		template <typename BT>
		constexpr Vector<T, Len, Aligment> operator*(BT b) const requires std::is_arithmetic_v<BT> {
			Vector<T, Len, Aligment> resultVector;
			for(std::uint32_t i = 0; i < Len; i++) {
				resultVector.v[i] = this->v[i] * b;
			}
			return resultVector;
		}
		template <typename BT>
		constexpr Vector<T, Len, Aligment> operator/(BT b) const requires std::is_arithmetic_v<BT> {
			Vector<T, Len, Aligment> resultVector;
			for(std::uint32_t i = 0; i < Len; i++) {
				resultVector.v[i] = this->v[i] / b;
			}
			return resultVector;
		}
		template <typename BT>
		constexpr void operator+=(BT b) requires std::is_arithmetic_v<BT>{
			for(std::uint32_t i = 0; i < Len; i++) {
				this->v[i] += b;
			}
		}
		template <typename BT>
		constexpr void operator-=(BT b) requires std::is_arithmetic_v<BT>{
			for(std::uint32_t i = 0; i < Len; i++) {
				this->v[i] -= b;
			}
		}
		template <typename BT>
		constexpr void operator*=(BT b) requires std::is_arithmetic_v<BT>{
			for(std::uint32_t i = 0; i < Len; i++) {
				this->v[i] *= b;
			}
		}
		template <typename BT>
		constexpr void operator/=(BT b) requires std::is_arithmetic_v<BT>{
			for(std::uint32_t i = 0; i < Len; i++) {
				this->v[i] /= b;
			}
		}

		template <typename BT, std::uint32_t Blen, std::uint32_t BAlignment>
		constexpr bool operator==(Vector<BT, Blen, BAlignment> b) const {
			for(std::uint32_t i = 0; i < std::min(Len, Blen); i++) {
				if(this->v[i] != b.v[i]) {
                    return false;
                }
			}
			return true;
		}

        template <typename BT, std::uint32_t Blen, std::uint32_t BAlignment>
		constexpr bool operator!=(Vector<BT, Blen, BAlignment> b) const {
			for(std::uint32_t i = 0; i < std::min(Len, Blen); i++) {
				if(this->v[i] != b.v[i]) {
                    return true;
                }
			}
			return false;
		}

		constexpr void Normalize() {
			T fLength = Length();
		
			fLength = 1.0f / fLength;

            for(std::uint32_t i = 0; i < Len; i++) {
				this->v[i] *= fLength;
			}
		}

        constexpr void Invert() {
            for(std::uint32_t i = 0; i < Len; i++) {
				this->v[i] = -this->v[i];
			}
		}

		constexpr T Length() const {
			T Result = LengthSq();
			return std::sqrtf(Result);
		}

		constexpr T LengthSq() const {
    		return Dot(*this, *this);
		}

        template <typename AT, std::uint32_t Alen, std::uint32_t AAlignment, typename BT, std::uint32_t Blen, std::uint32_t BAlignment>
        constexpr static Vector<T, Len, Aligment> Cross(Vector<AT, Alen, AAlignment> a, Vector<BT, Blen, BAlignment> b) requires(Len == 3 && Alen >= 3 && Blen >= 3) {
            return Vector<T, Len, Aligment>(
            	(a.v[1] * b.v[2]) - (a.v[2] * b.v[1]),
            	(a.v[2] * b.v[0]) - (a.v[0] * b.v[2]),
            	(a.v[0] * b.v[1]) - (a.v[1] * b.v[0])
			);
        }

        template <typename AT, std::uint32_t Alen, std::uint32_t AAlignment>
        constexpr static Vector<T, Len, Aligment> Normalize(Vector<AT, Alen, AAlignment> a) requires(Len == Alen) {
            Vector<T, Len, Aligment> returned;
            T fLength = a.Length();
		
			if (fLength > 0) {
				fLength = 1.0f / fLength;
			}

            for(std::uint32_t i = 0; i < Len; i++) {
				returned.v[i] = a.v[i] * fLength;
			}
            return returned;
        }

        template <typename AT, std::uint32_t Alen, std::uint32_t AAlignment, typename BT, std::uint32_t Blen, std::uint32_t BAlignment>
        constexpr static T Dot(Vector<AT, Alen, AAlignment> a, Vector<BT, Blen, BAlignment> b) requires(Alen >= Len && Blen >= Len) {
            T accumulate = a.v[0] * b.v[0];
            for(std::uint32_t i = 1; i < Len; i++) {
                accumulate += a.v[i] * b.v[i];
			}
			return accumulate;
		}

        template <typename AT, std::uint32_t AAlignment, typename BT, std::uint32_t BAlignment>
        constexpr static Vector<T, 3, Aligment> Rotate(Vector<AT, 3, AAlignment> v, Vector<BT, 4, BAlignment> q) requires(Len == 3) {
            Vector<T, 3, 0> qv(q.x, q.y, q.z);
            Vector<T, 3, 0> t = Vector<T, 3, Aligment>::Cross(qv, v) * T(2);
            return v + t * q.w + Vector<T, 3, Aligment>::Cross(qv, t);
		}

        template <typename AT, std::uint32_t AAlignment, typename BT, std::uint32_t BAlignment, typename PT, std::uint32_t PAlignment>
        constexpr static Vector<T, 3, Aligment> RotatePivot(Vector<AT, 3, AAlignment> v, Vector<BT, 4, BAlignment> q, Vector<PT, 3, PAlignment> pivot) requires(Len == 3) {
            Vector<T, 3, 0> translated = v - pivot;
            Vector<T, 3, 0> qv(q.x, q.y, q.z);
            Vector<T, 3, 0> t = Cross(qv, translated) * T(2);
            Vector<T, 3, 0> rotated = translated + t * q.w +Cross(qv, t);
            return rotated + pivot;
        }

        template <typename AT, std::uint32_t AAlignment, typename BT, std::uint32_t BAlignment, typename CT, std::uint32_t CAlignment>
        constexpr static Vector<T, 4, Aligment> QuanternionFromBasis(Vector<AT, 3, AAlignment> right, Vector<BT, 3, BAlignment> up, Vector<CT, 3, CAlignment> forward) requires(Len == 4) {
            T m00 = right.x;
            T m01 = up.x;
            T m02 = forward.x;

            T m10 = right.y;
            T m11 = up.y;
            T m12 = forward.y;

            T m20 = right.z;
            T m21 = up.z;
            T m22 = forward.z;

            T trace = m00 + m11 + m22;

            Vector<T, 4, Aligment> q;

            if (trace > std::numeric_limits<T>::epsilon()) {
                T s = std::sqrt(trace + T(1)) * T(2);
                q.w = T(0.25) * s;
                q.x = (m21 - m12) / s;
                q.y = (m02 - m20) / s;
                q.z = (m10 - m01) / s;
            }
            else if ((m00 > m11) && (m00 > m22)) {
                T s = std::sqrt(T(1) + m00 - m11 - m22) * T(2);
                q.w = (m21 - m12) / s;
                q.x = T(0.25) * s;
                q.y = (m01 + m10) / s;
                q.z = (m02 + m20) / s;
            }
            else if (m11 > m22) {
                T s = std::sqrt(T(1) + m11 - m00 - m22) * T(2);
                q.w = (m02 - m20) / s;
                q.x = (m01 + m10) / s;
                q.y = T(0.25) * s;
                q.z = (m12 + m21) / s;
            }
            else {
                T s = std::sqrt(T(1) + m22 - m00 - m11) * T(2);
                q.w = (m10 - m01) / s;
                q.x = (m02 + m20) / s;
                q.y = (m12 + m21) / s;
                q.z = T(0.25) * s;
            }

            q.Normalize();
            return q;
		}

        constexpr static Vector<T, 4, Aligment> QuanternionFromEuler(T rollHalf, T pitchHalf, T yawHalf) {
            T cr = std::cos(rollHalf);
            T sr = std::sin(rollHalf);
            T cp = std::cos(pitchHalf);
            T sp = std::sin(pitchHalf);
            T cy = std::cos(yawHalf);
            T sy = std::sin(yawHalf);

            return Vector<T, 4, Aligment>(
                sr * cp * cy - cr * sp * sy, 
                cr * sp * cy + sr * cp * sy, 
                cr * cp * sy - sr * sp * cy,
                cr * cp * cy + sr * sp * sy
            );
        }

        // The quaternions here are Vector<T, 4> in (x, y, z, w) order, w the
        // scalar part, applied as Rotate does (q v q*). They take and return
        // unit quaternions.

        // The Hamilton product: rotation b followed by rotation a, so
        // Rotate(v, QuanternionMultiply(a, b)) == Rotate(Rotate(v, b), a).
        template <typename AT, std::uint32_t AAlignment, typename BT, std::uint32_t BAlignment>
        constexpr static Vector<T, 4, Aligment> QuanternionMultiply(Vector<AT, 4, AAlignment> a, Vector<BT, 4, BAlignment> b) requires(Len == 4) {
            return Vector<T, 4, Aligment>(
                a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
                a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
                a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,
                a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z
            );
        }

        // The inverse rotation.
        template <typename AT, std::uint32_t AAlignment>
        constexpr static Vector<T, 4, Aligment> QuanternionConjugate(Vector<AT, 4, AAlignment> q) requires(Len == 4) {
            return Vector<T, 4, Aligment>(-q.x, -q.y, -q.z, q.w);
        }

        // `angle` radians about the unit vector `axis`, right-handed: a
        // positive angle about +Z turns +X towards +Y.
        template <typename AT, std::uint32_t AAlignment>
        constexpr static Vector<T, 4, Aligment> QuanternionFromAxisAngle(Vector<AT, 3, AAlignment> axis, T angle) requires(Len == 4) {
            const T s = std::sin(angle * T(0.5));
            return Vector<T, 4, Aligment>(axis.x * s, axis.y * s, axis.z * s, std::cos(angle * T(0.5)));
        }

        // The shortest rotation taking unit direction `from` onto unit
        // direction `to`. For opposite directions every axis perpendicular
        // to `from` is a shortest arc, and the cross product that picks the
        // axis otherwise is zero; the axis is then taken against whichever of
        // X and Y is further from parallel to `from`.
        template <typename AT, std::uint32_t AAlignment, typename BT, std::uint32_t BAlignment>
        constexpr static Vector<T, 4, Aligment> QuanternionFromUnitVectors(Vector<AT, 3, AAlignment> from, Vector<BT, 3, BAlignment> to) requires(Len == 4) {
            const Vector<T, 3, 0> f(from.x, from.y, from.z);
            const T d = f.x * to.x + f.y * to.y + f.z * to.z;
            if (d < T(-1) + T(1e-6)) {
                Vector<T, 3, 0> axis = std::abs(f.x) < T(0.9)
                    ? Vector<T, 3, 0>::Cross(Vector<T, 3, 0>(1, 0, 0), f)
                    : Vector<T, 3, 0>::Cross(Vector<T, 3, 0>(0, 1, 0), f);
                axis.Normalize();
                return Vector<T, 4, Aligment>(axis.x, axis.y, axis.z, T(0));
            }
            const Vector<T, 3, 0> c = Vector<T, 3, 0>::Cross(f, Vector<T, 3, 0>(to.x, to.y, to.z));
            Vector<T, 4, Aligment> q(c.x, c.y, c.z, T(1) + d);
            q.Normalize();
            return q;
        }

        // Normalized linear interpolation from a to b, along the shorter arc
        // (q and -q are the same rotation, and blending towards the wrong one
        // swings the long way round). Cheaper than Slerp; its angular speed
        // varies a little along the arc, which damping and blending between
        // nearby poses don't notice.
        template <typename AT, std::uint32_t AAlignment, typename BT, std::uint32_t BAlignment>
        constexpr static Vector<T, 4, Aligment> QuanternionNlerp(Vector<AT, 4, AAlignment> a, Vector<BT, 4, BAlignment> b, T t) requires(Len == 4) {
            const T sign = (a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w) < T(0) ? T(-1) : T(1);
            Vector<T, 4, Aligment> q(
                a.x + (b.x * sign - a.x) * t,
                a.y + (b.y * sign - a.y) * t,
                a.z + (b.z * sign - a.z) * t,
                a.w + (b.w * sign - a.w) * t
            );
            q.Normalize();
            return q;
        }

        // Spherical linear interpolation from a to b: constant angular speed,
        // along the shorter arc. Nearly identical inputs go through Nlerp,
        // which is exact enough there, instead of dividing by sin(~0).
        template <typename AT, std::uint32_t AAlignment, typename BT, std::uint32_t BAlignment>
        constexpr static Vector<T, 4, Aligment> QuanternionSlerp(Vector<AT, 4, AAlignment> a, Vector<BT, 4, BAlignment> b, T t) requires(Len == 4) {
            T cosAngle = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
            T sign = T(1);
            if (cosAngle < T(0)) {
                cosAngle = -cosAngle;
                sign = T(-1);
            }
            if (cosAngle > T(0.9995)) {
                return QuanternionNlerp(a, b, t);
            }
            const T angle = std::acos(cosAngle);
            const T sinAngle = std::sin(angle);
            const T wa = std::sin((T(1) - t) * angle) / sinAngle;
            const T wb = std::sin(t * angle) / sinAngle * sign;
            return Vector<T, 4, Aligment>(
                a.x * wa + b.x * wb,
                a.y * wa + b.y * wb,
                a.z * wa + b.z * wb,
                a.w * wa + b.w * wb
            );
        }
	};
}



template <typename T, std::uint32_t Len, std::uint32_t Aligment>
struct std::formatter<Crafter::Vector<T, Len, Aligment>> : std::formatter<std::string> {
    auto format(const Crafter::Vector<T, Len, Aligment>& vec, format_context& ctx) const {
        std::string out = "{";
        for(std::uint32_t i2 = 0; i2 < Len; i2++) {
            #ifdef __x86_64
            if constexpr(std::same_as<T, _Float16>) {
                out += std::format("{}", static_cast<float>(vec.v[i2]));
            } else
            #endif
            {
                out += std::format("{}", vec.v[i2]);
            }
            if (i2 + 1 < Len) out += ",";
        }
        out += "}";
        return std::formatter<std::string>::format(out, ctx);
    }
};

template <typename T, std::uint32_t Len, std::uint32_t Aligment, typename BT>
constexpr Crafter::Vector<T, Len, Aligment> operator*(BT b, const Crafter::Vector<T, Len, Aligment>& v) requires std::is_arithmetic_v<BT> {
    return v * b;
}