geometry.h

Go to the documentation of this file.

/*
Copyright (c) 2019, Michael Kazhdan
All rights reserved.
 
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
 
Redistributions of source code must retain the above copyright notice, this list of
conditions and the following disclaimer. Redistributions in binary form must reproduce
the above copyright notice, this list of conditions and the following disclaimer
in the documentation and/or other materials provided with the distribution. 
 
Neither the name of the Johns Hopkins University nor the names of its contributors
may be used to endorse or promote products derived from this software without specific
prior written permission. 
 
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES 
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
TO, PROCUREMENT OF SUBSTITUTE  GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
DAMAGE.
*/
 
#ifndef GEOMETRY_INCLUDED
#define GEOMETRY_INCLUDED
 
#include <iostream>
#include <string>
#include <limits>
#include <Util/algebra.h>
 
namespace Util
{
    static const double Pi = 3.1415926535897932384;
    static const double Epsilon = 1e-10;
    static const double Infinity = std::numeric_limits< double >::infinity();
 
    /*** This is a simple class defining globally accessible variables. */
    struct GlobalProperties
    {
        static bool DebugFlag;
    };
 
    template< unsigned int Dim >
    class Point : public InnerProductSpace< Point< Dim > >
    {
        double _p[Dim];
 
        void _init( const double *values , unsigned int sz );
    public:
        Point( void );
 
        Point( const Point &p );
 
        template< typename ... Doubles >
        Point( Doubles ... values );
 
        double &operator[] ( int index );
 
        const double &operator[] ( int index ) const;
 
        Point  operator *  ( const Point &p ) const;
 
        Point &operator *= ( const Point &p );  
 
        Point  operator /  ( const Point &p ) const;
 
        Point& operator /= ( const Point &p );
 
        template< typename ... Points >
        static Point CrossProduct( Points ... points );
 
        static Point CrossProduct( Point *points );
 
        static Point CrossProduct( const Point *points );
 
        // InnerProductSpace methods //
 
        Point operator * ( double s ) const;
 
        Point operator + ( const Point &p ) const;
 
        double dot( const Point &p ) const;
    };
 
    template<> class Point< 0 >{};
 
    template< unsigned int Dim >
    std::ostream &operator << ( std::ostream &stream , const Point< Dim > &p );
 
    template< unsigned int Dim >
    std::istream &operator >> ( std::istream &stream , Point< Dim > &p );
 
    template< unsigned int Rows , unsigned int Cols , typename MatrixType , typename MatrixTransposeType >
    class _BaseMatrix
    {
        double _m[Rows][Cols];
    public:
        _BaseMatrix( void );
 
        double &operator() ( int r , int c );
 
        const double &operator() ( int r , int c ) const;
 
        MatrixTransposeType transpose( void ) const;
 
        Point< Rows > operator * ( const Point< Cols > &p ) const;  
 
        // Algebra methods //
 
        MatrixType operator * ( double s ) const;
 
        MatrixType operator + ( const MatrixType &m ) const;
 
        // InnerProductSpace methods //
 
        double dot( const _BaseMatrix &p ) const;
    };
 
 
    template< unsigned int Rows , unsigned int Cols > class Matrix;
 
    template< unsigned int Rows , unsigned int Cols >
    class Matrix : public _BaseMatrix< Rows , Cols , Matrix< Rows , Cols > , Matrix< Cols , Rows > > , InnerProductSpace< Matrix< Rows , Cols > >
    {
    public:
        using _BaseMatrix< Rows , Cols , Matrix< Rows , Cols > , Matrix< Cols , Rows > >::_BaseMatrix;
        using _BaseMatrix< Rows , Cols , Matrix< Rows , Cols > , Matrix< Cols , Rows > >::operator *;
        using _BaseMatrix< Rows , Cols , Matrix< Rows , Cols > , Matrix< Cols , Rows > >::operator ();
        using _BaseMatrix< Rows , Cols , Matrix< Rows , Cols > , Matrix< Cols , Rows > >::transpose;
 
        Matrix( void );
 
        template< unsigned int _Cols >
        Matrix< Rows , _Cols > operator * ( const Matrix< Cols , _Cols > &m ) const;
    };
 
    template< unsigned int Dim >
    class Matrix< Dim , Dim > : public _BaseMatrix< Dim , Dim , Matrix< Dim , Dim > , Matrix< Dim , Dim > > , public InnerProductSpace< Matrix< Dim , Dim > > , Algebra< Matrix< Dim , Dim > >
    {
    public:
        using _BaseMatrix< Dim , Dim , Matrix< Dim , Dim > , Matrix< Dim , Dim > >::_BaseMatrix;
        using _BaseMatrix< Dim , Dim , Matrix< Dim , Dim > , Matrix< Dim , Dim > >::operator *;
        using _BaseMatrix< Dim , Dim , Matrix< Dim , Dim > , Matrix< Dim , Dim > >::operator ();
        using _BaseMatrix< Dim , Dim , Matrix< Dim , Dim > , Matrix< Dim , Dim > >::transpose;
 
