RigidTransformation3D.h

 
#pragma once
 
#if defined(RigidTransformation3D_RECURSES)
#error Recursive header files inclusion detected in RigidTransformation3D.h
#else // defined(RigidTransformation3D_RECURSES)
#define RigidTransformation3D_RECURSES
 
#if !defined RigidTransformation3D_h
#define RigidTransformation3D_h
 
// Inclusions
#include <iostream>
#include <cmath>
#include <climits>
#include <utility>
#include <exception>
#include "DGtal/kernel/BasicPointFunctors.h"
#include "DGtal/base/Common.h"
#include "DGtal/kernel/domains/CDomain.h"
#include "DGtal/kernel/CSpace.h"
 
namespace DGtal
{
namespace functors
{
// Template class ForwardRigidTransformation3D
template < typename TSpace, typename TInputValue = typename TSpace::RealPoint, typename TOutputValue = typename TSpace::Point,
           typename TFunctor = VectorRounding < TInputValue, TOutputValue > >
class ForwardRigidTransformation3D
{
    BOOST_CONCEPT_ASSERT(( concepts::CSpace<TSpace> ));
    BOOST_STATIC_ASSERT(( TSpace::dimension == 3 ));
    BOOST_STATIC_ASSERT(( TOutputValue::dimension == 3 ));
    BOOST_STATIC_ASSERT(( TInputValue::dimension == 3 ));
 
    // ----------------------- Types ------------------------------
public:
    typedef typename TSpace::RealPoint RealPoint;
    typedef typename TSpace::RealVector RealVector;
 
    // ----------------------- Interface --------------------------------------
public:
    ForwardRigidTransformation3D ( const RealPoint & aOrigin, const RealVector & aAxis,
                                   const double & angle, const RealVector & aTranslate )
        : axis(aAxis.getNormalized()), origin(aOrigin), trans(aTranslate)
 
 
    {
        if ( std::isnan( axis.norm() ) )
            throw std::runtime_error ( "Axis of rotation can not be set as a vector of length 0!" );
        t_sin = std::sin ( angle );
        t_cos = std::cos ( angle );
    }
 
    inline
    TOutputValue operator()( const TInputValue & aInput ) const
    {
        RealPoint p;
 
        p[0] = ( ( ( ( t_cos + ( axis[0] * axis[0] ) * ( 1. - t_cos ) ) * ( aInput[0] - origin[0] ) )
                + ( ( axis[0] * axis[1] * ( 1. - t_cos ) - axis[2] * t_sin ) * ( aInput[1] - origin[1] ) )
                + ( ( axis[1] * t_sin + axis[0] * axis[2] * ( 1. - t_cos )  ) * ( aInput[2] - origin[2] ) ) ) + trans[0] ) + origin[0];
 
        p[1] = ( ( ( ( axis[2] * t_sin + axis[0] * axis[1] * ( 1. - t_cos ) ) *  ( aInput[0] - origin[0] ) )
                + ( ( t_cos + ( axis[1] * axis[1] ) * ( 1. - t_cos ) ) * ( aInput[1] - origin[1] ) )
                + ( ( -axis[0] * t_sin + axis[1] * axis[2] * ( 1. - t_cos ) ) * ( aInput[2] - origin[2] ) ) ) + trans[1] ) + origin[1];
 
        p[2] = ( ( ( ( -axis[1] * t_sin + axis[0] * axis[2] * ( 1. - t_cos ) ) * ( aInput[0] - origin[0] ) )
                + ( ( axis[0] * t_sin + axis[1] * axis[2] * ( 1. - t_cos ) ) * ( aInput[1] - origin[1] ) )
                + ( ( t_cos + ( axis[2] * axis[2] ) * ( 1. - t_cos ) ) * ( aInput[2] - origin[2] ) ) ) + trans[2] ) + origin[2];
 
        return functor ( p );
    }
 
    // ------------------------- Protected Datas ------------------------------
protected:
    RealVector axis;
    RealPoint origin;
    double t_sin;
    double t_cos;
    RealVector trans;
    TFunctor functor;
};
 
 
// Template class BackwardRigidTransformation3D
template < typename TSpace, typename TInputValue = typename TSpace::RealPoint, typename TOutputValue = typename TSpace::Point,
           typename TFunctor = VectorRounding < TInputValue, TOutputValue > >
class BackwardRigidTransformation3D
{
    BOOST_CONCEPT_ASSERT(( concepts::CSpace<TSpace> ));
    BOOST_STATIC_ASSERT(( TSpace::dimension == 3 ));
    BOOST_STATIC_ASSERT(( TOutputValue::dimension == 3 ));
    BOOST_STATIC_ASSERT(( TInputValue::dimension == 3 ));
 
    // ----------------------- Types ------------------------------
public:
    typedef typename TSpace::RealPoint RealPoint;
    typedef typename TSpace::RealVector RealVector;
 
    // ----------------------- Interface --------------------------------------
public:
    BackwardRigidTransformation3D ( const RealPoint & aOrigin, const RealVector & aAxis,
                                    const double & angle, const RealVector & aTranslate )
        : axis(aAxis.getNormalized()), origin(aOrigin), trans(aTranslate)
    {
        if ( std::isnan( axis.norm() ) )
            throw std::runtime_error ( "Axis of rotation can not be set as a vector of length 0!" );
 
        t_sin = std::sin ( angle );
        t_cos = std::cos ( angle );
    }
 
