ICAROUS
All Classes Namespaces Files Functions Variables Enumerations Enumerator Macros Modules Pages
Static Public Member Functions | Static Public Attributes | Static Private Member Functions | List of all members
larcfm::PlanUtil Class Reference

#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 &currentPos)
 
static bool checkMySolution (const Plan &solution, double currentTime, const Position &currentPos, const Velocity &currentVel)
 
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, TcpDatamakeMidpoint (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)
 

Detailed Description

Utilities to operate on Plans

Member Function Documentation

◆ basicCheck()

bool larcfm::PlanUtil::basicCheck ( const Plan lpc,
const Plan kpc,
double  maxlastDt 
)
static

Check that first and last points are almost the same. Same means, same time, same latitude, longitude, and altitude.

Parameters
lpca plan
kpcanother plan
maxlastDtMaximum distance allowed at the last point
Returns
True, if the first and last points are almost the same.

◆ checkMySolution()

bool larcfm::PlanUtil::checkMySolution ( const Plan solution,
double  currentTime,
const Position currentPos,
const Velocity currentVel 
)
static

Used in Unit tests to make sure that a resolution does not change current position or velocity of vehicle

Parameters
solutionplan
currentTimetime
currentPosposition
currentVelvelocity
Returns
true if correct

◆ checkNamesInfoRetained()

bool larcfm::PlanUtil::checkNamesInfoRetained ( const Plan kpc,
const Plan lpc,
bool  verbose 
)
static

Test to make sure that all name and info in plan lpc is also in kpc

Parameters
kpckinematic plan
lpclinear plan
verbosetrue for verbose mode
Returns
true if correct

Test to make sure that all name and info in plan lpc is also in kpc

Parameters
kpc
lpc
verbose
Returns

◆ closestPoint3D()

NavPoint larcfm::PlanUtil::closestPoint3D ( const Plan plan,
const Position p,
double  maxLegLength 
)
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.

Parameters
planbase plan
preference point
maxLegLengthlinearly interpolate any latlon legs longer than this amount
Returns
point

◆ cutDownLinear()

Plan larcfm::PlanUtil::cutDownLinear ( const Plan plan,
double  startTime,
double  endTime 
)
static

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

Parameters
plansource plan
startTimeabsolute time of start time
endTimeabsolute time of end time
Returns
truncated plan

◆ cutDownTo() [1/2]

Plan larcfm::PlanUtil::cutDownTo ( const Plan plan,
double  timeOfCurrentPosition,
double  intentThreshold 
)
static

Create new Plan from time "timeOfCurrentPosition" to time "intentThreshold"

NOTE: currently only being used in Unit Tests

Parameters
planplan
timeOfCurrentPositiontime
intentThresholdtime
Returns
plan

◆ cutDownTo() [2/2]

Plan larcfm::PlanUtil::cutDownTo ( const Plan plan,
double  t1,
double  t2,
double  tExtend 
)
static

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.

Parameters
planPlan file to be cut
timeOfCurrentPositionCurrent location of aircraft in the plan file
intentThresholdthe absolute lookahead time – all acceleration information after this time is not copied
tExtendAfter the intentThreshold, the plan is extended linearly to this time (absolute time)
Returns
plan

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.

Parameters
planPlan file to be cut
t1Location to start cut out. If before first point or negative, first point is used
t2The absolute lookahead time – all plan information after this time is not copied
tExtendAfter the intentThreshold, the plan is extended linearly to this time (absolute time)
Returns
plan cut down to times [t1,t2].

◆ diffMetric()

double larcfm::PlanUtil::diffMetric ( const Plan lpc,
const Plan kpc 
)
static

This calculates a heuristic measure of the difference between two plans. Currently only used in KinematicsPosition test

Parameters
lpcplan 1
kpcplan 2
Returns
metric

◆ distanceBetween()

double larcfm::PlanUtil::distanceBetween ( const Plan A,
const Plan B 
)
static

Calculate a distance difference between two plans

Parameters
Aplan A
Bplan B
Returns
distance

◆ fixGsAccelAt()

void larcfm::PlanUtil::fixGsAccelAt ( Plan p,
int  ix,
double  maxAccel,
bool  checkTCP,
double  M 
)
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)

Parameters
pplan
ixindex of ground speed change
maxAccelmaximum ground speed acceleration (non-negative)
checkTCPif true, do not alter time if point and it is an EOT or EVS
Mmax time

◆ fixInfeasibleTurnAt()

bool larcfm::PlanUtil::fixInfeasibleTurnAt ( Plan lpc,
int  ix,
bool  allowRepairByIntersection 
)
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

Parameters
lpcsemi-linear plan (no GS or VS TCPs)
ixindex
Returns
true if point removed

◆ fixInfeasibleTurnAtViaIntersection()

