Plane.hpp

/*--------------------------------------------------------------------------------------------
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: Plane
Included in API: True
Author(s): Tyler Parke
 *-----------------------------------------------------------------------------------------**/
#pragma once
#include <NDEVR/Vertex.h>
#include <NDEVR/LineSegment.h>
#include <NDEVR/Matrix.h>
namespace NDEVR
{
    enum class PlanePosition
    {
          e_above_plane
        , e_below_plane
        , e_on_plane
    };

    template<uint01 t_dims, class t_type>
    class Plane
    {
    public:
        Plane()
            : normal()
            , d(0)
        {}
        constexpr Plane(const Ray<t_dims, t_type>& _normal, const Vertex<t_dims, t_type>& _p) 
            : normal(_normal)
            , d(dot(_p, _normal)) 
        {}
        constexpr Plane(const Vertex<t_dims, t_type>& a, const Vertex<t_dims, t_type>& b, const Vertex<t_dims, t_type>& c)
            : normal()
            , d()
        {
            const Vector<t_dims, t_type> edge1 = b - a;
            const Vector<t_dims, t_type> edge2 = c - a;
            normal = (cross(edge1, edge2)).template normalized<t_type>(); // Cross product
            d = dot(a, normal);
        }
        constexpr Plane(const Ray<t_dims, t_type>& _normal, t_type _d)
            : normal(_normal)
            , d(_d) 
        {}
        constexpr void negate()
        {
            normal = -normal; 
            d = -d;
        }
        template<uint01 t_new_dims, class t_new_type>
        [[nodiscard]] Plane<t_new_dims, t_new_type> as() const
        {
            return Plane<t_new_dims, t_new_type>(normal.template as<t_new_dims, t_new_type>(), cast<t_new_type>(d));
        }
        
        //untested
        [[nodiscard]] PlanePosition planePosition(const Vector<t_dims, t_type>& pos) const
        {
            t_type dot_product = distanceTo(pos);
            if (dot_product > 0)
                return PlanePosition::e_above_plane;
            else if (dot_product < 0)
                return PlanePosition::e_below_plane;
            else
                return PlanePosition::e_on_plane;
        }
        [[nodiscard]] bool isAbovePlane(const Vector<t_dims, t_type>& pos) const
        {
            return distanceTo(pos) > 0;
        }
        [[nodiscard]] bool isBelowPlane(const Vector<t_dims, t_type>& pos) const
        {
            return distanceTo(pos) < 0;
        }
        [[nodiscard]] bool liesOnPlane(const Vector<t_dims, t_type>& pos) const
        {
            return  distanceTo(pos) == 0;
        }
        [[nodiscard]] t_type distanceTo(const Vector<t_dims, t_type>& pos) const
        {
            return dot(normal, pos) - d;
        }
        //end untested
        [[nodiscard]] Vertex<t_dims, t_type> nearestPosition(const Vector<3, fltp08>& pos) const
        {
            t_type V0 = dot(normal, pos) - d;
            Vector<t_dims, t_type> v = pos - V0 * normal;
            return v;
        }
        [[nodiscard]] t_type calculateIntersectionPos(const LineSegment<t_dims, t_type>& line, t_type epsilon) const
        {
            t_type Vd = dot(normal, line.ray());
            t_type V0 = dot(normal, line[A]) - d;
 
            if (abs(Vd) < epsilon)
            {
                return abs(V0) < epsilon ? Constant<t_type>::Invalid : Constant<t_type>::Max;
            }
            t_type distance_along_line = -V0 / Vd;
            return distance_along_line;
        }
 
        [[nodiscard]] Vertex<t_dims, t_type> calculateIntersection(const LineSegment<t_dims, t_type>& line, t_type epsilon) const
        {
            t_type distance_along_line = calculateIntersectionPos(line, epsilon);
            Vertex<t_dims, t_type> v = line[A] + distance_along_line * line.ray();
            return v;
        }
 
