public static void MatrixToPY(Matrix3D Matrix, ref Angles.AnglePY ResultPY)
{
var _dbl = new XYZDouble();
VectorForwardsRotationByMatrix(Matrix, ref _dbl);
VectorToPY(_dbl, ref ResultPY);
}
public static double VectorAngleToVector(XYZDouble VectorA, XYZDouble VectorB)
{
var epy = new Angles.AnglePY();
var matrix = new Matrix3D();
VectorToPY(VectorA, ref epy);
MatrixSetToPY(matrix, epy);
return(MatrixAngleToVector(matrix, VectorB));
}
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);
}
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);
}