Introduce rotation methods to the Vector class

By: Bjarne Koll <LynxPlay101@gmail.com>

paper-api/src/main/java/org/bukkit/util/Vector.java

@@ -1,5 +1,6 @@
 package org.bukkit.util;
 
+import com.google.common.base.Preconditions;
 import java.util.LinkedHashMap;
 import java.util.Map;
 import java.util.Random;
@@ -373,6 +374,148 @@ public class Vector implements Cloneable, ConfigurationSerializable {
         return (NumberConversions.square(origin.x - x) + NumberConversions.square(origin.y - y) + NumberConversions.square(origin.z - z)) <= NumberConversions.square(radius);
     }
 
+    /**
+     * Returns if a vector is normalized
+     *
+     * @return whether the vector is normalised
+     */
+    public boolean isNormalized() {
+        return Math.abs(this.lengthSquared() - 1) < getEpsilon();
+    }
+
+    /**
+     * Rotates the vector around the x axis.
+     * <p>
+     * This piece of math is based on the standard rotation matrix for vectors
+     * in three dimensional space. This matrix can be found here:
+     * <a href="https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations">Rotation
+     * Matrix</a>.
+     *
+     * @param angle the angle to rotate the vector about. This angle is passed
+     * in radians
+     * @return the same vector
+     */
+    public Vector rotateAroundX(double angle) {
+        double angleCos = Math.cos(angle);
+        double angleSin = Math.sin(angle);
+
+        double y = angleCos * getY() - angleSin * getZ();
+        double z = angleSin * getY() + angleCos * getZ();
+        return setY(y).setZ(z);
+    }
+
+    /**
+     * Rotates the vector around the y axis.
+     * <p>
+     * This piece of math is based on the standard rotation matrix for vectors
+     * in three dimensional space. This matrix can be found here:
+     * <a href="https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations">Rotation
+     * Matrix</a>.
+     *
+     * @param angle the angle to rotate the vector about. This angle is passed
+     * in radians
+     * @return the same vector
+     */
+    public Vector rotateAroundY(double angle) {
+        double angleCos = Math.cos(angle);
+        double angleSin = Math.sin(angle);
+
+        double x = angleCos * getX() + angleSin * getZ();
+        double z = -angleSin * getX() + angleCos * getZ();
+        return setX(x).setZ(z);
+    }
+
+    /**
+     * Rotates the vector around the z axis
+     * <p>
+     * This piece of math is based on the standard rotation matrix for vectors
+     * in three dimensional space. This matrix can be found here:
+     * <a href="https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations">Rotation
+     * Matrix</a>.
+     *
+     * @param angle the angle to rotate the vector about. This angle is passed
+     * in radians
+     * @return the same vector
+     */
+    public Vector rotateAroundZ(double angle) {
+        double angleCos = Math.cos(angle);
+        double angleSin = Math.sin(angle);
+
+        double x = angleCos * getX() - angleSin * getY();
+        double y = angleSin * getX() + angleCos * getY();
+        return setX(x).setY(y);
+    }
+
+    /**
+     * Rotates the vector around a given arbitrary axis in 3 dimensional space.
+     *
+     * <p>
+     * Rotation will follow the general Right-Hand-Rule, which means rotation
+     * will be counterclockwise when the axis is pointing towards the observer.
+     * <p>
+     * This method will always make sure the provided axis is a unit vector, to
+     * not modify the length of the vector when rotating. If you are experienced
+     * with the scaling of a non-unit axis vector, you can use
+     * {@link Vector#rotateAroundNonUnitAxis(Vector, double)}.
+     *
+     * @param axis the axis to rotate the vector around. If the passed vector is
+     * not of length 1, it gets copied and normalized before using it for the
+     * rotation. Please use {@link Vector#normalize()} on the instance before
+     * passing it to this method
+     * @param angle the angle to rotate the vector around the axis
+     * @return the same vector
+     * @throws IllegalArgumentException if the provided axis vector instance is
+     * null
+     */
+    public Vector rotateAroundAxis(Vector axis, double angle) throws IllegalArgumentException {
+        Preconditions.checkArgument(axis != null, "The provided axis vector was null");
+
+        return rotateAroundNonUnitAxis(axis.isNormalized() ? axis : axis.clone().normalize(), angle);
+    }
+
+    /**
+     * Rotates the vector around a given arbitrary axis in 3 dimensional space.
+     *
+     * <p>
+     * Rotation will follow the general Right-Hand-Rule, which means rotation
+     * will be counterclockwise when the axis is pointing towards the observer.
+     * <p>
+     * Note that the vector length will change accordingly to the axis vector
+     * length. If the provided axis is not a unit vector, the rotated vector
+     * will not have its previous length. The scaled length of the resulting
+     * vector will be related to the axis vector. If you are not perfectly sure
+     * about the scaling of the vector, use
+     * {@link Vector#rotateAroundAxis(Vector, double)}
+     *
+     * @param axis the axis to rotate the vector around.
+     * @param angle the angle to rotate the vector around the axis
+     * @return the same vector
+     * @throws IllegalArgumentException if the provided axis vector instance is
+     * null
+     */
+    public Vector rotateAroundNonUnitAxis(Vector axis, double angle) throws IllegalArgumentException {
+        Preconditions.checkArgument(axis != null, "The provided axis vector was null");
+
+        double x = getX(), y = getY(), z = getZ();
+        double x2 = axis.getX(), y2 = axis.getY(), z2 = axis.getZ();
+
+        double cosTheta = Math.cos(angle);
+        double sinTheta = Math.sin(angle);
+        double dotProduct = this.dot(axis);
+
+        double xPrime = x2 * dotProduct * (1d - cosTheta)
+                + x * cosTheta
+                + (-z2 * y + y2 * z) * sinTheta;
+        double yPrime = y2 * dotProduct * (1d - cosTheta)
+                + y * cosTheta
+                + (z2 * x - x2 * z) * sinTheta;
+        double zPrime = z2 * dotProduct * (1d - cosTheta)
+                + z * cosTheta
+                + (-y2 * x + x2 * y) * sinTheta;
+
+        return setX(xPrime).setY(yPrime).setZ(zPrime);
+    }
+
     /**
      * Gets the X component.
      *