vctMatrixRotation3ConstBase.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-08-19

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

#include <cisstVector/vctFixedSizeMatrix.h>
#include <cisstVector/vctDynamicMatrix.h>
#include <cisstVector/vctForwardDeclarations.h>

#include <cisstVector/vctExport.h>

template <class _containerType>
class vctMatrixRotation3ConstBase: public _containerType
{
public:
    enum {ROWS = 3, COLS = 3};
    enum {DIMENSION = 3};
    typedef _containerType BaseType;
    typedef _containerType ContainerType;
    typedef vctMatrixRotation3ConstBase<ContainerType> ThisType;

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

    typedef vctMatrixRotation3<value_type> RotationValueType;

    typedef cmnTypeTraits<value_type> TypeTraits;


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

public:

    static CISST_EXPORT const RotationValueType & Identity();

    RotationValueType Normalized(void) const; // implemented in vctMatrixRotation3.h

    inline bool IsNormalized(value_type tolerance = TypeTraits::Tolerance()) const {
        NormType columnNorm;
        columnNorm = this->Column(0).Norm();
        if (vctUnaryOperations<NormType>::AbsValue::Operate(columnNorm - 1) > tolerance)
            return false;
        columnNorm = this->Column(1).Norm();
        if (vctUnaryOperations<NormType>::AbsValue::Operate(columnNorm - 1) > tolerance)
            return false;
        columnNorm = this->Column(2).Norm();
        if (vctUnaryOperations<NormType>::AbsValue::Operate(columnNorm - 1) > tolerance)
            return false;
        value_type columnDot;
        columnDot = vctDotProduct(this->Column(0), this->Column(1));
        if (vctUnaryOperations<value_type>::AbsValue::Operate(columnDot) > tolerance)
            return false;
        columnDot = vctDotProduct(this->Column(0), this->Column(2));
        if (vctUnaryOperations<value_type>::AbsValue::Operate(columnDot) > tolerance)
            return false;
        columnDot = vctDotProduct(this->Column(1), this->Column(2));
        if (vctUnaryOperations<value_type>::AbsValue::Operate(columnDot) > tolerance)
            return false;
        return true;
    }

