MRPT  2.0.4
TTwist3D.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/math/TPoseOrPoint.h>
12 
13 namespace mrpt::math
14 {
15 /** 3D twist: 3D velocity vector (vx,vy,vz) + angular velocity (wx,wy,wz)
16  * \sa mrpt::math::TTwist2D, mrpt::math::TPose3D
17  */
18 struct TTwist3D : public internal::ProvideStaticResize<TTwist3D>
19 {
20  enum
21  {
23  };
24 
25  /** Velocity components: X,Y (m/s) */
26  double vx{.0}, vy{.0}, vz{.0};
27  /** Angular velocity (rad/s) */
28  double wx{.0}, wy{.0}, wz{.0};
29 
30  /** Constructor from components */
31  constexpr TTwist3D(
32  double vx_, double vy_, double vz_, double wx_, double wy_, double wz_)
33  : vx(vx_), vy(vy_), vz(vz_), wx(wx_), wy(wy_), wz(wz_)
34  {
35  }
36  /** Default fast constructor. Initializes to zeros */
37  TTwist3D() = default;
38  /** Coordinate access using operator[]. Order: vx,vy,vz, wx, wy, wz */
39  double& operator[](size_t i)
40  {
41  switch (i)
42  {
43  case 0:
44  return vx;
45  case 1:
46  return vy;
47  case 2:
48  return vz;
49  case 3:
50  return wx;
51  case 4:
52  return wy;
53  case 5:
54  return wz;
55  default:
56  throw std::out_of_range("index out of range");
57  }
58  }
59  /// \overload
60  constexpr double operator[](size_t i) const
61  {
62  switch (i)
63  {
64  case 0:
65  return vx;
66  case 1:
67  return vy;
68  case 2:
69  return vz;
70  case 3:
71  return wx;
72  case 4:
73  return wy;
74  case 5:
75  return wz;
76  default:
77  throw std::out_of_range("index out of range");
78  }
79  }
80 
81  /** (i,0) access operator (provided for API compatibility with matrices).
82  * \sa operator[] */
83  double& operator()(int row, int col)
84  {
85  ASSERT_EQUAL_(col, 0);
86  return (*this)[row];
87  }
88  /// \overload
89  constexpr double operator()(int row, int col) const
90  {
91  ASSERT_EQUAL_(col, 0);
92  return (*this)[row];
93  }
94 
95  /** Scale factor */
96  void operator*=(const double k)
97  {
98  vx *= k;
99  vy *= k;
100  vz *= k;
101  wx *= k;
102  wy *= k;
103  wz *= k;
104  }
105 
106  /** Transformation into vector [vx vy vz wx wy wz] */
107  template <typename VECTORLIKE>
108  void asVector(VECTORLIKE& v) const
109  {
110  v.resize(6);
111  for (int i = 0; i < 6; i++) v[i] = (*this)[i];
112  }
113  template <typename VECTORLIKE>
114  VECTORLIKE asVector() const
115  {
116  VECTORLIKE v;
117  asVector(v);
118  return v;
119  }
120 
121  /** Sets from a vector [vx vy vz wx wy wz] */
122  template <typename VECTORLIKE>
123  void fromVector(const VECTORLIKE& v)
124  {
125  ASSERT_EQUAL_(v.size(), 6);
126  for (int i = 0; i < 6; i++) (*this)[i] = v[i];
127  }
128  bool operator==(const TTwist3D& o) const;
129  bool operator!=(const TTwist3D& o) const;
130  /** Returns a human-readable textual representation of the object (eg: "[vx
131  * vy vz wx wy wz]", omegas in deg/s)
132  * \sa fromString
133  */
134  void asString(std::string& s) const;
135  std::string asString() const
136  {
137  std::string s;
138  asString(s);
139  return s;
140  }
141 
142  /** Transform all 6 components for a change of reference frame from "A" to
143  * another frame "B" whose rotation with respect to "A" is given by `rot`.
144  * The translational part of the pose is ignored */
145  void rotate(const mrpt::math::TPose3D& rot);
146 
147  /** Set the current object value from a string generated by 'asString' (eg:
148  * "[vx vy vz wx wy wz]" )
149  * \sa asString
150  * \exception std::exception On invalid format
151  */
152  void fromString(const std::string& s);
153 
154  static TTwist3D FromString(const std::string& s)
155  {
156  TTwist3D o;
157  o.fromString(s);
158  return o;
159  }
160 };
161 
166 
167 } // namespace mrpt::math
168 
169 namespace mrpt::typemeta
170 {
171 // Specialization must occur in the same namespace
173 
174 } // namespace mrpt::typemeta
void asVector(VECTORLIKE &v) const
Transformation into vector [vx vy vz wx wy wz].
Definition: TTwist3D.h:108
constexpr double operator[](size_t i) const
Definition: TTwist3D.h:60
constexpr TTwist3D(double vx_, double vy_, double vz_, double wx_, double wy_, double wz_)
Constructor from components.
Definition: TTwist3D.h:31
double & operator[](size_t i)
Coordinate access using operator[].
Definition: TTwist3D.h:39
void operator*=(const double k)
Scale factor.
Definition: TTwist3D.h:96
3D twist: 3D velocity vector (vx,vy,vz) + angular velocity (wx,wy,wz)
Definition: TTwist3D.h:18
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, CMatrixD::Ptr &pObj)
static TTwist3D FromString(const std::string &s)
Definition: TTwist3D.h:154
This base provides a set of functions for maths stuff.
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[vx vy vz wx wy wz]" ) ...
Definition: Twist3D.cpp:30
bool operator!=(const TTwist3D &o) const
Definition: Twist3D.cpp:61
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
TTwist3D()=default
Default fast constructor.
#define MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(_TYPE, __NS)
Declares a typename to be "type" (without the NS prefix)
Definition: TTypeName.h:128
void fromVector(const VECTORLIKE &v)
Sets from a vector [vx vy vz wx wy wz].
Definition: TTwist3D.h:123
VECTORLIKE asVector() const
Definition: TTwist3D.h:114
double wx
Angular velocity (rad/s)
Definition: TTwist3D.h:28
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
double vx
Velocity components: X,Y (m/s)
Definition: TTwist3D.h:26
Provided for STL and matrices/vectors compatibility.
Definition: TPoseOrPoint.h:63
mrpt::vision::TStereoCalibResults out
void rotate(const mrpt::math::TPose3D &rot)
Transform all 6 components for a change of reference frame from "A" to another frame "B" whose rotati...
Definition: Twist3D.cpp:43
mrpt::serialization::CArchive & operator<<(mrpt::serialization::CArchive &s, const CVectorFloat &a)
Definition: math.cpp:626
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
double & operator()(int row, int col)
(i,0) access operator (provided for API compatibility with matrices).
Definition: TTwist3D.h:83
std::string asString() const
Definition: TTwist3D.h:135
bool operator==(const TTwist3D &o) const
Definition: Twist3D.cpp:56
constexpr double operator()(int row, int col) const
Definition: TTwist3D.h:89



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020