vctFrame4x4ConstBase.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:   2007-09-13

  (C) Copyright 2007-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 _vctFrame4x4ConstBase_h
#define _vctFrame4x4ConstBase_h

#include <cisstVector/vctFixedSizeVectorRef.h>
#include <cisstVector/vctMatrixRotation3Ref.h>
#include <cisstVector/vctMatrixRotation3ConstRef.h>
#include <cisstVector/vctExport.h>

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

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

    enum {ROWSTRIDE = ContainerType::ROWSTRIDE, COLSTRIDE = ContainerType::COLSTRIDE};

    typedef typename BaseType::RowValueType RowValueType;
    typedef typename BaseType::ColumnValueType ColumnValueType;
    typedef typename BaseType::RowRefType RowRefType;
    typedef typename BaseType::ColumnRefType ColumnRefType;
    typedef typename BaseType::ConstRowRefType ConstRowRefType;
    typedef typename BaseType::ConstColumnRefType ConstColumnRefType;
    typedef typename BaseType::DiagonalRefType DiagonalRefType;
    typedef typename BaseType::ConstDiagonalRefType ConstDiagonalRefType;
    typedef typename BaseType::RefTransposeType RefTransposeType;
    typedef typename BaseType::ConstRefTransposeType ConstRefTransposeType;
    typedef vctFrame4x4<value_type, COLSTRIDE <= ROWSTRIDE> FrameValueType;

    typedef cmnTypeTraits<value_type> TypeTraits;

    /* Types to maintain internal references to subparts of the 4x4 matrix. */
    typedef vctFixedSizeVectorRef<value_type, DIMENSION, ROWSTRIDE> TranslationRefType;
    typedef vctFixedSizeConstVectorRef<value_type, DIMENSION, ROWSTRIDE> ConstTranslationRefType;
    typedef vctFixedSizeVectorRef<value_type, DIMENSION, COLSTRIDE> PerspectiveRefType;
    typedef vctFixedSizeConstVectorRef<value_type, DIMENSION, COLSTRIDE> ConstPerspectiveRefType;
    typedef vctMatrixRotation3Ref<value_type, ROWSTRIDE, COLSTRIDE> RotationRefType;
    typedef vctMatrixRotation3ConstRef<value_type, ROWSTRIDE, COLSTRIDE> ConstRotationRefType;

protected:
    /* Internal references to rotation matrix, translation vector and
       first 3 element of last row. */
    RotationRefType RotationRef;
    TranslationRefType TranslationRef;
    PerspectiveRefType PerspectiveRef;

    inline void UpdateReferences(void) {
        // Rotation matrix is 3 by 3 starting at 0, 0
        RotationRef.SetRef(this->Pointer(0, 0));
        // Translation is a vector 3, column starting at 0, 3
        TranslationRef.SetRef(this->Pointer(0, DIMENSION));
        // Perspective is a vector 3, row starting a 3, 0
        PerspectiveRef.SetRef(this->Pointer(DIMENSION, 0));
    }

