Added a Graph datastructure and refactored the Line/Vertex classes

.idea/uiDesigner.xml

@@ -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>

src/Car.java

@@ -3,6 +3,8 @@ import java.util.ArrayList;
 import static java.lang.Math.PI;
 import static processing.core.PApplet.degrees;
 import static processing.core.PApplet.radians;
+
+import Vector.Vector;
 import processing.core.PApplet;
 
 public class Car{

src/Graph/Edge.java

@@ -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;
+    }
+
+}
+

src/Graph/Graph.java

@@ -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;
+}
+

src/Graph/LineEdge.java

@@ -0,0 +1,45 @@
+package Graph;
+
+import Vector.*;
+import processing.core.PApplet;
+
+import java.awt.*;
+
+public class LineEdge extends Edge implements LineInterface{
+    protected PointVertex vStart;
+    protected PointVertex vEnd;
+    protected Line line;
+    public LineEdge(PointVertex vStart, PointVertex vEnd) {
+        this.vStart = vStart;
+        this.vEnd = 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);
+    }
+}

src/Graph/PointGraph.java

@@ -0,0 +1,219 @@
+package Graph;
+
+import Vector.Vector;
+import processing.core.PApplet;
+
+import static java.lang.Math.pow;
+import static java.lang.Math.sqrt;
+
+import java.util.ArrayList;
+
+
+public class PointGraph extends Graph {
+    PointVertex selectedVertex;
+    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);
+            }
+        }
+    }
+
+    /**
+     * @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(float x, float y){
+        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 = (float)sqrt(pow(x-p2.x, 2) + pow(y-p2.y, 2));
+            if(dist < closestDist || closestDist == -1){
+                closestDist = dist;
+                closestVertex = v1;
+            }
+        }
+
+        return closestVertex;
+    }
+
+    /**
+     * @return a bundle with all of the graphs vertex and edge information saved into it
+     */
+    /*
+    public Bundle saveToBundle(){
+        Bundle bundle = new Bundle();
+        bundle.putInt("numVerts", super.numVertices());
+        bundle.putInt("numEdges", super.numEdges());
+
+        // turn the hash map into something linear
+        ArrayList<PointVertex> verts = new ArrayList<>();
+        ArrayList<Edge> edges = new ArrayList<>();
+        for(Vertex v : super.adjList.keySet()){
+            verts.add((PointVertex) v);
+            for(Edge e : super.adjList.get(v)){
+                edges.add(e);
+            }
+        }
+
+        // save the vertexes
+        int countVerts = 0;
+        for(PointVertex v : verts){
+            String countVertsString = "vert"+String.valueOf(countVerts);
+            // save the vertex position
+            bundle.putFloatArray(countVertsString+"Pos", v.getPos());
+            // save the vertex label
+            bundle.putString(countVertsString+"Label", v.getLabel());
+
+            // save if the vertex is selected
+            if(selectedVertex != null){
+                // save if it is the selected vertex
+                bundle.putBoolean(countVertsString+"isSelected", v == selectedVertex);
+            }
+            else{
+                bundle.putBoolean(countVertsString+"isSelected", false);
+            }
+            countVerts++;
+        }
+
+        // save the edges
+        int countEdges = 0;
+        for(Edge e : edges){
+            int idx = 0;
+            String countEdgesString = "edge" + String.valueOf(countEdges);
+            for(PointVertex v : verts){
+                if(e.getStartVertex() == (Vertex)v){
+                    bundle.putInt(countEdgesString+"Start", idx);
+                }
+                else if(e.getEndVertex() == (Vertex)v){
+                    bundle.putInt(countEdgesString+"End", idx);
+                }
+                idx++;
+            }
+            countEdges++;
+        }
+        return bundle;
+    }
+    */
+    /**
+     * @ brief add all graph information in ta bundle to the graph
+     * @param bundle the bundle to add to the graph
+     */
+    /*
+    public void addBundleToGraph(Bundle bundle){
+        int numVerts = bundle.getInt("numVerts", 0);
+        int numEdges = bundle.getInt("numEdges", 0);
+        ArrayList<PointVertex> verts = new ArrayList<>();
+
+        // add all of the vertexes from the bundle
+        for(int i = 0; i < numVerts; i++){
+            String countVertsString = "vert"+String.valueOf(i);
+            float[] pos = bundle.getFloatArray(countVertsString+"Pos");
+            String label = bundle.getString(countVertsString+"Label");
+            boolean isSelected = bundle.getBoolean(countVertsString+"isSelected");
+            PointVertex v = new PointVertex(pos[0], pos[1], label);
+            addVertex(v);
+            verts.add(v);
+            if(isSelected){
+                setSelectedVertex(v);
+            }
+        }
+
+        // add all of the edges from the bundle
+        for(int i = 0; i < numEdges; i++){
+            String countEdgesString = "edge" + String.valueOf(i);
+            int startVertIndex = bundle.getInt(countEdgesString+"Start");
+            int endVertIndex = bundle.getInt(countEdgesString+"End");
+            PointVertex vStart = verts.get(startVertIndex);
+            PointVertex vEnd = verts.get(endVertIndex);
+            addEdge(vStart, vEnd);
+        }
+    }
+    */
+
+    /**
+     * @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;
+    }
+}

src/Graph/PointVertex.java

@@ -0,0 +1,61 @@
+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
+     */
+    PointVertex(float xPos, float yPos){
+        super();
+        this.position = new Vector(xPos, yPos);
+    }
+
+    /**
+     * @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);
+    }
+}

src/Graph/Vertex.java

@@ -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;
+    }
+}

src/Processing.java

@@ -1,3 +1,4 @@
+import Vector.Vector;
 import processing.core.PApplet;
 
 import java.util.ArrayList;
@@ -16,7 +17,7 @@ public class Processing extends PApplet {
         processing = this;
         car = new Car(processing, 100,100,50,40);
         size(1000, 1000);
-        car.addView(180,90);
+        car.addView(360,180);
         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);
@@ -24,10 +25,16 @@ public class Processing extends PApplet {
     }
     public void draw(){
         background(0);
-        for(Wall object : objects){
+//        for(Wall object : objects){
-            object.drawWall();
+//            object.drawWall();
-        }
+//        }
         car.drawCar(objects);
+        View view = car.views.get(0);
+        view.look(objects);
+        ArrayList<Vector> points = view.getPoints();
+        for(Vector point: points){
+            circle(point.x, point.y, 5);
+        }
         strokeWeight(2);
         stroke(255);
         //car.drive(new int[] {0, 0});

src/Ray.java

@@ -1,11 +1,12 @@
+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)
@@ -33,9 +34,9 @@ public class Ray extends Line{
             // 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);
@@ -72,13 +73,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);

src/SLAM.java

@@ -1,3 +1,4 @@
+import Vector.*;
 import processing.core.*;
 
 import java.util.ArrayList;
@@ -99,10 +100,4 @@ public class SLAM{
 
     }
 
-    public void drawLines(){
-        for(Line line : lines){
-            line.draw(proc);
-        }
-    }
-
 }

src/Vector/Line.java

@@ -1,16 +1,19 @@
+package Vector;
+
+import Vector.Vector;
 import processing.core.PApplet;
 
 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){
+    public Line(Vector startPosition, Vector endPosition){
         direction = endPosition.sub(startPosition);
         position = startPosition;
     }
@@ -19,7 +22,7 @@ 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);
     }
 
@@ -75,11 +78,6 @@ 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(){
@@ -93,4 +91,8 @@ public class Line{
     public float getDistance(Vector point){
         return (point.sub(position).cross(direction)).mag() / direction.mag();
     }
+
+    public void draw(PApplet proc){
+        proc.line(position.x, position.y, endPoint().x, endPoint().y);
+    }
 }

src/Vector/LineInterface.java

@@ -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);
+
+}

src/Vector/Vector.java

@@ -1,3 +1,5 @@
+package Vector;
+
 import static java.lang.Math.*;
 
 public class Vector {
@@ -6,62 +8,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 +72,14 @@ 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);
     }
 }

src/View.java

@@ -1,8 +1,6 @@
+import Vector.Vector;
 import processing.core.*;
 import java.util.ArrayList;
-import java.util.Objects;
-
-import static processing.core.PApplet.*;
 
 public class View {
     Vector pose;
@@ -35,9 +33,11 @@ public class View {
     public void look(ArrayList<Wall> walls) {
         for (Ray ray : rays) {
             ray.castRay(walls);
+            if(ray.hasCollided()){
                 ray.drawRay(proc);
             }
         }
+    }
 
     //changes the position of the view
     public void setPos(Vector newPose) {
@@ -80,8 +80,7 @@ public class View {
         ArrayList<Vector> points = new ArrayList<>();
 
         for (Ray ray : rays) {
-            if (!Objects.equals(ray.getPoint(), new Vector(0, 0) {
+            if (ray.hasCollided()){
-            })) {
                 points.add(ray.getPoint());
             }
         }

src/Wall.java

@@ -1,8 +1,9 @@
+import Vector.*;
 import processing.core.*;
 
 import static processing.core.PApplet.*;
 
-public class Wall extends Line{
+public class Wall extends Line {
     int r;
     int g;
     int b;

tests/LineTest.java

@@ -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);
-    }
-
 }

tests/VectorTest.java

@@ -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((x1*x2+y1*y2), v1.dot(v2));
-                assertFloatEquals((float)(x1*x2+y1*y2), v2.dot(v1));
+                assertFloatEquals((x1*x2+y1*y2), v2.dot(v1));
 
                 // test addition
                 Vector vSum = v1.add(v2);