homogeneous_point.hpp

Go to the documentation of this file.

/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef ECL_GEOMETRY_HOMOGENEOUS_POINT_HPP_
#define ECL_GEOMETRY_HOMOGENEOUS_POINT_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

#include <ecl/mpl.hpp>
#include <ecl/type_traits.hpp>

#include <ecl/config/macros.hpp>
#include <ecl/exceptions/standard_exception.hpp>
#include <ecl/linear_algebra.hpp>
#include <ecl/math.hpp>

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace ecl {

/*****************************************************************************
** Interface [HomogeneousPoint]
*****************************************************************************/
template <typename T, typename Enable = void>
class HomogeneousPoint : public ecl::FailedObject {};

template<typename T>
class HomogeneousPoint<T, typename ecl::enable_if< ecl::is_float<T> >::type> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
        HomogeneousPoint(const T &value = T()) {
                elements.template block<3,1>(0,0) = ecl::linear_algebra::Matrix<T,3,1>::Constant(value);
                elements(3,0) = 1.0;
        }
        HomogeneousPoint(const ecl::linear_algebra::Matrix<T,3,1> &vec) {
                elements.template block<3,1>(0,0) = vec;
                elements[3] = 1.0;
        }
        HomogeneousPoint(const ecl::linear_algebra::Matrix<T,4,1> &vec) : elements(vec) {
                ecl_assert_throw(elements[3] == 1.0, ecl::StandardException(LOC,ecl::InvalidInputError));
        }
        HomogeneousPoint(const T &x_i, const T &y_i, const T &z_i) {
                elements << x_i, y_i, z_i, 1.0;
        }

        virtual ~HomogeneousPoint() {}

    /*********************
    ** Assignment
    **********************/
        ecl::linear_algebra::CommaInitializer< ecl::linear_algebra::Matrix<T,4,1> > operator<<(const T &value) {
                return elements.operator<<(value);
    }

        ecl::linear_algebra::Matrix<T,4,1>& positionVector() { return elements; }

        const ecl::linear_algebra::Matrix<T,4,1>& positionVector() const { return elements; }

        const T& x() const { return elements[0]; }
        const T& y() const { return elements[1]; }
        const T& z() const { return elements[2]; }
        T x() { return elements[0]; }
        T y() { return elements[1]; }
        T z() { return elements[2]; }

        void x(const T& value) { elements[0] = value; } 
        void y(const T& value) { elements[1] = value; } 
        void z(const T& value) { elements[2] = value; } 
        template <typename OutputStream, typename Type>
        friend OutputStream& operator<<(OutputStream &ostream , const HomogeneousPoint<Type> &point);

private:
        ecl::linear_algebra::Matrix<T,4,1> elements;
};

typedef HomogeneousPoint<float> HomogeneousPointf;  
typedef HomogeneousPoint<double> HomogeneousPointd;  
/*********************
** Streaming
**********************/
template <typename OutputStream, typename Type>
OutputStream& operator<<(OutputStream &ostream , const HomogeneousPoint<Type> &point) {
        ostream << "[ " << point.x() << " " << point.y() << " " << point.z() << " ]";
        return ostream;
}
}  // namespace ecl

#endif /* ECL_GEOMETRY_HOMOGENEOUS_POINT_HPP_ */