YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
Matrix.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2026 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7#include <yarp/sig/Matrix.h>
8
9#include <yarp/conf/system.h>
10
11#include <yarp/sig/Vector.h>
12#include <yarp/os/Bottle.h>
16#include <yarp/os/NetFloat64.h>
17#include <yarp/os/NetInt32.h>
18
19#include <vector>
20#include <cstdio>
21
22using namespace yarp::sig;
23using namespace yarp::os;
24
25#define RES(v) ((std::vector<T> *)v)
26
43
44// network stuff
45#include <yarp/os/NetInt32.h>
46
47bool yarp::sig::removeCols(const Matrix &in, Matrix &out, size_t first_col, size_t how_many)
48{
49 size_t nrows = in.rows();
50 size_t ncols = in.cols();
51 Matrix ret(nrows, ncols-how_many);
52 for (size_t r = 0; r < nrows; r++) {
53 for(size_t c_in=0,c_out=0;c_in<ncols; c_in++)
54 {
55 if (c_in==first_col)
56 {
57 c_in=c_in+(how_many-1);
58 continue;
59 }
60 ret[r][c_out]=(in)[r][c_in];
61 c_out++;
62 }
63 }
64 out=ret;
65 return true;
66}
67
68bool yarp::sig::removeRows(const Matrix &in, Matrix &out, size_t first_row, size_t how_many)
69{
70 size_t nrows = in.rows();
71 size_t ncols = in.cols();
72 Matrix ret(nrows-how_many, ncols);
73 for (size_t c = 0; c < ncols; c++) {
74 for(size_t r_in=0, r_out=0; r_in<nrows; r_in++)
75 {
76 if (r_in==first_row)
77 {
78 r_in=r_in+(how_many-1);
79 continue;
80 }
81 ret[r_out][c]=(in)[r_in][c];
82 r_out++;
83 }
84 }
85 out=ret;
86 return true;
87}
88
89bool yarp::sig::submatrix(const Matrix &in, Matrix &out, size_t r1, size_t r2, size_t c1, size_t c2)
90{
91 double *t=out.data();
92 const double *i=in.data()+in.cols()*r1+c1;
93 const int offset=in.cols()-(c2-c1+1);
94
95 if (i == nullptr || t == nullptr) {
96 return false;
97 }
98
99 for(size_t r=0;r<=(r2-r1);r++)
100 {
101 for(size_t c=0;c<=(c2-c1);c++)
102 {
103 *t++=*i++;
104 }
105 i+=offset;
106 }
107
108 return true;
109}
110
111/*
112bool Matrix::read(yarp::os::ConnectionReader& connection) {
113 // auto-convert text mode interaction
114 connection.convertTextMode();
115 MatrixPortContentHeader header;
116 bool ok = connection.expectBlock((char*)&header, sizeof(header));
117 if (!ok) {
118 return false;
119 }
120 size_t r=rows();
121 size_t c=cols();
122 if (header.listLen > 0)
123 {
124 if ( r != (size_t)(header.rows) || c!=(size_t)(header.cols))
125 {
126 resize(header.rows, header.cols);
127 }
128
129 int l=0;
130 double *tmp=data();
131 for (l = 0; l < header.listLen; l++) {
132 tmp[l] = connection.expectFloat64();
133 }
134 } else {
135 return false;
136 }
137
138 return true;
139}
140
141
142bool Matrix::write(yarp::os::ConnectionWriter& connection) const {
143 MatrixPortContentHeader header;
144
145 //header.totalLen = sizeof(header)+sizeof(double)*this->size();
146 header.outerListTag = BOTTLE_TAG_LIST;
147 header.outerListLen = 3;
148 header.rowsTag = BOTTLE_TAG_INT32;
149 header.colsTag = BOTTLE_TAG_INT32;
150 header.listTag = BOTTLE_TAG_LIST + BOTTLE_TAG_FLOAT64;
151 header.rows=rows();
152 header.cols=cols();
153 header.listLen = header.rows*header.cols;
154
155 connection.appendBlock((char*)&header, sizeof(header));
156
157 int l=0;
158 const double *tmp=data();
159 for (l = 0; l < header.listLen; l++) {
160 connection.appendFloat64(tmp[l]);
161 }
162
163 // if someone is foolish enough to connect in text mode,
164 // let them see something readable.
165 connection.convertTextMode();
166
167 return true;
168}
169*/
170
171std::string Matrix::toString(int precision, int width, const char* endRowStr) const {
172
173 // If the width is less than 1, use tabs, else use width number of spaces.
174 std::string spacer((width<0) ? "\t" : " ");
175
176 // Buffering.
177 std::string ret = "";
178 char buffer[350];
179 const double* src = (*this).data();
180
181 // Iterate through copying the contents from the matrix, into a string.
182 // Avoid unnecessary string resizes by only adding spacers at the beginning.
183 size_t r, c;
184 for (r = 0; r < nrows; r++) {
185 if (r) { ret += endRowStr; }
186 for (c = 0; c < ncols; c++) {
187 if (c) { ret += spacer; }
188 sprintf(buffer, "% *.*lf", width, precision, *src); src++;
189 ret += buffer;
190 }
191 }
192
193 return ret;
194}
195
196
197void Matrix::updatePointers()
198{
199 if (matrix != nullptr) {
200 delete[] matrix;
201 }
202
203 size_t r=0;
204 matrix=new double* [nrows];
205 if (nrows > 0) {
206 matrix[0] = storage.data();
207 }
208 for(r=1;r<nrows; r++)
209 {
210 matrix[r]=matrix[r-1]+ncols;
211 }
212}
213
215{
216 if (this == &r) {
217 return *this;
218 }
219
220 nrows=r.nrows;
221 ncols=r.ncols;
223 updatePointers();
224
225 return *this;
226}
227
228const Matrix &Matrix::operator=(double v)
229{
230 size_t nelem = nrows*ncols;
231 for (size_t k = 0; k < nelem; k++) {
232 storage[k] = v;
233 }
234
235 return *this;
236}
237
239{
240 if (matrix != nullptr) {
241 delete[] matrix;
242 }
243}
244
245void Matrix::resize(size_t new_r, size_t new_c)
246{
247 // if the size is the same, do nothing
248 if (new_r == nrows && new_c == ncols) {
249 return;
250 }
251
252 // if the storage is not empty, we need to copy the old data to the new storage
253 if (storage.size() != 0)
254 {
255 std::vector<double> tmp_storage(new_r * new_c);
256 double *tmp_new = tmp_storage.data();
257
258 const size_t copy_r = (new_r < nrows) ? new_r : nrows;
259 const size_t copy_c = (new_c < ncols) ? new_c : ncols;
260
261 // Copy rows that exist in both old and new matrix
262 for (size_t r = 0; r < copy_r; r++)
263 {
264 const double* src_row = matrix[r];
265 if (copy_c > 0) {
266 memcpy(tmp_new, src_row, sizeof(double) * copy_c);
267 }
268 // Zero-fill any additional columns in the new row (if new_c > copy_c)
269 if (new_c > copy_c) {
270 memset(tmp_new + copy_c, 0, sizeof(double) * (new_c - copy_c));
271 }
272 tmp_new += new_c;
273 }
274
275 // If new_r > copy_r, zero remaining new rows
276 for (size_t r = copy_r; r < new_r; r++)
277 {
278 if (new_c > 0) {
279 memset(tmp_new, 0, sizeof(double) * new_c);
280 }
281 tmp_new += new_c;
282 }
283
284 // Replace storage with the newly built buffer
285 storage = std::move(tmp_storage);
286 }
287 else
288 {
289 storage.resize(new_r * new_c);
290 memset(storage.data(), 0, new_r*new_c*sizeof(double));
291 }
292 nrows=new_r;
293 ncols=new_c;
294 updatePointers();
295}
296
298{
299 memset(storage.data(), 0, sizeof(double)*ncols*nrows);
300}
301
303{
304 Matrix ret;
306
307 for (size_t r = 0; r < nrows; r++) {
308 for(size_t c_in=0,c_out=0;c_in<ncols; c_in++)
309 {
310 if (c_in==first_col)
311 {
312 c_in=c_in+(how_many-1);
313 continue;
314 }
315 ret[r][c_out]=(*this)[r][c_in];
316 c_out++;
317 }
318 }
319
320 nrows=ret.nrows;
321 ncols=ret.ncols;
322 storage.resize(ncols*nrows);
323 memcpy(storage.data(), ret.storage.data(), ncols*nrows*sizeof(double));
324 updatePointers();
325 return ret;
326}
327
329{
330 Matrix ret;
332
333 for (size_t c = 0; c < ncols; c++) {
334 for(size_t r_in=0, r_out=0; r_in<nrows; r_in++)
335 {
336 if (r_in==first_row)
337 {
338 r_in=r_in+(how_many-1);
339 continue;
340 }
341 ret[r_out][c]=(*this)[r_in][c];
342 r_out++;
343 }
344 }
345
346 nrows=ret.nrows;
347 ncols=ret.ncols;
348 storage.resize(ncols*nrows);
349 memcpy(storage.data(), ret.storage.data(), ncols*nrows*sizeof(double));
350 updatePointers();
351 return ret;
352}
353
355{
356 Matrix ret;
357 ret.resize(ncols, nrows);
358
359 for (size_t r = 0; r < nrows; r++) {
360 for (size_t c = 0; c < ncols; c++) {
361 ret[c][r] = (*this)[r][c];
362 }
363 }
364
365 return ret;
366}
367
368Vector Matrix::getRow(size_t r) const
369{
370 Vector ret;
371 ret.resize(ncols);
372
373 for (size_t c = 0; c < ncols; c++) {
374 ret[c] = (*this)[r][c];
375 }
376
377 return ret;
378}
379
381{
382 Vector ret;
383 ret.resize(nrows);
384
385 for (size_t r = 0; r < nrows; r++) {
386 ret[r] = (*this)[r][c];
387 }
388
389 return ret;
390}
391
392Vector Matrix::subrow(size_t r, size_t c, size_t size) const
393{
394 if (r >= rows() || c + size - 1 >= cols()) {
395 return Vector(0);
396 }
397
398 Vector ret(size);
399
400 for (size_t i = 0; i < size; i++) {
401 ret[i] = (*this)[r][c + i];
402 }
403
404 return ret;
405}
406
407Vector Matrix::subcol(size_t r, size_t c, size_t size) const
408{
409 if (r + size - 1 >= rows() || c >= cols()) {
410 return Vector(0);
411 }
412
413 Vector ret(size);
414
415 for (size_t i = 0; i < size; i++) {
416 ret[i] = (*this)[r + i][c];
417 }
418
419 return ret;
420}
421
423{
424 zero();
425 size_t tmpR=nrows;
426 if (ncols < nrows) {
427 tmpR = ncols;
428 }
429
430 size_t c=0;
431 for (size_t r = 0; r < tmpR; r++, c++) {
432 (*this)[r][c] = 1.0;
433 }
434
435 return *this;
436}
437
439{
440 zero();
441 size_t tmpR=nrows;
442 if (ncols < nrows) {
443 tmpR = ncols;
444 }
445
446 size_t c=0;
447 for (size_t r = 0; r < tmpR; r++, c++) {
448 (*this)[r][c] = d[r];
449 }
450
451 return *this;
452}
453
455{
456 //check dimensions first
457 if ((rows() != r.rows()) || (cols() != r.cols())) {
458 return false;
459 }
460
461 const double *tmp1=data();
462 const double *tmp2=r.data();
463
464 if (tmp1 == nullptr || tmp2 == nullptr) {
465 return false;
466 }
467
468 int c=rows()*cols();
469 while(c--)
470 {
471 if (*tmp1++ != *tmp2++) {
472 return false;
473 }
474 }
475
476 return true;
477}
478
479bool Matrix::setRow(size_t row, const Vector &r)
480{
481 if ((row >= nrows) || (r.size() != ncols)) {
482 return false;
483 }
484
485 for (size_t c = 0; c < ncols; c++) {
486 (*this)[row][c] = r[c];
487 }
488
489 return true;
490}
491
492bool Matrix::setCol(size_t col, const Vector &c)
493{
494 if ((col >= ncols) || (c.size() != nrows)) {
495 return false;
496 }
497
498 for (size_t r = 0; r < nrows; r++) {
499 (*this)[r][col] = c[r];
500 }
501
502 return true;
503}
504
505bool Matrix::setSubmatrix(const yarp::sig::Matrix &m, size_t r, size_t c)
506{
507 if ((c + m.cols() > ncols) || (r + m.rows() > nrows)) {
508 return false;
509 }
510
511 for (size_t i = 0; i < m.rows(); i++) {
512 for (size_t j = 0; j < m.cols(); j++) {
513 (*this)[r + i][c + j] = m(i, j);
514 }
515 }
516 return true;
517}
518
519bool Matrix::setSubrow(const Vector &v, size_t r, size_t c)
520{
521 size_t s = v.size();
522 if (r >= nrows || c + s - 1 >= (size_t)ncols) {
523 return false;
524 }
525
526 for (size_t i = 0; i < s; i++) {
527 (*this)[r][i + c] = v[i];
528 }
529 return true;
530}
531
532bool Matrix::setSubcol(const Vector &v, size_t r, size_t c)
533{
534 size_t s = v.size();
535 if (r + s - 1 >= (size_t)nrows || c >= ncols) {
536 return false;
537 }
538
539 for (size_t i = 0; i < s; i++) {
540 (*this)[r + i][c] = v[i];
541 }
542 return true;
543}
544
545Matrix::Matrix(size_t r, size_t c)
546{
547 nrows = r;
548 ncols = c;
549 matrix = nullptr;
550 storage.resize(r*c);
551 memset(storage.data(), 0, r*c*sizeof(double));
552 updatePointers();
553}
554
556{
557 matrix = nullptr;
558 nrows=m.nrows;
559 ncols=m.ncols;
561 updatePointers();
562}
contains the definition of a Matrix type
contains the definition of a Vector type
yarp::os::NetInt32 outerListLen
Definition Matrix.cpp:32
MatrixPortContentHeader()=default
yarp::os::NetInt32 cols
Definition Matrix.cpp:36
yarp::os::NetInt32 listLen
Definition Matrix.cpp:38
yarp::os::NetInt32 rows
Definition Matrix.cpp:34
yarp::os::NetInt32 listTag
Definition Matrix.cpp:37
yarp::os::NetInt32 rowsTag
Definition Matrix.cpp:33
yarp::os::NetInt32 outerListTag
Definition Matrix.cpp:31
yarp::os::NetInt32 colsTag
Definition Matrix.cpp:35
A mini-server for performing network communication in the background.
std::vector< double > storage
Definition MatrixData.h:28
std::int32_t nrows
Definition MatrixData.h:26
std::int32_t ncols
Definition MatrixData.h:27
A class for a Matrix.
Definition Matrix.h:40
Matrix removeRows(size_t first_row, size_t how_many)
Modifies the matrix, removing one or more rows from it.
Definition Matrix.cpp:328
void zero()
Zero the matrix.
Definition Matrix.cpp:297
Vector getRow(size_t r) const
Get a row of the matrix as a vector.
Definition Matrix.cpp:368
Vector subcol(size_t r, size_t c, size_t size) const
Get a subcolumn of the matrix as a vector.
Definition Matrix.cpp:407
void resize(size_t r, size_t c)
Resize the matrix, if matrix is not empty preserve old content.
Definition Matrix.cpp:245
bool operator==(const yarp::sig::Matrix &r) const
True iff all elements of a match all element of b.
Definition Matrix.cpp:454
Matrix transposed() const
Return the transposed of the matrix.
Definition Matrix.cpp:354
bool setCol(size_t col, const Vector &c)
Set a column of the matrix copying the values from a vector: the vector length must be equal to the n...
Definition Matrix.cpp:492
bool setSubrow(const Vector &v, size_t r, size_t c)
Set a portion of a row of this matrix with the values of the specified vector v.
Definition Matrix.cpp:519
const Matrix & diagonal(const Vector &d)
Build a diagonal matrix, don't resize.
Definition Matrix.cpp:438
bool setRow(size_t row, const Vector &r)
Set a row of the matrix copying the values from a vector: the vector length must be equal to the numb...
Definition Matrix.cpp:479
size_t cols() const
Return number of columns.
Definition Matrix.h:91
~Matrix()
Destructor.
Definition Matrix.cpp:238
Vector getCol(size_t c) const
Get a columns of the matrix as a vector.
Definition Matrix.cpp:380
bool setSubmatrix(const Matrix &m, size_t r, size_t c)
Set a portion of this matrix with the values of the specified matrix m.
Definition Matrix.cpp:505
double * data()
Return a pointer to the first element.
Definition Matrix.h:294
bool setSubcol(const Vector &v, size_t r, size_t c)
Set a portion of a column of this matrix with the values of the specified vector v.
Definition Matrix.cpp:532
const Matrix & operator=(const Matrix &r)
Copy operator.
Definition Matrix.cpp:214
size_t rows() const
Return number of rows.
Definition Matrix.h:85
Vector subrow(size_t r, size_t c, size_t size) const
Get a subrow of the matrix as a vector.
Definition Matrix.cpp:392
Matrix removeCols(size_t first_col, size_t how_many)
Modifies the matrix, removing one or more columns from it.
Definition Matrix.cpp:302
std::string toString(int precision=-1, int width=-1, const char *endRowStr="\n") const
Print matrix to a string.
Definition Matrix.cpp:171
const Matrix & eye()
Build an identity matrix, don't resize.
Definition Matrix.cpp:422
size_t size() const
Returns the number of elements in the vector.
Definition VectorBase.h:167
void resize(size_t size)
Resize the vector.
Definition VectorBase.h:106
An interface to the operating system, including Port based communication.
std::int32_t NetInt32
Definition of the NetInt32 type.
Definition NetInt32.h:29
bool removeRows(const Matrix &in, Matrix &out, size_t first_row, size_t how_many)
Definition Matrix.cpp:68
bool submatrix(const Matrix &in, Matrix &out, size_t r1, size_t r2, size_t c1, size_t c2)
Definition Matrix.cpp:89
bool removeCols(const Matrix &in, Matrix &out, size_t first_col, size_t how_many)
Definition Matrix.cpp:47
#define YARP_END_PACK
Ends 1 byte packing for structs/classes.
Definition system.h:193
#define YARP_BEGIN_PACK
Starts 1 byte packing for structs/classes.
Definition system.h:192