        [[nodiscard]] Matrix<t_type> projectionMatrix(const Ray<3, t_type>& up = Ray<3, t_type>(0,0,1)) const
        {
            Vector<t_dims, t_type> right = cross(normal, up);
            if (right == 0.0)
            {
                Matrix<t_type> mat(1);
                if (dot(up, normal) > cast<t_type>(0))
                {
                    mat[1][1] = cast<t_type>(-1);
                    mat[2][2] = cast<t_type>(-1);
                }
                return mat;
            }
            right = right.template normalized<t_type>();
            Vector<t_dims, t_type> backward = cross(normal, right).template normalized<t_type>();
            Vector<t_dims, t_type> offset = normal * -d;
            return Matrix<t_type>(
                  right[X],  backward[X], normal[X], 0
                , right[Y],  backward[Y], normal[Y], 0
                , right[Z],  backward[Z], normal[Z], 0
                , offset[X],   offset[Y], offset[Z], 1);
        }
 
        template<class t_buffer_type>
        static Plane CreateBestFitPlane(const t_buffer_type& points, const Vector<t_dims, t_type>& direction_reference = Vector<t_dims, t_type>(0,0,1))
        {
            const uint04 vertex_size = points.size();
            lib_assert(vertex_size >= 3, "Plane requires at least 3 points");
            Vector<t_dims, t_type> centroid(0);
            Vector<t_dims, t_type> n = Vector<t_dims, t_type>(0);
            Vector<t_dims, t_type> v;
            Vector<t_dims, t_type> p1, p2, p3, c1, c2;
            
            p1 = c1 = points[0]; 
            p2 = c2 = points[1];
            
            for(uint04 i = 0; i < vertex_size; i++)
            {
                p3 = points[i];
                v = cross((p3 - p2), (p1 - p2));
                if(v[t_dims - 1] != 0) 
                    v = -v;
                n += v;
                p1 = p2; 
                p2 = p3;
                centroid += points[i];
            }
            centroid /= cast<t_type>(vertex_size);
            p1 = p2; 
            p2 = p3; 
            p3 = c1;
            v = cross((p3 - p2), (p1 - p2));
            if(v[t_dims - 1] != 0) 
                v = -v;
            n += v;
            p1 = p2; 
            p2 = p3; 
            p3 = c2;
            v = cross((p3 - p2), (p1 - p2));
            if (v[t_dims - 1]) 
                v = -v;
            n += v;
            n = n.template normalized<t_type>();
            if (dot(direction_reference, n) < 0)
                n = -n;
            Plane plane(n, dot(n, centroid));
            return plane;
        };
 
        bool contains(const Vector<3, t_type>& point, t_type epsilon) const
        {
            Vector<3, t_type> b = point - (normal * d);
            return (abs(dot(b, normal)) < epsilon);
        }
        bool isSamePlane(const Plane& plane, t_type epsilon) const
        {
            return (equals(normal, plane.normal, epsilon) && abs(d - plane.d) < epsilon)
                || (equals(-normal, plane.normal, epsilon) && abs(d + plane.d) < epsilon);
        }
 
        bool operator==(const Plane& plane) const
        {
            return normal == plane.normal && d == plane.d;
        }
        bool operator!=(const Plane& plane) const
        {
            return normal != plane.normal || d != plane.d;
        }
    public:
        Ray<t_dims, t_type> normal;
        t_type d;
 
    };

    template<uint01 t_dims, class t_type>
    struct Constant<Plane<t_dims, t_type>>
    {
        constexpr const static Plane<t_dims, t_type> Invalid{ Constant<Vector<t_dims, t_type>>::Invalid, Constant<t_type>::Invalid };
    };

 
    template<uint01 t_dims, class t_type>
    static constexpr bool IsInvalid(const Plane<t_dims, t_type>& value)
    {
        return IsInvalid(value.normal) || IsInvalid(value.d);
    }
    template<uint01 t_dims, class t_type>
    static constexpr bool IsValid(const Plane<t_dims, t_type>& value)
    {
        return !IsInvalid(value.normal) && !IsInvalid(value.d);
    }
    template<class t_type, uint01 t_row_dims, uint01 t_col_dims>
    Plane<3, t_type> operator*(const Matrix<t_type, t_row_dims, t_col_dims>& matrix, const Plane<3, t_type>& plane)
    {
        Vertex<3, t_type> origin = plane.normal * plane.d;
        return Plane<3, t_type>((matrix * plane.normal).template normalized<fltp08>(), matrix * origin);
    }
};