Vector3.h
Go to the documentation of this file.
00001 /*
00002  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
00003  * http://octomap.github.com/
00004  *
00005  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
00006  * All rights reserved.
00007  * License: New BSD
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions are met:
00011  *
00012  *     * Redistributions of source code must retain the above copyright
00013  *       notice, this list of conditions and the following disclaimer.
00014  *     * Redistributions in binary form must reproduce the above copyright
00015  *       notice, this list of conditions and the following disclaimer in the
00016  *       documentation and/or other materials provided with the distribution.
00017  *     * Neither the name of the University of Freiburg nor the names of its
00018  *       contributors may be used to endorse or promote products derived from
00019  *       this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00023  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00024  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00025  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00026  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00027  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00028  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00029  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00030  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  * POSSIBILITY OF SUCH DAMAGE.
00032  */
00033 
00034 #ifndef OCTOMATH_VECTOR3_H
00035 #define OCTOMATH_VECTOR3_H
00036 
00037 #include <iostream>
00038 #include <math.h>
00039 
00040 
00041 namespace octomath {
00042 
00050   class Vector3 {
00051   public:
00052 
00056     Vector3 () { data[0] = data[1] = data[2] = 0.0; }
00057 
00063     Vector3 (const Vector3& other) {
00064       data[0] = other(0);
00065       data[1] = other(1);
00066       data[2] = other(2);
00067     }
00068 
00075     Vector3 (float x, float y, float z) {
00076       data[0] = x;
00077       data[1] = y;
00078       data[2] = z;
00079     }
00080 
00081 
00082     /* inline Eigen3::Vector3f getVector3f() const { return Eigen3::Vector3f(data[0], data[1], data[2]) ; } */
00083     /* inline Eigen3::Vector4f& getVector4f() { return data; } */
00084     /* inline Eigen3::Vector4f getVector4f() const { return data; } */
00085 
00091     inline Vector3& operator= (const Vector3& other)  {
00092       data[0] = other(0);
00093       data[1] = other(1);
00094       data[2] = other(2);      
00095       return *this;
00096     }
00097 
00098 
00107     inline Vector3 cross (const Vector3& other) const 
00108     {
00109       //return (data.start<3> ().cross (other.data.start<3> ()));
00110       // \note should this be renamed?
00111       return Vector3(y()*other.z() - z()*other.y(),
00112                      z()*other.x() - x()*other.z(),
00113                      x()*other.y() - y()*other.x());
00114     }
00115 
00117     inline double dot (const Vector3& other) const 
00118     {
00119       return x()*other.x() + y()*other.y() + z()*other.z();
00120     }
00121 
00122     inline const float& operator() (unsigned int i) const
00123     {
00124       return data[i];
00125     }
00126     inline float& operator() (unsigned int i)
00127     {
00128       return data[i];
00129     }
00130 
00131     inline float& x() 
00132     {
00133       return operator()(0);
00134     }
00135 
00136     inline float& y() 
00137     {
00138       return operator()(1);
00139     }
00140 
00141     inline float& z() 
00142     {
00143       return operator()(2);
00144     }
00145 
00146     inline const float& x() const 
00147     {
00148       return operator()(0);
00149     }
00150 
00151     inline const float& y() const 
00152     {
00153       return operator()(1);
00154     }
00155 
00156     inline const float& z() const 
00157     {
00158       return operator()(2);
00159     }
00160 
00161     inline float& roll() 
00162     {
00163       return operator()(0);
00164     }
00165 
00166     inline float& pitch() 
00167     {
00168       return operator()(1);
00169     }
00170 
00171     inline float& yaw() 
00172     {
00173       return operator()(2);
00174     }
00175 
00176     inline const float& roll() const 
00177     {
00178       return operator()(0);
00179     }
00180 
00181     inline const float& pitch() const 
00182     {
00183       return operator()(1);
00184     }
00185 
00186     inline const float& yaw() const 
00187     {
00188       return operator()(2);
00189     }
00190 
00191     inline Vector3 operator- () const 
00192     {
00193       Vector3 result;
00194       result(0) = -data[0];
00195       result(1) = -data[1];
00196       result(2) = -data[2];
00197       return result;
00198     }
00199 
00200     inline Vector3 operator+ (const Vector3 &other) const 
00201     {
00202       Vector3 result(*this);
00203       result(0) += other(0);
00204       result(1) += other(1);
00205       result(2) += other(2);
00206       return result;
00207     }
00208 
00209     inline Vector3 operator*  (float x) const {
00210       Vector3 result(*this);
00211       result(0) *= x;
00212       result(1) *= x;
00213       result(2) *= x;
00214       return result;
00215     }
00216 
00217     inline Vector3 operator- (const Vector3 &other) const 
00218     {
00219       Vector3 result(*this);
00220       result(0) -= other(0);
00221       result(1) -= other(1);
00222       result(2) -= other(2);
00223       return result;
00224     }
00225 
00226     inline void operator+= (const Vector3 &other)
00227     {
00228       data[0] += other(0);
00229       data[1] += other(1);
00230       data[2] += other(2);      
00231     }
00232 
00233     inline void operator-= (const Vector3& other) {
00234       data[0] -= other(0);
00235       data[1] -= other(1);
00236       data[2] -= other(2);      
00237     }
00238 
00239     inline void operator/= (float x) {
00240       data[0] /= x;
00241       data[1] /= x;
00242       data[2] /= x;      
00243     }
00244 
00245     inline void operator*= (float x) {    
00246       data[0] *= x;
00247       data[1] *= x;
00248       data[2] *= x;      
00249     }
00250 
00251     inline bool operator== (const Vector3 &other) const {
00252       for (unsigned int i=0; i<3; i++) {
00253         if (operator()(i) != other(i)) 
00254           return false;
00255       }
00256       return true;
00257     }
00258         
00260     inline double norm () const {
00261       return sqrt(norm_sq());
00262     }
00263 
00265     inline double norm_sq() const {
00266       return (x()*x() + y()*y() + z()*z());
00267     }
00268 
00270     inline Vector3& normalize () {
00271       double len = norm();
00272       if (len > 0)
00273         *this /= (float) len;
00274       return *this;
00275     }
00276 
00278     inline Vector3 normalized () const {
00279       Vector3 result(*this);
00280       result.normalize ();
00281       return result;
00282     }
00283 
00284     inline double angleTo(const Vector3& other) const { 
00285       double dot_prod = this->dot(other);
00286       double len1 = this->norm();
00287       double len2 = other.norm();
00288       return acos(dot_prod / (len1*len2));
00289     }
00290 
00291 
00292     inline double distance (const Vector3& other) const {
00293       double dist_x = x() - other.x();
00294       double dist_y = y() - other.y();
00295       double dist_z = z() - other.z();
00296       return sqrt(dist_x*dist_x + dist_y*dist_y + dist_z*dist_z);
00297     }
00298 
00299     inline double distanceXY (const Vector3& other) const {
00300       double dist_x = x() - other.x();
00301       double dist_y = y() - other.y();
00302       return sqrt(dist_x*dist_x + dist_y*dist_y);
00303     }
00304 
00305     Vector3& rotate_IP (double roll, double pitch, double yaw);
00306 
00307     //    void read (unsigned char * src, unsigned int size);
00308     std::istream& read(std::istream &s);
00309     std::ostream& write(std::ostream &s) const;
00310     std::istream& readBinary(std::istream &s);
00311     std::ostream& writeBinary(std::ostream &s) const;
00312 
00313 
00314   protected:
00315     float data[3];
00316 
00317   };
00318 
00319 
00321   std::ostream& operator<<(std::ostream& out, octomath::Vector3 const& v);
00322 
00323 }
00324 
00325 
00326 #endif


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Feb 11 2016 23:50:59