MatrixFunctions.h

/*--------------------------------------------------------------------------------------------
Copyright (c) 2019, NDEVR LLC
tyler.parke@ndevr.org
  __    __   ____     _____ __     __  _______
 |  \  |  | |  __ \  |  ___|\ \   / / |   __  \
 |   \ |  | | |  \ \ | |___  \ \ / /  |  |__)  |
 |  . \|  | | |__/ / | |___   \ V /   |   _   /
 |  |\    |_|_____/__|_____|___\_/____|  | \  \
 |__| \__________________________________|  \__\
 
Subject to the terms of the Enterprise+ Agreement, NDEVR hereby grants
Licensee a limited, non-exclusive, non-transferable, royalty-free license
(without the right to sublicense) to use the API solely for the purpose of
Licensee's internal development efforts to develop applications for which
the API was provided.
 
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
 
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE
FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.
 
Library: Base
File: MatrixFunctions
Included in API: True
Author(s): Tyler Parke
 *-----------------------------------------------------------------------------------------**/
#pragma once
#include "DLLInfo.h"
#include "Matrix.hpp"
#include <NDEVR/Vertex.h>
#include <NDEVR/LineSegment.h>
#include <NDEVR/Bounds.h>
#include <NDEVR/Triangle.h>
#include <NDEVR/Polyline.h>
namespace NDEVR
{
    template<class t_type, class t_vector, uint01 t_dims, uint01 t_row_dims, uint01 t_col_dims>
    inline Vertex<t_dims, t_type> operator*(const Vertex<t_dims, t_type, t_vector>& vertex, const Matrix<t_type, t_row_dims, t_col_dims>& matrix)
    {
        return matrix * vertex;
    }

    template<class t_type, class t_vector, uint01 t_row_dims, uint01 t_col_dims>
    inline Vertex<1, t_type, t_vector> operator*(const Matrix<t_type, t_row_dims, t_col_dims>& matrix, const Vertex<1, t_type, t_vector>& right)
    {
        Vertex<2, t_type, t_vector> trans(right, cast<t_type>(1));
        trans = matrix * trans;
        return Vertex<1, t_type, t_vector>(trans.template as<1, t_type>() / trans[1]);
    }

    template<class t_type, class t_vector, uint01 t_row_dims, uint01 t_col_dims>
    inline Vertex<2, t_type, t_vector> operator*(const Matrix<t_type, t_row_dims, t_col_dims>& matrix, const Vertex<2, t_type, t_vector>& right)
    {
        t_type bottom = right[0] * matrix[0][3] + right[1] * matrix[1][3] + matrix[2][3] + matrix[3][3];
        return Vertex<2, t_type, t_vector>(
              (right[0] * matrix[0][0] + right[1] * matrix[1][0] + matrix[2][0] + matrix[3][0]) / bottom
            , (right[0] * matrix[0][1] + right[1] * matrix[1][1] + matrix[2][1] + matrix[3][1]) / bottom);
    }

    template<class t_type, class t_vector>
    Vertex<3, t_type, t_vector> operator*(const Matrix<t_type, 4, 4>& m, const Vertex<3, t_type, t_vector>& v)
    {
        const t_type x = v[0], y = v[1], z = v[2];
 
        const t_type bottom =
            x * m.inlineGet(3) +
            y * m.inlineGet(7) +
            z * m.inlineGet(11) +
            m.inlineGet(15);
 
        const t_type inv_bottom = t_type(1) / bottom;
 
        return Vertex<3, t_type, t_vector>(
            (x * m.inlineGet(0) + y * m.inlineGet(4) + z * m.inlineGet(8) + m.inlineGet(12)) * inv_bottom,
            (x * m.inlineGet(1) + y * m.inlineGet(5) + z * m.inlineGet(9) + m.inlineGet(13)) * inv_bottom,
            (x * m.inlineGet(2) + y * m.inlineGet(6) + z * m.inlineGet(10) + m.inlineGet(14)) * inv_bottom
        );
    }

    template<class t_type, class t_vector>
    Vertex<3, t_type, t_vector> operator*(const Matrix<t_type, 3, 3>& matrix, const Vertex<3, t_type, t_vector>& right)
    {
        return Vertex<3, t_type, t_vector>(
              (right[0] * matrix[0][0] + right[1] * matrix[1][0] + right[2] * matrix[2][0])
            , (right[0] * matrix[0][1] + right[1] * matrix[1][1] + right[2] * matrix[2][1])
            , (right[0] * matrix[0][2] + right[1] * matrix[1][2] + right[2] * matrix[2][2]));
    }

