34#ifndef OCTOMATH_VECTOR3_H
35#define OCTOMATH_VECTOR3_H
112 z()*other.
x() -
x()*other.
z(),
113 x()*other.
y() -
y()*other.
x());
119 return x()*other.
x() +
y()*other.
y() +
z()*other.
z();
146 inline const float&
x()
const
151 inline const float&
y()
const
156 inline const float&
z()
const
176 inline const float&
roll()
const
186 inline const float&
yaw()
const
194 result(0) = -
data[0];
195 result(1) = -
data[1];
196 result(2) = -
data[2];
203 result(0) += other(0);
204 result(1) += other(1);
205 result(2) += other(2);
220 result(0) -= other(0);
221 result(1) -= other(1);
222 result(2) -= other(2);
252 for (
unsigned int i=0; i<3; i++) {
253 if (
operator()(i) != other(i))
266 return (
x()*
x() +
y()*
y() +
z()*
z());
273 *
this /= (float) len;
285 double dot_prod = this->
dot(other);
286 double len1 = this->
norm();
287 double len2 = other.
norm();
288 return acos(dot_prod / (len1*len2));
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);
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);
308 std::istream&
read(std::istream &s);
309 std::ostream&
write(std::ostream &s)
const;
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