From f12e3362299539fcdffb0d7d849685c15c105fe5 Mon Sep 17 00:00:00 2001 From: Quinn Date: Thu, 6 Apr 2023 11:34:02 -0500 Subject: [PATCH] Working on debugging line of best fit code. --- src/Line.java | 42 ++++++++++++++++----------------- src/SLAM.java | 6 ++--- tests/LineTest.java | 57 +++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 80 insertions(+), 25 deletions(-) create mode 100644 tests/LineTest.java diff --git a/src/Line.java b/src/Line.java index 22b7738..d573874 100644 --- a/src/Line.java +++ b/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 points){ + Line(List 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); } /** diff --git a/src/SLAM.java b/src/SLAM.java index 449f137..3c5b443 100644 --- a/src/SLAM.java +++ b/src/SLAM.java @@ -47,7 +47,7 @@ public class SLAM{ */ private void extractFeature(ArrayList originalList, List 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 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); } } diff --git a/tests/LineTest.java b/tests/LineTest.java new file mode 100644 index 0000000..089ed66 --- /dev/null +++ b/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 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); + } + +} \ No newline at end of file