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