Crafter.Math/tests/Quaternion/main.cpp

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

// Quaternions on the scalar Vector<T, 4> (x, y, z, w), as Vector::Rotate
// applies them: the static Normalize on a 4-vector, the Hamilton product and
// its composition order, conjugate, axis-angle, shortest-arc between two
// directions (including the opposite-directions case), nlerp and slerp.
// Also RotatePivot, on the scalar Vector and on VectorF32 and VectorF16 for
// Packing 1 and for the widest packing a 3-vector and a quaternion share, and
// ToRadian from :Basic, which an importer such as this unit has to be able to
// name.

#include <cmath>
import Crafter.Math;
import std;
using namespace Crafter;

namespace {

using Q = Vector<float, 4, 0>;
using V3 = Vector<float, 3, 0>;

constexpr float kEps = 1e-4f;
constexpr float kPi = std::numbers::pi_v<float>;

bool Near(float a, float b, float epsilon = kEps) {
    return std::abs(a - b) < epsilon;
}

std::string* CheckV3(V3 got, V3 expected, std::string_view label) {
    if (!Near(got.x, expected.x) || !Near(got.y, expected.y) || !Near(got.z, expected.z)) {
        return new std::string(std::format("{}: expected ({},{},{}), got ({},{},{})",
            label, expected.x, expected.y, expected.z, got.x, got.y, got.z));
    }
    return nullptr;
}

// q and -q are the same rotation.
std::string* CheckSameRotation(Q got, Q expected, std::string_view label) {
    const float sign = (got.x * expected.x + got.y * expected.y + got.z * expected.z + got.w * expected.w) < 0 ? -1.0f : 1.0f;
    if (!Near(got.x * sign, expected.x) || !Near(got.y * sign, expected.y)
        || !Near(got.z * sign, expected.z) || !Near(got.w * sign, expected.w)) {
        return new std::string(std::format("{}: expected ({},{},{},{}), got ({},{},{},{})",
            label, expected.x, expected.y, expected.z, expected.w, got.x, got.y, got.z, got.w));
    }
    return nullptr;
}

std::string* CheckUnit(Q q, std::string_view label) {
    const float length = std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
    if (!Near(length, 1.0f)) {
        return new std::string(std::format("{}: length {}, expected 1", label, length));
    }
    return nullptr;
}

Q AboutZ(float angle) {
    return Q::QuanternionFromAxisAngle(V3(0, 0, 1), angle);
}

// ---------- Normalize ----------

std::string* TestNormalizeLen4() {
    // The static Normalize built a 3-vector whatever the length, so a
    // quaternion could not be normalized through it.
    const Q n = Q::Normalize(Q(1, 2, 2, 4));
    return CheckSameRotation(n, Q(0.2f, 0.4f, 0.4f, 0.8f), "Normalize of a 4-vector");
}

// ---------- Multiply / Conjugate ----------

std::string* TestMultiplyIdentity() {
    const Q q = Q::QuanternionFromAxisAngle(V3::Normalize(V3(1, 2, 3)), 0.7f);
    return CheckSameRotation(Q::QuanternionMultiply(q, Q(0, 0, 0, 1)), q, "q * identity");
}

std::string* TestConjugateUndoes() {
    const Q q = Q::QuanternionFromAxisAngle(V3::Normalize(V3(-2, 1, 0.5f)), 1.3f);
    return CheckSameRotation(Q::QuanternionMultiply(q, Q::QuanternionConjugate(q)), Q(0, 0, 0, 1), "q * conj(q)");
}

std::string* TestMultiplyOrder() {
    // a * b applies b first, then a, as Rotate composes.
    const Q a = AboutZ(kPi / 2);
    const Q b = Q::QuanternionFromAxisAngle(V3(1, 0, 0), kPi / 2);
    const V3 v(0, 1, 0);
    const V3 composed = V3::Rotate(v, Q::QuanternionMultiply(a, b));
    const V3 stepwise = V3::Rotate(V3::Rotate(v, b), a);
    return CheckV3(composed, stepwise, "Rotate(v, a * b) == Rotate(Rotate(v, b), a)");
}

// ---------- Axis-angle ----------

std::string* TestAxisAngle() {
    // Right-handed: +90° about Z takes X to Y.
    return CheckV3(V3::Rotate(V3(1, 0, 0), AboutZ(kPi / 2)), V3(0, 1, 0), "+90° about Z on X");
}

// ---------- Shortest arc ----------

std::string* TestFromUnitVectors() {
    const std::array<std::pair<V3, V3>, 5> pairs = {{
        { V3(1, 0, 0), V3(0, 1, 0) },
        { V3(0, 0, 1), V3::Normalize(V3(1, 1, 1)) },
        { V3::Normalize(V3(1, -2, 3)), V3::Normalize(V3(-3, 0.5f, 2)) },
        { V3(0, 1, 0), V3(0, 1, 0) },
        { V3(1, 0, 0), V3(-1, 0, 0) },  // opposite: any perpendicular axis will do
    }};
    for (const auto& [from, to] : pairs) {
        const Q q = Q::QuanternionFromUnitVectors(from, to);
        if (auto err = CheckUnit(q, "FromUnitVectors result")) return err;
        if (auto err = CheckV3(V3::Rotate(from, q), to, "Rotate(from, FromUnitVectors(from, to))")) return err;
    }
    // The opposite case for every axis, since the fallback axis depends on it.
    for (const V3 axis : { V3(0, 1, 0), V3(0, 0, 1), V3::Normalize(V3(1, 1, 0)) }) {
        const V3 opposite(-axis.x, -axis.y, -axis.z);
        if (auto err = CheckV3(V3::Rotate(axis, Q::QuanternionFromUnitVectors(axis, opposite)), opposite,
                               "FromUnitVectors to the opposite direction")) return err;
    }
    return nullptr;
}

// ---------- Interpolation ----------

std::string* TestNlerp() {
    const Q a(0, 0, 0, 1);
    const Q b = AboutZ(kPi / 2);
    if (auto err = CheckSameRotation(Q::QuanternionNlerp(a, b, 0.0f), a, "Nlerp t=0")) return err;
    if (auto err = CheckSameRotation(Q::QuanternionNlerp(a, b, 1.0f), b, "Nlerp t=1")) return err;
    if (auto err = CheckUnit(Q::QuanternionNlerp(a, b, 0.3f), "Nlerp t=0.3")) return err;
    // Halfway is exact for nlerp too, by symmetry.
    return CheckSameRotation(Q::QuanternionNlerp(a, b, 0.5f), AboutZ(kPi / 4), "Nlerp t=0.5");
}

std::string* TestSlerp() {
    const Q a(0, 0, 0, 1);
    const Q b = AboutZ(kPi / 2);
    if (auto err = CheckSameRotation(Q::QuanternionSlerp(a, b, 0.0f), a, "Slerp t=0")) return err;
    if (auto err = CheckSameRotation(Q::QuanternionSlerp(a, b, 1.0f), b, "Slerp t=1")) return err;
    // Constant angular speed: a quarter of the way is a quarter of the angle.
    if (auto err = CheckSameRotation(Q::QuanternionSlerp(a, b, 0.25f), AboutZ(kPi / 8), "Slerp t=0.25")) return err;
    // -b is the same rotation; slerp takes the short way either way.
    const Q negB(-b.x, -b.y, -b.z, -b.w);
    if (auto err = CheckSameRotation(Q::QuanternionSlerp(a, negB, 0.5f), AboutZ(kPi / 4), "Slerp toward -b")) return err;
    // Nearly identical inputs fall back to nlerp instead of dividing by sin(~0).
    const Q almost = AboutZ(1e-5f);
    const Q mid = Q::QuanternionSlerp(a, almost, 0.5f);
    if (std::isnan(mid.x) || std::isnan(mid.w)) return new std::string("Slerp of nearly identical quaternions is NaN");
    return CheckUnit(mid, "Slerp of nearly identical quaternions");
}

// ---------- ToRadian ----------

std::string* TestToRadian() {
    // Lived in :Basic without `export`, so no importer could name it.
    if (!Near(ToRadian(180.0f), kPi)) return new std::string(std::format("ToRadian(180) = {}, expected pi", ToRadian(180.0f)));
    if (!Near(static_cast<float>(ToRadian(90.0)), kPi / 2)) return new std::string("ToRadian(90.0) is not pi/2");
    return nullptr;
}

// ---------- RotatePivot ----------

std::string* TestRotatePivotScalar() {
    // +90° about Z around the pivot (1, 0, 0) swings (2, 0, 0) to (1, 1, 0).
    if (auto err = CheckV3(V3::RotatePivot(V3(2, 0, 0), AboutZ(kPi / 2), V3(1, 0, 0)), V3(1, 1, 0), "RotatePivot about (1,0,0)")) return err;
    // The pivot is a fixed point.
    const Q q = Q::QuanternionFromAxisAngle(V3::Normalize(V3(1, 2, 3)), 0.7f);
    const V3 pivot(0.5f, -1, 2);
    return CheckV3(V3::RotatePivot(pivot, q, pivot), pivot, "RotatePivot fixed point");
}

// `Packing` 3-vectors at stride 3, or quaternions at stride 4, in one register
// of T lanes, and back. The padding lanes stay zero.
template <template <std::uint8_t, std::uint8_t> class VectorType, typename T, std::uint8_t Packing>
VectorType<3, Packing> Pack3(const std::array<V3, Packing>& vs) {
    alignas(64) T buf[VectorType<3, Packing>::AlignmentElement] = {};
    for (std::uint8_t s = 0; s < Packing; ++s) {
        buf[s * 3 + 0] = static_cast<T>(vs[s].x);
        buf[s * 3 + 1] = static_cast<T>(vs[s].y);
        buf[s * 3 + 2] = static_cast<T>(vs[s].z);
    }
    return VectorType<3, Packing>(buf);
}

template <template <std::uint8_t, std::uint8_t> class VectorType, typename T, std::uint8_t Packing>
VectorType<4, Packing> Pack4(const std::array<Q, Packing>& qs) {
    alignas(64) T buf[VectorType<4, Packing>::AlignmentElement] = {};
    for (std::uint8_t s = 0; s < Packing; ++s) {
        buf[s * 4 + 0] = static_cast<T>(qs[s].x);
        buf[s * 4 + 1] = static_cast<T>(qs[s].y);
        buf[s * 4 + 2] = static_cast<T>(qs[s].z);
        buf[s * 4 + 3] = static_cast<T>(qs[s].w);
    }
    return VectorType<4, Packing>(buf);
}

template <template <std::uint8_t, std::uint8_t> class VectorType, typename T, std::uint8_t Packing>
std::array<V3, Packing> Unpack3(VectorType<3, Packing> v) {
    const auto lanes = v.template Store<T>();
    std::array<V3, Packing> out;
    for (std::uint8_t s = 0; s < Packing; ++s) {
        out[s] = V3(static_cast<float>(lanes[s * 3 + 0]), static_cast<float>(lanes[s * 3 + 1]), static_cast<float>(lanes[s * 3 + 2]));
    }
    return out;
}

// Every slot gets its own vector, quaternion and pivot, so a backend that
// reads the quaternion lanes at the wrong stride, or returns the wrong shape,
// disagrees with the scalar Vector somewhere.
template <template <std::uint8_t, std::uint8_t> class VectorType, typename T, std::uint8_t Packing>
std::string* CheckRotatePivotPacked(float epsilon, std::string_view label) {
    std::array<V3, Packing> vs, pivots, expected;
    std::array<Q, Packing> qs;
    for (std::uint8_t s = 0; s < Packing; ++s) {
        vs[s] = V3(1.0f + s, 2.0f - 0.5f * s, -1.0f + 0.25f * s);
        pivots[s] = V3(0.5f * s, 1.0f, -0.5f);
        qs[s] = Q::QuanternionFromAxisAngle(V3::Normalize(V3(1.0f, 2.0f + s, 3.0f)), 0.4f + 0.3f * s);
        expected[s] = V3::Rotate(vs[s] - pivots[s], qs[s]) + pivots[s];
    }
    const std::array<V3, Packing> got = Unpack3<VectorType, T, Packing>(VectorType<3, Packing>::RotatePivot(
        Pack3<VectorType, T, Packing>(vs), Pack4<VectorType, T, Packing>(qs), Pack3<VectorType, T, Packing>(pivots)));
    for (std::uint8_t s = 0; s < Packing; ++s) {
        if (!Near(got[s].x, expected[s].x, epsilon) || !Near(got[s].y, expected[s].y, epsilon) || !Near(got[s].z, expected[s].z, epsilon)) {
            return new std::string(std::format("{} slot {}: expected ({},{},{}), got ({},{},{})",
                label, s, expected[s].x, expected[s].y, expected[s].z, got[s].x, got[s].y, got[s].z));
        }
    }
    return nullptr;
}

std::string* TestRotatePivotSimd() {
    // The x86 VectorF32 and VectorF16 RotatePivot returned a <4, 2> whatever
    // the shape and took the quaternion register as a 3-vector register.
    constexpr float halfEps = 0.05f;  // half precision over a handful of operations on values up to about 5
    if (auto err = CheckRotatePivotPacked<VectorF32, float, 1>(kEps, "VectorF32<3,1>::RotatePivot")) return err;
    constexpr std::uint8_t packing32 = std::min(VectorF32<3, 1>::OptimalPacking, VectorF32<4, 1>::OptimalPacking);
    if (auto err = CheckRotatePivotPacked<VectorF32, float, packing32>(kEps, "VectorF32<3,P>::RotatePivot")) return err;
    if (auto err = CheckRotatePivotPacked<VectorF16, _Float16, 1>(halfEps, "VectorF16<3,1>::RotatePivot")) return err;
    constexpr std::uint8_t packing16 = std::min(VectorF16<3, 1>::OptimalPacking, VectorF16<4, 1>::OptimalPacking);
    return CheckRotatePivotPacked<VectorF16, _Float16, packing16>(halfEps, "VectorF16<3,P>::RotatePivot");
}

} // namespace

int main() {
    using Fn = std::string* (*)();
    constexpr std::array<std::pair<const char*, Fn>, 12> tests = {{
        { "NormalizeLen4",     TestNormalizeLen4     },
        { "MultiplyIdentity",  TestMultiplyIdentity  },
        { "ConjugateUndoes",   TestConjugateUndoes   },
        { "MultiplyOrder",     TestMultiplyOrder     },
        { "AxisAngle",         TestAxisAngle         },
        { "FromUnitVectors",   TestFromUnitVectors   },
        { "Nlerp",             TestNlerp             },
        { "Slerp",             TestSlerp             },
        { "SlerpIdentity",     [] { return CheckSameRotation(Q::QuanternionSlerp(Q(0, 0, 0, 1), Q(0, 0, 0, 1), 0.5f), Q(0, 0, 0, 1), "Slerp of identities"); } },
        { "ToRadian",          TestToRadian          },
        { "RotatePivotScalar", TestRotatePivotScalar },
        { "RotatePivotSimd",   TestRotatePivotSimd   },
    }};

    for (auto const& [name, fn] : tests) {
        if (auto err = std::unique_ptr<std::string>(fn())) {
            std::println(std::cerr, "[{}] {}", name, *err);
            return 1;
        }
    }
    return 0;
}