/**
* We've got a height change manoeuvre. Clip the time if necessary so that we don't overshoot our
* destination.
*
* @param speed - start speed
* @param heightDelta - the required height change
* @param moves - the vehicle movement characteristics
* @param heightChangeTimeMillis - the time taken for the height change
* @param distanceRemaining the distance left to travel
* @return the clipped time for this manoeuvre (in millis)
*/
protected static long calcClippedHeightChangeTimeFor(
WorldSpeed speed,
double heightDelta,
MovementCharacteristics moves,
long heightChangeTimeMillis,
double distanceRemaining) {
long res = 0;
// so, what's our climb rate
ClimbRateCharacteristics climbChars = (ClimbRateCharacteristics) moves;
WorldSpeed climbSpeed = TurnAlgorithm.calcClimbSpeed(heightDelta, climbChars);
// do we need to accelerate or decelerate?
boolean isAccelerating = TurnAlgorithm.areWeAccelerating(speed, climbSpeed);
WorldAcceleration accelRate = TurnAlgorithm.calcAccelRate(isAccelerating, moves);
// and find out how far through the time change we allow ourselves to go (without
// going past the marker)
res =
calcTrimmedAccelTime(
climbSpeed, speed, accelRate, heightChangeTimeMillis, distanceRemaining);
// hey, done.
return res;
}
/**
* 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;
}
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;
}
