MRPT  2.0.4
Twist2D.cpp
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 |
8  +------------------------------------------------------------------------+ */
9
10 #include "math-precomp.h" // Precompiled headers
11
12 #include <mrpt/core/bits_math.h>
13 #include <mrpt/math/TPose2D.h>
14 #include <mrpt/math/TTwist2D.h>
16
17 using namespace mrpt::math;
20
21 static_assert(std::is_trivially_copyable_v<TTwist2D>);
22
23 void TTwist2D::asString(std::string& s) const
24 {
25  s = mrpt::format("[%f %f %f]", vx, vy, RAD2DEG(omega));
26 }
27 void TTwist2D::fromString(const std::string& s)
28 {
29  CMatrixDouble m;
30  if (!m.fromMatlabStringFormat(s))
31  THROW_EXCEPTION("Malformed expression in ::fromString");
32  ASSERTMSG_(
33  m.rows() == 1 && m.cols() == 3, "Wrong size of vector in ::fromString");
34  vx = m(0, 0);
35  vy = m(0, 1);
37 }
38 // Transform the (vx,vy) components for a counterclockwise rotation of `ang`
40 void TTwist2D::rotate(const double ang)
41 {
42  const double nvx = vx * cos(ang) - vy * sin(ang);
43  const double nvy = vx * sin(ang) + vy * cos(ang);
44  vx = nvx;
45  vy = nvy;
46 }
47 bool TTwist2D::operator==(const TTwist2D& o) const
48 {
49  return vx == o.vx && vy == o.vy && omega == o.omega;
50 }
51 bool TTwist2D::operator!=(const TTwist2D& o) const { return !(*this == o); }
53 {
54  return mrpt::math::TPose2D(vx * dt, vy * dt, omega * dt);
55 }
56
59 {
60  for (size_t i = 0; i < o.size(); i++) in >> o[i];
61  return in;
62 }
65 {
66  for (size_t i = 0; i < o.size(); i++) out << o[i];
67  return out;
68 }
bool operator==(const TTwist2D &o) const
Definition: Twist2D.cpp:47
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
void rotate(const double ang)
Transform the (vx,vy) components for a counterclockwise rotation of ang radians.
Definition: Twist2D.cpp:40
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, CMatrixD::Ptr &pObj)
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
Definition: TTwist2D.h:19
std::string asString() const
Definition: TTwist2D.h:97
This base provides a set of functions for maths stuff.
mrpt::math::TPose2D operator*(const double dt) const
Returns the pose increment of multiplying each twist component times "dt" seconds.
Definition: Twist2D.cpp:52
double vx
Velocity components: X,Y (m/s)
Definition: TTwist2D.h:26
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[0.02 1.04 -45...
Definition: Twist2D.cpp:27
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
bool fromMatlabStringFormat(const std::string &s, mrpt::optional_ref< std::ostream > dump_errors_here=std::nullopt)
Reads a matrix from a string in Matlab-like format, for example: "[1 0 2; 0 4 -1]" The string must st...
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
constexpr std::size_t size() const
Definition: TPoseOrPoint.h:67
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::vision::TStereoCalibResults out
mrpt::serialization::CArchive & operator<<(mrpt::serialization::CArchive &s, const CVectorFloat &a)
Definition: math.cpp:626
bool operator!=(const TTwist2D &o) const
Definition: Twist2D.cpp:51