result = lhs + rhs + * @param rhs The vector addend. + * @return Sum of two vectors. + */ + S plus(S rhs); + + /** + * Perform subtraction on the lhs implicit vector with the rhs explicit vector. + *
result = lhs - rhs + * @param rhs The vector subtractend. + * @return Difference of two vectors. + */ + S minus(S rhs); + + /** + * Perform scalar multiplication to the implicit vector. + *
result = scalar * vector + * @apiNote This method should be used to divide by scaling the vector by 1 / divisor. + * User will need to check for zero division. + * @param scalar Coefficient to scale vector by. + * @return The scaled vector. + */ + S scalarMultiply(double scalar); + + /** + * Same as scalar multiplication by -1 to the implicit vector. + *
result = -1 * vector + * @return A vector pointing in the opposite geometric direction with same magnitude. + */ + S negate(); + + /** + * The geometric/euclidean norm of the vector. + * @return The geometric length of the vector. + */ + double magnitude(); + + /** + * The geometric/euclidean norm of the vector squared. + * @implNote Should be implemented without squaring the magnitude to optimize execution. + *
Ex: Cartesian2D magnitude squared = x * x + y * y. + * @return The geometric length of the vector squared. + */ + double magnitudeSquared(); + + /** + * Computes a normalized geometric vector (magnitude 1). + * @return Normalizes a vector. + */ + S unitVector(); + + /** + * Computes the dot/scalar product of an implicit and explicit geometric vector. + *
result = lhs * rhs + * @param rhs A second vector. + * @return The dot product of lhs implicit and rhs explicit. + */ + double dot(S rhs); + + /** + * The projection of implicit vector onto explicit vector. + *
result = proj(implicit) onto explicit
+ * @param targetVec Vector to project onto.
+ * @return The resulting projection.
+ */
+ S projectOnto(S targetVec);
+
+ /**
+ * The signed angle made between the vector and the positive x-axis
+ * @return Angle in radians
+ */
+ double azimuthalAngle();
+}
diff --git a/lib/src/main/java/com/titanrobotics2022/geometry/CoordinateSystem.java b/lib/src/main/java/com/titanrobotics2022/geometry/CoordinateSystem.java
new file mode 100644
index 0000000..af5c674
--- /dev/null
+++ b/lib/src/main/java/com/titanrobotics2022/geometry/CoordinateSystem.java
@@ -0,0 +1,13 @@
+package com.titanrobotics2022.geometry;
+
+/**
+ * A generic geometric coordinate system that represents a vector space.
+ */
+public interface CoordinateSystem {
+
+ /**
+ * Gets the number of dimensions in the space.
+ * @return The number of dimensions in the space.
+ */
+ int getDimension();
+}
diff --git a/lib/src/main/java/com/titanrobotics2022/geometry/Vector2DOperations.java b/lib/src/main/java/com/titanrobotics2022/geometry/Vector2DOperations.java
new file mode 100644
index 0000000..48375f4
--- /dev/null
+++ b/lib/src/main/java/com/titanrobotics2022/geometry/Vector2DOperations.java
@@ -0,0 +1,21 @@
+package com.titanrobotics2022.geometry;
+
+/**
+ * Represents 2-Dimensional general operations that can be performed on all 2-Dimensional geometric vectors.
+ * @param Type of the 2-Dimensional coordinate system that the vector is defined by.
+ * @implNote Does not check if the coordinate system contains 2 dimensions.
+ * @implNote All operations can be optimized for different vector spaces in implementing classes.
+ */
+public interface Vector2DOperations extends AbstractVectorOperations {
+
+ /**
+ * The cross product of two vectors.
+ * In two dimensions such a cross product has no z dimension which means that
+ * lhs and rhs vectors are coplanar so resulting cross product only contains the z dimension.
+ *
result = lhs X rhs
+ * @apiNote Useful for computing surface normals found in graphics computation.
+ * @param rhs The rhs vector of the cross product.
+ * @return The z component of the cross product (can be negative).
+ */
+ public double cross(S rhs);
+}
diff --git a/lib/src/main/java/com/titanrobotics2022/geometry/Vector3DOperations.java b/lib/src/main/java/com/titanrobotics2022/geometry/Vector3DOperations.java
new file mode 100644
index 0000000..a399e17
--- /dev/null
+++ b/lib/src/main/java/com/titanrobotics2022/geometry/Vector3DOperations.java
@@ -0,0 +1,18 @@
+package com.titanrobotics2022.geometry;
+
+/**
+ * Represents 3-Dimensional general operations that can be performed on all 3-Dimensional geometric vectors.
+ * @param Type of the 3-Dimensional coordinate system that the vector is defined by.
+ * @implNote Does not check if the coordinate system contains 3 dimensions.
+ * @implNote All operations can be optimized for different vector spaces in implementing classes.
+ */
+public interface Vector3DOperations extends AbstractVectorOperations {
+
+ /**
+ * The cross product of two vectors.
+ *
result = lhs X rhs
+ * @param rhs The rhs vector of the cross product.
+ * @return A 3-Dimensional vector.
+ */
+ public S cross(S rhs);
+}
diff --git a/lib/src/main/java/com/titanrobotics2022/geometry/geometry2d/Vector2D.java b/lib/src/main/java/com/titanrobotics2022/geometry/geometry2d/Vector2D.java
deleted file mode 100644
index 91ee57a..0000000
--- a/lib/src/main/java/com/titanrobotics2022/geometry/geometry2d/Vector2D.java
+++ /dev/null
@@ -1,207 +0,0 @@
-package com.titanrobotics2022.geometry.geometry2d;
-
-import org.apache.commons.math3.util.FastMath;
-
-/**
- * A vector 2D class
- */
-public class Vector2D {
- /** Origin (coordinates: 0, 0). */
- public static final Vector2D ZERO = new Vector2D(0, 0);
-
- /** A vector with all coordinates set to NaN. */
- public static final Vector2D NAN = new Vector2D(Double.NaN, Double.NaN);
-
- /** A vector with all coordinates set to positive infinity. */
- public static final Vector2D POSITIVE_INFINITY = new Vector2D(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY);
-
- /** A vector with all coordinates set to negative infinity. */
- public static final Vector2D NEGATIVE_INFINITY = new Vector2D(Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY);
-
- /** The x component. */
- public final double x;
-
- /** The y componenet. */
- public final double y;
-
- /**
- * Creates a cartesian vector.
- * @param x component
- * @param y component
- */
- public Vector2D(double x, double y)
- {
- this.x = x;
- this.y = y;
- }
-
- /**
- * Adds two vectors together
- * @param rhs the vector to be added
- * @return the resulting vector
- */
- public Vector2D plus(Vector2D rhs)
- {
- return new Vector2D(this.x + rhs.x, this.y + rhs.y);
- }
-
- /**
- * Subtracts two vectors
- * @param rhs rhs the vector to be subtracted
- * @return the resulting vector
- */
- public Vector2D minus(Vector2D rhs)
- {
- return new Vector2D(this.x - rhs.x, this.y - rhs.y);
- }
-
- /**
- *
- * @param scalar
- * @return
- */
- public Vector2D scalarMultiply(double scalar)
- {
- return new Vector2D(this.x * scalar, this.y * scalar);
- }
-
- /**
- *
- * @param divisor
- * @return
- */
- public Vector2D scalarDivide(double divisor)
- {
- return new Vector2D(this.x / divisor, this.y / divisor);
- }
-
- /**
- *
- * @return
- */
- public Vector2D negate()
- {
- return new Vector2D(-x, -y);
- }
-
- /**
- *
- * @return
- */
- public double magnitude()
- {
- return FastMath.sqrt(x * x + y * y);
- }
-
- /**
- *
- * @return
- */
- public double magnitudeSquared()
- {
- return x * x + y * y;
- }
-
-
- /**
- * The azimuthal angle is the angle from the x axis in the x-y plane (phi in physics, theta in math)
- * @return
- */
- public double azimuthalAngle()
- {
- double answer = FastMath.atan2(y, x);
- if (answer < 0)
- answer += 2 * Math.PI;
- return answer;
- }
-
- /**
- *
- * @return
- */
- public Vector2D unitVector()
- {
- double mag2 = magnitudeSquared();
- if (mag2 == 0)
- {
- return ZERO;
- }
- else
- {
- return scalarDivide(FastMath.sqrt(mag2));
- }
- }
-
- /**
- *
- * @param rhs
- * @return
- */
- public double dot(Vector2D rhs)
- {
- return x * rhs.x + y * rhs.y;
- }
-
- /**
- *
- * @param rhs
- * @return
- */
- public double cross(Vector2D rhs)
- {
- return x * rhs.y - rhs.x * y;
- }
-
- /**
- *
- * @param otherVec
- * @return
- */
- public Vector2D projectOnto(Vector2D otherVec)
- {
- Vector2D unitVec = otherVec.unitVector();
- return unitVec.scalarMultiply(dot(unitVec));
- }
-
- /**
- *
- * @param v1
- * @param v2
- * @return
- */
- public static double angleBetween(Vector2D v1, Vector2D v2)
- {
- return FastMath.acos(v1.dot(v2) / (v1.magnitude() * v2.magnitude()));
- }
-
- public static Vector2D polarVector(double r, double phi)
- {
- double x = r * FastMath.cos(phi);
- double y = r * FastMath.sin(phi);
- return new Vector2D(x, y);
- }
-
- /**
- *
- */
- @Override
- public boolean equals(Object obj)
- {
- if (obj instanceof Vector2D) {
- final Vector2D rhs = (Vector2D) obj;
- return (x == rhs.x) && (y == rhs.y);
- }
- return false;
- }
-
- /**
- *
- * @param rhs
- * @param tolerance
- * @return
- */
- public boolean equals(Vector2D rhs, double tolerance)
- {
- return FastMath.abs(x - rhs.x) < tolerance && FastMath.abs(y - rhs.y) < tolerance;
- }
-}
diff --git a/lib/src/main/java/com/titanrobotics2022/geometry/geometry2d/VectorCartesian2D.java b/lib/src/main/java/com/titanrobotics2022/geometry/geometry2d/VectorCartesian2D.java
new file mode 100644
index 0000000..8a6f666
--- /dev/null
+++ b/lib/src/main/java/com/titanrobotics2022/geometry/geometry2d/VectorCartesian2D.java
@@ -0,0 +1,269 @@
+package com.titanrobotics2022.geometry.geometry2d;
+
+import com.titanrobotics2022.geometry.CoordinateSystem;
+import com.titanrobotics2022.geometry.Vector2DOperations;
+
+import org.apache.commons.math3.util.FastMath;
+
+/**
+ * A cartesian 2D geometric vector of the ordered coordinates (x,y).
+ */
+public class VectorCartesian2D implements Vector2DOperations result = sum(ai * bi) from i to 2, i = 1
+ */
+ @Override
+ public double dot(VectorCartesian2D rhs) {
+ return x * rhs.x + y * rhs.y;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public VectorCartesian2D projectOnto(VectorCartesian2D otherVec) {
+ VectorCartesian2D unitVec = otherVec.unitVector();
+ return unitVec.scalarMultiply(dot(unitVec));
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public double azimuthalAngle() {
+ double answer = FastMath.atan2(y, x);
+ if (answer < 0) // Fixes round off error near 0
+ answer += 2 * Math.PI;
+ return answer;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean isNaN() {
+ return Double.isNaN(x) || Double.isNaN(y);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean isInfinite() {
+ return Double.isInfinite(x) || Double.isInfinite(y);
+ }
+
+ /**
+ * {@inheritDoc}
+ * The coordinate system is Cartesian 2D (x,y).
+ */
+ @Override
+ public CoordinateSystem getSpace() {
+ return coordinates;
+ }
+
+ /**
+ * {@inheritDoc}
+ * @implNote Tolerance is |lhs - rhs| < tolerance element wise.
+ */
+ @Override
+ public boolean equals(VectorCartesian2D rhs, double tolerance) {
+ return FastMath.abs(x - rhs.x) < tolerance && FastMath.abs(y - rhs.y) < tolerance;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public double cross(VectorCartesian2D rhs) {
+ return x * rhs.y - y * rhs.x;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public int getDimension() {
+ return coordinates.getDimension();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean equals(Object obj)
+ {
+ if (obj instanceof VectorCartesian2D)
+ {
+ final VectorCartesian2D rhs = (VectorCartesian2D) obj;
+ return (x == rhs.x) && (y == rhs.y);
+ }
+ return false;
+ }
+
+ private final static double delta = 1e-15;
+
+ /**
+ * Converts a Cartesian 2D vector into a Polar vector (r, theta (radians)).
+ * @return The Polar representation of the Cartesian 2D vector.
+ * If the Cartesian 2D vector is (0,0), then the returning
+ * vector will point along the Polar pole (theta = 0 radians).
+ */
+ public VectorPolar toPolar()
+ {
+ double rsquared = magnitudeSquared();
+ if(rsquared == 0)
+ return VectorPolar.ZERO;
+ else
+ {
+ double theta = FastMath.atan2(y, x);
+ if(theta < -delta)
+ theta += 2 * Math.PI;
+ else if(theta < delta) // Sets theta to be 0 exactly, theta was previous evaluated to be greater than negative delta
+ theta = 0;
+ return new VectorPolar(FastMath.sqrt(rsquared), theta);
+ }
+ }
+
+ /**
+ * Computes the shortest angle between two vectors.
+ * @param v1 Vector 1.
+ * @param v2 Vector 2.
+ * @return Angle in radians.
+ */
+ public static double shortestAngleBetween(VectorCartesian2D v1, VectorCartesian2D v2)
+ {
+ double cosAngle = v1.dot(v2) / (v1.magnitude() * v2.magnitude());
+
+ // result can exceed 1 due to roundoff error for nearly identical vectors
+ if (cosAngle > 1)
+ return 0;
+ else if (cosAngle < -1)
+ return Math.PI;
+ else
+ return FastMath.acos(cosAngle);
+ }
+
+ /**
+ * A singleton class representing the Cartesian 2D coordinate system.
+ * Coordinates are (x,y)
+ * x is the abscissa
+ * y is the ordinate
+ */
+ public static final class Cartesian2D implements CoordinateSystem
+ {
+ private final static Cartesian2D instance = new Cartesian2D();
+
+ private Cartesian2D(){}
+
+ public static Cartesian2D getInstance()
+ {
+ return instance;
+ }
+
+ @Override
+ public int getDimension() {
+ return 2;
+ }
+ }
+}
diff --git a/lib/src/main/java/com/titanrobotics2022/geometry/geometry2d/VectorPolar.java b/lib/src/main/java/com/titanrobotics2022/geometry/geometry2d/VectorPolar.java
new file mode 100644
index 0000000..096e336
--- /dev/null
+++ b/lib/src/main/java/com/titanrobotics2022/geometry/geometry2d/VectorPolar.java
@@ -0,0 +1,265 @@
+package com.titanrobotics2022.geometry.geometry2d;
+
+import com.titanrobotics2022.geometry.CoordinateSystem;
+import com.titanrobotics2022.geometry.Vector2DOperations;
+
+import org.apache.commons.math3.util.FastMath;
+
+/**
+ * A Polar geometric vector of the ordered coordinates (r, theta (radians)).
+ * The pole is at 0 theta (radians).
+ * This Polar vector is bijective (injective and surjective) such that there is a unique
+ * Polar vector for each point in 2D space.
+ */
+public class VectorPolar implements Vector2DOperations Precondition: radius >= 0
+ * Precondition: theta: [0, 2PI)
+ * @param r The radius.
+ * @param theta Polar angle counter clockwise from the pole.
+ */
+ public VectorPolar(double r, double theta)
+ {
+ this.r = r;
+ this.theta = theta;
+ this.lhsCartesian = toCartesian2D();
+ }
+
+ // Can be used to speed up initialization of vectors for lhsCartesian
+ // private VectorPolar(double r, double theta, VectorCartesian2D lhsCartesian)
+ // {
+ // this.r = r;
+ // this.theta = theta;
+ // this.lhsCartesian = lhsCartesian;
+ // }
+
+ /**
+ * {@inheritDoc}
+ * Polar coordinates have no origin so (0,0) is a useful origin along the pole.
+ * @return The zero vector (0,0).
+ */
+ @Override
+ public VectorPolar getZero() {
+ return ZERO;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public VectorPolar plus(VectorPolar rhs) {
+ VectorCartesian2D rhsCartesian = rhs.lhsCartesian;
+ return this.lhsCartesian.plus(rhsCartesian).toPolar();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public VectorPolar minus(VectorPolar rhs) {
+ VectorCartesian2D rhsCartesian = rhs.lhsCartesian;
+ return this.lhsCartesian.minus(rhsCartesian).toPolar();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public VectorPolar scalarMultiply(double scalar) {
+ return new VectorPolar(r * scalar, theta);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public VectorPolar negate() {
+ return new VectorPolar(r, (theta + Math.PI) % (2 * Math.PI));// Does not change r
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public double magnitude() {
+ return FastMath.abs(r);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public double magnitudeSquared() {
+ return r * r;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public VectorPolar unitVector() {
+ if (r == 0)
+ return ZERO;
+ else
+ return new VectorPolar(FastMath.copySign(1, r), theta);
+ }
+
+ /**
+ * {@inheritDoc}
+ * Polar coordinates are non-linear so the dot product is the dot product by its cartesian representation.
+ */
+ @Override
+ public double dot(VectorPolar rhs) {
+ VectorCartesian2D rhsCartesian = rhs.lhsCartesian;
+ return this.lhsCartesian.dot(rhsCartesian);
+ }
+
+ /**
+ * {@inheritDoc}
+ * Polar coordinates are non-linear so projections are geometric projections.
+ */
+ @Override
+ public VectorPolar projectOnto(VectorPolar targetVec) {
+ VectorCartesian2D rhsCartesian = targetVec.lhsCartesian;
+ return this.lhsCartesian.projectOnto(rhsCartesian).toPolar();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public double azimuthalAngle() {
+ return this.lhsCartesian.azimuthalAngle();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean isNaN() {
+ return Double.isNaN(r) || Double.isNaN(theta);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean isInfinite() {
+ return Double.isInfinite(r) || Double.isInfinite(theta);
+ }
+
+ /**
+ * {@inheritDoc}
+ * The coordinate system is Polar with pole at 0 theta.
+ * Theta is in radians and r >= 0: (r, theta).
+ */
+ @Override
+ public CoordinateSystem getSpace() {
+ return coordinates;
+ }
+
+ /**
+ * {@inheritDoc}
+ * @implNote Tolerance is from cartesian representation: |lhs - rhs| < tolerance element wise.
+ */
+ @Override
+ public boolean equals(VectorPolar rhs, double tolerance)
+ {
+ VectorCartesian2D rhsCartesian = rhs.lhsCartesian;
+ return lhsCartesian.equals(rhsCartesian, tolerance);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public double cross(VectorPolar rhs) {
+ VectorCartesian2D rhsCartesian = rhs.lhsCartesian;
+ return this.lhsCartesian.cross(rhsCartesian);
+ }
+
+ /**
+ * Converts a Polar vector into a Cartesian 2D vector (x,y).
+ * @return The Cartesian representation of the Polar vector.
+ */
+ public VectorCartesian2D toCartesian2D()
+ {
+ if(r == 0)
+ return VectorCartesian2D.ZERO;
+ else
+ {
+ double x = r * FastMath.cos(theta);
+ double y = r * FastMath.sin(theta);
+ return new VectorCartesian2D(x, y);
+ }
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public int getDimension() {
+ return coordinates.getDimension();
+ }
+
+ /**
+ * A singleton class representing the Polar coordinate system.
+ * Coordinates are (r, theta).
+ * The pole is at 0 theta.
+ * r is the radius, Domain: [0, inf).
+ * theta is the polar angle, Domain: [0, 2PI).
+ */
+ public static final class Polar implements CoordinateSystem
+ {
+ private final static Polar instance = new Polar();
+
+ private Polar(){}
+
+ public static Polar getInstance()
+ {
+ return instance;
+ }
+
+ @Override
+ public int getDimension() {
+ return 2;
+ }
+ }
+}
diff --git a/lib/src/main/java/com/titanrobotics2022/geometry/geometry3d/Vector3D.java b/lib/src/main/java/com/titanrobotics2022/geometry/geometry3d/Vector3D.java
deleted file mode 100644
index 30ee4ca..0000000
--- a/lib/src/main/java/com/titanrobotics2022/geometry/geometry3d/Vector3D.java
+++ /dev/null
@@ -1,31 +0,0 @@
-package com.titanrobotics2022.geometry.geometry3d;
-
-/**
- * A vector 3D class
- */
-public class Vector3D {
- public final double x, y, z;
-
- /**
- * Creates a 3D cartesian vector
- * @param x
- * @param y
- * @param z
- */
- public Vector3D(double x, double y, double z)
- {
- this.x = x;
- this.y = y;
- this.z = z;
- }
-
- /**
- * Adds two vectors together
- * @param rhs the vector to be added
- * @return the resulting vector
- */
- public Vector3D plus(Vector3D rhs)
- {
- return new Vector3D(this.x + rhs.x, this.y + rhs.y, this.z + rhs.z);
- }
-}
diff --git a/lib/src/main/java/com/titanrobotics2022/geometry/geometry3d/VectorCartesian3D.java b/lib/src/main/java/com/titanrobotics2022/geometry/geometry3d/VectorCartesian3D.java
new file mode 100644
index 0000000..9ff2697
--- /dev/null
+++ b/lib/src/main/java/com/titanrobotics2022/geometry/geometry3d/VectorCartesian3D.java
@@ -0,0 +1,316 @@
+package com.titanrobotics2022.geometry.geometry3d;
+
+import com.titanrobotics2022.geometry.CoordinateSystem;
+import com.titanrobotics2022.geometry.Vector3DOperations;
+
+import org.apache.commons.math3.util.FastMath;
+
+/**
+ * A cartesian 3D geometric vector of the ordered coordinates (x,y,z).
+ */
+public class VectorCartesian3D implements Vector3DOperations result = sum(ai * bi) from i to 3, i = 1
+ */
+ @Override
+ public double dot(VectorCartesian3D rhs) {
+ return this.x * rhs.x + this.y * rhs.y + this.z * rhs.z;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public VectorCartesian3D projectOnto(VectorCartesian3D targetVec) {
+ VectorCartesian3D unitVec = targetVec.unitVector();
+ return unitVec.scalarMultiply(dot(unitVec));
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public double azimuthalAngle() {
+ double answer = FastMath.atan2(y, x);
+ if (answer < 0)
+ answer += 2 * Math.PI;
+ return answer;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean isNaN() {
+ return Double.isNaN(x) || Double.isNaN(y) || Double.isNaN(z);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean isInfinite() {
+ return Double.isInfinite(x) || Double.isInfinite(y) || Double.isInfinite(z);
+ }
+
+ /**
+ * {@inheritDoc}
+ * The coordinate system is Cartesian 3D (x,y,z).
+ */
+ @Override
+ public CoordinateSystem getSpace() {
+ return coordinates;
+ }
+
+ /**
+ * {@inheritDoc}
+ * @implNote Tolerance is |lhs - rhs| < tolerance element wise.
+ */
+ @Override
+ public boolean equals(VectorCartesian3D rhs, double tolerance) {
+ return FastMath.abs(x - rhs.x) < tolerance
+ && FastMath.abs(y - rhs.y) < tolerance
+ && FastMath.abs(z - rhs.z) < tolerance;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public VectorCartesian3D cross(VectorCartesian3D rhs) {
+ return new VectorCartesian3D(this.y * rhs.z - this.z * rhs.y
+ ,this.z * rhs.x - this.x * rhs.z
+ ,this.x * rhs.y - this.y * rhs.x);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public int getDimension() {
+ return coordinates.getDimension();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean equals(Object obj)
+ {
+ if (obj instanceof VectorCartesian3D)
+ {
+ final VectorCartesian3D rhs = (VectorCartesian3D) obj;
+ return (x == rhs.x) && (y == rhs.y) && (z == rhs.z);
+ }
+ return false;
+ }
+
+ private final static double delta = 1e-15;
+
+ /**
+ * Converts a Cartesian 3D vector into a Cylindrical vector (r, theta (radians), z).
+ * @return The Cylindrical representation of the Cartesian 3D vector.
+ * If the Cartesian 3D vector is (0,0,0), then the returning
+ * vector will point along the Polar subvector pole (theta = 0 radians).
+ * @see https://tutorial.math.lamar.edu/Classes/CalcII/3DCoords.aspx
+ */
+ public VectorCylindrical toCylindrical()
+ {
+ // Same implementation as polar but with added z component
+ double rsquaredForTheta = x * x + y * y;
+ if(rsquaredForTheta == 0)
+ return VectorCylindrical.ZERO;
+ else
+ {
+ double theta = FastMath.atan2(y, x);
+ if(theta < -delta)
+ theta += 2 * Math.PI;
+ else if(theta < delta) // Sets theta to be 0 exactly, theta was previous evaluated to be greater than negative delta
+ theta = 0;
+ return new VectorCylindrical(FastMath.sqrt(rsquaredForTheta), theta, z);
+ }
+ }
+
+ /**
+ * Converts a Cartesian 3D vector into a Spherical vector (rho, theta (radians), phi (radians)).
+ * @return The Spherical representation of the Cartesian 3D vector.
+ * If the Cartesian 3D vector is (0,0,0), then the returning
+ * vector will point along the Polar subvector pole (theta = 0 radians).
+ */
+ public VectorSpherical toSpherical()
+ {
+ double rhosquared = magnitudeSquared();
+ if(rhosquared == 0)
+ return VectorSpherical.ZERO;
+ else
+ {
+ double theta;
+ if(x * x + y * y < delta)
+ theta = 0;
+ else
+ {
+ theta = FastMath.atan2(y, x);
+ if(theta < -delta)
+ theta += 2 * Math.PI;
+ else if(theta < delta) // Sets theta to be 0 exactly, theta was previous evaluated to be greater than negative delta
+ theta = 0;
+ }
+
+ double rhoMag = FastMath.sqrt(rhosquared);
+ double phi = FastMath.acos(z / rhoMag);
+ return new VectorSpherical(rhoMag, theta, phi);
+ }
+ }
+
+ /**
+ * Computes the shortest angle between two vectors.
+ * @param v1 Vector 1.
+ * @param v2 Vector 2.
+ * @return Angle in radians.
+ */
+ public static double shortestAngleBetween(VectorCartesian3D v1, VectorCartesian3D v2)
+ {
+ double cosAngle = v1.dot(v2) / (v1.magnitude() * v2.magnitude());
+
+ // result can exceed 1 due to roundoff error for nearly identical vectors
+ if (cosAngle > 1)
+ return 0;
+ else if (cosAngle < -1)
+ return Math.PI;
+ else
+ return FastMath.acos(cosAngle);
+ }
+
+ /**
+ * A singleton class representing the Cartesian 3D coordinate system.
+ * Coordinates are (x,y,z)
+ * x is the abscissa
+ * y is the ordinate
+ * z is the applicate
+ */
+ public static final class Cartesian3D implements CoordinateSystem
+ {
+ private final static Cartesian3D instance = new Cartesian3D();
+
+ private Cartesian3D(){}
+
+ public static Cartesian3D getInstance()
+ {
+ return instance;
+ }
+
+ @Override
+ public int getDimension() {
+ return 3;
+ }
+ }
+}
diff --git a/lib/src/main/java/com/titanrobotics2022/geometry/geometry3d/VectorCylindrical.java b/lib/src/main/java/com/titanrobotics2022/geometry/geometry3d/VectorCylindrical.java
new file mode 100644
index 0000000..73cd01d
--- /dev/null
+++ b/lib/src/main/java/com/titanrobotics2022/geometry/geometry3d/VectorCylindrical.java
@@ -0,0 +1,274 @@
+package com.titanrobotics2022.geometry.geometry3d;
+
+import com.titanrobotics2022.geometry.CoordinateSystem;
+import com.titanrobotics2022.geometry.Vector3DOperations;
+
+import org.apache.commons.math3.util.FastMath;
+
+/**
+ * A Cylindrical geometric vector of the ordered coordinates (r, theta (radians), z).
+ * The pole is at 0 theta (radians).
+ * This Cylindrical vector is bijective (injective and surjective) such that there is a unique
+ * Cylindrical vector for each point in 3D space.
+ * @see https://tutorial.math.lamar.edu/Classes/CalcII/CylindricalCoords.aspx
+ */
+public class VectorCylindrical implements Vector3DOperations Precondition: radius >= 0
+ * Precondition: theta: [0, 2PI)
+ * @param r The radius.
+ * @param theta Polar angle counter clockwise from the pole.
+ * @param z The vertical direction variable.
+ */
+ public VectorCylindrical(double r, double theta, double z)
+ {
+ this.r = r;
+ this.theta = theta;
+ this.z = z;
+ this.lhsCartesian = this.toCartesian3D();
+ }
+
+ /**
+ * {@inheritDoc}
+ * Cylindrical coordinates have no exact origin so (0,0,0) is a useful origin along the pole.
+ * @return The zero vector (0,0,0).
+ */
+ @Override
+ public VectorCylindrical getZero() {
+ return ZERO;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public VectorCylindrical plus(VectorCylindrical rhs) {
+ VectorCartesian3D rhsCartesian = rhs.lhsCartesian;
+ return this.lhsCartesian.plus(rhsCartesian).toCylindrical();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public VectorCylindrical minus(VectorCylindrical rhs) {
+ VectorCartesian3D rhsCartesian = rhs.lhsCartesian;
+ return this.lhsCartesian.minus(rhsCartesian).toCylindrical();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public VectorCylindrical scalarMultiply(double scalar) {
+ return new VectorCylindrical(r * scalar, theta, z * scalar);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public VectorCylindrical negate() {
+ return new VectorCylindrical(r, (theta + Math.PI) % (2 * Math.PI), -z);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public double magnitude() {
+ return FastMath.sqrt(r * r + z * z);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public double magnitudeSquared() {
+ return r * r + z * z;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public VectorCylindrical unitVector() {
+ double mag2 = magnitudeSquared();
+ if (mag2 == 0)
+ return ZERO;
+ else
+ return scalarMultiply(1 / FastMath.sqrt(mag2));
+ }
+
+ /**
+ * {@inheritDoc}
+ * Cylindrical coordinates are non-linear so the dot product is the dot product by its geoemtric cartesian representation.
+ */
+ @Override
+ public double dot(VectorCylindrical rhs) {
+ VectorCartesian3D rhsCartesian = rhs.lhsCartesian;
+ return this.lhsCartesian.dot(rhsCartesian);
+ }
+
+ /**
+ * {@inheritDoc}
+ * Cylindrical coordinates are non-linear so the projection is by its geoemtric cartesian representation.
+ */
+ @Override
+ public VectorCylindrical projectOnto(VectorCylindrical targetVec) {
+ VectorCartesian3D rhsCartesian = targetVec.lhsCartesian;
+ return this.lhsCartesian.projectOnto(rhsCartesian).toCylindrical();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public double azimuthalAngle() {
+ return this.lhsCartesian.azimuthalAngle();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean isNaN() {
+ return Double.isNaN(r) || Double.isNaN(theta) || Double.isNaN(z);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean isInfinite() {
+ return Double.isInfinite(r) || Double.isInfinite(theta) || Double.isInfinite(z);
+ }
+
+ /**
+ * {@inheritDoc}
+ * The coordinate system is Cylindrical with pole at 0 theta.
+ * Theta is in radians, and r >= 0: (r, theta).
+ */
+ @Override
+ public CoordinateSystem getSpace() {
+ return coordinates;
+ }
+
+ /**
+ * {@inheritDoc}
+ * @implNote Tolerance is from unnormalized vector difference: |lhs - rhs| < tolerance element wise.
+ */
+ @Override
+ public boolean equals(VectorCylindrical rhs, double tolerance) {
+ return FastMath.abs(r - rhs.r) < tolerance
+ && FastMath.abs(theta - rhs.theta) < tolerance
+ && FastMath.abs(z - rhs.z) < tolerance;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public int getDimension() {
+ return coordinates.getDimension();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public VectorCylindrical cross(VectorCylindrical rhs) {
+ VectorCartesian3D rhsCartesian = rhs.lhsCartesian;
+ return this.lhsCartesian.cross(rhsCartesian).toCylindrical();
+ }
+
+ /**
+ * Converts a Cylindrical vector into a Cartesian 3D vector (x,y,z).
+ * @return The Cartesian representation of the Cylindrical vector.
+ */
+ public VectorCartesian3D toCartesian3D()
+ {
+ double x = r * FastMath.cos(theta);
+ double y = r * FastMath.sin(theta);
+ return new VectorCartesian3D(x, y, z);
+ }
+
+ /**
+ * Converts a Cylindrical vector into a Spherical vector (rho, theta (radians), phi (radians)).
+ * @return The Spherical representation of the Cylindrical vector.
+ * @implNote theta is maintained.
+ * @see https://math.libretexts.org/Bookshelves/Calculus/Book%3A_Calculus_(OpenStax)/12%3A_Vectors_in_Space/12.7%3A_Cylindrical_and_Spherical_Coordinates.
+ */
+ public VectorSpherical toSpherical()
+ {
+ double rho = FastMath.sqrt(r * r + z * z);
+ double phi = FastMath.acos(z / rho);
+ return new VectorSpherical(rho, theta, phi);
+ }
+
+ /**
+ * A singleton class representing the Cylindrical coordinate system.
+ * Coordinates are (r, theta, z).
+ * The pole is at 0 theta.
+ * r is the radius, Domain: [0, inf).
+ * theta is the polar angle, Domain: [0, 2PI).
+ * z is the vertical coordinate, Domain: (-inf, inf).
+ */
+ public static final class Cylindrical implements CoordinateSystem
+ {
+ private final static Cylindrical instance = new Cylindrical();
+
+ private Cylindrical(){}
+
+ public static Cylindrical getInstance()
+ {
+ return instance;
+ }
+
+ @Override
+ public int getDimension() {
+ return 3;
+ }
+ }
+}
diff --git a/lib/src/main/java/com/titanrobotics2022/geometry/geometry3d/VectorSpherical.java b/lib/src/main/java/com/titanrobotics2022/geometry/geometry3d/VectorSpherical.java
new file mode 100644
index 0000000..be54ba1
--- /dev/null
+++ b/lib/src/main/java/com/titanrobotics2022/geometry/geometry3d/VectorSpherical.java
@@ -0,0 +1,278 @@
+package com.titanrobotics2022.geometry.geometry3d;
+
+import com.titanrobotics2022.geometry.CoordinateSystem;
+import com.titanrobotics2022.geometry.Vector3DOperations;
+
+import org.apache.commons.math3.util.FastMath;
+
+/**
+ * A Spherical geometric vector of the ordered coordinates (rho, theta (radians), phi).
+ * The pole is at 0 theta (radians).
+ * This Spherical vector is bijective (injective and surjective) such that there is a unique
+ * Spherical vector for each point in 3D space.
+ * @see https://tutorial.math.lamar.edu/classes/calcii/sphericalcoords.aspx
+ */
+public class VectorSpherical implements Vector3DOperations Precondition: rho >= 0
+ * Precondition: theta: [0, 2PI)
+ * @param rho The distance from origin.
+ * @param theta Angle counter clockwise from the pole in radians.
+ * @param phi The vertical direction variable.
+ */
+ public VectorSpherical(double rho, double theta, double phi)
+ {
+ this.rho = rho;
+ this.theta = theta;
+ this.phi = phi;
+ this.lhsCartesian = this.toCartesian3D();
+ }
+
+ /**
+ * {@inheritDoc}
+ * Spherical coordinates have no exact origin so (0,0,0) is a useful but arbitrary origin along the pole.
+ * @return The zero vector (0,0,0).
+ */
+ @Override
+ public VectorSpherical getZero() {
+ return ZERO;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public VectorSpherical plus(VectorSpherical rhs) {
+ VectorCartesian3D rhsCartesian = rhs.lhsCartesian;
+ return this.lhsCartesian.plus(rhsCartesian).toSpherical();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public VectorSpherical minus(VectorSpherical rhs) {
+ VectorCartesian3D rhsCartesian = rhs.lhsCartesian;
+ return this.lhsCartesian.minus(rhsCartesian).toSpherical();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public VectorSpherical scalarMultiply(double scalar) {
+ return this.lhsCartesian.scalarMultiply(scalar).toSpherical();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public VectorSpherical negate() {
+ return this.lhsCartesian.negate().toSpherical();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public double magnitude() {
+ return FastMath.abs(rho);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public double magnitudeSquared() {
+ return rho * rho;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public VectorSpherical unitVector() {
+ double mag2 = magnitudeSquared();
+ if (mag2 == 0)
+ return ZERO;
+ else
+ return scalarMultiply(Math.copySign(1, rho) / FastMath.sqrt(mag2));
+ }
+
+ /**
+ * {@inheritDoc}
+ * Spherical coordinates are non-linear so the dot product is the dot product by its geoemtric cartesian representation.
+ */
+ @Override
+ public double dot(VectorSpherical rhs) {
+ VectorCartesian3D rhsCartesian = rhs.lhsCartesian;
+ return this.lhsCartesian.dot(rhsCartesian);
+ }
+
+ /**
+ * {@inheritDoc}
+ * Spherical coordinates are non-linear so the projection is by its geoemtric cartesian representation.
+ */
+ @Override
+ public VectorSpherical projectOnto(VectorSpherical targetVec) {
+ VectorCartesian3D rhsCartesian = targetVec.lhsCartesian;
+ return this.lhsCartesian.projectOnto(rhsCartesian).toSpherical();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public double azimuthalAngle() {
+ return this.lhsCartesian.azimuthalAngle();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean isNaN() {
+ return Double.isNaN(rho) || Double.isNaN(theta) || Double.isNaN(phi);
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public boolean isInfinite() {
+ return Double.isInfinite(rho) || Double.isInfinite(theta) || Double.isInfinite(phi);
+ }
+
+ /**
+ * {@inheritDoc}
+ * The coordinate system is Spherical with pole at 0 theta.
+ * Theta and phi are in radians, and rho >= 0: (rho, theta, phi).
+ */
+ @Override
+ public CoordinateSystem getSpace() {
+ return coordinates;
+ }
+
+ /**
+ * {@inheritDoc}
+ * @implNote Tolerance is from unnormalized vector difference: |lhs - rhs| < tolerance element wise.
+ */
+ @Override
+ public boolean equals(VectorSpherical rhs, double tolerance) {
+ return FastMath.abs(rho - rhs.rho) < tolerance
+ && FastMath.abs(theta - rhs.theta) < tolerance
+ && FastMath.abs(phi - rhs.phi) < tolerance;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public int getDimension() {
+ return coordinates.getDimension();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public VectorSpherical cross(VectorSpherical rhs) {
+ VectorCartesian3D rhsCartesian = rhs.lhsCartesian;
+ return this.lhsCartesian.cross(rhsCartesian).toSpherical();
+ }
+
+ /**
+ * Converts a Spherical vector into a Cartesian 3D vector (x,y,z).
+ * @return The Cartesian representation of the Spherical vector.
+ */
+ public VectorCartesian3D toCartesian3D()
+ {
+ double x = rho * FastMath.sin(phi) * FastMath.cos(theta);
+ double y = rho * FastMath.sin(phi) * FastMath.sin(theta);
+ double z = rho * FastMath.cos(phi);
+ return new VectorCartesian3D(x, y, z);
+ }
+
+ /**
+ * Converts a Spherical vector into a Cylindrical vector (r, theta (radians), z).
+ * @return The Spherical representation of the Cylindrical vector.
+ * @implNote theta is maintained.
+ * @see https://math.libretexts.org/Bookshelves/Calculus/Book%3A_Calculus_(OpenStax)/12%3A_Vectors_in_Space/12.7%3A_Cylindrical_and_Spherical_Coordinates.
+ */
+ public VectorCylindrical toCylindrical()
+ {
+ double r = rho * FastMath.sin(phi);
+ double z = rho * FastMath.cos(phi);
+ return new VectorCylindrical(r, theta, z);
+ }
+
+ /**
+ * A singleton class representing the Spherical coordinate system.
+ * Coordinates are (rho, theta, phi).
+ * The pole is at 0 theta.
+ * rho is the distance from the origin, Domain: [0, inf).
+ * theta is the polar angle, Domain: [0, 2PI).
+ * phi is the descention angle, Domain: [0, PI].
+ */
+ public static final class Spherical implements CoordinateSystem
+ {
+ private final static Spherical instance = new Spherical();
+
+ private Spherical(){}
+
+ public static Spherical getInstance()
+ {
+ return instance;
+ }
+
+ @Override
+ public int getDimension() {
+ return 3;
+ }
+ }
+}
diff --git a/lib/src/test/java/com/titanrobotics2022/geometry/geometry2d/Vector2DTest.java b/lib/src/test/java/com/titanrobotics2022/geometry/geometry2d/Vector2DTest.java
deleted file mode 100644
index 62083a0..0000000
--- a/lib/src/test/java/com/titanrobotics2022/geometry/geometry2d/Vector2DTest.java
+++ /dev/null
@@ -1,157 +0,0 @@
-package com.titanrobotics2022.geometry.geometry2d;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpiutil.math.Vector;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-import org.apache.commons.math3.util.FastMath;
-
-public class Vector2DTest {
- private static final double delta = 1e-9;
- @Test
- void additionTest() {// TODO: edit to incorporate double precision error
- Vector2D a = new Vector2D(1, 1);
- Vector2D b = new Vector2D(2, 3);
- Vector2D actual = a.plus(b);
- assertEquals(1 + 2, actual.x, delta);
- assertEquals(1 + 3, actual.y, delta);
- }
- @Test
- void subtractionTest() {// TODO: edit to incorporate double precision error
- Vector2D a = new Vector2D(1, 1);
- Vector2D b = new Vector2D(2, 3);
- Vector2D actual = a.minus(b);
- assertEquals(1 - 2, actual.x, delta);
- assertEquals(1 - 3, actual.y, delta);
- }
- @Test
- void scalarMultiplicationTest() {// TODO: edit to incorporate double precision error
- final double scalar = 2.4;
- Vector2D a = new Vector2D(2.5, 3.5);
- Vector2D actual = a.scalarMultiply(scalar);
- assertEquals(2.5 * scalar, actual.x, delta);
- assertEquals(3.5 * scalar, actual.y, delta);
- }
- @Test
- void scalarDivisionTest() {// TODO: edit to incorporate double precision error
- final double divisor = 2.4;
- Vector2D a = new Vector2D(2.5, 3.5);
- Vector2D actual = a.scalarDivide(divisor);
- assertEquals(2.5 / divisor, actual.x, delta);
- assertEquals(3.5 / divisor, actual.y, delta);
- }
- @Test
- void negationTest()
- {
- Vector2D a = new Vector2D(1, -1);
- Vector2D actual = a.negate();
- assertEquals(-1, actual.x);
- assertEquals(1, actual.y);
- }
- @Test
- void magnitudeTest()
- {
- Vector2D a = new Vector2D(2, 2);
- double actual = a.magnitude();
- assertEquals(Math.sqrt(2 * 2 + 2 * 2), actual, delta);
- }
- @Test
- void magnitudeSquaredTest()
- {
- Vector2D a = new Vector2D(2, 2);
- double actual = a.magnitudeSquared();
- assertEquals(2 * 2 + 2 * 2, actual, delta);
- }
- @Test
- void azimuthalAngleTest()
- {
- Vector2D positiveX = new Vector2D(2, 2);
- Vector2D negativeX = new Vector2D(-2, 2);
- Vector2D alongPositiveX = new Vector2D(2,0);
- double actual = positiveX.azimuthalAngle();
- assertEquals(Math.PI / 4.0, actual, delta);
- actual = negativeX.azimuthalAngle();
- assertEquals(3 * Math.PI / 4.0, actual, delta);
- actual = alongPositiveX.azimuthalAngle();
- assertEquals(0, actual, delta);
- }
- @Test
- void unitVectorTest()
- {
- Vector2D zeroVector = new Vector2D(0, 0);
- Vector2D a = new Vector2D(-2, 2);
- Vector2D actual = zeroVector.unitVector();
- assertTrue(Vector2D.ZERO == actual); // Comparing object references
- actual = a.unitVector();
- double magnitude = FastMath.sqrt(-2 * -2 + 2 * 2);
- Vector2D expected = new Vector2D(-2 / magnitude, 2 / magnitude);
- assertTrue(actual.equals(expected, delta));
- }
- @Test
- void dotProductTest()
- {
- Vector2D a = new Vector2D(2, 3);
- Vector2D b = new Vector2D(-3, 3);
- double actual = a.dot(b);
- assertEquals(2 * -3 + 3 * 3, actual, delta);
- }
- @Test
- void crossProductTest()
- {
- Vector2D a = new Vector2D(2, 3);
- Vector2D b = new Vector2D(-3, 3);
- double actual = a.cross(b);
- assertEquals(2 * 3 - (-3 * 3), actual, delta);
- }
- @Test
- void projectionTest() // TODO: Implement
- {
- Vector2D a = new Vector2D(1, 1);
- Vector2D b = new Vector2D(2, 0);
- Vector2D longerB = new Vector2D(10, 0);
- Vector2D actual = a.projectOnto(b);
- assertEquals(1, actual.x, delta);
- assertEquals(0, actual.y, delta);
- actual = a.projectOnto(longerB);
- assertEquals(1, actual.x, delta);
- assertEquals(0, actual.y, delta);
- }
- @Test
- void angleBetweenTest() // TODO: Implement also
- {
- Vector2D a = new Vector2D(1, 1);
- Vector2D b = new Vector2D(1, 0);
- Vector2D c = new Vector2D(0, -1);
- double actual = Vector2D.angleBetween(a, b);
- assertEquals(Math.PI / 4, actual, delta);
- actual = Vector2D.angleBetween(b, a);
- assertEquals(Math.PI / 4, actual, delta);
- actual = Vector2D.angleBetween(a, b.negate());
- assertEquals(3 * Math.PI / 4, actual, delta);
- actual = Vector2D.angleBetween(a, c);
- assertEquals(3 * Math.PI / 4, actual, delta);
- }
- @Test
- void equalsTest()
- {
- final double x = 1, y = 2;
- Vector2D ones = new Vector2D(x, x);
- Vector2D ones2 = new Vector2D(x, x);
- Vector2D twoes = new Vector2D(y, y);
- assertEquals(true, ones.equals(ones2));
- assertEquals(false, ones.equals(twoes));
- }
- @Test
- void equalsWithToleranceTest()
- {
- final double x = 1.55, y = 2.55;
- Vector2D ones = new Vector2D(x, x);
- Vector2D ones2 = new Vector2D(x, x);
- Vector2D twoes = new Vector2D(y, y);
- assertEquals(true, ones.equals(ones2, delta));
- assertEquals(false, ones.equals(twoes, delta));
- }
-}
diff --git a/lib/src/test/java/com/titanrobotics2022/geometry/geometry2d/VectorCartesian2DTest.java b/lib/src/test/java/com/titanrobotics2022/geometry/geometry2d/VectorCartesian2DTest.java
new file mode 100644
index 0000000..5829f66
--- /dev/null
+++ b/lib/src/test/java/com/titanrobotics2022/geometry/geometry2d/VectorCartesian2DTest.java
@@ -0,0 +1,200 @@
+package com.titanrobotics2022.geometry.geometry2d;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.apache.commons.math3.util.FastMath;
+
+public class VectorCartesian2DTest {
+ private static final double delta = 1e-9;
+ @Test
+ void getZeroVectorTest()
+ {
+ VectorCartesian2D z = VectorCartesian2D.ZERO;
+ assertEquals(0, z.x);
+ assertEquals(0, z.y);
+ }
+ @Test
+ void additionTest() {
+ VectorCartesian2D a = new VectorCartesian2D(1, 1);
+ VectorCartesian2D b = new VectorCartesian2D(2, 3);
+ VectorCartesian2D actual = a.plus(b);
+ assertEquals(1 + 2, actual.x, delta);
+ assertEquals(1 + 3, actual.y, delta);
+ }
+ @Test
+ void subtractionTest() {
+ VectorCartesian2D a = new VectorCartesian2D(1, 1);
+ VectorCartesian2D b = new VectorCartesian2D(2, 3);
+ VectorCartesian2D actual = a.minus(b);
+ assertEquals(1 - 2, actual.x, delta);
+ assertEquals(1 - 3, actual.y, delta);
+ }
+ @Test
+ void scalarMultiplicationTest() {
+ final double scalar = 2.4;
+ VectorCartesian2D a = new VectorCartesian2D(2.5, 3.5);
+ VectorCartesian2D actual = a.scalarMultiply(scalar);
+ assertEquals(2.5 * scalar, actual.x, delta);
+ assertEquals(3.5 * scalar, actual.y, delta);
+ }
+ @Test
+ void negationTest()
+ {
+ VectorCartesian2D a = new VectorCartesian2D(1, -1);
+ VectorCartesian2D actual = a.negate();
+ assertEquals(-1, actual.x);
+ assertEquals(1, actual.y);
+ }
+ @Test
+ void magnitudeTest()
+ {
+ VectorCartesian2D a = new VectorCartesian2D(2, 2);
+ double actual = a.magnitude();
+ assertEquals(Math.sqrt(2 * 2 + 2 * 2), actual, delta);
+ }
+ @Test
+ void magnitudeSquaredTest()
+ {
+ VectorCartesian2D a = new VectorCartesian2D(2, 2);
+ double actual = a.magnitudeSquared();
+ assertEquals(2 * 2 + 2 * 2, actual, delta);
+ }
+ @Test
+ void unitVectorTest()
+ {
+ VectorCartesian2D zeroVector = new VectorCartesian2D(0, 0);
+ VectorCartesian2D a = new VectorCartesian2D(-2, 2);
+ VectorCartesian2D actual = zeroVector.unitVector();
+ assertTrue(VectorCartesian2D.ZERO == actual); // Comparing object references
+ actual = a.unitVector();
+ double magnitude = FastMath.sqrt(-2 * -2 + 2 * 2);
+ VectorCartesian2D expected = new VectorCartesian2D(-2 / magnitude, 2 / magnitude);
+ assertTrue(actual.equals(expected, delta));
+ }
+ @Test
+ void dotProductTest()
+ {
+ VectorCartesian2D a = new VectorCartesian2D(2, 3);
+ VectorCartesian2D b = new VectorCartesian2D(-3, 3);
+ double actual = a.dot(b);
+ double expected = 2 * -3 + 3 * 3;
+ assertEquals(expected, actual, delta);
+ actual = b.dot(a);
+ assertEquals(expected, actual, delta);
+ }
+ @Test
+ void crossProductTest()
+ {
+ VectorCartesian2D a = new VectorCartesian2D(2, 3);
+ VectorCartesian2D b = new VectorCartesian2D(-3, 3);
+ double actual = a.cross(b);
+ assertEquals(2 * 3 - (-3 * 3), actual, delta); //Magnitude of a 3D cross product of vectors on xy plane
+ }
+ @Test
+ void projectionTest()
+ {
+ VectorCartesian2D a = new VectorCartesian2D(1, 1);
+ VectorCartesian2D b = new VectorCartesian2D(2, 0);
+ VectorCartesian2D longerB = new VectorCartesian2D(10, 0);
+ VectorCartesian2D actual = a.projectOnto(b);
+ assertEquals(1, actual.x, delta);
+ assertEquals(0, actual.y, delta);
+ actual = a.projectOnto(longerB);
+ assertEquals(1, actual.x, delta);
+ assertEquals(0, actual.y, delta);
+ }
+ @Test
+ void azimuthalAngleTest()
+ {
+ VectorCartesian2D positiveX = new VectorCartesian2D(2, 2);
+ VectorCartesian2D negativeX = new VectorCartesian2D(-2, 2);
+ VectorCartesian2D alongPositiveX = new VectorCartesian2D(2,0);
+ double actual = positiveX.azimuthalAngle();
+ assertEquals(Math.PI / 4.0, actual, delta);
+ actual = negativeX.azimuthalAngle();
+ assertEquals(3 * Math.PI / 4.0, actual, delta);
+ actual = alongPositiveX.azimuthalAngle();
+ assertEquals(0, actual, delta);
+ }
+ @Test
+ void angleBetweenTest()
+ {
+ VectorCartesian2D a = new VectorCartesian2D(1, 1);
+ VectorCartesian2D b = new VectorCartesian2D(1, 0);
+ VectorCartesian2D c = new VectorCartesian2D(0, -1);
+ double actual = VectorCartesian2D.shortestAngleBetween(a, b);
+ assertEquals(Math.PI / 4, actual, delta);
+ actual = VectorCartesian2D.shortestAngleBetween(b, a);
+ assertEquals(Math.PI / 4, actual, delta);
+ actual = VectorCartesian2D.shortestAngleBetween(a, b.negate());
+ assertEquals(3 * Math.PI / 4, actual, delta);
+ actual = VectorCartesian2D.shortestAngleBetween(a, c);
+ assertEquals(3 * Math.PI / 4, actual, delta);
+ }
+ @Test()
+ void specialDoubleTypesTest()
+ {
+ VectorCartesian2D nan = new VectorCartesian2D(1, Double.NaN);
+ VectorCartesian2D posInf = new VectorCartesian2D(1, Double.POSITIVE_INFINITY);
+ VectorCartesian2D negInf = new VectorCartesian2D(1, Double.NEGATIVE_INFINITY);
+ assertTrue(nan.isNaN());
+ assertFalse(nan.isInfinite());
+ assertTrue(posInf.isInfinite());
+ assertTrue(negInf.isInfinite());
+ }
+ @Test()
+ void dimensionsTest()
+ {
+ VectorCartesian2D a = new VectorCartesian2D(0, 0);
+ assertEquals(2, a.getDimension());
+ }
+ @Test
+ void equalsTest()
+ {
+ final double x = 1, y = 2;
+ VectorCartesian2D ones = new VectorCartesian2D(x, x);
+ VectorCartesian2D ones2 = new VectorCartesian2D(x, x);
+ VectorCartesian2D twoes = new VectorCartesian2D(y, y);
+ assertEquals(true, ones.equals(ones2));
+ assertEquals(false, ones.equals(twoes));
+ }
+ @Test
+ void toPolarTransformTest()
+ {
+ VectorPolar quadrant1 = new VectorCartesian2D(1, 1).toPolar();
+ VectorPolar quadrant2 = new VectorCartesian2D(-1, 1).toPolar();
+ VectorPolar quadrant3 = new VectorCartesian2D(-1, -1).toPolar();
+ VectorPolar quadrant4 = new VectorCartesian2D(1, -1).toPolar();
+ double r = FastMath.sqrt(1 + 1);
+ VectorPolar quadrant1Expected = new VectorPolar(r, Math.PI / 4.0);
+ VectorPolar quadrant2Expected = new VectorPolar(r, 3 * Math.PI / 4.0);
+ VectorPolar quadrant3Expected = new VectorPolar(r, 5 * Math.PI / 4.0);
+ VectorPolar quadrant4Expected = new VectorPolar(r, 7 * Math.PI / 4.0);
+ assertTrue(quadrant1.equals(quadrant1Expected, delta));
+ assertTrue(quadrant2.equals(quadrant2Expected, delta));
+ assertTrue(quadrant3.equals(quadrant3Expected, delta));
+ assertTrue(quadrant4.equals(quadrant4Expected, delta));
+
+ //Edge Cases
+ VectorPolar zeroRadians = new VectorCartesian2D(1, 0).toPolar();
+ assertEquals(1, zeroRadians.r);
+ assertEquals(0, zeroRadians.theta);
+
+ VectorPolar zeroPolarVector = new VectorCartesian2D(0, 0).toPolar();
+ assertTrue(zeroPolarVector == VectorPolar.ZERO); // Checking Object References
+ }
+ @Test
+ void equalsWithToleranceTest()
+ {
+ final double x = 1.55, y = 2.55;
+ VectorCartesian2D ones = new VectorCartesian2D(x, x);
+ VectorCartesian2D ones2 = new VectorCartesian2D(x, x);
+ VectorCartesian2D twoes = new VectorCartesian2D(y, y);
+ assertEquals(true, ones.equals(ones2, delta));
+ assertEquals(false, ones.equals(twoes, delta));
+ }
+}
diff --git a/lib/src/test/java/com/titanrobotics2022/geometry/geometry2d/VectorPolarTest.java b/lib/src/test/java/com/titanrobotics2022/geometry/geometry2d/VectorPolarTest.java
new file mode 100644
index 0000000..15578a4
--- /dev/null
+++ b/lib/src/test/java/com/titanrobotics2022/geometry/geometry2d/VectorPolarTest.java
@@ -0,0 +1,215 @@
+package com.titanrobotics2022.geometry.geometry2d;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.apache.commons.math3.util.FastMath;
+
+public class VectorPolarTest {
+ private static final double delta = 1e-9;
+ @Test
+ void getZeroVectorTest()
+ {
+ VectorPolar z = VectorPolar.ZERO;
+ assertEquals(0, z.r);
+ assertEquals(0, z.theta);
+ }
+ @Test
+ void additionTest() {
+ // 45 degree right triangles
+ VectorPolar a = new VectorPolar(FastMath.sqrt(2), Math.PI / 4);
+ VectorPolar b = new VectorPolar(FastMath.sqrt(2), 3 * Math.PI / 4);
+ VectorPolar actual = a.plus(b);
+ assertEquals(1 + 1, actual.r, delta); // Y = 1 + 1, X = 1 - 1
+ assertEquals(Math.PI / 2, actual.theta, delta);
+
+ // Adding ZERO vector gives other vector
+ VectorPolar c = new VectorPolar(1, Math.PI);
+ actual = VectorPolar.ZERO.plus(c);
+ assertTrue(actual.equals(c, delta));
+ }
+ @Test
+ void subtractionTest() {
+ // 45 degree right triangles
+ VectorPolar a = new VectorPolar(FastMath.sqrt(2), Math.PI / 4);
+ VectorPolar b = new VectorPolar(FastMath.sqrt(2), 3 * Math.PI / 4);
+ VectorPolar actual = a.minus(b);
+ assertEquals(1 + 1, actual.r, delta); // Y = 1 - 1, X = 1 - (-1)
+ assertEquals(0, actual.theta, delta);
+
+ // Adding ZERO vector gives other vector
+ VectorPolar c = new VectorPolar(1, Math.PI);
+ actual = VectorPolar.ZERO.minus(c);
+ assertTrue(actual.equals(c.negate(), delta));
+ }
+ @Test
+ void scalarMultiplicationTest() {
+ final double scalar = 1.0 / 3.0;
+ VectorPolar a = new VectorPolar(2.5, 3.5);
+ VectorPolar actual = a.scalarMultiply(scalar);
+ assertEquals(2.5 * scalar, actual.r, delta);
+ assertEquals(3.5, actual.theta, delta);
+ }
+ @Test
+ void negationTest()
+ {
+ VectorPolar a = new VectorPolar(1, Math.PI / 4);
+ VectorPolar actual = a.negate();
+ assertEquals(1, actual.r, delta);
+ assertEquals(Math.PI / 4 + Math.PI, actual.theta);
+ }
+ @Test
+ void magnitudeTest()
+ {
+ VectorPolar a = new VectorPolar(2, 2);
+ double actual = a.magnitude();
+ assertEquals(2, actual, delta);
+ }
+ @Test
+ void magnitudeSquaredTest()
+ {
+ VectorPolar a = new VectorPolar(2, 2);
+ double actual = a.magnitudeSquared();
+ assertEquals(2 * 2, actual, delta);
+ }
+ @Test
+ void unitVectorTest()
+ {
+ VectorPolar zeroVector = new VectorPolar(0, Math.PI);
+ VectorPolar actual = zeroVector.unitVector();
+ assertTrue(VectorPolar.ZERO == actual); // Comparing object references
+ double magnitude = 5;
+ VectorPolar a = new VectorPolar(magnitude, 2);
+ actual = a.unitVector();
+ assertEquals(1, actual.r);
+ assertEquals(2, actual.theta);
+
+ // Negative r
+ magnitude = -5;
+ a = new VectorPolar(magnitude, 2);
+ actual = a.unitVector();
+ assertEquals(-1, actual.r);
+ assertEquals(2, actual.theta);
+ }
+ @Test
+ void dotProductTest()
+ {
+ VectorPolar a = new VectorPolar(2 * FastMath.sqrt(2), Math.PI / 4);
+ VectorPolar b = new VectorPolar(3 * FastMath.sqrt(2), 3 * Math.PI / 4);
+ double actual = a.dot(b);
+ double expected = 2 * 3 + -2 * 3;
+ assertEquals(expected, actual, delta);
+ actual = b.dot(a);
+ assertEquals(expected, actual, delta);
+ }
+ @Test
+ void crossProductTest()
+ {
+ VectorPolar a = new VectorCartesian2D(2, 3).toPolar();
+ VectorPolar b = new VectorCartesian2D(-3, 3).toPolar();
+ double actual = a.cross(b);
+ assertEquals(2 * 3 - (-3 * 3), actual, delta); //Magnitude of a 3D cross product of vectors on xy plane
+ }
+ @Test
+ void projectionTest()
+ {
+ VectorPolar a = new VectorPolar(FastMath.sqrt(2), Math.PI / 4);
+ VectorPolar b = new VectorPolar(2, 0);
+ VectorPolar longerB = new VectorPolar(10, 0);
+ VectorPolar actual = a.projectOnto(b);
+ assertEquals(1, actual.r, delta);
+ assertEquals(0, actual.theta, delta);
+ actual = a.projectOnto(longerB);
+ assertEquals(1, actual.r, delta);
+ assertEquals(0, actual.theta, delta);
+ }
+ @Test
+ void azimuthalAngleTest()
+ {
+ VectorPolar positiveX = new VectorPolar(2, Math.PI / 4.0);
+ VectorPolar negativeX = new VectorPolar(2, 3.0 * Math.PI / 4.0);
+ VectorPolar alongPositiveX = new VectorPolar(2,0);
+ double actual = positiveX.azimuthalAngle();
+ assertEquals(Math.PI / 4.0, actual, delta);
+ actual = negativeX.azimuthalAngle();
+ assertEquals(3 * Math.PI / 4.0, actual, delta);
+ actual = alongPositiveX.azimuthalAngle();
+ assertEquals(0, actual, delta);
+
+ VectorPolar negativeR = new VectorPolar(-2, Math.PI / 4);
+ actual = negativeR.azimuthalAngle();
+ assertEquals(5.0 * Math.PI / 4.0, actual, delta);
+ }
+ @Test()
+ void specialDoubleTypesTest()
+ {
+ VectorPolar nan = new VectorPolar(1, Double.NaN);
+ VectorPolar posInf = new VectorPolar(1, Double.POSITIVE_INFINITY);
+ VectorPolar negInf = new VectorPolar(1, Double.NEGATIVE_INFINITY);
+ assertTrue(nan.isNaN());
+ assertFalse(nan.isInfinite());
+ assertTrue(posInf.isInfinite());
+ assertTrue(negInf.isInfinite());
+ }
+ @Test()
+ void dimensionsTest()
+ {
+ VectorPolar a = new VectorPolar(0, 0);
+ assertEquals(2, a.getDimension());
+ }
+ @Test
+ void equalsTest()
+ {
+ final double x = 1, y = 2;
+ VectorPolar ones = new VectorPolar(x, x);
+ VectorPolar ones2 = new VectorPolar(x, x);
+ VectorPolar twoes = new VectorPolar(y, y);
+ assertTrue(ones.equals(ones2, delta));
+ assertFalse(ones.equals(twoes, delta));
+
+ VectorPolar positiveR = new VectorPolar(1, Math.PI / 4.0);
+ VectorPolar negativeR = new VectorPolar(-1, (Math.PI / 4.0) + Math.PI);
+ assertTrue(positiveR.equals(negativeR, delta));
+ }
+ @Test
+ void toCartesianTransformTest()
+ {
+ double r = FastMath.sqrt(1 + 1);
+ VectorCartesian2D quadrant1 = new VectorPolar(r, Math.PI / 4.0).toCartesian2D();
+ VectorCartesian2D quadrant2 = new VectorPolar(r, 3 * Math.PI / 4.0).toCartesian2D();
+ VectorCartesian2D quadrant3 = new VectorPolar(r, 5 * Math.PI / 4.0).toCartesian2D();
+ VectorCartesian2D quadrant4 = new VectorPolar(r, 7 * Math.PI / 4.0).toCartesian2D();
+ VectorCartesian2D quadrant1Expected = new VectorCartesian2D(1, 1);
+ VectorCartesian2D quadrant2Expected = new VectorCartesian2D(-1, 1);
+ VectorCartesian2D quadrant3Expected = new VectorCartesian2D(-1, -1);
+ VectorCartesian2D quadrant4Expected = new VectorCartesian2D(1, -1);
+
+ assertTrue(quadrant1.equals(quadrant1Expected, delta));
+ assertTrue(quadrant2.equals(quadrant2Expected, delta));
+ assertTrue(quadrant3.equals(quadrant3Expected, delta));
+ assertTrue(quadrant4.equals(quadrant4Expected, delta));
+
+ //Edge Cases
+ VectorCartesian2D zeroRadians = new VectorPolar(1, 0).toCartesian2D();
+ assertEquals(1, zeroRadians.x, delta);
+ assertEquals(0, zeroRadians.y, delta);
+
+ VectorCartesian2D zeroCartesianVector = new VectorPolar(0, 0).toCartesian2D();
+ assertTrue(zeroCartesianVector == VectorCartesian2D.ZERO); // Checking Object References
+ zeroCartesianVector = new VectorPolar(0, Math.PI).toCartesian2D();
+ assertTrue(zeroCartesianVector == VectorCartesian2D.ZERO);
+ }
+ @Test
+ void equalsWithToleranceTest()
+ {
+ final double x = 1.55, y = 2.55;
+ VectorPolar ones = new VectorPolar(x, x);
+ VectorPolar ones2 = new VectorPolar(x, x);
+ VectorPolar twoes = new VectorPolar(y, y);
+ assertEquals(true, ones.equals(ones2, delta));
+ assertEquals(false, ones.equals(twoes, delta));
+ }
+}
diff --git a/lib/src/test/java/com/titanrobotics2022/geometry/geometry3d/Vector3DTest.java b/lib/src/test/java/com/titanrobotics2022/geometry/geometry3d/Vector3DTest.java
deleted file mode 100644
index 380df51..0000000
--- a/lib/src/test/java/com/titanrobotics2022/geometry/geometry3d/Vector3DTest.java
+++ /dev/null
@@ -1,17 +0,0 @@
-package com.titanrobotics2022.geometry.geometry3d;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-public class Vector3DTest {
- @Test
- public void additionTest() {// TODO: edit to incorporate double precision error
- Vector3D a = new Vector3D(1, 1, 1);
- Vector3D b = new Vector3D(2, 3, 4);
- Vector3D actual = a.plus(b);
- assertTrue(actual.x == 1 + 2);
- assertTrue(actual.y == 1 + 3);
- assertTrue(actual.z == 1 + 4);
- }
-}
diff --git a/lib/src/test/java/com/titanrobotics2022/geometry/geometry3d/VectorCartesian3DTest.java b/lib/src/test/java/com/titanrobotics2022/geometry/geometry3d/VectorCartesian3DTest.java
new file mode 100644
index 0000000..032260d
--- /dev/null
+++ b/lib/src/test/java/com/titanrobotics2022/geometry/geometry3d/VectorCartesian3DTest.java
@@ -0,0 +1,251 @@
+package com.titanrobotics2022.geometry.geometry3d;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.apache.commons.math3.util.FastMath;
+
+public class VectorCartesian3DTest {
+ private static final double delta = 1e-9;
+ @Test
+ void getZeroVectorTest()
+ {
+ VectorCartesian3D z = VectorCartesian3D.ZERO;
+ assertEquals(0, z.x);
+ assertEquals(0, z.y);
+ assertEquals(0, z.z);
+ }
+ @Test
+ public void additionTest() {
+ VectorCartesian3D a = new VectorCartesian3D(1, 1, 1);
+ VectorCartesian3D b = new VectorCartesian3D(2, 3, 4);
+ VectorCartesian3D actual = a.plus(b);
+ assertEquals(1 + 2, actual.x, delta);
+ assertEquals(1 + 3, actual.y, delta);
+ assertEquals(1 + 4, actual.z, delta);
+ }
+ @Test
+ void subtractionTest() {
+ VectorCartesian3D a = new VectorCartesian3D(1, 1, 1);
+ VectorCartesian3D b = new VectorCartesian3D(2, 3, 4);
+ VectorCartesian3D actual = a.minus(b);
+ assertEquals(1 - 2, actual.x, delta);
+ assertEquals(1 - 3, actual.y, delta);
+ assertEquals(1 - 4, actual.z, delta);
+ }
+ @Test
+ void scalarMultiplicationTest() {
+ final double scalar = 2.4;
+ VectorCartesian3D a = new VectorCartesian3D(2.5, 3.5, 4.5);
+ VectorCartesian3D actual = a.scalarMultiply(scalar);
+ assertEquals(2.5 * scalar, actual.x, delta);
+ assertEquals(3.5 * scalar, actual.y, delta);
+ assertEquals(4.5 * scalar, actual.z, delta);
+ }
+ @Test
+ void negationTest()
+ {
+ VectorCartesian3D a = new VectorCartesian3D(1, -1, 1);
+ VectorCartesian3D actual = a.negate();
+ assertEquals(-1, actual.x);
+ assertEquals(1, actual.y);
+ assertEquals(-1, actual.z);
+ }
+ @Test
+ void magnitudeTest()
+ {
+ VectorCartesian3D a = new VectorCartesian3D(2, 2, 2);
+ double actual = a.magnitude();
+ assertEquals(Math.sqrt(2 * 2 + 2 * 2 + 2 * 2), actual, delta);
+ }
+ @Test
+ void magnitudeSquaredTest()
+ {
+ VectorCartesian3D a = new VectorCartesian3D(2, 2, 2);
+ double actual = a.magnitudeSquared();
+ assertEquals(2 * 2 + 2 * 2 + 2 * 2, actual, delta);
+ }
+ @Test
+ void unitVectorTest()
+ {
+ VectorCartesian3D zeroVector = new VectorCartesian3D(0, 0, 0);
+ VectorCartesian3D a = new VectorCartesian3D(-2, 2, -2);
+ VectorCartesian3D actual = zeroVector.unitVector();
+ assertTrue(VectorCartesian3D.ZERO == actual); // Comparing object references
+ actual = a.unitVector();
+ double magnitude = FastMath.sqrt(-2 * -2 + 2 * 2 + -2 * -2);
+ VectorCartesian3D expected = new VectorCartesian3D(-2 / magnitude, 2 / magnitude, -2 / magnitude);
+ assertTrue(actual.equals(expected, delta));
+ }
+ @Test
+ void dotProductTest()
+ {
+ VectorCartesian3D a = new VectorCartesian3D(2, 3, 1);
+ VectorCartesian3D b = new VectorCartesian3D(-3, 3, -1);
+ double actual = a.dot(b);
+ double expected = 2 * -3 + 3 * 3 + 1 * -1;
+ assertEquals(expected, actual, delta);
+ actual = b.dot(a);
+ assertEquals(expected, actual, delta);
+ }
+ @Test
+ void crossProductTest()
+ {
+ VectorCartesian3D a = new VectorCartesian3D(2, 3, 4);
+ VectorCartesian3D b = new VectorCartesian3D(-3, 3, 5);
+ VectorCartesian3D actual = a.cross(b);
+ assertEquals(3 * 5 - 4 * 3, actual.x, delta);
+ assertEquals(-(2 * 5 - 4 * -3), actual.y, delta);
+ assertEquals(2 * 3 - 3 * -3, actual.z, delta);
+ }
+ @Test
+ void projectionTest()
+ {
+ VectorCartesian3D a = new VectorCartesian3D(1, 1, 0);
+ VectorCartesian3D b = new VectorCartesian3D(2, 0, 0);
+ VectorCartesian3D longerB = new VectorCartesian3D(10, 0, 0);
+ VectorCartesian3D actual = a.projectOnto(b);
+ assertEquals(1, actual.x, delta);
+ assertEquals(0, actual.y, delta);
+ actual = a.projectOnto(longerB);
+ assertEquals(1, actual.x, delta);
+ assertEquals(0, actual.y, delta);
+ }
+ @Test
+ void azimuthalAngleTest()
+ {
+ VectorCartesian3D positiveX = new VectorCartesian3D(2, 2, 3);
+ VectorCartesian3D negativeX = new VectorCartesian3D(-2, 2, 3);
+ VectorCartesian3D alongPositiveX = new VectorCartesian3D(2, 0, 0);
+ double actual = positiveX.azimuthalAngle();
+ assertEquals(Math.PI / 4.0, actual, delta);
+ actual = negativeX.azimuthalAngle();
+ assertEquals(3 * Math.PI / 4.0, actual, delta);
+ actual = alongPositiveX.azimuthalAngle();
+ assertEquals(0, actual, delta);
+ }
+ @Test
+ void angleBetweenTest()
+ {
+ VectorCartesian3D a = new VectorCartesian3D(1, 1, 0);
+ VectorCartesian3D b = new VectorCartesian3D(1, 0, 0);
+ VectorCartesian3D c = new VectorCartesian3D(0, -1, 0);
+ double actual = VectorCartesian3D.shortestAngleBetween(a, b);
+ assertEquals(Math.PI / 4, actual, delta);
+ actual = VectorCartesian3D.shortestAngleBetween(b, a);
+ assertEquals(Math.PI / 4, actual, delta);
+ actual = VectorCartesian3D.shortestAngleBetween(a, b.negate());
+ assertEquals(3 * Math.PI / 4, actual, delta);
+ actual = VectorCartesian3D.shortestAngleBetween(a, c);
+ assertEquals(3 * Math.PI / 4, actual, delta);
+ }
+ @Test()
+ void specialDoubleTypesTest()
+ {
+ VectorCartesian3D nan = new VectorCartesian3D(1, 1, Double.NaN);
+ VectorCartesian3D posInf = new VectorCartesian3D(1, 1, Double.POSITIVE_INFINITY);
+ VectorCartesian3D negInf = new VectorCartesian3D(1, 1, Double.NEGATIVE_INFINITY);
+ assertTrue(nan.isNaN());
+ assertFalse(nan.isInfinite());
+ assertTrue(posInf.isInfinite());
+ assertTrue(negInf.isInfinite());
+ }
+ @Test()
+ void dimensionsTest()
+ {
+ VectorCartesian3D a = new VectorCartesian3D(0, 0, 0);
+ assertEquals(3, a.getDimension());
+ }
+ @Test
+ void equalsTest()
+ {
+ final double x = 1, y = 2;
+ VectorCartesian3D ones = new VectorCartesian3D(x, x, x);
+ VectorCartesian3D ones2 = new VectorCartesian3D(x, x, x);
+ VectorCartesian3D twoes = new VectorCartesian3D(y, y, y);
+ assertEquals(true, ones.equals(ones2));
+ assertEquals(false, ones.equals(twoes));
+ }
+ @Test
+ void convertToCylindricalTest()
+ {
+ // Positive z octants
+ double r = FastMath.sqrt(1 + 1);
+ VectorCylindrical octantPos1Expected = new VectorCartesian3D(1, 1, 1).toCylindrical();
+ VectorCylindrical octantPos2Expected = new VectorCartesian3D(-1, 1, 1).toCylindrical();
+ VectorCylindrical octantPos3Expected = new VectorCartesian3D(-1, -1, 1).toCylindrical();
+ VectorCylindrical octantPos4Expected = new VectorCartesian3D(1, -1, 1).toCylindrical();
+ VectorCylindrical octantPos1 = new VectorCylindrical(r, Math.PI / 4.0, 1);
+ VectorCylindrical octantPos2 = new VectorCylindrical(r, 3 * Math.PI / 4.0, 1);
+ VectorCylindrical octantPos3 = new VectorCylindrical(r, 5 * Math.PI / 4.0, 1);
+ VectorCylindrical octantPos4 = new VectorCylindrical(r, 7 * Math.PI / 4.0, 1);
+
+ assertTrue(octantPos1.equals(octantPos1Expected, delta));
+ assertTrue(octantPos2.equals(octantPos2Expected, delta));
+ assertTrue(octantPos3.equals(octantPos3Expected, delta));
+ assertTrue(octantPos4.equals(octantPos4Expected, delta));
+
+ // Negative z octants
+ VectorCylindrical octantNeg1Expected = new VectorCartesian3D(1, 1, -1).toCylindrical();
+ VectorCylindrical octantNeg2Expected = new VectorCartesian3D(-1, 1, -1).toCylindrical();
+ VectorCylindrical octantNeg3Expected = new VectorCartesian3D(-1, -1, -1).toCylindrical();
+ VectorCylindrical octantNeg4Expected = new VectorCartesian3D(1, -1, -1).toCylindrical();
+ VectorCylindrical octantNeg1 = new VectorCylindrical(r, Math.PI / 4.0, -1);
+ VectorCylindrical octantNeg2 = new VectorCylindrical(r, 3 * Math.PI / 4.0, -1);
+ VectorCylindrical octantNeg3 = new VectorCylindrical(r, 5 * Math.PI / 4.0, -1);
+ VectorCylindrical octantNeg4 = new VectorCylindrical(r, 7 * Math.PI / 4.0, -1);
+
+ assertTrue(octantNeg1.equals(octantNeg1Expected, delta));
+ assertTrue(octantNeg2.equals(octantNeg2Expected, delta));
+ assertTrue(octantNeg3.equals(octantNeg3Expected, delta));
+ assertTrue(octantNeg4.equals(octantNeg4Expected, delta));
+ }
+ @Test
+ void convertToSphericalTest()
+ {
+ // Positive z octants
+ double r = FastMath.sqrt(1 + 1 + 1);
+ double phi = Math.PI / 2 - Math.atan(1 / Math.sqrt(2));
+ VectorSpherical octantPos1Expected = new VectorCartesian3D(1, 1, 1).toSpherical();
+ VectorSpherical octantPos2Expected = new VectorCartesian3D(-1, 1, 1).toSpherical();
+ VectorSpherical octantPos3Expected = new VectorCartesian3D(-1, -1, 1).toSpherical();
+ VectorSpherical octantPos4Expected = new VectorCartesian3D(1, -1, 1).toSpherical();
+ VectorSpherical octantPos1 = new VectorSpherical(r, Math.PI / 4, phi);
+ VectorSpherical octantPos2 = new VectorSpherical(r, 3 * Math.PI / 4, phi);
+ VectorSpherical octantPos3 = new VectorSpherical(r, 5 * Math.PI / 4, phi);
+ VectorSpherical octantPos4 = new VectorSpherical(r, 7 *Math.PI / 4, phi);
+
+ assertTrue(octantPos1.equals(octantPos1Expected, delta));
+ assertTrue(octantPos2.equals(octantPos2Expected, delta));
+ assertTrue(octantPos3.equals(octantPos3Expected, delta));
+ assertTrue(octantPos4.equals(octantPos4Expected, delta));
+
+ // Negative z octants
+ VectorSpherical octantNeg1Expected = new VectorCartesian3D(1, 1, -1).toSpherical();
+ VectorSpherical octantNeg2Expected = new VectorCartesian3D(-1, 1, -1).toSpherical();
+ VectorSpherical octantNeg3Expected = new VectorCartesian3D(-1, -1, -1).toSpherical();
+ VectorSpherical octantNeg4Expected = new VectorCartesian3D(1, -1, -1).toSpherical();
+ VectorSpherical octantNeg1 = new VectorSpherical(r, Math.PI / 4.0, Math.PI - phi);
+ VectorSpherical octantNeg2 = new VectorSpherical(r, 3 * Math.PI / 4.0, Math.PI - phi);
+ VectorSpherical octantNeg3 = new VectorSpherical(r, 5 * Math.PI / 4.0, Math.PI - phi);
+ VectorSpherical octantNeg4 = new VectorSpherical(r, 7 * Math.PI / 4.0, Math.PI - phi);
+
+ assertTrue(octantNeg1.equals(octantNeg1Expected, delta));
+ assertTrue(octantNeg2.equals(octantNeg2Expected, delta));
+ assertTrue(octantNeg3.equals(octantNeg3Expected, delta));
+ assertTrue(octantNeg4.equals(octantNeg4Expected, delta));
+ }
+ @Test
+ void equalsWithToleranceTest()
+ {
+ final double x = Math.PI, y = Math.E;
+ VectorCartesian3D ones = new VectorCartesian3D(x, x, x);
+ VectorCartesian3D ones2 = new VectorCartesian3D(x, x, x);
+ VectorCartesian3D twoes = new VectorCartesian3D(y, y, y);
+ assertEquals(true, ones.equals(ones2, delta));
+ assertEquals(false, ones.equals(twoes, delta));
+ }
+}
diff --git a/lib/src/test/java/com/titanrobotics2022/geometry/geometry3d/VectorCylindricalTest.java b/lib/src/test/java/com/titanrobotics2022/geometry/geometry3d/VectorCylindricalTest.java
new file mode 100644
index 0000000..e4969da
--- /dev/null
+++ b/lib/src/test/java/com/titanrobotics2022/geometry/geometry3d/VectorCylindricalTest.java
@@ -0,0 +1,267 @@
+package com.titanrobotics2022.geometry.geometry3d;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.apache.commons.math3.util.FastMath;
+import org.junit.jupiter.api.Test;
+
+public class VectorCylindricalTest {
+ private static final double delta = 1e-9;
+ @Test
+ void getZeroVectorTest()
+ {
+ VectorCylindrical z = VectorCylindrical.ZERO;
+ assertEquals(0, z.r);
+ assertEquals(0, z.theta);
+ }
+ @Test
+ void additionTest() {
+ // 45 degree right triangles
+ VectorCylindrical a = new VectorCylindrical(FastMath.sqrt(2), Math.PI / 4, 0);
+ VectorCylindrical b = new VectorCylindrical(FastMath.sqrt(2), 3 * Math.PI / 4, 0);
+ VectorCylindrical actual = a.plus(b);
+ assertEquals(1 + 1, actual.r, delta); // Y = 1 + 1, X = 1 - 1
+ assertEquals(Math.PI / 2, actual.theta, delta);
+
+ // Adding ZERO vector gives other vector
+ VectorCylindrical c = new VectorCylindrical(1, Math.PI, 0);
+ actual = VectorCylindrical.ZERO.plus(c);
+ assertTrue(actual.equals(c, delta));
+ }
+ @Test
+ void subtractionTest() {
+ // 45 degree right triangles
+ VectorCylindrical a = new VectorCylindrical(FastMath.sqrt(2), Math.PI / 4, 0);
+ VectorCylindrical b = new VectorCylindrical(FastMath.sqrt(2), 3 * Math.PI / 4, 0);
+ VectorCylindrical actual = a.minus(b);
+ assertEquals(1 + 1, actual.r, delta); // Y = 1 - 1, X = 1 - (-1)
+ assertEquals(0, actual.theta, delta);
+
+ // Adding ZERO vector gives other vector
+ VectorCylindrical c = new VectorCylindrical(1, Math.PI, 0);
+ actual = VectorCylindrical.ZERO.minus(c);
+ assertTrue(actual.equals(c.negate(), delta));
+ }
+ @Test
+ void scalarMultiplicationTest() {
+ final double scalar = 1.0 / 3.0;
+ VectorCylindrical a = new VectorCylindrical(2.5, 3.5, 4.5);
+ VectorCylindrical actual = a.scalarMultiply(scalar);
+ assertEquals(2.5 * scalar, actual.r, delta);
+ assertEquals(4.5 * scalar, actual.z, delta);
+ assertEquals(3.5, actual.theta, delta);
+ }
+ @Test
+ void negationTest()
+ {
+ VectorCylindrical a = new VectorCylindrical(1, Math.PI / 4, 5);
+ VectorCylindrical actual = a.negate();
+ assertEquals(1, actual.r, delta);
+ assertEquals(-5, actual.z, delta);
+ assertEquals(Math.PI / 4 + Math.PI, actual.theta);
+ }
+ @Test
+ void magnitudeTest()
+ {
+ VectorCylindrical a = new VectorCylindrical(2, 2, 3);
+ double actual = a.magnitude();
+ assertEquals(FastMath.sqrt(2 * 2 + 3 * 3), actual, delta);
+ }
+ @Test
+ void magnitudeSquaredTest()
+ {
+ VectorCylindrical a = new VectorCylindrical(2, 2, 3);
+ double actual = a.magnitudeSquared();
+ assertEquals(2 * 2 + 3 * 3, actual, delta);
+ }
+ @Test
+ void unitVectorTest()
+ {
+ VectorCylindrical zeroVector = new VectorCylindrical(0, Math.PI, 0);
+ VectorCylindrical actual = zeroVector.unitVector();
+ assertTrue(VectorCylindrical.ZERO == actual); // Comparing object references
+
+ VectorCylindrical a = new VectorCylindrical(FastMath.sqrt(2), 2,FastMath.sqrt(2));
+ actual = a.unitVector();
+ assertEquals(FastMath.sqrt(2) / 2.0, actual.r);
+ assertEquals(FastMath.sqrt(2) / 2.0, actual.z);
+ assertEquals(2, actual.theta);
+ assertEquals(1, actual.magnitude(), delta);
+
+ // Negative r
+ a = new VectorCylindrical(-FastMath.sqrt(2), 2, -FastMath.sqrt(2));
+ actual = a.unitVector();
+ assertEquals(-FastMath.sqrt(2) / 2.0, actual.r, delta);
+ assertEquals(-FastMath.sqrt(2) / 2.0, actual.z, delta);
+ assertEquals(2, actual.theta, delta);
+ assertEquals(1, actual.magnitude(), delta);
+ }
+ @Test
+ void dotProductTest()
+ {
+ VectorCylindrical a = new VectorCylindrical(2 * FastMath.sqrt(2), Math.PI / 4, 0);
+ VectorCylindrical b = new VectorCylindrical(3 * FastMath.sqrt(2), 3 * Math.PI / 4, 0);
+ double actual = a.dot(b);
+ double expected = 2 * 3 + -2 * 3;
+ assertEquals(expected, actual, delta);
+ actual = b.dot(a);
+ assertEquals(expected, actual, delta);
+ }
+ @Test
+ void crossProductTest()
+ {
+ VectorCylindrical a = new VectorCartesian3D(2, 3, 4).toCylindrical();
+ VectorCylindrical b = new VectorCartesian3D(-3, 3, 5).toCylindrical();
+ VectorCartesian3D actual = a.cross(b).toCartesian3D();
+ assertEquals(3 * 5 - 4 * 3, actual.x, delta);
+ assertEquals(-(2 * 5 - 4 * -3), actual.y, delta);
+ assertEquals(2 * 3 - 3 * -3, actual.z, delta);
+ }
+ @Test
+ void projectionTest()
+ {
+ VectorCylindrical a = new VectorCylindrical(FastMath.sqrt(2), Math.PI / 4, 0);
+ VectorCylindrical b = new VectorCylindrical(2, 0, 0);
+ VectorCylindrical longerB = new VectorCylindrical(10, 0, 0);
+ VectorCylindrical actual = a.projectOnto(b);
+ assertEquals(1, actual.r, delta);
+ assertEquals(0, actual.theta, delta);
+ actual = a.projectOnto(longerB);
+ assertEquals(1, actual.r, delta);
+ assertEquals(0, actual.theta, delta);
+ }
+ @Test
+ void azimuthalAngleTest()
+ {
+ VectorCylindrical positiveX = new VectorCylindrical(2, Math.PI / 4.0, 4);
+ VectorCylindrical negativeX = new VectorCylindrical(2, 3.0 * Math.PI / 4.0, 3);
+ VectorCylindrical alongPositiveX = new VectorCylindrical(2, 0, 10);
+ double actual = positiveX.azimuthalAngle();
+ assertEquals(Math.PI / 4.0, actual, delta);
+ actual = negativeX.azimuthalAngle();
+ assertEquals(3 * Math.PI / 4.0, actual, delta);
+ actual = alongPositiveX.azimuthalAngle();
+ assertEquals(0, actual, delta);
+
+ VectorCylindrical negativeR = new VectorCylindrical(-2, Math.PI / 4, -10);
+ actual = negativeR.azimuthalAngle();
+ assertEquals(5.0 * Math.PI / 4.0, actual, delta);
+ }
+ @Test()
+ void specialDoubleTypesTest()
+ {
+ VectorCylindrical nan = new VectorCylindrical(1, 1, Double.NaN);
+ VectorCylindrical posInf = new VectorCylindrical(1, 1, Double.POSITIVE_INFINITY);
+ VectorCylindrical negInf = new VectorCylindrical(1, 1, Double.NEGATIVE_INFINITY);
+ assertTrue(nan.isNaN());
+ assertFalse(nan.isInfinite());
+ assertTrue(posInf.isInfinite());
+ assertTrue(negInf.isInfinite());
+ }
+ @Test()
+ void dimensionsTest()
+ {
+ VectorCylindrical a = new VectorCylindrical(0, 0, 0);
+ assertEquals(3, a.getDimension());
+ }
+ @Test
+ void equalsTest()
+ {
+ final double x = 1, y = 2;
+ VectorCylindrical ones = new VectorCylindrical(x, x, x);
+ VectorCylindrical ones2 = new VectorCylindrical(x, x, x);
+ VectorCylindrical twoes = new VectorCylindrical(y, y, y);
+ assertTrue(ones.equals(ones2, delta));
+ assertFalse(ones.equals(twoes, delta));
+ }
+ @Test
+ void toCartesianTransformTest()
+ {
+ // Positive z octants
+ double r = FastMath.sqrt(1 + 1);
+ VectorCartesian3D octantPos1 = new VectorCylindrical(r, Math.PI / 4.0, 1).toCartesian3D();
+ VectorCartesian3D octantPos2 = new VectorCylindrical(r, 3 * Math.PI / 4.0, 1).toCartesian3D();
+ VectorCartesian3D octantPos3 = new VectorCylindrical(r, 5 * Math.PI / 4.0, 1).toCartesian3D();
+ VectorCartesian3D octantPos4 = new VectorCylindrical(r, 7 * Math.PI / 4.0, 1).toCartesian3D();
+ VectorCartesian3D octantPos1Expected = new VectorCartesian3D(1, 1, 1);
+ VectorCartesian3D octantPos2Expected = new VectorCartesian3D(-1, 1, 1);
+ VectorCartesian3D octantPos3Expected = new VectorCartesian3D(-1, -1, 1);
+ VectorCartesian3D octantPos4Expected = new VectorCartesian3D(1, -1, 1);
+
+ assertTrue(octantPos1.equals(octantPos1Expected, delta));
+ assertTrue(octantPos2.equals(octantPos2Expected, delta));
+ assertTrue(octantPos3.equals(octantPos3Expected, delta));
+ assertTrue(octantPos4.equals(octantPos4Expected, delta));
+
+ // Negative z octants
+ VectorCartesian3D octantNeg1 = new VectorCylindrical(r, Math.PI / 4.0, -1).toCartesian3D();
+ VectorCartesian3D octantNeg2 = new VectorCylindrical(r, 3 * Math.PI / 4.0, -1).toCartesian3D();
+ VectorCartesian3D octantNeg3 = new VectorCylindrical(r, 5 * Math.PI / 4.0, -1).toCartesian3D();
+ VectorCartesian3D octantNeg4 = new VectorCylindrical(r, 7 * Math.PI / 4.0, -1).toCartesian3D();
+ VectorCartesian3D octantNeg1Expected = new VectorCartesian3D(1, 1, -1);
+ VectorCartesian3D octantNeg2Expected = new VectorCartesian3D(-1, 1, -1);
+ VectorCartesian3D octantNeg3Expected = new VectorCartesian3D(-1, -1, -1);
+ VectorCartesian3D octantNeg4Expected = new VectorCartesian3D(1, -1, -1);
+
+ assertTrue(octantNeg1.equals(octantNeg1Expected, delta));
+ assertTrue(octantNeg2.equals(octantNeg2Expected, delta));
+ assertTrue(octantNeg3.equals(octantNeg3Expected, delta));
+ assertTrue(octantNeg4.equals(octantNeg4Expected, delta));
+
+ //Edge Cases
+ VectorCartesian3D zeroRadians = new VectorCylindrical(1, 0, 0).toCartesian3D();
+ assertEquals(1, zeroRadians.x, delta);
+ assertEquals(0, zeroRadians.y, delta);
+
+ VectorCartesian3D zeroCartesianVector = new VectorCylindrical(0, 0, 0).toCartesian3D();
+ assertEquals(zeroCartesianVector, VectorCartesian3D.ZERO);
+ zeroCartesianVector = new VectorCylindrical(0, Math.PI, 0).toCartesian3D();
+ assertEquals(zeroCartesianVector, VectorCartesian3D.ZERO);
+ }
+ @Test
+ void toSphericalTest()
+ {
+ // Positive z octants
+ double r = FastMath.sqrt(1 + 1);
+ VectorSpherical octantPos1 = new VectorCylindrical(r, Math.PI / 4.0, 1).toSpherical();
+ VectorSpherical octantPos2 = new VectorCylindrical(r, 3 * Math.PI / 4.0, 1).toSpherical();
+ VectorSpherical octantPos3 = new VectorCylindrical(r, 5 * Math.PI / 4.0, 1).toSpherical();
+ VectorSpherical octantPos4 = new VectorCylindrical(r, 7 * Math.PI / 4.0, 1).toSpherical();
+ VectorSpherical octantPos1Expected = new VectorCartesian3D(1, 1, 1).toSpherical();
+ VectorSpherical octantPos2Expected = new VectorCartesian3D(-1, 1, 1).toSpherical();
+ VectorSpherical octantPos3Expected = new VectorCartesian3D(-1, -1, 1).toSpherical();
+ VectorSpherical octantPos4Expected = new VectorCartesian3D(1, -1, 1).toSpherical();
+
+ assertTrue(octantPos1.equals(octantPos1Expected, delta));
+ assertTrue(octantPos2.equals(octantPos2Expected, delta));
+ assertTrue(octantPos3.equals(octantPos3Expected, delta));
+ assertTrue(octantPos4.equals(octantPos4Expected, delta));
+
+ // Negative z octants
+ VectorSpherical octantNeg1 = new VectorCylindrical(r, Math.PI / 4.0, -1).toSpherical();
+ VectorSpherical octantNeg2 = new VectorCylindrical(r, 3 * Math.PI / 4.0, -1).toSpherical();
+ VectorSpherical octantNeg3 = new VectorCylindrical(r, 5 * Math.PI / 4.0, -1).toSpherical();
+ VectorSpherical octantNeg4 = new VectorCylindrical(r, 7 * Math.PI / 4.0, -1).toSpherical();
+ VectorSpherical octantNeg1Expected = new VectorCartesian3D(1, 1, -1).toSpherical();
+ VectorSpherical octantNeg2Expected = new VectorCartesian3D(-1, 1, -1).toSpherical();
+ VectorSpherical octantNeg3Expected = new VectorCartesian3D(-1, -1, -1).toSpherical();
+ VectorSpherical octantNeg4Expected = new VectorCartesian3D(1, -1, -1).toSpherical();
+
+ assertTrue(octantNeg1.equals(octantNeg1Expected, delta));
+ assertTrue(octantNeg2.equals(octantNeg2Expected, delta));
+ assertTrue(octantNeg3.equals(octantNeg3Expected, delta));
+ assertTrue(octantNeg4.equals(octantNeg4Expected, delta));
+ }
+ @Test
+ void equalsWithToleranceTest()
+ {
+ final double x = 1.55, y = 2.55;
+ VectorCylindrical ones = new VectorCylindrical(x, x, x);
+ VectorCylindrical ones2 = new VectorCylindrical(x, x, x);
+ VectorCylindrical twoes = new VectorCylindrical(y, y, y);
+ assertEquals(true, ones.equals(ones2, delta));
+ assertEquals(false, ones.equals(twoes, delta));
+ }
+}
diff --git a/lib/src/test/java/com/titanrobotics2022/geometry/geometry3d/VectorSphericalTest.java b/lib/src/test/java/com/titanrobotics2022/geometry/geometry3d/VectorSphericalTest.java
new file mode 100644
index 0000000..8606fcf
--- /dev/null
+++ b/lib/src/test/java/com/titanrobotics2022/geometry/geometry3d/VectorSphericalTest.java
@@ -0,0 +1,260 @@
+package com.titanrobotics2022.geometry.geometry3d;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.apache.commons.math3.util.FastMath;
+import org.junit.jupiter.api.Test;
+
+public class VectorSphericalTest {
+ private static final double delta = 1e-9;
+ @Test
+ void getZeroVectorTest()
+ {
+ VectorSpherical z = VectorSpherical.ZERO;
+ assertEquals(0, z.rho);
+ assertEquals(0, z.theta);
+ assertEquals(0, z.phi);
+ }
+ @Test
+ void additionTest() {
+ // 45 degree right triangles
+ VectorSpherical a = new VectorSpherical(FastMath.sqrt(2), Math.PI / 4, Math.PI / 4);
+ VectorSpherical b = new VectorSpherical(FastMath.sqrt(2), Math.PI / 4, 3 * Math.PI / 4);
+ VectorSpherical actual = a.plus(b);
+ assertEquals(1 + 1, actual.rho, delta); // Y = 1 + 1, Z = 1 - 1
+ assertEquals(Math.PI / 4, actual.theta, delta);
+ assertEquals(Math.PI / 2, actual.phi, delta);
+
+ // Adding ZERO vector gives other vector
+ VectorSpherical c = new VectorSpherical(1, Math.PI, Math.PI / 2);
+ actual = VectorSpherical.ZERO.plus(c);
+ assertTrue(actual.equals(c, delta));
+ }
+ @Test
+ void subtractionTest() {
+ // 45 degree right triangles
+ VectorSpherical a = new VectorSpherical(FastMath.sqrt(2), Math.PI / 4, Math.PI / 4);
+ VectorSpherical b = new VectorSpherical(FastMath.sqrt(2), Math.PI / 4, 3 * Math.PI / 4);
+ VectorSpherical actual = a.minus(b);
+ assertEquals(1 + 1, actual.rho, delta); // Y = 1 - 1, Z = 1 - (-1)
+ assertEquals(0, actual.theta, delta);
+ assertEquals(0, actual.phi, delta);
+
+ // Adding ZERO vector gives other vector
+ VectorSpherical c = new VectorSpherical(1, Math.PI, Math.PI / 2);
+ actual = VectorSpherical.ZERO.minus(c);
+ assertTrue(actual.equals(c.negate(), delta));
+ }
+ @Test
+ void scalarMultiplicationTest() {
+ final double scalar = 1.0 / 3.0;
+ VectorSpherical a = new VectorSpherical(5, Math.PI, Math.PI / 4);
+ VectorSpherical actual = a.scalarMultiply(scalar);
+ assertEquals(5 * scalar, actual.rho, delta);
+ assertEquals(Math.PI, actual.theta, delta);
+ assertEquals(Math.PI / 4, actual.phi, delta);
+ }
+ @Test
+ void negationTest()
+ {
+ VectorSpherical a = new VectorSpherical(1, Math.PI / 4, Math.PI / 4);
+ VectorSpherical actual = a.negate();
+ assertEquals(1, actual.rho, delta);
+ assertEquals(Math.PI + Math.PI / 4, actual.theta, delta);
+ assertEquals(3 * Math.PI / 4, actual.phi);
+ }
+ @Test
+ void magnitudeTest()
+ {
+ VectorSpherical a = new VectorSpherical(2, Math.PI, Math.PI / 4);
+ double actual = a.magnitude();
+ assertEquals(2, actual, delta);
+ }
+ @Test
+ void magnitudeSquaredTest()
+ {
+ VectorSpherical a = new VectorSpherical(2, Math.PI, Math.PI / 4);
+ double actual = a.magnitudeSquared();
+ assertEquals(2 * 2, actual, delta);
+ }
+ @Test
+ void unitVectorTest()
+ {
+ VectorSpherical zeroVector = new VectorSpherical(0, Math.PI, 0);
+ VectorSpherical actual = zeroVector.unitVector();
+ assertTrue(VectorSpherical.ZERO == actual); // Comparing object references
+
+ VectorSpherical a = new VectorSpherical(4, Math.PI , Math.PI / 4);
+ actual = a.unitVector();
+ assertEquals(1, actual.rho, delta);
+ assertEquals(Math.PI, actual.theta, delta);
+ assertEquals(Math.PI / 4, actual.phi, delta);
+ assertEquals(1, actual.magnitude(), delta);
+ }
+ @Test
+ void dotProductTest()
+ {
+ VectorSpherical a = new VectorSpherical(2 * FastMath.sqrt(2), Math.PI / 4, Math.PI / 2);
+ VectorSpherical b = new VectorSpherical(3 * FastMath.sqrt(2), 3 * Math.PI / 4, Math.PI / 2);
+ double actual = a.dot(b);
+ double expected = 2 * 3 + -2 * 3;
+ assertEquals(expected, actual, delta);
+ actual = b.dot(a);
+ assertEquals(expected, actual, delta);
+ }
+ @Test
+ void crossProductTest()
+ {
+ VectorSpherical a = new VectorCartesian3D(2, 3, 4).toSpherical();
+ VectorSpherical b = new VectorCartesian3D(-3, 3, 5).toSpherical();
+ VectorCartesian3D actual = a.cross(b).toCartesian3D();
+ assertEquals(3 * 5 - 4 * 3, actual.x, delta);
+ assertEquals(-(2 * 5 - 4 * -3), actual.y, delta);
+ assertEquals(2 * 3 - 3 * -3, actual.z, delta);
+ }
+ @Test
+ void projectionTest()
+ {
+ VectorSpherical a = new VectorSpherical(FastMath.sqrt(2), 0, Math.PI / 4);
+ VectorSpherical b = new VectorSpherical(2, 0, Math.PI / 2);
+ VectorSpherical longerB = new VectorSpherical(10, 0, Math.PI / 2);
+ VectorSpherical actual = a.projectOnto(b);
+ assertEquals(1, actual.rho, delta);
+ assertEquals(0, actual.theta, delta);
+ actual = a.projectOnto(longerB);
+ assertEquals(1, actual.rho, delta);
+ assertEquals(0, actual.theta, delta);
+ }
+ @Test
+ void azimuthalAngleTest()
+ {
+ VectorSpherical positiveX = new VectorSpherical(2, Math.PI / 4, Math.PI / 2);
+ VectorSpherical negativeX = new VectorSpherical(2, 3 * Math.PI / 4, Math.PI / 2);
+ VectorSpherical alongPositiveX = new VectorSpherical(2, 0, Math.PI / 4);
+ double actual = positiveX.azimuthalAngle();
+ assertEquals(Math.PI / 4.0, actual, delta);
+ actual = negativeX.azimuthalAngle();
+ assertEquals(3 * Math.PI / 4.0, actual, delta);
+ actual = alongPositiveX.azimuthalAngle();
+ assertEquals(0, actual, delta);
+ }
+ @Test()
+ void specialDoubleTypesTest()
+ {
+ VectorSpherical nan = new VectorSpherical(1, 1, Double.NaN);
+ VectorSpherical posInf = new VectorSpherical(1, 1, Double.POSITIVE_INFINITY);
+ VectorSpherical negInf = new VectorSpherical(1, 1, Double.NEGATIVE_INFINITY);
+ assertTrue(nan.isNaN());
+ assertFalse(nan.isInfinite());
+ assertTrue(posInf.isInfinite());
+ assertTrue(negInf.isInfinite());
+ }
+ @Test()
+ void dimensionsTest()
+ {
+ VectorSpherical a = new VectorSpherical(0, 0, 0);
+ assertEquals(3, a.getDimension());
+ }
+ @Test
+ void equalsTest()
+ {
+ final double x = 1, y = 2;
+ VectorSpherical ones = new VectorSpherical(x, x, x);
+ VectorSpherical ones2 = new VectorSpherical(x, x, x);
+ VectorSpherical twoes = new VectorSpherical(y, y, y);
+ assertTrue(ones.equals(ones2, delta));
+ assertFalse(ones.equals(twoes, delta));
+ }
+ @Test
+ void toCartesianTransformTest()
+ {
+ // Positive z octants
+ double r = FastMath.sqrt(1 + 1 + 1);
+ double phi = Math.PI / 2 - Math.atan(1 / Math.sqrt(2));
+ VectorCartesian3D octantPos1 = new VectorSpherical(r, Math.PI / 4, phi).toCartesian3D();
+ VectorCartesian3D octantPos2 = new VectorSpherical(r, 3 * Math.PI / 4, phi).toCartesian3D();
+ VectorCartesian3D octantPos3 = new VectorSpherical(r, 5 * Math.PI / 4, phi).toCartesian3D();
+ VectorCartesian3D octantPos4 = new VectorSpherical(r, 7 * Math.PI / 4, phi).toCartesian3D();
+ VectorCartesian3D octantPos1Expected = new VectorCartesian3D(1, 1, 1);
+ VectorCartesian3D octantPos2Expected = new VectorCartesian3D(-1, 1, 1);
+ VectorCartesian3D octantPos3Expected = new VectorCartesian3D(-1, -1, 1);
+ VectorCartesian3D octantPos4Expected = new VectorCartesian3D(1, -1, 1);
+
+ assertTrue(octantPos1.equals(octantPos1Expected, delta));
+ assertTrue(octantPos2.equals(octantPos2Expected, delta));
+ assertTrue(octantPos3.equals(octantPos3Expected, delta));
+ assertTrue(octantPos4.equals(octantPos4Expected, delta));
+
+ // Negative z octants
+ VectorCartesian3D octantNeg1 = new VectorSpherical(r, Math.PI / 4.0, Math.PI - phi).toCartesian3D();
+ VectorCartesian3D octantNeg2 = new VectorSpherical(r, 3 * Math.PI / 4.0, Math.PI - phi).toCartesian3D();
+ VectorCartesian3D octantNeg3 = new VectorSpherical(r, 5 * Math.PI / 4.0, Math.PI - phi).toCartesian3D();
+ VectorCartesian3D octantNeg4 = new VectorSpherical(r, 7 * Math.PI / 4.0, Math.PI - phi).toCartesian3D();
+ VectorCartesian3D octantNeg1Expected = new VectorCartesian3D(1, 1, -1);
+ VectorCartesian3D octantNeg2Expected = new VectorCartesian3D(-1, 1, -1);
+ VectorCartesian3D octantNeg3Expected = new VectorCartesian3D(-1, -1, -1);
+ VectorCartesian3D octantNeg4Expected = new VectorCartesian3D(1, -1, -1);
+
+ assertTrue(octantNeg1.equals(octantNeg1Expected, delta));
+ assertTrue(octantNeg2.equals(octantNeg2Expected, delta));
+ assertTrue(octantNeg3.equals(octantNeg3Expected, delta));
+ assertTrue(octantNeg4.equals(octantNeg4Expected, delta));
+
+ //Edge Cases
+ VectorCartesian3D zeroRadians = new VectorSpherical(1, 0, Math.PI / 2).toCartesian3D();
+ assertEquals(1, zeroRadians.x, delta);
+ assertEquals(0, zeroRadians.y, delta);
+
+ VectorCartesian3D zeroCartesianVector = new VectorSpherical(0, 0, 0).toCartesian3D();
+ assertEquals(zeroCartesianVector, VectorCartesian3D.ZERO);
+ zeroCartesianVector = new VectorSpherical(0, Math.PI, 0).toCartesian3D();
+ assertEquals(zeroCartesianVector, VectorCartesian3D.ZERO);
+ }
+ @Test
+ void toCylindricalTest()
+ {
+ // Positive z octants
+ double r = FastMath.sqrt(1 + 1 + 1);
+ double phi = Math.PI / 2 - Math.atan(1 / Math.sqrt(2));
+ VectorCylindrical octantPos1 = new VectorSpherical(r, Math.PI / 4, phi).toCylindrical();
+ VectorCylindrical octantPos2 = new VectorSpherical(r, 3 * Math.PI / 4, phi).toCylindrical();
+ VectorCylindrical octantPos3 = new VectorSpherical(r, 5 * Math.PI / 4, phi).toCylindrical();
+ VectorCylindrical octantPos4 = new VectorSpherical(r, 7 * Math.PI / 4, phi).toCylindrical();
+ VectorCylindrical octantPos1Expected = new VectorCartesian3D(1, 1, 1).toCylindrical();
+ VectorCylindrical octantPos2Expected = new VectorCartesian3D(-1, 1, 1).toCylindrical();
+ VectorCylindrical octantPos3Expected = new VectorCartesian3D(-1, -1, 1).toCylindrical();
+ VectorCylindrical octantPos4Expected = new VectorCartesian3D(1, -1, 1).toCylindrical();
+
+ assertTrue(octantPos1.equals(octantPos1Expected, delta));
+ assertTrue(octantPos2.equals(octantPos2Expected, delta));
+ assertTrue(octantPos3.equals(octantPos3Expected, delta));
+ assertTrue(octantPos4.equals(octantPos4Expected, delta));
+
+ // Negative z octants
+ VectorCylindrical octantNeg1 = new VectorSpherical(r, Math.PI / 4.0, Math.PI - phi).toCylindrical();
+ VectorCylindrical octantNeg2 = new VectorSpherical(r, 3 * Math.PI / 4.0, Math.PI - phi).toCylindrical();
+ VectorCylindrical octantNeg3 = new VectorSpherical(r, 5 * Math.PI / 4.0, Math.PI - phi).toCylindrical();
+ VectorCylindrical octantNeg4 = new VectorSpherical(r, 7 * Math.PI / 4.0, Math.PI - phi).toCylindrical();
+ VectorCylindrical octantNeg1Expected = new VectorCartesian3D(1, 1, -1).toCylindrical();
+ VectorCylindrical octantNeg2Expected = new VectorCartesian3D(-1, 1, -1).toCylindrical();
+ VectorCylindrical octantNeg3Expected = new VectorCartesian3D(-1, -1, -1).toCylindrical();
+ VectorCylindrical octantNeg4Expected = new VectorCartesian3D(1, -1, -1).toCylindrical();
+
+ assertTrue(octantNeg1.equals(octantNeg1Expected, delta));
+ assertTrue(octantNeg2.equals(octantNeg2Expected, delta));
+ assertTrue(octantNeg3.equals(octantNeg3Expected, delta));
+ assertTrue(octantNeg4.equals(octantNeg4Expected, delta));
+ }
+ @Test
+ void equalsWithToleranceTest()
+ {
+ final double x = 1.55, y = 2.55;
+ VectorSpherical ones = new VectorSpherical(x, x, x);
+ VectorSpherical ones2 = new VectorSpherical(x, x, x);
+ VectorSpherical twoes = new VectorSpherical(y, y, y);
+ assertEquals(true, ones.equals(ones2, delta));
+ assertEquals(false, ones.equals(twoes, delta));
+ }
+}