public void Rotate(Vector3 angles)
{
decimal[,] rotationMatrix = MatrixMath.GetRotationMatrix(angles.Y, angles.X, angles.Z);
decimal x = X, y = Y, z = Z;
X = x * rotationMatrix[0, 0] + y * rotationMatrix[0, 1] + z * rotationMatrix[0, 2];
Y = x * rotationMatrix[1, 0] + y * rotationMatrix[1, 1] + z * rotationMatrix[1, 2];
Z = x * rotationMatrix[2, 0] + y * rotationMatrix[2, 1] + z * rotationMatrix[2, 2];
}
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);
}