math.h

Go to the documentation of this file.

/*
 *  Copyright (C) 2010 - 2015 Leonid Kostrykin
 *
 *  Chair of Medical Engineering (mediTEC)
 *  RWTH Aachen University
 *  Pauwelsstr. 20
 *  52074 Aachen
 *  Germany
 *
 */

#ifndef MATH_H_6014714286
#define MATH_H_6014714286

#include <Carna/Carna.h>
#include <Carna/base/CarnaException.h>
#include <algorithm>
#include <type_traits>
#include <cmath>
#include <Eigen/Dense>

#ifdef min
#error MIN macro defined, define NOMINMAX first!
#endif

#ifdef max
#error MAX macro defined, define NOMINMAX first!
#endif

#ifndef NOMINMAX
#define NOMINMAX
#endif

namespace Carna
{

namespace base
{

namespace math
{

    template< typename T >
    T clamp( T val, T my_min, T my_max )
    {
        return std::min( std::max( val, my_min ), my_max );
    }

    template< typename T >
    T sq( T x )
    {
        return x * x;
    }
    
    inline float deg2rad( float deg )
    {
        return deg * 3.1415926f / 180.f;
    }

    inline float rad2deg( float rad )
    {
        return 180.f * rad / 3.1415926f;
    }

    template< typename T >
    T epsilon()
    {
        return static_cast< T >( 0 );
    }

    template< >
    inline float epsilon< float >()
    {
        return 1e-4f;
    }

    template< >
    inline double epsilon< double >()
    {
        return 1e-6;
    }

    template< typename T >
    struct element_type_of
    {
        typedef T type;
    };

    template< typename VectorElementType, int rows, int cols >
    struct element_type_of< Eigen::Matrix< VectorElementType, rows, cols > >
    {
        typedef VectorElementType type;

    };

    template< typename T >
    T length2( const T& x )
    {
        return x * x;
    }

    template< typename VectorElementType, int dimension >
    VectorElementType length2( const Eigen::Matrix< VectorElementType, dimension, 1 >& x )
    {
        return x.squaredNorm();
    }

    template< typename InputType >
    bool isEqual( const InputType& x, const InputType& y )
    {
        typedef typename element_type_of< InputType >::type ScalarType;
        const InputType difference = x - y;
        const ScalarType distance2 = length2( InputType( difference ) );
        const ScalarType _epsilon  = epsilon< ScalarType >();
        return distance2 <= _epsilon;
    }
    
    template< >
    inline bool isEqual( const bool& x, const bool& y )
    {
        return x == y;
    }

    typedef Eigen::Matrix< float, 4, 4, Eigen::ColMajor > Matrix4f; 
    typedef Eigen::Matrix< float, 3, 3, Eigen::ColMajor > Matrix3f; 
    typedef Eigen::Matrix< float, 4, 1 > Vector4f;                  
    typedef Eigen::Matrix< float, 3, 1 > Vector3f;                  
    typedef Eigen::Matrix< float, 2, 1 > Vector2f;                  
    typedef Eigen::Matrix< signed int, 3, 1 > Vector3i;             
    typedef Eigen::Matrix< unsigned int, 3, 1 > Vector3ui;          
    typedef Eigen::Matrix< unsigned int, 2, 1 > Vector2ui;          

    inline Matrix4f identity4f()
    {
        Matrix4f result;
        result.setIdentity();
        return result;
    }

    inline Matrix3f identity3f()
    {
        Matrix3f result;
        result.setIdentity();
        return result;
    }
    
    template< typename MatrixType >
    MatrixType zeros()
    {
        MatrixType result;
        result.setZero();
        return result;
    }

    inline Matrix4f basis4f( const Vector4f& x, const Vector4f& y, const Vector4f& z, const Vector4f& t = Vector4f( 0, 0, 0, 0 ) )
    {
        Matrix4f result;
        result.col( 0 ) = x;
        result.col( 1 ) = y;
        result.col( 2 ) = z;
        result.col( 3 ) = t;
        for( unsigned int i = 0; i < 3; ++i )
        {
            result( 3, i ) = 0;
        }
        result( 3, 3 ) = 1;
        return result;
    }

