worldsim/include/wvector.h

/********************************************************************************
 *  WorldSim -- library for robot simulations                                   *
 *  Copyright (C) 2008-2011 Gianluca Massera <emmegian@yahoo.it>                *
 *                                                                              *
 *  This program is free software; you can redistribute it and/or modify        *
 *  it under the terms of the GNU General Public License as published by        *
 *  the Free Software Foundation; either version 2 of the License, or           *
 *  (at your option) any later version.                                         *
 *                                                                              *
 *  This program is distributed in the hope that it will be useful,             *
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of              *
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               *
 *  GNU General Public License for more details.                                *
 *                                                                              *
 *  You should have received a copy of the GNU General Public License           *
 *  along with this program; if not, write to the Free Software                 *
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA  *
 ********************************************************************************/

#ifndef WVECTOR_H
#define WVECTOR_H

#include "worldsimconfig.h"
#include "qglviewer/vec.h"
#include <cmath>

namespace farsa {

template <bool Shared>
struct FARSA_WSIM_TEMPLATE InternalData
{
    real data[4];
};

template <>
struct FARSA_WSIM_TEMPLATE InternalData<true>
{
};

template <bool Shared = false>
class FARSA_WSIM_TEMPLATE wVectorT : protected InternalData<Shared> {
public:
    wVectorT();
    template <bool OtherShared>
    wVectorT( const wVectorT<OtherShared>& );
    wVectorT( const wVectorT& );
    wVectorT( const real *ptr );
    wVectorT( real *ptr );
    wVectorT( real x, real y, real z, real w=1.0 );

    operator qglviewer::Vec() const;

    wVectorT<false> scale( real s ) const;
    wVectorT& normalize();

    real& operator[]( int i );
    const real& operator[]( int i ) const;

    wVectorT<false> operator-() const;
    const wVectorT& operator+() const;

    template <bool OtherShared>
    wVectorT<false> operator+( const wVectorT<OtherShared> &A ) const;
    template <bool OtherShared>
    wVectorT<false> operator-( const wVectorT<OtherShared> &A ) const;
    wVectorT& operator=( const wVectorT &A );
    template <bool OtherShared>
    wVectorT& operator=( const wVectorT<OtherShared> &A );
    template <bool OtherShared>
    wVectorT& operator+=( const wVectorT<OtherShared> &A );
    template <bool OtherShared>
    wVectorT& operator-=( const wVectorT<OtherShared> &A );

    template <bool OtherShared>
    bool operator==( const wVectorT<OtherShared> &A ) const;

    template <bool OtherShared>
    real operator%( const wVectorT<OtherShared> &A ) const;

    template <bool OtherShared>
    wVectorT<false> operator*( const wVectorT<OtherShared> &B ) const;

    template <bool OtherShared>
    wVectorT<false> compProduct( const wVectorT<OtherShared> &A ) const;

    real norm() const;

    template <bool SharedA, bool SharedB>
    static real distance( const wVectorT<SharedA> &A, const wVectorT<SharedB> &B );

    //template <bool OtherShared>
    wVectorT& rotateAround( wVectorT<false> axis, real theta );

    static wVectorT<false> X();
    static wVectorT<false> Y();
    static wVectorT<false> Z();

