@Override
public SystemPositionAndAttitude step(SystemPositionAndAttitude state, double timestep) {
model.setState(state);
double distToDestination = state.getPosition().getDistanceInMeters(destination);
if (loiterType.equalsIgnoreCase("circular")) {
if (distToDestination - 2 > radius) {
model.guide(
destination, speed, destination.getDepth() >= 0 ? null : -destination.getDepth());
if (!enteredLoiter) loiterTime = 0;
} else {
enteredLoiter = true;
double perpend = state.getPosition().getXYAngle(destination) + Math.PI / 2;
LocationType loc = new LocationType(state.getPosition());
loc.setDepth(destination.getDepth());
if (clockwise) loc.translatePosition(Math.cos(perpend) * -20, Math.sin(perpend) * -20, 0);
else loc.translatePosition(Math.cos(perpend) * 20, Math.sin(perpend) * 20, 0);
model.guide(loc, speed, destination.getDepth() >= 0 ? null : -destination.getDepth());
loiterTime += timestep;
}
} else {
if (distToDestination < speed * 2) loiterTime += timestep;
else
model.guide(
destination, speed, destination.getDepth() >= 0 ? null : -destination.getDepth());
}
model.advance(timestep);
if (loiterTime > duration) finished = true;
return model.getState();
}
@Subscribe
public void consume(SimulatedState simState) {
LocationType loc =
new LocationType(Math.toDegrees(simState.getLat()), Math.toDegrees(simState.getLon()));
loc.setHeight(simState.getHeight());
loc.translatePosition(simState.getX(), simState.getY(), simState.getZ());
simulatedState =
new SystemPositionAndAttitude(
loc, simState.getPhi(), simState.getTheta(), simState.getPsi());
lastStateMillis = System.currentTimeMillis();
}
private DataDiscretizer parseLog(String entity) {
DataDiscretizer dd = new DataDiscretizer(10);
IMraLog parser = logSource.getLog(messageName);
IMraLog stateParser = logSource.getLog("EstimatedState");
if (parser == null || stateParser == null) {
return dd;
}
IMCMessage entry = parser.nextLogEntry();
IMCMessage stateEntry = stateParser.nextLogEntry();
LocationType ref = IMCUtils.getLocation(stateEntry).convertToAbsoluteLatLonDepth();
entityList.clear();
entityList.add("ALL");
while (entry != null) {
parser.advance(1);
entry = parser.getCurrentEntry();
if (entry != null) {
String entName = logSource.getEntityName(entry.getSrc(), entry.getSrcEnt());
if (!entityList.contains(entName)) entityList.add(entName);
if (!entity.equalsIgnoreCase("ALL") && !entName.equalsIgnoreCase(entity)) continue;
stateEntry = stateParser.getEntryAtOrAfter(parser.currentTimeMillis());
LocationType loc = new LocationType();
loc.setLatitudeRads(stateEntry.getDouble("lat"));
loc.setLongitudeRads(stateEntry.getDouble("lon"));
loc.translatePosition(
stateEntry.getDouble("x"), stateEntry.getDouble("y"), stateEntry.getDouble("z"));
double[] offsets = loc.getOffsetFrom(ref);
double[] vals = new double[4];
vals[0] = Double.NaN;
if (stateEntry != null) {
vals[0] = entry.getDouble(varName);
dd.addPoint(offsets[1], -offsets[0], vals);
}
}
}
entCombo.setModel(new DefaultComboBoxModel<>(entityList));
return dd;
}
private void solve() {
if (pe == null) {
GuiUtils.errorMessage(getConsole(), "Coverage Plan Solver", "The polygon is not valid");
return;
}
double north, east, south, west;
double[] bounds = pe.getBounds3d();
south = bounds[PathElement.SOUTH_COORD];
west = bounds[PathElement.WEST_COORD];
north = bounds[PathElement.NORTH_COORD];
east = bounds[PathElement.EAST_COORD];
CoverageCell[][] cells =
new CoverageCell[(int) ((north - south) / grid) + 1][(int) ((east - west) / grid) + 1];
for (int i = 0; i < cells.length; i++)
for (int j = 0; j < cells[i].length; j++) {
cells[i][j] = new CoverageCell();
cells[i][j].i = i;
cells[i][j].j = j;
}
int i = 0, j = 0;
int desiredCells = 0;
for (double n = south + grid / 2; n < north; n += grid) {
j = 0;
for (double e = west + grid / 2; e < east; e += grid) {
LocationType lt = new LocationType(pe.getCenterLocation());
lt.translatePosition(n, e, 0);
CoverageCell cell = cells[i][j];
cell.realWorldLoc = lt.getNewAbsoluteLatLonDepth();
if (pe.containsPoint(lt, null)) {
cell.desired = true;
desiredCells++;
}
cells[i][j] = cell;
j++;
}
i++;
}
CoverageCell initialCell = null;
i = 0;
for (j = 0; j < cells[0].length - 1 && initialCell == null; j++)
for (i = 0; i < cells.length && initialCell == null; i++)
if (cells[i][j].desired) initialCell = cells[i][j];
if (initialCell == null) {
GuiUtils.errorMessage("Polygon coverage", "Polygon area is invalid");
return;
}
CoverageCell current = initialCell;
desiredCells--;
int dir = -1;
while (desiredCells > 0) {
current.visited = true;
current.active = false;
if (dir == 1) {
if (current.i < cells.length - 1
&& cells[current.i + 1][current.j].desired == true
&& cells[current.i + 1][current.j].visited == false) {
current.next = cells[current.i + 1][current.j];
cells[current.i + 1][current.j].previous = current;
current = current.next;
current.active = true;
} else {
dir = -1;
if (current.j == cells[0].length - 1) break;
while (!cells[current.i][current.j + 1].desired && i > 0 && current.previous != null) {
current.active = false;
current = current.previous;
}
if (i == 0) break;
current.next = cells[current.i][current.j + 1];
cells[current.i][current.j + 1].previous = current;
current = current.next;
current.active = true;
}
} else {
if (current.i > 0
&& cells[current.i - 1][current.j].desired == true
&& cells[current.i - 1][current.j].visited == false) {
current.next = cells[current.i - 1][current.j];
cells[current.i - 1][current.j].previous = current;
current = current.next;
current.active = true;
} else {
dir = 1;
if (current.j == cells[0].length - 1) break;
while (current.previous != null
&& !cells[current.i][current.j + 1].desired
&& i < cells.length) {
current.active = false;
current = current.previous;
}
if (i == cells.length) break;
current.next = cells[current.i][current.j + 1];
cells[current.i][current.j + 1].previous = current;
current = current.next;
current.active = true;
}
}
desiredCells--;
}
generatePlans(cells, initialCell);
}
public LocationType getBottomRight() {
LocationType loc = new LocationType(topLeft);
loc.translatePosition(-image.getHeight() * zoom, image.getWidth() * zoom, 0);
return loc;
}
