C++ QMat类代码示例

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

void RMat::QMat::SVD(QMat & U, QMat & D, QMat & V)
{
#ifdef COMPILE_IPP
// 	double ippA[rows*cols];
	double ippU[rows*rows], ippD[rows*cols], ippV[cols*cols];
	int i;
	
	U = QMat(rows, rows);
	D = QMat(rows, cols);
	V = QMat(cols, cols);

// 	for(i=0; i<rows*cols;i++)
// 	  ippA[i] = toData()[i];
	
	
	for(i=0; i<rows*rows;i++)
	  U.toData()[i] = ippU[i];
	
	for(i=0; i<cols;i+=1)
	  D.toData()[i*(cols+1)] = ippD[i];
	
	for(i=0; i<cols*cols;i++)
	  V.toData()[i] = ippV[i];
#else
	qFatal("QMat does not support eigen svd");
#endif

}

/**
 * \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 Matrix to vector product operator; \f$ C = this * vector \f$
 * @param A vector factor for operation
 * @return QVec New vector result
 */
QVec RMat::QMat::operator *(const QVec & vector) const
{
	Q_ASSERT ( cols == vector.size());
	QMat aux = vector.toColumnMatrix();
	QMat result = operator*(aux);
	return result.toVector();
}

QMat RMat::QMat::sqrt( )
{
	int i;
	QMat R ( rows, cols );
	for ( i=0;i< rows*cols;i++ )
		R.getWriteData()[i] = ::sqrt ( fabs ( getReadData()[i]) );
	return R;
}

/**
 * \brief Copy function
 *
 * Return a copy of this matrix
 * @return QMat matrix copied
*/
QMat QMat::copy()
{
	QMat A (rows, cols);
#ifdef COMPILE_IPP
	ippmCopy_ma_32f_SS ( toData(), rows*cols*sizeof ( T ), cols*sizeof ( T ), sizeof ( T ), A.toData(),rows*cols*sizeof ( T ), cols*sizeof ( T ), sizeof ( T ),  cols, rows, 1 );
#else
	memcpy(A.toData(), toData(), rows*cols*sizeof(T));
#endif
	return A;
}

/**
 * \brief Change the order of the indexes in the matrix \f$ C = this^t \f$
 * @return QMat new matrix with indexes reordered
 */
QMat QMat::transpose( ) const
{
	QMat C ( cols, rows );
#ifdef COMPILE_IPP
	ippmTranspose_m_32f ( toDataConst(), cols*sizeof ( T ), sizeof ( T ), cols, rows, C.toData(), rows*sizeof ( T ), sizeof ( T ) );
#else
	for (int fila=0; fila<rows; fila++ )
		for ( int columna=0; columna<cols; columna++ )
			C(columna,fila) = operator()(fila,columna);
#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;
 }

QMat RMat::QMat::makeDefPos()
{
	QVec vals ( rows,0. );
	QMat V = eigenValsVectors ( vals );
	//V.print("autovectores originales");
	for ( int i=0; i< rows; i++ )
	{
		if ( vals ( i ) <= 0. )
			vals ( i ) = 0.0000001;
	}
	QMat DD = QMat::makeDiagonal ( vals );
	QMat R = ( V * ( DD * V.transpose() ) );

	return R;
}

/**
 * \brief Multiplies a n-column vector times a m-row vector to generate a nxm matrix
 * @param vector vector to play the rol of row vector
 * @return resulting QMat matrix
 */
QMat QVec::externProduct(const QVec & vector) const
{
	Q_ASSERT(size() == vector.size());

	const int s = size();
	QMat C ( size(), vector.size() );
#ifdef COMPILE_IPP
	ippmMul_mm_32f ( getReadData(), s * sizeof ( T ), sizeof ( T ), s , s , vector.getReadData(), vector.size() * sizeof ( T ), sizeof ( T ), vector.size() , vector.size() , C.getWriteData(), vector.size() * sizeof ( T ), sizeof ( T ) );
#else
	for(int r=0;r<s;r++)
		for(int c=0;c<vector.size();c++)
			C(r,c) = this->operator()(r) * vector(c);
#endif
	return C;
}

void DMRGCat::SuperBlock::getReNormU(QMat& U, BlockQBase& UBase)const{
	QMat waveMat;
	GsWave.wave2QMat(PToS->QSpace, PToM->QSpace, PToN->QSpace, PToE->QSpace, waveMat);
	waveMat.getReNormUAndBase(Para->getD(), U, UBase);
}

int QMat::minDim ( const QMat &A )
{
	return qMin ( A.nRows(), A.nCols() );
}

int QMat::maxDim ( const QMat &A )
{
	return qMax ( 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);
}