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

Porting over Ray-Tracing-2 to Intellij. Fixed processing core reference issues. still need to fix other issues though.

Browse Source
This commit is contained in:
Quinn
2023-04-05 22:45:50 -05:00
parent b280f0e98d
commit c17ee2e960
4 changed files with 349 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

79
src/Car.java Normal file
View File

@@ -0,0 +1,79 @@
import processing.core.*;
import java.util.ArrayList;
import static java.lang.Math.PI;
import static processing.core.PApplet.degrees;
import static processing.core.PApplet.radians;
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();
ArrayList<View> views = new ArrayList<View>();
ArrayList<PVector> points = new ArrayList<PVector>();
// default constructor
Car(){}
Car(int xPos, int yPos, int carLength, int carWidth){
this.pose = new PVector(xPos, yPos);
this.carLength = carLength;
this.carWidth = carWidth;
}
//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)));
}
//draw the car and its views
public void drawCar(ArrayList<Wall> walls){
stroke(255);
ellipse(pose.x, pose.y, carWidth, carLength);
this.updateScan(walls);
}
//With all of the views that the car has, get their point list
void updateScan(ArrayList<Wall> walls){
for(View view : views){
view.look(walls);
}
for(View view : views){
ArrayList<PVector> pointList = view.getPoints();
slam.addPoints(pointList);
}
}
public PVector getPose(){
return pose;
}
//always returns a positive angle between 0 and 360 degrees
public float getAngle(){
return degrees(this.angle);
}
//the given angle is in DEGREES!
public void setAngle(float angle){
//converts from degrees to radians
angle = radians(angle);
while(angle >= 2*PI){angle -= 2*PI;}
while(angle <= -2*PI){angle += 2*PI;}
this.angle = angle;
for(View view : views){
view.setAngle(angle);
}
return;
}
public void setPose(PVector newPose){
pose = newPose;
for(View view : views){
view.setPos(pose);
}
}
}

69
src/SLAM.java Normal file
View File

@@ -0,0 +1,69 @@
import processing.core.*;
import java.util.ArrayList;
public class SLAM{
ArrayList<PVector> points = new ArrayList<PVector>();
SLAM(){
}
public void addPoints(ArrayList<PVector> newPoints){
Line line = new Line(newPoints);
}
}
class Line{
PVector direction = new PVector(0,0);
PVector position = new PVector(0,0);
Line(){}
Line(PVector direction, PVector position){
this.direction = direction;
this.position = position;
}
/**
* @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){
bestFit(points);
}
// least squares line of best fit algorithm
private void bestFit(ArrayList<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 = direciton;
}
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;
}
}

165
src/View.java Normal file
View File

@@ -0,0 +1,165 @@
import processing.core.*;
import java.util.ArrayList;
import java.util.Objects;
import static processing.core.PApplet.*;
public class View{
PVector pose;
float angle = 0;
float FOV;
ArrayList<Ray> rays = new ArrayList<Ray>();
//the x,y position of the view, what angle it's looking at and its FOV
View(PVector newPose, int numberOfRays, float FOV){
this.pose = newPose;
this.FOV = FOV;
this.setRayNum(numberOfRays, FOV, this.angle);
}
//sets the number of rays and their starting values in the ray list
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
for(int i = 0; i < numberOfRays; i++){
Ray ray = new Ray(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){
for (Ray ray : rays){
ray.castRay(walls);
ray.drawRay();
}
}
//changes the position of the view
public void setPos(PVector newPose){
pose = newPose;
for(Ray ray : rays){ray.setPos(pose);}
}
//changes the angle of the view
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){
this.FOV = FOV;
this.setRayNum(this.rays.size(), this.FOV, this.angle);
}
public PVector getPos(){return pose;}
public float getAngle(){return this.angle;}
public float getFOV(){return this.FOV;}
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<PVector>();
for(Ray ray : rays){
if(!Objects.equals(ray.getPoint(), new PVector(0, 0) {
})){
points.add(ray.getPoint());
}
}
return points;
}
}
class Ray{
PVector pose;
int rayLength;
int defaultRayLength;
float angle; // IN RADIANS
//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){
this.pose = position;
this.defaultRayLength = defaultRayLength;
this.rayLength = defaultRayLength;
this.angle = angle;
}
public void drawRay(){
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<Integer>();
//sees what objects it collides with
for(Wall object : objects){
float theta1 = angle;
float theta2 = radians(object.getAngle());
PVector 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 PVector getPos(){ return pose;}
public int getRayLength(){return this.rayLength;}
public float getAngle(){return this.angle;}
public boolean hasCollided(){
if(this.defaultRayLength != this.rayLength){return true;}
else{return false;}
}
//returns the absolute position of the point
public PVector getPoint(){
if(this.rayLength != this.defaultRayLength){
return new PVector(rayLength * (int)cos(this.angle) + pose.x, rayLength * (int)sin(this.angle) + pose.y);
}
else{
return new PVector(0,0);
}
}
public void setPos(PVector 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;}
}

36
src/Wall.java Normal file
View File

@@ -0,0 +1,36 @@
import processing.core.*;
import static processing.core.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);
Wall(PVector pos, float angle, int wallLength){
this.pos = pos;
this.angle = angle;
this.wallLength = wallLength;
}
void drawWall(){
stroke(r,g,b);
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);
}
PVector getPos(){
return pos;
}
float getAngle(){
return angle;
}
int getLength(){
return wallLength;
}
}