        Matrix( void );
 
        Matrix( const Matrix< Dim+1 , Dim+1 > &m );
 
        Matrix( const Matrix< Dim-1 , Dim-1 > &m , Point< Dim-1 > p = Point< Dim-1 >() );
 
        double subDeterminant( int r , int c ) const;
 
        double determinant( void ) const;
 
        double trace( void ) const;
 
        Matrix inverse( void ) const;
 
        bool setInverse( Matrix &m ) const;
 
        Point< Dim-1 > operator * ( const Point< Dim-1 > &p ) const;
 
        static Matrix Identity( void );
 
        static Matrix Log( const Matrix &m , double eps=0.0001 );
 
        static Matrix SquareRoot( const Matrix &m , double eps=0.000001 );
 
        void SVD( Matrix &r1 , Matrix &diagonal , Matrix &r2 ) const;
 
        Matrix symmetrize( void ) const;
 
        Matrix skewSymmetrize( void ) const;
 
        static Matrix Exp( const Matrix &m , int terms=100 );
 
        Matrix closestRotation( void ) const;
 
        // Algebra methods //
 
        template< unsigned int Cols >
        Matrix< Dim , Cols > operator * ( const Matrix< Dim , Cols > &m ) const;
    };
 
    template< unsigned int Rows , unsigned int Cols , typename MatrixType , typename MatrixTransposeType >
    std::ostream &operator << ( std::ostream &stream , const _BaseMatrix< Rows , Cols , MatrixType , MatrixTransposeType > &m );
 
    template< unsigned int Rows , unsigned int Cols , typename MatrixType , typename MatrixTransposeType >
    std::istream &operator >> ( std::istream &stream , _BaseMatrix< Rows , Cols , MatrixType , MatrixTransposeType > &m );
 
 
    template< unsigned int Dim >
    class Plane
    {
    public:
        Point< Dim > normal;
        double distance;
 
        Plane( void );
 
        Plane( const Point< Dim > &n , const Point< Dim > &p );
 
        template< typename ... Points >
        Plane( Points ... points );
 
        Plane( Point< Dim > *points );
 
        Plane( const Point< Dim > *points );
 
        double operator()( const Point< Dim > &p ) const;
    };
 
    template< unsigned int Dim >
    class Ray
    {
    public:
        Point< Dim > position;
 
        Point< Dim > direction;
 
        Ray( void );
 
        Ray( const Point< Dim > &position , const Point< Dim > &direction );
 
        Ray  operator +  ( const Point< Dim > &p ) const;
 
        Ray &operator += ( const Point< Dim > &p );
 
        Ray  operator -  ( const Point< Dim > &p ) const;
 
        Ray &operator -= ( const Point< Dim > &p );
 
        Point< Dim > operator() ( double t ) const;
    };
 
    template< unsigned int Dim >
    Ray< Dim > operator * ( const Matrix< Dim+1 , Dim+1 > &m , const Ray< Dim > &ray );
 
    template< unsigned int Dim >
    std::ostream &operator << ( std::ostream &stream , const Ray< Dim > &ray )
    {
        stream << "[ " << ray.position << " ] [ " << ray.direction << " ]";
        return stream;
    }
 
    template< unsigned int Dim >
    class BoundingBox
    {
        template< unsigned int _Dim >
        friend BoundingBox< _Dim > operator * ( const Matrix< _Dim+1 , _Dim+1 > & , const BoundingBox< _Dim > & );
 
        Point< Dim > _p[2];
    public:
        BoundingBox( void );
 
        BoundingBox( const Point< Dim > &p1 , const Point< Dim > &p2 );
 
        BoundingBox( const Point< Dim > *pList , int pSize );
 
        Point< Dim > &operator[] ( int index );
 
        const Point< Dim > &operator[] ( int index ) const;
 
        BoundingBox  operator +  ( const BoundingBox &b ) const;
 
        BoundingBox& operator += ( const BoundingBox &b );
 
        BoundingBox  operator ^  ( const BoundingBox &b ) const;
 
        BoundingBox& operator ^= ( const BoundingBox &b );
 
        bool isInside( const Point< Dim > &p ) const;
 
        bool isEmpty( void ) const;
 
