Ravelin
Vector3.h
1 /****************************************************************************
2  * Copyright 2013 Evan Drumwright
3  * This library is distributed under the terms of the Apache V2.0
4  * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0).
5  ****************************************************************************/
6 
7 #ifndef VECTOR3
8 #error This class is not to be included by the user directly. Use Vector3d.h or Vector3f.h instead.
9 #endif
10 
11 class POSE3;
12 class MATRIX3;
13 
15 class VECTOR3
16 {
17  public:
18  VECTOR3(boost::shared_ptr<const POSE3> pose = boost::shared_ptr<const POSE3>()) { this->pose = pose; }
19  VECTOR3(boost::shared_ptr<POSE3> pose) { this->pose = boost::const_pointer_cast<const POSE3>(pose); }
20  VECTOR3(REAL x, REAL y, REAL z, boost::shared_ptr<const POSE3> pose = boost::shared_ptr<const POSE3>());
21  VECTOR3(REAL x, REAL y, REAL z, boost::shared_ptr<POSE3> pose);
22  VECTOR3(const REAL* array, boost::shared_ptr<const POSE3> pose = boost::shared_ptr<const POSE3>());
23  VECTOR3(const REAL* array, boost::shared_ptr<POSE3> pose);
24  VECTOR3(const VECTOR3& source) { operator=(source); }
25  VECTOR3(const ORIGIN3& source, boost::shared_ptr<const POSE3> pose) { this->pose = pose; operator=(source); }
26  VECTOR3(const ORIGIN3& source, boost::shared_ptr<POSE3> pose) { this->pose = boost::const_pointer_cast<const POSE3>(pose); operator=(source); }
27  REAL dot(const VECTOR3& v) const { return dot(*this, v); }
28  static REAL dot(const VECTOR3& v1, const VECTOR3& v2);
29  void normalize() { assert(norm() > std::numeric_limits<REAL>::epsilon()); operator/=(norm()); }
30  void normalize_or_zero() { REAL nrm = norm(); if (nrm > std::numeric_limits<REAL>::epsilon()) operator/=(nrm); else _data[0] = _data[1] = _data[2] = (REAL) 0.0; }
31  static VECTOR3 normalize(const VECTOR3& v) { VECTOR3 w = v; w.normalize(); return w; }
32  unsigned size() const { return 3; }
33  bool is_finite() const;
34  REAL norm_inf() const { return std::max(std::fabs(_data[0]), std::max(std::fabs(_data[1]), std::fabs(_data[2]))); }
35  REAL norm() const { return std::sqrt(norm_sq()); }
36  REAL norm_sq() const { return dot(*this, *this); }
37  static REAL norm(const VECTOR3& v) { return std::sqrt(norm_sq(v)); }
38  static REAL norm_sq(const VECTOR3& v) { return v.dot(v); }
39  VECTOR3& set_zero() { _data[0] = _data[1] = _data[2] = 0.0; return *this; }
40  VECTOR3& set_one() { _data[0] = _data[1] = _data[2] = 1.0; return *this; }
41  VECTOR3& set_zero(boost::shared_ptr<const POSE3> pose) { _data[0] = _data[1] = _data[2] = 0.0; this->pose = pose; return *this; }
42  VECTOR3& set_one(boost::shared_ptr<const POSE3> pose) { _data[0] = _data[1] = _data[2] = 1.0; this->pose = pose; return *this; }
43  static VECTOR3 zero(boost::shared_ptr<const POSE3> pose = boost::shared_ptr<const POSE3>()) { return VECTOR3(0.0, 0.0, 0.0, pose); }
44  static VECTOR3 one(boost::shared_ptr<const POSE3> pose = boost::shared_ptr<const POSE3>()) { return VECTOR3(1.0, 1.0, 1.0, pose); }
45  bool operator<(const VECTOR3& v) const;
46  VECTOR3& operator=(const ORIGIN3& o) { x() = o.x(); y() = o.y(); z() = o.z(); return *this; }
47  VECTOR3& operator=(const VECTOR3& v) { pose = v.pose; x() = v.x(); y() = v.y(); z() = v.z(); return *this; }
48  VECTOR3 operator+(const VECTOR3& v) const { VECTOR3 result = *this; result += v; return result; }
49  VECTOR3 operator-(const VECTOR3& v) const { VECTOR3 result = *this; result -= v; return result; }
50  VECTOR3 operator+(const ORIGIN3& o) const { VECTOR3 result = *this; result += o; return result; }
51  VECTOR3 operator-(const ORIGIN3& o) const { VECTOR3 result = *this; result -= o; return result; }
52  VECTOR3& operator+=(const VECTOR3& v);
53  VECTOR3& operator-=(const VECTOR3& v);
54  VECTOR3& operator+=(const ORIGIN3& o);
55  VECTOR3& operator-=(const ORIGIN3& o);
56  VECTOR3& operator*=(REAL scalar) { _data[0] *= scalar; _data[1] *= scalar; _data[2] *= scalar; return *this; }
57  VECTOR3 operator*(REAL scalar) const { VECTOR3 v = *this; v *= scalar; return v; }
58  VECTOR3 operator/(REAL scalar) const { VECTOR3 v = *this; v /= scalar; return v; }
59  VECTOR3& operator/=(REAL scalar) { _data[0] /= scalar; _data[1] /= scalar; _data[2] /= scalar; return *this; }
60  VECTOR3 operator-() const { return VECTOR3(-_data[0], -_data[1], -_data[2], pose); }
61  static VECTOR3 cross(const VECTOR3& v1, const VECTOR3& v2);
62  static VECTOR3 determine_orthogonal_vec(const VECTOR3& v);
63  static void determine_orthonormal_basis(const VECTOR3& v1, VECTOR3& v2, VECTOR3& v3);
64  REAL* data() { return _data; }
65  const REAL* data() const { return _data; }
66  unsigned rows() const { return 3; }
67  unsigned columns() const { return 1; }
68  VECTOR3& resize(unsigned m, unsigned n, bool preserve = false);
69  unsigned inc() const { return 1; }
70  unsigned leading_dim() const { return 3; }
71  REAL& x() { return _data[0]; }
72  const REAL& x() const { return _data[0]; }
73  REAL& y() { return _data[1]; }
74  const REAL& y() const { return _data[1]; }
75  REAL& z() { return _data[2]; }
76  const REAL& z() const { return _data[2]; }
77 
78  REAL& operator[](const unsigned i);
79  const REAL& operator[](const unsigned i) const;
80  const REAL* data(unsigned i) const;
81  REAL* data(unsigned i);
82  COLUMN_ITERATOR begin() { return column_iterator_begin(); }
83  CONST_COLUMN_ITERATOR begin() const { return column_iterator_begin(); }
84  COLUMN_ITERATOR end() { return column_iterator_end(); }
85  CONST_COLUMN_ITERATOR end() const { return column_iterator_end(); }
86  COLUMN_ITERATOR column_iterator_begin();
87  CONST_COLUMN_ITERATOR column_iterator_begin() const;
88  COLUMN_ITERATOR column_iterator_end();
89  CONST_COLUMN_ITERATOR column_iterator_end() const;
90  ROW_ITERATOR row_iterator_begin();
91  CONST_ROW_ITERATOR row_iterator_begin() const;
92  ROW_ITERATOR row_iterator_end();
93  CONST_ROW_ITERATOR row_iterator_end() const;
94 
95  VECTOR3& resize(unsigned N, bool keep = true)
96  {
97  #ifndef NEXCEPT
98  if (N != 3)
99  throw std::runtime_error("Can't resize a VECTOR3 to size other than 3!");
100  #endif
101  return *this;
102  }
103 
105  boost::shared_ptr<const POSE3> pose;
106 
107  private:
108  REAL _data[3];
109 }; // end class
110 
111 inline VECTOR3 operator*(REAL scalar, const VECTOR3& v) { return v * scalar; }
112 
114 inline std::ostream& operator<<(std::ostream& out, const VECTOR3& v)
115 {
116  out << "[" << v[0] << ", " << v[1] << ", " << v[2] << "] ";
117  return out;
118 };
119 
VECTOR3::operator<
bool operator<(const VECTOR3 &v) const
Compares the two vectors lexographically.
Definition: Vector3.cpp:124
CONST_ROW_ITERATOR
A construct for iterating over a rectangular block of a matrix.
Definition: RowIterator.h:210
VECTOR3::operator+=
VECTOR3 & operator+=(const VECTOR3 &v)
Adds two vectors in the same frame.
Definition: Vector3.cpp:195
VECTOR3::pose
boost::shared_ptr< const POSE3 > pose
The frame that this vector is defined in.
Definition: Vector3.h:105
VECTOR3::determine_orthonormal_basis
static void determine_orthonormal_basis(const VECTOR3 &v1, VECTOR3 &v2, VECTOR3 &v3)
Computes an orthonormal basis, given a single vector.
Definition: Vector3.cpp:114
POSE3
A rigid body pose.
Definition: Pose3.h:15
VECTOR3::cross
static VECTOR3 cross(const VECTOR3 &v1, const VECTOR3 &v2)
Computes the cross-product of two vectors.
Definition: Vector3.cpp:60
VECTOR3::is_finite
bool is_finite() const
Determines whether all components of this vector are finite.
Definition: Vector3.cpp:54
VECTOR3
A three-dimensional floating point vector used for representing points and vectors in 3D with associa...
Definition: Vector3.h:15
ORIGIN3
A three-dimensional floating point vector used for representing points and vectors in 3D and without ...
Definition: Origin3.h:16
COLUMN_ITERATOR
A construct for iterating over a rectangular block of a matrix.
Definition: ColumnIterator.h:12
VECTOR3::operator-=
VECTOR3 & operator-=(const VECTOR3 &v)
Subtracts a vector from this (in the same frame)
Definition: Vector3.cpp:218
CONST_COLUMN_ITERATOR
A construct for iterating over a rectangular block of a matrix.
Definition: ColumnIterator.h:215
MATRIX3
A 3x3 matrix that may be used for orientation, inertia tensors, etc.
Definition: Matrix3.h:20
ROW_ITERATOR
A construct for iterating over a rectangular block of a matrix.
Definition: RowIterator.h:12
VECTOR3::determine_orthogonal_vec
static VECTOR3 determine_orthogonal_vec(const VECTOR3 &v)
Determines a vector orthogonal to v.
Definition: Vector3.cpp:78
Generated on Wed Mar 9 2016 11:34:02 for Ravelin by   doxygen 1.8.6