    inline Matrix4f basis4f( const Vector3f& x, const Vector3f& y, const Vector3f& z, const Vector3f& t = Vector3f( 0, 0, 0 ) )
    {
        const Vector4f x4( x.x(), x.y(), x.z(), 0 );
        const Vector4f y4( y.x(), y.y(), y.z(), 0 );
        const Vector4f z4( z.x(), z.y(), z.z(), 0 );
        const Vector4f t4( t.x(), t.y(), t.z(), 0 );
        return basis4f( x4, y4, z4, t4 );
    }

    inline Matrix4f translation4f( float x, float y, float z )
    {
        Matrix4f result;
        result.setIdentity();
        result( 0, 3 ) = x;
        result( 1, 3 ) = y;
        result( 2, 3 ) = z;
        return result;
    }

    template< typename Vector >
    Matrix4f translation4f( const Vector& v )
    {
        return translation4f( v.x(), v.y(), v.z() );
    }

    inline Matrix4f scaling4f( float x, float y, float z )
    {
        Matrix4f result;
        result.setIdentity();
        result( 0, 0 ) = x;
        result( 1, 1 ) = y;
        result( 2, 2 ) = z;
        return result;
    }

    template< typename VectorElementType >
    inline Matrix4f scaling4f( const Eigen::Matrix< VectorElementType, 3, 1 >& v )
    {
        return scaling4f( v.x(), v.y(), v.z() );
    }
    
    inline Matrix4f scaling4f( float uniformScaleFactor )
    {
        return scaling4f( uniformScaleFactor, uniformScaleFactor, uniformScaleFactor );
    }

    inline Matrix4f rotation4f( float x, float y, float z, float radians )
    {
        const float c = std::cos( radians );
        const float s = std::sin( radians );

        Matrix4f result;
        result.setIdentity();

        result( 0, 0 ) = x * x * ( 1 - c ) + c;
        result( 1, 0 ) = y * x * ( 1 - c ) + z * s;
        result( 2, 0 ) = x * z * ( 1 - c ) - y * s;

        result( 0, 1 ) = x * y * ( 1 - c ) - z * s;
        result( 1, 1 ) = y * y * ( 1 - c ) + c;
        result( 2, 1 ) = y * z * ( 1 - c ) + x * s;

        result( 0, 2 ) = x * z * ( 1 - c ) + y * s;
        result( 1, 2 ) = y * z * ( 1 - c ) - x * s;
        result( 2, 2 ) = z * z * ( 1 - c ) + c;

        return result;
    }

    template< typename Vector >
    Matrix4f rotation4f( const Vector& v, float radians )
    {
        return rotation4f( v.x(), v.y(), v.z(), radians );
    }

    inline Matrix3f rotation3f( float x, float y, float z, float radians )
    {
        return rotation4f( x, y, z, radians ).block< 3, 3 >( 0, 0 );
    }

    inline Vector3f orthogonal3f( const Vector3f& in )
    {
        Vector3f out;
        if( std::abs( in.x() - in.y() ) > 1e-4f )
        {
            out.x() = in.y();
            out.y() = in.x();
            out.z() = 0;

            if( std::abs( out.x() ) > std::abs( out.y() ) )
            {
                out.x() = -out.x();
            }
            else
            {
                out.y() = -out.y();
            }
        }
        else
        if( std::abs( in.x() - in.z() ) > 1e-4f )
        {
            out.x() = in.z();
            out.y() = 0;
            out.z() = in.x();

            if( std::abs( out.x() ) > std::abs( out.z() ) )
            {
                out.x() = -out.x();
            }
            else
            {
                out.z() = -out.z();
            }
        }
        else
        if( std::abs( in.y() - in.z() ) > 1e-4f )
        {
            out.x() = 0;
            out.y() = in.z();
            out.z() = in.x();

            if( std::abs( out.y() ) > std::abs( out.z() ) )
            {
                out.y() = -out.y();
            }
            else
            {
                out.z() = -out.z();
            }
        }
        else // all components are equal
        {
            out = Vector3f( -in.x(), in.y(), 0 );
        }

        CARNA_ASSERT( isEqual< float >( in.dot( out ), 0 ) );
        return out;
    }

