Ticket #16472: 16472.5.patch

src/org/openstreetmap/josm/gui/util/imagery/CameraPlane.java

@@ -4 +4 @@
 import java.awt.Point;
 import java.awt.geom.Point2D;
 import java.awt.image.BufferedImage;
-import java.awt.image.DataBuffer;
-import java.awt.image.DataBufferDouble;
-import java.awt.image.DataBufferInt;
 import java.util.stream.IntStream;
+
 import javax.annotation.Nullable;
 
 /**
@@ -54 +52 @@
         this.rotation = new Vector3D(Vector3D.VectorType.RPA, distance, 0, 0);
         this.vectors = new Vector3D[width][height];
         IntStream.range(0, this.height).parallel().forEach(y -> IntStream.range(0, this.width).parallel()
-            .forEach(x -> this.vectors[x][y] = this.getVector3D((double) x, y)));
+            .forEach(x -> this.vectors[x][y] = this.getNonRotatedVector3D((double) x, y)));
     }
 
     /**
@@ -114 +112 @@
     }
 
     /**
-     * Convert a point to a 3D vector (vectors are cached)
+     * Convert a point to a 3D vector (vectors in image are cached)
      *
      * @param x The x coordinate
      * @param y The y coordinate
      * @return The vector
      */
     public Vector3D getVector3D(final int x, final int y) {
-        Vector3D res;
-        try {
-            res = rotate(vectors[x][y]);
-        } catch (Exception e) {
-            res = Vector3D.DEFAULT_VECTOR_3D;
-        }
-        return res;
+        return this.rotate(this.getNonRotatedVector3D(x, y));
     }
 
     /**
@@ -138 +130 @@
      * @return The vector (the middle of the image is 0, 0)
      */
     public Vector3D getVector3D(final double x, final double y) {
+        return this.rotate(this.getNonRotatedVector3D(x, y));
+    }
+
+    private Vector3D getNonRotatedVector3D(final int x, final int y) {
+        // Prefer cached vectors
+        if (x >= 0 && y >= 0 && x < this.vectors.length && y < this.vectors[0].length) {
+            return this.vectors[x][y];
+        }
+
+        return this.getNonRotatedVector3D(x, (double) y);
+    }
+
+    private Vector3D getNonRotatedVector3D(final double x, final double y) {
         return new Vector3D(x - width / 2d, y - height / 2d, this.rotation.getRadialDistance()).normalize();
     }
 
@@ -151 +156 @@
     }
 
     /**
-     * Set the rotation from the difference of two points
+     * Set the rotation from the difference of two points on the original plane
      *
-     * @param from The originating point
-     * @param to The new point
+     * @param fromPoint The originating point
+     * @param toPoint The new point
      */
