MRPT  1.9.9
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-2019, 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 
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;
18 using mrpt::DEG2RAD;
19 using mrpt::RAD2DEG;
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);
36  omega = DEG2RAD(m(0, 2));
37 }
38 // Transform the (vx,vy) components for a counterclockwise rotation of `ang`
39 // radians
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
constexpr double DEG2RAD(const double x)
Degrees to radians.
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:627
bool operator!=(const TTwist2D &o) const
Definition: Twist2D.cpp:51
constexpr double RAD2DEG(const double x)
Radians to degrees.
Lightweight 2D pose.
Definition: TPose2D.h:22
double omega
Angular velocity (rad/s)
Definition: TTwist2D.h:28



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 9690a7a25 Mon Feb 24 17:42:02 2020 +0100 at lun feb 24 18:00:10 CET 2020