    template<class t_type, class t_vector>
    inline Vertex<4, t_type> operator*(const Matrix<t_type, 4, 4>& m, const Vertex<4, t_type, t_vector>& v)
    {
        const t_type x = v[0], y = v[1], z = v[2], w = v[3];
        return Vertex<4, t_type, t_vector>(
            x * m.inlineGet(0) + y * m.inlineGet(4) + z * m.inlineGet(8) + w * m.inlineGet(12),
            x * m.inlineGet(1) + y * m.inlineGet(5) + z * m.inlineGet(9) + w * m.inlineGet(13),
            x * m.inlineGet(2) + y * m.inlineGet(6) + z * m.inlineGet(10) + w * m.inlineGet(14),
            x * m.inlineGet(3) + y * m.inlineGet(7) + z * m.inlineGet(11) + w * m.inlineGet(15)
        );
    }

    template<class t_type, class t_vector, uint01 t_dims, uint01 t_row_dims, uint01 t_col_dims>
    inline Ray<t_dims, t_type> operator*(const Vertex<t_dims, t_type, t_vector>& vertex, const Matrix<t_type, t_row_dims, t_col_dims>& matrix)
    {
        return matrix * vertex;
    }

    template<class t_type, class t_vector, uint01 t_row_dims, uint01 t_col_dims>
    inline Ray<1, t_type, t_vector> operator*(const Matrix<t_type, t_row_dims, t_col_dims>& matrix, const Ray<1, t_type, t_vector>& right)
    {
        return Ray<1, t_type, t_vector>(right[0] * matrix[0][0]);
    }

    template<class t_type, class t_vector, uint01 t_row_dims, uint01 t_col_dims>
    inline Ray<2, t_type, t_vector> operator*(const Matrix<t_type, t_row_dims, t_col_dims>& matrix, const Ray<2, t_type, t_vector>& right)
    {
        return Ray<2, t_type, t_vector>(
              (right[0] * matrix[0][0] + right[1] * matrix[1][0] + matrix[2][0])
            , (right[0] * matrix[0][1] + right[1] * matrix[1][1] + matrix[2][1]));
    }

    template<class t_type, class t_vector>
    inline Ray<3, t_type> operator*(const Matrix<t_type, 4, 4>& matrix, const Ray<3, t_type, t_vector>& right)
    {
        return Ray<3, t_type, t_vector>(
              (right[0] * matrix[0][0] + right[1] * matrix[1][0] + right[2] * matrix[2][0])
            , (right[0] * matrix[0][1] + right[1] * matrix[1][1] + right[2] * matrix[2][1])
            , (right[0] * matrix[0][2] + right[1] * matrix[1][2] + right[2] * matrix[2][2]));
    }

    template<class t_type, class t_vector>
    inline Ray<3, t_type> operator*(const Matrix<t_type, 3, 3>& matrix, const Ray<3, t_type, t_vector>& right)
    {
        return Ray<3, t_type, t_vector>(
              (right[0] * matrix[0][0] + right[1] * matrix[1][0] + right[2] * matrix[2][0])
            , (right[0] * matrix[0][1] + right[1] * matrix[1][1] + right[2] * matrix[2][1])
            , (right[0] * matrix[0][2] + right[1] * matrix[1][2] + right[2] * matrix[2][2]));
    }

    template<class t_type, class t_vector>
    inline Ray<4, t_type> operator*(const Matrix<t_type, 4, 4>& matrix, const Ray<4, t_type, t_vector>& right)
    {
        return Ray<4, t_type, t_vector>(
              right[0] * matrix[0][0] + right[1] * matrix[1][0] + right[2] * matrix[2][0] + right[3] * matrix[3][0]
            , right[0] * matrix[0][1] + right[1] * matrix[1][1] + right[2] * matrix[2][1] + right[3] * matrix[3][1]
            , right[0] * matrix[0][2] + right[1] * matrix[1][2] + right[2] * matrix[2][2] + right[3] * matrix[3][2]
            , right[0] * matrix[0][3] + right[1] * matrix[1][3] + right[2] * matrix[2][3] + right[3] * matrix[3][3]);
    }

