Added some unit tests for the ScanMatcher and fixed some broken functionality.

tests/ScanGraph/ScanMatcherTest.java

@@ -1,5 +1,6 @@
 package ScanGraph;
 
+import org.ejml.simple.SimpleMatrix;
 import org.junit.jupiter.api.Test;
 import Vector.Vector;
 
@@ -16,32 +17,29 @@ class ScanMatcherTest {
      * @param scanDescription A vector which describes the length of the line and direction of the line
      * @return A scan point with the given offset and scan description
      */
-    ScanPoint generateScanPoint(Vector offset, Vector scanDescription){
+    ScanPoint generateScanPoint(Vector offset, Vector scanDescription, int numPoints){
         // generate a scan point with the given offset and scan description
-        Vector scanPosition = new Vector(0, 0);
         ArrayList<Vector> scan = new ArrayList<>();
 
-        // calculate the total number of points in the scan
-        int numPoints = (int) scanDescription.mag();
-        // calculate the slope of the line the scan is on
-        float m = scanDescription.y / scanDescription.x;
+        // divide the scan description by the number of points to allow us to scale it back up in the loop
+        Vector directionVector = scanDescription.div(numPoints-1);
 
-        // add the points to the scan
-        for(int i = 0; i < numPoints; i++){
-            float x = i;
-            float y = m * x;
-            scan.add(new Vector(x + offset.x, y + offset.y));
+        for (int i = 0; i < numPoints; i++) {
+            scan.add(offset.add(directionVector.mul(i)));
         }
 
-        return new ScanPoint(scanPosition, 0, scan);
+        return new ScanPoint(new Vector(0, 0), 0, scan);
     }
 
     @Test
     void applyRotationAndTranslationMatrices() {
         // generate one scan that is level and another that is rotated 45 degrees.
         Vector scanDescription = new Vector(10, 0);
-        ScanPoint referenceScan = generateScanPoint(new Vector(0, 0), scanDescription);
-        ScanPoint newScan = generateScanPoint(new Vector(0, 0), scanDescription.rotate2D((float) Math.PI / 4));
+        ScanPoint referenceScan = generateScanPoint(new Vector(0, 0), scanDescription, 10);
+        ScanPoint newScan = generateScanPoint(new Vector(0, 0), scanDescription.rotate2D((float) Math.PI / 4), 10);
+
+        Vector test = scanDescription.rotate2D((float) Math.PI / 4);
+        float mag = test.mag();
 
         // calculate the rotation and translation matrices between the two scans
         ScanMatcher matcher = new ScanMatcher();
@@ -53,7 +51,8 @@ class ScanMatcherTest {
         ArrayList<Vector> points = newScanWithRotationAndTranslation.getPoints();
         Vector firstPoint = points.get(0);
         Vector lastPoint = points.get(points.size() - 1);
-        float angle = firstPoint.angleDiff(lastPoint);
+        Vector rotatedDirection = lastPoint.sub(firstPoint);
+        float angle = scanDescription.angleDiff(rotatedDirection);
 
         // The angle between the first and last points should be zero
         assertEquals(0, angle);
@@ -62,10 +61,15 @@ class ScanMatcherTest {
     @Test
     void getError() {
         // generate two scans that are the same. The error should be zero.
-        ScanPoint scan1 = generateScanPoint(new Vector(0, 0), new Vector(10, 10));
-        ScanPoint scan2 = generateScanPoint(new Vector(0, 0), new Vector(10, 10));
+        ScanPoint scan1 = generateScanPoint(new Vector(0, 0), new Vector(10, 10), 12);
+        ScanPoint scan2 = generateScanPoint(new Vector(0, 0), new Vector(10, 10), 12);
         ScanMatcher matcher = new ScanMatcher();
         matcher.calculateRotationAndTranslationMatrices(scan1, scan2);
         assertEquals(0, matcher.getError(scan1, scan2));
     }
+
+    @Test
+    void iterativeScanMatch() {
+        // TODO: Write a test for this
+    }
 }