Cynopolis/SLAM-Sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

We now have full functionality like we did in Ray-Tracing-2. It's time to begin adding features.

Browse Source
This commit is contained in:
Quinn
2023-04-05 23:28:21 -05:00
parent 913dc74fd6
commit 1dcd5b3614
5 changed files with 96 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
src/Car.java
View File

@@ -6,20 +6,25 @@ import static processing.core.PApplet.radians;
import processing.core.PVector;
import processing.core.PApplet;
public class Car extends PApplet{
public class Car{
PVector pose = new PVector(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;
SLAM slam = new SLAM();
SLAM slam;
private static PApplet proc;
ArrayList<View> views = new ArrayList<View>();
ArrayList<PVector> points = new ArrayList<PVector>();
ArrayList<View> views = new ArrayList<>();
// default constructor
Car(){}
Car(PApplet processing){
this.proc = processing;
slam = new SLAM(proc);
}
Car(int xPos, int yPos, int carLength, int carWidth){
Car(PApplet processing, int xPos, int yPos, int carLength, int carWidth){
this.proc = processing;
slam = new SLAM(proc);
this.pose = new PVector(xPos, yPos);
this.carLength = carLength;
this.carWidth = carWidth;
@@ -27,17 +32,17 @@ public class Car extends PApplet{
//adds a new view with the specified FOV and ray number
public void addView(float FOV, int numberOfRays){
views.add(new View(pose, numberOfRays, radians(FOV)));
views.add(new View(proc, pose, numberOfRays, radians(FOV)));
}
//draw the car and its views
public void drawCar(ArrayList<Wall> walls){
stroke(255);
ellipse(pose.x, pose.y, carWidth, carLength);
proc.stroke(255);
proc.ellipse(pose.x, pose.y, carWidth, carLength);
this.updateScan(walls);
}
//With all of the views that the car has, get their point list
//With all the views that the car has, get their point list
void updateScan(ArrayList<Wall> walls){
for(View view : views){
view.look(walls);
@@ -68,7 +73,6 @@ public class Car extends PApplet{
for(View view : views){
view.setAngle(angle);
}
return;
}
public void setPose(PVector newPose){

47
src/Processing.java
View File

@@ -1,17 +1,60 @@
import processing.core.PApplet;
import processing.core.PVector;
import java.util.ArrayList;
public class Processing extends PApplet {
Car car;
ArrayList<Wall> objects = new ArrayList<Wall>();
public static PApplet processing;
public static void main(String[] args) {
PApplet.main("Processing");
}
public void settings(){
size(200, 200);
processing = this;
car = new Car(processing, 100,100,50,40);
size(1000, 1000);
car.addView(60,6);
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));
objects.add(wall);
}
}
public void draw(){
background(0);
ellipse(mouseX, mouseY, 20, 20);
for(Wall object : objects){
object.drawWall();
}
car.drawCar(objects);
//car.drive(new int[] {0, 0});
}
public void keyPressed(){
if(key == 'd'){
car.setPose(car.getPose().add(1, 0));
}
if(key == 'w'){
car.setPose(car.getPose().add(0, -1));
}
if(key == 'a'){
car.setPose(car.getPose().add(-1, 0));
}
if(key == 's'){
car.setPose(car.getPose().add(0, 1));
}
if(key == 'q'){
car.setAngle(car.getAngle()+1);
}
if(key == 'e'){
car.setAngle(car.getAngle()-1);
}
}
}

19
src/SLAM.java
View File

@@ -4,14 +4,16 @@ import java.util.ArrayList;
import static processing.core.PApplet.pow;
public class SLAM extends PApplet{
public class SLAM{
ArrayList<PVector> points = new ArrayList<PVector>();
private static PApplet proc;
SLAM(){
SLAM(PApplet processing){
proc = processing;
}
public void addPoints(ArrayList<PVector> newPoints){
Line line = new Line(newPoints);
Line line = new Line(proc, newPoints);
}
}
@@ -19,19 +21,24 @@ public class SLAM extends PApplet{
class Line{
PVector direction = new PVector(0,0);
PVector position = new PVector(0,0);
private static PApplet proc;
Line(){}
Line(PVector direction, PVector position){
Line(PApplet processing){
proc = processing;
}
Line(PApplet processing, PVector direction, PVector position){
this.direction = direction;
this.position = position;
proc = processing;
}
/**
* @brief attempt to find the line of best fit for the given points
* @param points the points to get the line of best for
*/
Line(ArrayList<PVector> points){
Line(PApplet processing, ArrayList<PVector> points){
bestFit(points);
proc = processing;
}
// least squares line of best fit algorithm

18
src/View.java
View File

@@ -4,14 +4,16 @@ import java.util.Objects;
import static processing.core.PApplet.*;
public class View extends PApplet{
public class View{
PVector pose;
float angle = 0;
float FOV;
ArrayList<Ray> rays = new ArrayList<Ray>();
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(PVector newPose, int numberOfRays, float FOV){
View(PApplet processing, PVector newPose, int numberOfRays, float FOV){
proc = processing;
this.pose = newPose;
this.FOV = FOV;
this.setRayNum(numberOfRays, FOV, this.angle);
@@ -23,7 +25,7 @@ public class View extends PApplet{
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(pose, 100000, angle);
Ray ray = new Ray(proc, pose, 100000, angle);
angle = angle + rayStep;
rays.add(ray);
}
@@ -77,14 +79,16 @@ public class View extends PApplet{
}
}
class Ray extends PApplet{
class Ray{
PVector 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(PVector position, int defaultRayLength, float angle){
Ray(PApplet processing, PVector position, int defaultRayLength, float angle){
proc = processing;
this.pose = position;
this.defaultRayLength = defaultRayLength;
this.rayLength = defaultRayLength;
@@ -92,7 +96,7 @@ class Ray extends PApplet{
}
public void drawRay(){
line(pose.x, pose.y, (pose.x + cos(angle)*rayLength), (pose.y + sin(angle)*rayLength));
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.

19
src/Wall.java
View File

@@ -2,23 +2,28 @@ import processing.core.*;
import static processing.core.PApplet.*;
public class Wall extends PApplet{
public class Wall{
PVector pos;
float angle;
int wallLength;
int r = (int)random(50, 255);
int g = (int)random(50, 255);
int b = (int)random(50, 255);
private static PApplet proc;
int r;
int g;
int b;
Wall(PVector pos, float angle, int wallLength){
Wall(PApplet processing, PVector pos, float angle, int wallLength){
proc = processing;
this.pos = pos;
this.angle = angle;
this.wallLength = wallLength;
r = (int)proc.random(50, 255);
g = (int)proc.random(50, 255);
b = (int)proc.random(50, 255);
}
void drawWall(){
stroke(r,g,b);
line(pos.x, pos.y, (pos.x + cos(radians(angle))*wallLength), (pos.y + sin(radians(angle))*wallLength));
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);
}