    template<class t_type, uint01 t_dims, class t_vertex, uint01 t_row_dims, uint01 t_col_dims>
    inline LineSegment<t_dims, t_type> operator*(const Matrix<t_type, t_row_dims, t_col_dims>& matrix, const LineSegment<t_dims, t_type, t_vertex>& line)
    {
        return LineSegment<t_dims, t_type>(matrix * line.vertex(A), matrix * line.vertex(B));
    }

    template<class t_type, uint01 t_dims, class t_vertex, uint01 t_row_dims, uint01 t_col_dims>
    inline Triangle<t_dims, t_type, t_vertex> operator*(const Matrix<t_type, t_row_dims, t_col_dims>& matrix, const Triangle<t_dims, t_type, t_vertex>& tri)
    {
        return Triangle<t_dims, t_type, t_vertex>(matrix * tri.vertex(A), matrix * tri.vertex(B), matrix * tri.vertex(C));
    }

    template<class t_type, class t_vertex, uint01 t_dims, uint01 t_row_dims, uint01 t_col_dims>
    inline Polyline<t_dims, t_type, t_vertex> operator*(const Matrix<t_type, t_row_dims, t_col_dims>& matrix, const Polyline<t_dims, t_type, t_vertex>& poly)
    {
        Polyline<t_dims, t_type, t_vertex> polyline(poly.vertexCount());
        for (uint04 i = 0; i < poly.vertexCount(); i++)
        {
            polyline.add(matrix * poly.vertex(i));
        }
        return polyline;
    }

    template<class t_type, class t_vertex, uint01 t_row_dims, uint01 t_col_dims>
    Bounds<1, t_type, t_vertex> operator*(const Matrix<t_type, t_row_dims, t_col_dims>& matrix, const Bounds<1, t_type, t_vertex>& bounds)
    {
        Bounds<1, t_type, t_vertex> bounds_2(Constant<Bounds<1, t_type, t_vertex>>::Min);
        t_vertex upper_1;
        for (uint01 max_min = 0; max_min < 2; max_min++)
        {
            for (uint01 n = 0; n < 3; n++)
                upper_1[n] = matrix[0][n] * bounds[max_min][X] + matrix[3][n];
            t_type lower_1 = matrix[0][3] * bounds[max_min][X] + matrix[3][3];
            bounds_2.addToBounds(upper_1 / lower_1);
        }
        return bounds_2;
    }

    template<class t_type, class t_vertex, uint01 t_row_dims, uint01 t_col_dims>
    Bounds<2, t_type, t_vertex> operator*(const Matrix<t_type, t_row_dims, t_col_dims>& matrix, const Bounds<2, t_type>& bounds)
    {
        Bounds<2, t_type, t_vertex> trans_bounds(Constant<Bounds<2, t_type, t_vertex>>::Min);
        t_vertex upper_1;
        for (uint01 i = MIN; i <= MAX; i++)
        {
            for (uint01 n = 0; n < 3; n++)
                upper_1[n] = matrix[0][n] * bounds[i][X] + matrix[3][n];
            t_type lower_1 = matrix[0][3] * bounds[i][X] + matrix[3][3];
            t_vertex upper_2;
            for (uint01 j = MIN; j <= MAX; j++)
            {
                for (uint01 n = 0; n < 3; n++)
                    upper_2[n] = upper_1[n] + matrix[1][n] * bounds[j][Y];
                t_type lower_2 = lower_1 + matrix[1][3] * bounds[j][Y];
                trans_bounds.addToBounds(upper_2 / lower_2);
            }
        }
        return trans_bounds;
    }

