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

Compare commits

base: Cynopolis:Ray-Refactor
Branches Tags
Cynopolis:master Cynopolis:5-close-graph-loop Cynopolis:slam-feature-graph Cynopolis:SLAM Cynopolis:interactive-map Cynopolis:Ray-Refactor
..
compare: Cynopolis:SLAM
Branches Tags
Cynopolis:5-close-graph-loop Cynopolis:slam-feature-graph Cynopolis:master Cynopolis:SLAM Cynopolis:interactive-map Cynopolis:Ray-Refactor

7 Commits
Ray-Refact ... SLAM

Author SHA1 Message Date
Quinn
bda601d326 Further tweaks to RANSAC algorithm and line of best fit 2023-05-03 16:22:38 -05:00
Quinn
62e7232435 Tweaking SLAM parameters to get better accuracy 2023-05-03 00:12:17 -05:00
Quinn
7298f80d36 First semi-successful test of feature extraction. I think the algorithm for line of best fit needs some work. Specifically, ifnding the starting and end point for the line. 2023-05-02 23:51:59 -05:00
Quinn
2f46605837 Added a saving and loading system for graph maps 2023-05-02 12:10:58 -05:00
Quinn
38847abead Added interactivity and can now save the graph state to a text file 2023-05-02 11:19:39 -05:00
Quinn
1a4d6e6909 Everything can now be drawn 2023-05-02 10:28:06 -05:00
Quinn
3321e77061 Added a Graph datastructure and refactored the Line/Vertex classes 2023-05-02 09:49:38 -05:00
21 changed files with 1307 additions and 176 deletions
Expand all files Collapse all files

124
.idea/uiDesigner.xml generated Normal file
View File

@@ -0,0 +1,124 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="Palette2">
<group name="Swing">
<item class="com.intellij.uiDesigner.HSpacer" tooltip-text="Horizontal Spacer" icon="/com/intellij/uiDesigner/icons/hspacer.svg" removable="false" auto-create-binding="false" can-attach-label="false">
<default-constraints vsize-policy="1" hsize-policy="6" anchor="0" fill="1" />
</item>
<item class="com.intellij.uiDesigner.VSpacer" tooltip-text="Vertical Spacer" icon="/com/intellij/uiDesigner/icons/vspacer.svg" removable="false" auto-create-binding="false" can-attach-label="false">
<default-constraints vsize-policy="6" hsize-policy="1" anchor="0" fill="2" />
</item>
<item class="javax.swing.JPanel" icon="/com/intellij/uiDesigner/icons/panel.svg" removable="false" auto-create-binding="false" can-attach-label="false">
<default-constraints vsize-policy="3" hsize-policy="3" anchor="0" fill="3" />
</item>
<item class="javax.swing.JScrollPane" icon="/com/intellij/uiDesigner/icons/scrollPane.svg" removable="false" auto-create-binding="false" can-attach-label="true">
<default-constraints vsize-policy="7" hsize-policy="7" anchor="0" fill="3" />
</item>
<item class="javax.swing.JButton" icon="/com/intellij/uiDesigner/icons/button.svg" removable="false" auto-create-binding="true" can-attach-label="false">
<default-constraints vsize-policy="0" hsize-policy="3" anchor="0" fill="1" />
<initial-values>
<property name="text" value="Button" />
</initial-values>
</item>
<item class="javax.swing.JRadioButton" icon="/com/intellij/uiDesigner/icons/radioButton.svg" removable="false" auto-create-binding="true" can-attach-label="false">
<default-constraints vsize-policy="0" hsize-policy="3" anchor="8" fill="0" />
<initial-values>
<property name="text" value="RadioButton" />
</initial-values>
</item>
<item class="javax.swing.JCheckBox" icon="/com/intellij/uiDesigner/icons/checkBox.svg" removable="false" auto-create-binding="true" can-attach-label="false">
<default-constraints vsize-policy="0" hsize-policy="3" anchor="8" fill="0" />
<initial-values>
<property name="text" value="CheckBox" />
</initial-values>
</item>
<item class="javax.swing.JLabel" icon="/com/intellij/uiDesigner/icons/label.svg" removable="false" auto-create-binding="false" can-attach-label="false">
<default-constraints vsize-policy="0" hsize-policy="0" anchor="8" fill="0" />
<initial-values>
<property name="text" value="Label" />
</initial-values>
</item>
<item class="javax.swing.JTextField" icon="/com/intellij/uiDesigner/icons/textField.svg" removable="false" auto-create-binding="true" can-attach-label="true">
<default-constraints vsize-policy="0" hsize-policy="6" anchor="8" fill="1">
<preferred-size width="150" height="-1" />
</default-constraints>
</item>
<item class="javax.swing.JPasswordField" icon="/com/intellij/uiDesigner/icons/passwordField.svg" removable="false" auto-create-binding="true" can-attach-label="true">
<default-constraints vsize-policy="0" hsize-policy="6" anchor="8" fill="1">
<preferred-size width="150" height="-1" />
</default-constraints>
</item>
<item class="javax.swing.JFormattedTextField" icon="/com/intellij/uiDesigner/icons/formattedTextField.svg" removable="false" auto-create-binding="true" can-attach-label="true">
<default-constraints vsize-policy="0" hsize-policy="6" anchor="8" fill="1">
<preferred-size width="150" height="-1" />
</default-constraints>
</item>
<item class="javax.swing.JTextArea" icon="/com/intellij/uiDesigner/icons/textArea.svg" removable="false" auto-create-binding="true" can-attach-label="true">
<default-constraints vsize-policy="6" hsize-policy="6" anchor="0" fill="3">
<preferred-size width="150" height="50" />
</default-constraints>
</item>
<item class="javax.swing.JTextPane" icon="/com/intellij/uiDesigner/icons/textPane.svg" removable="false" auto-create-binding="true" can-attach-label="true">
<default-constraints vsize-policy="6" hsize-policy="6" anchor="0" fill="3">
<preferred-size width="150" height="50" />
</default-constraints>
</item>
<item class="javax.swing.JEditorPane" icon="/com/intellij/uiDesigner/icons/editorPane.svg" removable="false" auto-create-binding="true" can-attach-label="true">
<default-constraints vsize-policy="6" hsize-policy="6" anchor="0" fill="3">
<preferred-size width="150" height="50" />
</default-constraints>
</item>
<item class="javax.swing.JComboBox" icon="/com/intellij/uiDesigner/icons/comboBox.svg" removable="false" auto-create-binding="true" can-attach-label="true">
<default-constraints vsize-policy="0" hsize-policy="2" anchor="8" fill="1" />
</item>
<item class="javax.swing.JTable" icon="/com/intellij/uiDesigner/icons/table.svg" removable="false" auto-create-binding="true" can-attach-label="false">
<default-constraints vsize-policy="6" hsize-policy="6" anchor="0" fill="3">
<preferred-size width="150" height="50" />
</default-constraints>
</item>
<item class="javax.swing.JList" icon="/com/intellij/uiDesigner/icons/list.svg" removable="false" auto-create-binding="true" can-attach-label="false">
<default-constraints vsize-policy="6" hsize-policy="2" anchor="0" fill="3">
<preferred-size width="150" height="50" />
</default-constraints>
</item>
<item class="javax.swing.JTree" icon="/com/intellij/uiDesigner/icons/tree.svg" removable="false" auto-create-binding="true" can-attach-label="false">
<default-constraints vsize-policy="6" hsize-policy="6" anchor="0" fill="3">
<preferred-size width="150" height="50" />
</default-constraints>
</item>
<item class="javax.swing.JTabbedPane" icon="/com/intellij/uiDesigner/icons/tabbedPane.svg" removable="false" auto-create-binding="true" can-attach-label="false">
<default-constraints vsize-policy="3" hsize-policy="3" anchor="0" fill="3">
<preferred-size width="200" height="200" />
</default-constraints>
</item>
<item class="javax.swing.JSplitPane" icon="/com/intellij/uiDesigner/icons/splitPane.svg" removable="false" auto-create-binding="false" can-attach-label="false">
<default-constraints vsize-policy="3" hsize-policy="3" anchor="0" fill="3">
<preferred-size width="200" height="200" />
</default-constraints>
</item>
<item class="javax.swing.JSpinner" icon="/com/intellij/uiDesigner/icons/spinner.svg" removable="false" auto-create-binding="true" can-attach-label="true">
<default-constraints vsize-policy="0" hsize-policy="6" anchor="8" fill="1" />
</item>
<item class="javax.swing.JSlider" icon="/com/intellij/uiDesigner/icons/slider.svg" removable="false" auto-create-binding="true" can-attach-label="false">
<default-constraints vsize-policy="0" hsize-policy="6" anchor="8" fill="1" />
</item>
<item class="javax.swing.JSeparator" icon="/com/intellij/uiDesigner/icons/separator.svg" removable="false" auto-create-binding="false" can-attach-label="false">
<default-constraints vsize-policy="6" hsize-policy="6" anchor="0" fill="3" />
</item>
<item class="javax.swing.JProgressBar" icon="/com/intellij/uiDesigner/icons/progressbar.svg" removable="false" auto-create-binding="true" can-attach-label="false">
<default-constraints vsize-policy="0" hsize-policy="6" anchor="0" fill="1" />
</item>
<item class="javax.swing.JToolBar" icon="/com/intellij/uiDesigner/icons/toolbar.svg" removable="false" auto-create-binding="false" can-attach-label="false">
<default-constraints vsize-policy="0" hsize-policy="6" anchor="0" fill="1">
<preferred-size width="-1" height="20" />
</default-constraints>
</item>
<item class="javax.swing.JToolBar$Separator" icon="/com/intellij/uiDesigner/icons/toolbarSeparator.svg" removable="false" auto-create-binding="false" can-attach-label="false">
<default-constraints vsize-policy="0" hsize-policy="0" anchor="0" fill="1" />
</item>
<item class="javax.swing.JScrollBar" icon="/com/intellij/uiDesigner/icons/scrollbar.svg" removable="false" auto-create-binding="true" can-attach-label="false">
<default-constraints vsize-policy="6" hsize-policy="0" anchor="0" fill="2" />
</item>
</group>
</component>
</project>