    real &x;
    real &y;
    real &z;
    real &w;
};

typedef wVectorT<false> wVector;

template <bool Shared>
inline wVectorT<false> wVectorT<Shared>::X() {
    return wVectorT<false>(1,0,0,0);
}

template <bool Shared>
inline wVectorT<false> wVectorT<Shared>::Y() {
    return wVectorT<false>(0,1,0,0);
}

template <bool Shared>
inline wVectorT<false> wVectorT<Shared>::Z() {
    return wVectorT<false>(0,0,1,0);
}

template <>
inline wVectorT<false>::wVectorT() :
    x(InternalData<false>::data[0]),
    y(InternalData<false>::data[1]),
    z(InternalData<false>::data[2]),
    w(InternalData<false>::data[3])
{
    x = 0.0;
    y = 0.0;
    z = 0.0;
    w = 1.0;
}

template <>
template <bool OtherShared>
inline wVectorT<false>::wVectorT( const wVectorT<OtherShared>& src ) :
    x(InternalData<false>::data[0]),
    y(InternalData<false>::data[1]),
    z(InternalData<false>::data[2]),
    w(InternalData<false>::data[3])
{
    x = src.x;
    y = src.y;
    z = src.z;
    w = src.w;
}

template <>
inline wVectorT<false>::wVectorT( const wVectorT<false>& src ) :
    x(InternalData<false>::data[0]),
    y(InternalData<false>::data[1]),
    z(InternalData<false>::data[2]),
    w(InternalData<false>::data[3])
{
    x = src.x;
    y = src.y;
    z = src.z;
    w = src.w;
}

template <>
inline wVectorT<false>::wVectorT(const real *ptr) :
    x(InternalData<false>::data[0]),
    y(InternalData<false>::data[1]),
    z(InternalData<false>::data[2]),
    w(InternalData<false>::data[3])
{
    x = ptr[0];
    y = ptr[1];
    z = ptr[2];
    w = 1.0;
}

template <bool Shared>
inline wVectorT<Shared>::wVectorT(real *ptr) :
    x(InternalData<Shared>::data[0]),
    y(InternalData<Shared>::data[1]),
    z(InternalData<Shared>::data[2]),
    w(InternalData<Shared>::data[3])
{
    x = ptr[0];
    y = ptr[1];
    z = ptr[2];
    w = 1.0;
}

template <>
inline wVectorT<true>::wVectorT(real *ptr) :
    x(ptr[0]),
    y(ptr[1]),
    z(ptr[2]),
    w(ptr[3])
{
    w = 1.0;
}

template <>
inline wVectorT<false>::wVectorT(real _x, real _y, real _z, real _w) :
    x(InternalData<false>::data[0]),
    y(InternalData<false>::data[1]),
    z(InternalData<false>::data[2]),
    w(InternalData<false>::data[3])
{
    this->x = _x;
    this->y = _y;
    this->z = _z;
    this->w = _w;
}

template <bool Shared>
inline wVectorT<Shared>::operator qglviewer::Vec() const {
    return qglviewer::Vec( x, y, z );
}

template <bool Shared>
inline real& wVectorT<Shared>::operator[](int i) {
    return (&x)[i];
}

template <bool Shared>
inline const real& wVectorT<Shared>::operator[](int i) const {
    return (&x)[i];
}

template <bool Shared>
inline wVectorT<false> wVectorT<Shared>::scale(real scale) const {
    return wVectorT<false>(x*scale, y*scale, z*scale, w);
}

template <bool Shared>
inline wVectorT<Shared>& wVectorT<Shared>::normalize() {
    wVectorT& self = (*this);
    real n = sqrt( self%self );
    self.x /= n;
    self.y /= n;
    self.z /= n;
    self.w = 0.0;
    return self;
}

template <bool Shared>
inline wVectorT<false> wVectorT<Shared>::operator-() const {
    return wVectorT<false>( -x, -y, -z, w );
}

template <bool Shared>
inline const wVectorT<Shared>& wVectorT<Shared>::operator+() const {
    return (*this);
}

template <bool Shared>
template <bool OtherShared>
inline wVectorT<false> wVectorT<Shared>::operator+(const wVectorT<OtherShared> &B) const {
    return wVectorT<false>(x+B.x, y+B.y, z+B.z, w);
}

template <bool Shared>
template <bool OtherShared>
inline wVectorT<false> wVectorT<Shared>::operator-(const wVectorT<OtherShared> &A) const {
    return wVectorT<false>(x-A.x, y-A.y, z-A.z, w);
}

template <bool Shared>
inline wVectorT<Shared>& wVectorT<Shared>::operator=( const wVectorT &A )
{
    x = A.x;
    y = A.y;
    z = A.z;
    w = A.w;
    return *this;
}

template <bool Shared>
template <bool OtherShared>
inline wVectorT<Shared>& wVectorT<Shared>::operator=(const wVectorT<OtherShared> &A) {
    x = A.x;
    y = A.y;
    z = A.z;
    w = A.w;
    return *this;
}

template <bool Shared>
template <bool OtherShared>
inline wVectorT<Shared>& wVectorT<Shared>::operator+=(const wVectorT<OtherShared> &A) {
    x += A.x;
    y += A.y;
    z += A.z;
    w = 1.0;
    return *this;
}

template <bool Shared>
template <bool OtherShared>
inline wVectorT<Shared>& wVectorT<Shared>::operator-=(const wVectorT<OtherShared> &A) {
    x -= A.x;
    y -= A.y;
    z -= A.z;
    w = 1.0;
    return *this;
}

template <bool Shared>
template <bool OtherShared>
inline bool wVectorT<Shared>::operator==( const wVectorT<OtherShared> &A ) const {
    return ( x==A.x && y==A.y && z==A.z );
}

template <bool Shared>
template <bool OtherShared>
inline real wVectorT<Shared>::operator%(const wVectorT<OtherShared> &A) const {
    return x*A.x + y*A.y + z*A.z;
}

template <bool Shared>
template <bool OtherShared>
inline wVectorT<false> wVectorT<Shared>::operator*(const wVectorT<OtherShared> &B) const {
    return wVectorT<false>(
            y*B.z - z*B.y,
            z*B.x - x*B.z,
            x*B.y - y*B.x,
            w);
}

template <bool Shared>
template <bool OtherShared>
inline wVectorT<false> wVectorT<Shared>::compProduct(const wVectorT<OtherShared> &A) const {
    return wVectorT<false>(x*A.x, y*A.y, z*A.z, A.w);
}

template <bool Shared>
inline real wVectorT<Shared>::norm() const {
    return sqrt( x*x + y*y + z*z );
}

template <bool Shared>
template <bool SharedA, bool SharedB>
inline real wVectorT<Shared>::distance( const wVectorT<SharedA> &A, const wVectorT<SharedB> &B ) {
    return sqrt( (A.x-B.x)*(A.x-B.x) + (A.y-B.y)*(A.y-B.y) + (A.z-B.z)*(A.z-B.z) );
}

template <bool Shared>
//template <bool OtherShared>
inline wVectorT<Shared>& wVectorT<Shared>::rotateAround( wVectorT<false> axis, real theta ) {
    wVector q(0,0,0);
    double costheta, sintheta;
    axis.normalize();
    costheta = cos(theta);
    sintheta = sin(theta);
    q.x += (costheta + (1 - costheta) * axis.x * axis.x) * x;
    q.x += ((1 - costheta) * axis.x * axis.y - axis.z * sintheta) * y;
    q.x += ((1 - costheta) * axis.x * axis.z + axis.y * sintheta) * z;

    q.y += ((1 - costheta) * axis.x * axis.y + axis.z * sintheta) * x;
    q.y += (costheta + (1 - costheta) * axis.y * axis.y) * y;
    q.y += ((1 - costheta) * axis.y * axis.z - axis.x * sintheta) * z;

    q.z += ((1 - costheta) * axis.x * axis.z - axis.y * sintheta) * x;
    q.z += ((1 - costheta) * axis.y * axis.z + axis.x * sintheta) * y;
    q.z += (costheta + (1 - costheta) * axis.z * axis.z) * z;
    x = q.x;
    y = q.y;
    z = q.z;
    return (*this);
}
} // end namespace farsa

#endif