Wrote tests for the Vector class

SLAM-Sim.iml

@@ -4,6 +4,7 @@
     <exclude-output />
     <content url="file://$MODULE_DIR$">
       <sourceFolder url="file://$MODULE_DIR$/src" isTestSource="false" />
+      <sourceFolder url="file://$MODULE_DIR$/tests" isTestSource="true" />
     </content>
     <orderEntry type="inheritedJdk" />
     <orderEntry type="sourceFolder" forTests="false" />
@@ -16,5 +17,31 @@
         <SOURCES />
       </library>
     </orderEntry>
+    <orderEntry type="module-library" scope="TEST">
+      <library name="JUnit4">
+        <CLASSES>
+          <root url="jar://$MODULE_DIR$/lib/junit-4.13.1.jar!/" />
+          <root url="jar://$MODULE_DIR$/lib/hamcrest-core-1.3.jar!/" />
+        </CLASSES>
+        <JAVADOC />
+        <SOURCES />
+      </library>
+    </orderEntry>
+    <orderEntry type="module-library" scope="TEST">
+      <library name="JUnit5.8.1">
+        <CLASSES>
+          <root url="jar://$MODULE_DIR$/lib/junit-jupiter-5.8.1.jar!/" />
+          <root url="jar://$MODULE_DIR$/lib/junit-jupiter-api-5.8.1.jar!/" />
+          <root url="jar://$MODULE_DIR$/lib/opentest4j-1.2.0.jar!/" />
+          <root url="jar://$MODULE_DIR$/lib/junit-platform-commons-1.8.1.jar!/" />
+          <root url="jar://$MODULE_DIR$/lib/apiguardian-api-1.1.2.jar!/" />
+          <root url="jar://$MODULE_DIR$/lib/junit-jupiter-params-5.8.1.jar!/" />
+          <root url="jar://$MODULE_DIR$/lib/junit-jupiter-engine-5.8.1.jar!/" />
+          <root url="jar://$MODULE_DIR$/lib/junit-platform-engine-1.8.1.jar!/" />
+        </CLASSES>
+        <JAVADOC />
+        <SOURCES />
+      </library>
+    </orderEntry>
   </component>
 </module>

src/Car.java

@@ -3,11 +3,10 @@ import java.util.ArrayList;
 import static java.lang.Math.PI;
 import static processing.core.PApplet.degrees;
 import static processing.core.PApplet.radians;
