vctEulerRotation3.h

Go to the documentation of this file.

/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*-    */
/* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */

/*

  Created on:   2011-05-18

  (C) Copyright 2011-2014 Johns Hopkins University (JHU), All Rights Reserved.

--- begin cisst license - do not edit ---

This software is provided "as is" under an open source license, with
no warranty.  The complete license can be found in license.txt and
http://www.cisst.org/cisst/license.txt.

--- end cisst license ---
*/

#ifndef _vctEulerRotation3_h
#define _vctEulerRotation3_h

#include <cisstCommon/cmnConstants.h>
#include <cisstVector/vctForwardDeclarations.h>
#include <cisstVector/vctFixedSizeVectorTypes.h>

// Always include last!
#include <cisstVector/vctExport.h>

#ifndef SWIG
// helper functions for subtemplated methods of a templated class

#define VCT_DECLARE_EULER_CONVERSIONS(ORDER) \
    template <class _matrixType> \
    void \
    vctEulerFromMatrixRotation3(vctEulerRotation3<ORDER> & eulerRot, \
                        const vctMatrixRotation3Base<_matrixType> & matrixRot); \
    template <class _matrixType> \
    void \
    vctEulerToMatrixRotation3(const vctEulerRotation3<ORDER> & eulerRot, \
                      vctMatrixRotation3Base<_matrixType> & matrixRot);

VCT_DECLARE_EULER_CONVERSIONS(vctEulerRotation3Order::ZYZ)
VCT_DECLARE_EULER_CONVERSIONS(vctEulerRotation3Order::ZYX)
#endif


namespace vctEulerRotation3Order {
    std::string CISST_EXPORT ToString(vctEulerRotation3Order::OrderType order);
};


// This base class may not be necessary; if we keep it, it could be moved to vctEulerRotation3Base.h
class CISST_EXPORT vctEulerRotation3Base {
protected:
    typedef cmnTypeTraits<double> TypeTraits;

    vct3 Angles;

    inline void ThrowUnlessIsNormalized(void) const throw(std::runtime_error) {
        if (! IsNormalized()) {
            cmnThrow(std::runtime_error("vctEulerRotation3Base: This rotation is not normalized"));
        }
    }

    template <class _inputType>
    inline void ThrowUnlessIsNormalized(const _inputType & input) const throw(std::runtime_error) {
        if (! input.IsNormalized()) {
            cmnThrow(std::runtime_error("vctEulerRotation3Base: Input is not normalized"));
        }
    }

public:
    inline vctEulerRotation3Base(void) : Angles(0.0, 0.0, 0.0) {}
    inline vctEulerRotation3Base(const vctEulerRotation3Base & other) : Angles(other.Angles) {}
    inline vctEulerRotation3Base(double phi, double theta, double psi) : Angles(phi, theta, psi) {}
    inline vctEulerRotation3Base(double * angles) : Angles(angles) {}
    inline vctEulerRotation3Base(const vct3 & angles) : Angles(angles) {}

    ~vctEulerRotation3Base() {}

    inline double phi(void) const { return Angles[0]; }
    inline double theta(void) const { return Angles[1]; }
    inline double psi(void) const { return Angles[2]; }

    inline double alpha(void) const { return Angles[0]; }
    inline double beta(void) const { return Angles[1]; }
    inline double gamma(void) const { return Angles[2]; }

    void Assign(double phi, double theta, double psi);

    vctEulerRotation3Base & InverseSelf(void);

    vctEulerRotation3Base & NormalizedSelf(void);

    bool IsNormalized(double tolerance = TypeTraits::Tolerance()) const;

};

// Euler angle class templated by order convention

template <vctEulerRotation3Order::OrderType _order>
class vctEulerRotation3 : public vctEulerRotation3Base {
    typedef vctEulerRotation3<_order> ThisType;
    typedef vctEulerRotation3Base BaseType;

public:

    inline vctEulerRotation3() : BaseType() {}
    inline vctEulerRotation3(const ThisType & other) : BaseType(other) {}
    inline vctEulerRotation3(double phi, double theta, double psi) : BaseType(phi, theta, psi) {}
    inline vctEulerRotation3(double * angles) : BaseType(angles) {}
    inline vctEulerRotation3(const vct3 & angles) : BaseType(angles) {}