        BoundingBox< 1 > intersect( const Ray< Dim > &ray ) const;
    };
 
    template< unsigned int Dim > struct QuadricBoundingBoxIntersectionInfo;
 
    template< unsigned int Dim >
    class Quadric
    {
        Matrix< Dim , Dim > _Q;
        Point< Dim > _L;
        double _C;
 
        template< unsigned int _Dim >
        friend class Quadric;
 
    public:
        Quadric( void );
 
        Quadric( const Matrix< Dim , Dim > &Q , const Point< Dim > &L , const double &C );
 
        Quadric( const Matrix< Dim+1 , Dim+1 > &Q );
 
        Matrix< Dim+1 , Dim+1 > operator()( void ) const;
 
        Matrix< Dim , Dim > getQuadratic( void ) const;
 
        Point< Dim > getLinear( void ) const;
 
        double getConstant( void ) const;
 
        void setQuadratic( Matrix< Dim , Dim > Q );
 
        void setLinear( Point< Dim > L );
 
        void setConstant( double C );
 
        double operator()( const Point< Dim > &p ) const;
 
        Quadric< 1 > intersect( const Ray< Dim > &ray ) const;
 
        bool setExtremum( Point< Dim > &extremum ) const;
 
        template< unsigned int _Dim >
        Quadric< _Dim > operator * ( const Matrix< Dim , _Dim > &T ) const;
 
        Quadric< Dim > operator + ( const Point< Dim > &t ) const;
 
        struct BoundingBoxOverlap;
 
        friend struct BoundingBoxOverlap;
    };
 
    template< unsigned int Dim >
    struct Quadric< Dim >::BoundingBoxOverlap
    {
        BoundingBoxOverlap( const Quadric< Dim > &Q );
 
        bool operator()( const BoundingBox< Dim > & bBox ) const;
    protected:
        template< unsigned int _Dim > friend struct Quadric< _Dim >::BoundingBoxOverlap;
        Quadric< Dim > _quadric;
        Matrix< Dim , Dim > _Q , _Qinv;
        Matrix< Dim+1 , Dim > _T;
        Matrix< Dim , Dim+1 > _Tt , _Tt_Q;
        struct Quadric< Dim-1 >::BoundingBoxOverlap _boundaryInfo[Dim];
 
        BoundingBoxOverlap( void );
        void _set( const Quadric< Dim > &Q );
        bool _intersect( const Quadric< Dim > &Q , const BoundingBox< Dim > &bBox ) const;
    };
 
    template<>
    struct Quadric<1>::BoundingBoxOverlap
    {
        BoundingBoxOverlap( const Quadric< 1 > &Q ){ _set(Q); }
 
        bool operator()( const BoundingBox< 1 > & bBox ) const;
    protected:
        template< unsigned int _Dim > friend struct Quadric< _Dim >::BoundingBoxOverlap;
        Quadric< 1 > _quadric;
        Matrix< 1 , 1 > _Q , _Qinv;
        Matrix< 2 , 1 > _T;
        Matrix< 1 , 2 > _Tt , _Tt_Q;
 
        BoundingBoxOverlap( void ){}
        void _set( const Quadric< 1 > &Q );
        bool _intersect( const Quadric< 1 > &Q , const BoundingBox< 1 > &bBox ) const;
    };
 
    template< unsigned int Dim >
    BoundingBox< Dim > operator * ( const Matrix< Dim+1 , Dim+1 > &m , const BoundingBox< Dim > &b );
 
    template< unsigned int Dim >
    std::ostream &operator << ( std::ostream &stream , const Point< Dim > &p );
 
    // Classes specialized for 2D, 3D, and 4D //
 
    typedef Point< 1 > Point1D;
 
    typedef Point< 2 > Point2D;
 
    typedef Point< 3 > Point3D;
 
    typedef Point< 4 > Point4D;
 
    template< unsigned int Dim > using SquareMatrix = Matrix< Dim , Dim >;
 
    typedef Matrix< 1 , 1 > Matrix1D;
 
    typedef Matrix< 2 , 2 > Matrix2D;
 
    typedef Matrix< 3 , 3 > Matrix3D;
 
    typedef Matrix< 4 , 4 > Matrix4D;
 
    typedef Plane< 2 > Plane2D;
 
    typedef Plane< 3 > Plane3D;
 
    typedef Plane< 4 > Plane4D;
 
    typedef Ray< 1 > Ray1D;
 
    typedef Ray< 2 > Ray2D;
 
    typedef Ray< 3 > Ray3D;
 
    typedef Ray< 4 > Ray4D;
 
    typedef BoundingBox< 1 > BoundingBox1D;
 
