vctQuaternionBase.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-18

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

#include <cisstCommon/cmnTypeTraits.h>
#include <cisstVector/vctContainerTraits.h>
#include <cisstVector/vctDynamicVector.h>
// always the last file to include
#include <cisstVector/vctExport.h>


template <class _containerType>
class vctQuaternionBase: public _containerType
{
protected:
    inline void Allocate(void) {}

 public:
    enum {SIZE = 4};
    typedef _containerType BaseType;
    typedef _containerType ContainerType;
    typedef vctQuaternionBase<_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;


    inline vctQuaternionBase(void)
    {
        this->Allocate();
    }


    inline vctQuaternionBase(const value_type & x,
                             const value_type & y,
                             const value_type & z,
                             const value_type & r)
    {
        this->Allocate();
        this->X() = x;
        this->Y() = y;
        this->Z() = z;
        this->R() = r;
    }


    /* Methods X, Y, Z are inherited, add methods R() */

    const_reference R(void) const {
        return *(this->Pointer(3));
    }

    reference R(void) {
        return *(this->Pointer(3));
    }


    template <class __containerTypeOther>
    inline ThisType & ConjugateOf(const vctQuaternionBase<__containerTypeOther> & otherQuaternion) {
        this->X() = - otherQuaternion.X();
        this->Y() = - otherQuaternion.Y();
        this->Z() = - otherQuaternion.Z();
        this->R() = otherQuaternion.R();
        return *this;
    }


    inline ThisType & ConjugateSelf(void) {
        this->X() = - this->X();
        this->Y() = - this->Y();
        this->Z() = - this->Z();
        return *this;
    }


    inline ThisType Conjugate(void) const {
        ThisType result;
        result.ConjugateOf(*this);
        return result;
    }


    template <class __containerType1, class __containerType2>
    inline ThisType & ProductOf(const vctQuaternionBase<__containerType1> & quat1,
                                const vctQuaternionBase<__containerType2> & quat2) {
        this->X() = quat1.R() * quat2.X() +  quat1.X() * quat2.R() +  quat1.Y() * quat2.Z() -  quat1.Z() * quat2.Y();
        this->Y() = quat1.R() * quat2.Y() -  quat1.X() * quat2.Z() +  quat1.Y() * quat2.R() +  quat1.Z() * quat2.X();
        this->Z() = quat1.R() * quat2.Z() +  quat1.X() * quat2.Y() -  quat1.Y() * quat2.X() +  quat1.Z() * quat2.R();
        this->R() = quat1.R() * quat2.R() -  quat1.X() * quat2.X() -  quat1.Y() * quat2.Y() -  quat1.Z() * quat2.Z();
        return *this;
    }

    template <class __containerType1, class __containerType2>
    inline ThisType & QuotientOf(const vctQuaternionBase<__containerType1> & quat1,
                                 const vctQuaternionBase<__containerType2> & quat2) {
        const value_type quat2Norm = value_type(quat2.Norm());
        BaseType quat2I;
        quat2I.Assign(-quat2.X() / quat2Norm, -quat2.Y() / quat2Norm, -quat2.Z() / quat2Norm, quat2.R() / quat2Norm);
        this->X() = quat1.R() * quat2I.X() +  quat1.X() * quat2.R() +  quat1.Y() * quat2I.Z() -  quat1.Z() * quat2I.Y();
        this->Y() = quat1.R() * quat2I.Y() -  quat1.X() * quat2I.Z() +  quat1.Y() * quat2.R() +  quat1.Z() * quat2I.X();
        this->Z() = quat1.R() * quat2I.Z() +  quat1.X() * quat2I.Y() -  quat1.Y() * quat2I.X() +  quat1.Z() * quat2.R();
        this->R() = quat1.R() * quat2.R() -  quat1.X() * quat2I.X() -  quat1.Y() * quat2I.Y() -  quat1.Z() * quat2I.Z();
        return *this;
    }

    template <class __containerTypeOther>
    inline ThisType & PostMultiply(const vctQuaternionBase<__containerTypeOther> & other) {
        ThisType result;
        result = ProductOf(*this, other);
        *this = result;
        return *this;
    }

    template <class __containerTypeOther>
    inline ThisType & PreMultiply(const vctQuaternionBase<__containerTypeOther> & other) {
        ThisType result;
        result = ProductOf(other, *this);
        *this = result;
        return *this;
    }

    template <class __containerTypeOther>
    inline ThisType & Divide(const vctQuaternionBase<__containerTypeOther> & other) {
        ThisType result;
        result.QuotientOf(*this, other);
        *this = result;
        return *this;
    }

    inline ThisType & Divide(const value_type s) {
        (static_cast<BaseType *>(this))->Divide(s);
        return *this;
    }

};


/* Specialization of Allocate for a dynamic matrix */
template<>
inline void vctQuaternionBase<vctDynamicVector<double> >::Allocate(void)
{
    this->SetSize(SIZE);
}

template<>
inline void vctQuaternionBase<vctDynamicVector<float> >::Allocate(void)
{
    this->SetSize(SIZE);
}



template <class _containerType>
inline vctQuaternionBase<_containerType>
operator * (const vctQuaternionBase<_containerType> & quat1,
            const vctQuaternionBase<_containerType> & quat2) {
    vctQuaternionBase<_containerType> result;
    result.ProductOf(quat1, quat2);
    return result;
}


template <class _containerType>
inline vctQuaternionBase<_containerType>
operator / (const vctQuaternionBase<_containerType> & quat1,
            const vctQuaternionBase<_containerType> & quat2) {
    vctQuaternionBase<_containerType> result;
    result.QuotientOf(quat1, quat2);
    return result;
}


#endif  // _vctQuaternionBase_h