MultiVector.hpp

/***************************************************************************
  tag: Peter Soetens  Thu Oct 10 16:21:19 CEST 2002  MultiVector.hpp

                        MultiVector.hpp -  description
                           -------------------
    begin                : Thu October 10 2002
    copyright            : (C) 2002 Peter Soetens
    email                : peter.soetens@mech.kuleuven.ac.be

 ***************************************************************************
 *   This library 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;                 *
 *   version 2 of the License.                                             *
 *                                                                         *
 *   As a special exception, you may use this file as part of a free       *
 *   software library without restriction.  Specifically, if other files   *
 *   instantiate templates or use macros or inline functions from this     *
 *   file, or you compile this file and link it with other files to        *
 *   produce an executable, this file does not by itself cause the         *
 *   resulting executable to be covered by the GNU General Public          *
 *   License.  This exception does not however invalidate any other        *
 *   reasons why the executable file might be covered by the GNU General   *
 *   Public License.                                                       *
 *                                                                         *
 *   This library 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     *
 *   Lesser General Public License for more details.                       *
 *                                                                         *
 *   You should have received a copy of the GNU General Public             *
 *   License along with this library; if not, write to the Free Software   *
 *   Foundation, Inc., 59 Temple Place,                                    *
 *   Suite 330, Boston, MA  02111-1307  USA                                *
 *                                                                         *
 ***************************************************************************/


#ifndef MULTIVECTOR_HPP
#define MULTIVECTOR_HPP

#include "../rtt-config.h"

#include <ostream>
#ifdef OS_HAVE_STREAMS
#include <sstream>
#endif

#include <vector>

#ifdef ORO_PRAGMA_INTERFACE
#pragma interface
#endif

