Matrix.cpp

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
 * Copyright (C) 2006-2010 RobotCub Consortium
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms of the
 * BSD-3-Clause license. See the accompanying LICENSE file for details.
 */
 
#include <yarp/sig/Matrix.h>
 
#include <yarp/conf/system.h>
 
#include <yarp/sig/Vector.h>
#include <yarp/os/Bottle.h>
#include <yarp/os/ConnectionReader.h>
#include <yarp/os/ConnectionWriter.h>
#include <yarp/os/ManagedBytes.h>
#include <yarp/os/NetFloat64.h>
#include <yarp/os/NetInt32.h>
 
#include <vector>
#include <cstdio>
 
using namespace yarp::sig;
using namespace yarp::os;
 
#define RES(v) ((std::vector<T> *)v)
 
YARP_BEGIN_PACK
class MatrixPortContentHeader
{
public:
    yarp::os::NetInt32 outerListTag{0};
    yarp::os::NetInt32 outerListLen{0};
    yarp::os::NetInt32 rowsTag{0};
    yarp::os::NetInt32 rows{0};
    yarp::os::NetInt32 colsTag{0};
    yarp::os::NetInt32 cols{0};
    yarp::os::NetInt32 listTag{0};
    yarp::os::NetInt32 listLen{0};
 
    MatrixPortContentHeader() = default;
};
YARP_END_PACK
 
// network stuff
#include <yarp/os/NetInt32.h>
 
bool yarp::sig::removeCols(const Matrix &in, Matrix &out, size_t first_col, size_t how_many)
{
    size_t nrows = in.rows();
    size_t ncols = in.cols();
    Matrix ret(nrows, ncols-how_many);
    for(size_t r=0; r<nrows; r++)
        for(size_t c_in=0,c_out=0;c_in<ncols; c_in++)
        {
            if (c_in==first_col)
            {
                c_in=c_in+(how_many-1);
                continue;
            }
            ret[r][c_out]=(in)[r][c_in];
            c_out++;
        }
    out=ret;
    return true;
}
 
bool yarp::sig::removeRows(const Matrix &in, Matrix &out, size_t first_row, size_t how_many)
{
    size_t nrows = in.rows();
    size_t ncols = in.cols();
    Matrix ret(nrows-how_many, ncols);
    for(size_t c=0; c<ncols; c++)
        for(size_t r_in=0, r_out=0; r_in<nrows; r_in++)
        {
            if (r_in==first_row)
            {
                r_in=r_in+(how_many-1);
                continue;
            }
            ret[r_out][c]=(in)[r_in][c];
            r_out++;
        }
     out=ret;
     return true;
}
 
bool yarp::sig::submatrix(const Matrix &in, Matrix &out, size_t r1, size_t r2, size_t c1, size_t c2)
{
    double *t=out.data();
    const double *i=in.data()+in.cols()*r1+c1;
    const int offset=in.cols()-(c2-c1+1);
 
    if(i == nullptr || t == nullptr)
        return false;
 
    for(size_t r=0;r<=(r2-r1);r++)
    {
        for(size_t c=0;c<=(c2-c1);c++)
        {
            *t++=*i++;
        }
        i+=offset;
    }
 
    return true;
}
 
 
bool Matrix::read(yarp::os::ConnectionReader& connection) {
    // auto-convert text mode interaction
    connection.convertTextMode();
    MatrixPortContentHeader header;
    bool ok = connection.expectBlock((char*)&header, sizeof(header));
    if (!ok) return false;
    size_t r=rows();
    size_t c=cols();
    if (header.listLen > 0)
    {
        if ( r != (size_t)(header.rows) || c!=(size_t)(header.cols))
        {
            resize(header.rows, header.cols);
        }
 
        int l=0;
        double *tmp=data();
        for(l=0;l<header.listLen;l++)
            tmp[l]=connection.expectFloat64();
    }
    else
        return false;
 
    return true;
}
 
 
bool Matrix::write(yarp::os::ConnectionWriter& connection) const {
    MatrixPortContentHeader header;
 
    //header.totalLen = sizeof(header)+sizeof(double)*this->size();
    header.outerListTag = BOTTLE_TAG_LIST;
    header.outerListLen = 3;
    header.rowsTag = BOTTLE_TAG_INT32;
    header.colsTag = BOTTLE_TAG_INT32;
    header.listTag = BOTTLE_TAG_LIST + BOTTLE_TAG_FLOAT64;
    header.rows=rows();
    header.cols=cols();
    header.listLen = header.rows*header.cols;
 
    connection.appendBlock((char*)&header, sizeof(header));
 
    int l=0;
    const double *tmp=data();
    for(l=0;l<header.listLen;l++)
        connection.appendFloat64(tmp[l]);
 
    // if someone is foolish enough to connect in text mode,
    // let them see something readable.
    connection.convertTextMode();
 
    return true;
}
 
 
std::string Matrix::toString(int precision, int width, const char* endRowStr) const {
 
    // If the width is less than 1, use tabs, else use width number of spaces.
    std::string spacer((width<0) ? "\t" : " ");
 
    // Buffering.
    std::string ret = "";
    char buffer[350];
    const double* src = (*this).data();
 
    // Iterate through copying the contents from the matrix, into a string.
    // Avoid unnecessary string resizes by only adding spacers at the beginning.
    size_t r, c;
    for (r = 0; r < nrows; r++) {
        if (r) { ret += endRowStr; }
        for (c = 0; c < ncols; c++) {
            if (c) { ret += spacer; }
            sprintf(buffer, "% *.*lf", width, precision, *src); src++;
            ret += buffer;
        }
    }
 
    return ret;
}
 
 
void Matrix::updatePointers()
{
    if (matrix!=nullptr)
        delete [] matrix;
 
    size_t r=0;
    matrix=new double* [nrows];
    if (nrows>0) matrix[0]=storage;
    for(r=1;r<nrows; r++)
    {
        matrix[r]=matrix[r-1]+ncols;
    }
}
 
