octomap  1.9.7
Vector3.h
Go to the documentation of this file.
1 /*
2  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3  * https://octomap.github.io/
4  *
5  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
6  * All rights reserved.
7  * License: New BSD
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above copyright
15  * notice, this list of conditions and the following disclaimer in the
16  * documentation and/or other materials provided with the distribution.
17  * * Neither the name of the University of Freiburg nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
25  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  */
33 
34 #ifndef OCTOMATH_VECTOR3_H
35 #define OCTOMATH_VECTOR3_H
36 
37 #include <iostream>
38 #include <math.h>
39 
40 
41 namespace octomath {
42 
50  class Vector3 {
51  public:
52 
56  Vector3 () { data[0] = data[1] = data[2] = 0.0; }
57 
63  Vector3 (const Vector3& other) {
64  data[0] = other(0);
65  data[1] = other(1);
66  data[2] = other(2);
67  }
68 
75  Vector3 (float x, float y, float z) {
76  data[0] = x;
77  data[1] = y;
78  data[2] = z;
79  }
80 
81 
82  /* inline Eigen3::Vector3f getVector3f() const { return Eigen3::Vector3f(data[0], data[1], data[2]) ; } */
83  /* inline Eigen3::Vector4f& getVector4f() { return data; } */
84  /* inline Eigen3::Vector4f getVector4f() const { return data; } */
85 
91  inline Vector3& operator= (const Vector3& other) {
92  data[0] = other(0);
93  data[1] = other(1);
94  data[2] = other(2);
95  return *this;
96  }
97 
98 
107  inline Vector3 cross (const Vector3& other) const
108  {
109  //return (data.start<3> ().cross (other.data.start<3> ()));
110  // \note should this be renamed?
111  return Vector3(y()*other.z() - z()*other.y(),
112  z()*other.x() - x()*other.z(),
113  x()*other.y() - y()*other.x());
114  }
115 
117  inline double dot (const Vector3& other) const
118  {
119  return x()*other.x() + y()*other.y() + z()*other.z();
120  }
121 
122  inline const float& operator() (unsigned int i) const
123  {
124  return data[i];
125  }
126  inline float& operator() (unsigned int i)
127  {
128  return data[i];
129  }
130 
131  inline float& x()
132  {
133  return operator()(0);
134  }
135 
136  inline float& y()
137  {
138  return operator()(1);
139  }
140 
141  inline float& z()
142  {
143  return operator()(2);
144  }
145 
146  inline const float& x() const
147  {
148  return operator()(0);
149  }
150 
151  inline const float& y() const
152  {
153  return operator()(1);
154  }
155 
156  inline const float& z() const
157  {
158  return operator()(2);
159  }
160 
161  inline float& roll()
162  {
163  return operator()(0);
164  }
165 
166  inline float& pitch()
167  {
168  return operator()(1);
169  }
170 
171  inline float& yaw()
172  {
173  return operator()(2);
174  }
175 
176  inline const float& roll() const
177  {
178  return operator()(0);
179  }
180 
181  inline const float& pitch() const
182  {
183  return operator()(1);
184  }
185 
186  inline const float& yaw() const
187  {
188  return operator()(2);
189  }
190 
191  inline Vector3 operator- () const
192  {
193  Vector3 result;
194  result(0) = -data[0];
195  result(1) = -data[1];
196  result(2) = -data[2];
197  return result;
198  }
199 
200  inline Vector3 operator+ (const Vector3 &other) const
201  {
202  Vector3 result(*this);
203  result(0) += other(0);
204  result(1) += other(1);
205  result(2) += other(2);
206  return result;
207  }
208 
209  inline Vector3 operator* (float x) const {
210  Vector3 result(*this);
211  result(0) *= x;
212  result(1) *= x;
213  result(2) *= x;
214  return result;
215  }
216 
217  inline Vector3 operator- (const Vector3 &other) const
218  {
219  Vector3 result(*this);
220  result(0) -= other(0);
221  result(1) -= other(1);
222  result(2) -= other(2);
223  return result;
224  }
225 
226  inline void operator+= (const Vector3 &other)
227  {
228  data[0] += other(0);
229  data[1] += other(1);
230  data[2] += other(2);
231  }
232 
233  inline void operator-= (const Vector3& other) {
234  data[0] -= other(0);
235  data[1] -= other(1);
236  data[2] -= other(2);
237  }
238 
239  inline void operator/= (float x) {
240  data[0] /= x;
241  data[1] /= x;
242  data[2] /= x;
243  }
244 
245  inline void operator*= (float x) {
246  data[0] *= x;
247  data[1] *= x;
248  data[2] *= x;
249  }
250 
251  inline bool operator== (const Vector3 &other) const {
252  for (unsigned int i=0; i<3; i++) {
253  if (operator()(i) != other(i))
254  return false;
255  }
256  return true;
257  }
258 
260  inline double norm () const {
261  return sqrt(norm_sq());
262  }
263 
265  inline double norm_sq() const {
266  return (x()*x() + y()*y() + z()*z());
267  }
268 
270  inline Vector3& normalize () {
271  double len = norm();
272  if (len > 0)
273  *this /= (float) len;
274  return *this;
275  }
276 
278  inline Vector3 normalized () const {
279  Vector3 result(*this);
280  result.normalize ();
281  return result;
282  }
283 
284  inline double angleTo(const Vector3& other) const {
285  double dot_prod = this->dot(other);
286  double len1 = this->norm();
287  double len2 = other.norm();
288  return acos(dot_prod / (len1*len2));
289  }
290 
291 
292  inline double distance (const Vector3& other) const {
293  double dist_x = x() - other.x();
294  double dist_y = y() - other.y();
295  double dist_z = z() - other.z();
296  return sqrt(dist_x*dist_x + dist_y*dist_y + dist_z*dist_z);
297  }
298 
299  inline double distanceXY (const Vector3& other) const {
300  double dist_x = x() - other.x();
301  double dist_y = y() - other.y();
302  return sqrt(dist_x*dist_x + dist_y*dist_y);
303  }
304 
305  Vector3& rotate_IP (double roll, double pitch, double yaw);
306 
307  // void read (unsigned char * src, unsigned int size);
308  std::istream& read(std::istream &s);
309  std::ostream& write(std::ostream &s) const;
310  std::istream& readBinary(std::istream &s);
311  std::ostream& writeBinary(std::ostream &s) const;
312 
313 
314  protected:
315  float data[3];
316 
317  };
318 
319 
321  std::ostream& operator<<(std::ostream& out, octomath::Vector3 const& v);
322 
323 }
324 
325 
326 #endif
octomath::Vector3::pitch
float & pitch()
Definition: Vector3.h:166
octomath::Vector3::norm
double norm() const
Definition: Vector3.h:260
octomath::Vector3::Vector3
Vector3()
Default constructor.
Definition: Vector3.h:56
octomath::Vector3::z
const float & z() const
Definition: Vector3.h:156
octomath::Vector3::yaw
float & yaw()
Definition: Vector3.h:171
octomath::Vector3::operator=
Vector3 & operator=(const Vector3 &other)
Assignment operator.
Definition: Vector3.h:91
octomath::Vector3::yaw
const float & yaw() const
Definition: Vector3.h:186
octomath::Vector3::writeBinary
std::ostream & writeBinary(std::ostream &s) const
Definition: Vector3.cpp:99
octomath::Vector3::operator+
Vector3 operator+(const Vector3 &other) const
Definition: Vector3.h:200
octomath::Vector3::Vector3
Vector3(const Vector3 &other)
Copy constructor.
Definition: Vector3.h:63
octomath::Vector3::operator-=
void operator-=(const Vector3 &other)
Definition: Vector3.h:233
octomath::operator<<
std::ostream & operator<<(std::ostream &s, const Pose6D &p)
user friendly output in format (x y z, u x y z) which is (translation, rotation)
Definition: Pose6D.cpp:152
octomath::Vector3::operator==
bool operator==(const Vector3 &other) const
Definition: Vector3.h:251
octomath::Vector3::rotate_IP
Vector3 & rotate_IP(double roll, double pitch, double yaw)
Definition: Vector3.cpp:41
octomath::Vector3::roll
float & roll()
Definition: Vector3.h:161
octomath::Vector3::pitch
const float & pitch() const
Definition: Vector3.h:181
octomath::Vector3::normalize
Vector3 & normalize()
normalizes this vector, so that it has norm=1.0
Definition: Vector3.h:270
octomath::Vector3::distanceXY
double distanceXY(const Vector3 &other) const
Definition: Vector3.h:299
octomath::Vector3::operator/=
void operator/=(float x)
Definition: Vector3.h:239
octomath::Vector3
This class represents a three-dimensional vector.
Definition: Vector3.h:50
octomath::Vector3::dot
double dot(const Vector3 &other) const
dot product
Definition: Vector3.h:117
octomath::Vector3::read
std::istream & read(std::istream &s)
Definition: Vector3.cpp:69
octomath
octomath::Vector3::operator*=
void operator*=(float x)
Definition: Vector3.h:245
octomath::Vector3::norm_sq
double norm_sq() const
Definition: Vector3.h:265
octomath::Vector3::Vector3
Vector3(float x, float y, float z)
Constructor.
Definition: Vector3.h:75
octomath::Vector3::operator*
Vector3 operator*(float x) const
Definition: Vector3.h:209
octomath::Vector3::readBinary
std::istream & readBinary(std::istream &s)
Definition: Vector3.cpp:87
octomath::Vector3::operator+=
void operator+=(const Vector3 &other)
Definition: Vector3.h:226
octomath::Vector3::operator-
Vector3 operator-() const
Definition: Vector3.h:191
octomath::Vector3::write
std::ostream & write(std::ostream &s) const
Definition: Vector3.cpp:78
octomath::Vector3::x
const float & x() const
Definition: Vector3.h:146
octomath::Vector3::angleTo
double angleTo(const Vector3 &other) const
Definition: Vector3.h:284
octomath::Vector3::normalized
Vector3 normalized() const
Definition: Vector3.h:278
octomath::Vector3::cross
Vector3 cross(const Vector3 &other) const
Three-dimensional vector (cross) product.
Definition: Vector3.h:107
octomath::Vector3::y
const float & y() const
Definition: Vector3.h:151
octomath::Vector3::y
float & y()
Definition: Vector3.h:136
octomath::Vector3::distance
double distance(const Vector3 &other) const
Definition: Vector3.h:292
octomath::Vector3::data
float data[3]
Definition: Vector3.h:315
octomath::Vector3::operator()
const float & operator()(unsigned int i) const
Definition: Vector3.h:122
octomath::Vector3::z
float & z()
Definition: Vector3.h:141
octomath::Vector3::x
float & x()
Definition: Vector3.h:131
octomath::Vector3::roll
const float & roll() const
Definition: Vector3.h:176