namespace RTT
{ namespace extras {


    template <unsigned S = 6, class T = double>
    struct MultiVector
    {
        enum Size {size = S};

        typedef T DataType[ S ];

        DataType data;

        operator const T* () const
        {
            return data;
        }

        operator T&()
        {
            return *data;
        }

        MultiVector( const T d )
        {
            for ( unsigned int i = 0; i < S;++i )
                data[ i ] = d;
        }

        MultiVector()
        {
            for ( unsigned int i = 0; i < S;++i )
                data[ i ] = 0;
        }

        template<class Alloc>
        MultiVector(const std::vector<T,Alloc>& vect)
        {
            for ( unsigned int i = 0; i < S;++i )
                if ( i < vect.size() )
                    data[ i ] = vect[i];
                else
                    data[ i ] = 0;
        }

        MultiVector& operator += ( const MultiVector& d )
        {
            for ( unsigned int i = 0; i < S; ++i )
                data[ i ] += d.data[ i ];

            return *this;
        }

        MultiVector& operator *= ( const MultiVector& d )
        {
            for ( unsigned int i = 0; i < S; ++i )
                data[ i ] *= d.data[ i ];

            return *this;
        }

        MultiVector& operator *= ( const T d )
        {
            for ( unsigned int i = 0; i < S; ++i )
                data[ i ] *= d;

            return *this;
        }

        MultiVector operator - ( const MultiVector& d ) const
        {
            MultiVector tmp;

            for ( unsigned int i = 0; i < S; ++i )
                tmp.data[ i ] = data[ i ] - d.data[ i ];

            return tmp;
        }

        MultiVector operator - () const
        {
            MultiVector tmp;

            for ( unsigned int i = 0; i < S; ++i )
                tmp.data[ i ] = - data[ i ];

            return tmp;
        }

        MultiVector operator + ( const MultiVector& d ) const
        {
            MultiVector tmp;

            for ( unsigned int i = 0; i < S; ++i )
                tmp[ i ] = data[ i ] + d.data[ i ];

            return tmp;
        }

        MultiVector operator * ( const MultiVector& d ) const
        {
            MultiVector tmp;

            for ( unsigned int i = 0; i < S; ++i )
                tmp[ i ] = data[ i ] * d.data[ i ];

            return tmp;
        }

        MultiVector operator / ( const T d ) const
        {
            MultiVector tmp;

            for ( unsigned int i = 0; i < S; ++i )
                tmp[ i ] = data[ i ] / d;

            return tmp;
        }

        MultiVector operator * ( const T d ) const
        {
            MultiVector tmp;

            for ( unsigned int i = 0; i < S; ++i )
                tmp[ i ] = d * data[ i ];

            return tmp;
        }

        MultiVector& operator = ( const MultiVector& d )
        {
            for ( unsigned int i = 0; i < S; ++i )
                data[ i ] = d.data[ i ];

            return *this;
        }

        bool operator == ( const MultiVector& d )
        {
            for ( unsigned int i = 0; i < S; ++i )
                if (data[ i ] != d.data[ i ])
                    return false;
            return true;
        }

        bool operator != ( const MultiVector& d )
        {
            for ( unsigned int i = 0; i < S; ++i )
                if (data[ i ] != d.data[ i ])
                    return true;
            return false;
        }

        template<class Alloc>
        void getVector( std::vector<T, Alloc>& vect ) const
        {
            vect.resize(S);
            for ( unsigned int i = 0; i < S; ++i )
                vect[i] = data[i];
        }

        template<class Alloc>
        bool setVector(const std::vector<T, Alloc>& vect )
        {
            if ( vect.size() != S )
                return false;
            for ( unsigned int i = 0; i < S; ++i )
                data[i] = vect[i];
            return true;
        }

        /*
         * This is just plain wrong since assignment with integer
         * would call this method.
         * Actually, it's the standard that's wrong for doing
         * int == pointer
         *
            MultiVector& operator = (const DataType d)
            {
        //        if (d != 0)
                    for (int i=0; i<S; ++i)
                        data[i] = d[i];
                return *this;
            }
         *
         */

        MultiVector& operator = ( const T d )
        {
            for ( unsigned int i = 0; i < S; ++i )
                data[ i ] = d;

            return *this;
        }

        //replaced int parameter with unsigned int to unconfuse MSVC
        T& operator[] ( unsigned int i )
        {
            return data[ i ];
        }
    };

    template<unsigned S, typename D>
    MultiVector<S,D> operator * ( const D d, const MultiVector<S,D>& v )
    {
        return v*d;
    }

    template<unsigned S, typename D>
    MultiVector<S,D> operator + ( const D d, const MultiVector<S,D>& v )
    {
        return v+d;
    }

    template<unsigned S, typename D>
    MultiVector<S,D> operator - ( const D d, const MultiVector<S,D>& v )
    {
        return v-d;
    }


    typedef MultiVector<6, double> Double6D;

    typedef MultiVector<6, int> Int6D;

    typedef MultiVector<6, bool> Bool6D;

    typedef MultiVector<6, long> Long6D;

    template <unsigned int S, class T>
    std::ostream &operator<<(std::ostream &os, MultiVector<S,T> &q)
    {
#ifdef OS_HAVE_STREAMS
        std::stringstream ss;
        ss << "(";
        for (unsigned int i = 0; i < (S - 1) ; i++) {
            ss << q[i] << ", ";
        }
        ss << q[ S - 1 ] << ")";
        os << ss.str();
#endif
        return os;
    }

    template <unsigned int S, class T>
    std::istream &operator>>(std::istream &os, MultiVector<S,T> &q)
    {
#ifdef OS_HAVE_STREAMS
        MultiVector<S, T> p;
        char c;
        os >> c; // '('
        for (unsigned int i = 0; i < (S - 1) ; i++) {
            os >> p[i];
            os >> c; // ','
            os >> c; // ' '
        }
        os >> p[ S - 1 ];
        os >> c; // ')';
        if ( os )
            q = p;
#endif
        return os;
    }

}}

#endif