/**
* \brief Construct a diagonal matrix
*
* Use diagonal values to create a new diagonal matrix
* @param d Matrix to get diagonal values
* @return QMat new diagonal matrix
*/
QMat QMat::diagonal ( const QMat & d )
{
QMat R ( d.nRows(),d.nRows() );
for (int i=0; i<d.nRows(); i++)
{
for (int j=0; j<d.nCols(); j++)
{
R(i,j) = 0;
}
}
for ( int i=0; i < d.nRows(); i++ )
R ( i,i ) = d ( i );
return R;
}
/**
* \brief Matrix to matrix product operator; \f$ C = this * A \f$
*
* IPP coge (columnas, filas) en las llamadas
* @param A matrix factor for operation
* @return QMat New matrix result
*/
QMat QMat::operator * ( const QMat & A ) const
{
// printf("Operator *: (%d,%d) x (%d,%d)\n", rows, cols, A.rows, A.cols);
QMat C=zeros( rows, A.nCols() );
if ( cols != A.nRows())
{
QString ex= "QMat::operator* - a.cols!=b.rows";
throw ex;
}
else
{
#ifdef COMPILE_IPP
ippmMul_mm_32f ( toDataConst(), cols*sizeof ( T ), sizeof ( T ), cols, rows, A.toDataConst(), A.nCols() *sizeof ( T ), sizeof ( T ), A.nCols(), A.nRows(), C.toData(), C.nCols() *sizeof ( T ), sizeof ( T ) );
#else
for(int i=0;i<rows;i++)
{
for(int j=0;j<A.cols;j++)
{
C(i,j)=0;
for(int k=0;k<cols;k++)
{
C(i,j) += operator()(i,k)*A(k,j);
}
// printf("%f ", C(i,j));
}
// printf("\n");
}
#endif
}
return C;
}
/**
* @brief Moves a virtual copy of the robot along the road checking for enough free space around it
*
* @param innermodel ...
* @param road ...
* @param laserData ...
* @param robotRadius ...
* @return bool
*/
bool ElasticBand::checkCollisionAlongRoad(InnerModel *innermodel, const RoboCompLaser::TLaserData& laserData, WayPoints &road, WayPoints::const_iterator robot,
WayPoints::const_iterator target, float robotRadius)
{
//Simplify laser polyline using Ramer-Douglas-Peucker algorithm
std::vector<Point> points, res;
QVec wd;
for( auto &ld : laserData)
{
wd = innermodel->laserTo("world", "laser", ld.dist, ld.angle); //OPTIMIZE THIS FOR ALL CLASS METHODS
points.push_back(Point(wd.x(), wd.z()));
}
res = simPath.simplifyWithRDP(points, 70);
qDebug() << __FUNCTION__ << "laser polygon after simp" << res.size();
// Create a QPolygon so we can check if robot outline falls inside
QPolygonF polygon;
for (auto &p: res)
polygon << QPointF(p.x, p.y);
// Move the robot along the road
QVec memo = innermodel->transform6D("world","robot");
bool free = false;
for( WayPoints::const_iterator it = robot; it != target; ++it)
{
if( it->isVisible == false)
break;
// compute orientation of the robot at the point
innermodel->updateTransformValues("robot", it->pos.x(), it->pos.y(), it->pos.z(), 0, it->rot.y(), 0);
//get Robot transformation matrix
QMat m = innermodel->getTransformationMatrix("world", "robot");
// Transform all points at one
qDebug() << __FUNCTION__ << "hello2";
m.print("m");
pointsMat.print("pointsMat");
QMat newPoints = m * pointsMat;
qDebug() << __FUNCTION__ << "hello3";
//Check if they are inside the laser polygon
for( int i=0; i<newPoints.nRows(); i++)
if( polygon.containsPoint(QPointF(pointsMat(i,0)/pointsMat(i,3), pointsMat(i,2)/pointsMat(i,3)), Qt::OddEvenFill ) == false)
{
free = false;
break;
}
free = true;
}
qDebug() << __FUNCTION__ << "hello";
// Set the robot back to its original state
innermodel->updateTransformValues("robot", memo.x(), memo.y(), memo.z(), 0, memo.ry(), 0);
return free ? true : false;
}
int QMat::maxDim ( const QMat &A )
{
return qMax ( A.nRows(), A.nCols() );
}
int QMat::minDim ( const QMat &A )
{
return qMin ( A.nRows(), A.nCols() );
}
bool QMat::isSquare ( const QMat &A ) const
{
return ( A.nRows() == A.nCols() );
}
bool RMat::QMat::is3ColumnVector(const QMat & A) const
{
return (A.nRows() == 3 and A.nCols() == 1);
}