const Matrix &Matrix::operator=(const Matrix &r)
{
    if (this == &r) return *this;
 
    if(nrows!=r.nrows || ncols!=r.ncols)
    {
        if (storage)
            delete [] storage;
 
        nrows=r.nrows;
        ncols=r.ncols;
 
        storage=new double[ncols*nrows];
        memcpy(storage, r.storage, ncols*nrows*sizeof(double));
        updatePointers();
    }
    else
    {
        if(!storage)
            storage=new double[ncols*nrows];
        memcpy(storage, r.storage, ncols*nrows*sizeof(double));
    }
 
    return *this;
}
 
const Matrix &Matrix::operator=(double v)
{
    size_t nelem = nrows*ncols;
    for(size_t k=0; k<nelem; k++)
        storage[k]=v;
 
    return *this;
}
 
Matrix::~Matrix()
{
    if (matrix!=nullptr)
        delete [] matrix;
 
    if (storage!=nullptr)
        delete [] storage;
}
 
void Matrix::resize(size_t new_r, size_t new_c)
{
    if(new_r==nrows && new_c==ncols)
        return;
 
    auto* new_storage=new double[new_r*new_c];
 
    const size_t copy_r=(new_r<nrows) ? new_r:nrows;
    const size_t copy_c=(new_c<ncols) ? new_c:ncols;
    //copy_r = (new_r<nrows) ? new_r:nrows;
 
    if (storage!=nullptr)
    {
        double *tmp_new=new_storage;
        double *tmp_current=storage;
        // copy content
 
        // favor performance for small matrices
#if 0
        const int stepN=(new_c-copy_c);
        const int stepC=(ncols-copy_c);
 
        for(int r=0; r<copy_r;r++)
        {
            for(int c=0;c<copy_c;c++)
                *tmp_new++=*tmp_current++;
            tmp_new+=stepN;
            tmp_current=matrix[r];
        }
#endif
 
        // favor performance with large matrices
        for(size_t r=0; r<copy_r;r++)
        {
            tmp_current=matrix[r];
            memcpy(tmp_new, tmp_current, sizeof(double)*copy_c);
            tmp_new+=new_c;
        }
 
 
        delete [] storage;
    }
    else
    {
        //zero memory
        memset(new_storage, 0, sizeof(double)*new_r*new_c);
    }
 
    storage=new_storage;
 
    nrows=new_r;
    ncols=new_c;
 
    // storage.resize(r*c);
    updatePointers();
}
 
void Matrix::zero()
{
    memset(storage, 0, sizeof(double)*ncols*nrows);
}
 
Matrix Matrix::removeCols(size_t first_col, size_t how_many)
{
    Matrix ret;
    ret.resize(nrows, ncols-how_many);
 
    for(size_t r=0; r<nrows; r++)
        for(size_t c_in=0,c_out=0;c_in<ncols; c_in++)
        {
            if (c_in==first_col)
            {
                c_in=c_in+(how_many-1);
                continue;
            }
            ret[r][c_out]=(*this)[r][c_in];
            c_out++;
        }
 
    if (storage)
      delete [] storage;
 
    nrows=ret.nrows;
    ncols=ret.ncols;
    storage=new double[ncols*nrows];
    memcpy(storage, ret.storage, ncols*nrows*sizeof(double));
    updatePointers();
    return ret;
}
 
Matrix Matrix::removeRows(size_t first_row, size_t how_many)
{
    Matrix ret;
    ret.resize(nrows-how_many, ncols);
 
    for(size_t c=0; c<ncols; c++)
        for(size_t r_in=0, r_out=0; r_in<nrows; r_in++)
        {
            if (r_in==first_row)
            {
                r_in=r_in+(how_many-1);
                continue;
            }
            ret[r_out][c]=(*this)[r_in][c];
            r_out++;
        }
 
    if (storage)
        delete [] storage;
 
    nrows=ret.nrows;
    ncols=ret.ncols;
    storage=new double[ncols*nrows];
    memcpy(storage, ret.storage, ncols*nrows*sizeof(double));
    updatePointers();
    return ret;
}
 
