Main MRPT website > C++ reference for MRPT 1.3.2
Vector3.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 // $Id: Vector3.h 431 2012-10-04 13:42:51Z ahornung $
10 
11 /**
12  * OctoMap:
13  * A probabilistic, flexible, and compact 3D mapping library for robotic systems.
14  * @author K. M. Wurm, A. Hornung, University of Freiburg, Copyright (C) 2009.
15  * @see http://octomap.sourceforge.net/
16  * License: New BSD License
17  */
18 
19 /*
20  * Copyright (c) 2009-2011, K. M. Wurm, A. Hornung, University of Freiburg
21  * All rights reserved.
22  *
23  * Redistribution and use in source and binary forms, with or without
24  * modification, are permitted provided that the following conditions are met:
25  *
26  * * Redistributions of source code must retain the above copyright
27  * notice, this list of conditions and the following disclaimer.
28  * * Redistributions in binary form must reproduce the above copyright
29  * notice, this list of conditions and the following disclaimer in the
30  * documentation and/or other materials provided with the distribution.
31  * * Neither the name of the University of Freiburg nor the names of its
32  * contributors may be used to endorse or promote products derived from
33  * this software without specific prior written permission.
34  *
35  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
36  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
37  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
38  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
39  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
40  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
41  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
42  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
43  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
44  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
45  * POSSIBILITY OF SUCH DAMAGE.
46  */
47 
48 #ifndef OCTOMATH_VECTOR3_H
49 #define OCTOMATH_VECTOR3_H
50 
51 #include <iostream>
52 #include <math.h>
53 #include <mrpt/maps/link_pragmas.h> // For DLL export within mrpt-maps via the MAPS_IMPEXP macro
54 
55 
56 namespace octomath {
57 
58  /*!
59  * \brief This class represents a three-dimensional vector
60  *
61  * The three-dimensional vector can be used to represent a
62  * translation in three-dimensional space or to represent the
63  * attitude of an object using Euler angle.
64  */
66  public:
67 
68  /*!
69  * \brief Default constructor
70  */
71  Vector3 () { data[0] = data[1] = data[2] = 0; }
72 
73  /*!
74  * \brief Copy constructor
75  *
76  * @param other a vector of dimension 3
77  */
78  Vector3 (const Vector3& other) {
79  data[0] = other(0);
80  data[1] = other(1);
81  data[2] = other(2);
82  }
83 
84  /*!
85  * \brief Constructor
86  *
87  * Constructs a three-dimensional vector from
88  * three single values x, y, z or roll, pitch, yaw
89  */
90  Vector3 (float x, float y, float z) {
91  data[0] = x;
92  data[1] = y;
93  data[2] = z;
94  }
95 
96 
97  /* inline Eigen3::Vector3f getVector3f() const { return Eigen3::Vector3f(data[0], data[1], data[2]) ; } */
98  /* inline Eigen3::Vector4f& getVector4f() { return data; } */
99  /* inline Eigen3::Vector4f getVector4f() const { return data; } */
100 
101  /*!
102  * \brief Assignment operator
103  *
104  * @param other a vector of dimension 3
105  */
106  inline Vector3& operator= (const Vector3& other) {
107  data[0] = other(0);
108  data[1] = other(1);
109  data[2] = other(2);
110  return *this;
111  }
112 
113 
114  /*!
115  * \brief Three-dimensional vector (cross) product
116  *
117  * Calculates the tree-dimensional cross product, which
118  * represents the vector orthogonal to the plane defined
119  * by this and other.
120  * @return this x other
121  */
122  inline Vector3 cross (const Vector3& other) const
123  {
124  //return (data.start<3> ().cross (other.data.start<3> ()));
125  // \note should this be renamed?
126  return Vector3(y()*other.z() - z()*other.y(),
127  z()*other.x() - x()*other.z(),
128  x()*other.y() - y()*other.x());
129  }
130 
131  inline double dot (const Vector3& other) const
132  {
133  return x()*other.x() + y()*other.y() + z()*other.z();
134  }
135 
136  inline const float& operator() (unsigned int i) const
137  {
138  return data[i];
139  }
140  inline float& operator() (unsigned int i)
141  {
142  return data[i];
143  }
144 
145  inline float& x()
146  {
147  return operator()(0);
148  }
149 
150  inline float& y()
151  {
152  return operator()(1);
153  }
154 
155  inline float& z()
156  {
157  return operator()(2);
158  }
159 
160  inline const float& x() const
161  {
162  return operator()(0);
163  }
164 
165  inline const float& y() const
166  {
167  return operator()(1);
168  }
169 
170  inline const float& z() const
171  {
172  return operator()(2);
173  }
174 
175  inline float& roll()
176  {
177  return operator()(0);
178  }
179 
180  inline float& pitch()
181  {
182  return operator()(1);
183  }
184 
185  inline float& yaw()
186  {
187  return operator()(2);
188  }
189 
190  inline const float& roll() const
191  {
192  return operator()(0);
193  }
194 
195  inline const float& pitch() const
196  {
197  return operator()(1);
198  }
199 
200  inline const float& yaw() const
201  {
202  return operator()(2);
203  }
204 
205  inline Vector3 operator- () const
206  {
207  Vector3 result;
208  result(0) = -data[0];
209  result(1) = -data[1];
210  result(2) = -data[2];
211  return result;
212  }
213 
214  inline Vector3 operator+ (const Vector3 &other) const
215  {
216  Vector3 result(*this);
217  result(0) += other(0);
218  result(1) += other(1);
219  result(2) += other(2);
220  return result;
221  }
222 
223  inline Vector3 operator* (float x) const {
224  Vector3 result(*this);
225  result(0) *= x;
226  result(1) *= x;
227  result(2) *= x;
228  return result;
229  }
230 
231  inline Vector3 operator- (const Vector3 &other) const
232  {
233  Vector3 result(*this);
234  result(0) -= other(0);
235  result(1) -= other(1);
236  result(2) -= other(2);
237  return result;
238  }
239 
240  inline void operator+= (const Vector3 &other)
241  {
242  data[0] += other(0);
243  data[1] += other(1);
244  data[2] += other(2);
245  }
246 
247  inline void operator-= (const Vector3& other) {
248  data[0] -= other(0);
249  data[1] -= other(1);
250  data[2] -= other(2);
251  }
252 
253  inline void operator/= (float x) {
254  data[0] /= x;
255  data[1] /= x;
256  data[2] /= x;
257  }
258 
259  inline void operator*= (float x) {
260  data[0] *= x;
261  data[1] *= x;
262  data[2] *= x;
263  }
264 
265  inline bool operator== (const Vector3 &other) const {
266  for (unsigned int i=0; i<3; i++) {
267  if (operator()(i) != other(i))
268  return false;
269  }
270  return true;
271  }
272 
273  inline double norm () const {
274  double n = 0;
275  for (unsigned int i=0; i<3; i++) {
276  n += operator()(i) * operator()(i);
277  }
278  return sqrt(n);
279  }
280 
281  inline Vector3& normalize () {
282  double len = norm ();
283  if (len > 0)
284  *this /= (float) len;
285  return *this;
286  }
287 
288  inline Vector3 normalized () const {
289  Vector3 result(*this);
290  result.normalize ();
291  return result;
292  }
293 
294  inline double angleTo(const Vector3& other) const {
295  double dot_prod = this->dot(other);
296  double len1 = this->norm();
297  double len2 = other.norm();
298  return acos(dot_prod / (len1*len2));
299  }
300 
301 
302  inline double distance (const Vector3& other) const {
303  double dist_x = x() - other.x();
304  double dist_y = y() - other.y();
305  double dist_z = z() - other.z();
306  return sqrt(dist_x*dist_x + dist_y*dist_y + dist_z*dist_z);
307  }
308 
309  inline double distanceXY (const Vector3& other) const {
310  double dist_x = x() - other.x();
311  double dist_y = y() - other.y();
312  return sqrt(dist_x*dist_x + dist_y*dist_y);
313  }
314 
315  Vector3& rotate_IP (double roll, double pitch, double yaw);
316 
317  // void read (unsigned char * src, unsigned int size);
318  std::istream& read(std::istream &s);
319  std::ostream& write(std::ostream &s) const;
320  std::istream& readBinary(std::istream &s);
321  std::ostream& writeBinary(std::ostream &s) const;
322 
323 
324  protected:
325  float data[3];
326 
327  };
328 
329 
330  //! user friendly output in format (x y z)
331  std::ostream MAPS_IMPEXP & operator<<(std::ostream& out, octomath::Vector3 const& v);
332 
333 }
334 
335 
336 #endif
const float & y() const
Definition: Vector3.h:165
OctoMap: A probabilistic, flexible, and compact 3D mapping library for robotic systems.
Definition: Pose6D.h:55
std::ostream MAPS_IMPEXP & operator<<(std::ostream &s, const Pose6D &p)
user friendly output in format (x y z, u x y z) which is (translation, rotation)
double norm() const
Definition: Vector3.h:273
double dot(const Vector3 &other) const
Definition: Vector3.h:131
float & roll()
Definition: Vector3.h:175
float & z()
Definition: Vector3.h:155
double distanceXY(const Vector3 &other) const
Definition: Vector3.h:309
Vector3(const Vector3 &other)
Copy constructor.
Definition: Vector3.h:78
Vector3(float x, float y, float z)
Constructor.
Definition: Vector3.h:90
std::vector< T1 > operator+(const std::vector< T1 > &a, const std::vector< T2 > &b)
a+b (element-wise sum)
Definition: ops_vectors.h:89
double distance(const Vector3 &other) const
Definition: Vector3.h:302
double angleTo(const Vector3 &other) const
Definition: Vector3.h:294
const float & z() const
Definition: Vector3.h:170
const float & roll() const
Definition: Vector3.h:190
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
Definition: ops_vectors.h:70
bool operator==(const CArray< T, N > &x, const CArray< T, N > &y)
Definition: CArray.h:279
Vector3()
Default constructor.
Definition: Vector3.h:71
float & yaw()
Definition: Vector3.h:185
const float & yaw() const
Definition: Vector3.h:200
float & y()
Definition: Vector3.h:150
Vector3 normalized() const
Definition: Vector3.h:288
Vector3 cross(const Vector3 &other) const
Three-dimensional vector (cross) product.
Definition: Vector3.h:122
const float & x() const
Definition: Vector3.h:160
const float & pitch() const
Definition: Vector3.h:195
TPoint3D operator-(const TPoint3D &p1)
Unary minus operator for 3D points.
float & pitch()
Definition: Vector3.h:180
Vector3 & normalize()
Definition: Vector3.h:281
std::vector< T1 > & operator*=(std::vector< T1 > &a, const std::vector< T2 > &b)
a*=b (element-wise multiplication)
Definition: ops_vectors.h:40
std::vector< T1 > operator*(const std::vector< T1 > &a, const std::vector< T2 > &b)
a*b (element-wise multiplication)
Definition: ops_vectors.h:59
CONTAINER::Scalar norm(const CONTAINER &v)
float & x()
Definition: Vector3.h:145
This class represents a three-dimensional vector.
Definition: Vector3.h:65



Page generated by Doxygen 1.8.11 for MRPT 1.3.2 SVN:Unversioned directory at Sun May 1 08:45:24 UTC 2016