112
map.txt Normal file
View File

@@ -0,0 +1,112 @@
numVerts,60
numEdges,50
vert,206.0,522.0
vert,554.0,422.0
vert,525.0,281.0
vert,404.0,666.0
vert,482.0,615.0
vert,545.0,326.0
vert,520.0,338.0
vert,713.0,810.0
vert,401.0,616.0
vert,520.0,399.0
vert,367.0,375.0
vert,599.0,418.0
vert,591.0,332.0
vert,483.0,810.0
vert,602.0,652.0
vert,711.0,234.0
vert,366.0,275.0
vert,611.0,359.0
vert,570.0,324.0
vert,302.0,249.0
vert,259.0,476.0
vert,411.0,811.0
vert,503.0,375.0
vert,168.0,471.0
vert,603.0,346.0
vert,601.0,491.0
vert,165.0,235.0
vert,405.0,237.0
vert,606.0,811.0
vert,165.0,809.0
vert,213.0,812.0
vert,165.0,521.0
vert,223.0,273.0
vert,294.0,378.0
vert,267.0,616.0
vert,493.0,238.0
vert,403.0,483.0
vert,210.0,380.0
vert,330.0,376.0
vert,714.0,488.0
vert,603.0,537.0
vert,715.0,655.0
vert,224.0,247.0
vert,267.0,723.0
vert,503.0,357.0
vert,618.0,397.0
vert,713.0,539.0
vert,334.0,727.0
vert,485.0,693.0
vert,211.0,346.0
vert,300.0,277.0
vert,294.0,347.0
vert,577.0,423.0
vert,531.0,414.0
vert,329.0,345.0
vert,327.0,615.0
vert,367.0,347.0
vert,304.0,479.0
vert,621.0,379.0
vert,487.0,485.0
edge,start,1,end,53
edge,start,2,end,16
edge,start,3,end,21
edge,start,5,end,18
edge,end,5,start,6
edge,start,7,end,15
edge,start,9,end,22
edge,start,11,end,52
edge,start,12,end,24
edge,start,14,end,41
edge,start,14,end,28
edge,start,15,end,35
edge,start,17,end,58
edge,end,12,start,18
edge,start,19,end,42
edge,start,19,end,50
edge,start,22,end,44
edge,end,20,start,23
edge,end,17,start,24
edge,start,26,end,29
edge,end,26,start,27
edge,end,7,start,29
edge,end,0,start,30
edge,end,0,start,31
edge,start,32,end,42
edge,start,32,end,50
edge,end,8,start,36
edge,start,36,end,57
edge,start,37,end,49
edge,end,33,start,37
edge,start,38,end,54
edge,end,10,start,38
edge,end,25,start,40
edge,start,40,end,46
edge,end,34,start,43
edge,start,43,end,47
edge,end,6,start,44
edge,end,11,start,45
edge,end,13,start,48
edge,end,49,start,51
edge,end,33,start,51
edge,end,1,start,52
edge,end,9,start,53
edge,end,34,start,55
edge,end,47,start,55
edge,end,54,start,56
edge,end,10,start,56
edge,end,45,start,58
edge,end,4,start,59
edge,end,39,start,59

20
src/Car.java
View File