-import processing.core.PVector;
 import processing.core.PApplet;
 
 public class Car{
-    PVector pose = new PVector(0,0); // the car's x, y position
+    Vector pose = new Vector(0,0); // the car's x, y position
     float angle = 0; // the current angle that the car is at.
     int carLength = 50;
     int carWidth = 40;
@@ -25,7 +24,7 @@ public class Car{
     Car(PApplet processing, int xPos, int yPos, int carLength, int carWidth){
         proc = processing;
         slam = new SLAM(proc);
-        this.pose = new PVector(xPos, yPos);
+        this.pose = new Vector(xPos, yPos);
         this.carLength = carLength;
         this.carWidth = carWidth;
     }
@@ -40,6 +39,7 @@ public class Car{
         proc.stroke(255);
         proc.ellipse(pose.x, pose.y, carWidth, carLength);
         this.updateScan(walls);
+        this.slam.drawLines();
     }
 
     //With all the views that the car has, get their point list
@@ -49,12 +49,12 @@ public class Car{
         }
 
         for(View view : views){
-            ArrayList<PVector> pointList = view.getPoints();
+            ArrayList<Vector> pointList = view.getPoints();
             slam.RANSAC(pointList, view.getFOV() / view.getRayNum());
         }
     }
 
-    public PVector getPose(){
+    public Vector getPose(){
         return pose;
     }
 
@@ -75,7 +75,7 @@ public class Car{
         }
     }
 
-    public void setPose(PVector newPose){
+    public void setPose(Vector newPose){
         pose = newPose;
         for(View view : views){
             view.setPos(pose);

src/Line.java

@@ -0,0 +1,98 @@
+import processing.core.PApplet;
+
+import java.util.List;
+
+import static processing.core.PApplet.abs;
+import static processing.core.PApplet.pow;
+
+public class Line{
+    Vector direction = new Vector(0,0);
+    Vector position = new Vector(0,0);
+    float length = 0;
+    private static PApplet proc;
+
+    Line(PApplet processing, Vector startPosition, Vector endPosition){
+        this.proc = processing;
+        this.position = startPosition;
+        this.direction = endPosition.sub(startPosition).normalize();
+        this.length = direction.mag();
+    }
+    Line(PApplet processing, 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){
+        bestFit(points);
+        proc = processing;
+    }
+
+    // least squares line of best fit algorithm
+    private void bestFit(List<Vector> points){
+        // get the mean of all the points
+        Vector mean = new Vector();
+        for(Vector point : points){
+            mean.add(point);
+        }
+        mean.div(points.size());
+
+        // this section calculates the direction vector of the line of best fit
+        Vector direction = new Vector();
+
+        // get the rise and run of the line of best fit
+        for(Vector point : points){
+            direction.y += (point.x - mean.x)*(point.y - mean.y); // rise
+            direction.x += pow((point.x - mean.x),2);
+
+            // find the point that's furthest from the mean and use it to set the line length.
+            float dist = abs(point.sub(mean).mag());
+            if(dist > this.length){
+                this.length = 2*dist;
+            }
+
+        }
+
+        this.position = mean.sub(direction.div(direction.mag()).mul(this.length / 2));
+        this.direction = direction.normalize();
+    }
+
+    public Vector getSlopeIntForm(){
+        float slope = direction.y / direction.x;
+        float intercept = position.y - slope * position.x;
+        return new Vector(slope, intercept);
+    }
+
+    public Vector getDirection(){
+        return direction;
+    }
+
+    public Vector getPosition(){
+        return position;
+    }
+
+    public float getLength(){
+        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);
+    }
+
+    /**
+     * @param point
+     * @return the smallest distance from the point to this line
+     */
+    public float getDistance(Vector point){
+        return (point.sub(position).cross(direction)).mag() / direction.mag();
+    }
+}

src/Processing.java

@@ -1,5 +1,4 @@
 import processing.core.PApplet;
-import processing.core.PVector;
 
 import java.util.ArrayList;
 
@@ -20,15 +19,15 @@ public class Processing extends PApplet {
         size(1000, 1000);
         car.addView(180,180);
         for(int i = 0; i < 20; i++){
-            Wall wall = new Wall(processing, new PVector((int)random(40, 1840), (int)random(40, 960)), (int)random(360), (int)random(100, 1000));
+            Wall wall = new Wall(processing, new Vector((int)random(40, 1840), (int)random(40, 960)), (int)random(360), (int)random(100, 1000));
             objects.add(wall);
         }
     }
     public void draw(){
         background(0);
-        for(Wall object : objects){
-            object.drawWall();
-        }
+//        for(Wall object : objects){
+//            object.drawWall();
+//        }
         car.drawCar(objects);
         //car.drive(new int[] {0, 0});
     }

src/SLAM.java

@@ -1,11 +1,10 @@
-import static processing.core.PApplet.radians;
 import processing.core.*;
 
 import java.util.ArrayList;
 import java.util.Collections;
 import java.util.List;
 
-import static processing.core.PApplet.pow;
+import static processing.core.PApplet.*;
 
 public class SLAM{
     ArrayList<Line> lines = new ArrayList<>();
@@ -21,15 +20,18 @@ public class SLAM{
      * @param subSampleSize the size of the sub sample
      * @return A random subset of the set within an indexRange and of size: subSampleSize
      */
-    private List<PVector> randomSample(ArrayList<PVector> set, int indexRange, int subSampleSize){
+    private List<Vector> randomSample(ArrayList<Vector> set, int indexRange, int subSampleSize){
         // select a random laser data reading
         int randomIdx = (int) proc.random(set.size() - 1); // index of starter reading
-        PVector point = set.get(randomIdx); // point of starter reading
+        Vector point = set.get(randomIdx); // point of starter reading
 
         // get a random sample of size numSampleReadings within degreeRange degrees of this laser reading.
-        List<PVector> subSample = set.subList(randomIdx - indexRange, randomIdx + indexRange); // get the sub-sample
+        List<Vector> subSample;
+        int rangeStart = randomIdx - indexRange >= 0 ? randomIdx - indexRange : 0;
+        int rangeEnd = randomIdx + indexRange < set.size() ? randomIdx + indexRange : set.size()-1;
+        subSample = set.subList(rangeStart, rangeEnd); // get the sub-sample
         Collections.shuffle(subSample); // shuffle the list
-        List<PVector> randomSample = subSample.subList(0, subSampleSize); // get our random sample
+        List<Vector> randomSample = subSample.subList(0, rangeEnd-rangeStart); // get our random sample
         if (!randomSample.contains(point)) {
             randomSample.add(point);
         }
@@ -43,12 +45,12 @@ public class SLAM{
      * @param maxRange the maximum distance away from the line of best fit of the subSample of points for a given point's consensus to count.
      * @param consensus the number of points that have to give their consensus for the line of best fit to count as a valid feature.
      */
-    private void extractFeature(ArrayList<PVector> originalList, List<PVector> randomSample, float maxRange, int consensus){
+    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);
         int count = 0;
-        ArrayList<PVector> newRandomSample = new ArrayList<>();
-        for (PVector v : randomSample) {
+        ArrayList<Vector> newRandomSample = new ArrayList<>();
+        for (Vector v : randomSample) {
             if (bestFit.getDistance(v) <= maxRange) {
                 count++;
                 newRandomSample.add(v);
@@ -59,12 +61,17 @@ public class SLAM{
             bestFit = new Line(proc, newRandomSample.subList(0, newRandomSample.size() - 1));
             lines.add(bestFit);
             // remove the associated readings from the total available readings.
-            for (PVector v : newRandomSample) {
+            for (Vector v : newRandomSample) {
                 originalList.remove(v);
             }
         }
     }
-    public void RANSAC(ArrayList<PVector> newPoints, float raysPerDegree){
+
+    /**
+     * @param newPoints a new scan of points to perform feature detection on
+     * @param raysPerDegree How many degrees apart are each ray that was cast
+     */
+    public void RANSAC(ArrayList<Vector> newPoints, float raysPerDegree){
         float degreeRange = radians(10/2); // range to randomly sample readings within
         int indexRange = (int) (degreeRange / raysPerDegree);
         int numSampleReadings = 10; // number of readings to randomly sample
@@ -83,7 +90,7 @@ public class SLAM{
             }
 
             // get a random sub sample of newPoints within the index range of a given size
-            List<PVector> randomSample = this.randomSample(newPoints, indexRange, numSampleReadings);
+            List<Vector> randomSample = this.randomSample(newPoints, indexRange, numSampleReadings);
 
             // check if the sub sample forms a valid line and remove the randomSample points if it does.
             extractFeature(newPoints, randomSample, maxRange, consensus);
@@ -92,71 +99,10 @@ public class SLAM{
 
     }
 
-}
-
-class Line{
-    PVector direction = new PVector(0,0);
-    PVector position = new PVector(0,0);
-    private static PApplet proc;
-
-    Line(PApplet processing){
-        proc = processing;
-    }
-    Line(PApplet processing, PVector direction, PVector position){
-        this.direction = direction;
-        this.position = position;
-        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<PVector> points){
-        bestFit(points);
-        proc = processing;
-    }
-
-    // least squares line of best fit algorithm
-    private void bestFit(List<PVector> points){
-        // get the mean of all the points
-        PVector mean = new PVector();
-        for(PVector point : points){
-            mean.add(point);
-        }
-        mean.div(points.size());
-
-        // this section calculates the direction vector of the line of best fit
-        PVector direction = new PVector();
-        // get the rise and run of the line of best fit
-        for(PVector point : points){
-            direction.y += (point.x - mean.x)*(point.y - mean.y); // rise
-            direction.x += pow((point.x - mean.x),2);
-        }
-
-        this.position = mean;
-        this.direction = direction;
-    }
-
-    public PVector getSlopeIntForm(){
-        float slope = direction.y / direction.x;
-        float intercept = position.y - slope * position.x;
-        return new PVector(slope, intercept);
-    }
-
-    public PVector getDirection(){
-        return direction;
-    }
-
-    public PVector getPosition(){
-        return position;
-    }
-
-    /**
-     * @param point
-     * @return the smallest distance from the point to this line
-     */
-    public float getDistance(PVector point){
-        return (point.sub(position).cross(direction)).mag() / direction.mag();
-    }
+    public void drawLines(){
+        for(Line line : lines){
+            line.draw();
+        }
+    }
+
 }

src/Vector.java

@@ -0,0 +1,68 @@
+import static java.lang.Math.sqrt;
+
+public class Vector {
+    public float x = 0;
+    public float y = 0;
+    public float z = 0;
+
+    Vector(){}
+    Vector(float x, float y){
+        this.x = x;
+        this.y = y;
+    }
+    
+    Vector(float x, float y, float z){
+        this.x = x;
+        this.y = y;
+        this.z = z;
+    }
+    
+    Vector add(Vector other){
+        return new Vector(this.x + other.x, this.y + other.y, this.z + other.z);
+    }
+
+    Vector add(float x, float y){
+        return new Vector(this.x + x, this.y + y);
+    }
+
+    Vector add(float x, float y, float z){
+        return new Vector(this.x + x, this.y + y, this.z + z);
+    }
+    
+    Vector sub(Vector other){
+        return new Vector(this.x - other.x, this.y - other.y, this.z - other.z);
+    }
+
+    Vector sub(float x, float y){
+        return new Vector(this.x - x, this.y - y);
+    }
+
+    Vector sub(float x, float y, float z){
+        return new Vector(this.x - x, this.y - y, this.z - z);
+    }
+    
+    Vector mul(float scalar){
+        return new Vector(this.x * scalar, this.y * scalar, this.z * scalar);
+    }
+    
+    Vector div(float scalar){
+        return mul(1/scalar);
+    }
+    
+    float mag(){
+        return (float)sqrt(x*x + y*y + z*z);
+    }
+
+    float dot(Vector other){
+        return x * other.x + y * other.y + z * other.z;
+    }
+
+    Vector cross(Vector other){
+        return new Vector(this.y*other.z - this.z*other.y, this.z*other.x - this.x*other.z, this.x*other.y - this.y*other.x);
+    }
+
+    Vector normalize(){
+        float mag = this.mag();
+        return new Vector(x / mag, y / mag, z / mag);
+    }
+}

src/View.java

@@ -5,14 +5,14 @@ import java.util.Objects;
 import static processing.core.PApplet.*;
 
 public class View{
-    PVector pose;
+    Vector pose;
     float angle = 0;
     float FOV;
     ArrayList<Ray> rays = new ArrayList<>();
     private static PApplet proc;
 
     //the x,y position of the view, what angle it's looking at and its FOV
-    View(PApplet processing, PVector newPose, int numberOfRays, float FOV){
+    View(PApplet processing, Vector newPose, int numberOfRays, float FOV){
         proc = processing;
         this.pose = newPose;
         this.FOV = FOV;
@@ -23,7 +23,7 @@ public class View{
     public void setRayNum(int numberOfRays, float FOV, float angleOffset){
         float rayStep = FOV/numberOfRays;
         rays.clear();
-        float angle = (float) (0.01-angleOffset); //the 0.01 fixes some bugs
+        float angle =  (float)(0.01-angleOffset); //the 0.01 fixes some bugs
         for(int i = 0; i < numberOfRays; i++){
             Ray ray = new Ray(proc, pose, 100000, angle);
             angle = angle + rayStep;
@@ -40,7 +40,7 @@ public class View{
     }
 
     //changes the position of the view
-    public void setPos(PVector newPose){
+    public void setPos(Vector newPose){
         pose = newPose;
         for(Ray ray : rays){ray.setPos(pose);}
     }
@@ -57,7 +57,7 @@ public class View{
         this.setRayNum(this.rays.size(), this.FOV, this.angle);
     }
 
-    public PVector getPos(){return pose;}
+    public Vector getPos(){return pose;}
 
     public float getAngle(){return this.angle;}
 
@@ -66,11 +66,11 @@ public class View{
     public int getRayNum(){return this.rays.size();}
 
     //gets the point that each ray has collided with
-    public ArrayList<PVector> getPoints(){
-        ArrayList<PVector> points = new ArrayList<>();
+    public ArrayList<Vector> getPoints(){
+        ArrayList<Vector> points = new ArrayList<>();
 
         for(Ray ray : rays){
-            if(!Objects.equals(ray.getPoint(), new PVector(0, 0) {
+            if(!Objects.equals(ray.getPoint(), new Vector(0, 0) {
             })){
                 points.add(ray.getPoint());
             }
@@ -80,14 +80,14 @@ public class View{
 }
 
 class Ray{
-    PVector pose;
+    Vector pose;
     int rayLength;
     int defaultRayLength;
     float angle; // IN RADIANS
     private static PApplet proc;
 
     //takes the starting position of the ray, the length of the ray, and it's casting angle (radians)
-    Ray(PApplet processing, PVector position, int defaultRayLength, float angle){
+    Ray(PApplet processing, Vector position, int defaultRayLength, float angle){
         proc = processing;
         this.pose = position;
         this.defaultRayLength = defaultRayLength;
@@ -107,7 +107,7 @@ class Ray{
         for(Wall object : objects){
             float theta1 = angle;
             float theta2 = radians(object.getAngle());
-            PVector wallPos = object.getPos();
+            Vector wallPos = object.getPos();
 
             //finds where along the wall the ray collides
             float b = (pose.x*sin(theta1) + wallPos.y*cos(theta1) - pose.y*cos(theta1) - wallPos.x*sin(theta1)) / (cos(theta2)*sin(theta1) - sin(theta2)*cos(theta1));
@@ -134,7 +134,7 @@ class Ray{
         else this.rayLength = defaultRayLength;
     }
 
-    public PVector getPos(){ return pose;}
+    public Vector getPos(){ return pose;}
 
     public int getRayLength(){return this.rayLength;}
 
@@ -145,16 +145,16 @@ class Ray{
     }
 
     //returns the absolute position of the point
-    public PVector getPoint(){
+    public Vector getPoint(){
         if(this.rayLength != this.defaultRayLength){
-            return new PVector(rayLength * (int)cos(this.angle) + pose.x, rayLength * (int)sin(this.angle) + pose.y);
+            return new Vector(rayLength * (int)cos(this.angle) + pose.x, rayLength * (int)sin(this.angle) + pose.y);
         }
         else{
-            return new PVector(0,0);
+            return new Vector(0,0);
         }
     }
 
-    public void setPos(PVector newPose){
+    public void setPos(Vector newPose){
         pose = newPose;
     }
 

src/Wall.java

@@ -3,7 +3,7 @@ import processing.core.*;
 import static processing.core.PApplet.*;
 
 public class Wall{
-    PVector pos;
+    Vector pos;
     float angle;
     int wallLength;
     private static PApplet proc;
@@ -11,7 +11,7 @@ public class Wall{
     int g;
     int b;
 
-    Wall(PApplet processing, PVector pos, float angle, int wallLength){
+    Wall(PApplet processing, Vector pos, float angle, int wallLength){
         proc = processing;
         this.pos = pos;
         this.angle = angle;
@@ -27,7 +27,7 @@ public class Wall{
         //ellipse((xPos + cos(radians(angle))*wallLength), (yPos + sin(radians(angle))*wallLength), 20, 20);
     }
 
-    PVector getPos(){
+    Vector getPos(){
         return pos;
     }
 

tests/VectorTest.java

@@ -0,0 +1,84 @@
+import org.junit.jupiter.api.Test;
+import processing.core.PApplet;
+
+import static java.lang.Math.*;
+import static org.junit.jupiter.api.Assertions.*;
+
+class VectorTest{
+
+    @Test
+    public void vector2DOperations(){
+        for(int i = 0; i < 20; i++){
+            float x1 = (float)(1000*random() - 500);
+            float y1 = (float)(1000*random() - 500);
+            Vector v1 = new Vector(x1, y1);
+            for(int j = 0; j < 20; j++){
+                float x2 = (float)(1000*random());
+                float y2 = (float)(1000*random());
+                Vector v2 = new Vector(x2, y2);
+
+                // test general setters
+                assertFloatEquals(x1, v1.x);
+                assertFloatEquals(y1, v1.y);
+                assertFloatEquals(x2, v2.x);
+                assertFloatEquals(y2, v2.y);
+
+                // test magnitude
+                assertFloatEquals((float)sqrt(x1*x1 + y1*y1), v1.mag());
+                assertFloatEquals((float)sqrt(x2*x2 + y2*y2), v2.mag());
+
+                // test dot product
+                assertFloatEquals((float)(x1*x2+y1*y2), v1.dot(v2));
+                assertFloatEquals((float)(x1*x2+y1*y2), v2.dot(v1));
+
+                // test addition
+                Vector vSum = v1.add(v2);
+                assertFloatEquals(x1+x2, vSum.x);
+                assertFloatEquals(y1+y2, vSum.y);
+
+                // test subtraction
+                Vector vSub = v1.sub(v2);
+                assertFloatEquals(x1-x2, vSub.x);
+                assertFloatEquals(y1-y2, vSub.y);
+                vSub = v2.sub(v1);
+                assertFloatEquals(x2-x1, vSub.x);
+                assertFloatEquals(y2-y1, vSub.y);
+
+                // test scaling
+                Vector vScale = v1.mul(x2);
+                assertFloatEquals(x1*x2, vScale.x);
+                assertFloatEquals(y1*x2, vScale.y);
+
+                // test normalization
+                Vector vNorm = v1.normalize();
+                assertFloatEquals(1, vNorm.mag());
+            }
+        }
+    }
+
+    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 testCrossProduct(){
+        Vector v1 = new  Vector(1, 2, 3);
+        Vector v2 = new Vector(4, 5, 6);
+        Vector cross = v1.cross(v2);
+        assertFloatEquals(-3, cross.x);
+        assertFloatEquals(6, cross.y);
+        assertFloatEquals(-3, cross.z);
+
+        v1 = new Vector(-3, 7, -9);
+        v2 = new Vector((float)2.6, 66, (float)-3.14159);
+        cross = v1.cross(v2);
+        assertFloatEquals((float)572.00887, cross.x, (float)0.1);
+        assertFloatEquals((float)-32.82477, cross.y, (float)0.1);
+        assertFloatEquals((float)-216.2, cross.z, (float)0.1);
+    }
+
+}