    template<class t_type, class t_vertex, uint01 t_row_dims, uint01 t_col_dims>
    Bounds<3, t_type, t_vertex> operator*(const Matrix<t_type, t_row_dims, t_col_dims>& matrix, const Bounds<3, t_type, t_vertex>& bounds)
    {
        if (IsInvalid(bounds))
            return bounds;
        else if (bounds == Constant<Bounds<3, t_type, t_vertex>>::Max)
            return bounds;
        else if (bounds == Constant<Bounds<3, t_type, t_vertex>>::Min)
            return bounds;
        Bounds<3, t_type, t_vertex> trans_bounds(Constant<Bounds<3, t_type, t_vertex>>::Min);
        t_vertex upper_1;
        for (uint01 max_min = MIN; max_min <= MAX; max_min++)
        {
            for (uint01 n = 0; n < 3; n++)
                upper_1[n] = matrix[0][n] * bounds[max_min][X] + matrix[3][n];
            t_type lower_1 = matrix[0][3] * bounds[max_min][X] + matrix[3][3];
            t_vertex upper_2;
            for (uint01 max_min_2 = MIN; max_min_2 <= MAX; max_min_2++)
            {
                for (uint01 n = 0; n < 3; n++)
                    upper_2[n] = upper_1[n] + matrix[1][n] * bounds[max_min_2][Y];
                t_type lower_2 = lower_1 + matrix[1][3] * bounds[max_min_2][Y];
                t_vertex upper_3;
                for (uint01 max_min_3 = MIN; max_min_3 <= MAX; max_min_3++)
                {
                    for (uint01 n = 0; n < 3; n++)
                        upper_3[n] = upper_2[n] + matrix[2][n] * bounds[max_min_3][Z];
                    t_type lower_3 = lower_2 + matrix[2][3] * bounds[max_min_3][Z];
                    trans_bounds.addToBounds(upper_3 / lower_3);
                }
            }
        }
        /*Bounds<3, t_type, t_vertex> trans_bounds(Constant<Bounds<3, t_type, t_vertex>>::Min);
        trans_bounds.addToBounds(matrix * t_vertex(bounds[MIN][X], bounds[MIN][Y], bounds[MIN][Z]));
        trans_bounds.addToBounds(matrix * t_vertex(bounds[MIN][X], bounds[MIN][Y], bounds[MAX][Z]));
        trans_bounds.addToBounds(matrix * t_vertex(bounds[MIN][X], bounds[MAX][Y], bounds[MIN][Z]));
        trans_bounds.addToBounds(matrix * t_vertex(bounds[MIN][X], bounds[MAX][Y], bounds[MAX][Z]));
        trans_bounds.addToBounds(matrix * t_vertex(bounds[MAX][X], bounds[MIN][Y], bounds[MIN][Z]));
        trans_bounds.addToBounds(matrix * t_vertex(bounds[MAX][X], bounds[MIN][Y], bounds[MAX][Z]));
        trans_bounds.addToBounds(matrix * t_vertex(bounds[MAX][X], bounds[MAX][Y], bounds[MIN][Z]));
        trans_bounds.addToBounds(matrix * t_vertex(bounds[MAX][X], bounds[MAX][Y], bounds[MAX][Z]));*/
        return trans_bounds;
    }

    template<class t_type, class t_vertex>
    Bounds<3, t_type, t_vertex> operator*(const Matrix<t_type, 4, 4>& matrix, const Bounds<3, t_type, t_vertex>& bounds)
    {
        Bounds<3, t_type, t_vertex> trans_bounds(Constant<Bounds<3, t_type, t_vertex>>::Min);
        const t_type x[2] = { bounds[MIN][X], bounds[MAX][X] };
        const t_type y[2] = { bounds[MIN][Y], bounds[MAX][Y] };
        const t_type z[2] = { bounds[MIN][Z], bounds[MAX][Z] };
 
        const auto& m = matrix;
        const t_type m00 = m[0][0], m01 = m[0][1], m02 = m[0][2], m03 = m[0][3];
        const t_type m10 = m[1][0], m11 = m[1][1], m12 = m[1][2], m13 = m[1][3];
        const t_type m20 = m[2][0], m21 = m[2][1], m22 = m[2][2], m23 = m[2][3];
        const t_type m30 = m[3][0], m31 = m[3][1], m32 = m[3][2], m33 = m[3][3];
 
        for (uint08 i = 0; i < 8; ++i)
        {
            t_type xi = x[(i >> 0) & 1];
            t_type yi = y[(i >> 1) & 1];
            t_type zi = z[(i >> 2) & 1];
            t_type w = xi * m03 + yi * m13 + zi * m23 + m33;
            t_type rx = (xi * m00 + yi * m10 + zi * m20 + m30) / w;
            t_type ry = (xi * m01 + yi * m11 + zi * m21 + m31) / w;
            t_type rz = (xi * m02 + yi * m12 + zi * m22 + m32) / w;
            trans_bounds.addToBounds(t_vertex(rx, ry, rz));
        }
        return trans_bounds;
    }