Matrix Matrix::transposed() const
{
    Matrix ret;
    ret.resize(ncols, nrows);
 
    for(size_t r=0; r<nrows; r++)
        for(size_t c=0;c<ncols; c++)
            ret[c][r]=(*this)[r][c];
 
    return ret;
}
 
Vector Matrix::getRow(size_t r) const
{
    Vector ret;
    ret.resize(ncols);
 
    for(size_t c=0;c<ncols;c++)
        ret[c]=(*this)[r][c];
 
    return ret;
}
 
Vector Matrix::getCol(size_t c) const
{
    Vector ret;
    ret.resize(nrows);
 
    for(size_t r=0;r<nrows;r++)
        ret[r]=(*this)[r][c];
 
    return ret;
}
 
Vector Matrix::subrow(size_t r, size_t c, size_t size) const
{
    if(r>=rows() || c+size-1>=cols())
        return Vector(0);
 
    Vector ret(size);
 
    for(size_t i=0;i<size;i++)
        ret[i]=(*this)[r][c+i];
 
    return ret;
}
 
Vector Matrix::subcol(size_t r, size_t c, size_t size) const
{
    if(r+size-1>=rows() || c>=cols())
        return Vector(0);
 
    Vector ret(size);
 
    for(size_t i=0;i<size;i++)
        ret[i]=(*this)[r+i][c];
 
    return ret;
}
 
const Matrix &Matrix::eye()
{
    zero();
    size_t tmpR=nrows;
    if (ncols<nrows)
        tmpR=ncols;
 
    size_t c=0;
    for(size_t r=0; r<tmpR; r++,c++)
        (*this)[r][c]=1.0;
 
    return *this;
}
 
const Matrix &Matrix::diagonal(const Vector &d)
{
    zero();
    size_t tmpR=nrows;
    if (ncols<nrows)
        tmpR=ncols;
 
    size_t c=0;
    for(size_t r=0; r<tmpR; r++,c++)
        (*this)[r][c]=d[r];
 
    return *this;
}
 
bool Matrix::operator==(const yarp::sig::Matrix &r) const
{
    //check dimensions first
    if ( (rows()!=r.rows()) || (cols()!=r.cols()))
        return false;
 
    const double *tmp1=data();
    const double *tmp2=r.data();
 
    if(tmp1 == nullptr || tmp2 == nullptr)
        return false;
 
    int c=rows()*cols();
    while(c--)
    {
        if (*tmp1++!=*tmp2++)
            return false;
    }
 
    return true;
}
 
bool Matrix::setRow(size_t row, const Vector &r)
{
    if((row>=nrows) || (r.length() != ncols))
        return false;
 
    for(size_t c=0;c<ncols;c++)
        (*this)[row][c]=r[c];
 
    return true;
}
 
bool Matrix::setCol(size_t col, const Vector &c)
{
    if((col>=ncols) || (c.length() != nrows))
        return false;
 
    for(size_t r=0;r<nrows;r++)
        (*this)[r][col]=c[r];
 
    return true;
}
 
bool Matrix::setSubmatrix(const yarp::sig::Matrix &m, size_t r, size_t c)
{
    if((c+m.cols()>ncols) || (r+m.rows()>nrows))
        return false;
 
    for(size_t i=0;i<m.rows();i++)
        for(size_t j=0;j<m.cols();j++)
            (*this)[r+i][c+j] = m(i,j);
    return true;
}
 
bool Matrix::setSubrow(const Vector &v, size_t r, size_t c)
{
    size_t s = v.size();
    if(r>=nrows || c+s-1>=(size_t)ncols)
        return false;
 
    for(size_t i=0;i<s;i++)
        (*this)[r][i+c] = v[i];
    return true;
}
 
bool Matrix::setSubcol(const Vector &v, size_t r, size_t c)
{
    size_t s = v.size();
    if(r+s-1>=(size_t)nrows || c>=ncols)
        return false;
 
    for(size_t i=0;i<s;i++)
        (*this)[r+i][c] = v[i];
    return true;
}
 
Matrix::Matrix(size_t r, size_t c):
    storage(nullptr),
    matrix(nullptr),
    nrows(r),
    ncols(c)
{
    storage=new double [r*c];
    memset(storage, 0, r*c*sizeof(double));
    updatePointers();
}
 
Matrix::Matrix(const Matrix &m): yarp::os::Portable(),
    storage(nullptr),
    matrix(nullptr)
{
    nrows=m.nrows;
    ncols=m.ncols;
 
    if (m.storage!=nullptr)
    {
        storage=new double [nrows*ncols];
        memcpy(storage, m.storage, nrows*ncols*sizeof(double));
 
        updatePointers();
    }
}