matrix2.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 "translate2.hpp"
#include "scale2.hpp"
#include "rotate2.hpp"
#include "aarectangle.hpp"
#include "transform_fwd.hpp"
#include "../macros.hpp"
#include <array>
 
namespace hi { inline namespace v1 {
 
class matrix2 {
public:
    constexpr matrix2(matrix2 const&) noexcept = default;
    constexpr matrix2(matrix2&&) noexcept = default;
    constexpr matrix2& operator=(matrix2 const&) noexcept = default;
    constexpr matrix2& operator=(matrix2&&) noexcept = default;
 
    constexpr matrix2() noexcept
    {
        hilet a = f32x4::broadcast(1.0f);
        _col0 = a.x000();
        _col1 = a._0y00();
        _col2 = a._00z0();
        _col3 = a._000w();
    };
 
    constexpr matrix2(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)
    {
        hi_axiom(holds_invariant());
    }
 
    constexpr matrix2(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())
    {
        hi_axiom(holds_invariant());
    }
 
    constexpr matrix2(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})
    {
        hi_axiom(holds_invariant());
    }
 
    constexpr matrix2(
        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)
    {
        hi_axiom(holds_invariant());
    }
 
    constexpr matrix2(
        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)
    {
        hi_axiom(holds_invariant());
    }
 
    [[nodiscard]] constexpr matrix2(translate2 const& rhs) noexcept
    {
        hilet ones = f32x4::broadcast(1.0f);
        _col0 = ones.x000();
        _col1 = ones._0y00();
        _col2 = ones._00z0();
        _col3 = ones._000w() + f32x4{rhs};
    }
 
    [[nodiscard]] constexpr matrix2(scale2 const& rhs) noexcept
    {
        _col0 = f32x4{rhs}.x000();
        _col1 = f32x4{rhs}._0y00();
        _col2 = f32x4{rhs}._00z0();
        _col3 = f32x4{rhs}._000w();
    }
 
    [[nodiscard]] constexpr matrix2(rotate2 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.
        hilet x_mul = f32x4{rhs}.xxxx() * f32x4{rhs};
        hilet y_mul = f32x4{rhs}.yyyy() * f32x4{rhs};
        hilet 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<0b0011>(z_mul.zwxy(), y_mul.yxwz()) * twos;
        one = one.yxzw();
        twos = twos.yxzw();
        _col1 = one + addsub<0b0110>(x_mul.yxwz(), z_mul.wzyx()) * twos;
        one = one.xzyw();
        twos = twos.xzyw();
        _col2 = one + addsub<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 matrix2
    uniform(aarectangle src_rectangle, aarectangle dst_rectangle, alignment alignment) noexcept
    {
        hilet scale = scale2::uniform(src_rectangle.size(), dst_rectangle.size());
        hilet scaled_rectangle = scale * src_rectangle;
        hilet translation = translate2::align(scaled_rectangle, dst_rectangle, alignment);
        return translation * scale;
    }
 
    template<int I>
    [[nodiscard]] friend constexpr f32x4 const& get(matrix2 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(matrix2& 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 _col0.z() == 0.0f and _col0.w() == 0.0f and _col1.z() == 0.0f and _col1.w() == 0.0f and _col2.x() == 0.0f and
            _col2.y() == 0.0f and _col2.z() == 1.0f and _col2.w() == 0.0f and _col3.z() == 0.0f and _col3.w() == 1.0f;
    }
 
    [[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 matrix2 transpose(matrix2 const& rhs) noexcept
    {
        auto tmp = transpose(rhs._col0, rhs._col1, rhs._col2, rhs._col3);
        return matrix2{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 matrix2 reflect(matrix2 const& rhs) noexcept
    {
        return matrix2{reflect_column<DstX>(), reflect_column<DstY>(), reflect_column<DstZ>(), reflect_column<DstW>()} * rhs;
    }
 
    [[nodiscard]] constexpr friend bool operator==(matrix2 const& lhs, matrix2 const& rhs) noexcept
    {
        return equal(lhs._col0, rhs._col0) and equal(lhs._col1, rhs._col1) and equal(lhs._col3, rhs._col3);
    }
 
    [[nodiscard]] constexpr matrix2 operator~() const
    {
        //                   rc
        // var s0 : Number = i00 * i11 -
        //                  i10 * i01;
        // var c0 : Number = i20 * i31 -
        //                  i30 * i21;
        hilet s0c0 = _col0 * _col1.yxwz();
 
        // var s1 : Number = i00 * i12 -
        //                  i10 * i02;
        // var c1 : Number = i20 * i32 -
        //                  i30 * i22;
        hilet s1c1 = _col0 * _col2.yxwz();
        hilet s0c0s1c1 = hsub(s0c0, s1c1);
 
        // var s2 : Number = i00 * i13 -
        //                  i10 * i03;
        // var c2 : Number = i20 * i33 -
        //                  i30 * i23;
        hilet s2c2 = _col0 * _col3.yxwz();
 
        // var s3 : Number = i01 * i12 -
        //                  i11 * i02;
        // var c3 : Number = i21 * i32 -
        //                  i31 * i22;
        hilet s3c3 = _col1 * _col2.yxwz();
        hilet s2c2s3c3 = hsub(s2c2, s3c3);
 
        // var s4 : Number = i01 * i13 -
        //                  i11 * i03;
        // var c4 : Number = i21 * i33 -
        //                  i31 * i23;
        hilet s4c4 = _col1 * _col3.yxwz();
 
        // var s5 : Number = i02 * i13 -
        //                  i12 * i03;
        // var c5 : Number = i22 * i33 -
        //                  i32 * i23;
        hilet s5c5 = _col2 * _col3.yxwz();
        hilet s4c4s5c5 = hsub(s4c4, s5c5);
 
        // det = (s0 * c5 +
        //       -s1 * c4 +
        //        s2 * c3 +
        //        s3 * c2 +
        //       -s4 * c1 +
        //        s5 * c0)
        hilet s0123 = s0c0s1c1.xz00() + s2c2s3c3._00xz();
        hilet s45__ = s4c4s5c5.xz00();
 
        hilet c5432 = s4c4s5c5.wy00() + s2c2s3c3._00wy();
        hilet c10__ = s0c0s1c1.wy00();
 
        hilet det_prod_half0 = neg<0b0010>(s0123 * c5432);
        hilet det_prod_half1 = neg<0b0001>(s45__ * c10__);
 
        hilet det_sum0 = hadd(det_prod_half0, det_prod_half1);
        hilet det_sum1 = hadd(det_sum0, det_sum0);
        hilet det = hadd(det_sum1, det_sum1).xxxx();
 
        if (det.x() == 0.0f) {
            throw std::domain_error("Divide by zero");
        }
 
        hilet invdet = rcp(det);
 
        hilet 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<0b1010>(c5432.xxyz());
        auto tmp_c4221 = neg<0b0101>(c5432.yww0() + c10__._000x());
        auto tmp_c3100 = neg<0b1010>(c5432.z000() + c10__._0xyy());
        hilet 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;
        hilet 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<0b1010>(s45__.yyx0() + s0123._000w());
        auto tmp_s4221 = neg<0b0101>(s45__.x000() + s0123._0zzy());
        auto tmp_s3100 = neg<0b1010>(s0123.wyxx());
        hilet 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;
        hilet inv_col3 = ((t._col2.yxxx() * tmp_s5543) + (t._col2.zzyy() * tmp_s4221) + (t._col2.wwwz() * tmp_s3100)) * invdet;
 
        return matrix2{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