Пример #1
0
  /**
   * process a turn component of a larger manoeuvre
   *
   * @param turnRequired how far we have to turn (-ve is left)
   * @param moves the movement chars for this participant
   * @param current the current status
   * @param endTime the end time for this step
   * @param turner the turn algorithm we're using
   * @param demandedHeight the demanded height (normally the same as the current height - to
   *     maintain height (and therefore speed) in a manoeuvre.
   * @return
   */
  protected Status doThisPartOfTheTurn(
      double turnRequired,
      MovementCharacteristics moves,
      Status current,
      long endTime,
      TurnAlgorithm turner,
      double demandedHeight) {
    // what's our turn rate?
    double turnRate =
        DirectedOnTopWaypoint.getTurnRateFor(
            moves, current.getSpeed().getValueIn(WorldSpeed.M_sec));
    turnRate = MWC.Algorithms.Conversions.Rads2Degs(turnRate);

    // hmm, so how long will it take?
    long turnTime = (long) (Math.abs(turnRequired) / turnRate * 1000d);

    // hey, trim the turn time to the remaining time
    long thisStep = endTime - current.getTime();
    turnTime = Math.min(turnTime, thisStep);

    // now, to make sure we turn the right way, we're going to turn 1/2 way first, then the second
    // part
    double semiTurn = turnRequired / 2;

    double semiTurnTime = turnTime / 2;

    // hey, make sure we at least have a step to make!
    semiTurnTime = Math.max(1, semiTurnTime);

    // amd do the turn!
    SimpleDemandedStatus sds =
        new SimpleDemandedStatus(1, (long) (current.getTime() + (semiTurnTime)));
    sds.setCourse(current.getCourse() + semiTurn);
    sds.setSpeed(current.getSpeed());
    sds.setHeight(demandedHeight);
    current = turner.doTurn(current, sds, moves, sds.getTime());

    // and the second part of the turn!
    sds = new SimpleDemandedStatus(1, current.getTime() + (long) (turnTime - semiTurnTime));
    sds.setCourse(current.getCourse() + semiTurn);
    sds.setSpeed(current.getSpeed());
    sds.setHeight(demandedHeight);
    current = turner.doTurn(current, sds, moves, sds.getTime());

    return current;
  }