-    public void setRotationFromDelta(final Point from, final Point to) {
-        // Bound check (bounds are essentially the image viewer component)
-        if (from.x < 0 || from.y < 0 || to.x < 0 || to.y < 0
-            || from.x > this.vectors.length || from.y > this.vectors[0].length
-            || to.x > this.vectors.length || to.y > this.vectors[0].length) {
-            return;
-        }
-        Vector3D f1 = this.vectors[from.x][from.y];
-        Vector3D t1 = this.vectors[to.x][to.y];
-        double deltaPolarAngle = f1.getPolarAngle() - t1.getPolarAngle();
-        double deltaAzimuthalAngle = t1.getAzimuthalAngle() - f1.getAzimuthalAngle();
-        double polarAngle = this.rotation.getPolarAngle() + deltaPolarAngle;
-        double azimuthalAngle = this.rotation.getAzimuthalAngle() + deltaAzimuthalAngle;
+    public void setRotationFromDelta(final Point fromPoint, final Point toPoint) {
+        final Vector3D from = this.getVector3D(fromPoint.x, fromPoint.y);
+        final Vector3D to = this.getVector3D(toPoint.x, toPoint.y);
+        final double deltaPolarAngle = from.getPolarAngle() - to.getPolarAngle();
+        final double deltaAzimuthalAngle = to.getAzimuthalAngle() - from.getAzimuthalAngle();
+        final double polarAngle = this.rotation.getPolarAngle() + deltaPolarAngle;
+        final double azimuthalAngle = this.rotation.getAzimuthalAngle() + deltaAzimuthalAngle;
         this.setRotation(azimuthalAngle, polarAngle);
     }
 
@@ -178 +177 @@
      * @param vec vector pointing new view position.
      */
     public void setRotation(Vector3D vec) {
-        setRotation(vec.getPolarAngle(), vec.getAzimuthalAngle());
+        setRotation(vec.getAzimuthalAngle(), vec.getPolarAngle());
     }
 
     public Vector3D getRotation() {
@@ -189 +188 @@
         // Note: Something, somewhere, is switching the two.
         // FIXME: Figure out what is switching them and why
         // Prevent us from going much outside 2pi
-        if (polarAngle < 0) {
-            polarAngle = polarAngle + TWO_PI;
-        } else if (polarAngle > TWO_PI) {
-            polarAngle = polarAngle - TWO_PI;
+        if (azimuthalAngle < 0) {
+            azimuthalAngle = azimuthalAngle + TWO_PI;
+        } else if (azimuthalAngle > TWO_PI) {
+            azimuthalAngle = azimuthalAngle - TWO_PI;
         }
         // Avoid flipping the camera
-        if (azimuthalAngle > HALF_PI) {
-            azimuthalAngle = HALF_PI;
-        } else if (azimuthalAngle < -HALF_PI) {
-            azimuthalAngle = -HALF_PI;
+        if (polarAngle > HALF_PI) {
+            polarAngle = HALF_PI;
+        } else if (polarAngle < -HALF_PI) {
+            polarAngle = -HALF_PI;
         }
         this.rotation = new Vector3D(Vector3D.VectorType.RPA, this.rotation.getRadialDistance(), polarAngle, azimuthalAngle);
     }
@@ -229 +228 @@
         // We only do pitch/roll (we specifically do not do roll -- this would lead to tilting the image)
         // So yaw (alpha) -> azimuthalAngle, pitch (beta) -> polarAngle, roll (gamma) -> 0 (sin(gamma) -> 0, cos(gamma) -> 1)
         // gamma is set here just to make it slightly easier to tilt images in the future -- we just have to set the gamma somewhere else.
-        // Ironically enough, the alpha (yaw) and gama (roll) got reversed somewhere. TODO figure out where and fix this.
+        // Ironically enough, the alpha (yaw) and gama (roll) got reversed somewhere. TODO figure out where and fix it.
+        // z == roll axis
+        // x == pitch axis
+        // y == yaw axis
         final int gamma = 0;
-        final double sinAlpha = Math.sin(gamma);
-        final double cosAlpha = Math.cos(gamma);
-        final double cosGamma = this.rotation.getAzimuthalAngleCos();
-        final double sinGamma = this.rotation.getAzimuthalAngleSin();
-        final double cosBeta = this.rotation.getPolarAngleCos();
-        final double sinBeta = this.rotation.getPolarAngleSin();
+        final double sinGamma = Math.sin(gamma);
+        final double cosGamma = Math.cos(gamma);
+        final double cosBeta = this.rotation.getAzimuthalAngleCos();
+        final double sinBeta = this.rotation.getAzimuthalAngleSin();
+        final double cosAlpha = this.rotation.getPolarAngleCos();
+        final double sinAlpha = this.rotation.getPolarAngleSin();
         final double x = vec.getX();
         final double y = YAW_DIRECTION * vec.getY();
         final double z = vec.getZ();
@@ -249 +251 @@
     }
 
     public void mapping(BufferedImage sourceImage, BufferedImage targetImage) {
-        DataBuffer sourceBuffer = sourceImage.getRaster().getDataBuffer();
-        DataBuffer targetBuffer = targetImage.getRaster().getDataBuffer();
-        // Faster mapping
-        if (sourceBuffer.getDataType() == DataBuffer.TYPE_INT && targetBuffer.getDataType() == DataBuffer.TYPE_INT) {
-            int[] sourceImageBuffer = ((DataBufferInt) sourceImage.getRaster().getDataBuffer()).getData();
-            int[] targetImageBuffer = ((DataBufferInt) targetImage.getRaster().getDataBuffer()).getData();
-            IntStream.range(0, targetImage.getHeight()).parallel()
-                    .forEach(y -> IntStream.range(0, targetImage.getWidth()).forEach(x -> {
-                        final Point2D.Double p = mapPoint(x, y);
-                        int tx = (int) (p.x * (sourceImage.getWidth() - 1));
-                        int ty = (int) (p.y * (sourceImage.getHeight() - 1));
-                        int color = sourceImageBuffer[ty * sourceImage.getWidth() + tx];
-                        targetImageBuffer[y * targetImage.getWidth() + x] = color;
-                    }));
-        } else if (sourceBuffer.getDataType() == DataBuffer.TYPE_DOUBLE && targetBuffer.getDataType() == DataBuffer.TYPE_DOUBLE) {
-            double[] sourceImageBuffer = ((DataBufferDouble) sourceImage.getRaster().getDataBuffer()).getData();
-            double[] targetImageBuffer = ((DataBufferDouble) targetImage.getRaster().getDataBuffer()).getData();
-            IntStream.range(0, targetImage.getHeight()).parallel()
-                    .forEach(y -> IntStream.range(0, targetImage.getWidth()).forEach(x -> {
-                        final Point2D.Double p = mapPoint(x, y);
-                        int tx = (int) (p.x * (sourceImage.getWidth() - 1));
-                        int ty = (int) (p.y * (sourceImage.getHeight() - 1));
-                        double color = sourceImageBuffer[ty * sourceImage.getWidth() + tx];
-                        targetImageBuffer[y * targetImage.getWidth() + x] = color;
-                    }));
-        } else {
-            IntStream.range(0, targetImage.getHeight()).parallel()
-                .forEach(y -> IntStream.range(0, targetImage.getWidth()).parallel().forEach(x -> {
-                    final Point2D.Double p = mapPoint(x, y);
-                    targetImage.setRGB(x, y, sourceImage.getRGB((int) (p.x * (sourceImage.getWidth() - 1)),
-                        (int) (p.y * (sourceImage.getHeight() - 1))));
-                }));
-        }
+        // There can be slightly faster mapping if both data buffers are the same time (image.getRaster().getDataBuffer())
+        IntStream.range(0, targetImage.getHeight()).parallel()
+            .forEach(y -> IntStream.range(0, targetImage.getWidth()).parallel().forEach(x -> {
+                final Point2D.Double p = mapPoint(x, y);
+                targetImage.setRGB(x, y, sourceImage.getRGB((int) (p.x * (sourceImage.getWidth() - 1)),
+                    (int) (p.y * (sourceImage.getHeight() - 1))));
+            }));
     }
 
     /**
-     * Map a real point to the displayed point. This method uses cached vectors.
+     * Map a real point to the displayed point. This method may use cached vectors.
      * @param x The original x coordinate
      * @param y The original y coordinate
      * @return The scaled (0-1) point in the image. Use {@code p.x * (image.getWidth() - 1)} or {@code p.y * image.getHeight() - 1}.

src/org/openstreetmap/josm/gui/util/imagery/Vector3D.java

@@ -30 +30 @@
     private final double z;
     /* The following are all lazily calculated, but should always be the same */
     /** The radius r */
-    private volatile double radialDistance = Double.NaN;
+    private double radialDistance = Double.NaN;
     /** The polar angle theta (inclination) */
-    private volatile double polarAngle = Double.NaN;
+    private double polarAngle = Double.NaN;
     /** Cosine of polar angle (angle from Z axis, AKA straight up) */
-    private volatile double polarAngleCos = Double.NaN;
+    private double polarAngleCos = Double.NaN;
     /** Sine of polar angle (angle from Z axis, AKA straight up) */
-    private volatile double polarAngleSin = Double.NaN;
+    private double polarAngleSin = Double.NaN;
     /** The azimuthal angle phi */
-    private volatile double azimuthalAngle = Double.NaN;
+    private double azimuthalAngle = Double.NaN;
     /** Cosine of azimuthal angle (angle from X axis) */
-    private volatile double azimuthalAngleCos = Double.NaN;
+    private double azimuthalAngleCos = Double.NaN;
     /** Sine of azimuthal angle (angle from X axis) */
-    private volatile double azimuthalAngleSin = Double.NaN;
+    private double azimuthalAngleSin = Double.NaN;
 
     /**
      * Create a new Vector3D object using the XYZ coordinate system
@@ -130 +130 @@
      */
     public double getPolarAngle() {
         if (Double.isNaN(this.polarAngle)) {
-            // This was Math.atan(x, z) in the Mapillary plugin
-            // This should be Math.atan(y, z)
-            this.polarAngle = Math.atan2(this.x, this.z);
+            if (Double.isNaN(this.radialDistance)) {
+                // Force calculation
+                this.getRadialDistance();
+            }
+            // Avoid issues where x, y, and z are 0
+            if (this.radialDistance == 0) {
+                this.polarAngle = 0;
+            } else {
+                // This was Math.acos(y / radialDistance) in the Mapillary plugin
+                // This should be Math.acos(z / radialDistance)
+                this.polarAngle = Math.acos(this.z / this.radialDistance);
+            }
         }
         return this.polarAngle;
     }
@@ -168 +177 @@
      */
     public double getAzimuthalAngle() {
         if (Double.isNaN(this.azimuthalAngle)) {
-            if (Double.isNaN(this.radialDistance)) {
-                // Force calculation
-                this.getRadialDistance();
-            }
-            // Avoid issues where x, y, and z are 0
-            if (this.radialDistance == 0) {
-                this.azimuthalAngle = 0;
-            } else {
-                // This was Math.acos(y / radialDistance) in the Mapillary plugin
-                // This should be Math.acos(z / radialDistance)
-                this.azimuthalAngle = Math.acos(this.y / this.radialDistance);
-            }
+            // This was Math.atan(x, z) in the Mapillary plugin
+            // This should be Math.atan(y, z)
+            this.azimuthalAngle = Math.atan2(this.y, this.z);
         }
         return this.azimuthalAngle;
     }
@@ -239 +239 @@
     public boolean equals(Object o) {
         if (o instanceof Vector3D) {
             Vector3D other = (Vector3D) o;
-            return this.x == other.x && this.y == other.y && this.z == other.z;
+            return Double.compare(this.x, other.x) == 0
+                    && Double.compare(this.y, other.y) == 0
+                    && Double.compare(this.z, other.z) == 0;
         }
         return false;
     }
 
     @Override
     public String toString() {
+        // Initialize the various ephemeral objects
+        this.getRadialDistance();
+        this.getAzimuthalAngle();
+        this.getPolarAngle();
         return "[x=" + this.x + ", y=" + this.y + ", z=" + this.z + ", r=" + this.radialDistance + ", inclination="
             + this.polarAngle + ", azimuthal=" + this.azimuthalAngle + "]";
     }

test/unit/org/openstreetmap/josm/gui/util/imagery/CameraPlaneTest.java

@@ -9 +9 @@
 import java.util.stream.Stream;
 
 import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Disabled;
 import org.junit.jupiter.api.Test;
 import org.junit.jupiter.params.ParameterizedTest;
 import org.junit.jupiter.params.provider.Arguments;
@@ -20 +19 @@
     private static final int CAMERA_PLANE_WIDTH = 800;
     private static final int CAMERA_PLANE_HEIGHT = 600;
 
+    private static final double MAX_DELTA = 1e-15;
+
     private CameraPlane cameraPlane;
 
     @BeforeEach
@@ -28 +29 @@
     }
 
     @Test
-    @Disabled("Currently broken")
+    void testDimensions() {
+        assertAll(() -> assertEquals(CAMERA_PLANE_HEIGHT, this.cameraPlane.getHeight()),
+                () -> assertEquals(CAMERA_PLANE_WIDTH, this.cameraPlane.getWidth()));
+    }
+
+    @Test
     void testSetRotation() {
         Vector3D vec = new Vector3D(0, 0, 1);
-        cameraPlane.setRotation(vec);
-        Vector3D out = cameraPlane.getRotation();
-        assertAll(() -> assertEquals(280.0830152838839, out.getRadialDistance(), 0.001),
-            () -> assertEquals(0, out.getPolarAngle(), 0.001), () -> assertEquals(0, out.getAzimuthalAngle(), 0.001));
+        this.cameraPlane.setRotation(vec);
+        Vector3D out = this.cameraPlane.getRotation();
+        assertAll(() -> assertEquals(280.0830152838839, out.getRadialDistance(), 0.001, "Radial distance"),
+            () -> assertEquals(0, out.getPolarAngle(), 0.001, "Polar angle"),
+                () -> assertEquals(0, out.getAzimuthalAngle(), 0.001, "Azimuth angle"));
+    }
+
+    static Stream<Arguments> testRotationFromDelta() {
+        return Stream.of(Arguments.of(new Point(CAMERA_PLANE_WIDTH - 1, CAMERA_PLANE_HEIGHT / 2), 0, Math.PI),
+                Arguments.of(new Point(0, CAMERA_PLANE_HEIGHT / 2), 0, -Math.PI),
+                Arguments.of(new Point(CAMERA_PLANE_WIDTH / 2, CAMERA_PLANE_HEIGHT - 1), Math.PI / 2, 0),
+                Arguments.of(new Point(CAMERA_PLANE_WIDTH / 2, 0), - Math.PI / 2, 0));
+    }
+
+    @ParameterizedTest
+    @MethodSource
+    void testRotationFromDelta(final Point to, final double expectedPolarAngle, final double expectedAzimuthalAngle) {
+        final double maxDelta = 1E-15;
+        // Ensure that rotation is set to the default, and it correctly works
+        this.testSetRotation();
+        final Vector3D initialVector = this.cameraPlane.getRotation();
+        final Point zero = new Point(CAMERA_PLANE_WIDTH / 2, CAMERA_PLANE_HEIGHT / 2);
+        // This checks that we can correctly rotate to the edge of the image
+        this.cameraPlane.setRotationFromDelta(zero, to);
+        final Vector3D newRotation = this.cameraPlane.getRotation();
+        assertAll(() -> assertEquals(initialVector.getRadialDistance(), newRotation.getRadialDistance(), "Radial distance should not change"),
+                () -> assertEquals(expectedPolarAngle, newRotation.getPolarAngle(), maxDelta, "Pitch"),
+                () -> assertEquals(expectedAzimuthalAngle, newRotation.getAzimuthalAngle(), maxDelta, "Yaw"));
+    }
+
+    static Stream<Arguments> testMapPoint() {
+        return Stream.of(Arguments.of(new Point2D.Double(0.5, 0.5), new Point2D.Double(CAMERA_PLANE_WIDTH / 2d, CAMERA_PLANE_HEIGHT / 2d), new Vector3D(0, 0, 1)),
+                Arguments.of(new Point2D.Double(0.3472222222222222, 0.5), new Point2D.Double(0, CAMERA_PLANE_HEIGHT / 2d), new Vector3D(0, 0, 1)),
+                Arguments.of(new Point2D.Double(0.6525905178237755, 0.5), new Point2D.Double(CAMERA_PLANE_WIDTH - 1, CAMERA_PLANE_HEIGHT / 2d), new Vector3D(0, 0, 1)),
+                Arguments.of(new Point2D.Double(0.5, 0.7603945765026368), new Point2D.Double(CAMERA_PLANE_WIDTH / 2d, CAMERA_PLANE_HEIGHT - 1), new Vector3D(0, 0, 1)),
+                Arguments.of(new Point2D.Double(0.5, 0.239075212564189), new Point2D.Double(CAMERA_PLANE_WIDTH / 2d, 0), new Vector3D(0, 0, 1)));
+    }
+
+    @ParameterizedTest
+    @MethodSource
+    void testMapPoint(final Point2D.Double expectedScaled, final Point2D.Double initial, final Vector3D rotation) {
+        this.cameraPlane.setRotation(rotation);
+        final Point2D.Double actual = this.cameraPlane.mapPoint(initial.x, initial.y);
+        assertAll(() -> assertEquals(expectedScaled.x, actual.x, MAX_DELTA, "X"),
+                () -> assertEquals(expectedScaled.y, actual.y, MAX_DELTA, "Y"));
+        final Point2D.Double intActual = this.cameraPlane.mapPoint((int) initial.x, (int) initial.y);
+        assertAll(() -> assertEquals(expectedScaled.x, intActual.x, MAX_DELTA, "X"),
+                () -> assertEquals(expectedScaled.y, intActual.y, MAX_DELTA, "Y"));
     }
 
     @Test
-    @Disabled("Currently broken")
     void testGetVector3D() {
         Vector3D vec = new Vector3D(0, 0, 1);
-        cameraPlane.setRotation(vec);
-        Vector3D out = cameraPlane.getVector3D(new Point(CAMERA_PLANE_WIDTH / 2, CAMERA_PLANE_HEIGHT / 2));
-        assertAll(() -> assertEquals(0.0, out.getX(), 1.0E-04), () -> assertEquals(0.0, out.getY(), 1.0E-04),
-            () -> assertEquals(1.0, out.getZ(), 1.0E-04));
+        this.cameraPlane.setRotation(vec);
+        Vector3D out = this.cameraPlane.getVector3D(new Point(CAMERA_PLANE_WIDTH / 2, CAMERA_PLANE_HEIGHT / 2));
+        assertAll(() -> assertEquals(0.0, out.getX(), 1.0E-04, "X"),
+                () -> assertEquals(0.0, out.getY(), 1.0E-04, "Y"),
+            () -> assertEquals(1.0, out.getZ(), 1.0E-04, "Z"));
     }
 
     static Stream<Arguments> testGetVector3DFloat() {
@@ -63 +113 @@
     @ParameterizedTest
     @MethodSource
     void testGetVector3DFloat(final Vector3D expected, final Point toCheck) {
-        Vector3D out = cameraPlane.getVector3D(toCheck.getX(), toCheck.getY());
-        assertAll(() -> assertEquals(expected.getX(), out.getX(), 1.0E-04),
-            () -> assertEquals(expected.getY(), out.getY(), 1.0E-04),
-            () -> assertEquals(expected.getZ(), out.getZ(), 1.0E-04), () -> assertEquals(1,
-                Math.sqrt(Math.pow(out.getX(), 2) + Math.pow(out.getY(), 2) + Math.pow(out.getZ(), 2)), 1.0E-04));
+        Vector3D out = this.cameraPlane.getVector3D(toCheck.getX(), toCheck.getY());
+        assertAll(() -> assertEquals(expected.getX(), out.getX(), 1.0E-04, "X"),
+            () -> assertEquals(expected.getY(), out.getY(), 1.0E-04, "Y"),
+            () -> assertEquals(expected.getZ(), out.getZ(), 1.0E-04, "Z"),
+                () -> assertEquals(1, Math.sqrt(Math.pow(out.getX(), 2) + Math.pow(out.getY(), 2) + Math.pow(out.getZ(), 2)),
+                        1.0E-04, "Radial distance"));
     }
 
     @Test
-    @Disabled("Currently broken")
     void testMapping() {
         Vector3D vec = new Vector3D(0, 0, 1);
-        cameraPlane.setRotation(vec);
-        Vector3D out = cameraPlane.getVector3D(new Point(300, 200));
+        this.cameraPlane.setRotation(vec);
+        Vector3D out = this.cameraPlane.getVector3D(new Point(300, 200));
         Point2D map = UVMapping.getTextureCoordinate(out);
-        assertAll(() -> assertEquals(0.44542099, map.getX(), 1e-8), () -> assertEquals(0.39674936, map.getY(), 1e-8));
+        assertAll(() -> assertEquals(0.44542099, map.getX(), 1e-8, "X"),
+                () -> assertEquals(0.39674936, map.getY(), 1e-8, "Y"));
     }
 }

test/unit/org/openstreetmap/josm/gui/util/imagery/Vector3DTest.java

@@ -3 +3 @@
 
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.fail;
 
 import java.util.stream.Stream;
 
-import org.junit.jupiter.api.Disabled;
+import nl.jqno.equalsverifier.EqualsVerifier;
+import org.junit.jupiter.api.Test;
 import org.junit.jupiter.params.ParameterizedTest;
 import org.junit.jupiter.params.provider.Arguments;
 import org.junit.jupiter.params.provider.MethodSource;
@@ -18 +18 @@
  */
 class Vector3DTest {
 
+    private static final double MAX_DELTA = 1E-14;
+
     static Stream<Arguments> vectorInformation() {
         return Stream.of(
-            Arguments.of(0, 0, 0, 0),
-            Arguments.of(1, 1, 1, Math.sqrt(3)),
-            Arguments.of(-1, -1, -1, Math.sqrt(3)),
-            Arguments.of(-2, 2, -2, Math.sqrt(12))
+            // (x, y, z, radialDistance, polarAngle, azimuthAngle)
+            Arguments.of(0, 0, 0, 0, 0, 0),
+            Arguments.of(1, 1, 1, Math.sqrt(3), 0.9553166181245092, Math.PI / 4),
+            Arguments.of(-1, -1, -1, Math.sqrt(3), 2.1862760354652844, -3 * Math.PI / 4),
+            Arguments.of(-2, 2, -2, Math.sqrt(12), 2.1862760354652844, 3 * Math.PI / 4)
         );
     }
 
@@ -52 +55 @@
     @MethodSource("vectorInformation")
     void getRadialDistance(final double x, final double y, final double z, final double radialDistance) {
         final Vector3D vector3D = new Vector3D(x, y, z);
-        assertEquals(radialDistance, vector3D.getRadialDistance());
+        assertEquals(radialDistance, vector3D.getRadialDistance(), MAX_DELTA);
     }
 
     @ParameterizedTest
     @MethodSource("vectorInformation")
-    @Disabled("Angle calculations may be corrected")
-    void getPolarAngle() {
-        fail("Not yet implemented");
+    void getPolarAngle(final double x, final double y, final double z, final double ignoredRadialDistance, final double polarAngle) {
+        final Vector3D vector3D = new Vector3D(x, y, z);
+        assertEquals(polarAngle, vector3D.getPolarAngle(), MAX_DELTA);
     }
 
     @ParameterizedTest
     @MethodSource("vectorInformation")
-    @Disabled("Angle calculations may be corrected")
-    void getAzimuthalAngle() {
-        fail("Not yet implemented");
+    void getAzimuthalAngle(final double x, final double y, final double z,
+            final double ignoredRadialDistance, final double ignoredPolarAngle, final double azimuthAngle) {
+        final Vector3D vector3D = new Vector3D(x, y, z);
+        assertEquals(azimuthAngle, vector3D.getAzimuthalAngle(), MAX_DELTA);
     }
 
     @ParameterizedTest
@@ -81 +85 @@
 
     @ParameterizedTest
     @MethodSource("vectorInformation")
-    @Disabled("Angle calculations may be corrected")
-    void testToString() {
-        fail("Not yet implemented");
+    void testConstructorArgs(final double x, final double y, final double z) {
+        final Vector3D vector3D = new Vector3D(x, y, z);
+        final Vector3D vector3DRPA = new Vector3D(Vector3D.VectorType.RPA, vector3D.getRadialDistance(),
+                vector3D.getPolarAngle(), vector3D.getAzimuthalAngle());
+        final Vector3D vector3DRAP = new Vector3D(Vector3D.VectorType.RAP, vector3D.getRadialDistance(),
+                vector3D.getAzimuthalAngle(), vector3D.getPolarAngle());
+        assertAll(() -> assertEquals(vector3D.getX(), vector3DRPA.getX(), MAX_DELTA, "RPA X"),
+                () -> assertEquals(vector3D.getY(), vector3DRPA.getY(), MAX_DELTA, "RPA Y"),
+                () -> assertEquals(vector3D.getZ(), vector3DRPA.getZ(), MAX_DELTA, "RPA Z"),
+                () -> assertEquals(vector3D.getX(), vector3DRAP.getX(), MAX_DELTA, "RAP X"),
+                () -> assertEquals(vector3D.getY(), vector3DRAP.getY(), MAX_DELTA, "RAP Y"),
+                () -> assertEquals(vector3D.getZ(), vector3DRAP.getZ(), MAX_DELTA, "RAP Z"));
+    }
+
+    @ParameterizedTest
+    @MethodSource("vectorInformation")
+    void testToString(final double x, final double y, final double z,
+            final double radialDistance, final double polarAngle, final double azimuthAngle) {
+        final Vector3D vector3D = new Vector3D(x, y, z);
+        assertEquals("[x=" + x + ", y=" + y + ", z=" + z + ", r=" + radialDistance + ", inclination="
+                + polarAngle + ", azimuthal=" + azimuthAngle + "]", vector3D.toString());
+    }
+
+    @Test
+    void testEqualsVerifier() {
+        EqualsVerifier.forClass(Vector3D.class)
+                // These fields are all dependendent upon x, y, and z, and should always evaluate to the same numbers
+                .withIgnoredFields("radialDistance",
+                        "polarAngle", "polarAngleCos", "polarAngleSin",
+                        "azimuthalAngle", "azimuthalAngleCos", "azimuthalAngleSin")
+                .verify();
     }
 }