MatrixFunctions.h

Go to the documentation of this file.

/**--------------------------------------------------------------------------------------------
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>& matrix, const Vertex<3, t_type, t_vector>& right)
    {
        t_type bottom = right[0] * matrix[0][3] + right[1] * matrix[1][3] + right[2] * matrix[2][3] + matrix[3][3];
        return Vertex<3, t_type, t_vector>(
              (right[0] * matrix[0][0] + right[1] * matrix[1][0] + right[2] * matrix[2][0] + matrix[3][0]) / bottom
            , (right[0] * matrix[0][1] + right[1] * matrix[1][1] + right[2] * matrix[2][1] + matrix[3][1]) / bottom
            , (right[0] * matrix[0][2] + right[1] * matrix[1][2] + right[2] * matrix[2][2] + matrix[3][2]) / 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>& matrix, const Vertex<4, t_type, t_vector>& right)
    {
        return Vertex<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, 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 (isNaN(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);
                }
            }
        }
        /*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, 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);
    };
    
}