Пример #2
0
  protected static Status processStraightCourse(
      Double requiredDistance,
      Status current,
      long endTime,
      HighLevelDemandedStatus highLevelDemStat,
      TurnAlgorithm turner,
      MovementCharacteristics moves) {

    // How far do we still have to go?
    double straightDistance = requiredDistance.doubleValue();

    // how far will we travel in this time step?

    // hey, how long of the time step is remaining?
    long thisTimeStep = endTime - current.getTime();

    // do we have a demanded height change ('cos this really buggers things up)
    double demHeight = -highLevelDemStat.getCurrentTarget().getDepth();
    double curHeight = -current.getLocation().getDepth();

    WorldLocation origin = new WorldLocation(current.getLocation());

    // take note of the original height.  We may change height during a height change, but wish to
    // return to the original
    // height on completion.  Alternatively we may wish to change to a demanded speed on completion
    // of the height change
    WorldSpeed originalSpeed = null;

    if (demHeight != curHeight) {

      // ok, and is there any distance remaining?
      double distanceTravelled = current.getLocation().subtract(origin).getRange();
      distanceTravelled = MWC.Algorithms.Conversions.Degs2m(distanceTravelled);
      double distanceRemaining = straightDistance - distanceTravelled;
      if (distanceRemaining > 0.1) {
        // yup, let's continue

        // yup, height change there is...
        double heightDelta = demHeight - curHeight;

        // find out what the height change speed is
        double stepTime = (endTime - current.getTime()) / 1000d;

        // and how long will the height change take?
        long heightChangeTimeMillis =
            (long) (TurnAlgorithm.calcHeightChangeTime(heightDelta, moves, stepTime) * 1000d);

        // WORKAROUND - a very small height change may require a time less than one millisecond -
        // represented
        // as zero time.  If this is the case, give us a tiny time step
        if (heightChangeTimeMillis == 0) heightChangeTimeMillis = 1;

        // hey, trim the height change to our remaining time
        heightChangeTimeMillis = Math.min(heightChangeTimeMillis, thisTimeStep);

        long timeTakenMillis;

        // right then.  Is this a vehicle with standard climb and dive speeds?
        if (moves instanceof ClimbRateCharacteristics) {
          // yes, we need to factor in the speed change

          // ok, we're doing a height change with speed consequences.  remember the current speed,
          // so
          // we can return to it on completion
          originalSpeed = new WorldSpeed(current.getSpeed());

          // ok, how far will we travel during this period
          timeTakenMillis =
              DirectedOnTopWaypoint.calcClippedHeightChangeTimeFor(
                  current.getSpeed(),
                  heightDelta,
                  moves,
                  heightChangeTimeMillis,
                  distanceRemaining);

          // todo - double-check that height change is achievable in the time-step,
          // though the turn decision algorithm should only produce achievable height changes

        } else {
          // right, we're just going to travel at normal speed for the turn.
          // calc how far we are due to travel in the time step

          // how long will it take to cover the remaining distance at this speed
          timeTakenMillis =
              (long) (1000 * straightDistance / current.getSpeed().getValueIn(WorldSpeed.M_sec));
        }

        // trim to the step time
        timeTakenMillis = Math.min(heightChangeTimeMillis, timeTakenMillis);

        // and create a demanded status
        SimpleDemandedStatus sds = new SimpleDemandedStatus(1, current);

        // and set the height
        sds.setHeight(demHeight);

        // and do the step
        current = turner.doTurn(current, sds, moves, current.getTime() + timeTakenMillis);
      } // if we have distance to run
    }

    // that's the height change done - we should be at our demanded height now.  If there's any time
    // left we will allow
    // a speed change

    // ok, height change is done.  Is there any time left?
    if (current.getTime() < endTime) {
      // ok, and is there any distance remaining?
      double distanceTravelled = current.getLocation().subtract(origin).getRange();
      distanceTravelled = MWC.Algorithms.Conversions.Degs2m(distanceTravelled);
      double distanceRemaining = straightDistance - distanceTravelled;
      if (distanceRemaining > 0.1) {
        // yup, let's continue

        // is there a demanded speed change?
        WorldSpeed demSpeedObj = highLevelDemStat.getSpeed();
        if (demSpeedObj != null) {
          double demSpeed = demSpeedObj.getValueIn(WorldSpeed.M_sec);
          double curSpeed = current.getSpeed().getValueIn(WorldSpeed.M_sec);

          if (curSpeed != demSpeed) {
            // ok, process the speed change
            long remainingTime = endTime - current.getTime();

            boolean isAccelerating = TurnAlgorithm.areWeAccelerating(curSpeed, demSpeed);
            WorldAcceleration accelRate = TurnAlgorithm.calcAccelRate(isAccelerating, moves);
            long timeAllowed =
                DirectedOnTopWaypoint.calcTrimmedAccelTime(
                    demSpeedObj, current.getSpeed(), accelRate, remainingTime, distanceRemaining);

            // and move us forward
            SimpleDemandedStatus sds = new SimpleDemandedStatus(1, current);

            // and set the speed
            sds.setSpeed(demSpeedObj);

            // and do the step
            current = turner.doTurn(current, sds, moves, current.getTime() + timeAllowed);
          } // if we need a speed change
        } // if we have a demanded speed
        else {
          // hey, no demanded speed change.  Was there a dem height change?
          // This may have caused a speed change which we now need to counter
          if (originalSpeed != null) {
            // yup, we changed height.  We need to head back to the old speed
            // ok, process the speed change
            long remainingTime = endTime - current.getTime();

            boolean isAccelerating =
                TurnAlgorithm.areWeAccelerating(current.getSpeed(), originalSpeed);
            WorldAcceleration accelRate = TurnAlgorithm.calcAccelRate(isAccelerating, moves);
            long timeAllowed =
                DirectedOnTopWaypoint.calcTrimmedAccelTime(
                    current.getSpeed(),
                    current.getSpeed(),
                    accelRate,
                    remainingTime,
                    distanceRemaining);
            //            long timeAllowed = DirectedOnTopWaypoint.calcTrimmedAccelTime(demSpeedObj,
            // current.getSpeed(),
            //                accelRate, remainingTime, distanceRemaining);

            // and move us forward
            SimpleDemandedStatus sds = new SimpleDemandedStatus(1, current);

            // and do the step
            current = turner.doTurn(current, sds, moves, current.getTime() + timeAllowed);
          }
        }
      } // if we have distance to run
    } // if there is time left

    // hmm, that's the speed change over. do we have any time left?
    if (current.getTime() < endTime) {
      // how far have we travelled
      double distanceTravelled = current.getLocation().subtract(origin).getRange();
      distanceTravelled = MWC.Algorithms.Conversions.Degs2m(distanceTravelled);

      // which leaves how far?
      double distanceRemaining = straightDistance - distanceTravelled;

      if (distanceRemaining > 0.1) {
        // yup, let's continue travelling in a straight line at steady speed/height
        // collate the input data.

        // how long will it take to cover the remaining distance at this speed
        long timeTakenMillis =
            (long) (1000 * straightDistance / current.getSpeed().getValueIn(WorldSpeed.M_sec));

        // trim to the step time
        timeTakenMillis = Math.min(thisTimeStep, timeTakenMillis);

        // and create a demanded status
        SimpleDemandedStatus sds = new SimpleDemandedStatus(1, current);

        // and do the step
        current = turner.doTurn(current, sds, moves, current.getTime() + timeTakenMillis);
      }
    }

    return current;
  }
