ICAROUS
|
#include <PlanUtil.h>
Static Public Member Functions | |
static int | prevTrackChange (const Plan &fp, int iNow) |
static int | nextTrackChange (const Plan &fp, int iNow) |
static int | prevVsChange (const Plan &p, int iNow) |
static int | nextVsChange (const Plan &p, int iNow) |
static std::pair< Position, int > | positionFromDistance (const Plan &p, double currentTime, double advanceDist, bool linear) |
static double | timeFromGs (double gsInit, double gsAccel, double dist) |
static Plan | applyWindField (const Plan &pin, const Velocity &v) |
static bool | isCurrentPositionUnchanged (const Plan &solution, double currentTime, const Position ¤tPos) |
static bool | checkMySolution (const Plan &solution, double currentTime, const Position ¤tPos, const Velocity ¤tVel) |
static double | distanceBetween (const Plan &A, const Plan &B) |
static double | lonCross (const Plan &ac, int i, double lat3) |
static double | latMax (const Plan &ac, int i) |
static int | addLocalMaxLat (Plan &ac, int i) |
static void | insertLocalMax (Plan &ac) |
static int | insertVirtual (Plan &ac, double time) |
static double | getLegDist (const Plan &ac, int i, double accuracy, double mindist) |
static void | interpolateVirtuals (Plan &ac, double accuracy, double startTm, double endTm) |
static void | interpolateVirtuals (Plan &ac, double accuracy) |
static Plan | revertAllTCPs (const Plan &pln) |
static Plan | revertAllTCPs (const Plan &pln, bool markIndices) |
static bool | removeVirtualsRange (Plan &ac, double startTm, double endTm, bool all) |
static bool | removeVirtuals (Plan &ac) |
static int | revertGroupOfTCPs (Plan &pln, int ix) |
static int | revertGsTCP (Plan &pln, int ix, bool revertPreviousTurn) |
static void | makeWellFormedEnds (Plan &lpc) |
static Plan | cutDownLinear (const Plan &plan, double startTime, double endTime) |
static Plan | cutDownTo (const Plan &plan, double timeOfCurrentPosition, double intentThreshold, double tExtend) |
static Plan | cutDownTo (const Plan &plan, double timeOfCurrentPosition, double intentThreshold) |
static std::pair< bool, double > | enoughDistanceForAccel (const Plan &p, int ix, double maxAccel, double M) |
static void | fixGsAccelAt (Plan &p, int ix, double maxAccel, bool checkTCP, double M) |
static int | hasPointsTooClose (const Plan &plan) |
static double | diffMetric (const Plan &lpc, const Plan &kpc) |
static Plan | unZigZag (const Plan &pp) |
static Plan | unZigZag (const Plan &pp, double maxLegSize) |
static bool | aboutTheSameTrk (const Velocity &v1, const Velocity &v2, double sameTrackBound) |
static Plan | removeCollinearTrk (const Plan &pp, double sameTrackBound) |
static Plan | mkGsConstant (const Plan &p, int i, int j, double gs) |
static Plan | mkGsConstant (const Plan &p, double gs) |
static Plan | mkGsConstant (const Plan &p) |
static Plan | mkGsConstant (const Plan &p, int wp1, int wp2) |
static void | mkVsConstant (Plan &p, int wp1, int wp2, double vs) |
static void | mkVsConstant (Plan &p, int start, int end) |
static void | mkVsConstant (Plan &p) |
static void | mkVsConstant (Plan &p, double vs) |
static int | mkVsShortLegsContinuous (Plan &p, int start, int end, double vsAccel, bool allowFixGs0, double aggressiveFactor, bool inhibitFixVs0) |
static int | mkVsShortLegsContinuous (Plan &p, double vsAccel, bool allowFixGs0, double aggressiveFactor, bool inhibitFixGs0) |
static int | mkVsContinuousAt (Plan &p, int i) |
static Plan | repairShortVsLegs (const Plan &fp, double vsAccel) |
static int | shareSegment (const Plan &p1, const Plan &p2) |
static std::pair< NavPoint, TcpData > | makeMidpoint (TcpData &tcp, const Position &p, double t) |
static Plan | projectPlan (const Plan &p, const EuclideanProjection &proj) |
static bool | basicCheck (const Plan &lpc, const Plan &kpc, double maxlastDt) |
static int | infeasibleTurns (Plan &lpc, int startIx, int endIx, double default_bank_angle, bool repair) |
static int | fixBadTurns (Plan &lpc, int startIx, int endIx, double default_bank_angle) |
static int | fixBadTurns (Plan &lpc, double default_bank_angle) |
static int | countBadTurns (Plan &lpc, double default_bank_angle) |
static bool | fixInfeasibleTurnAt (Plan &lpc, int ix, bool allowRepairByIntersection) |
static bool | fixInfeasibleTurnAtViaIntersection (Plan &lpc, int ix) |
static bool | checkReversion (const Plan &kpc, const Plan &lpc, bool verbose) |
static bool | checkReversion (const Plan &kpc, const Plan &lpc) |
static bool | checkNamesInfoRetained (const Plan &kpc, const Plan &lpc, bool verbose) |
static void | createAndAddMOT (Plan &kpc, int ixBOT, int ixEOT) |
static bool | isVelocityContAtTcps (const Plan &p, bool silent) |
static Plan | repairSmallNegativeGS (const Plan &p) |
static Plan | fixAccelConsistency (const Plan &p) |
static NavPoint | closestPoint3D (const Plan &plan, const Position &p, double maxLegLength) |
Static Public Attributes | |
static const double | maxTurnDeltaStraight = Units::from("deg",1.0) |
static const double | maximumInterpolationNumber = 900 |
static const double | minimumInterpolationLegSize = 1000.0 |
Static Private Member Functions | |
static Position | positionFromDistanceInSeg (const Plan &p, double curTm, double advDistance, bool linear) |
static Plan | setAltPreserveByDelta (const Plan &lpc) |
static double | findVsAccel (const Plan &kpc) |
static int | turnDir (Plan lpc, int i) |
static NavPoint | intersection (Plan lpc, int i) |
Utilities to operate on Plans
Check that first and last points are almost the same. Same means, same time, same latitude, longitude, and altitude.
lpc | a plan |
kpc | another plan |
maxlastDt | Maximum distance allowed at the last point |
|
static |
Used in Unit tests to make sure that a resolution does not change current position or velocity of vehicle
solution | plan |
currentTime | time |
currentPos | position |
currentVel | velocity |
|
static |
Test to make sure that all name and info in plan lpc is also in kpc
kpc | kinematic plan |
lpc | linear plan |
verbose | true for verbose mode |
Test to make sure that all name and info in plan lpc is also in kpc
kpc | |
lpc | |
verbose |
|
static |
Experimental Return the closest geometric point on a plan to a reference point, as measured as a Euclidean 3D norm from each linear segment. For latlon plans, perform linear interpolation on any legs longer then the given amount. (Kinematic turns are somewhat accounted for in this distance calculation, via linear interpolation.)
Choosing a shorter maxLegLength can increase the accuracy of the closest point calculation, but it can significantly increase the computational expense of the operation.
plan | base plan |
p | reference point |
maxLegLength | linearly interpolate any latlon legs longer than this amount |
make a new plan that is identical to plan from startTime to endTime It assumes that startTime and endTime are in linear segments. See also Plan.cut
plan | source plan |
startTime | absolute time of start time |
endTime | absolute time of end time |
|
static |
Create new Plan from time "timeOfCurrentPosition" to time "intentThreshold"
NOTE: currently only being used in Unit Tests
plan | plan |
timeOfCurrentPosition | time |
intentThreshold | time |
Cut down a plan so that it only contains points between timeOfCurrentPosition and intentThreshold. This method cuts a Plan so that the acceleration information after intentThreshold is discarded. The plan is continued linearly to time tExtend. The first time point of the new plan is the NavPoint before timeOfCurrentPosition in the plan. The intentThreshold and tExtend times are absolute.
plan | Plan file to be cut |
timeOfCurrentPosition | Current location of aircraft in the plan file |
intentThreshold | the absolute lookahead time – all acceleration information after this time is not copied |
tExtend | After the intentThreshold, the plan is extended linearly to this time (absolute time) |
Cut down a plan to the region between t1 and t2. The plan is continued linearly to time tExtend. A new point is created at t1 and t2 if necessary. The t1, t2 and tExtend times are absolute.
plan | Plan file to be cut |
t1 | Location to start cut out. If before first point or negative, first point is used |
t2 | The absolute lookahead time – all plan information after this time is not copied |
tExtend | After the intentThreshold, the plan is extended linearly to this time (absolute time) |
This calculates a heuristic measure of the difference between two plans. Currently only used in KinematicsPosition test
lpc | plan 1 |
kpc | plan 2 |
Calculate a distance difference between two plans
A | plan A |
B | plan B |
|
static |
Fix Plan p at ix if there is not enough distance for the current speed and specified acceleration (maxAccel) It makes the new ground speed as close to the original as possible (that is achievable over the distance)
p | plan |
ix | index of ground speed change |
maxAccel | maximum ground speed acceleration (non-negative) |
checkTCP | if true, do not alter time if point and it is an EOT or EVS |
M | max time |
|
static |
Repairs turn problem at index ix using several different strategies: – create a new vertex point (that makes one turn our of two) – removing the vertex point – moving the vertex point
lpc | semi-linear plan (no GS or VS TCPs) |
ix | index |
|
static |
Create an intersection point between ix-1 and ix
lpc | Plan to be repaired |
ix | index of problem turn |
|
static |
Repair turns for semi-linear plan. Removes vertex point if turn is infeasible.
lpc | Plan that needs repair |
startIx | Starting index |
endIx | Ending index |
default_bank_angle | Default bank angle |
repair | If true, attempt repair. If false, only determine the number of bad turns remaining. |
Note: This assumes that there are no GS or VS TCPs in lpc and no existing turns in the repair range
Repair turns for semi-linear plan. Removes vertex point if turn is infeasible.
fp | Plan that needs repair |
bankAngle | default bank angle |
repair | if true, attempt repair. If false, only determine the number of bad turns remaining. |
Note: This assumes that there are no GS or VS TCPs in lpc and no existing turns in the repair range
|
static |
this adds virtual points int the plan starting at or before startTm, and ending before or at endTm
ac | plan |
accuracy | accuracy of projections |
startTm | starting segment |
endTm | ending segment |
Create intersection point between (i-1) and i
lpc | linear plan |
i | index |
Create intersection point between (i-1) and i
lpc | |
i |
|
static |
Simple check that current position (currentPos) is where it should be according to plan "solution"
solution | the trajectory |
currentTime | current time |
currentPos | current position |
|
static |
Only tests velocity continuity at TCPs.
Note: If given a linear plan as input, Stratway will only generate a kinematic subplan over the conflict region. The output can still be linear elsewhere. This test is appropriate for Stratway output.
p | Plan to be tested |
silent | if true, output is suppressed |
Make ground speed constant gs between wp1 and wp2.
p | a plan |
Make ground speed constant gs for entire plan.
p | plan |
gs | ground speed |
Make a new plan with constant ground speed from wp1 to wp2. Assumes input plan is linear.
200 200 200 200 200 0 ----- 1 ----- 2 ----- 3 ----- 4 ----- 5 mkGSConstant(p, 1, 3, 500) 200 500 500 200 200 200 0 ----- 1 ----- 2 ----- 3 ----- 4 ----- 5 Note that if wp1 == wp2 no change is made.
p | plan |
i | index of waypoint 1 |
j | index of waypoint 2 |
gs | ground speed |
Make ground speed constant between wp1 and wp2 as an average of the total distance and time travelled.
p | trajectory |
wp1 | starting waypoint |
wp2 | starting waypoint |
|
static |
Make vertical speed constant vs over plan with vs being the average speed.
p | plan |
|
static |
Make vertical speed constant vs for full plan.
p | plan |
vs | vertical speed |
|
static |
Make vertical speed constant vs between wp1 and wp2, with vs being the average speed.
p | plan |
start | starting index |
end | ending index |
|
static |
Make vertical speed constant vs between wp1 and wp2. Assumes plan does not contain any vertical speed accel regions
|
static |
Repair Method: make the vertical speed continuous at index i (i.e. vsIn(i) = vsOut(i). Accomplish this by changing the altitude at index i. If the new altitude makes the point redundant, then remove it.
p | trajectory |
i | index |
|
static |
Examine each vertical segment [i,i+1] over the specified range. Calculate needed time for the vertical accelerations at each end. If the sum of half of each of these accels is longer in time than the segment dt, then smooth away this segment vertically. Either side could be potentially smooth. This method smooths the side with the smallest delta vs.
p | Plan to be modified |
start | starting index |
end | ending index |
vsAccel | vertical speed acceleration |
inhibitFixGs0 | if true will NOT attempt to repair on segments with gsOut = 0 |
inhibitFixVs0 | if true will NOT attempt to repair level segments (i.e. vertical speed = 0) |
aggressiveFactor | a factor in the interval (0,1] that determines how aggressively the repair is done |
Examine each vertical segment [i,i+1] over the range. Calculate needed time for the vertical accelerations at each end. If the sum of half of each of these accels is longer in time than the segment dt, then smooth away this segment vertically. Either side could be potentially smooth. This method smooths the side with the smallest delta vs.
p | Plan to be modified |
start | starting index |
end | ending index |
vsAccel | vertical speed acceleration |
|
static |
Returns an index after iNow where there is a significant enough track change to bother with This is used to move forward through collinear points in order to get a reasonable sized leg to work with.
If there is not next track change it returns fp.size()-1
fp | plan |
iNow | index |
|
static |
Returns an index after iNow where there is a significant enough vertical speed change to bother with This is used to move forward through collinear points in order to get a reasonable sized leg to work with.
If there is not a next vs change it returns fp.size()-1
p | plan |
iNow | index |
|
staticprivate |
Advance forward in plan "p" starting at time "curTm" a distance of "advDistance" within a single segment
Note : assumes the advance by distance will not leave current segment Note : this can be used before there is a ground speed profile – it does not depend upon correct velocities
p | plan of interest |
curTm | currentTime of so |
advDistance | distance to advance |
linear | if true, treat plan as a linear plan (i.e. path is not curved) |
|
static |
Returns an index before iNow where there is a significant enough track change to bother with. This is used to "backtrack" through collinear points in order to get a reasonable sized leg to work with. If there is not previous track change it returns 0
Note: must use both trkIn and TrkOut because of large great circles
fp | plan |
iNow | index |
|
static |
Returns an index after iNow where there is a significant enough vertical speed change to bother with This is used to move forward through collinear points in order to get a reasonable sized leg to work with.
If there is not a next vs change it returns fp.size()-1
p | plan |
iNow | index |
Project plan from latlonalt to Euclidean. Note: some tcp turn information may no longer be valid.
p | plan |
proj | projection |
Attempts to repair plan with infeasible vertical points by averaging vertical speeds over 2 segments. Assumes linear plan input.
fp | |
vsAccel | |
minVsChangeRequired |
Revert all TCPs in the plan, i.e. remove all acceleration zones and replace with instantaneous velocity changes. This undoes TrajGen.makeKinematicPlans.
Note: This retains the acceleration values in the resulting linear plan.
pln | kinematic plan to be reverted to a linear plan |
markIndices | if true, add :ksrc-#: to each point's info (original behavior is "false") This may include multiple flags in a single point. |
|
static |
Used in STRATWAY to structurally revert all TCPS "near" ix. Note that this function will timeshift the points after ix to regain original ground speed into the point after ix.
NOTE: This method does not depend upon source time or source position
pln | plan |
ix | The index of one of the TCPs created together that should be reverted |
Structurally revert all TCPS that create acceleration zones containing ix if the point is a not a TCP do nothing. Note that this function will timeshift the points after ix to regain original ground speed into the point after ix.
NOTE This method does not depend upon source time!!
ix | The index of one of the TCPs created together that should be reverted |
|
static |
if "ix" is a BGS, then it reverts the BGS-EGS pair back to a single linear point Note: It (does not depend upon source time or source position!)
pln | plan |
ix | index of BGS |
revertPreviousTurn | if true then if this GS segment is right after a turn then revert the turn as well |
Called after revertVsTCPs to set AltPreserve. Assumes that vsAccels have been saved in reverted Plan
Note: this is very similar to TrajGen.markVsChanges
lpc | |
vsAccel |
Return the index of p1 that shares a segment (has same start and end positions) as p2, or -1 if there are none This returns the last such shared segment
p1 | point 1 |
p2 | point 2 |
|
static |
time required to cover distance "dist" if initial speed is "gsInit" and acceleration is "gsAccel"
gsAccel | ground speed acceleration |
gsInit | initial ground speed |
dist | distance travelled |
time required to cover distance "dist" if initial speed is "gsInit" and acceleration is "gsAccel"
gsAccel | ground speed acceleration |
gsInit | initial ground speed |
dist | distance travelled |