bool larcfm::PlanUtil::fixInfeasibleTurnAtViaIntersection ( Plan lpc,
int  ix 
)
static

Create an intersection point between ix-1 and ix

Parameters
lpcPlan to be repaired
ixindex of problem turn
Returns
true if plan was successfully repaired

◆ infeasibleTurns()

int larcfm::PlanUtil::infeasibleTurns ( Plan lpc,
int  startIx,
int  endIx,
double  default_bank_angle,
bool  repair 
)
static

Repair turns for semi-linear plan. Removes vertex point if turn is infeasible.

Parameters
lpcPlan that needs repair
startIxStarting index
endIxEnding index
default_bank_angleDefault bank angle
repairIf true, attempt repair. If false, only determine the number of bad turns remaining.
Returns
Number of repaired or infeasible vertices

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.

Parameters
fpPlan that needs repair
bankAngledefault bank angle
repairif true, attempt repair. If false, only determine the number of bad turns remaining.
Returns
number of repaired or infeasible vertices

Note: This assumes that there are no GS or VS TCPs in lpc and no existing turns in the repair range

◆ interpolateVirtuals()

void larcfm::PlanUtil::interpolateVirtuals ( Plan ac,
double  accuracy,
double  startTm,
double  endTm 
)
static

this adds virtual points int the plan starting at or before startTm, and ending before or at endTm

Parameters
acplan
accuracyaccuracy of projections
startTmstarting segment
endTmending segment

◆ intersection()

NavPoint larcfm::PlanUtil::intersection ( Plan  lpc,
int  i 
)
staticprivate

Create intersection point between (i-1) and i

Parameters
lpclinear plan
iindex
Returns
point

Create intersection point between (i-1) and i

Parameters
lpc
i
Returns

◆ isCurrentPositionUnchanged()

bool larcfm::PlanUtil::isCurrentPositionUnchanged ( const Plan solution,
double  currentTime,
const Position currentPos 
)
static

Simple check that current position (currentPos) is where it should be according to plan "solution"

Parameters
solutionthe trajectory
currentTimecurrent time
currentPoscurrent position
Returns
true if position is almost equal to currentPos.position(currentTime)

◆ isVelocityContAtTcps()

bool larcfm::PlanUtil::isVelocityContAtTcps ( const Plan p,
bool  silent 
)
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.

Parameters
pPlan to be tested
silentif true, output is suppressed
Returns

◆ mkGsConstant() [1/4]

Plan larcfm::PlanUtil::mkGsConstant ( const Plan p)
static

Make ground speed constant gs between wp1 and wp2.

Parameters
pa plan
Returns
a new plan with constant ground speed

◆ mkGsConstant() [2/4]

Plan larcfm::PlanUtil::mkGsConstant ( const Plan p,
double  gs 
)
static

Make ground speed constant gs for entire plan.

Parameters
pplan
gsground speed
Returns
a new plan

◆ mkGsConstant() [3/4]

Plan larcfm::PlanUtil::mkGsConstant ( const Plan p,
int  i,
int  j,
double  gs 
)
static

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.
Parameters
pplan
iindex of waypoint 1
jindex of waypoint 2
gsground speed
Returns
a new plan

◆ mkGsConstant() [4/4]

Plan larcfm::PlanUtil::mkGsConstant ( const Plan p,
int  wp1,
int  wp2 
)
static

Make ground speed constant between wp1 and wp2 as an average of the total distance and time travelled.

Parameters
ptrajectory
wp1starting waypoint
wp2starting waypoint
Returns
trajectory revised so that ground speed is constant on [wp1,wp2]

◆ mkVsConstant() [1/4]

void larcfm::PlanUtil::mkVsConstant ( Plan p)
static

Make vertical speed constant vs over plan with vs being the average speed.

Parameters
pplan

◆ mkVsConstant() [2/4]

void larcfm::PlanUtil::mkVsConstant ( Plan p,
double  vs 
)
static

Make vertical speed constant vs for full plan.

Parameters
pplan
vsvertical speed

◆ mkVsConstant() [3/4]

void larcfm::PlanUtil::mkVsConstant ( Plan p,
int  start,
int  end 
)
static

Make vertical speed constant vs between wp1 and wp2, with vs being the average speed.

Parameters
pplan
startstarting index
endending index

◆ mkVsConstant() [4/4]

void larcfm::PlanUtil::mkVsConstant ( Plan p,
int  wp1,
int  wp2,
double  vs 
)
static

Make vertical speed constant vs between wp1 and wp2. Assumes plan does not contain any vertical speed accel regions

◆ mkVsContinuousAt()

int larcfm::PlanUtil::mkVsContinuousAt ( Plan p,
int  i 
)
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.

Parameters
ptrajectory
iindex
Returns
revised trajectory

◆ mkVsShortLegsContinuous()