Пример #3
0
  public DemandedStatus decide(
      final Status status,
      ASSET.Models.Movement.MovementCharacteristics chars,
      DemandedStatus demStatus,
      ASSET.Models.Detection.DetectionList detections,
      ASSET.Scenario.ScenarioActivityMonitor monitor,
      final long time) {
    // create the output object
    SimpleDemandedStatus res = null;

    String thisActivity = null;

    // have we completed this manoeuvre?
    if (_transit_complete) {
      res = null;
      return res;
    }

    // find out where we are
    WorldLocation currentLoc = status.getLocation();

    // are we running towards a destination?
    if (_theDistance == null) {
      // ok, we're not heading towards a particular point, just put us onto the correct course and
      // speed
      double curSpeed = status.getSpeed().getValueIn(WorldSpeed.M_sec);
      double curCourse = status.getCourse();
      double curHeight = -status.getLocation().getDepth();

      thisActivity = "";

      // are we already working to a simple demanded course/speed/depth?
      if (demStatus instanceof SimpleDemandedStatus) {
        res = new SimpleDemandedStatus(time, (SimpleDemandedStatus) demStatus);
      } else {
        res = new SimpleDemandedStatus(time, status);
      }

      if (_mySpeed != null) {
        if (curSpeed != _mySpeed.getValueIn(WorldSpeed.M_sec)) {
          thisActivity += " speed to:" + (int) _mySpeed.getValueIn(WorldSpeed.Kts);
          res.setSpeed(_mySpeed.getValueIn(WorldSpeed.M_sec));
        }
      }

      if (_myCourse != null) {
        if (curCourse != _myCourse.doubleValue()) {
          thisActivity += " course to:" + _myCourse.doubleValue();
          res.setCourse(_myCourse.doubleValue());
        }
      }

      if (_myHeight != null) {
        if (curHeight != _myHeight.getValueIn(WorldDistance.METRES)) {
          thisActivity += " height to:" + _myHeight.getValueIn(WorldDistance.METRES);
          res.setHeight(_myHeight.getValueIn(WorldDistance.METRES));
        }
      }

      // did we update any?
      if (thisActivity == "") {
        // no - we must be ok,
        res = null;
      } else {
        // cool, the res is set already
      }

    } else {

      // do we have our destination?
      if (_theDestination == null) {
        // no, this is the first time we've been called.  Calculate where we're going to
        WorldVector vector =
            new WorldVector(
                MWC.Algorithms.Conversions.Degs2Rads(this._myCourse.doubleValue()),
                _theDistance.getValueIn(WorldDistance.DEGS),
                0);

        _theDestination = new WorldLocation(currentLoc);
        _theDestination.addToMe(vector);
      } else {
        // ok, we're up and running, have we reached our destination

        // how far to the target
        double rngDegs = currentLoc.subtract(_theDestination).getRange();

        // and in yards
        double rngYds = MWC.Algorithms.Conversions.Degs2Yds(rngDegs);

        if (rngYds < _threshold) {
          // right, we've got there. Mark complete
          _transit_complete = true;

          super.setLastActivity(thisActivity);

          // and drop out
          return res;
        }
      }

      // ok, now steer to the destination
      double brg_rads = _theDestination.subtract(currentLoc).getBearing();
      final double brgDegs = MWC.Algorithms.Conversions.Rads2Degs(brg_rads);

      // and set the course in degs
      res = new SimpleDemandedStatus(time, status);
      res.setCourse(brgDegs);

      // do we have depth?
      if (_myHeight != null) res.setHeight(_myHeight.getValueIn(WorldDistance.METRES));

      // and the speed
      if (_mySpeed != null) res.setSpeed(_mySpeed.getValueIn(WorldSpeed.M_sec));

      thisActivity = "Heading for target location";
    }

    super.setLastActivity(thisActivity);

    return res;
  }