/** * \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); }