    template <class __containerType>
    inline vctEulerRotation3(const vctMatrixRotation3Base<__containerType> & matrixRotation)
        throw(std::runtime_error)
    {
        From(matrixRotation);
    }

    template <class __containerType>
    inline vctEulerRotation3(const vctMatrixRotation3Base<__containerType> & matrixRotation,
                             bool normalizeInput)
    {
        if (normalizeInput) {
            FromNormalized(matrixRotation);
        } else {
            FromRaw(matrixRotation);
        }
    }

    ~vctEulerRotation3() {}

    template <class _matrixType>
    ThisType & From(const vctMatrixRotation3Base<_matrixType> & matrixRot) throw(std::runtime_error) {
        ThrowUnlessIsNormalized(matrixRot);
        return FromRaw(matrixRot);
    }

    template <class _matrixType>
    ThisType & FromNormalized(const vctMatrixRotation3Base<_matrixType> & matrixRot) {
        return FromRaw(matrixRot.Normalized());
    }

    template <class _matrixType>
    ThisType & FromRaw(const vctMatrixRotation3Base<_matrixType> & matrixRotation) {
        vctEulerFromMatrixRotation3(*this, matrixRotation);
        return *this;
    }

    vct3 GetAngles(void) const { return Angles; }

    vct3 GetAnglesInDegrees(void) const { return (180.0/cmnPI)*Angles; }

    inline ThisType & InverseOf(const ThisType & otherRotation) {
        *this = otherRotation;
        InverseSelf();
        return *this;
    }

    inline ThisType Inverse(void) const {
        ThisType result(*this);
        result.InverseSelf();
        return result;
    }

    inline ThisType & NormalizedOf(const ThisType & otherRotation) {
        Angles = otherRotation.Angles;
        NormalizedSelf();
        return *this;
    }

    inline ThisType Normalized(void) const {
        ThisType result(*this);
        result.NormalizedSelf();
        return result;
    }

    inline bool Equal(const ThisType & other) const {
        return (this->Angles == other.Angles);
    }

    inline bool operator==(const ThisType & other) const {
        return this->Equal(other);
    }


    inline bool AlmostEqual(const ThisType & other,
                            double tolerance = TypeTraits::Tolerance()) const {
        const vct3 angleDiff(this->Angles - other.Angles);
        return (angleDiff.MaxAbsElement() < tolerance);
    }


    inline bool AlmostEquivalent(const ThisType & other,
                                 double tolerance = TypeTraits::Tolerance()) const {
        ThisType thisNorm = this->Normalized();
        return thisNorm.AlmostEqual(other.Normalized(), tolerance);
    }

    std::string ToString(void) const {
        std::stringstream outputStream;
        ToStream(outputStream);
        return outputStream.str();
    }

    void ToStream(std::ostream & outputStream) const {
        outputStream << "Euler " << vctEulerRotation3Order::ToString(_order) << ": " << Angles << std::endl;
    }

    void ToStreamRaw(std::ostream & outputStream, const char delimiter = ' ',
                     bool headerOnly = false, const std::string & headerPrefix = "") const {
        this->Angles.ToStreamRaw(outputStream, delimiter, headerOnly, headerPrefix + "angle-");
    }

    void SerializeRaw(std::ostream & outputStream) const
    {
        Angles.SerializeRaw(outputStream);
    }

    void DeSerializeRaw(std::istream & inputStream)
    {
        Angles.DeSerializeRaw(inputStream);
    }

};

typedef vctEulerRotation3<vctEulerRotation3Order::ZYZ> vctEulerZYZRotation3;

typedef vctEulerRotation3<vctEulerRotation3Order::ZYX> vctEulerZYXRotation3;

