C++ QVec::size方法代码示例

/**
 * \brief Construct a diagonal matrix 
 *
 * Use vector values to create a new diagonal matrix
 * @param v Vector to get diagonal values
 * @return QMat new diagonal matrix
 */
QMat RMat::QMat::makeDiagonal ( const QVec &v )
{
	QMat R ( v.size(),v.size(), (T)0 );
	int f = v.size();
	for ( int i=0; i<f; i++ )
		R ( i,i ) = v ( i );
	return R;
}

/**
 * \brief Computes the coefficients of the implicit line equation: y = mx + n passing by two points
 * from two points (x1,y1) e (x2,y2) satisfying  (y-y1)/(y2-y1) = (x-x1)/(x2-x1),
 * that solving for y gives:  y = x( (y2-y1)/(x2-x1) ) - x1( (y2-y1)/(x2-x1) ) + y1
 * @param p1 containing x1,y1
 * @param p2 containing x2,y2
 * @return a QVec vector of 2 dimensions with m and n
 */
QVec RMat::QVec::line2DImplicitCoefsFrom2Points(const QVec & p1, const QVec & p2) 
{
	Q_ASSERT( p1.size() == 2 and p2.size() == 2 and p2(0)-p1(0) != 0 );
	QVec res(2);
	res(0) = (p2(1)-p1(1))/(p2(0)-p1(0)) ;
	res(1) =   p1(1) - p1(0) * ((p2(1)-p1(1))/(p2(0)-p1(0))) ;
	return res;
}

/**
 * \brief Vector to matrix constructor
 *
 * Creates a row or column matrix from vector
 * @param vector Vector to convert to matrix type
 * @param rowVector if true, creates a row matrix from the vector, else creates a column matrix from it
*/
QMat::QMat(const QVec &vector, const bool rowVector)
{
	cols = rowVector?vector.size():1;
	rows = rowVector?1:vector.size();
	
	data = new DataBuffer(cols*rows);
	
	if (rowVector)
		setRow(0, vector);
	else
		setCol(0, vector);
}

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

RMat::T RMat::QVec::distanceTo2DLine( const QVec & line ) const
{
  Q_ASSERT_X( size() == 2 and line.size() == 3, "QVec::distanceTo2DLine", "incorrect size of parameters");

   return fabs(line(0)*operator()(0) + line(1)*operator()(1) + line(2)) / sqrt(line(0)*line(0) + line(1)*line(1));

}

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

/**
 * \brief Dot product of operand with current vector
 * @param vector
 * @return dot product vector
 */
T QVec::dotProduct(const QVec &vector) const
{
	Q_ASSERT(size() == vector.size());

	double accum = 0;
	for (int i = 0; i < size(); i++)
		accum += at(i) * vector[i];
	return accum;
}

/**
 * \brief operator that subtracts a vector from the current one
 * @param vector
 * @return the difference vector
 */
QVec QVec::operator -(const QVec & vector) const
{
	Q_ASSERT( size() == vector.size() );
	QVec result(size());
	for (int i = 0; i < size(); i++)
		result[i] = at(i) - vector[i];
	return result;

}

void RMat::QMat::setRow(const int row, QVec vector)
{
	Q_ASSERT(row < nRows());
	Q_ASSERT(nCols() == vector.size());
 
	const int cols = nCols();
	for (int col= 0; col < cols; col++)
		operator()(row,col) = vector[col];
}

/**
 * \brief Element to element product
 * @param vector
 * @return product vector
 */
QVec  QVec::pointProduct(const QVec & vector) const
{
 	Q_ASSERT(size() == vector.size());

	QVec result = *this;
	for (int i = 0; i < size(); i++)
		result[i] *= vector[i];
	return result;
}

//Do it with inject
void RMat::QMat::setCol(const int col, QVec vector)
{
	Q_ASSERT(col < nCols());
	Q_ASSERT(nRows() == vector.size());
 
	const int rows = nRows();
	for (int row= 0; row < rows; row++)
		operator()(row,col) = vector[row];

}

/**
 * \brief Compares two vector for equal values element to element
 * @param vector
 * @return true if both are equal, false if not
 */
bool RMat::QVec::equals(const QVec & vector ) const
{
	if (size() != vector.size())
		return false;

	for (int i = 0; i < size(); i++)
		if (at(i) != vector[i])
			return false;

	return true;
}

/**
 * \brief Computes de crossproduct of current 3-vector and a 3-vector parameter
 * @param vector 3-vector that operates on current vector
 * @return crossproduct
 */
QVec RMat::QVec::crossProduct(const QVec & vector) const
{
	Q_ASSERT(size() == vector.size());
	Q_ASSERT(size() == 3);
	const double    x1 = at(0), y1 = at(1), z1 = at(2), x2 = vector[0], y2 = vector[1], z2 = vector[2];
	QVec v(3);
	v[0] = -y2*z1 + y1*z2;
	v[1] = x2*z1 - x1*z2;
	v[2] = -x2*y1 + x1*y2;

	return v;
}

/** REVISARRRR
 * @brief Sends the robot bakcwards on a straight line until targetT is reached.
 *
 * @param innerModel ...
 * @param target position in World Reference System
 * @return bool
 */
bool SpecificWorker::goBackwardsCommand(InnerModel *innerModel, CurrentTarget &current, CurrentTarget &currentT, TrajectoryState &state, WayPoints &myRoad)
{
	const float MAX_ADV_SPEED = 400.f;
	const float MAX_POSITIONING_ERROR = 40;  //mm
	static float errorAnt = std::numeric_limits<float>::max();
	
	///////////////////
	//CHECK PARAMETERS
	///////////////////
	QVec target = current.getTranslation();
	if (target.size() < 3 or std::isnan(target.x()) or std::isnan(target.y()) or std::isnan(target.z()))
	{
		qDebug() << __FUNCTION__ << "Returning. Invalid target";
		RoboCompTrajectoryRobot2D::RoboCompException ex;
		ex.text = "Returning. Invalid target";
		throw ex;
		return false;
	}
	
	state.setState("BACKWARDS");
	QVec rPose = innerModel->transform("world", "robot");
	float error = (rPose - target).norm2();
	bool errorIncreasing = false;
	if (error > errorAnt)
		errorIncreasing = true;
	qDebug() << __FUNCTION__ << "doing backwards" << error;

	if ((error < MAX_POSITIONING_ERROR) or (errorIncreasing == true))        //TASK IS FINISHED
	{
		controller->stopTheRobot(omnirobot_proxy);
		myRoad.requiresReplanning = true;
		currentT.setWithoutPlan(true);
		currentT.setState(CurrentTarget::State::GOTO);
		errorAnt = std::numeric_limits<float>::max();
	}
	else
	{
		float vadv = -0.5 * error;  //Proportional controller
		if (vadv < -MAX_ADV_SPEED) vadv = -MAX_ADV_SPEED;
		try
		{	omnirobot_proxy->setSpeedBase(0, vadv, 0);	} 
		catch (const Ice::Exception &ex)
		{ std::cout << ex << std::endl; }
	}
	errorAnt = error;
	return true;
}

std::tuple< bool, QString > Sampler::checkRobotValidStateAtTarget(const QVec& target) const
{
	if( target.size() != 6 )
		return std::make_tuple(false, QString("Invalid target vector. A 6D vector is required"));
	return checkRobotValidStateAtTarget(target.subVector(0,2), target.subVector(3,5));
}