    typedef BoundingBox< 2 > BoundingBox2D;
 
    typedef BoundingBox< 3 > BoundingBox3D;
 
    typedef BoundingBox< 4 > BoundingBox4D;
 
    class Quaternion : public Field< Quaternion > , public _InnerProductSpace< Quaternion >
    {
    public:
        double real;
 
        Point3D imag;
 
        Quaternion( double r=0 , Point3D i = Point3D() );
 
        double dot( const Quaternion& q ) const;
 
        Quaternion conjugate( void ) const;
 
        // Field methods //
 
        Quaternion additiveInverse( void ) const;
 
        Quaternion multiplicativeInverse( void ) const;
 
        Quaternion operator * ( double scale ) const;
 
        Quaternion  operator +  ( const Quaternion& q ) const;
 
        Quaternion  operator *  ( const Quaternion& q ) const;
 
    };
 
    namespace RotationParameters
    {
        enum
        {
            TRIVIAL ,
            ROTATION ,
            EULER ,
            SKEW_SYMMETRIC ,
            QUATERNION ,
            COUNT
        };
 
        const std::string Names[] = { "trivial" , "closest rotation" , "euler" , "skew symmetric" , "quaternion" };
    }
 
    template< typename RotationParameterType , typename ParameterType >
    class RotationParameter
    {
    public:
        ParameterType parameter;
 
        virtual Matrix3D operator()( void ) const = 0;
 
        // VectorSpace methods //
 
        RotationParameterType operator * ( double scale ) const;
 
        RotationParameterType operator + ( const RotationParameterType &p ) const;
    };
 
    class TrivialRotationParameter : public RotationParameter< TrivialRotationParameter , Matrix3D > , public VectorSpace< TrivialRotationParameter >
    {
    public:
        TrivialRotationParameter( void );
 
        TrivialRotationParameter( const Matrix3D &r );
 
        TrivialRotationParameter( const Matrix3D &r , const TrivialRotationParameter &previous );
 
        Matrix3D operator()( void ) const;
    };
 
    class EulerRotationParameter : public RotationParameter< EulerRotationParameter , Point3D > ,  public VectorSpace< EulerRotationParameter >
    {
    public:
        EulerRotationParameter( void );
 
        EulerRotationParameter( const Matrix3D &r );
 
        EulerRotationParameter( const Matrix3D &r , const EulerRotationParameter &previous );
 
        Matrix3D operator()( void ) const;
    };
 
    class MatrixRotationParameter : public RotationParameter< MatrixRotationParameter , Matrix3D > , public VectorSpace< MatrixRotationParameter >
    {
    public:
        MatrixRotationParameter( void );
 
        MatrixRotationParameter( const Matrix3D &r );
 
        MatrixRotationParameter( const Matrix3D &r , const MatrixRotationParameter &previous );
 
        Matrix3D operator()( void ) const;
    };
 
    class SkewSymmetricRotationParameter : public RotationParameter< SkewSymmetricRotationParameter , Point3D > , public VectorSpace< SkewSymmetricRotationParameter >
    {
        Matrix3D _toMatrix( void ) const;
        void _fromMatrix( const Matrix3D &skew );
    public:
        SkewSymmetricRotationParameter( void );
 
        SkewSymmetricRotationParameter( const Matrix3D &r );
 
        SkewSymmetricRotationParameter( const Matrix3D &r , const SkewSymmetricRotationParameter &previous );
 
        Matrix3D operator()( void ) const;
    };
 
    class QuaternionRotationParameter : public RotationParameter< QuaternionRotationParameter , Quaternion > , public VectorSpace< QuaternionRotationParameter >
    {
    public:
        QuaternionRotationParameter( void );
 
        QuaternionRotationParameter( const Matrix3D &r );
 
        QuaternionRotationParameter( const Matrix3D &r , const QuaternionRotationParameter &previous );
 
        Matrix3D operator()( void ) const;
    };
 
    template< typename RotationParameterType >
    class TransformationParameter : public VectorSpace< TransformationParameter< RotationParameterType > >
    {
    public:
        RotationParameterType rotationParameter;
 
        Point3D translation;
 
        TransformationParameter( void );
 
        TransformationParameter( const Matrix4D &m );
 
        TransformationParameter( const Matrix4D &m , const TransformationParameter &p );
 
        TransformationParameter operator * ( double scale ) const;
 
        TransformationParameter operator + ( const TransformationParameter &tp ) const;
 
        Matrix4D operator()( void ) const;
    };
}
#include "geometry.inl"
#include "geometry.todo.inl"
#endif // GEOMETRY_INCLUDED