vctMatrixRotation2Base.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-12-01

  (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 _vctMatrixRotation2Base_h
#define _vctMatrixRotation2Base_h

#include <cisstVector/vctFixedSizeMatrix.h>
#include <cisstVector/vctDynamicMatrix.h>
#include <cisstVector/vctAngleRotation2.h>
#include <cisstVector/vctExport.h>


template <class _containerType>
class vctMatrixRotation2Base: public _containerType
{
public:

    enum {ROWS = 2, COLS = 2};
    enum {DIMENSION = 2};
    typedef _containerType BaseType;
    typedef _containerType ContainerType;
    typedef vctMatrixRotation2Base<ContainerType> ThisType;

    /* no need to document, inherit doxygen documentation from base class */
    VCT_CONTAINER_TRAITS_TYPEDEFS(typename ContainerType::value_type);

    typedef cmnTypeTraits<value_type> TypeTraits;


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


    inline void Allocate(void) {}


public:

    inline vctMatrixRotation2Base()
    {
        this->Allocate();
        this->Assign(Identity());
    }

    inline ThisType & operator = (const ContainerType & other) {
        return reinterpret_cast<ThisType &>(this->Assign(other));
    }

    inline ThisType & operator = (const ThisType & other) {
        return reinterpret_cast<ThisType &>(this->Assign(other));
    }

    inline vctMatrixRotation2Base(const ThisType & other):
        BaseType()
    {
        this->Allocate();
        this->Assign(other);
    }

    inline vctMatrixRotation2Base(const BaseType & other)
    {
        this->Allocate();
        this->Assign(other);
    }





    inline vctMatrixRotation2Base(const value_type & element00, const value_type & element01,
                                  const value_type & element10, const value_type & element11)
        throw(std::runtime_error)
    {
        this->Allocate();
        this->From(element00, element01,
                   element10, element11);
    }



    template <stride_type __stride1, class __dataPtrType1,
              stride_type __stride2, class __dataPtrType2>
    inline vctMatrixRotation2Base(const vctFixedSizeConstVectorBase<2, __stride1, value_type, __dataPtrType1>& v1,
                                  const vctFixedSizeConstVectorBase<2, __stride2, value_type, __dataPtrType2>& v2,
                                  bool vectorsAreColumns = true)
        throw(std::runtime_error)
    {
        this->Allocate();
        this->From(v1, v2, vectorsAreColumns);
    }

    template <class __vectorOwnerType1,
              class __vectorOwnerType2>
    inline vctMatrixRotation2Base(const vctDynamicConstVectorBase<__vectorOwnerType1, value_type>& v1,
                                  const vctDynamicConstVectorBase<__vectorOwnerType2, value_type>& v2,
                                  bool vectorsAreColumns = true)
        throw(std::runtime_error)
    {
        this->Allocate();
        this->From(v1, v2, vectorsAreColumns);
    }

    inline vctMatrixRotation2Base(const vctAngleRotation2 & angleRotation)
        throw(std::runtime_error)
    {
        this->Allocate();
        this->From(angleRotation);
    }






    inline vctMatrixRotation2Base(const value_type & element00, const value_type & element01,
                                  const value_type & element10, const value_type & element11,
                                  bool normalizeInput)
    {
        this->Allocate();
        if (normalizeInput) {
            this->FromNormalized(element00, element01,
                                 element10, element11);
        } else {
            this->FromRaw(element00, element01,
                          element10, element11);
        }
    }



    template <stride_type __stride1, class __dataPtrType1,
              stride_type __stride2, class __dataPtrType2>
    inline vctMatrixRotation2Base(const vctFixedSizeConstVectorBase<DIMENSION, __stride1, value_type, __dataPtrType1>& v1,
                                  const vctFixedSizeConstVectorBase<DIMENSION, __stride2, value_type, __dataPtrType2>& v2,
                                  bool vectorsAreColumns, bool normalizeInput)
    {
        this->Allocate();
        if (normalizeInput) {
            this->FromNormalized(v1, v2, vectorsAreColumns);
        } else {
            this->FromRaw(v1, v2, vectorsAreColumns);
        }
    }

    template <class __vectorOwnerType1,
              class __vectorOwnerType2>
    inline vctMatrixRotation2Base(const vctDynamicConstVectorBase<__vectorOwnerType1, value_type>& v1,
                                  const vctDynamicConstVectorBase<__vectorOwnerType2, value_type>& v2,
                                  bool vectorsAreColumns, bool normalizeInput)
    {
        this->Allocate();
        if (normalizeInput) {
            this->FromNormalized(v1, v2, vectorsAreColumns);
        } else {
            this->FromRaw(v1, v2, vectorsAreColumns);
        }
    }

    inline vctMatrixRotation2Base(const vctAngleRotation2 & angleRotation,
                                  bool normalizeInput)
    {
        this->Allocate();
        if (normalizeInput) {
            this->FromNormalized(angleRotation);
        } else {
            this->FromRaw(angleRotation);
        }
    }


    template <stride_type __rowStride, stride_type __colStride, class __dataPtrType>
    explicit inline
    vctMatrixRotation2Base(const vctFixedSizeMatrixBase<ROWS, COLS, __rowStride, __colStride, value_type, __dataPtrType> & matrix)
    {
        this->Assign(matrix);
    }


    static CISST_EXPORT const ThisType & Identity();



    inline ThisType &
    From(const value_type & element00, const value_type & element01,
         const value_type & element10, const value_type & element11)
        throw(std::runtime_error)
    {
        this->FromRaw(element00, element01,
                      element10, element11);
        this->ThrowUnlessIsNormalized();
        return *this;
    }

    template <stride_type __stride1, class __dataPtrType1,
              stride_type __stride2, class __dataPtrType2>
    inline ThisType &
    From(const vctFixedSizeConstVectorBase<DIMENSION, __stride1, value_type, __dataPtrType1>& v1,
         const vctFixedSizeConstVectorBase<DIMENSION, __stride2, value_type, __dataPtrType2>& v2,
         bool vectorsAreColumns = true)
        throw(std::runtime_error)
    {
        this->FromRaw(v1, v2, vectorsAreColumns);
        this->ThrowUnlessIsNormalized();
        return *this;
    }

    template <class __vectorOwnerType1,
              class __vectorOwnerType2>
    inline ThisType &
    From(const vctDynamicConstVectorBase<__vectorOwnerType1, value_type>& v1,
         const vctDynamicConstVectorBase<__vectorOwnerType2, value_type>& v2,
         bool vectorsAreColumns = true)
        throw (std::runtime_error)
    {
        this->FromRaw(v1, v2, vectorsAreColumns);
        this->ThrowUnlessIsNormalized();
        return *this;
    }

    inline ThisType &
    From(const vctAngleRotation2 & angleRotation)
        throw(std::runtime_error)
    {
        this->ThrowUnlessIsNormalized(angleRotation);
        return this->FromRaw(angleRotation);
    }







    inline ThisType &
    FromNormalized(const value_type & element00, const value_type & element01,
                   const value_type & element10, const value_type & element11)
    {
        this->FromRaw(element00, element01,
                      element10, element11);
        this->NormalizedSelf();
        return *this;
    }

    template <stride_type __stride1, class __dataPtrType1,
              stride_type __stride2, class __dataPtrType2>
    inline ThisType &
    FromNormalized(const vctFixedSizeConstVectorBase<DIMENSION, __stride1, value_type, __dataPtrType1>& v1,
                   const vctFixedSizeConstVectorBase<DIMENSION, __stride2, value_type, __dataPtrType2>& v2,
                   bool vectorsAreColumns = true)
        throw(std::runtime_error)
    {
        this->FromRaw(v1, v2, vectorsAreColumns);
        this->NormalizedSelf();
        return *this;
    }


    template <class __vectorOwnerType1,
              class __vectorOwnerType2>
    inline ThisType &
    FromNormalized(const vctDynamicConstVectorBase<__vectorOwnerType1, value_type>& v1,
                   const vctDynamicConstVectorBase<__vectorOwnerType2, value_type>& v2,
                   bool vectorsAreColumns = true)
    {
        this->FromRaw(v1, v2, vectorsAreColumns);
        this->NormalizedSelf();
        return *this;
    }

    inline ThisType &
    FromNormalized(const vctAngleRotation2 & angleRotation)
    {
        return this->FromRaw(angleRotation.Normalized());
    }








    inline ThisType &
    FromRaw(const value_type & element00, const value_type & element01,
            const value_type & element10, const value_type & element11)
    {
        this->Assign(element00, element01,
                     element10, element11);
        return *this;
    }

    template <stride_type __stride1, class __dataPtrType1,
              stride_type __stride2, class __dataPtrType2>
    inline ThisType &
    FromRaw(const vctFixedSizeConstVectorBase<DIMENSION, __stride1, value_type, __dataPtrType1>& v1,
            const vctFixedSizeConstVectorBase<DIMENSION, __stride2, value_type, __dataPtrType2>& v2,
            bool vectorsAreColumns = true)
    {
        if (vectorsAreColumns) {
            this->Column(0).Assign(v1);
            this->Column(1).Assign(v2);
        } else {
            this->Row(0).Assign(v1);
            this->Row(1).Assign(v2);
        }
        return *this;
    }

    template <class __vectorOwnerType1,
              class __vectorOwnerType2>
    inline ThisType &
    FromRaw(const vctDynamicConstVectorBase<__vectorOwnerType1, value_type>& v1,
            const vctDynamicConstVectorBase<__vectorOwnerType2, value_type>& v2,
            bool vectorsAreColumns = true)
        throw (std::runtime_error)
    {
        CMN_ASSERT(v1.size() == DIMENSION);
        CMN_ASSERT(v2.size() == DIMENSION);
        if (vectorsAreColumns) {
            this->Column(0).Assign(v1);
            this->Column(1).Assign(v2);
        } else {
            this->Row(0).Assign(v1);
            this->Row(1).Assign(v2);
        }
        return *this;
    }

    CISST_EXPORT ThisType &
    FromRaw(const vctAngleRotation2 & angleRotation);

    inline ThisType &
    FromRaw(const ThisType & otherRotation) {
        return reinterpret_cast<ThisType &>(this->Assign(otherRotation));
    }

    template <stride_type __rowStride, stride_type __colStride, class __dataPtrType>
    inline ThisType &
    FromRaw(const vctFixedSizeMatrixBase<DIMENSION, DIMENSION, __rowStride, __colStride, value_type, __dataPtrType> & matrix) {
        this->Assign(matrix);
        return *this;
    }




    CISST_EXPORT ThisType & NormalizedSelf(void);

    inline ThisType & NormalizedOf(const ThisType & otherMatrix) {
        *this = otherMatrix;
        this->NormalizedSelf();
        return *this;
    }

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


    inline bool IsNormalized(value_type tolerance = TypeTraits::Tolerance()) const
    {
        if ((vctUnaryOperations<value_type>::AbsValue::Operate(value_type(this->Column(0).Norm() - 1)) > tolerance)
            || (vctUnaryOperations<value_type>::AbsValue::Operate(value_type(this->Column(1).Norm() - 1)) > tolerance)
            || (vctUnaryOperations<value_type>::AbsValue::Operate(this->Column(0).DotProduct(this->Column(1))) > tolerance)) {
            return false;
        } else {
            return true;
        }
    }


    inline ThisType & InverseSelf(void) {
        // could use the transpose operator but this seems more efficient
        value_type tmp;
        tmp = this->Element(0, 1);
        this->Element(0, 1) = this->Element(1, 0);
        this->Element(1, 0) = tmp;
        return *this;
    }


    inline ThisType & InverseOf(const ThisType & otherRotation) {
        this->TransposeOf(otherRotation);
        return *this;
    }


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


    template <stride_type __stride1, class __dataPtrType1, stride_type __stride2, class __dataPtrType2>
    inline void
    ApplyTo(const vctFixedSizeConstVectorBase<DIMENSION, __stride1, value_type, __dataPtrType1> & input,
            vctFixedSizeVectorBase<DIMENSION, __stride2, value_type, __dataPtrType2> & output) const {
        CMN_ASSERT(input.Pointer() != output.Pointer());
        output.ProductOf(*this, input);
    }


    template <stride_type __stride, class __dataPtrType>
    inline vctFixedSizeVector<value_type, 2>
    ApplyTo(const vctFixedSizeConstVectorBase<DIMENSION, __stride, value_type, __dataPtrType> & input) const
    {
        vctFixedSizeVector<value_type, 2> result;
        this->ApplyTo(input, result);
        return result;
    }


    inline void ApplyTo(const ThisType & input, ThisType & output) const {
        CMN_ASSERT(input.Pointer() != output.Pointer());
        output.ProductOf(*this, input);
    }


    inline ThisType
    ApplyTo(const ThisType & input) const {
        CMN_ASSERT(input.Pointer() != this->Pointer());
        ThisType result;
        this->ApplyTo(input, result);
        return result;
    }


    template <class _vectorOwnerType1, class _vectorOwnerType2>
    inline void
    ApplyTo(const vctDynamicConstVectorBase<_vectorOwnerType1, value_type> & input,
            vctDynamicVectorBase<_vectorOwnerType2, value_type> & output) const {
        CMN_ASSERT(input.Pointer() != output.Pointer());
        CMN_ASSERT(input.size() == DIMENSION);
        CMN_ASSERT(output.size() == DIMENSION);
        output[0] = this->Element(0, 0) * input[0] + this->Element(0, 1) * input[1];
        output[1] = this->Element(1, 0) * input[0] + this->Element(1, 1) * input[1];
    }


    template <stride_type __stride1, class __dataPtrType1, stride_type __stride2, class __dataPtrType2>
    inline void
    ApplyInverseTo(const vctFixedSizeConstVectorBase<DIMENSION, __stride1, value_type, __dataPtrType1> & input,
                   vctFixedSizeVectorBase<DIMENSION, __stride2, value_type, __dataPtrType2> & output) const {
        CMN_ASSERT(input.Pointer() != output.Pointer());
        output.ProductOf(this->TransposeRef(), input);
    }


    template <stride_type __stride, class __dataPtrType>
    inline vctFixedSizeVector<value_type, 2>
    ApplyInverseTo(const vctFixedSizeConstVectorBase<DIMENSION, __stride, value_type, __dataPtrType> & input) const {
        vctFixedSizeVector<value_type, 2> result;
        this->ApplyInverseTo(input, result);
        return result;
    }


    inline void ApplyInverseTo(const ThisType & input, ThisType & output) const {
        CMN_ASSERT(input.Pointer() != output.Pointer());
        output.ProductOf(this->TransposeRef(), input);
    }


    inline ThisType ApplyInverseTo(const ThisType & input) const {
        CMN_ASSERT(input.Pointer() != this->Pointer());
        ThisType result;
        this->ApplyInverseTo(input, result);
        return result;
    }


    template <class _vectorOwnerType1, class _vectorOwnerType2>
    inline void
    ApplyInverseTo(const vctDynamicConstVectorBase<_vectorOwnerType1, value_type> & input,
                   vctDynamicVectorBase<_vectorOwnerType2, value_type> & output) const
    {
        CMN_ASSERT(input.Pointer() != output.Pointer());
        CMN_ASSERT(input.size() == DIMENSION);
        CMN_ASSERT(output.size() == DIMENSION);
        output[0] = this->Element(0, 0) * input[0] + this->Element(1, 0) * input[1];
        output[1] = this->Element(0, 1) * input[0] + this->Element(1, 1) * input[1];
    }

    ThisType operator * (const ThisType & input) const
    {
        return this->ApplyTo(input);
    }

    template <stride_type __stride, class __dataPtrType>
    inline vctFixedSizeVector<value_type, 2>
    operator * (const vctFixedSizeConstVectorBase<DIMENSION, __stride, value_type, __dataPtrType> & input) const
    {
        return this->ApplyTo(input);
    }


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

};


#endif  // _vctMatrixRotation2Base_h