Flock of Birds Library: vector.h Source File

00001 /*
00002 -------------------------------------------------------------------------------
00003 libfob - C++ interface to Ascension Technology Corporation's
00004          Flock of Birds position and orientation measurement system.
00005 Copyright (C) 2002 Nathan Cournia
00006 
00007 This library is free software; you can redistribute it and/or
00008 modify it under the terms of the GNU Lesser General Public
00009 License as published by the Free Software Foundation; either
00010 version 2.1 of the License, or (at your option) any later version.
00011 
00012 This library is distributed in the hope that it will be useful,
00013 but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00015 Lesser General Public License for more details.
00016 
00017 You should have received a copy of the GNU Lesser General Public
00018 License along with this library; if not, write to the Free Software
00019 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00020 -------------------------------------------------------------------------------
00021 */
00022 
00023 //FILE:         vector.h
00024 //AUTHOR:       Nathan Cournia <nathan@cournia.com>
00025 
00026 #ifndef COURNIA_VECTOR_H
00027 #define COURNIA_VECTOR_H 1
00028 
00029 #include <iostream>
00030 #include <cassert>
00031 #include "fob/mathematics.h"
00032 
00034 namespace math {
00035     class vector3;
00036 }
00037 
00038 namespace math {
00039     math::vector3 cross( const math::vector3& lhs, const math::vector3& rhs );
00040     void add( math::vector3& dest, const math::vector3& lhs, const math::vector3& rhs );
00041     void subtract( math::vector3& dest, const math::vector3& lhs, const math::vector3& rhs );
00042     void multiply( math::vector3& dest, const math::vector3& lhs, const math::vector3& rhs );
00043     void divide( math::vector3& dest, const math::vector3& lhs, const math::vector3& rhs );
00044     void multiply( math::vector3& dest, const math::vector3& lhs, real_t rhs );
00045     void multiply( math::vector3& dest, real_t rhs, const math::vector3& lhs );
00046     real_t dot( const math::vector3& lhs, const math::vector3& rhs ); 
00047     math::vector3 operator* ( real_t lhs, const math::vector3& rhs );
00048     std::ostream& operator<< ( std::ostream& o, const math::vector3& vec );
00049 }
00050 
00052 class math::vector3
00053 {
00054 private:
00055     real_t m_data[ 3 ]; 
00056 
00058 
00065     friend math::vector3 math::operator* ( real_t lhs, const math::vector3& rhs );
00066 
00068 
00073     friend std::ostream& math::operator<< ( std::ostream& o, const math::vector3& vec );
00074 
00076 
00081     friend math::vector3 math::cross( const math::vector3& lhs, const math::vector3& rhs );
00082 
00084 
00093     friend void math::add( math::vector3& dest, const math::vector3& lhs, const math::vector3& rhs );
00094 
00096 
00105     friend void math::subtract( math::vector3& dest, const math::vector3& lhs, const math::vector3& rhs );
00106 
00108 
00116     friend void math::multiply( math::vector3& dest, const math::vector3& lhs, const math::vector3& rhs );
00117 
00119 
00128     friend void math::divide( math::vector3& dest, const math::vector3& lhs, const math::vector3& rhs );
00129 
00131 
00141     friend void math::multiply( math::vector3& dest, const math::vector3& lhs, real_t rhs );
00142 
00144 
00154     friend void math::multiply( math::vector3& dest, real_t rhs, const math::vector3& lhs );
00155 
00156 
00158 
00163     friend real_t math::dot( const math::vector3& lhs, const math::vector3& rhs );
00164 
00165 public:
00167 
00170     vector3( void ) {
00171         m_data[ 0 ] = 0.0;
00172         m_data[ 1 ] = 0.0;
00173         m_data[ 2 ] = 0.0;
00174     }
00175     
00177 
00180     vector3( real_t x_, real_t y_, real_t z_ ) {
00181         m_data[ 0 ] = x_;
00182         m_data[ 1 ] = y_;
00183         m_data[ 2 ] = z_;
00184     }
00185 
00187 
00191     vector3( real_t *r3 ) {
00192         m_data[ 0 ] = r3[ 0 ];
00193         m_data[ 1 ] = r3[ 1 ];
00194         m_data[ 2 ] = r3[ 2 ];
00195     }
00196 
00198     vector3( const math::vector3& p ) {
00199         m_data[ 0 ] = p.m_data[ 0 ];
00200         m_data[ 1 ] = p.m_data[ 1 ];
00201         m_data[ 2 ] = p.m_data[ 2 ];
00202     }
00203     
00205     inline math::vector3& operator= ( const math::vector3& p ) {
00206         m_data[ 0 ] = p.m_data[ 0 ];
00207         m_data[ 1 ] = p.m_data[ 1 ];
00208         m_data[ 2 ] = p.m_data[ 2 ];
00209         return *this;
00210     }
00211 
00213     inline void set( real_t x_, real_t y_, real_t z_ ) {
00214         m_data[ 0 ] = x_;
00215         m_data[ 1 ] = y_;
00216         m_data[ 2 ] = z_;
00217     }
00218 
00220 
00224     inline void set( real_t *r3 ) {
00225         m_data[ 0 ] = r3[ 0 ];
00226         m_data[ 1 ] = r3[ 1 ];
00227         m_data[ 2 ] = r3[ 2 ];
00228     }
00229 
00231 
00237     inline real_t operator( ) ( unsigned int index ) const {
00238         assert( index < 3 );
00239         return m_data[ index ];
00240     }
00241 
00243 
00248     inline real_t& operator( ) ( unsigned int index ) {
00249         assert( index < 3 );
00250         return m_data[ index ];
00251     }
00252 
00254 
00260     inline math::vector3 operator+ ( const math::vector3& rhs ) const {
00261         return math::vector3( 
00262             (m_data[ 0 ] + rhs.m_data[ 0 ]), 
00263             (m_data[ 1 ] + rhs.m_data[ 1 ]),
00264             (m_data[ 2 ] + rhs.m_data[ 2 ])
00265         );
00266     }
00267 
00269 
00275     inline math::vector3 operator- ( const math::vector3& rhs ) const {
00276         return math::vector3( 
00277             (m_data[ 0 ] - rhs.m_data[ 0 ]), 
00278             (m_data[ 1 ] - rhs.m_data[ 1 ]),
00279             (m_data[ 2 ] - rhs.m_data[ 2 ])
00280         );
00281     }
00282 
00284 
00289     inline math::vector3 operator* ( const math::vector3& rhs ) const {
00290         return math::vector3( 
00291             (m_data[ 0 ] * rhs.m_data[ 0 ]), 
00292             (m_data[ 1 ] * rhs.m_data[ 1 ]),
00293             (m_data[ 2 ] * rhs.m_data[ 2 ])
00294         );
00295     }
00296 
00298 
00304     inline math::vector3 operator/ ( const math::vector3& rhs ) const {
00305         return math::vector3( 
00306             (m_data[ 0 ] / rhs.m_data[ 0 ]), 
00307             (m_data[ 1 ] / rhs.m_data[ 1 ]),
00308             (m_data[ 2 ] / rhs.m_data[ 2 ])
00309         );
00310     }
00311 
00313 
00316     inline math::vector3 operator- ( void ) const {
00317         return math::vector3( 
00318             (m_data[ 0 ] * -1.0), 
00319             (m_data[ 1 ] * -1.0),
00320             (m_data[ 2 ] * -1.0)
00321         );
00322     }
00323 
00325 
00332     inline math::vector3 operator* ( real_t rhs ) const {
00333         return math::vector3( 
00334             (m_data[ 0 ] * rhs), 
00335             (m_data[ 1 ] * rhs),
00336             (m_data[ 2 ] * rhs)
00337         );
00338     }
00339 
00341 
00347     inline math::vector3 operator/ ( real_t rhs ) const {
00348         return math::vector3( 
00349             (m_data[ 0 ] / rhs), 
00350             (m_data[ 1 ] / rhs),
00351             (m_data[ 2 ] / rhs)
00352         );
00353     }
00354 
00356 
00359     inline void operator-= ( const math::vector3& v ) {
00360         m_data[ 0 ] -= v.m_data[ 0 ];
00361         m_data[ 1 ] -= v.m_data[ 1 ];
00362         m_data[ 2 ] -= v.m_data[ 2 ];
00363     }
00364 
00366 
00369     inline void operator+= ( const math::vector3& v ) {
00370         m_data[ 0 ] += v.m_data[ 0 ];
00371         m_data[ 1 ] += v.m_data[ 1 ];
00372         m_data[ 2 ] += v.m_data[ 2 ];
00373     }
00374 
00376 
00380     inline void operator*= ( const math::vector3 &rhs ) {
00381         m_data[ 0 ] *= rhs.m_data[ 0 ];
00382         m_data[ 1 ] *= rhs.m_data[ 1 ];
00383         m_data[ 2 ] *= rhs.m_data[ 2 ];
00384     }
00385 
00387 
00392     inline void operator/= ( const math::vector3 &rhs ) {
00393         m_data[ 0 ] /= rhs.m_data[ 0 ];
00394         m_data[ 1 ] /= rhs.m_data[ 1 ];
00395         m_data[ 2 ] /= rhs.m_data[ 2 ];
00396     }
00397 
00399 
00403     inline void operator-= ( real_t rhs ) {
00404         m_data[ 0 ] -= rhs;
00405         m_data[ 1 ] -= rhs;
00406         m_data[ 2 ] -= rhs;
00407     }
00408 
00410 
00414     inline void operator+= ( real_t rhs ) {
00415         m_data[ 0 ] += rhs;
00416         m_data[ 1 ] += rhs;
00417         m_data[ 2 ] += rhs;
00418     }
00419 
00421 
00425     inline void operator*= ( real_t rhs ) {
00426         m_data[ 0 ] *= rhs;
00427         m_data[ 1 ] *= rhs;
00428         m_data[ 2 ] *= rhs;
00429     }
00430 
00432 
00437     inline void operator/= ( real_t rhs ) {
00438         m_data[ 0 ] /= rhs;
00439         m_data[ 1 ] /= rhs;
00440         m_data[ 2 ] /= rhs;
00441     }
00442 
00444     inline real_t x( void ) const { 
00445         return m_data[ 0 ];
00446     }
00447     
00449     inline real_t y( void ) const { 
00450         return m_data[ 1 ];
00451     }
00452 
00454     inline real_t z( void ) const { 
00455         return m_data[ 2 ];
00456     }
00457     
00459     inline void x( real_t x_ ) {
00460         m_data[ 0 ] = x_;
00461     }
00462     
00464     inline void y( real_t y_ ) {
00465         m_data[ 1 ] = y_;
00466     }
00467     
00469     inline void z( real_t z_ ) {
00470         m_data[ 2 ] = z_;
00471     }
00472 
00474 
00477     inline double length( void ) const {
00478         return std::sqrt( (m_data[ 0 ] * m_data[ 0 ]) +
00479             (m_data[ 1 ] * m_data[ 1 ]) +
00480             (m_data[ 2 ] * m_data[ 2 ])
00481         );
00482     }
00483 
00485 
00488     inline void normalize( void ) {
00489         double len = length( );
00490         if( !math::equals( len, 0.0 ) ) {
00491             m_data[ 0 ] /= len;
00492             m_data[ 1 ] /= len;
00493             m_data[ 2 ] /= len;
00494         }
00495     }
00496 
00498 
00502     inline void negate( void )
00503     {
00504         m_data[ 0 ] *= -1.0;
00505         m_data[ 1 ] *= -1.0;
00506         m_data[ 2 ] *= -1.0;
00507     }
00508 
00510 
00515     inline bool operator== ( const math::vector3& rhs ) const {
00516         return (math::equals( m_data[ 0 ], rhs.m_data[ 0 ] ) &&
00517             math::equals( m_data[ 1 ], rhs.m_data[ 1 ] ) &&
00518             math::equals( m_data[ 2 ], rhs.m_data[ 2 ] )
00519         );
00520     }
00521 
00523 
00528     inline bool operator!= ( const math::vector3& rhs ) const {
00529         return !(math::equals( m_data[ 0 ], rhs.m_data[ 0 ] ) &&
00530             math::equals( m_data[ 1 ], rhs.m_data[ 1 ] ) &&
00531             math::equals( m_data[ 2 ], rhs.m_data[ 2 ] )
00532         );
00533     }
00534 
00536 
00539     inline operator const real_t*( void ) const {
00540         return m_data;
00541     }
00542 
00544 
00550     static math::vector3 lerp( real_t percent, const math::vector3& a, 
00551         const math::vector3& b );
00552     
00554     static const vector3 ZERO; 
00555     static const vector3 X_AXIS; 
00556     static const vector3 Y_AXIS; 
00557     static const vector3 Z_AXIS; 
00558 };
00559     
00560 #endif