C++ VectorRn::GetLength方法代码示例

// ********************************************************************************************
// Singular value decomposition.
// Return othogonal matrices U and V and diagonal matrix with diagonal w such that
//     (this) = U * Diag(w) * V^T     (V^T is V-transpose.)
// Diagonal entries have all non-zero entries before all zero entries, but are not
//		necessarily sorted.  (Someday, I will write ComputedSortedSVD that handles
//		sorting the eigenvalues by magnitude.)
// ********************************************************************************************
void MatrixRmn::ComputeSVD( MatrixRmn& U, VectorRn& w, MatrixRmn& V ) const
{
	assert ( U.NumRows==NumRows && V.NumCols==NumCols 
			 && U.NumRows==U.NumCols && V.NumRows==V.NumCols
			 && w.GetLength()==Min(NumRows,NumCols) );

//	double temp=0.0;
	VectorRn& superDiag = VectorRn::GetWorkVector( w.GetLength()-1 );		// Some extra work space.  Will get passed around.

	// Choose larger of U, V to hold intermediate results
	// If U is larger than V, use U to store intermediate results
	// Otherwise use V.  In the latter case, we form the SVD of A transpose,
	//		(which is essentially identical to the SVD of A).
	MatrixRmn* leftMatrix;
	MatrixRmn* rightMatrix;
	if ( NumRows >= NumCols ) {
		U.LoadAsSubmatrix( *this );				// Copy A into U
		leftMatrix = &U;
		rightMatrix = &V;
	}
	else {
		V.LoadAsSubmatrixTranspose( *this );		// Copy A-transpose into V
		leftMatrix = &V;
		rightMatrix = &U;
	}

	// Do the actual work to calculate the SVD 
	// Now matrix has at least as many rows as columns
	CalcBidiagonal( *leftMatrix, *rightMatrix, w, superDiag );
	ConvertBidiagToDiagonal( *leftMatrix, *rightMatrix, w, superDiag );

}

// Multiply transpose of this matrix by column vector v.
//    Result is column vector "result"
// Equivalent to mult by row vector on left
void MatrixRmn::MultiplyTranspose(const VectorRn &v, VectorRn &result) const
{
	assert(v.GetLength() == NumRows && result.GetLength() == NumCols);
	double *out = result.GetPtr(); // Points to entry in result vector
	const double *colPtr = x;	  // Points to beginning of next column in matrix
	for (long i = NumCols; i > 0; i--) {
		const double *in = v.GetPtr();
		*out = 0.0f;
		for (long j = NumRows; j > 0; j--) {
			*out += (*(in++)) * (*(colPtr++));
		}
		out++;
	}
}

// Solves the equation   (*this)*xVec = b;
// Uses row operations.  Assumes *this is square and invertible.
// No error checking for divide by zero or instability (except with asserts)
void MatrixRmn::Solve(const VectorRn &b, VectorRn *xVec) const
{
	assert(NumRows == NumCols && NumCols == xVec->GetLength() && NumRows == b.GetLength());

	// Copy this matrix and b into an Augmented Matrix
	MatrixRmn &AugMat = GetWorkMatrix(NumRows, NumCols + 1);
	AugMat.LoadAsSubmatrix(*this);
	AugMat.SetColumn(NumRows, b);

	// Put into row echelon form with row operations
	AugMat.ConvertToRefNoFree();

	// Solve for x vector values using back substitution
	double *xLast = xVec->x + NumRows - 1;			   // Last entry in xVec
	double *endRow = AugMat.x + NumRows * NumCols - 1; // Last entry in the current row of the coefficient part of Augmented Matrix
	double *bPtr = endRow + NumRows;				   // Last entry in augmented matrix (end of last column, in augmented part)
	for (long i = NumRows; i > 0; i--) {
		double accum = *(bPtr--);
		// Next loop computes back substitution terms
		double *rowPtr = endRow; // Points to entries of the current row for back substitution.
		double *xPtr = xLast;	// Points to entries in the x vector (also for back substitution)
		for (long j = NumRows - i; j > 0; j--) {
			accum -= (*rowPtr) * (*(xPtr--));
			rowPtr -= NumCols; // Previous entry in the row
		}
		assert(*rowPtr != 0.0); // Are not supposed to be any free variables in this matrix
		*xPtr = accum / (*rowPtr);
		endRow--;
	}
}

// Multiply this matrix by column vector v.
// Result is column vector "result"
void MatrixRmn::Multiply(const VectorRn &v, VectorRn &result) const
{
	assert(v.GetLength() == NumCols && result.GetLength() == NumRows);
	double *out = result.GetPtr(); // Points to entry in result vector
	const double *rowPtr = x;	  // Points to beginning of next row in matrix
	for (long j = NumRows; j > 0; j--) {
		const double *in = v.GetPtr();
		const double *m = rowPtr++;
		*out = 0.0f;
		for (long i = NumCols; i > 0; i--) {
			*out += (*(in++)) * (*m);
			m += NumRows;
		}
		out++;
	}
}

// Set the i-th column equal to d.
void MatrixRmn::SetColumn(long i, const VectorRn &d)
{
	assert(NumRows == d.GetLength());
	double *to = x + i * NumRows;
	const double *from = d.x;
	for (i = NumRows; i > 0; i--) {
		*(to++) = *(from++);
	}
}