    template<class t_type, class t_vertex>
    Bounds<3, t_type> TransformBoundsAndClipNearPlane(const Bounds<3, t_type, t_vertex>& bounds, const Matrix<t_type, 4, 4>& m)
    {
        const Vertex<3, t_type> box_corners[8] = {
            { bounds[MIN][X], bounds[MIN][Y], bounds[MIN][Z] },
            { bounds[MIN][X], bounds[MIN][Y], bounds[MAX][Z] },
            { bounds[MIN][X], bounds[MAX][Y], bounds[MIN][Z] },
            { bounds[MIN][X], bounds[MAX][Y], bounds[MAX][Z] },
            { bounds[MAX][X], bounds[MIN][Y], bounds[MIN][Z] },
            { bounds[MAX][X], bounds[MIN][Y], bounds[MAX][Z] },
            { bounds[MAX][X], bounds[MAX][Y], bounds[MIN][Z] },
            { bounds[MAX][X], bounds[MAX][Y], bounds[MAX][Z] }
        };
        // Precompute all transformed corners
        Vertex<4, t_type> transformed[8];
        for (uint08 i = 0; i < 8; ++i)
            transformed[i] = m * Vertex<4, t_type>(box_corners[i], t_type(1));
        // 12 edges (precomputed corner indices)
        const uint01 edge_pairs[12][2] = {
            {0, 1}, {0, 2}, {0, 4},
            {1, 3}, {1, 5},
            {2, 3}, {2, 6},
            {3, 7},
            {4, 5}, {4, 6},
            {5, 7},
            {6, 7}
        };
 
        Bounds<3, t_type, t_vertex> result(Constant<Bounds<3, t_type, t_vertex>>::Min);
        bool added[8] = {false, false, false, false, false, false, false, false};
 
        for (uint08 e = 0; e < 12; ++e)
        {
            const uint01 i0 = edge_pairs[e][0];
            const uint01 i1 = edge_pairs[e][1];
            const Vertex<4, t_type>& a = transformed[i0];
            const Vertex<4, t_type>& b = transformed[i1];
            const t_type wa = a[W], wb = b[W];
 
            const bool a_in = wa > t_type(0);
            const bool b_in = wb > t_type(0);
 
            if (a_in && b_in)
            {
                if (!added[i0]) {
                    result.addToBounds(a.template as<3, t_type>() / wa);
                    added[i0] = true;
                }
                if (!added[i1]) {
                    result.addToBounds(b.template as<3, t_type>() / wb);
                    added[i1] = true;
                }
            }
            else if (a_in || b_in)
            {
                const t_type t = (t_type(1e-5) - wa) / (wb - wa);
                const Vertex<4, t_type> clip = a + t * (b - a);
                const Vertex<3, t_type> clipped = clip.template as<3, t_type>() / clip[W];
                if (a_in && !added[i0]) {
                    result.addToBounds(a.template as<3, t_type>() / wa);
                    added[i0] = true;
                }
                else if (b_in && !added[i1]) {
                    result.addToBounds(b.template as<3, t_type>() / wb);
                    added[i1] = true;
                }
                result.addToBounds(clipped);
            }
        }
        return result;
    }

    template<class t_type, uint01 t_row_dims, uint01 t_col_dims>
    bool equals(const Matrix<t_type, t_row_dims, t_col_dims>& a, const Matrix<t_type, t_row_dims, t_col_dims>& b, t_type epsilon)
    {
        for (uint01 i = 0; i < t_row_dims; i++)
            if (!equals(a[i], b[i], epsilon))
                return false;
        return true;
    }

    template<class t_type, uint01 t_row_dims, uint01 t_col_dims>
    Matrix<t_type, t_row_dims, t_col_dims> operator*(const t_type mult, const Matrix<t_type, t_row_dims, t_col_dims>& matrix)
    {
        Matrix<t_type, t_row_dims, t_col_dims> multiplied_matrix;
        for (uint01 i = 0; i < t_row_dims; i++)
            multiplied_matrix[i] = mult * matrix[i];
        return multiplied_matrix;
    }

    class NDEVR_BASE_API MatrixFunctions
    {
    public:
        struct TransformSolveOptions
        {
            Buffer<Vertex<3, fltp08>> a; 
            Buffer<Vertex<3, fltp08>> b; 
            bool solve_offset_xy = true;; 
            bool solve_offset_z = true; 
            bool solve_heading = true; 
            bool solve_rotation = false; 
            bool solve_scale = false; 
        };

        static Matrix<fltp08> SolveBestFitTransform(const TransformSolveOptions& options);

        static Matrix<fltp08> SolveLeastSquaredAffine(const Buffer<Vertex<3, fltp08>>& a, const Buffer<Vertex<3, fltp08>>& b);

        static Matrix<fltp08> Solve2DAffine(const Buffer<Vertex<2, fltp08>>& a, const Buffer<Vertex<2, fltp08>>& b);
    };
 
}