int larcfm::PlanUtil::mkVsShortLegsContinuous ( Plan p,
int  start,
int  end,
double  vsAccel,
bool  inhibitFixGs0,
double  aggressiveFactor,
bool  inhibitFixVs0 
)
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.

Parameters
pPlan to be modified
startstarting index
endending index
vsAccelvertical speed acceleration
inhibitFixGs0if true will NOT attempt to repair on segments with gsOut = 0
inhibitFixVs0if true will NOT attempt to repair level segments (i.e. vertical speed = 0)
aggressiveFactora factor in the interval (0,1] that determines how aggressively the repair is done
Returns
number of segments that were smoothed

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.

Parameters
pPlan to be modified
startstarting index
endending index
vsAccelvertical speed acceleration
Returns
number of segments that were smoothed

◆ nextTrackChange()

int larcfm::PlanUtil::nextTrackChange ( const Plan fp,
int  iNow 
)
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

Parameters
fpplan
iNowindex
Returns
index of next track change

◆ nextVsChange()

int larcfm::PlanUtil::nextVsChange ( const Plan p,
int  iNow 
)
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

Parameters
pplan
iNowindex
Returns
index of next vertical speed change

◆ positionFromDistanceInSeg()

Position larcfm::PlanUtil::positionFromDistanceInSeg ( const Plan p,
double  curTm,
double  advDistance,
bool  linear 
)
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

Parameters
pplan of interest
curTmcurrentTime of so
advDistancedistance to advance
linearif true, treat plan as a linear plan (i.e. path is not curved)
Returns
Position "advDistance" ahead of "curTm"

◆ prevTrackChange()

int larcfm::PlanUtil::prevTrackChange ( const Plan fp,
int  iNow 
)
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

Parameters
fpplan
iNowindex
Returns
index of track change

◆ prevVsChange()

int larcfm::PlanUtil::prevVsChange ( const Plan p,
int  iNow 
)
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

Parameters
pplan
iNowindex
Returns
index of vertical speed change

◆ projectPlan()

Plan larcfm::PlanUtil::projectPlan ( const Plan p,
const EuclideanProjection &  proj 
)
static

Project plan from latlonalt to Euclidean. Note: some tcp turn information may no longer be valid.

Parameters
pplan
projprojection
Returns
plan

◆ repairShortVsLegs()

Plan larcfm::PlanUtil::repairShortVsLegs ( const Plan fp,
double  vsAccel 
)
static

Attempts to repair plan with infeasible vertical points by averaging vertical speeds over 2 segments. Assumes linear plan input.

Parameters
fp
vsAccel
minVsChangeRequired
Returns

◆ revertAllTCPs()

Plan larcfm::PlanUtil::revertAllTCPs ( const Plan pln,
bool  markIndices 
)
static

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.

Parameters
plnkinematic plan to be reverted to a linear plan
markIndicesif true, add :ksrc-#: to each point's info (original behavior is "false") This may include multiple flags in a single point.
Returns
plan

◆ revertGroupOfTCPs()

int larcfm::PlanUtil::revertGroupOfTCPs ( Plan pln,
int  ix 
)
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

Parameters
plnplan
ixThe index of one of the TCPs created together that should be reverted
Returns
index of the reverted point

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!!

Parameters
ixThe index of one of the TCPs created together that should be reverted
Returns
index of the reverted point

◆ revertGsTCP()

int larcfm::PlanUtil::revertGsTCP ( Plan pln,
int  ix,
bool  revertPreviousTurn 
)
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!)

Parameters
plnplan
ixindex of BGS
revertPreviousTurnif true then if this GS segment is right after a turn then revert the turn as well
Returns
index of reverted point

◆ setAltPreserveByDelta()

Plan larcfm::PlanUtil::setAltPreserveByDelta ( const Plan lpc)
staticprivate

Called after revertVsTCPs to set AltPreserve. Assumes that vsAccels have been saved in reverted Plan

Note: this is very similar to TrajGen.markVsChanges

Parameters
lpc
vsAccel
Returns

◆ shareSegment()

int larcfm::PlanUtil::shareSegment ( const Plan p1,
const Plan p2 
)
static

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

Parameters
p1point 1
p2point 2
Returns
segment number

◆ timeFromGs()

double larcfm::PlanUtil::timeFromGs ( double  gsInit,
double  gsAccel,
double  dist 
)
static

time required to cover distance "dist" if initial speed is "gsInit" and acceleration is "gsAccel"

Parameters
gsAccelground speed acceleration
gsInitinitial ground speed
distdistance travelled
Returns
time

time required to cover distance "dist" if initial speed is "gsInit" and acceleration is "gsAccel"

Parameters
gsAccelground speed acceleration
gsInitinitial ground speed
distdistance travelled
Returns

The documentation for this class was generated from the following files: