matrix3.hpp

// Copyright Take Vos 2021-2022.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt)

 
#pragma once
 
#include "matrix2.hpp"
#include "translate3.hpp"
#include "scale3.hpp"
#include "rotate3.hpp"
#include "../macros.hpp"
#include <array>
#include <exception>
#include <compare>
#include <stdexcept>
 
hi_export_module(hikogui.geometry : matrix3);
 
hi_export namespace hi { inline namespace v1 {
 
class matrix3;
[[nodiscard]] constexpr matrix3 operator*(matrix3 const& lhs, matrix3 const& rhs) noexcept;
[[nodiscard]] constexpr matrix3 operator*(translate3 const& lhs, scale3 const& rhs) noexcept;
[[nodiscard]] constexpr matrix3 operator*(translate3 const& lhs, rotate3 const& rhs) noexcept;

class matrix3 {
public:
    constexpr matrix3(matrix3 const&) noexcept = default;
    constexpr matrix3(matrix3&&) noexcept = default;
    constexpr matrix3& operator=(matrix3 const&) noexcept = default;
    constexpr matrix3& operator=(matrix3&&) noexcept = default;

    constexpr matrix3() noexcept
    {
        auto const a = f32x4::broadcast(1.0f);
        _col0 = a.x000();
        _col1 = a._0y00();
        _col2 = a._00z0();
        _col3 = a._000w();
    };

    constexpr matrix3(f32x4 col0, f32x4 col1, f32x4 col2, f32x4 col3 = f32x4{0.0f, 0.0f, 0.0f, 1.0f}) noexcept :
        _col0(col0), _col1(col1), _col2(col2), _col3(col3)
    {
    }

    constexpr matrix3(vector3 col0, vector3 col1, vector3 col2, vector3 col3 = vector3{}) noexcept :
        _col0(static_cast<f32x4>(col0)),
        _col1(static_cast<f32x4>(col1)),
        _col2(static_cast<f32x4>(col2)),
        _col3(static_cast<f32x4>(col3).xyz1())
    {
    }

    // constexpr matrix3(vector2 col0, vector2 col1) noexcept
    //     :
    //     _col0(static_cast<f32x4>(col0)),
    //     _col1(static_cast<f32x4>(col1)),
    //     _col2(f32x4{0.0f, 0.0f, 1.0f, 0.0f}),
    //     _col3(f32x4{0.0f, 0.0f, 0.0f, 1.0f})
    //{
    // }

    constexpr matrix3(
        float c0r0,
        float c1r0,
        float c2r0,
        float c0r1,
        float c1r1,
        float c2r1,
        float c0r2,
        float c1r2,
        float c2r2) noexcept :
        _col0(c0r0, c0r1, c0r2, 0.0f), _col1(c1r0, c1r1, c1r2, 0.0f), _col2(c2r0, c2r1, c2r2, 0.0f), _col3(0.0f, 0.0f, 0.0f, 1.0f)
    {
    }

    constexpr matrix3(
        float c0r0,
        float c1r0,
        float c2r0,
        float c3r0,
        float c0r1,
        float c1r1,
        float c2r1,
        float c3r1,
        float c0r2,
        float c1r2,
        float c2r2,
        float c3r2,
        float c0r3,
        float c1r3,
        float c2r3,
        float c3r3) noexcept :
        _col0(c0r0, c0r1, c0r2, c0r3), _col1(c1r0, c1r1, c1r2, c1r3), _col2(c2r0, c2r1, c2r2, c2r3), _col3(c3r0, c3r1, c3r2, c3r3)
    {
    }

    [[nodiscard]] constexpr matrix3(matrix2 const& other) noexcept :
        _col0(get<0>(other)), _col1(get<1>(other)), _col2(get<2>(other)), _col3(get<3>(other))
    {
    }
 
    [[nodiscard]] constexpr explicit operator matrix2() const noexcept
    {
        auto tmp0 = _col0;
        auto tmp1 = _col1;
        auto tmp2 = _col2;
        auto tmp3 = _col3;
        tmp0.z() = 0.0f;
        tmp0.w() = 0.0f;
        tmp1.z() = 0.0f;
        tmp1.w() = 0.0f;
        tmp2.x() = 0.0f;
        tmp2.y() = 0.0f;
        tmp2.z() = 1.0f;
        tmp2.w() = 0.0f;
        tmp3.z() = 0.0f;
        tmp3.w() = 1.0f;
        return matrix2{tmp0, tmp1, tmp2, tmp3};
    }
 
    [[nodiscard]] constexpr matrix3(scale3 const& rhs) noexcept
    {
        _col0 = f32x4{rhs}.x000();
        _col1 = f32x4{rhs}._0y00();
        _col2 = f32x4{rhs}._00z0();
        _col3 = f32x4{rhs}._000w();
    }
 
    [[nodiscard]] constexpr matrix3(translate3 const& rhs) noexcept
    {
        auto const ones = f32x4::broadcast(1.0f);
        _col0 = ones.x000();
        _col1 = ones._0y00();
        _col2 = ones._00z0();
        _col3 = ones._000w() + f32x4{rhs};
    }

    [[nodiscard]] constexpr matrix3(rotate3 const& rhs) noexcept
    {
        // Original from https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
        //   1 - 2(yy + zz) |     2(xy - zw) |     2(xz + yw)
        //       2(xy + zw) | 1 - 2(xx + zz) |     2(yz - xw)
        //       2(xz - yw) |     2(yz + xw) | 1 - 2(xx + yy)
 
        // Flipping adds and multiplies:
        //   1 - 2(zz + yy) |     2(xy - zw) |     2(yw + xz)
        //       2(zw + yx) | 1 - 2(xx + zz) |     2(yz - xw)
        //       2(zx - yw) |     2(xw + zy) | 1 - 2(yy + xx)
 
        // All multiplies.
        auto const x_mul = f32x4{rhs}.xxxx() * f32x4{rhs};
        auto const y_mul = f32x4{rhs}.yyyy() * f32x4{rhs};
        auto const z_mul = f32x4{rhs}.zzzz() * f32x4{rhs};
 
        auto twos = f32x4{-2.0f, 2.0f, 2.0f, 0.0f};
        auto one = f32x4{1.0f, 0.0f, 0.0f, 0.0f};
        _col0 = one + addsub_mask<0b0011>(z_mul.zwxy(), y_mul.yxwz()) * twos;
        one = one.yxzw();
        twos = twos.yxzw();
        _col1 = one + addsub_mask<0b0110>(x_mul.yxwz(), z_mul.wzyx()) * twos;
        one = one.xzyw();
        twos = twos.xzyw();
        _col2 = one + addsub_mask<0b0101>(y_mul.wzyx(), x_mul.zwxy()) * twos;
        _col3 = one.xywz();
    }

    [[nodiscard]] constexpr explicit operator std::array<f32x4, 4>() const noexcept
    {
        hi_axiom(holds_invariant());
        return {_col0, _col1, _col2, _col3};
    }

    [[nodiscard]] constexpr static matrix3
    uniform(aarectangle src_rectangle, aarectangle dst_rectangle, alignment alignment) noexcept
    {
        return matrix3{matrix2::uniform(src_rectangle, dst_rectangle, alignment)};
    }

    template<int I>
    [[nodiscard]] friend constexpr f32x4 const& get(matrix3 const& rhs) noexcept
    {
        if constexpr (I == 0) {
            return rhs._col0;
        } else if constexpr (I == 1) {
            return rhs._col1;
        } else if constexpr (I == 2) {
            return rhs._col2;
        } else if constexpr (I == 3) {
            return rhs._col3;
        } else {
            hi_static_no_default();
        }
    }

    template<int I>
    [[nodiscard]] friend constexpr f32x4& get(matrix3& rhs) noexcept
    {
        if constexpr (I == 0) {
            return rhs._col0;
        } else if constexpr (I == 1) {
            return rhs._col1;
        } else if constexpr (I == 2) {
            return rhs._col2;
        } else if constexpr (I == 3) {
            return rhs._col3;
        } else {
            hi_static_no_default();
        }
    }
 
    [[nodiscard]] constexpr bool holds_invariant() const noexcept
    {
        return true;
    }

    [[nodiscard]] constexpr f32x4 operator*(f32x4 const& rhs) const noexcept
    {
        return {_col0 * rhs.xxxx() + _col1 * rhs.yyyy() + _col2 * rhs.zzzz() + _col3 * rhs.wwww()};
    }
 
    
    [[nodiscard]] friend constexpr matrix3 transpose(matrix3 const& rhs) noexcept
    {
        auto tmp = transpose(rhs._col0, rhs._col1, rhs._col2, rhs._col3);
        return matrix3{std::get<0>(tmp), std::get<1>(tmp), std::get<2>(tmp), std::get<3>(tmp)};
    }

    template<char DstX, char DstY, char DstZ, char DstW = 'w'>
    [[nodiscard]] friend constexpr matrix3 reflect(matrix3 const& rhs) noexcept
    {
        return matrix3{reflect_column<DstX>(), reflect_column<DstY>(), reflect_column<DstZ>(), reflect_column<DstW>()} * rhs;
    }

    [[nodiscard]] constexpr friend bool operator==(matrix3 const& lhs, matrix3 const& rhs) noexcept
    {
        return equal(lhs._col0, rhs._col0) and equal(lhs._col1, rhs._col1) and equal(lhs._col2, rhs._col2) and
            equal(lhs._col3, rhs._col3);
    }

    [[nodiscard]] constexpr matrix3 operator~() const
    {
        //                   rc
        // var s0 : Number = i00 * i11 -
        //                  i10 * i01;
        // var c0 : Number = i20 * i31 -
        //                  i30 * i21;
        auto const s0c0 = _col0 * _col1.yxwz();
 
        // var s1 : Number = i00 * i12 -
        //                  i10 * i02;
        // var c1 : Number = i20 * i32 -
        //                  i30 * i22;
        auto const s1c1 = _col0 * _col2.yxwz();
        auto const s0c0s1c1 = hsub(s0c0, s1c1);
 
        // var s2 : Number = i00 * i13 -
        //                  i10 * i03;
        // var c2 : Number = i20 * i33 -
        //                  i30 * i23;
        auto const s2c2 = _col0 * _col3.yxwz();
 
        // var s3 : Number = i01 * i12 -
        //                  i11 * i02;
        // var c3 : Number = i21 * i32 -
        //                  i31 * i22;
        auto const s3c3 = _col1 * _col2.yxwz();
        auto const s2c2s3c3 = hsub(s2c2, s3c3);
 
        // var s4 : Number = i01 * i13 -
        //                  i11 * i03;
        // var c4 : Number = i21 * i33 -
        //                  i31 * i23;
        auto const s4c4 = _col1 * _col3.yxwz();
 
        // var s5 : Number = i02 * i13 -
        //                  i12 * i03;
        // var c5 : Number = i22 * i33 -
        //                  i32 * i23;
        auto const s5c5 = _col2 * _col3.yxwz();
        auto const s4c4s5c5 = hsub(s4c4, s5c5);
 
        // det = (s0 * c5 +
        //       -s1 * c4 +
        //        s2 * c3 +
        //        s3 * c2 +
        //       -s4 * c1 +
        //        s5 * c0)
        auto const s0123 = s0c0s1c1.xz00() + s2c2s3c3._00xz();
        auto const s45__ = s4c4s5c5.xz00();
 
        auto const c5432 = s4c4s5c5.wy00() + s2c2s3c3._00wy();
        auto const c10__ = s0c0s1c1.wy00();
 
        auto const det_prod_half0 = neg_mask<0b0010>(s0123 * c5432);
        auto const det_prod_half1 = neg_mask<0b0001>(s45__ * c10__);
 
        auto const det_sum0 = hadd(det_prod_half0, det_prod_half1);
        auto const det_sum1 = hadd(det_sum0, det_sum0);
        auto const det = hadd(det_sum1, det_sum1).xxxx();
 
        if (det.x() == 0.0f) {
            throw std::domain_error("Divide by zero");
        }
 
        auto const invdet = rcp(det);
 
        auto const t = transpose(*this);
 
        //   rc     rc          rc          rc
        // m.i00 := (i11 *  c5 + i12 * -c4 + i13 *  c3) * invdet;
        // m.i10 := (i10 * -c5 + i12 *  c2 + i13 * -c1) * invdet;
        // m.i20 := (i10 *  c4 + i11 * -c2 + i13 *  c0) * invdet;
        // m.i30 := (i10 * -c3 + i11 *  c1 + i12 * -c0) * invdet;
        auto tmp_c5543 = neg_mask<0b1010>(c5432.xxyz());
        auto tmp_c4221 = neg_mask<0b0101>(c5432.yww0() + c10__._000x());
        auto tmp_c3100 = neg_mask<0b1010>(c5432.z000() + c10__._0xyy());
        auto const inv_col0 = ((t._col1.yxxx() * tmp_c5543) + (t._col1.zzyy() * tmp_c4221) + (t._col1.wwwz() * tmp_c3100)) * invdet;
 
        // m.i01 := (i01 * -c5 + i02 *  c4 + i03 * -c3) * invdet;
        // m.i11 := (i00 *  c5 + i02 * -c2 + i03 *  c1) * invdet;
        // m.i21 := (i00 * -c4 + i01 *  c2 + i03 * -c0) * invdet;
        // m.i31 := (i00 *  c3 + i01 * -c1 + i02 *  c0) * invdet;
        tmp_c5543 = -tmp_c5543;
        tmp_c4221 = -tmp_c4221;
        tmp_c3100 = -tmp_c3100;
        auto const inv_col1 = ((t._col0.yxxx() * tmp_c5543) + (t._col0.zzyy() * tmp_c4221) + (t._col0.wwwz() * tmp_c3100)) * invdet;
 
        // m.i02 := (i31 *  s5 + i32 * -s4 + i33 *  s3) * invdet;
        // m.i12 := (i30 * -s5 + i32 *  s2 + i33 * -s1) * invdet;
        // m.i22 := (i30 *  s4 + i31 * -s2 + i33 *  s0) * invdet;
        // m.i32 := (i30 * -s3 + i31 *  s1 + i32 * -s0) * invdet;
        auto tmp_s5543 = neg_mask<0b1010>(s45__.yyx0() + s0123._000w());
        auto tmp_s4221 = neg_mask<0b0101>(s45__.x000() + s0123._0zzy());
        auto tmp_s3100 = neg_mask<0b1010>(s0123.wyxx());
        auto const inv_col2 = ((t._col3.yxxx() * tmp_s5543) + (t._col3.zzyy() * tmp_s4221) + (t._col3.wwwz() * tmp_s3100)) * invdet;
 
        // m.i03 := (i21 * -s5 + i22 *  s4 + i23 * -s3) * invdet;
        // m.i13 := (i20 *  s5 + i22 * -s2 + i23 *  s1) * invdet;
        // m.i23 := (i20 * -s4 + i21 *  s2 + i23 * -s0) * invdet;
        // m.i33 := (i20 *  s3 + i21 * -s1 + i22 *  s0) * invdet;
        tmp_s5543 = -tmp_s5543;
        tmp_s4221 = -tmp_s4221;
        tmp_s3100 = -tmp_s3100;
        auto const inv_col3 = ((t._col2.yxxx() * tmp_s5543) + (t._col2.zzyy() * tmp_s4221) + (t._col2.wwwz() * tmp_s3100)) * invdet;
 
        return matrix3{inv_col0, inv_col1, inv_col2, inv_col3};
    }
 
private:
    f32x4 _col0;
    f32x4 _col1;
    f32x4 _col2;
    f32x4 _col3;
 
    template<char Axis>
    [[nodiscard]] constexpr static f32x4 reflect_column() noexcept
    {
        if constexpr (Axis == 'x') {
            return f32x4{1.0f, 0.0f, 0.0f, 0.0f};
        } else if constexpr (Axis == 'X') {
            return f32x4{-1.0f, 0.0f, 0.0f, 0.0f};
        } else if constexpr (Axis == 'y') {
            return f32x4{0.0f, 1.0f, 0.0f, 0.0f};
        } else if constexpr (Axis == 'Y') {
            return f32x4{0.0f, -1.0f, 0.0f, 0.0f};
        } else if constexpr (Axis == 'z') {
            return f32x4{0.0f, 0.0f, 1.0f, 0.0f};
        } else if constexpr (Axis == 'Z') {
            return f32x4{0.0f, 0.0f, -1.0f, 0.0f};
        } else if constexpr (Axis == 'w') {
            return f32x4{0.0f, 0.0f, 0.0f, 1.0f};
        } else if constexpr (Axis == 'W') {
            return f32x4{0.0f, 0.0f, 0.0f, -1.0f};
        } else {
            hi_static_no_default();
        }
    }
};
 
 
 
}} // namespace hi::v1