/**
* \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 A point of the road is visible if it is between the robot and the laser beam running through it, and if the previous point was visible
* All points in the road are updated
* @param road ...
* @param laserData ...
* @return bool
*/
bool ElasticBand::checkVisiblePoints(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData)
{
//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
wd = innermodel->getNode<InnerModelLaser>("laser")->laserTo("world", ld.dist, ld.angle);
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
int robot = road.getIndexOfNextPoint();
QVec memo = innermodel->transform6D("world", "robot");
for(int it = robot; it<road.size(); ++it)
{
road[it].isVisible = true;
innermodel->updateTransformValues("robot", road[it].pos.x(), road[it].pos.y(), road[it].pos.z(), 0, road[it].rot.y(), 0);
//get Robot transformation matrix
QMat m = innermodel->getTransformationMatrix("world", "robot");
// Transform all points at one to world RS
//m.print("m");
//pointsMat.print("pointsMat");
QMat newPoints = m * pointsMat;
//Check if they are inside the laser polygon
for (int i = 0; i < newPoints.nCols(); i++)
{
// qDebug() << __FUNCTION__ << "----------------------------------";
// qDebug() << __FUNCTION__ << QPointF(newPoints(0, i), newPoints(2, i));
// qDebug() << __FUNCTION__ << polygon;
if (polygon.containsPoint(QPointF(newPoints(0, i), newPoints(2, i)),Qt::OddEvenFill) == false)
{
road[it].isVisible = false;
//qFatal("fary");
break;
}
}
// if( road[it].isVisible == false)
// {
// for (int k = it; k < road.size(); ++k)
// road[k].isVisible = false;
// break;
// }
}
// Set the robot back to its original state
innermodel->updateTransformValues("robot", memo.x(), memo.y(), memo.z(), 0, memo.ry(), 0);
//road.print();
return true;
}
/**
* \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;
}
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);
}
