/**************************************************************************************************
* Procecure *
* *
* Description: gramSchmidtNormalization *
* Class : ApproachMovementSpace *
**************************************************************************************************/
Matrix3f ApproachMovementSpace::gramSchmidtNormalization3f(Matrix3f matrix)
{
// cout << endl << "Non-Normalized Matrix: " << endl << matrix << endl;
uint matrix_size = matrix.diagonalSize();
for(uint column = 0; column < matrix_size; column += 1)
{
// Step 1: Normalize vector
matrix.col(column) = normalizeVector3f( matrix.col(column) );
// Step 2: Eliminate the projection of this vector
for(uint column_proj = column + 1; column_proj < matrix_size; column_proj += 1)
{
Vector3f vj = matrix.col(column_proj);
matrix.col(column_proj) -= vj.dot( matrix.col(column) ) * matrix.col(column);
}
}
// cout << endl << "Normalized Matrix: " << endl << matrix << endl;
return matrix;
}
Vector3f normalizeDriveForce( Vector3f driveForce )
{
driveForce[2] = 0;
return normalizeVector3f( driveForce, max_drive_force );
}
