Working on debugging line of best fit code.

src/Line.java

@@ -2,35 +2,31 @@ import processing.core.PApplet;
 
 import java.util.List;
 
-import static processing.core.PApplet.abs;
-import static processing.core.PApplet.pow;
+import static processing.core.PApplet.*;
 
 public class Line{
-    Vector direction = new Vector(0,0);
-    Vector position = new Vector(0,0);
-    float length = 0;
-    private static PApplet proc;
+    private Vector direction = new Vector(0,0);
+    private Vector position = new Vector(0,0);
+    private float length = 0;
 
-    Line(PApplet processing, Vector startPosition, Vector endPosition){
-        this.proc = processing;
-        this.position = startPosition;
-        this.direction = endPosition.sub(startPosition).normalize();
-        this.length = direction.mag();
+    Line(Vector startPosition, Vector endPosition){
+        direction = endPosition.sub(startPosition);
+        position = startPosition;
+        length = direction.mag();
+        direction = direction.normalize();
     }
-    Line(PApplet processing, Vector direction, Vector position, float lineLength){
+    Line(Vector direction, Vector position, float lineLength){
         this.direction = direction.normalize();
         this.position = position;
         this.length = lineLength;
-        proc = processing;
     }
 
     /**
      * attempt to find the line of best fit for the given points
      * @param points the points to get the line of best for
      */
-    Line(PApplet processing, List<Vector> points){
+    Line(List<Vector> points){
         bestFit(points);
-        proc = processing;
     }
 
     // least squares line of best fit algorithm
@@ -57,7 +53,12 @@ public class Line{
             }
 
         }
-
+        if(direction.y == 0){
+            this.direction = new Vector(0, 1);
+        }
+        else{
+            this.direction = new Vector(1, direction.y/direction.x);
+        }
         this.position = mean.sub(direction.div(direction.mag()).mul(this.length / 2));
         this.direction = direction.normalize();
     }
@@ -80,12 +81,9 @@ public class Line{
         return length;
     }
 
-    /**
-     * Draw the line on screen
-     */
-    public void draw(){
-        Vector endPoint = position.add(direction.mul(length));
-        proc.line(position.x, position.y, endPoint.x, endPoint.y);
+    public void draw(PApplet screen){
+        Vector endPoint = this.position.add(this.direction.mul(this.length));
+        screen.line(position.x, position.y, endPoint.x, endPoint.y);
     }
 
     /**

src/SLAM.java

@@ -47,7 +47,7 @@ public class SLAM{
      */
     private void extractFeature(ArrayList<Vector> originalList, List<Vector> randomSample, float maxRange, int consensus){
         // get a line of best fit for this list.
-        Line bestFit = new Line(proc, randomSample);
+        Line bestFit = new Line(randomSample);
         int count = 0;
         ArrayList<Vector> newRandomSample = new ArrayList<>();
         for (Vector v : randomSample) {
@@ -58,7 +58,7 @@ public class SLAM{
         }
         // if the count is above the consensus, add the line to our list and remove the points that gave the consensus.
         if (count >= consensus) {
-            bestFit = new Line(proc, newRandomSample.subList(0, newRandomSample.size() - 1));
+            bestFit = new Line(newRandomSample.subList(0, newRandomSample.size() - 1));
             lines.add(bestFit);
             // remove the associated readings from the total available readings.
             for (Vector v : newRandomSample) {
@@ -101,7 +101,7 @@ public class SLAM{
 
     public void drawLines(){
         for(Line line : lines){
-            line.draw();
+            line.draw(proc);
         }
     }
 

tests/LineTest.java

@@ -0,0 +1,57 @@
+import org.junit.jupiter.api.Test;
+import processing.core.PApplet;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import static java.lang.Math.abs;
+import static org.junit.jupiter.api.Assertions.*;
+
+class LineTest{
+    public void assertFloatEquals(float expected, float actual){
+        assertFloatEquals(expected, actual, (float)0.0001);
+    }
+
+    public void assertFloatEquals(float expected, float actual, float range){
+        assertTrue(abs(expected-actual) < range);
+    }
+    @Test
+    public void P2PLine(){
+        Vector v1 = new Vector(10, 0);
+        Vector v2 = new Vector(10, 10);
+        Line line = new Line(v1, v2);
+
+        // make sure the line is pointing in the right direction
+        Vector lineDirection = v2.sub(v1);
+        assertFloatEquals(lineDirection.angleDiff(line.getDirection()), 0);
+
+        // make sure the line is the correct length
+        assertFloatEquals(lineDirection.mag(), line.getLength());
+
+        // make sure the line is starting in the correct place
+        assertFloatEquals(v1.x, line.getPosition().x);
+        assertFloatEquals(v1.y, line.getPosition().y);
+    }
+
+    @Test
+    public void bestFitLine(){
+        Vector v1 = new Vector(10, 0);
+        Vector v2 = new Vector(10, 10);
+        ArrayList<Vector> points = new ArrayList<>();
+        points.add(v1);
+        points.add(v2);
+        Line line = new Line(points);
+
+        // make sure the line is pointing in the right direction
+        Vector lineDirection = v2.sub(v1);
+        assertFloatEquals(lineDirection.angleDiff(line.getDirection()), 0);
+
+        // make sure the line is the correct length
+        assertFloatEquals(lineDirection.mag(), line.getLength());
+
+        // make sure the line is starting in the correct place
+        assertFloatEquals(v1.x, line.getPosition().x);
+        assertFloatEquals(v1.y, line.getPosition().y);
+    }
+
+}