コード例 #1
0
ファイル: LoiterPreview.java プロジェクト: kcisek/neptus
  @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();
  }
コード例 #2
0
 @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();
 }
コード例 #3
0
  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;
  }
コード例 #4
0
  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);
  }
コード例 #5
0
ファイル: ImageLayer.java プロジェクト: kcisek/neptus
 public LocationType getBottomRight() {
   LocationType loc = new LocationType(topLeft);
   loc.translatePosition(-image.getHeight() * zoom, image.getWidth() * zoom, 0);
   return loc;
 }