The ray class is mostly refactored, but the collision code needs to be debugged.

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

@@ -78,6 +78,8 @@ public class Line{
         screen.line(position.x, position.y, endPoint.x, endPoint.y);
     }
 
+    public float getAngle(){return atan2(this.direction.y, this.direction.x);}
+
     /**
      * @param point
      * @return the smallest distance from the point to this line

src/Processing.java

@@ -6,7 +6,6 @@ public class Processing extends PApplet {
 
     Car car;
     ArrayList<Wall> objects = new ArrayList<>();
-
     public static PApplet processing;
 
     public static void main(String[] args) {
@@ -25,10 +24,12 @@ public class Processing extends PApplet {
     }
     public void draw(){
         background(0);
-//        for(Wall object : objects){
-//            object.drawWall();
-//        }
+        for(Wall object : objects){
+            object.drawWall();
+        }
         car.drawCar(objects);
+        strokeWeight(2);
+        stroke(255);
         //car.drive(new int[] {0, 0});
     }
 

src/Ray.java

@@ -6,9 +6,11 @@ import java.util.ArrayList;
 import static processing.core.PApplet.*;
 
 public class Ray extends Line{
+    float maxRayDistance = 1000;
     //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){
@@ -17,9 +19,37 @@ public class Ray extends Line{
 
     //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 maxRange = 1000;
+        float shortestWallDistance = maxRayDistance;
         for(Wall wall : walls){
-            // TODO: find the intersection between the wall and ray.
+            Vector rayLine = this.getSlopeIntForm(); // stored as x:m, y:b
+            Vector wallLine = wall.getSlopeIntForm(); // m, b
+
+            // find the point where they intersect
+            float x = (wallLine.y - rayLine.y) / (rayLine.x - wallLine.x);
+            float y = rayLine.x * x + rayLine.y;
+            Vector intersect = new Vector(x, y);
+
+            // if the intersect doesn't actually lie on the wallLine, go to the next wall
+            float distAlongLine = intersect.sub(wall.getPosition()).mag();
+            float angleFromLine = wall.direction.angleDiff(intersect.sub(wall.position));
+            if(distAlongLine > wall.getLength() && abs(angleFromLine) < 5*PI/180){
+                continue;
+            }
+
+            // if the distance from the ray to the intersection is shorter than the shortestWallDistance, this is our new closest wall
+            float distance = this.position.sub(intersect).mag();
+            if(distance < shortestWallDistance){
+                shortestWallDistance = distance;
+            }
+
+        }
+
+        // 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);
+        }
+        else{
+            this.direction = this.direction.normalize().mul(maxRayDistance);
         }
     }
 
@@ -27,10 +57,9 @@ public class Ray extends Line{
 
     public float getRayLength(){return this.direction.mag();}
 
-    public float getAngle(){return atan2(this.direction.y, this.direction.x);}
 
     public boolean hasCollided(){
-        return this.direction.mag() == 0;
+        return this.direction.mag() != maxRayDistance;
     }
 
     //returns the absolute position of the point
@@ -46,7 +75,7 @@ public class Ray extends Line{
         this.position = newPosition;
     }
 
-    public void setRayLength(int rayLength){this.direction = this.direction.normalize().mul(rayLength)}
+    public void setRayLength(int rayLength){this.direction = this.direction.normalize().mul(rayLength);}
 
     public void setAngle(float angle){
         float distance = this.direction.mag();

src/View.java

@@ -25,7 +25,7 @@ public class View {
         rays.clear();
         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);
+            Ray ray = new Ray(pose, angle);
             angle = angle + rayStep;
             rays.add(ray);
         }
@@ -35,7 +35,7 @@ public class View {
     public void look(ArrayList<Wall> walls) {
         for (Ray ray : rays) {
             ray.castRay(walls);
-            ray.drawRay();
+            ray.drawRay(proc);
         }
     }
 

src/Wall.java

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