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/base/Common.h"
#include <DGtal/kernel/domains/CDomain.h>

namespace DGtal
{
namespace functors
{
// Template class ForwardRigidTransformation3D
template <typename TSpace>
class ForwardRigidTransformation3D : std::unary_function <typename TSpace::Point, typename TSpace::Point>
{
    BOOST_CONCEPT_ASSERT(( concepts::CSpace<TSpace> ));
    BOOST_STATIC_ASSERT(( TSpace::dimension == 3 ));

    // ----------------------- Types ------------------------------
public:
    typedef typename TSpace::Point Point;
    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
    Point operator()( const Point& aInput ) const
    {
        Point p;

        p[0] = std::floor ( ( ( ( ( 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] + 0.5 );

        p[1] = std::floor ( ( ( ( ( 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] + 0.5 );

        p[2] = std::floor ( ( ( ( ( -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] + 0.5 );

        return p;
    }

    // ------------------------- Protected Datas ------------------------------
protected:
    RealVector axis;
    RealPoint origin;
    double t_sin;
    double t_cos;
    RealVector trans;
};

// Template class BackwardRigidTransformation3D
template <typename TSpace>
class BackwardRigidTransformation3D : std::unary_function <typename TSpace::Point, typename TSpace::Point>
{
    BOOST_CONCEPT_ASSERT(( concepts::CSpace<TSpace> ));
    BOOST_STATIC_ASSERT(( TSpace::dimension == 3 ));

    // ----------------------- Types ------------------------------
public:
    typedef typename TSpace::Point Point;
    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
    Point operator()( const Point& aInput ) const
    {
        Point p;

        p[0] = std::floor ( ( ( ( ( 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] + 0.5 );

        p[1] = std::floor ( ( ( ( ( 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] + 0.5 );

        p[2] = std::floor ( ( ( ( ( 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] + 0.5 );
        return p;
    }

    // ------------------------- Protected Datas ------------------------------
private:
    RealVector axis;
    RealPoint origin;
    double t_sin;
    double t_cos;
    RealVector trans;
};

// Template class DomainRigidTransformation3D
template <typename TDomain, typename TRigidTransformFunctor >
class DomainRigidTransformation3D :
        std::unary_function < std::pair < typename TDomain::Point, typename TDomain::Point >, TDomain>
{
    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] ) );

        typename Point::Component cmax = std::numeric_limits<typename Point::Component>::max();
        typename Point::Component cmin = std::numeric_limits<typename Point::Component>::min();
        Point t_min ( cmax, cmax, cmax ), t_max ( cmin, cmin, cmin );
        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)