diff --git a/src/Car.java b/src/Car.java index 852046d..5f3000e 100644 --- a/src/Car.java +++ b/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 pointList = view.getPoints(); - slam.RANSAC(pointList, view.getFOV() / view.getRayNum()); +// slam.RANSAC(pointList, view.getFOV() / view.getRayNum()); } } diff --git a/src/Line.java b/src/Line.java index 09bb43f..4ee9dbe 100644 --- a/src/Line.java +++ b/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 diff --git a/src/Processing.java b/src/Processing.java index e3c3492..d742132 100644 --- a/src/Processing.java +++ b/src/Processing.java @@ -6,7 +6,6 @@ public class Processing extends PApplet { Car car; ArrayList 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}); } diff --git a/src/Ray.java b/src/Ray.java index 5baa52c..0864446 100644 --- a/src/Ray.java +++ b/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 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(); diff --git a/src/View.java b/src/View.java index 367911b..df3bbf7 100644 --- a/src/View.java +++ b/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 walls) { for (Ray ray : rays) { ray.castRay(walls); - ray.drawRay(); + ray.drawRay(proc); } } diff --git a/src/Wall.java b/src/Wall.java index 711a603..93c2f88 100644 --- a/src/Wall.java +++ b/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; } } \ No newline at end of file