public static void MatrixNormalize(Matrix3D Matrix)
{
var erpy = new Angles.AngleRPY();
MatrixToRPY(Matrix, ref erpy);
MatrixSetToRPY(Matrix, erpy);
}
public static void MatrixSetToRPY(Matrix3D Matrix, Angles.AngleRPY AngleRPY)
{
var matrix = new Matrix3D();
var matrixd2 = new Matrix3D();
var matrixd3 = new Matrix3D();
var resultMatrix = new Matrix3D();
MatrixSetToZAngle(matrix, AngleRPY.Roll);
MatrixSetToXAngle(matrixd2, AngleRPY.Pitch);
MatrixSetToYAngle(matrixd3, AngleRPY.Yaw);
MatrixRotationByMatrix(matrixd2, matrix, resultMatrix);
MatrixRotationByMatrix(matrixd3, resultMatrix, Matrix);
}
public static void MatrixToRPY(Matrix3D Matrix, ref Angles.AngleRPY ResultRPY)
{
var epy = new Angles.AnglePY();
var _dbl = new XYZDouble();
var _dbl2 = new XYZDouble();
var matrix = new Matrix3D();
var matrixd = new Matrix3D();
VectorForwardsRotationByMatrix(Matrix, ref _dbl2);
VectorToPY(_dbl2, ref epy);
ResultRPY.PY = epy;
VectorRightRotationByMatrix(Matrix, ref _dbl2);
MatrixSetToXAngle(matrixd, -epy.Pitch);
MatrixSetToYAngle(matrix, -epy.Yaw);
VectorRotationByMatrix(matrix, _dbl2, ref _dbl);
VectorRotationByMatrix(matrixd, _dbl, ref _dbl2);
ResultRPY.Roll = Math.Atan2(_dbl2.Y, _dbl2.X);
}