public:


    vctFrame4x4ConstBase(void) {
        this->UpdateReferences();
    }

    static CISST_EXPORT const FrameValueType & Identity(void);

    inline ConstTranslationRefType Translation(void) const {
        return this->TranslationRef;
    }

    inline ConstRotationRefType Rotation(void) const {
        return this->RotationRef;
    }

    inline ConstPerspectiveRefType Perspective(void) const {
        return this->PerspectiveRef;
    }


    FrameValueType Inverse(void) const;  // implemented in vctFrame4x4.h


    inline bool IsNormalized(value_type tolerance = TypeTraits::Tolerance()) const {
        return (this->Row(DIMENSION).AlmostEqual(RowValueType(static_cast<value_type>(0.0),
                                                              static_cast<value_type>(0.0),
                                                              static_cast<value_type>(0.0),
                                                              static_cast<value_type>(1.0)),
                                                 tolerance)
                && this->RotationRef.IsNormalized(tolerance));
    }

    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 {
        this->RotationRef.ApplyTo(input, output);
        output.X() += this->TranslationRef.X();
        output.Y() += this->TranslationRef.Y();
        output.Z() += this->TranslationRef.Z();
    }


    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;
    }


    template <class __containerType1, class __containerType2>
    inline void ApplyTo(const vctFrame4x4ConstBase<__containerType1> & input,
                        vctFrame4x4Base<__containerType2> & output) const {
        typename TranslationRefType::CopyType temp;
        RotationRef.ApplyTo(input.Translation(), temp);
        (output.Rotation()).ProductOf(RotationRef, input.Rotation());
        (output.Translation()).SumOf(temp, TranslationRef);
    }


    template <class __containerType>
    inline FrameValueType ApplyTo(const vctFrame4x4ConstBase<__containerType> & input) const {
        FrameValueType 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
    {
        this->RotationRef.ApplyTo(input, output);
        output[0] += this->TranslationRef[0];
        output[1] += this->TranslationRef[1];
        output[2] += this->TranslationRef[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
    {
        typedef vctFixedSizeConstMatrixBase<DIMENSION, __cols, __rowStride1, __colStride1, value_type, __dataPtrType1> inputType;
        typedef typename inputType::ConstColumnRefType inputColumnType;
        typedef vctFixedSizeMatrixBase<DIMENSION, __cols, __rowStride2, __colStride2, value_type, __dataPtrType2> outputType;
        typedef typename outputType::ColumnRefType outputColumnType;
        index_type columnIndex;
        for (columnIndex = 0; columnIndex < __cols; ++columnIndex) {
            const inputColumnType inputColumn(input.Column(columnIndex));
            outputColumnType outputColumn(output.Column(columnIndex));
            this->ApplyTo(inputColumn, outputColumn);
        }
    }


    inline void
    ApplyTo(size_type inputSize, const vctFixedSizeVector<value_type, DIMENSION> * input,
            vctFixedSizeVector<value_type, DIMENSION> * output) const
    {
        index_type index;
        for (index = 0; index < inputSize; ++index) {
            this->ApplyTo(input[index], output[index]);
        }
    }


    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 {
        vctFixedSizeVector<value_type, DIMENSION> temp;
        temp.DifferenceOf(input, this->TranslationRef);
        this->RotationRef.ApplyInverseTo(temp, output);
    }

    template <class __vectorOwnerType1, class __vectorOwnerType2>
    inline void
    ApplyInverseTo(const vctDynamicConstVectorBase<__vectorOwnerType1, value_type> & input,
                   vctDynamicVectorBase<__vectorOwnerType2, value_type> & output) const {
        const vctFixedSizeVector<value_type, DIMENSION> temp(input[0] - this->TranslationRef[0],
                                                             input[1] - this->TranslationRef[1],
                                                             input[2] - this->TranslationRef[2]);
        typename TranslationRefType::CopyType result;
        this->RotationRef.ApplyInverseTo(temp, result);
        output[0] = result[0];
        output[1] = result[1];
        output[2] = result[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 __containerType1, class __containerType2>
    inline void ApplyInverseTo(const vctFrame4x4ConstBase<__containerType1> & input,
                               vctFrame4x4Base<__containerType2> & output) const {
        FrameValueType inverse;
        inverse.InverseOf(*this);
        inverse.ApplyTo(input, output);
    }

    template <class __containerType>
    inline FrameValueType ApplyInverseTo(const vctFrame4x4ConstBase<__containerType> & input) const {
        FrameValueType result;
        this->ApplyInverseTo(input, result);
        return result;
    }

    inline bool Equal(const ThisType & other) const {
        return (this->RotationRef.Equal(other.Rotation())
                && this->TranslationRef.Equal(other.Translation()));
    }

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


    inline bool AlmostEqual(const ThisType & other,
                            value_type tolerance = TypeTraits::Tolerance()) const {
        return (this->RotationRef.AlmostEqual(other.Rotation(), tolerance)
                && this->TranslationRef.AlmostEqual(other.Translation(), tolerance));
    }


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

};


#endif  // _vctFrame4x4Base_h