    template< typename VectorElementType, int rows, typename WType >
    Eigen::Matrix< VectorElementType, 4, 1 > vector4( const Eigen::Matrix< VectorElementType, rows, 1 >& v, WType w )
    {
        static_assert( rows >= 3, "math::vector4 requires input vector with 3 rows or more." );
        return Eigen::Matrix< VectorElementType, 4, 1 >( v.x(), v.y(), v.z(), static_cast< VectorElementType >( w ) );
    }

    template< typename VectorElementType, int rows >
    Eigen::Matrix< VectorElementType, 3, 1 > vector3( const Eigen::Matrix< VectorElementType, rows, 1 >& v )
    {
        static_assert( rows >= 3, "math::vector3 requires input vector with 3 rows or more." );
        return Eigen::Matrix< VectorElementType, 3, 1 >( v.x(), v.y(), v.z() );
    }

    inline Matrix4f plane4f( const Vector3f& normal, float distance )
    {
        CARNA_ASSERT( isEqual< float >( normal.norm(), 1 ) );
        const Vector3f tangent  ( orthogonal3f(  normal ).normalized() );
        const Vector3f bitangent( normal.cross( tangent ).normalized() );
        const Vector3f translation( normal * distance );
        return basis4f
            ( vector4(   bitangent, 0 )
            , vector4(     tangent, 0 )
            , vector4(      normal, 0 )
            , vector4( translation, 1 ) );
    }

    inline Matrix4f plane4f( const Vector3f& normal, const Vector3f& support )
    {
        CARNA_ASSERT( isEqual< float >( normal.norm(), 1 ) );
        const float distance = normal.dot( support );
        return plane4f( normal, distance );
    }

    inline float translationDistance2( const Matrix4f& m )
    {
        return m( 0, 3 ) * m( 0, 3 ) + m( 1, 3 ) * m( 1, 3 ) + m( 2, 3 ) * m( 2, 3 );
    }

    template< typename Matrix >
    typename Matrix::Scalar maxAbsElement( const Matrix& m )
    {
        const std::size_t length = m.rows() * m.cols();
        typename Matrix::Scalar maxAbs = 0;
        for( std::size_t i = 0; i < length; ++i )
        {
            maxAbs = std::max( maxAbs, std::abs( m.data()[ i ] ) );
        }
        return maxAbs;
    }

    inline Matrix4f frustum4f( float left, float right, float bottom, float top, float zNear, float zFar )
    {
        Matrix4f result;
        result.setZero();

        result( 0, 0 ) = +2 * zNear / ( right - left );
        result( 1, 1 ) = +2 * zNear / ( top - bottom );
        result( 0, 2 ) =  ( right + left ) / ( right - left );
        result( 1, 2 ) =  ( top + bottom ) / ( top - bottom );
        result( 2, 2 ) = -( zFar + zNear ) / ( zFar - zNear );
        result( 3, 2 ) = -1;
        result( 2, 3 ) = -2 * zFar * zNear / ( zFar - zNear );

        return result;
    }

    inline Matrix4f frustum4f( float fovRadiansHorizontal, float heightOverWidth, float zNear, float zFar )
    {
        const float halfProjPlaneWidth  = zNear * std::tan( fovRadiansHorizontal );
        const float halfProjPlaneHeight = halfProjPlaneWidth * heightOverWidth;
        return frustum4f( -halfProjPlaneWidth, +halfProjPlaneWidth, -halfProjPlaneHeight, +halfProjPlaneHeight, zNear, zFar );
    }
    
