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 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);
}