#ifndef SWIG
#ifdef CISST_COMPILER_IS_MSVC
// declare instances of helper functions
#define VCT_DECLARE_EULER_CONVERSION_TEMPLATES(ORDER) \
    template CISST_EXPORT void \
    vctEulerFromMatrixRotation3(vctEulerRotation3<ORDER> & eulerRot, \
            const vctMatrixRotation3Base<vctFixedSizeMatrix<double, 3, 3, VCT_ROW_MAJOR> > & matrixRot); \
    template CISST_EXPORT void \
    vctEulerFromMatrixRotation3(vctEulerRotation3<ORDER> & eulerRot, \
            const vctMatrixRotation3Base<vctFixedSizeMatrix<double, 3, 3, VCT_COL_MAJOR> > & matrixRot); \
    template CISST_EXPORT void \
    vctEulerFromMatrixRotation3(vctEulerRotation3<ORDER> & eulerRot, \
            const vctMatrixRotation3Base<vctFixedSizeMatrix<float, 3, 3, VCT_ROW_MAJOR> > & matrixRot); \
    template CISST_EXPORT void \
    vctEulerFromMatrixRotation3(vctEulerRotation3<ORDER> & eulerRot, \
            const vctMatrixRotation3Base<vctFixedSizeMatrix<float, 3, 3, VCT_COL_MAJOR> > & matrixRot); \
    template CISST_EXPORT void \
    vctEulerFromMatrixRotation3(vctEulerRotation3<ORDER> & eulerRot, \
            const vctMatrixRotation3Base<vctFixedSizeMatrixRef<double, 3, 3, 4, 1> > & matrixRot); \
    template CISST_EXPORT void \
    vctEulerFromMatrixRotation3(vctEulerRotation3<ORDER> & eulerRot, \
            const vctMatrixRotation3Base<vctFixedSizeMatrixRef<double, 3, 3, 1, 4> > & matrixRot); \
    template CISST_EXPORT void \
    vctEulerFromMatrixRotation3(vctEulerRotation3<ORDER> & eulerRot, \
            const vctMatrixRotation3Base<vctFixedSizeMatrixRef<float, 3, 3, 4, 1> > & matrixRot); \
    template CISST_EXPORT void \
    vctEulerFromMatrixRotation3(vctEulerRotation3<ORDER> & eulerRot, \
            const vctMatrixRotation3Base<vctFixedSizeMatrixRef<float, 3, 3, 1, 4> > & matrixRot); \
    template CISST_EXPORT void  \
    vctEulerToMatrixRotation3(const vctEulerRotation3<ORDER> & eulerRot,  \
            vctMatrixRotation3Base<vctFixedSizeMatrix<double, 3, 3, VCT_ROW_MAJOR> > & matrixRot); \
    template CISST_EXPORT void  \
    vctEulerToMatrixRotation3(const vctEulerRotation3<ORDER> & eulerRot,  \
            vctMatrixRotation3Base<vctFixedSizeMatrix<double, 3, 3, VCT_COL_MAJOR> > & matrixRot); \
    template CISST_EXPORT void  \
    vctEulerToMatrixRotation3(const vctEulerRotation3<ORDER> & eulerRot,  \
            vctMatrixRotation3Base<vctFixedSizeMatrix<float, 3, 3, VCT_ROW_MAJOR> > & matrixRot); \
    template CISST_EXPORT void  \
    vctEulerToMatrixRotation3(const vctEulerRotation3<ORDER> & eulerRot,  \
            vctMatrixRotation3Base<vctFixedSizeMatrix<float, 3, 3, VCT_COL_MAJOR> > & matrixRot); \
    template CISST_EXPORT void \
    vctEulerToMatrixRotation3(const vctEulerRotation3<ORDER> & eulerRot, \
            vctMatrixRotation3Base<vctFixedSizeMatrixRef<double, 3, 3, 4, 1> > & matrixRot); \
    template CISST_EXPORT void \
    vctEulerToMatrixRotation3(const vctEulerRotation3<ORDER> & eulerRot, \
            vctMatrixRotation3Base<vctFixedSizeMatrixRef<double, 3, 3, 1, 4> > & matrixRot); \
    template CISST_EXPORT void \
    vctEulerToMatrixRotation3(const vctEulerRotation3<ORDER> & eulerRot, \
            vctMatrixRotation3Base<vctFixedSizeMatrixRef<float, 3, 3, 4, 1> > & matrixRot); \
    template CISST_EXPORT void \
    vctEulerToMatrixRotation3(const vctEulerRotation3<ORDER> & eulerRot, \
            vctMatrixRotation3Base<vctFixedSizeMatrixRef<float, 3, 3, 1, 4> > & matrixRot);

VCT_DECLARE_EULER_CONVERSION_TEMPLATES(vctEulerRotation3Order::ZYZ)
VCT_DECLARE_EULER_CONVERSION_TEMPLATES(vctEulerRotation3Order::ZYX)
#endif // CISST_COMPILER_IS_MSVC
#endif // !SWIG

#endif  // _vctEulerRotation3_h