octomap 1.9.8
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
41namespace 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
This class represents a three-dimensional vector.
Definition: Vector3.h:50
std::ostream & writeBinary(std::ostream &s) const
Definition: Vector3.cpp:99
const float & z() const
Definition: Vector3.h:156
void operator/=(float x)
Definition: Vector3.h:239
const float & pitch() const
Definition: Vector3.h:181
Vector3 & normalize()
normalizes this vector, so that it has norm=1.0
Definition: Vector3.h:270
double angleTo(const Vector3 &other) const
Definition: Vector3.h:284
float & x()
Definition: Vector3.h:131
Vector3 operator-() const
Definition: Vector3.h:191
const float & operator()(unsigned int i) const
Definition: Vector3.h:122
double distance(const Vector3 &other) const
Definition: Vector3.h:292
Vector3 operator+(const Vector3 &other) const
Definition: Vector3.h:200
Vector3 & rotate_IP(double roll, double pitch, double yaw)
Definition: Vector3.cpp:41
Vector3 cross(const Vector3 &other) const
Three-dimensional vector (cross) product.
Definition: Vector3.h:107
Vector3(const Vector3 &other)
Copy constructor.
Definition: Vector3.h:63
double dot(const Vector3 &other) const
dot product
Definition: Vector3.h:117
float & y()
Definition: Vector3.h:136
float & z()
Definition: Vector3.h:141
const float & roll() const
Definition: Vector3.h:176
Vector3(float x, float y, float z)
Constructor.
Definition: Vector3.h:75
std::istream & read(std::istream &s)
Definition: Vector3.cpp:69
Vector3 operator*(float x) const
Definition: Vector3.h:209
void operator-=(const Vector3 &other)
Definition: Vector3.h:233
Vector3 & operator=(const Vector3 &other)
Assignment operator.
Definition: Vector3.h:91
void operator+=(const Vector3 &other)
Definition: Vector3.h:226
float & roll()
Definition: Vector3.h:161
float & yaw()
Definition: Vector3.h:171
const float & y() const
Definition: Vector3.h:151
Vector3 normalized() const
Definition: Vector3.h:278
void operator*=(float x)
Definition: Vector3.h:245
const float & x() const
Definition: Vector3.h:146
float data[3]
Definition: Vector3.h:315
double norm_sq() const
Definition: Vector3.h:265
double distanceXY(const Vector3 &other) const
Definition: Vector3.h:299
Vector3()
Default constructor.
Definition: Vector3.h:56
const float & yaw() const
Definition: Vector3.h:186
std::ostream & write(std::ostream &s) const
Definition: Vector3.cpp:78
std::istream & readBinary(std::istream &s)
Definition: Vector3.cpp:87
float & pitch()
Definition: Vector3.h:166
bool operator==(const Vector3 &other) const
Definition: Vector3.h:251
double norm() const
Definition: Vector3.h:260
Namespace of the math library in OctoMap.
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