Create README.md

README.md

@@ -0,0 +1,2 @@
+# SLAM-Sim
+SImulate SLAM algorithm using java and processing for visualizaiton.

src/Car.java

@@ -39,7 +39,7 @@ public class Car{
         proc.stroke(255);
         proc.ellipse(pose.x, pose.y, carWidth, carLength);
         this.updateScan(walls);
-//        this.slam.drawLines();
+        this.slam.drawLines();
     }
 
     //With all the views that the car has, get their point list
@@ -50,7 +50,7 @@ public class Car{
 
         for(View view : views){
             ArrayList<Vector> pointList = view.getPoints();
-//            slam.RANSAC(pointList, view.getFOV() / view.getRayNum());
+            slam.RANSAC(pointList, view.getFOV() / view.getRayNum());
         }
     }
 

src/Line.java

@@ -5,10 +5,8 @@ import java.util.List;
 import static processing.core.PApplet.*;
 
 public class Line{
-    // vector which represents  the direction and length of the line from its starting position
+    private Vector direction = new Vector(0,0);
-    protected Vector direction = new Vector(0,0);
+    private Vector position = new Vector(0,0);
-    // store the starting position of the line
-    protected Vector position = new Vector(0,0);
 
     Line(Vector startPosition, Vector endPosition){
         direction = endPosition.sub(startPosition);
@@ -80,12 +78,6 @@ public class Line{
         screen.line(position.x, position.y, endPoint.x, endPoint.y);
     }
 
-    public float getAngle(){return atan2(this.direction.y, this.direction.x);}
-
-    public Vector endPoint(){
-        return this.position.add(this.direction);
-    }
-
     /**
      * @param point
      * @return the smallest distance from the point to this line

src/Processing.java

@@ -6,6 +6,7 @@ public class Processing extends PApplet {
 
     Car car;
     ArrayList<Wall> objects = new ArrayList<>();
+
     public static PApplet processing;
 
     public static void main(String[] args) {
@@ -16,35 +17,33 @@ public class Processing extends PApplet {
         processing = this;
         car = new Car(processing, 100,100,50,40);
         size(1000, 1000);
-        car.addView(180,90);
+        car.addView(180,180);
-        for(int i = 0; i < 15; i++){
+        for(int i = 0; i < 20; i++){
-            Wall wall = new Wall(processing, new Vector((int)random(50, 950), (int)random(50, 950)), new Vector((int)random(50, 950), (int)random(50, 950)));
+            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){
+//        for(Wall object : objects){
-            object.drawWall();
+//            object.drawWall();
-        }
+//        }
         car.drawCar(objects);
-        strokeWeight(2);
-        stroke(255);
         //car.drive(new int[] {0, 0});
     }
 
     public void keyPressed(){
         if(key == 'd'){
-            car.setPose(car.getPose().add(10, 0));
+            car.setPose(car.getPose().add(1, 0));
         }
         if(key == 'w'){
-            car.setPose(car.getPose().add(0, -10));
+            car.setPose(car.getPose().add(0, -1));
         }
         if(key == 'a'){
-            car.setPose(car.getPose().add(-10, 0));
+            car.setPose(car.getPose().add(-1, 0));
         }
         if(key == 's'){
-            car.setPose(car.getPose().add(0, 10));
+            car.setPose(car.getPose().add(0, 1));
         }
         if(key == 'q'){
             car.setAngle(car.getAngle()+1);

src/Ray.java

@@ -1,98 +0,0 @@
-import processing.core.PApplet;
-import processing.core.PVector;
-
-import java.util.ArrayList;
-
-import static processing.core.PApplet.*;
-
-public class Ray extends Line{
-    float maxRayDistance = 1000;
-    int[] color = new int[]{255, 255, 255};
-    //takes the starting position of the ray, the length of the ray, and it's casting angle (radians)
-    Ray(Vector startPosition, float angle){
-        super(startPosition, startPosition.add(new Vector(cos(angle), sin(angle))));
-        direction = direction.mul(maxRayDistance);
-    }
-
-    public void drawRay(PApplet proc){
-        proc.stroke(color[0], color[1], color[2]);
-        proc.line(position.x, position.y, position.x + direction.x, position.y + direction.y);
-//        proc.noFill();
-//        proc.circle(position.x, position.y, 2*direction.mag());
-//        proc.fill(255);
-    }
-
-
-
-    //checks to see at what coordinate the ray will collide with an object and sets the ray length to meet that point.
-    public void castRay(ArrayList<Wall> walls){
-        float shortestWallDistance = maxRayDistance;
-        int[] newColor = new int[]{255, 255, 255};
-        for(Wall wall : walls){
-
-            // get the necessary vectors for two parameterized lines
-            // parameterized lines are of the form L = d*t + p
-            Vector d1 = this.direction.normalize().mul(maxRayDistance);
-            Vector d2 = wall.direction;
-            Vector p1 = this.position;
-            Vector p2 = wall.position;
-
-            // calculate the parameters for the intersection t and u
-            float t = -(d2.x*(p2.y-p1.y) + d2.y*(p1.x-p2.x))/(d1.x*d2.y - d2.x*d1.y);
-            float u = -(d1.x*(p2.y-p1.y) + d1.y*(p1.x-p2.x))/(d1.x*d2.y-d2.x*d1.y);
-
-            // the lines will only be intersecting when both t and u are between 0 and 1.
-            if(!(0 <= t && t <= 1 && 0 <= u && u <= 1)){
-                continue;
-            }
-
-            // if the distance from the ray to the intersection is shorter than the shortestWallDistance, this is our new closest wall
-            float distance = d1.mul(t).add(p1).sub(this.position).mag();
-            if(distance < shortestWallDistance){
-                shortestWallDistance = distance;
-                newColor = new int[]{wall.r, wall.g, wall.b};
-            }
-
-        }
-
-        // if we collided with a wall, set the ray's length to the distance from it to the collision
-        if(shortestWallDistance != maxRayDistance){
-            this.direction = this.direction.normalize().mul(shortestWallDistance);
-            this.color = newColor;
-        }
-        else{
-            this.direction = this.direction.normalize().mul(maxRayDistance);
-            this.color = new int[]{255, 255, 255};
-        }
-    }
-
-    public Vector getPos(){ return this.position;}
-
-    public float getRayLength(){return this.direction.mag();}
-
-
-    public boolean hasCollided(){
-        return this.direction.mag() != maxRayDistance;
-    }
-
-    //returns the absolute position of the point
-    public Vector getPoint(){
-        if(this.direction.mag() == 0){
-            return new Vector();
-        }
-
-        return this.position.add(this.direction);
-    }
-
-    public void setPos(Vector newPosition){
-        this.position = newPosition;
-    }
-
-    public void setRayLength(int rayLength){this.direction = this.direction.normalize().mul(rayLength);}
-
-    public void setAngle(float angle){
-        float distance = this.direction.mag();
-        this.direction = new Vector(cos(angle), sin(angle)).mul(distance);
-    }
-
-}

src/Vector.java

@@ -1,4 +1,5 @@
-import static java.lang.Math.*;
+import static java.lang.Math.acos;
+import static java.lang.Math.sqrt;
 
 public class Vector {
     public float x = 0;
@@ -66,18 +67,8 @@ public class Vector {
         return new Vector(x / mag, y / mag, z / mag);
     }
 
-    /**
-     * @param other
-     * @return
-     */
     float angleDiff(Vector other){
-        float dot = this.dot(other);     // dot product
+        float dot = this.dot(other);
-        float det = this.x*other.y - this.y*other.x;    // determinant
+        return (float)acos(dot / (this.mag() * other.mag()));
-        float angle = (float) atan2(det, dot);  // atan2(y, x) or atan2(sin, cos)
-        return angle;
-    }
-
-    float angle(){
-        return (float) atan2(y, x);
     }
 }

src/View.java

@@ -4,7 +4,7 @@ import java.util.Objects;
 
 import static processing.core.PApplet.*;
 
-public class View {
+public class View{
     Vector pose;
     float angle = 0;
     float FOV;
@@ -12,7 +12,7 @@ public class View {
     private static PApplet proc;
 
     //the x,y position of the view, what angle it's looking at and its FOV
-    View(PApplet processing, Vector newPose, int numberOfRays, float FOV) {
+    View(PApplet processing, Vector newPose, int numberOfRays, float FOV){
         proc = processing;
         this.pose = newPose;
         this.FOV = FOV;
@@ -20,68 +20,58 @@ public class View {
     }
 
     //sets the number of rays and their starting values in the ray list
-    public void setRayNum(int numberOfRays, float FOV, float angleOffset) {
+    public void setRayNum(int numberOfRays, float FOV, float angleOffset){
-        float rayStep = FOV / numberOfRays;
+        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++) {
+        for(int i = 0; i < numberOfRays; i++){
-            Ray ray = new Ray(pose, angle);
+            Ray ray = new Ray(proc, pose, 100000, angle);
             angle = angle + rayStep;
             rays.add(ray);
         }
     }
 
     //sees if the ray will collide with the walls in the wall list
-    public void look(ArrayList<Wall> walls) {
+    public void look(ArrayList<Wall> walls){
-        for (Ray ray : rays) {
+        for (Ray ray : rays){
             ray.castRay(walls);
-            ray.drawRay(proc);
+            ray.drawRay();
         }
     }
 
     //changes the position of the view
-    public void setPos(Vector newPose) {
+    public void setPos(Vector newPose){
         pose = newPose;
-        for (Ray ray : rays) {
+        for(Ray ray : rays){ray.setPos(pose);}
-            ray.setPos(pose);
-        }
     }
 
     //changes the angle of the view
-    public void setAngle(float angle) {
+    public void setAngle(float angle){
         this.angle = angle;
         this.setRayNum(rays.size(), this.FOV, angle);
     }
 
     //changes the field of view of the view
-    public void setFOV(float FOV) {
+    public void setFOV(float FOV){
         this.FOV = FOV;
         this.setRayNum(this.rays.size(), this.FOV, this.angle);
     }
 
-    public Vector getPos() {
+    public Vector getPos(){return pose;}
-        return pose;
-    }
 
-    public float getAngle() {
+    public float getAngle(){return this.angle;}
-        return this.angle;
-    }
 
-    public float getFOV() {
+    public float getFOV(){return this.FOV;}
-        return this.FOV;
-    }
 
-    public int getRayNum() {
+    public int getRayNum(){return this.rays.size();}
-        return this.rays.size();
-    }
 
     //gets the point that each ray has collided with
-    public ArrayList<Vector> getPoints() {
+    public ArrayList<Vector> getPoints(){
         ArrayList<Vector> points = new ArrayList<>();
 
-        for (Ray ray : rays) {
+        for(Ray ray : rays){
-            if (!Objects.equals(ray.getPoint(), new Vector(0, 0) {
+            if(!Objects.equals(ray.getPoint(), new Vector(0, 0) {
-            })) {
+            })){
                 points.add(ray.getPoint());
             }
         }
@@ -89,3 +79,90 @@ public class View {
     }
 }
 
+class Ray{
+    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, Vector position, int defaultRayLength, float angle){
+        proc = processing;
+        this.pose = position;
+        this.defaultRayLength = defaultRayLength;
+        this.rayLength = defaultRayLength;
+        this.angle = angle;
+    }
+
+    public void drawRay(){
+        proc.line(pose.x, pose.y, (pose.x + cos(angle)*rayLength), (pose.y + sin(angle)*rayLength));
+    }
+
+    //checks to see at what coordinate the ray will collide with an object and sets the ray length to meet that point.
+    public void castRay(ArrayList<Wall> objects){
+        this.rayLength = defaultRayLength;
+        ArrayList<Integer> distances = new ArrayList<>();
+        //sees what objects it collides with
+        for(Wall object : objects){
+            float theta1 = angle;
+            float theta2 = radians(object.getAngle());
+            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));
+
+            //if the place along the wall is further away than the wall extends, then it didn't collide
+            if(b < object.getLength() && b > 0){
+                //finds the length of the ray needed to collide with the wall
+                float a = (b*sin(theta2) + wallPos.y-pose.y) / sin(theta1);
+                //add that length to a list
+                if(a > 0){
+                    distances.add((int)abs(a));
+                }
+
+            }
+        }
+        //finds the shortest distance and sets the length of the ray to that distance
+        if(distances.size() > 0){
+            for(Integer distance : distances){
+                if(distance < rayLength){
+                    rayLength = distance;
+                }
+            }
+        }
+        else this.rayLength = defaultRayLength;
+    }
+
+    public Vector getPos(){ return pose;}
+
+    public int getRayLength(){return this.rayLength;}
+
+    public float getAngle(){return this.angle;}
+
+    public boolean hasCollided(){
+        return this.defaultRayLength != this.rayLength;
+    }
+
+    //returns the absolute position of the point
+    public Vector getPoint(){
+        if(this.rayLength != this.defaultRayLength){
+            return new Vector(rayLength * (int)cos(this.angle) + pose.x, rayLength * (int)sin(this.angle) + pose.y);
+        }
+        else{
+            return new Vector(0,0);
+        }
+    }
+
+    public void setPos(Vector newPose){
+        pose = newPose;
+    }
+
+    public void setRayLength(int rayLength){this.rayLength = rayLength;}
+
+    public void setDefaultRayLength(int defaultRayLength){this.defaultRayLength = defaultRayLength;}
+
+    public void setAngle(float angle){this.angle = angle;}
+
+}
+

src/Wall.java

@@ -2,24 +2,20 @@ import processing.core.*;
 
 import static processing.core.PApplet.*;
 
-public class Wall extends Line{
+public class Wall{
+    Vector pos;
+    float angle;
+    int wallLength;
+    private static PApplet proc;
     int r;
     int g;
     int b;
 
-    private final PApplet proc;
+    Wall(PApplet processing, Vector pos, float angle, int wallLength){
-
+        proc = processing;
-    Wall(PApplet proc, Vector pos, float angle, int wallLength){
+        this.pos = pos;
-        super(pos, (new Vector(cos(angle), sin(angle)).mul(wallLength)).add(pos));
+        this.angle = angle;
-        this.proc = proc;
+        this.wallLength = wallLength;
-        r = (int)proc.random(50, 255);
-        g = (int)proc.random(50, 255);
-        b = (int)proc.random(50, 255);
-    }
-
-    Wall(PApplet proc, Vector startPos, Vector endPos){
-        super(startPos, endPos);
-        this.proc = proc;
         r = (int)proc.random(50, 255);
         g = (int)proc.random(50, 255);
         b = (int)proc.random(50, 255);
@@ -27,13 +23,19 @@ public class Wall extends Line{
 
     void drawWall(){
         proc.stroke(r,g,b);
-        proc.strokeWeight(10);
+        proc.line(pos.x, pos.y, (pos.x + cos(radians(angle))*wallLength), (pos.y + sin(radians(angle))*wallLength));
-        proc.circle(position.x, position.y, 10);
+        //ellipse((xPos + cos(radians(angle))*wallLength), (yPos + sin(radians(angle))*wallLength), 20, 20);
-        proc.strokeWeight(2);
-        proc.line(position.x, position.y, position.x + direction.x, position.y + direction.y);
     }
 
     Vector getPos(){
-        return position;
+        return pos;
+    }
+
+    float getAngle(){
+        return angle;
+    }
+
+    int getLength(){
+        return wallLength;
     }
 }

tests/LineTest.java

@@ -3,7 +3,6 @@ import processing.core.PApplet;
 
 import java.util.ArrayList;
 import java.util.List;
-import java.util.Random;
 
 import static java.lang.Math.abs;
 import static org.junit.jupiter.api.Assertions.*;
@@ -32,34 +31,6 @@ class LineTest{
         // make sure the line is starting in the correct place
         assertFloatEquals(v1.x, line.getPosition().x);
         assertFloatEquals(v1.y, line.getPosition().y);
-
-        // make sure the line ends in the correct place
-        assertFloatEquals(v2.x, line.endPoint().x);
-        assertFloatEquals(v2.y, line.endPoint().y);
-        
-        Random rand = new Random();
-        // repeat this test with 100 random lines
-        for(int i = 0; i < 100; i++){
-            v1 = new Vector(rand.nextInt(1001), rand.nextInt(1001));
-            v2 = new Vector(rand.nextInt(1001), rand.nextInt(1001));
-            line = new Line(v1, v2);
-
-            // make sure the line is pointing in the right direction
-            lineDirection = v2.sub(v1);
-            float angleDiff = lineDirection.angleDiff(line.getDirection());
-            assertFloatEquals(0, angleDiff, 0.001f);
-
-            // 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);
-
-            // make sure the line ends in the correct place
-            assertFloatEquals(v2.x, line.endPoint().x);
-            assertFloatEquals(v2.y, line.endPoint().y);
-        }
     }
 
     @Test
@@ -83,20 +54,4 @@ class LineTest{
         assertFloatEquals(v1.y, line.getPosition().y);
     }
 
-    @Test
-    public void testAngle(){
-        Line ray = new Line(new Vector(680, 560), new Vector(680+116, 560+1));
-        Line wall = new Line(new Vector(675, 587), new Vector(675-114, 587-29));
-
-
-        float angle1 = wall.position.sub(ray.position).angle();
-        float angle2 = wall.endPoint().sub(ray.position).angle();
-        float rayAngle = ray.direction.angle();
-
-        System.out.println("Angle 1: " + String.valueOf(angle1) + ", Angle 2: " + String.valueOf(angle2) + ", Ray Angle: " + String.valueOf(rayAngle));
-        assertFloatEquals(1.753907144057f, angle1, 0.001f);
-        assertFloatEquals(3.1583977941048f, angle2, 0.001f);
-        assertFloatEquals(0.0086204761121f, rayAngle, 0.001f);
-    }
-
 }