48 #ifndef OCTOMATH_VECTOR3_H 49 #define OCTOMATH_VECTOR3_H 71 Vector3 () { data[0] = data[1] = data[2] = 0; }
126 return Vector3(y()*other.
z() - z()*other.
y(),
127 z()*other.
x() - x()*other.
z(),
128 x()*other.
y() - y()*other.
x());
133 return x()*other.
x() + y()*other.
y() + z()*other.
z();
136 inline const float& operator() (
unsigned int i)
const 140 inline float& operator() (
unsigned int i)
147 return operator()(0);
152 return operator()(1);
157 return operator()(2);
160 inline const float&
x()
const 162 return operator()(0);
165 inline const float&
y()
const 167 return operator()(1);
170 inline const float&
z()
const 172 return operator()(2);
177 return operator()(0);
182 return operator()(1);
187 return operator()(2);
190 inline const float&
roll()
const 192 return operator()(0);
197 return operator()(1);
200 inline const float&
yaw()
const 202 return operator()(2);
208 result(0) = -data[0];
209 result(1) = -data[1];
210 result(2) = -data[2];
217 result(0) += other(0);
218 result(1) += other(1);
219 result(2) += other(2);
234 result(0) -= other(0);
235 result(1) -= other(1);
236 result(2) -= other(2);
247 inline void operator-= (
const Vector3& other) {
253 inline void operator/= (
float x) {
266 for (
unsigned int i=0; i<3; i++) {
267 if (
operator()(i) != other(i))
275 for (
unsigned int i=0; i<3; i++) {
276 n += operator()(i) * operator()(i);
282 double len =
norm ();
284 *
this /= (float) len;
295 double dot_prod = this->dot(other);
296 double len1 = this->
norm();
297 double len2 = other.
norm();
298 return acos(dot_prod / (len1*len2));
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);
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);
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;
OctoMap: A probabilistic, flexible, and compact 3D mapping library for robotic systems.
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 dot(const Vector3 &other) const
double distanceXY(const Vector3 &other) const
Vector3(const Vector3 &other)
Copy constructor.
Vector3(float x, float y, float z)
Constructor.
std::vector< T1 > operator+(const std::vector< T1 > &a, const std::vector< T2 > &b)
a+b (element-wise sum)
double distance(const Vector3 &other) const
double angleTo(const Vector3 &other) const
const float & roll() const
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
bool operator==(const CArray< T, N > &x, const CArray< T, N > &y)
Vector3()
Default constructor.
const float & yaw() const
Vector3 normalized() const
Vector3 cross(const Vector3 &other) const
Three-dimensional vector (cross) product.
const float & pitch() const
TPoint3D operator-(const TPoint3D &p1)
Unary minus operator for 3D points.
std::vector< T1 > & operator*=(std::vector< T1 > &a, const std::vector< T2 > &b)
a*=b (element-wise multiplication)
std::vector< T1 > operator*(const std::vector< T1 > &a, const std::vector< T2 > &b)
a*b (element-wise multiplication)
CONTAINER::Scalar norm(const CONTAINER &v)
This class represents a three-dimensional vector.