void KinematicIntegerBands::traj_conflict_only_bands(std::vector<Integerval>& l,
Detection3D* conflict_det, Detection3D* recovery_det, double tstep, double B, double T, double B2, double T2,
bool trajdir, int max, const TrafficState& ownship, const std::vector<TrafficState>& traffic) const {
int d = -1; // Set to the first index with no conflict
for (int k = 0; k <= max; ++k) {
double tsk = tstep*k;
if (d >=0 && no_conflict(conflict_det,recovery_det,B,T,B2,T2,trajdir,tsk,ownship,traffic)) {
continue;
} else if (d >=0) {
l.push_back(Integerval(d,k-1));
d = -1;
} else if (no_conflict(conflict_det,recovery_det,B,T,B2,T2,trajdir,tsk,ownship,traffic)) {
d = k;
}
}
if (d >= 0) {
l.push_back(Integerval(d,max));
}
}
bool KinematicAltBands::conflict_free_traj_step(Detection3D* conflict_det, Detection3D* recovery_det,
double B, double T, double B2, double T2,
const TrafficState& ownship, const std::vector<TrafficState>& traffic) const {
bool trajdir = true;
if (instantaneous_bands()) {
return no_conflict(conflict_det,recovery_det,B,T,B2,T2,trajdir,0,ownship,traffic);
} else {
double tstep = time_step(ownship);
double target_alt = min_val(ownship)+j_step_*get_step();
Tuple5<double,double,double,double,double> tsqj = Kinematics::vsLevelOutTimes(ownship.altitude(),ownship.verticalSpeed(),
vertical_rate_,target_alt,vertical_accel_,-vertical_accel_,true);
double tsqj1 = tsqj.first+0;
double tsqj2 = tsqj.second+0;
double tsqj3 = tsqj.third+tstep;
for (int i=0; i<=std::floor(tsqj1/tstep);++i) {
double tsi = i*tstep;
if ((B<=tsi && tsi<=T && any_los_aircraft(conflict_det,trajdir,tsi,ownship,traffic)) ||
(recovery_det != NULL && B2 <= tsi && tsi <= T2 &&
any_los_aircraft(recovery_det,trajdir,tsi,ownship,traffic))) {
return false;
}
}
if ((tsqj2>=B &&
any_conflict_aircraft(conflict_det,B,std::min(T,tsqj2),trajdir,std::max(tsqj1,0.0),ownship,traffic)) ||
(recovery_det != NULL && tsqj2>=B2 &&
any_conflict_aircraft(recovery_det,B2,std::min(T2,tsqj2),trajdir,std::max(tsqj1,0.0),ownship,traffic))) {
return false;
}
for (int i=(int)std::ceil(tsqj2/tstep); i<=std::floor(tsqj3/tstep);++i) {
double tsi = i*tstep;
if ((B<=tsi && tsi<=T && any_los_aircraft(conflict_det,trajdir,tsi,ownship,traffic)) ||
(recovery_det != NULL && B2 <= tsi && tsi <= T2 &&
any_los_aircraft(recovery_det,trajdir,tsi,ownship,traffic))) {
return false;
}
}
return no_conflict(conflict_det,recovery_det,B,T,B2,T2,trajdir,std::max(tsqj3,0.0),ownship,traffic);
}
}
/**
* @brief Generates an initial cluster of atoms for starting simulation
*
* @param at Atom array
* @param dat Common data for simulation
* @param from first atom of the list on which to work
* @param to last atom of the list on which to work
* @param mode Building mode : -1 sets coordinates to 9999.9 (infinity), 0 sets all atom at the origin, 1 sets atom at a random position (with constraints)
*/
void build_cluster(ATOM at[], DATA *dat, uint32_t from, uint32_t to, int32_t mode)
{
uint32_t i = 0 ;
// fprintf(stdout,"Entered in build cluster : from %d to %d mode %d\n",from,to,mode);
// fflush(stdout);
if (mode==-1) //infinite initialisation mode
{
for (i=from; i<to; i++)
at[i].x=at[i].y=at[i].z=9999.9;
}
else if (mode==0) //zero everywhere : all at origin
{
for (i=from; i<to; i++)
at[i].x=at[i].y=at[i].z=0.0;
}
else if (mode==1) //random mode
{
double randvec[3] = {0.0} ;
// double rtot=0.0;
//
// for (i=0; i<dat->natom; i++)
// rtot += at[i].ljp.sig;
//
// rtot /= (double)dat->natom;
// rtot = (double)dat->natom*4.0*3.14159*X3(rtot)/3.0;
// dat->inid = pow(3.0*rtot/(4.0*3.14159),0.333);
dat->inid = dat->natom/8.0;
for (i=from; i<to; i++)
{
// fprintf(stdout,"loop %d\n",i);
// fflush(stdout);
do
{
get_vector(dat,-1,randvec);
at[i].x = dat->inid*randvec[0];
at[i].y = dat->inid*randvec[1];
at[i].z = dat->inid*randvec[2];
}
while( (X2(at[i].x)+X2(at[i].y)+X2(at[i].z))>X2(dat->inid) || no_conflict(at,i) != NO_CONFLICT );
}
}
}
bool KinematicIntegerBands::no_instantaneous_conflict(Detection3D* conflict_det, Detection3D* recovery_det,
double B, double T, double B2, double T2,
bool trajdir, const TrafficState& ownship, const std::vector<TrafficState>& traffic,
const TrafficState& repac,
int epsh, int epsv) {
bool usehcrit = repac.isValid() && epsh != 0;
bool usevcrit = repac.isValid() && epsv != 0;
std::pair<Vect3,Velocity> nsovo = trajectory(ownship,0,trajdir);
Vect3 so = ownship.get_s();
Vect3 vo = ownship.get_v();
Vect3 si = repac.get_s();
Vect3 vi = repac.get_v();
Vect3 nvo = nsovo.second;
Vect3 s = so.Sub(si);
return
(!usehcrit || CriteriaCore::horizontal_new_repulsive_criterion(s,vo,vi,nvo,epsh)) &&
(!usevcrit || CriteriaCore::vertical_new_repulsive_criterion(s,vo,vi,nvo,epsv)) &&
no_conflict(conflict_det,recovery_det,B,T,B2,T2,trajdir,0,ownship,traffic);
}
