lejos.robotics.pathfinding.FourWayGridMesh Maven / Gradle / Ivy
Go to download
Show more of this group Show more artifacts with this name
Show all versions of lejos-ev3-api Show documentation
Show all versions of lejos-ev3-api Show documentation
leJOS (pronounced like the Spanish word "lejos" for "far") is a tiny Java Virtual Machine. In 2013 it was ported to the LEGO EV3 brick.
The newest version!
package lejos.robotics.pathfinding;
import java.util.ArrayList;
import java.util.Collection;
import lejos.robotics.geometry.Line;
import lejos.robotics.geometry.Rectangle;
import lejos.robotics.mapping.LineMap;
/**
* Generates a grid of nodes. Spacing between the grid nodes and clearance around map geometry can be specified.
* This set can be generated once at the beginning of a user program, and the same node set can be used for all
* subsequent navigation.
* @author BB
*
*/
public class FourWayGridMesh implements NavigationMesh {
private ArrayList mesh = null;
private LineMap map = null;
private float clearance;
private float gridspace;
/**
* Instantiates a grid mesh of nodes which won't interconnect between any map geometry. Will also keep away
* the set parameter from map geometry. Grid spacing is adjustable via the constructor.
* @param map The map containing geometry.
* @param gridSpace The size of each grid square.
* @param clearance The safety zone between all nodes/connections and the map geometry.
*/
public FourWayGridMesh(LineMap map, float gridSpace, float clearance) {
setMap(map);
setClearance(clearance);
setGridSpacing(gridSpace);
// TODO: OPTION 1: Generate now (predictable) or later (allows changes to be made to map? Nah.)
// Maybe generate called here? If called later, someone could add a node before it was
// generated and expect it to be connected, which it won't be.
}
public Collection getMesh(){
if(mesh == null) regenerate();
return mesh;
}
/**
* Change the size of each grid square. NOTE: When grid space value is changed, this class does not regenerate
* the navigation mesh until regenerate() is explicitly called.
* @param gridSpace The unit size of each grid square.
*/
public void setGridSpacing(float gridSpace) {
this.gridspace = gridSpace;
}
/**
* Changes the safety zone between all nodes/connections and map geometry. This leaves a margin of error between
* potential object collisions and the robot. NOTE: When clearance value is changed,
* this class does not regenerate the navigation mesh until regenerate() is explicitly called.
* @param clearance The safety clearance between nodes/connections and map geometry.
*/
public void setClearance(float clearance) {
this.clearance = clearance;
}
/**
* Feeds this class a new map. NOTE: When Map is changed, this class does not regenerate the navigation mesh
* until regenerate() is explicitly called.
* @param map The new map data.
*/
public void setMap(LineMap map) {
this.map = map;
}
public void regenerate() {
//long startNanoT = System.nanoTime();
//long startFreeMem = Runtime.getRuntime().freeMemory();
mesh = new ArrayList ();
// First node is "clearance" from the corner of the map
Rectangle bounds = map.getBoundingRect();
float startx = bounds.x + clearance;
float starty = bounds.y + clearance;
float endx = bounds.width + bounds.x - clearance;
float endy = bounds.height + bounds.y - clearance;
int x_grid_squares = 0;
int y_grid_squares = 0;
for(float y = starty;y= neighbors) {
mesh.add(node);
return total;
}
}
}
}
mesh.add(node);
return total;
}
public boolean removeNode(Node node) {
//System.out.print("MAIN NODE ");
//outputNodeData(node);
Collection coll = node.getNeighbors();
ArrayList arr = new ArrayList (coll);
for(int i=0;i