vctAxisAngleRotation3.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: */

/*

  Author(s):    Anton Deguet
  Created on:   2005-01-13

  (C) Copyright 2005-2012 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 ---
*/

#pragma once
#ifndef _vctAxisAngleRotation3_h
#define _vctAxisAngleRotation3_h

#include <cisstCommon/cmnConstants.h>

#include <cisstVector/vctFixedSizeVector.h>
#include <cisstVector/vctDynamicVector.h>
#include <cisstVector/vctQuaternionRotation3Base.h>
#include <cisstVector/vctMatrixRotation3Base.h>
#include <cisstVector/vctRodriguezRotation3Base.h>
#include <cisstVector/vctExport.h>

#ifndef DOXYGEN
#ifndef SWIG

template <class _elementType, class _containerType>
void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<_elementType> & axisAngleRotation,
                             const vctQuaternionRotation3Base<_containerType> & quaternionRotation);

template <class _elementType, class _containerType>
void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<_elementType> & axisAngleRotation,
                             const vctMatrixRotation3Base<_containerType> & matrixRotation);

template <class _elementType, class _containerType>
void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<_elementType> & axisAngleRotation,
                             const vctRodriguezRotation3Base<_containerType> & rodriguezRotation);

#ifdef CISST_COMPILER_IS_MSVC
template CISST_EXPORT void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<double> & axisAngleRotation,
                             const vctQuaternionRotation3Base<vctFixedSizeVector<double, 4> > & quaternionRotation);
template CISST_EXPORT void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<float> & axisAngleRotation,
                             const vctQuaternionRotation3Base<vctFixedSizeVector<float, 4> > & quaternionRotation);
template CISST_EXPORT void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<double> & axisAngleRotation,
                             const vctMatrixRotation3Base<vctFixedSizeMatrix<double, 3, 3> > & matrixRotation);
template CISST_EXPORT void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<float> & axisAngleRotation,
                             const vctMatrixRotation3Base<vctFixedSizeMatrix<float, 3, 3> > & matrixRotation);
template CISST_EXPORT void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<double> & axisAngleRotation,
                             const vctRodriguezRotation3Base<vctFixedSizeVector<double, 3> > & rodriguezRotation);
template CISST_EXPORT void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<float> & axisAngleRotation,
                             const vctRodriguezRotation3Base<vctFixedSizeVector<float, 3> > & rodriguezRotation);
#endif
#endif // SWIG
#endif // DOXYGEN


template <class _elementType>
class vctAxisAngleRotation3
{
public:
    /* no need to document, inherit doxygen documentation from vctFixedSizeVectorBase */
    VCT_CONTAINER_TRAITS_TYPEDEFS(_elementType);
    enum {DIMENSION = 3};
    typedef vctAxisAngleRotation3<value_type> ThisType;
    typedef vctFixedSizeVector<value_type, DIMENSION> AxisType;
    typedef cmnTypeTraits<_elementType> TypeTraits;

protected:
    AxisType AxisMember;
    AngleType AngleMember;