    inline
    TOutputValue operator()( const TInputValue & aInput ) const
    {
        RealPoint p;
 
        p[0] = ( ( ( ( t_cos + ( axis[0] * axis[0] ) * ( 1. - t_cos ) ) * ( aInput[0] - trans[0] - origin[0] ) )
                + ( ( axis[2] * t_sin + axis[0] * axis[1] * ( 1. - t_cos ) ) * ( aInput[1] - trans[1] - origin[1] ) )
                + ( ( -axis[1] * t_sin + axis[0] * axis[2] * ( 1. - t_cos ) ) * ( aInput[2] - trans[2] - origin[2] ) ) ) ) + origin[0];
 
        p[1] = ( ( ( ( axis[0] * axis[1] * ( 1. - t_cos ) - axis[2] * t_sin )  * ( aInput[0] - trans[0] - origin[0] ) )
                + ( ( t_cos + ( axis[1] * axis[1] ) * ( 1. - t_cos ) ) * ( aInput[1] - trans[1] - origin[1] ) )
                + ( ( axis[0] * t_sin + axis[1] * axis[2] * ( 1. - t_cos ) ) * ( aInput[2] - trans[2] - origin[2] ) ) ) ) + origin[1];
 
        p[2] = ( ( ( ( axis[1] * t_sin + axis[0] * axis[2] * ( 1. - t_cos )  ) * ( aInput[0] - trans[0] - origin[0] ) )
                + ( ( -axis[0] * t_sin + axis[1] * axis[2] * ( 1. - t_cos ) ) * ( aInput[1] - trans[1] - origin[1] ) )
                + ( ( t_cos + ( axis[2] * axis[2] ) * ( 1. - t_cos ) ) * ( aInput[2] - trans[2] - origin[2] ) ) ) ) + origin[2];
        return functor ( p );
    }
 
    // ------------------------- Protected Datas ------------------------------
private:
    RealVector axis;
    RealPoint origin;
    double t_sin;
    double t_cos;
    RealVector trans;
    TFunctor functor;
};
 
// Template class DomainRigidTransformation3D
template <typename TDomain, typename TRigidTransformFunctor >
class DomainRigidTransformation3D
{
    BOOST_STATIC_ASSERT(( TDomain::dimension == 3 ));
    BOOST_CONCEPT_ASSERT(( concepts::CDomain<TDomain> ));
 
    // ----------------------- Types ------------------------------
public:
    typedef std::pair < typename TDomain::Space::Point, typename TDomain::Space::Point > Bounds;
 
    // ----------------------- Interface --------------------------------------
public:
    DomainRigidTransformation3D ( const TRigidTransformFunctor & aRigidFunctor ) : transform ( aRigidFunctor ) {}
 
    inline
    Bounds operator()( const TDomain & aInput ) const
    {
        typedef typename TDomain::Point Point;
 
        Point points[8];
        points[0] = transform ( aInput.lowerBound() );
        points[1] = transform ( aInput.upperBound() );
        points[2] = transform ( Point ( aInput.upperBound()[0], aInput.lowerBound()[1], aInput.lowerBound()[2] ) );
        points[3] = transform ( Point ( aInput.lowerBound()[0], aInput.upperBound()[1], aInput.upperBound()[2] ) );
        points[4] = transform ( Point ( aInput.upperBound()[0], aInput.lowerBound()[1], aInput.upperBound()[2] ) );
        points[5] = transform ( Point ( aInput.lowerBound()[0], aInput.upperBound()[1], aInput.lowerBound()[2] ) );
        points[6] = transform ( Point ( aInput.lowerBound()[0], aInput.lowerBound()[1], aInput.upperBound()[2] ) );
        points[7] = transform ( Point ( aInput.upperBound()[0], aInput.upperBound()[1], aInput.lowerBound()[2] ) );
 
        Point t_min ( INT_MAX, INT_MAX, INT_MAX ), t_max ( INT_MIN, INT_MIN, INT_MIN );
        for ( int i = 0; i < 8; i++ )
        {
            if ( points[i][0] < t_min[0] )
                t_min[0] = points[i][0];
            if ( points[i][1] < t_min[1] )
                t_min[1] = points[i][1];
            if ( points[i][2] < t_min[2] )
                t_min[2] = points[i][2];
 
            if ( points[i][0] > t_max[0] )
                t_max[0] = points[i][0];
            if ( points[i][1] > t_max[1] )
                t_max[1] = points[i][1];
            if ( points[i][2] > t_max[2] )
                t_max[2] = points[i][2];
        }
        Bounds bounds;
        bounds.first = t_min;
        bounds.second = t_max;
        return bounds;
    }
 
    // ------------------------- Protected Datas ------------------------------
protected:
    const TRigidTransformFunctor & transform;
};
} // namespace DGtal::functors
} // namespace DGtal
 
 
#endif // !defined RigidTransformation3D_h
 
#undef RigidTransformation3D_RECURSES
#endif // else defined(RigidTransformation3D_RECURSES)