    inline Matrix4f ortho4f( float left, float right, float bottom, float top, float zNear, float zFar )
    {
        Matrix4f result;
        result.setZero();

        result( 0, 0 ) =  2 / ( right - left );
        result( 1, 1 ) =  2 / ( top - bottom );
        result( 2, 2 ) = -2 / ( zFar - zNear );
        result( 0, 3 ) = -( right + left ) / ( right - left );
        result( 1, 3 ) = -( top + bottom ) / ( bottom - top );
        result( 2, 3 ) = -( zFar + zNear ) / ( zFar - zNear );
        result( 3, 3 ) = +1;

        return result;
    }

    template< typename ScalarType >
    unsigned int round_ui( ScalarType x )
    {
        CARNA_ASSERT( !std::numeric_limits< ScalarType >::is_signed || x >= 0 );
        return static_cast< unsigned int >( x + static_cast< ScalarType >( 0.5 ) );
    }

    template< typename MatrixElementType, int cols, int rows >
    Eigen::Matrix< unsigned int, cols, rows > round_ui( const Eigen::Matrix< MatrixElementType, cols, rows >& m )
    {
        Eigen::Matrix< unsigned int, cols, rows > result;
        for( int i = 0; i < cols; ++i )
        for( int j = 0; j < rows; ++j )
        {
            result( i, j ) = round_ui( m( i, j ) );
        }
        return result;
    }

    template< typename ScalarType >
    ScalarType makeEven( ScalarType x, int s )
    {
        CARNA_ASSERT( s == +1 || s == -1 );
        static_assert( std::is_integral< ScalarType >::value, "Only integral data types allowed." );
        return x + s * ( x % 2 );
    }

    template< typename MatrixElementType, int cols, int rows >
    Eigen::Matrix< MatrixElementType, cols, rows > makeEven( const Eigen::Matrix< MatrixElementType, cols, rows >& m, int s )
    {
        Eigen::Matrix< unsigned int, cols, rows > result;
        for( int i = 0; i < cols; ++i )
        for( int j = 0; j < rows; ++j )
        {
            result( i, j ) = makeEven( m( i, j ), s );
        }
        return result;
    }
    
    template< typename T >
    struct Statistics
    {
        T mean;     
        T variance; 
        
        Statistics( T mean, T variance ) : mean( mean ), variance( variance )
        {
        }
        
        Statistics( std::size_t size, const std::function< T( std::size_t ) > values )
        {
            if( size == 0 )
            {
                mean = variance = 0;
            }
            else
            {
                /* Compute mean.
                 */
                T sum = 0;
                for( std::size_t idx = 0; idx < size; ++idx )
                {
                    sum += values( idx );
                }
                mean = sum / size;
                
                /* Compute variance.
                 */
                sum = 0;
                for( std::size_t idx = 0; idx < size; ++idx )
                {
                    sum += sq( mean - values( idx ) );
                }
                variance = sum / ( size - 1 );
            }
        }
        
        Statistics< T >& operator=( const Statistics< T >& other )
        {
            mean = other.mean;
            variance = other.variance;
            return *this;
        }
        
        T standardDeviation() const
        {
            return std::sqrt( variance );
        }
    };
    
    template< typename ResultType, typename SupportType >
    ResultType mix( const SupportType& a, const SupportType& b, float t )
    {
        return a * ( 1 - t ) + b * t;
    }



#define CARNA_FOR_VECTOR3UI_EX( vecName, vecLimit, vecStart ) \
    Carna::base::math::Vector3ui vecName; \
    for( vecName.z() = vecStart.x(); vecName.z() < vecLimit.z(); ++vecName.z() ) \
    for( vecName.y() = vecStart.y(); vecName.y() < vecLimit.y(); ++vecName.y() ) \
    for( vecName.x() = vecStart.z(); vecName.x() < vecLimit.x(); ++vecName.x() )



#define CARNA_FOR_VECTOR3UI( vecName, vecLimit ) \
    CARNA_FOR_VECTOR3UI_EX( vecName, vecLimit, Carna::base::math::Vector3ui( 0, 0, 0 ) )



}  // namespace Carna :: base :: math

}  // namespace Carna :: base

}  // namespace Carna



#endif // MATH_H_6014714286