    inline void ThrowUnlessIsNormalized(void) const throw(std::runtime_error) {
        if (! IsNormalized()) {
            cmnThrow(std::runtime_error("vctAxisAngleRotation3: 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("vctAxisAngleRotation3: Input is not normalized"));
        }
    }

public:

    inline vctAxisAngleRotation3():
        AxisMember(1.0, 0.0, 0.0),
        AngleMember(0.0)
    {}


    inline ThisType & Assign(const ThisType & other) {
        AxisMember.Assign(other.Axis());
        AngleMember = other.Angle();
        return *this;
    }




    template <stride_type __stride, class __dataPtrType>
    inline vctAxisAngleRotation3(const vctFixedSizeConstVectorBase<DIMENSION, __stride, value_type, __dataPtrType> & axis,
                                 const AngleType angle)
        throw(std::runtime_error)
    {
        From(axis, angle);
    }

    template <class _vectorOwnerType>
    inline vctAxisAngleRotation3(const vctDynamicConstVectorBase<_vectorOwnerType, value_type> & axis,
                                 AngleType angle)
        throw(std::runtime_error)
    {
        From(axis, angle);
    }

    template <class _containerType>
    explicit inline
    vctAxisAngleRotation3(const vctQuaternionRotation3Base<_containerType> & quaternionRotation)
        throw(std::runtime_error)
    {
        From(quaternionRotation);
    }

    template <class _containerType>
    explicit inline
    vctAxisAngleRotation3(const vctMatrixRotation3Base<_containerType> & matrixRotation)
        throw(std::runtime_error)
    {
        From(matrixRotation);
    }

    template <class _containerType>
    explicit inline
    vctAxisAngleRotation3(const vctRodriguezRotation3Base<_containerType> & rodriguezRotation)
        throw(std::runtime_error)
    {
        From(rodriguezRotation);
    }




    template <stride_type __stride, class __dataPtrType>
    inline vctAxisAngleRotation3(const vctFixedSizeConstVectorBase<DIMENSION, __stride, value_type, __dataPtrType> & axis,
                                 const AngleType angle,
                                 bool normalizeInput)
    {
        if (normalizeInput) {
            FromNormalized(axis, angle);
        } else {
            FromRaw(axis, angle);
        }
    }

    template <class _vectorOwnerType>
    inline vctAxisAngleRotation3(const vctDynamicConstVectorBase<_vectorOwnerType, value_type> & axis,
                                 AngleType angle,
                                 bool normalizeInput)
    {
        if (normalizeInput) {
            FromNormalized(axis, angle);
        } else {
            FromRaw(axis, angle);
        }
    }

    template <class _containerType>
    inline vctAxisAngleRotation3(const vctQuaternionRotation3Base<_containerType> & quaternionRotation,
                                 bool normalizeInput)
    {
        if (normalizeInput) {
            FromNormalized(quaternionRotation);
        } else {
            FromRaw(quaternionRotation);
        }
    }

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

    template <class _containerType>
    inline vctAxisAngleRotation3(const vctRodriguezRotation3Base<_containerType> & rodriguezRotation,
                                 bool normalizeInput)
    {
        if (normalizeInput) {
            FromNormalized(rodriguezRotation);
        } else {
            FromRaw(rodriguezRotation);
        }
    }




    static CISST_EXPORT const ThisType & Identity();

    inline const AxisType & Axis(void) const {
        return AxisMember;
    }

    inline AxisType & Axis(void) {
        return AxisMember;
    }

    inline const AngleType & Angle(void) const {
        return AngleMember;
    }

    inline AngleType & Angle(void) {
        return AngleMember;
    }



    template <stride_type __stride, class __dataPtrType>
    inline ThisType & From(const vctFixedSizeConstVectorBase<DIMENSION, __stride, value_type, __dataPtrType> & axis,
                           const AngleType angle)
        throw(std::runtime_error)
    {
        FromRaw(axis, angle);
        ThrowUnlessIsNormalized();
        return *this;
    }

    template <class _vectorOwnerType>
    inline ThisType & From(const vctDynamicConstVectorBase<_vectorOwnerType, value_type> & axis,
                           AngleType angle)
        throw(std::runtime_error)
    {
        AxisMember[0] = axis[0];
        AxisMember[1] = axis[1];
        AxisMember[2] = axis[2];
        AngleMember = angle;
        ThrowUnlessIsNormalized();
        return *this;
    }

    template <class _containerType>
    inline ThisType & From(const vctQuaternionRotation3Base<_containerType> & quaternionRotation)
        throw(std::runtime_error)
    {
        ThrowUnlessIsNormalized(quaternionRotation);
        return FromRaw(quaternionRotation);
    }

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

    template <class _containerType>
    inline ThisType & From(const vctRodriguezRotation3Base<_containerType> & rodriguezRotation)
        throw(std::runtime_error)
    {
        ThrowUnlessIsNormalized(rodriguezRotation);
        return FromRaw(rodriguezRotation);
    }





    template <stride_type __stride, class __dataPtrType>
    inline ThisType & FromNormalized(const vctFixedSizeConstVectorBase<DIMENSION, __stride, value_type, __dataPtrType> & axis,
                                           const AngleType angle)
    {
        FromRaw(axis, angle);
        NormalizedSelf();
        return *this;
    }

    template <class _containerType>
    inline ThisType & FromNormalized(const vctQuaternionRotation3Base<_containerType> & quaternionRotation) {
        return FromRaw(quaternionRotation.Normalized());
    }

    template <class _containerType>
    inline ThisType & FromNormalized(const vctMatrixRotation3Base<_containerType> & matrixRotation) {
        return FromRaw(matrixRotation.Normalized());
    }

    template <class _containerType>
    inline ThisType & FromNormalized(const vctRodriguezRotation3Base<_containerType> & rodriguezRotation) {
        return FromRaw(rodriguezRotation.Normalized());
    }




    template <stride_type __stride, class __dataPtrType>
    inline ThisType & FromRaw(const vctFixedSizeConstVectorBase<DIMENSION, __stride, value_type, __dataPtrType> & axis,
                                    const AngleType angle)
    {
        AxisMember.Assign(axis);
        AngleMember = angle;
        return *this;
    }

    template <class _containerType>
    inline ThisType & FromRaw(const vctQuaternionRotation3Base<_containerType> & quaternionRotation) {
        vctAxisAngleRotation3FromRaw(*this, quaternionRotation);
        return *this;
    }

    template <class _containerType>
    inline ThisType & FromRaw(const vctMatrixRotation3Base<_containerType> & matrixRotation) {
        vctAxisAngleRotation3FromRaw(*this, matrixRotation);
        return *this;
    }

    template <class _containerType>
    inline ThisType & FromRaw(const vctRodriguezRotation3Base<_containerType> & rodriguezRotation) {
        vctAxisAngleRotation3FromRaw(*this, rodriguezRotation);
        return *this;
    }



    inline ThisType & InverseSelf(void) {
        AngleMember = 2.0 * cmnPI - AngleMember;
        return *this;
    }

    inline ThisType & InverseOf(const ThisType & otherRotation) {
        AxisMember.Assign(otherRotation.Axis());
        AngleMember = 2.0 * cmnPI - otherRotation.Angle();
        return *this;
    }

    inline ThisType Inverse(void) const {
        ThisType result;
        result.Angle() = 2 * cmnPI - this->Angle();
        return result;
    }

    inline ThisType & NormalizedSelf(void) {
        AxisMember.NormalizedSelf();
        AngleMember = fmod(AngleMember, 2.0 * cmnPI);
        if (AngleMember < 0.0) {
            AngleMember += (2.0 * cmnPI);
        }
        return *this;
    }

    inline ThisType & NormalizedOf(const ThisType & otherRotation) {
        AxisMember.Assign(otherRotation.Axis());
        AngleMember = otherRotation.Angle();
        NormalizedSelf();
        return *this;
    }

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


    inline bool IsNormalized(value_type tolerance = TypeTraits::Tolerance()) const {
        return AxisMember.IsNormalized(tolerance);
    }


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

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


    inline bool AlmostEqual(const ThisType & other,
                            value_type tolerance = TypeTraits::Tolerance()) const {
        const AngleType angleDiff = this->Angle() - other.Angle();
        const AngleType angle = (angleDiff < 0) ? -angleDiff : angleDiff;

        return ((angle <= tolerance) && (this->Axis().AlmostEqual(other.Axis(), tolerance)));
    }


    inline bool AlmostEquivalent(const ThisType & other,
                                 value_type tolerance = TypeTraits::Tolerance()) const {
        const AngleType angleThis = this->Angle();
        const AngleType angleOther = other.Angle();
        // two quasi null rotations
        if ((angleThis <= tolerance) && (angleOther <= tolerance)) {
            return true;
        }
        // one rotation is almost null
        if ((angleThis <= tolerance) || (angleOther <= tolerance)) {
            return false;
        }

        // both rotations are non null
        const AngleType dotProduct = AngleType(this->Axis().Normalized().DotProduct(other.Axis().Normalized()));
        const AngleType absDotProduct = dotProduct > value_type(0.0) ? dotProduct : -dotProduct;
        // axis don't seem aligned
        if ((absDotProduct <= (AngleType(1.0) - tolerance))
            || (absDotProduct >= (AngleType(1.0) + tolerance))) {
            return false;
        }
        // axis are aligned
        const AngleType angleDifference = angleThis - dotProduct * angleOther;
        AngleType angleRemain = fmod(angleDifference, 2.0 * cmnPI);
        if (angleRemain > cmnPI) {
            angleRemain -= (2.0 * cmnPI);
        } else if (angleRemain < -cmnPI) {
            angleRemain += (2.0 *cmnPI);
        }
        if ((angleRemain > -tolerance) && (angleRemain < tolerance)) {
            return true;
        }
        return false;
    }


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

    void ToStream(std::ostream & outputStream) const {
        outputStream << "axis: "
                     << std::endl
                     << this->Axis()
                     << std::endl
                     << "angle: "
                     << std::endl
                     << this->Angle();
    }

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

    void SerializeRaw(std::ostream & outputStream) const
    {
        this->Axis().SerializeRaw(outputStream);
        cmnSerializeRaw(outputStream, this->Angle());
    }

    void DeSerializeRaw(std::istream & inputStream)
    {
        this->Axis().DeSerializeRaw(inputStream);
        cmnDeSerializeRaw(inputStream, this->Angle());
    }

};


template<class _elementType>
std::ostream & operator << (std::ostream & output,
                            const vctAxisAngleRotation3<_elementType> & axisAngle) {
    axisAngle.ToStream(output);
    return output;
}


#endif  // _vctAxisAngleRotation3_h