    RotationValueType Inverse(void) const; // implemented in vctMatrixRotation3.h


    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());
        // Implementation note: We think working on a local copy of the input
        // is faster than working on the members, because of fewer indirections,
        // especially when combined with the dot product operation.
        const vctFixedSizeVector<value_type, 3> inputCopy( input[0], input[1], input[2] );
        output[0] = vctDotProduct( this->Row(0), inputCopy );
        output[1] = vctDotProduct( this->Row(1), inputCopy );
        output[2] = vctDotProduct( this->Row(2), inputCopy );
    }


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


    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
        throw(std::runtime_error)
    {
        CMN_ASSERT(input.Pointer() != output.Pointer());
        CMN_ASSERT(input.size() == DIMENSION);
        CMN_ASSERT(output.size() == DIMENSION);
        // see implementation note for fixed size i/o version
        const vctFixedSizeVector<value_type, 3> inputCopy( input[0], input[1], input[2] );
        output[0] = vctDotProduct( this->Row(0), inputCopy );
        output[1] = vctDotProduct( this->Row(1), inputCopy );
        output[2] = vctDotProduct( this->Row(2), inputCopy );
    }


    template <class __vectorOwnerType, stride_type __stride, class __dataPtrType>
    inline void
    ApplyTo(const vctDynamicConstVectorBase<__vectorOwnerType, value_type> & input,
            vctFixedSizeVectorBase<DIMENSION, __stride, value_type, __dataPtrType> & output) const
        throw(std::runtime_error)
    {
        CMN_ASSERT(input.Pointer() != output.Pointer());
        CMN_ASSERT(input.size() == DIMENSION);
        // see implementation note for fixed size i/o version
        const vctFixedSizeVector<value_type, 3> inputCopy( input[0], input[1], input[2] );
        output[0] = vctDotProduct( this->Row(0), inputCopy );
        output[1] = vctDotProduct( this->Row(1), inputCopy );
        output[2] = vctDotProduct( this->Row(2), inputCopy );
    }


    template <class __vectorOwnerType>
    inline vctFixedSizeVector<value_type, DIMENSION>
    ApplyTo(const vctDynamicConstVectorBase<__vectorOwnerType, value_type> & input) const {
        vctFixedSizeVector<value_type, DIMENSION> result;
        this->ApplyTo(input, result);
        return result;
    }

    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());
        // see implementation note for fixed size i/o ApplyTo() version
        const vctFixedSizeVector<value_type, 3> inputCopy( input[0], input[1], input[2] );
        output[0] = vctDotProduct( inputCopy, this->Column(0) );
        output[1] = vctDotProduct( inputCopy, this->Column(1) );
        output[2] = vctDotProduct( inputCopy, this->Column(2) );
    }


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

    template <class __vectorOwnerType>
    inline vctFixedSizeVector<value_type, DIMENSION>
    ApplyInverseTo(const vctDynamicConstVectorBase<__vectorOwnerType, value_type> & input) const {
        vctFixedSizeVector<value_type, DIMENSION> 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);
        // see implementation note for fixed size i/o ApplyTo() version
        const vctFixedSizeVector<value_type, 3> inputCopy( input[0], input[1], input[2] );
        output[0] = vctDotProduct( inputCopy, this->Column(0) );
        output[1] = vctDotProduct( inputCopy, this->Column(1) );
        output[2] = vctDotProduct( inputCopy, this->Column(2) );
    }

    template <class __vectorOwnerType, stride_type __stride, class __dataPtrType>
    inline void
    ApplyInverseTo(const vctDynamicConstVectorBase<__vectorOwnerType, value_type> & input,
                   vctFixedSizeVectorBase<DIMENSION, __stride, value_type, __dataPtrType> & output) const
    {
        CMN_ASSERT(input.Pointer() != output.Pointer());
        CMN_ASSERT(input.size() == DIMENSION);
        // see implementation note for fixed size i/o ApplyTo() version
        const vctFixedSizeVector<value_type, 3> inputCopy( input[0], input[1], input[2] );
        output[0] = vctDotProduct( inputCopy, this->Column(0) );
        output[1] = vctDotProduct( inputCopy, this->Column(1) );
        output[2] = vctDotProduct( inputCopy, this->Column(2) );
    }

    template <size_type __cols,
              stride_type __rowStride1, stride_type __colStride1, class __dataPtrType1,
              stride_type __rowStride2, stride_type __colStride2, class __dataPtrType2>
    inline void ApplyTo(const vctFixedSizeConstMatrixBase<DIMENSION, __cols, __rowStride1, __colStride1, value_type, __dataPtrType1> & input,
                        vctFixedSizeMatrixBase<DIMENSION, __cols, __rowStride2, __colStride2, value_type, __dataPtrType2> & output) const
    {
        CMN_ASSERT(input.Pointer() != output.Pointer());
        output.ProductOf(*this, input);
    }

    template <class __matrixOwnerType1, class __matrixOwnerType2>
    inline void ApplyTo(const vctDynamicConstMatrixBase<__matrixOwnerType1, value_type> & input,
                        vctDynamicMatrixBase<__matrixOwnerType2, value_type> & output) const
    {
        CMN_ASSERT((input.rows() == DIMENSION) && (output.rows() == DIMENSION) && (input.cols() == output.cols()));
        CMN_ASSERT(input.Pointer() != output.Pointer());
        vctDynamicConstMatrixRef<value_type> myRef(*this);
        output.ProductOf(myRef, input);
    }

    template <size_type __cols,
              stride_type __rowStride1, stride_type __colStride1, class __dataPtrType1,
              stride_type __rowStride2, stride_type __colStride2, class __dataPtrType2>
    inline void ApplyInverseTo(const vctFixedSizeConstMatrixBase<DIMENSION, __cols, __rowStride1, __colStride1, value_type, __dataPtrType1> & input,
                               vctFixedSizeMatrixBase<DIMENSION, __cols, __rowStride2, __colStride2, value_type, __dataPtrType2> & output) const
    {
        CMN_ASSERT(input.Pointer() != output.Pointer());
        output.ProductOf(this->TransposeRef(), input);
    }

    template <class __matrixOwnerType1, class __matrixOwnerType2>
    inline void ApplyInverseTo(const vctDynamicConstMatrixBase<__matrixOwnerType1, value_type> & input,
                               vctDynamicMatrixBase<__matrixOwnerType2, value_type> & output) const
    {
        CMN_ASSERT((input.rows() == DIMENSION) && (output.rows() == DIMENSION) && (input.cols() == output.cols()));
        CMN_ASSERT(input.Pointer() != output.Pointer());
        vctDynamicConstMatrixRef<value_type> myInvRef(this->TransposeRef());
        output.ProductOf(myInvRef, input);
    }

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

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

    template <class __vectorOwnerType1>
    inline vctFixedSizeVector<value_type, DIMENSION>
    operator * (const vctDynamicConstVectorBase<__vectorOwnerType1, value_type> & input) const
    {
        return this->ApplyTo(input);
    }



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


#endif  // _vctMatrixRotation3ConstBase_h