public static void VectorToPY(XYZDouble Vector, ref Angles.AnglePY ResultPY)
{
ResultPY.Pitch = Math.Atan2(-Vector.Y, Math.Sqrt((Vector.X * Vector.X) + (Vector.Z * Vector.Z)));
if ( ResultPY.Pitch > 1.5707963267948966 )
{
ResultPY.Pitch = 3.1415926535897931 - ResultPY.Pitch;
}
else if ( ResultPY.Pitch < -1.5707963267948966 )
{
ResultPY.Pitch = -ResultPY.Pitch - 3.1415926535897931;
}
ResultPY.Yaw = Math.Atan2(Vector.X, Vector.Z);
}
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);
}
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 MatrixToPY(Matrix3D Matrix, ref Angles.AnglePY ResultPY)
{
var _dbl = new XYZDouble();
VectorForwardsRotationByMatrix(Matrix, ref _dbl);
VectorToPY(_dbl, ref ResultPY);
}
public static void MatrixSetToPY(Matrix3D Matrix, Angles.AnglePY AnglePY)
{
var matrix = new Matrix3D();
var matrixd3 = new Matrix3D();
MatrixSetToXAngle(matrix, AnglePY.Pitch);
MatrixSetToYAngle(matrixd3, AnglePY.Yaw);
MatrixRotationByMatrix(matrixd3, matrix, Matrix);
}
