-
Notifications
You must be signed in to change notification settings - Fork 15
/
Copy pathCriteriaCore.h
270 lines (223 loc) · 11.5 KB
/
CriteriaCore.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
/*
* CriteriaCore.h
* Release: ACCoRD++-2.b (08/22/10)
*
* State-based Implicit Criteria
*
* Copyright (c) 2011-2021 United States Government as represented by
* the National Aeronautics and Space Administration. No copyright
* is claimed in the United States under Title 17, U.S.Code. All Other
* Rights Reserved.
*/
#ifndef CRITERIACORE_H_
#define CRITERIACORE_H_
#include "Velocity.h"
#include "CDCylinder.h"
namespace larcfm {
/**
* This class provides criteria for
*
* horizontal conflict (See ACCoRD PVS theory: horizontal_criterion.pvs)
* vertical conflict (See ACCoRD PVS theory: vertical_criterion.pvs)
* horizontal loss of separation (See ACCoRD PVS theory: repulsive.pvs)
* vertical loss of separation (See ACCoRD PVS Theory: vertical_los_generic.pvs)
*/
class CriteriaCore {
public:
/**
* Horizontal coordination.
*
* @param s the relative position
* @param v the relative velocity
*
* @return the horizontal coordination sign
* -1 corresponds to turning right in the relative system
* +1 corresponds to turing left in the relative system
*/
static int horizontalCoordination(const Vect2& s, const Vect2& v);
/**
* Horizontal coordination.
*
* @param s the relative position
* @param v the relative velocity
*
* @return the horizontal coordination sign
* -1 corresponds to turning right in the relative system
* +1 corresponds to turning left in the relative system
*/
static int horizontalCoordination(const Vect3& s, const Vect3& v);
/**
* Vertical coordination.
*
* @param s the relative position
* @param vo the ownship velocity
* @param vi the traffic aircraft velocity
* @param D the horizontal distance
* @param H the vertical distance
* @param ownship the name of the ownship
* @param traffic the name of the traffic aircraft
*
* @return the vertical coordination sign.
* -1 corresponds to reducing current vertical speed,
* +1 corresponds to increasing current vertical speed
*/
static int verticalCoordination(const Vect3& s, const Vect3& vo, const Vect3& vi,double D, double H, const std::string& ownship, const std::string& traffic);
/** The fundamental horizontal criterion (Conflict Case)
* @param sp relative position assumed to be horizontally separated (Sp_vect2 : TYPE = (horizontal_sep?))
* @param v relative velocity
* @param D protection zone diameter
* @param epsh +1 or -1
*/
static bool horizontal_criterion(const Vect2& sp, const Vect2& v, double D, int epsh);
/** vertical criterion (Conflict Case) (currently only used in VisualCriteriaCore)
* @param epsv vertical coordination parameter
* @param s relative position
* @param v relative velocity
* @param nv new relative velocity
* @param D protection zone diameter
* @param H protection zone height
*/
static bool vertical_criterion(int eps, const Vect3& s, const Vect3& v, const Vect3& nv, double D, double H);
/**
* Checks whether a horizontal solution is repulsive or divergent in the turn direction indicated by nvo
* (2D geometry)
* @param s relative position of the ownship
* @param vo velocity of the ownship aircraft
* @param vi velocity of the traffic aircraft
* @param nvo resolution velocity of the traffic aircraft
* @param eps horizontalCoordination
*/
static bool horizontal_los_criterion(const Vect2& s, const Vect2& vo, const Vect2& vi,const Vect2& nvo, int eps);
/**
* Checks whether a horizontal solution is repulsive or divergent in the turn direction indicated by nvo3
* @param s3 relative position of the ownship
* @param vo3 velocity of the ownship aircraft
* @param vi3 velocity of the traffic aircraft
* @param nvo3 resolution velocity of the traffic aircraft
* @param eps horizontalCoordination
*/
static bool horizontalRepulsiveCriteria(const Vect3& s3, const Velocity& vo3, const Velocity& vi3, const Velocity& nvo3, int eps);
/** Checks whether a velocity vector satisfies the repulsive LoS criteria
* @param s relative position of the ownship
* @param vo velocity of the ownship aircraft
* @param vi velocity of the traffic aircraft
* @param nvo resolution velocity of the traffic aircraft
* @param H protection zone height
* @param minrelvs minimum relative exit speed
* @param epsv vertical coordination parameter
*/
static bool verticalRepulsiveCriterion(const Vect3& s, const Vect3& vo, const Vect3& vi, const Vect3& nvo,
double H, double minrelvs, int epsv);
/**
* true iff the velocity vector nvo satisfies the implicit coordination criteria
* @param s relative position of the ownship
* @param vo velocity of the ownship aircraft
* @param vi velocity of the traffic aircraft
* @param nvo velocity vector to be checked
* @param minRelVs desired minimum relative exit ground speed (used in LoS only)
* @param D diameter of the protection zone
* @param H height of the protection zone
*
*/
static bool criteria(const Vect3& s, const Velocity& vo, const Velocity& vi, const Velocity& nvo,
double minRelVs, double D, double H, int epsh, int epsv);
static Vect2 incr_trk_vect(const Vect2& vo, double step, int dir);
static int losr_trk_iter_dir(const Vect2& s, const Vect2& vo, const Vect2& vi, double step, int eps);
/** Return an absolute repulsive track search direction (or none)
* Assumes velocity vo is non-zero.
* @return +1 search is increasing, -1 search is decreasing
*/
static int trkSearchDirection(const Vect3& s, const Vect3& vo, const Vect3& vi, int eps);
// Compute a new ground speed only vector that is one step to the dir of vo
// dir = 1 is increasing. Velocity vo is non-zero.
static Vect2 incr_gs_vect(const Vect2& vo, double step, int dir);
// Compute an absolute repulsive ground speed direction (or none)
// dir = 1 is increasing. Velocity vo is non-zero.
static int losr_gs_iter_dir(const Vect2& s, const Vect2& vo, const Vect2& vi, double mings, double maxgs, double step, int eps);
/** Return an absolute repulsive ground speed search direction (or none)
* Assumes Velocity vo is non-zero.
* @return +1 search is increasing, -1 search is decreasing
*/
static int gsSearchDirection(const Vect3& s, const Vect3& vo, const Vect3& vi, int eps);
/** Return an absolute repulsive vertical speed search direction (or none)
* Assumes Velocity vo is non-zero.
* @return +1 search is increasing, -1 search is decreasing
*/
static int vsSearchDirection(int epsv);
/** Return the horizontal epsilon that corresponds to the direction the traffic aircraft is currently turning indicated by sign of trackRate
*
* @param s relative position
* @param vo velocity of ownship
* @param vi velocity of intruder
* @param epsh horizontal epsilon
* @param trackRate trackRate of traffic
* @return horizontal epsilon
*/
static int dataVsRateEpsilon(int epsv, double vsRate);
/** Return the horizontal epsilon that corresponds to the direction the traffic aircraft is currently turning indicated by sign of trackRate
*
* @param s relative position
* @param vo velocity of ownship
* @param vi velocity of intruder
* @param epsh horizontal epsilon
* @param trackRate trackRate of traffic
* @return horizontal epsilon
*/
static int dataTurnEpsilon(const Vect3& s, const Velocity& vo, const Velocity& vi, int epsh, double trackRate);
/**
* [CAM] This method replaces horizontal_old_repulsive_criterion. This definition is independent
* and coordinated for both state-based and iterative algorithms. This method is intended to be used
* with kinematic solvers, that is it is called iteratively with nvo progressing with each iteration.
*
* Checks whether a horizontal solution is repulsive or divergent in the turn direction indicated by nvo
* (2D geometry). Derived from PVS predicate: repulsive.repulsive_criteria(s,v,eps)(nv): bool
* @param s relative position of the ownship
* @param vo velocity of the ownship aircraft
* @param vi velocity of the traffic aircraft
* @param nvo resolution velocity of the traffic aircraft
* @param eps horizontalCoordination
*/
static bool horizontal_new_repulsive_criterion(const Vect2& s, const Vect2& vo, const Vect2& vi, const Vect2& nvo, int eps);
/**
* [CAM] This method replaces horizontal_old_repulsive_criterion. This definition is independent
* and coordinated for both state-based and iterative algorithms. This method is intended to be used
* with kinematic solvers, that is it is called iteratively with nvo progressing with each iteration.
*
* Checks whether a horizontal solution is repulsive or divergent in the turn direction indicated by nvo
* (2D geometry). Derived from PVS predicate: repulsive.repulsive_criteria(s,v,eps)(nv): bool
* @param s relative position of the ownship
* @param vo velocity of the ownship aircraft
* @param vi velocity of the traffic aircraft
* @param nvo resolution velocity of the traffic aircraft
* @param eps horizontalCoordination
*/
static bool horizontal_new_repulsive_criterion(const Vect3& s, const Vect3& vo, const Vect3& vi, const Vect3& nvo, int eps);
/**
* [CAM] This method replaces vertical_old_repulsive_criterion. It's intended to be used for kinematic,
* iterative maneuvers. This criterion is like vs_bound_crit? but removes the else branch that restricts
* some vertical maneuvers.
*
* @param s relative position of the ownship
* @param vo velocity of the ownship aircraft
* @param vi velocity of the traffic aircraft
* @param nvo resolution velocity of the traffic aircraft
* @param eps Vertical coordination
*/
static bool vertical_new_repulsive_criterion(const Vect3& s, const Vect3& vo, const Vect3& vi, const Vect3& nvo, int eps);
static int verticalCoordinationLoS(const Vect3& s, const Vect3& vo, const Vect3& vi, const std::string& ownship, const std::string& traffic);
private:
static bool horizontal_criterion_0(const Vect2& sp, int eps, const Vect2& v, double D);
static int verticalCoordinationConflict(const Vect3& s, const Vect3& v, double D, const std::string& ownship, const std::string& traffic);
static bool criterion_3D(const Vect3& sp, const Velocity& v, int epsH, int epsV, const Velocity& nv, double D, double H);
// caD and caH are the diameter and height of a collision avoidance zone, e.g., 350ft x 100 ft
static Vect3 vertical_decision_vect(const Vect3& s, const Vect3& vo, const Vect3& vi, double caD, double caH);
// Compute an absolute repulsive vertical direction
static int losr_vs_dir(const Vect3& s, const Vect3& vo, const Vect3& vi, double caD, double caH, const std::string& ownship, const std::string& traffic);
static bool vs_bound_crit(const Vect3& v, const Vect3& nv, int eps);
static double min_rel_vert_speed(double vz, int eps, double minrelvs);
static bool vertical_los_criterion(const Vect3& s, const Vect3& v, const Vect3& nv, int eps, double H, double minrelvs);
/** Perform a symmetry calculation */
static int breakSymmetry(const Vect3& s, const std::string& ownship, const std::string& traffic);
};
}
#endif