/**
* 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;
}
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;
}