// Set the i-th column equal to d.
void MatrixRmn::SetRow(long i, const VectorRn &d)
{
	assert(NumCols == d.GetLength());
	double *to = x + i;
	const double *from = d.x;
	for (i = NumRows; i > 0; i--) {
		*to = *(from++);
		to += NumRows;
	}
}

// Form the dot product of a vector v with the i-th column of the array
double MatrixRmn::DotProductColumn(const VectorRn &v, long colNum) const
{
	assert(v.GetLength() == NumRows);
	double *ptrC = x + colNum * NumRows;
	double *ptrV = v.x;
	double ret = 0.0;
	for (long i = NumRows; i > 0; i--) {
		ret += (*(ptrC++)) * (*(ptrV++));
	}
	return ret;
}

void Jacobian::CalcDeltaThetasPseudoinverse()
{
	MatrixRmn &J = const_cast<MatrixRmn &>(Jend);

	// costruisco matrice J1
	MatrixRmn J1;
	J1.SetSize(2, J.getNumColumns());

	for (int i = 0; i < 2; i++)
		for (int j = 0; j < J.getNumColumns(); j++)
			J1.Set(i, j, J.Get(i, j));

	// COSTRUISCO VETTORI ds1 e ds2
	VectorRn dS1(2);

	for (int i = 0; i < 2; i++)
		dS1.Set(i, dS.Get(i));

	// calcolo dtheta1
	MatrixRmn U, V;
	VectorRn w;

	U.SetSize(J1.getNumRows(), J1.getNumRows());
	w.SetLength(min(J1.getNumRows(), J1.getNumColumns()));
	V.SetSize(J1.getNumColumns(), J1.getNumColumns());

	J1.ComputeSVD(U, w, V);

	// Next line for debugging only
	assert(J1.DebugCheckSVD(U, w, V));

	// Calculate response vector dTheta that is the DLS solution.
	//	Delta target values are the dS values
	//  We multiply by Moore-Penrose pseudo-inverse of the J matrix

	double pseudoInverseThreshold = PseudoInverseThresholdFactor * w.MaxAbs();

	long diagLength = w.GetLength();
	double *wPtr = w.GetPtr();
	dTheta.SetZero();
	for (long i = 0; i < diagLength; i++) {
		double dotProdCol = U.DotProductColumn(dS1, i); // Dot product with i-th column of U
		double alpha = *(wPtr++);
		if (fabs(alpha) > pseudoInverseThreshold) {
			alpha = 1.0 / alpha;
			MatrixRmn::AddArrayScale(V.getNumRows(), V.GetColumnPtr(i), 1, dTheta.GetPtr(), 1, dotProdCol * alpha);
		}
	}

	MatrixRmn JcurrentPinv(V.getNumRows(), J1.getNumRows());		   // pseudoinversa di J1
	MatrixRmn JProjPre(JcurrentPinv.getNumRows(), J1.getNumColumns()); // Proiezione di J1
	if (skeleton->getNumEffector() > 1) {
		// calcolo la pseudoinversa di J1
		MatrixRmn VD(V.getNumRows(), J1.getNumRows()); // matrice del prodotto V*w

		double *wPtr = w.GetPtr();
		pseudoInverseThreshold = PseudoInverseThresholdFactor * w.MaxAbs();
		for (int j = 0; j < VD.getNumColumns(); j++) {
			double *VPtr = V.GetColumnPtr(j);
			double alpha = *(wPtr++); // elemento matrice diagonale
			for (int i = 0; i < V.getNumRows(); i++) {
				if (fabs(alpha) > pseudoInverseThreshold) {
					double entry = *(VPtr++);
					VD.Set(i, j, entry * (1.0 / alpha));
				}
			}
		}
		MatrixRmn::MultiplyTranspose(VD, U, JcurrentPinv);

		// calcolo la proiezione J1
		MatrixRmn::Multiply(JcurrentPinv, J1, JProjPre);

		for (int j = 0; j < JProjPre.getNumColumns(); j++)
			for (int i = 0; i < JProjPre.getNumRows(); i++) {
				double temp = JProjPre.Get(i, j);
				JProjPre.Set(i, j, -1.0 * temp);
			}
		JProjPre.AddToDiagonal(diagMatIdentity);
	}

	//task priority strategy
	for (int i = 1; i < skeleton->getNumEffector(); i++) {
		// costruisco matrice Jcurrent (Ji)
		MatrixRmn Jcurrent(2, J.getNumColumns());
		for (int j = 0; j < J.getNumColumns(); j++)
			for (int k = 0; k < 2; k++)
				Jcurrent.Set(k, j, J.Get(k + 2 * i, j));

		// costruisco il vettore dScurrent
		VectorRn dScurrent(2);
		for (int k = 0; k < 2; k++)
			dScurrent.Set(k, dS.Get(k + 2 * i));

		// Moltiplico Jcurrent per la proiezione di J(i-1)
		MatrixRmn Jdst(Jcurrent.getNumRows(), JProjPre.getNumColumns());
		MatrixRmn::Multiply(Jcurrent, JProjPre, Jdst);

		// Calcolo la pseudoinversa di Jdst
		MatrixRmn UU(Jdst.getNumRows(), Jdst.getNumRows()), VV(Jdst.getNumColumns(), Jdst.getNumColumns());
		VectorRn ww(min(Jdst.getNumRows(), Jdst.getNumColumns()));
//.........这里部分代码省略.........