示例#1
0
 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);
 }
示例#2
0
 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);
 }
示例#3
0
 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);
 }
示例#4
0
 public static void MatrixToPY(Matrix3D Matrix, ref Angles.AnglePY ResultPY)
 {
     var _dbl = new XYZDouble();
     VectorForwardsRotationByMatrix(Matrix, ref _dbl);
     VectorToPY(_dbl, ref ResultPY);
 }
示例#5
0
 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);
 }