vctAngleRotation2.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-2007 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 _vctAngleRotation2_h
#define _vctAngleRotation2_h

#include <cisstCommon/cmnConstants.h>
#include <cisstCommon/cmnSerializer.h>

#include <cisstVector/vctFixedSizeVector.h>
#include <cisstVector/vctExport.h>

class vctAngleRotation2
{
public:
    VCT_CONTAINER_TRAITS_TYPEDEFS(double);
    enum {DIMENSION = 2};
    typedef vctAngleRotation2 ThisType;
    typedef cmnTypeTraits<AngleType> TypeTraits;

protected:
    AngleType AngleMember;


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


 public:

    inline vctAngleRotation2():
        AngleMember(0.0)
    {}


    inline explicit
    vctAngleRotation2(const AngleType angle)
        throw(std::runtime_error)
    {
        From(angle);
    }

    template <class _containerType>
    inline vctAngleRotation2(const vctMatrixRotation2Base<_containerType> & matrixRotation)
        throw(std::runtime_error)
    {
        From(matrixRotation);
    }


    inline vctAngleRotation2(const AngleType angle,
                             bool normalizeInput)
    {
        if (normalizeInput) {
            FromNormalized(angle);
        } else {
            FromRaw(angle);
        }
    }

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


    static CISST_EXPORT const ThisType & Identity();


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

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




    inline ThisType &
    From(const AngleType angle)
        throw(std::runtime_error)
    {
        this->AngleMember = angle;
        // this->ThrowUnlessIsNormalized();
        return *this;
    }

    template <class _containerType>
    inline ThisType &
    From(const vctMatrixRotation2Base<_containerType> & matrixRotation)
        throw(std::runtime_error)
    {
        this->ThrowUnlessIsNormalized(matrixRotation);
        this->AngleMember = atan2(matrixRotation.Element(0, 1), matrixRotation.Element(0, 0));
        return *this;
    }



    inline ThisType &
    FromNormalized(const AngleType angle)
    {
        this->AngleMember = angle;
        this->NormalizedSelf();
        return *this;
    }

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



    inline ThisType &
    FromRaw(const AngleType angle)
    {
        this->AngleMember = angle;
        return *this;
    }

    template <class _containerType>
    inline ThisType &
    FromRaw(const vctMatrixRotation2Base<_containerType> & matrixRotation) {
        AngleMember = atan2(matrixRotation.Element(0, 1), matrixRotation.Element(0, 0));
        return *this;
    }



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

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

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


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

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

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

    inline bool IsNormalized(AngleType CMN_UNUSED(tolerance) = TypeTraits::Tolerance()) const {
        return true;
    }


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

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

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

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

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


inline
std::ostream & operator << (std::ostream & output,
                            const vctAngleRotation2 & angleRotation) {
    angleRotation.ToStream(output);
    return output;
}


#endif  // _vctAngleRotation2_h