YARP
Yet Another Robot Platform
Vec2D.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * All rights reserved.
4  *
5  * This software may be modified and distributed under the terms of the
6  * BSD-3-Clause license. See the accompanying LICENSE file for details.
7  */
8 
9 #include <yarp/math/Vec2D.h>
10 
13 #include <yarp/os/LogComponent.h>
14 #include <yarp/math/Math.h>
15 #include <sstream>
16 #include <cmath>
17 #include <cstdio>
18 
19 // network stuff
20 #include <yarp/os/NetInt32.h>
21 
22 using namespace yarp::math;
23 
24 namespace {
25 YARP_LOG_COMPONENT(VEC2D, "yarp.math.Vec2D")
26 }
27 
28 
31 {
32 public:
33  yarp::os::NetInt32 listTag{0};
34  yarp::os::NetInt32 listLen{0};
36 };
38 
39 namespace yarp {
40 namespace math {
41 template<>
43 {
44  // auto-convert text mode interaction
45  connection.convertTextMode();
47  bool ok = connection.expectBlock((char*)&header, sizeof(header));
48  if (!ok) return false;
49 
50  if (header.listLen == 2 && header.listTag == (BOTTLE_TAG_LIST | BOTTLE_TAG_FLOAT64))
51  {
52  this->x = connection.expectFloat64();
53  this->y = connection.expectFloat64();
54  }
55  else
56  {
57  return false;
58  }
59 
60  return !connection.isError();
61 }
62 
63 template<>
65 {
66  // auto-convert text mode interaction
67  connection.convertTextMode();
69  bool ok = connection.expectBlock((char*)&header, sizeof(header));
70  if (!ok) return false;
71 
72  if (header.listLen == 2 && header.listTag == (BOTTLE_TAG_LIST | BOTTLE_TAG_INT32))
73  {
74  this->x = connection.expectInt32();
75  this->y = connection.expectInt32();
76  }
77  else
78  {
79  return false;
80  }
81 
82  return !connection.isError();
83 }
84 
85 template<>
87 {
88  // auto-convert text mode interaction
89  connection.convertTextMode();
91  bool ok = connection.expectBlock((char*)&header, sizeof(header));
92  if (!ok) return false;
93 
94  if (header.listLen == 2 && header.listTag == (BOTTLE_TAG_LIST | BOTTLE_TAG_INT64))
95  {
96  this->x = connection.expectInt64();
97  this->y = connection.expectInt64();
98  }
99  else
100  {
101  return false;
102  }
103 
104  return !connection.isError();
105 }
106 
107 template<>
109 {
110  Vec2DPortContentHeader header;
111 
113  header.listLen = 2;
114 
115  connection.appendBlock((char*)&header, sizeof(header));
116 
117  connection.appendFloat64(this->x);
118  connection.appendFloat64(this->y);
119 
120  connection.convertTextMode();
121 
122  return !connection.isError();
123 }
124 
125 template<>
127 {
128  Vec2DPortContentHeader header;
129 
131  header.listLen = 2;
132 
133  connection.appendBlock((char*)&header, sizeof(header));
134 
135  connection.appendInt32(this->x);
136  connection.appendInt32(this->y);
137 
138  connection.convertTextMode();
139 
140  return !connection.isError();
141 }
142 
143 template<>
145 {
146  Vec2DPortContentHeader header;
147 
149  header.listLen = 2;
150 
151  connection.appendBlock((char*)&header, sizeof(header));
152 
153  connection.appendInt64(this->x);
154  connection.appendInt64(this->y);
155 
156  connection.convertTextMode();
157 
158  return !connection.isError();
159 }
160 
161 } // namespace math
162 } // namespace yarp
163 
164 
165 template <typename T>
166 std::string yarp::math::Vec2D<T>::toString(int precision, int width) const
167 {
168  std::ostringstream stringStream;
169  stringStream.precision(precision);
170  stringStream.width(width);
171  stringStream << std::string("x:") << x << std::string(" y:") << y;
172  return stringStream.str();
173 }
174 
175 template <typename T>
177 {
178  return T(sqrt(x*x + y*y));
179 }
180 
181 //constructors
182 template <typename T>
184 {
185 }
186 
187 template <typename T>
189 {
190  yCAssert(VEC2D, v.size() == 2);
191  x = T(v[0]);
192  y = T(v[1]);
193 }
194 
195 template <typename T>
196 yarp::math::Vec2D<T>::Vec2D(const T& x_value, const T& y_value)
197 {
198  x = x_value;
199  y = y_value;
200 }
201 
202 template <typename T>
204 {
205  yCAssert(VEC2D, lhs.rows() == 2 && lhs.cols() == 2);
206  T x = rhs.x; T y = rhs.y;
207  rhs.x = T(lhs[0][0] * x + lhs[0][1] * y);
208  rhs.y = T(lhs[1][0] * x + lhs[1][1] * y);
209  return rhs;
210 }
211 
212 template <typename T>
214 {
215  lhs += rhs;
216  return lhs;
217 }
218 
219 template <typename T>
221 {
222  lhs -= rhs;
223  return lhs;
224 }
225 
226 template <typename T>
228 {
229  this->x += rhs.x;
230  this->y += rhs.y;
231  return *this;
232 }
233 
234 template <typename T>
236 {
237  this->x -= rhs.x;
238  this->y -= rhs.y;
239  return *this;
240 }
241 
242 template <typename T>
244 {
245  if (this->x == rhs.x &&
246  this->y == rhs.y) return true;
247  return false;
248 }
249 
250 template <typename T>
252 {
253  if (this->x == rhs.x &&
254  this->y == rhs.y) return false;
255  return true;
256 }
257 
264 
#define BOTTLE_TAG_FLOAT64
Definition: Bottle.h:27
#define BOTTLE_TAG_INT64
Definition: Bottle.h:24
#define BOTTLE_TAG_INT32
Definition: Bottle.h:23
#define BOTTLE_TAG_LIST
Definition: Bottle.h:30
yarp::math::Vec2D< T > operator*(const yarp::sig::Matrix &lhs, yarp::math::Vec2D< T > rhs)
Definition: Vec2D.cpp:203
yarp::math::Vec2D< T > operator-(yarp::math::Vec2D< T > lhs, const yarp::math::Vec2D< T > &rhs)
Definition: Vec2D.cpp:220
yarp::math::Vec2D< T > operator+(yarp::math::Vec2D< T > lhs, const yarp::math::Vec2D< T > &rhs)
Definition: Vec2D.cpp:213
yarp::os::NetInt32 listLen
Definition: Vec2D.cpp:34
yarp::os::NetInt32 listTag
Definition: Vec2D.cpp:33
Vec2DPortContentHeader()=default
std::string toString(int precision=-1, int width=-1) const
Creates a string object containing a text representation of the object.
Definition: Vec2D.cpp:166
yarp::math::Vec2D< T > & operator-=(const yarp::math::Vec2D< T > &rhs)
Definition: Vec2D.cpp:235
bool operator==(const yarp::math::Vec2D< T > &rhs) const
Definition: Vec2D.cpp:243
T norm() const
Returns the Euclidean norm of the Vec2D, i.e.
Definition: Vec2D.cpp:176
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
bool write(yarp::os::ConnectionWriter &connection) const override
Write vector to a connection.
bool operator!=(const yarp::math::Vec2D< T > &rhs) const
Definition: Vec2D.cpp:251
yarp::math::Vec2D< T > & operator+=(const yarp::math::Vec2D< T > &rhs)
Definition: Vec2D.cpp:227
An interface for reading from a network connection.
virtual bool expectBlock(char *data, size_t len)=0
Read a block of data from the network connection.
virtual std::int32_t expectInt32()=0
Read a 32-bit integer from the network connection.
virtual bool convertTextMode()=0
Reads in a standard description in text mode, and converts it to a standard description in binary.
virtual std::int64_t expectInt64()=0
Read a 64-bit integer from the network connection.
virtual bool isError() const =0
virtual yarp::conf::float64_t expectFloat64()=0
Read a 64-bit floating point number from the network connection.
An interface for writing to a network connection.
virtual bool isError() const =0
virtual void appendInt64(std::int64_t data)=0
Send a representation of a 64-bit integer to the network connection.
virtual bool convertTextMode()=0
Converts a standard description in binary into a textual description, if the connection is in text-mo...
virtual void appendInt32(std::int32_t data)=0
Send a representation of a 32-bit integer to the network connection.
virtual void appendFloat64(yarp::conf::float64_t data)=0
Send a representation of a 64-bit floating point number to the network connection.
virtual void appendBlock(const char *data, size_t len)=0
Send a block of data to the network connection.
A class for a Matrix.
Definition: Matrix.h:46
size_t cols() const
Return number of columns.
Definition: Matrix.h:101
size_t rows() const
Return number of rows.
Definition: Matrix.h:95
size_t size() const
Definition: Vector.h:326
#define yCAssert(component, x)
Definition: LogComponent.h:172
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:80
std::int32_t NetInt32
Definition of the NetInt32 type.
Definition: NetInt32.h:33
The main, catch-all namespace for YARP.
Definition: environment.h:25
#define YARP_END_PACK
Ends 1 byte packing for structs/classes.
Definition: system.h:194
#define YARP_BEGIN_PACK
Starts 1 byte packing for structs/classes.
Definition: system.h:193
#define YARP_math_API
Definition: api.h:18