public void AddAngles(Vector3 angles)
{
var a = MatrixMath.GetRotationMatrix(X, Y, Z);
var b = MatrixMath.GetRotationMatrix(angles.X, angles.Y, angles.Z);
decimal[,] angleMatrix = MatrixMath.Multiply3v3(b, a);
double yaw = Math.Atan2((double)angleMatrix[1, 0], (double)angleMatrix[0, 0]) / Math.PI * 180.0;
angleMatrix = MatrixMath.Multiply3v3(MatrixMath.GetZRotation(Convert.ToDecimal(-yaw)), angleMatrix);
double pitch = Math.Atan2((double)-angleMatrix[2, 0], (double)angleMatrix[0, 0]) / Math.PI * 180.0;
double roll = Math.Atan2((double)-angleMatrix[1, 2], (double)angleMatrix[1, 1]) / Math.PI * 180.0;
X = Convert.ToDecimal(pitch - Math.Floor(pitch / 360.0) * 360.0);
Y = Convert.ToDecimal(yaw - Math.Floor(yaw / 360.0) * 360.0);
Z = Convert.ToDecimal(roll - Math.Floor(roll / 360.0) * 360.0);
}
public static decimal[,] GetRotationMatrix(decimal Pitch, decimal Yaw, decimal Roll)
{
return(MatrixMath.Multiply3v3(MatrixMath.GetZRotation(Yaw), MatrixMath.Multiply3v3(MatrixMath.GetYRotation(Pitch), MatrixMath.GetXRotation(Roll))));
}