/**
     * Sets a costmap grid and fills it with the geometry polygons.
     */
// TODO(playerone) remove sync
private synchronized void fillCostMapWithGeometries() {
        if (mGeometryList.isEmpty()) {
            Log.wtf(TAG, "Empty geometries list");
            setValid(false);
            return;
        }
        // Get the grid boundaries to determine the size of the cost map.
        double[][] positions = findGeometriesBoundaries();
        if (positions == null) {
            Log.wtf(TAG, "Could not find grid boundaries");
            setValid(false);
            return;
        }
        // Create the grid.
        makeGrid(positions, MIN_COST);
        for (Geometry g : mGeometryList) {
            int size = g.mPolygon.getSize();
            List<Transform> points = g.mPolygon.getPoints();
            // DiscretizedVertices contain only X and Y.
            int[][] discretizedVertices = new int[size][2];
            for (int i = 0; i < size; i++) {
                double[] pointPosition = points.get(i).getPosition();
                discretizedVertices[i][0] = (int) Math.floor(pointPosition[0] / getResolution());
                discretizedVertices[i][1] = (int) Math.floor(pointPosition[1] / getResolution());
            }
            // TODO mCost is not used, function below sets polygon regions to OBSTACLE_COST.
            drawPolygonOnGrid(discretizedVertices);
        }
    }
private synchronized void fillCostMapWithGeometries() {
if (mGeometryList.isEmpty()) {
Log.wtf(TAG, "Empty geometries list");
setValid(false);
return;
}
// Get the grid boundaries to determine the size of the cost map.
double[][] positions = findGeometriesBoundaries();
if (positions == null) {
Log.wtf(TAG, "Could not find grid boundaries");
setValid(false);
return;
}
// Create the grid.
makeGrid(positions, MIN_COST);
for (Geometry g : mGeometryList) {
int size = g.mPolygon.getSize();
List<Transform> points = g.mPolygon.getPoints();
// DiscretizedVertices contain only X and Y.
int[][] discretizedVertices = new int[size][2];
for (int i = 0; i < size; i++) {
double[] pointPosition = points.get(i).getPosition();
discretizedVertices[i][0] = (int) Math.floor(pointPosition[0] / getResolution());
discretizedVertices[i][1] = (int) Math.floor(pointPosition[1] / getResolution());
}
// TODO mCost is not used, function below sets polygon regions to OBSTACLE_COST.
drawPolygonOnGrid(discretizedVertices);
}
}
private synchronized void fillCostMapWithGeometries() {
if (mGeometryList.isEmpty()) {
Log.wtf(TAG, "Empty geometries list");
setValid(false);
return;
}
// Get the grid boundaries to determine the size of the cost map.
double[][] positions = findGeometriesBoundaries();
if (positions == null) {
Log.wtf(TAG, "Could not find grid boundaries");
setValid(false);
return;
}
// Create the grid.
makeGrid(positions, MIN_COST);
for (Geometry g : mGeometryList) {
int size = g.mPolygon.getSize();
List<Transform> points = g.mPolygon.getPoints();
// DiscretizedVertices contain only X and Y.
int[][] discretizedVertices = new int[size][2];
for (int i = 0; i < size; i++) {
double[] pointPosition = points.get(i).getPosition();
discretizedVertices[i][0] = (int) Math.floor(pointPosition[0] / getResolution());
discretizedVertices[i][1] = (int) Math.floor(pointPosition[1] / getResolution());
}
// TODO mCost is not used, function below sets polygon regions to OBSTACLE_COST.
drawPolygonOnGrid(discretizedVertices);
}
}
private synchronized void fillCostMapWithGeometries() {
if (mGeometryList.isEmpty()) {
Log.wtf(TAG, "Empty geometries list");
setValid(false);
return;
}
// Get the grid boundaries to determine the size of the cost map.
double[][] positions = findGeometriesBoundaries();
if (positions == null) {
Log.wtf(TAG, "Could not find grid boundaries");
setValid(false);
return;
}
// Create the grid.
makeGrid(positions, MIN_COST);
for (Geometry g : mGeometryList) {
int size = g.mPolygon.getSize();
List<Transform> points = g.mPolygon.getPoints();
// DiscretizedVertices contain only X and Y.
int[][] discretizedVertices = new int[size][2];
for (int i = 0; i < size; i++) {
double[] pointPosition = points.get(i).getPosition();
discretizedVertices[i][0] = (int) Math.floor(pointPosition[0] / getResolution());
discretizedVertices[i][1] = (int) Math.floor(pointPosition[1] / getResolution());
}
// TODO mCost is not used, function below sets polygon regions to OBSTACLE_COST.
drawPolygonOnGrid(discretizedVertices);
}
}