@@ -3,6 +3,9 @@ import java.util.ArrayList;
import static java.lang.Math.PI;
import static processing.core.PApplet.degrees;
import static processing.core.PApplet.radians;
import Graph.*;
import Vector.Vector;
import processing.core.PApplet;
public class Car{
@@ -35,22 +38,21 @@ public class Car{
}
//draw the car and its views
public void drawCar(ArrayList<Wall> walls){
public void drawCar(PointGraph map, boolean SLAMIsHidden){
proc.stroke(255);
proc.ellipse(pose.x, pose.y, carWidth, carLength);
this.updateScan(walls);
// this.slam.drawLines();
this.updateScan(map);
if(!SLAMIsHidden){
this.slam.drawFeatures(proc);
}
}
//With all the views that the car has, get their point list
void updateScan(ArrayList<Wall> walls){
void updateScan(PointGraph map){
for(View view : views){
view.look(walls);
}
view.look(map);
slam.RANSAC(view);
for(View view : views){
ArrayList<Vector> pointList = view.getPoints();
// slam.RANSAC(pointList, view.getFOV() / view.getRayNum());
}
}

59
src/Graph/Edge.java Normal file
View File

@@ -0,0 +1,59 @@
package Graph;
public class Edge {
Vertex vStart = null;
Vertex vEnd = null;
float weight = 0;
/**
* @param vStart the vertex the edge starts at
* @param vEnd the vertex the edge ends at
* @param weight the weight of the edge
*/
Edge(Vertex vStart, Vertex vEnd, float weight){
this.vStart = vStart;
this.vEnd = vEnd;
this.weight = weight;
}
/**
* @param vStart the vertex the edge starts at
* @param vEnd the vertex the edge ends at
*/
Edge(Vertex vStart, Vertex vEnd){
this.vStart = vStart;
this.vEnd = vEnd;
}
Edge(){}
/**
* @return the weight of the edge
*/
public float getWeight(){
return weight;
}
/**
* @param newWeight set the edge's weight to something new
*/
public void setWeight(float newWeight){
this.weight = newWeight;
}
/**
* @return the vertex at the end of the edge
*/
public Vertex getEndVertex(){
return vEnd;
}
/**
* @return the vertex at the start of the edge
*/
public Vertex getStartVertex(){
return vStart;
}
}

225
src/Graph/Graph.java Normal file
View File

@@ -0,0 +1,225 @@
package Graph;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.LinkedList;
public class Graph {
/**
* Create a new empty graph
*/
public Graph(){
// hash maps require an initial size to get started
adjList = new HashMap<Vertex, LinkedList<Edge>>(1024);
}
/**
* @return the total number of vertices in the graph
*/
public int numVertices(){
return adjList.size();
}
/**
* @return the total number of edges in the graph.
*/
public int numEdges(){
int sum = 0;
for(Vertex v : adjList.keySet()){
sum += adjList.get(v).size();
}
return sum;
}
/**
* @param v add the vertex v to the graph
*/
public void addVertex(Vertex v){
// check if it's already in the adjacency listVertex
if(adjList.containsKey(v)){
return;
}
adjList.put(v, new LinkedList<Edge>());
}
/**
* @param vStart the starting vertex
* @param vEnd the ending vertex
*/
public void addEdge(Vertex vStart, Vertex vEnd){
// don't add the edge if it is already added
for(Edge e : adjList.get(vStart)){
if(e.getEndVertex() == vEnd){
return;
}
}
adjList.get(vStart).add(new Edge(vStart, vEnd));
}
/**
* @brief Gets how many other vertices a given vertex points to
* @param v the vertex you want to know about
* @return the number of vertices this vertex points to
*/
public int outDegree(Vertex v){
return adjList.get(v).size();
}
/**
* @param v the vertex of interest
* @return the number of edges going into the vertex
*/
public int inDegree(Vertex v){
int count = 0;
for(Vertex v1 : adjList.keySet()){
for(Edge edge : adjList.get(v1)){
if(edge.getEndVertex() == v){
count++;
}
}
}
return count;
}
/**
* Print out all vertexes in the graph
*/
public void print(){
for(Vertex key : adjList.keySet()){
System.out.print("Key: " + key.getLabel() + ": ");
for(Edge edge : adjList.get(key)){
System.out.print(edge.getEndVertex().getLabel());
System.out.print(",");
}
System.out.println("NULL");
}
}
/**
* @param v the vertext to start the traverse at
* @return a list of vertexes in order of when they were visited from first to last.
*/
public ArrayList<Vertex> depthFirstTraverse(Vertex v){
ArrayList<Vertex> vertexList = new ArrayList<Vertex>();
DFSTraverse(v, vertexList);
resetVisits();
return vertexList;
}
/**
* @param v the vertex to begin the traverse at
* @param vertexList the vertex list to append visited vertexes to
*/
private void DFSTraverse(Vertex v, ArrayList<Vertex> vertexList){
vertexList.add(v);
v.setVisitedStatus(true);
LinkedList<Edge> edgeList = adjList.get(v);
for(Edge edge : edgeList){
if(!(edge.getEndVertex().visitedStatus())){
DFSTraverse(edge.getEndVertex(), vertexList);
}
}
}
/**
* @post all vertexes in the graph will be set to unvisited
*/
private void resetVisits(){
for(Vertex v : adjList.keySet()){
v.setVisitedStatus(false);
}
}
/**
* @param v the vertex that you wish to see the neighbors of
* @return a list of vertexes that are connected to v with ingoing or outgoing edges
*/
public ArrayList<Vertex> getNeightborVerts(Vertex v){
// iterate through all of the vertexes and make sure they're marked as unvisited
for(Vertex v1 : adjList.keySet()){
v1.setVisitedStatus(false);
}
ArrayList<Vertex> neighbors = new ArrayList<>();
for(Vertex v1 : adjList.keySet()){
for(Edge edge : adjList.get(v1)){
if(edge.getStartVertex() == v && !edge.getEndVertex().visitedStatus()){
edge.getEndVertex().setVisitedStatus(true);
neighbors.add(edge.getEndVertex());
}
if(edge.getEndVertex() == v && !edge.getStartVertex().visitedStatus()){
edge.getStartVertex().setVisitedStatus(true);
neighbors.add(edge.getStartVertex());
}
}
}
resetVisits();
return neighbors;
}
/**
* @param v the vertex that you wish to start the search at
* @return A list of vertexes in order of visitation where the search goes wide and then deep
*/
public ArrayList<Vertex> breadthFirstSearch(Vertex v){
ArrayList<Vertex> queue = new ArrayList<>();
ArrayList<Vertex> outputList = new ArrayList<>();
outputList.add(v);
queue.add(v);
v.setVisitedStatus(true);
while(queue.size() > 0){
Vertex next = queue.get(0);
queue.remove(0);
LinkedList<Edge> edgeList = adjList.get(next);
for(Edge edge : edgeList){
if(!edge.getEndVertex().visitedStatus()){
queue.add(edge.getEndVertex());
outputList.add(edge.getEndVertex());
edge.getEndVertex().setVisitedStatus(true);
}
}
}
resetVisits();
return outputList;
}
/**
* @brief remove the given vertex and all edges that reference it from the graph
* @param v the vertex to remove from the graph.
*/
public void removeVertex(Vertex v){
// find all edges that point to the removed vertex
ArrayList<Edge> edgesToRemove = new ArrayList<>();
ArrayList<Vertex> startVertex = new ArrayList<>();
for(Vertex v1 : adjList.keySet()){
for(Edge e : adjList.get(v1)){
if(e.getEndVertex() == v){
edgesToRemove.add(e);
startVertex.add(e.getStartVertex());
}
}
}
// remove all of those edges from the adjList
int i = 0;
for(Edge e : edgesToRemove){
adjList.get(startVertex.get(i)).remove(e);
i++;
}
// remove the vertex from the adjacency list
adjList.remove(v);
}
protected HashMap<Vertex, LinkedList<Edge>> adjList;
}

53
src/Graph/LineEdge.java Normal file
View File

@@ -0,0 +1,53 @@
package Graph;
import Vector.*;
import processing.core.PApplet;
import java.awt.*;
import static java.lang.Math.PI;
public class LineEdge extends Edge implements LineInterface{
protected PointVertex vStart;
protected PointVertex vEnd;
protected Line line;
public LineEdge(PointVertex vStart, PointVertex vEnd) {
super(vStart, vEnd);
this.line = new Line(vStart.getPos(), vEnd.getPos());
}
public Vector getDirection(){
return line.getDirection();
}
public Vector getPosition(){
return line.getPosition();
}
public float getLength(){
return line.getLength();
}
public float getAngle(){
return line.getAngle();
}
public Vector endPoint(){
return line.endPoint();
}
public float getDistance(Vector point){
return line.getDistance(point);
}
public void draw(PApplet proc){
line.draw(proc);
Vector leftFlange = line.getDirection().rotate2D((float)(-3*PI/4)).normalize().mul(20);
Vector rightFlange = line.getDirection().rotate2D((float) (3*PI/4)).normalize().mul(20);
Line l1 = new Line(line.endPoint(), line.endPoint().add(leftFlange));
Line l2 = new Line(line.endPoint(), line.endPoint().add(rightFlange));
l1.draw(proc);
l2.draw(proc);
}
}

152
src/Graph/PointGraph.java Normal file
View File

@@ -0,0 +1,152 @@
package Graph;
import Vector.Vector;
import processing.core.PApplet;
import java.io.File; // Import the File class
import java.io.FileWriter;
import java.io.IOException; // Import the IOException class to handle errors
import static java.lang.Math.pow;
import static java.lang.Math.sqrt;
import java.util.ArrayList;
public class PointGraph extends Graph {
PointVertex selectedVertex;
public PointGraph(){
super();
}
public void draw(PApplet proc){
for(Vertex v : adjList.keySet()){
PointVertex v1 = (PointVertex) v;
v1.draw(proc);
for(Edge e : adjList.get(v)){
LineEdge e1 = (LineEdge) e;
e1.draw(proc);
}
}
}
public void addEdge(PointVertex vStart, PointVertex vEnd){
addVertex(vStart);
// don't add the edge if it is already added
for(Edge e : adjList.get(vStart)){
if(e.getEndVertex() == (Vertex) vEnd){
return;
}
}
addVertex(vEnd);
LineEdge edge = new LineEdge(vStart, vEnd);
adjList.get((Vertex) vStart).add(new LineEdge(vStart, vEnd));
}
/**
* @param v set this vertex as the selected vertex in the graph
*/
public void setSelectedVertex(PointVertex v){
if(selectedVertex != null){
deselectVertex();
}
selectedVertex = v;
selectedVertex.setColor(new int[]{255, 0, 255, 0});
}
/**
* @pre a vertex needs to be selected
* @return the current pointvertex which is selected in the graph
*/
public PointVertex getSelectedVertex(){
if(selectedVertex == null){
if(super.adjList.size() > 0){
selectedVertex = (PointVertex)super.adjList.keySet().iterator().next();
}
else{
throw new NullPointerException();
}
}
return selectedVertex;
}
/**
* @return true if a vertex is currently selected in the graph
*/
public boolean vertexIsSelected(){
return selectedVertex != null;
}
/**
* @brief deselect the currently selected vertex
*/
public void deselectVertex(){
if(selectedVertex == null){
return;
}
selectedVertex.setColor(new int[]{127, 255, 0, 0});
selectedVertex = null;
}
/**
* @param x the x coordinate to search by
* @param y the y coordinate to search by
* @return the pointvertex closest to the given x, y coordinates
*/
public PointVertex getClosestVertex(Vector point){
if(super.adjList.size() == 0){
// TODO: choose a better exception name
throw new NullPointerException();
}
PointVertex closestVertex = (PointVertex) super.adjList.keySet().iterator().next();
float closestDist = -1;
for(Vertex v : super.adjList.keySet()){
PointVertex v1 = (PointVertex) v;
Vector p2 = v1.getPos();
float dist = p2.sub(point).mag();
if(dist < closestDist || closestDist == -1){
closestDist = dist;
closestVertex = v1;
}
}
return closestVertex;
}
public void removeVertex(PointVertex v){
if(selectedVertex == v){
selectedVertex = null;
}
super.removeVertex(v);
}
/**
* @return all edges in the graph
*/
public ArrayList<LineEdge> getAllEdges(){
ArrayList<LineEdge> edges = new ArrayList<>();
for(Vertex v : adjList.keySet()){
for(Edge e : adjList.get(v)){
LineEdge e1 = (LineEdge) e;
edges.add(e1);
}
}
return edges;
}
/**
* @return all vertexes in the graph
*/
public ArrayList<PointVertex> getAllVertexes(){
ArrayList<PointVertex> points = new ArrayList<>();
for(Vertex v : adjList.keySet()){
PointVertex v1 = (PointVertex) v;
points.add(v1);
}
return points;
}
}

115
src/Graph/PointGraphWriter.java Normal file
View File

@@ -0,0 +1,115 @@
package Graph;
import java.io.File;
import java.io.FileNotFoundException;
import java.io.FileWriter;
import java.io.IOException;
import java.util.ArrayList;
import java.util.Scanner;
public class PointGraphWriter {
public void save(String filename, PointGraph g) throws IOException {
FileWriter file = new FileWriter(filename);
file.write("numVerts," + g.numVertices());
file.write("\nnumEdges," + g.numEdges());
// turn the hash map into something linear
ArrayList<PointVertex> verts = g.getAllVertexes();
ArrayList<LineEdge> edges = g.getAllEdges();
// save the vertexes
int countVerts = 0;
for(PointVertex v : verts){
// save the vertex position
file.write("\nvert,"+v.getPos().x+","+v.getPos().y);
countVerts++;
}
// save the edges
for(Edge e : edges){
int idx = 0;
file.write("\nedge,");
boolean otherIsWritten = false;
for(PointVertex v : verts){
if(e.getStartVertex() == (Vertex)v){
file.write("start,"+ idx);
if(!otherIsWritten){
file.write(",");
otherIsWritten = true;
}
}
else if(e.getEndVertex() == (Vertex)v){
file.write("end," + idx);
if(!otherIsWritten){
file.write(",");
otherIsWritten = true;
}
}
idx++;
}
}
file.close();
}
public PointGraph loadFile(String filename) throws FileNotFoundException, NumberFormatException {
PointGraph g = new PointGraph();
File file = new File(filename);
Scanner reader = new Scanner(file);
ArrayList<PointVertex> vertices = new ArrayList<>();
while(reader.hasNextLine()){
String line = reader.nextLine();
ArrayList<String> args = parseLine(line);
String key = args.get(0);
switch (key) {
case "numVerts" -> System.out.println("Number of Vertexes: " + Integer.parseInt(args.get(1)));
case "numEdges" -> System.out.println("Number of Edges: " + Integer.parseInt(args.get(1)));
case "vert" -> {
float x = Float.parseFloat(args.get(1));
float y = Float.parseFloat(args.get(2));
PointVertex v = new PointVertex(x, y);
g.addVertex(v);
vertices.add(v);
}
case "edge" -> {
int startIdx;
int endIdx;
if (args.get(1).contains("start")) {
startIdx = Integer.parseInt(args.get(2));
endIdx = Integer.parseInt(args.get(4));
}
else{
startIdx = Integer.parseInt(args.get(4));
endIdx = Integer.parseInt(args.get(2));
}
g.addEdge(vertices.get(startIdx), vertices.get(endIdx));
}
default -> System.out.println("Unrecognized Line: " + line);
}
}
return g;
}
private ArrayList<String> parseLine(String line){
ArrayList<String> args = new ArrayList<>();
StringBuilder arg = new StringBuilder();
for(char letter : line.toCharArray()){
if(letter == ','){
args.add(arg.toString());
arg = new StringBuilder();
continue;
}
if(letter == '\n'){
args.add(arg.toString());
break;
}
arg.append(letter);
}
if(!args.get(args.size()-1).contains(arg.toString())){
args.add(arg.toString());
}
return args;
}
}

65
src/Graph/PointVertex.java Normal file
View File

@@ -0,0 +1,65 @@
package Graph;
import Vector.Vector;
import processing.core.PApplet;
public class PointVertex extends Vertex {
private Vector position;
private int[] color = new int[]{127, 255, 0, 0};
/**
* @param xPos the x position of the vertex
* @param yPos the y posiiton of the vertex
*/
public PointVertex(float xPos, float yPos){
this.position = new Vector(xPos, yPos);
}
public PointVertex(Vector position){
super();
this.position = position;
}
/**
* @param xPos the x position of the vertex
* @param yPos the y posiiton of the vertex
* @param label the label of the vertex
*/
PointVertex(float xPos, float yPos, String label){
super(label);
this.position = new Vector(xPos, yPos);
}
/**
* @param x the new x position of the vertex
* @param y the new y posiiton of the vertex
*/
public void setPos(float x, float y){
this.position = new Vector(x, y);
}
/**
* @return a two eleement float array containing the x and y coordinates of the vertex respectively.
*/
public Vector getPos(){
return position;
}
/**
* @param newColor a 4 element int array containing th alpha, r, g ,and b components respectively
*/
public void setColor(int[] newColor){
this.color = newColor;
}
/**
* @return a 4 element int array containing th alpha, r, g ,and b components respectively
*/
public int[] getColor(){
return color;
}
public void draw(PApplet proc){
proc.stroke(color[1], color[2], color[3], color[0]);
proc.circle(position.x, position.y, 20);
}
}

45
src/Graph/Vertex.java Normal file
View File

@@ -0,0 +1,45 @@
package Graph;
public class Vertex {
private String vertexLabel;
private boolean isVisitedStatus = false;
/**
* Create a new vertex with a default label
*/
public Vertex(){
this.vertexLabel = "Unassigned";
}
/**
* @param label create a new vertex with this label
*/
public Vertex(String label){
this.vertexLabel = label;
}
/**
* @return the label of the vertex
*/
public String getLabel(){
return vertexLabel;
}
public void setLabel(String newLabel){
vertexLabel = newLabel;
}
/**
* @return if the node has been visited or not
*/
public boolean visitedStatus(){
return this.isVisitedStatus;
}
/**
* @param status set the visited status to true or false
*/
public void setVisitedStatus(boolean status){
this.isVisitedStatus = status;
}
}

93
src/Processing.java
View File

@@ -1,13 +1,20 @@
import Graph.*;
import Vector.Vector;
import processing.core.PApplet;
import java.util.ArrayList;
import java.io.FileNotFoundException;
import java.io.IOException;
public class Processing extends PApplet {
Car car;
ArrayList<Wall> objects = new ArrayList<>();
public static PApplet processing;
PointGraph map = new PointGraph();
boolean mapIsHidden = false;
boolean SLAMIsHidden = false;
public static void main(String[] args) {
PApplet.main("Processing");
}
@@ -16,18 +23,21 @@ public class Processing extends PApplet {
processing = this;
car = new Car(processing, 100,100,50,40);
size(1000, 1000);
car.addView(180,90);
for(int i = 0; i < 15; 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)));
objects.add(wall);
}
car.addView(360,360);
// for(int i = 0; i < 10; i++){
// PointVertex vStart = new PointVertex(random(50, 950), random(50, 950));
// PointVertex vEnd = new PointVertex(random(50, 950), random(50, 950));
// map.addEdge(vStart, vEnd);
// }
}
public void draw(){
background(0);
for(Wall object : objects){
object.drawWall();
if(!mapIsHidden){
map.draw(processing);
}
car.drawCar(objects);
car.drawCar(map, SLAMIsHidden);
strokeWeight(2);
stroke(255);
//car.drive(new int[] {0, 0});
@@ -52,7 +62,70 @@ public class Processing extends PApplet {
if(key == 'e'){
car.setAngle(car.getAngle()-1);
}
if(key == DELETE && map.vertexIsSelected()){
map.removeVertex(map.getSelectedVertex());
}
if(key == ' ' && map.vertexIsSelected()){
map.deselectVertex();
}
if(key == ESC){
System.out.println("Attempting to save map to file.");
try{
PointGraphWriter writer = new PointGraphWriter();
writer.save("map.txt", map);
}
catch(IOException e){
e.printStackTrace();
}
}
if(key == 'l'){
System.out.println("Attempting to load a map from file");
try{
PointGraphWriter writer = new PointGraphWriter();
map = writer.loadFile("map.txt");
}
catch (FileNotFoundException e){
System.out.println("File not found");
e.printStackTrace();
}
catch (NumberFormatException e){
System.out.println("Number format incorrect");
e.printStackTrace();
}
}
if(key == 'h'){
mapIsHidden = !mapIsHidden;
}
if(key == 'j'){
SLAMIsHidden = !SLAMIsHidden;
}
}
public void mousePressed(){
Vector clickPosition = new Vector(mouseX, mouseY);
if(map.numVertices() == 0){
PointVertex v = new PointVertex(clickPosition);
map.addVertex(v);
return;
}
PointVertex closestVertex = map.getClosestVertex(clickPosition);
float distance = closestVertex.getPos().sub(clickPosition).mag();
if(distance < 15){
if(map.vertexIsSelected()){
if(map.getSelectedVertex() == closestVertex){
map.deselectVertex();
}
else{
map.addEdge(map.getSelectedVertex(), closestVertex);
}
return;
}
map.setSelectedVertex(closestVertex);
return;
}
PointVertex v = new PointVertex(clickPosition);
map.addVertex(v);
}

27
src/Ray.java
View File

@@ -1,11 +1,13 @@
import Graph.*;
import Vector.*;
import processing.core.PApplet;
import processing.core.PVector;
import java.util.ArrayList;
import static processing.core.PApplet.*;
public class Ray extends Line{
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)
@@ -25,17 +27,17 @@ 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){
public void castRay(PointGraph map){
float shortestWallDistance = maxRayDistance;
int[] newColor = new int[]{255, 255, 255};
for(Wall wall : walls){
ArrayList<LineEdge> walls = map.getAllEdges();
for(LineEdge 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 d2 = wall.getDirection();
Vector p1 = this.position;
Vector p2 = wall.position;
Vector p2 = wall.getPosition();
// 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);
@@ -50,7 +52,6 @@ public class Ray extends Line{
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};
}
}
@@ -58,11 +59,9 @@ public class Ray extends Line{
// 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};
}
}
@@ -72,13 +71,13 @@ public class Ray extends Line{
public boolean hasCollided(){
return this.direction.mag() != maxRayDistance;
return abs(this.direction.mag() - maxRayDistance) > 0.001;
}
//returns the absolute position of the point
public Vector getPoint(){
if(this.direction.mag() == 0){
return new Vector();
return this.position;
}
return this.position.add(this.direction);
@@ -91,8 +90,8 @@ public class Ray extends Line{
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);
float currentAngle = direction.angle();
this.direction = direction.rotate2D(angle - currentAngle);
}
}

99
src/SLAM.java
View File

@@ -1,13 +1,17 @@
import Vector.*;
import processing.core.*;
import java.lang.reflect.Array;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import static java.lang.Math.random;
import static processing.core.PApplet.*;
public class SLAM{
ArrayList<Line> lines = new ArrayList<>();
ShortTermMem unassociatedPoints = new ShortTermMem();
private static PApplet proc;
SLAM(PApplet processing){
@@ -16,38 +20,41 @@ public class SLAM{
/**
* @param set the set to take a sub sample of
* @param indexRange the range within to take the sub sample
* @param subSampleSize the size of the sub sample
* @return A random subset of the set within an indexRange and of size: subSampleSize
* @param minAngle the minimum angle allowed in the subset
* @param maxAngle the maximum angle allowed in the subset
* @return A random subset of the set within the angle range
*/
private List<Vector> randomSample(ArrayList<Vector> set, int indexRange, int subSampleSize){
// select a random laser data reading
int randomIdx = (int) proc.random(set.size() - 1); // index of starter reading
Vector point = set.get(randomIdx); // point of starter reading
private List<Vector> randomSampleInAngleRange(ArrayList<Vector> set, int subSampleSize, float minAngle, float maxAngle){
// get a random sample of size numSampleReadings within degreeRange degrees of this laser reading.
List<Vector> subSample;
int rangeStart = randomIdx - indexRange >= 0 ? randomIdx - indexRange : 0;
int rangeEnd = randomIdx + indexRange < set.size() ? randomIdx + indexRange : set.size()-1;
subSample = set.subList(rangeStart, rangeEnd); // get the sub-sample
Collections.shuffle(subSample); // shuffle the list
List<Vector> randomSample = subSample.subList(0, rangeEnd-rangeStart); // get our random sample
if (!randomSample.contains(point)) {
randomSample.add(point);
// create an arraylist with all points within the angle range from the given set
ArrayList<Vector> pointsInAngleRange = new ArrayList<>();
for(Vector point : set){
if(minAngle <= point.z && point.z <= maxAngle){
pointsInAngleRange.add(point);
}
}
return randomSample;
// shuffle the list to randomize it
Collections.shuffle(pointsInAngleRange);
// if the list is too small, just return the whole list
if(pointsInAngleRange.size() < subSampleSize){
return pointsInAngleRange;
}
// return a subSample of the list
return pointsInAngleRange.subList(0, subSampleSize);
}
/**
* @param originalList the list which the randomSample of points originated from
* @param randomSample a random subsampling of points from the originalList
* @param maxRange the maximum distance away from the line of best fit of the subSample of points for a given point's consensus to count.
* @param consensus the number of points that have to give their consensus for the line of best fit to count as a valid feature.
*/
private void extractFeature(ArrayList<Vector> originalList, List<Vector> randomSample, float maxRange, int consensus){
private void extractFeature(List<Vector> randomSample, float maxRange, int consensus){
// get a line of best fit for this list.
Line bestFit = new Line(randomSample);
// check that there are enough points in the sample that are less than the maxRange away to form a consensus
int count = 0;
ArrayList<Vector> newRandomSample = new ArrayList<>();
for (Vector v : randomSample) {
@@ -62,44 +69,66 @@ public class SLAM{
lines.add(bestFit);
// remove the associated readings from the total available readings.
for (Vector v : newRandomSample) {
originalList.remove(v);
this.unassociatedPoints.remove(v);
}
}
}
/**
* @param newPoints a new scan of points to perform feature detection on
* @param raysPerDegree How many degrees apart are each ray that was cast
*/
public void RANSAC(ArrayList<Vector> newPoints, float raysPerDegree){
float degreeRange = radians(10/2); // range to randomly sample readings within
int indexRange = (int) (degreeRange / raysPerDegree);
int numSampleReadings = 10; // number of readings to randomly sample
// constrain numSampleReadings so that it cant be higher than possible
if(numSampleReadings >= 2 * indexRange){
numSampleReadings = 2 * indexRange;
private void fitToPreviousReadings(List<Vector> sample, float maxRange){
// keep track of points that were succesffully associated so they can be removed from the sample at the end
ArrayList<Vector> pointsToRemove = new ArrayList<>();
// try to associate points from the smaple with pre-existing lines
for(Vector v: sample){
for(Line l : lines){
if(l.getDistance(v) < maxRange){
l.refitLine(v);
pointsToRemove.add(v);
}
int consensus = 6; // the number of points that need to lie near a line for it to be considered valid.
}
}
for(Vector v : pointsToRemove){
sample.remove(v);
}
}
/**
* @param view a laser scan view
*/
public void RANSAC(View view){
unassociatedPoints.addScan(view.getPos(), view.getPoints());
float degreeRange = radians(5); // range to randomly sample readings within
int numSampleReadings = 15; // number of readings to randomly sample
int consensus = 10; // the number of points that need to lie near a line for it to be considered valid.
float maxRange = 10; // the maximum distance a point can be away from the line for it to count as a consensus
// this for loop determines the maximum number of trials we're willing to do.
for(int j = 0; j < 20; j++) {
// if there aren't enough points left in the set to form a consensus, we're done.
if(newPoints.size() < consensus){
if(this.unassociatedPoints.size() < maxRange){
break;
}
// get a random angle between -PI and PI
float randomAngle = (float) (2*PI*(random()) - 0.5);
// get a random sub sample of newPoints within the index range of a given size
List<Vector> randomSample = this.randomSample(newPoints, indexRange, numSampleReadings);
List<Vector> randomSample = this.randomSampleInAngleRange(this.unassociatedPoints.getPoints(), numSampleReadings, randomAngle-degreeRange, randomAngle+degreeRange);
if(randomSample.size() >= numSampleReadings){
// try to associate points from the sample with previously made lines
fitToPreviousReadings(randomSample, maxRange);
// check if the sub sample forms a valid line and remove the randomSample points if it does.
extractFeature(newPoints, randomSample, maxRange, consensus);
extractFeature(randomSample, maxRange, consensus);
}
}
}
public void drawLines(){
public void drawFeatures(PApplet proc){
for(Line line : lines){
line.draw(proc);
}

59
src/ShortTermMem.java Normal file
View File

@@ -0,0 +1,59 @@
import java.util.ArrayList;
import Vector.Vector;
public class ShortTermMem {
ArrayList<ArrayList<Vector>> scans = new ArrayList<>();
ArrayList<Vector> scanPositions = new ArrayList<>();
ArrayList<Long> scanTimes = new ArrayList<>();
private int size = 0;
public void addScan(Vector scanPosition, ArrayList<Vector> scan){
size += scan.size();
scans.add(scan);
scanPositions.add(scanPosition);
scanTimes.add(System.currentTimeMillis());
purgeScans();
}
public ArrayList<Vector> getPoints(){
ArrayList<Vector> points = new ArrayList<>();
for(ArrayList<Vector> pointList : this.scans){
points.addAll(pointList);
}
return points;
}
public void remove(Vector point){
for(ArrayList<Vector> pointList : this.scans){
int listSize = pointList.size();
pointList.remove(point);
if(listSize - pointList.size() != 0){
size--;
break;
}
}
}
private void purgeScans(){
long currentTime = System.currentTimeMillis();
int i = scanTimes.size();
// loop through the list backwards and remove all scans that are over second old
// we loop backwards to avoid removal conflicts
while(i > 0){
i--;
long dt = currentTime - scanTimes.get(i);
if(dt < 1000){
continue;
}
size -= scans.get(i).size();
scanTimes.remove(i);
scanPositions.remove(i);
scans.remove(i);
}
}
public int size(){
return this.size;
}
}

56
src/Line.java → src/Vector/Line.java
View File

@@ -1,16 +1,22 @@
package Vector;
import Vector.Vector;
import processing.core.PApplet;
import java.util.ArrayList;
import java.util.List;
import static processing.core.PApplet.*;
public class Line{
public class Line implements LineInterface{
// vector which represents the direction and length of the line from its starting position
protected Vector direction = new Vector(0,0);
// store the starting position of the line
protected Vector position = new Vector(0,0);
Line(Vector startPosition, Vector endPosition){
protected ArrayList<Vector> points = new ArrayList<>();
public Line(Vector startPosition, Vector endPosition){
direction = endPosition.sub(startPosition);
position = startPosition;
}
@@ -19,12 +25,20 @@ public class Line{
* attempt to find the line of best fit for the given points
* @param points the points to get the line of best for
*/
Line(List<Vector> points){
public Line(List<Vector> points){
bestFit(points);
}
public void refitLine(Vector newPoint){
// add the new point to our list
this.points.add(newPoint);
// rerun the bestFit algorithm with the new point
bestFit(new ArrayList<>());
}
// least squares line of best fit algorithm
private void bestFit(List<Vector> points){
public void bestFit(List<Vector> fitPoints){
this.points.addAll(fitPoints);
// get the mean of all the points
Vector mean = new Vector();
for(Vector point : points){
@@ -34,26 +48,28 @@ public class Line{
// this section calculates the direction vector of the line of best fit
Vector direction = new Vector();
float length = 1;
float length = 0;
// get the rise and run of the line of best fit
for(Vector point : points){
direction.y += (point.x - mean.x)*(point.y - mean.y); // rise
direction.x += pow((point.x - mean.x),2);
// find the point that's furthest from the mean and use it to set the line length.
// get the average distance avery point is away from the mean.
float dist = abs(point.sub(mean).mag());
if(dist > length){
length = 2*dist;
}
length += dist;
}
length = 2f*length/points.size();
// if the direction is perfectly vertical create a line to represent that.
if(direction.y == 0){
this.direction = new Vector(0, 1);
}
else{
this.direction = new Vector(1, direction.y/direction.x);
}
// scale the direction vector to be the correct length of the line.
this.direction = this.direction.normalize().mul(length);
this.position = mean.sub(this.direction.div(2));
}
@@ -75,22 +91,24 @@ public class Line{
return direction.mag();
}
public void draw(PApplet screen){
Vector endPoint = this.position.add(this.direction);
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
*/
public float getDistance(Vector point){
return (point.sub(position).cross(direction)).mag() / direction.mag();
float line_dist = direction.mag();
if(line_dist == 0) return this.position.sub(point).mag();
Vector l2 = this.endPoint();
float t = ((point.x - position.x) * (l2.x - position.x) + (point.y - position.y) * (l2.y - position.y) + (point.z - position.z) * (l2.z - position.z)) / line_dist;
t = constrain(t, 0, 1);
Vector closestPoint = new Vector(position.x + t * (l2.x - position.x), position.y + t * (l2.y - position.y), position.z + t * (l2.z - position.z));
return closestPoint.sub(point).mag();
}
public void draw(PApplet proc){
proc.line(position.x, position.y, endPoint().x, endPoint().y);
}
}

20
src/Vector/LineInterface.java Normal file
View File

@@ -0,0 +1,20 @@
package Vector;
import processing.core.PApplet;
public interface LineInterface {
Vector getDirection();
Vector getPosition();
float getLength();
float getAngle();
Vector endPoint();
float getDistance(Vector point);
void draw(PApplet proc);
}

48
src/Vector.java → src/Vector/Vector.java
View File

@@ -1,4 +1,10 @@
package Vector;
import processing.core.PApplet;
import static java.lang.Math.*;
import static processing.core.PApplet.cos;
import static processing.core.PApplet.sin;
public class Vector {
public float x = 0;
@@ -6,62 +12,62 @@ public class Vector {
public float z = 0;
Vector(){}
Vector(float x, float y){
public Vector(float x, float y){
this.x = x;
this.y = y;
}
Vector(float x, float y, float z){
public Vector(float x, float y, float z){
this.x = x;
this.y = y;
this.z = z;
}
Vector add(Vector other){
public Vector add(Vector other){
return new Vector(this.x + other.x, this.y + other.y, this.z + other.z);
}
Vector add(float x, float y){
public Vector add(float x, float y){
return new Vector(this.x + x, this.y + y);
}
Vector add(float x, float y, float z){
public Vector add(float x, float y, float z){
return new Vector(this.x + x, this.y + y, this.z + z);
}
Vector sub(Vector other){
public Vector sub(Vector other){
return new Vector(this.x - other.x, this.y - other.y, this.z - other.z);
}
Vector sub(float x, float y){
public Vector sub(float x, float y){
return new Vector(this.x - x, this.y - y);
}
Vector sub(float x, float y, float z){
public Vector sub(float x, float y, float z){
return new Vector(this.x - x, this.y - y, this.z - z);
}
Vector mul(float scalar){
public Vector mul(float scalar){
return new Vector(this.x * scalar, this.y * scalar, this.z * scalar);
}
Vector div(float scalar){
public Vector div(float scalar){
return mul(1/scalar);
}
float mag(){
public float mag(){
return (float)sqrt(x*x + y*y + z*z);
}
float dot(Vector other){
public float dot(Vector other){
return x * other.x + y * other.y + z * other.z;
}
Vector cross(Vector other){
public Vector cross(Vector other){
return new Vector(this.y*other.z - this.z*other.y, this.z*other.x - this.x*other.z, this.x*other.y - this.y*other.x);
}
Vector normalize(){
public Vector normalize(){
float mag = this.mag();
return new Vector(x / mag, y / mag, z / mag);
}
@@ -70,14 +76,24 @@ public class Vector {
* @param other
* @return
*/
float angleDiff(Vector other){
public float angleDiff(Vector other){
float dot = this.dot(other); // dot product
float det = this.x*other.y - this.y*other.x; // determinant
float angle = (float) atan2(det, dot); // atan2(y, x) or atan2(sin, cos)
return angle;
}
float angle(){
public float angle(){
return (float) atan2(y, x);
}
public Vector rotate2D(float angle){
float distance = mag();
float currentAngle = this.angle();
return new Vector(cos(currentAngle + angle), sin(currentAngle + angle)).mul(distance);
}
public void draw(PApplet proc){
proc.circle(this.x, this.y, 8);
}
}

42
src/View.java
View File

@@ -1,8 +1,7 @@
import Graph.PointGraph;
import Vector.Vector;
import processing.core.*;
import java.util.ArrayList;
import java.util.Objects;
import static processing.core.PApplet.*;
public class View {
Vector pose;
@@ -23,7 +22,7 @@ public class View {
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
float angle = (float) (angleOffset); //the 0.01 fixes some bugs
for (int i = 0; i < numberOfRays; i++) {
Ray ray = new Ray(pose, angle);
angle = angle + rayStep;
@@ -32,10 +31,13 @@ public class View {
}
//sees if the ray will collide with the walls in the wall list
public void look(ArrayList<Wall> walls) {
public void look(PointGraph map) {
for (Ray ray : rays) {
ray.castRay(walls);
ray.drawRay(proc);
ray.castRay(map);
if(ray.hasCollided()){
ray.getPoint().draw(proc);
// ray.drawRay(proc);
}
}
}
@@ -50,7 +52,10 @@ public class View {
//changes the angle of the view
public void setAngle(float angle) {
this.angle = angle;
this.setRayNum(rays.size(), this.FOV, angle);
for(Ray ray : rays){
float angleOffset = ray.getAngle() - this.angle;
ray.setAngle(this.angle+angleOffset);
}
}
//changes the field of view of the view
@@ -80,12 +85,27 @@ public class View {
ArrayList<Vector> points = new ArrayList<>();
for (Ray ray : rays) {
if (!Objects.equals(ray.getPoint(), new Vector(0, 0) {
})) {
points.add(ray.getPoint());
if(ray.hasCollided()){
Vector point = ray.getPoint();
// store the angle information for that point in the z coordinate
point.z = ray.getAngle();
points.add(point);
}
}
return points;
}
/**
* @return A list of the angles where a collision was detected
*/
public ArrayList<Float> getAngles(){
ArrayList<Float> angles = new ArrayList<>();
for (Ray ray : rays) {
if (ray.hasCollided()){
angles.add(ray.getAngle());
}
}
return angles;
}
}

39
src/Wall.java
View File

@@ -1,39 +0,0 @@
import processing.core.*;
import static processing.core.PApplet.*;
public class Wall extends Line{
int r;
int g;
int b;
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);
}
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);
}
void drawWall(){
proc.stroke(r,g,b);
proc.strokeWeight(10);
proc.circle(position.x, position.y, 10);
proc.strokeWeight(2);
proc.line(position.x, position.y, position.x + direction.x, position.y + direction.y);
}
Vector getPos(){
return position;
}
}

20
tests/LineTest.java
View File

@@ -1,8 +1,8 @@
import Vector.Vector;
import Vector.Line;
import org.junit.jupiter.api.Test;
import processing.core.PApplet;
import java.util.ArrayList;
import java.util.List;
import java.util.Random;
import static java.lang.Math.abs;
@@ -83,20 +83,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);
}
}

6
tests/VectorTest.java
View File

@@ -1,5 +1,5 @@
import Vector.Vector;
import org.junit.jupiter.api.Test;
import processing.core.PApplet;
import static java.lang.Math.*;
import static org.junit.jupiter.api.Assertions.*;
@@ -28,8 +28,8 @@ class VectorTest{
assertFloatEquals((float)sqrt(x2*x2 + y2*y2), v2.mag());
// test dot product
assertFloatEquals((float)(x1*x2+y1*y2), v1.dot(v2));
assertFloatEquals((float)(x1*x2+y1*y2), v2.dot(v1));
assertFloatEquals((x1*x2+y1*y2), v1.dot(v2));
assertFloatEquals((x1*x2+y1*y2), v2.dot(v1));
// test addition
Vector vSum = v1.add(v2);