Exemple #1
0
LossData WCV_TAUMOD::horizontal_WCV_interval(double T, const Vect2& s, const Vect2& v) const {
  double time_in = T;
  double time_out = 0;
  double sqs = s.sqv();
  double sdotv = s.dot(v);
  double sqD = Util::sq(table.getDTHR());
  double a = v.sqv();
  double b = 2*sdotv+table.getTTHR()*v.sqv();
  double c = sqs+table.getTTHR()*sdotv-sqD;
  if (Util::almost_equals(a,0) && sqs <= sqD) { // [CAM] Changed from == to almost_equals to mitigate numerical problems
    time_in = 0;
    time_out = T;
    return LossData(time_in,time_out);
  }
  if (sqs <= sqD) {
    time_in = 0;
    time_out = std::min(T,Horizontal::Theta_D(s,v,1,table.getDTHR()));
    return LossData(time_in,time_out);
  }
  double discr = Util::sq(b)-4*a*c;
  if (sdotv >= 0 || discr < 0)
    return LossData(time_in,time_out);
  double t = (-b - std::sqrt(discr))/(2*a);
  if (Horizontal::Delta(s, v,table.getDTHR()) >= 0 && t <= T) {
    time_in = std::max(0.0,t);
    time_out = std::min(T, Horizontal::Theta_D(s,v,1,table.getDTHR()));
  }
  return LossData(time_in,time_out);
}
Exemple #2
0
LossData WCV_TEP::horizontal_WCV_interval(double T, const Vect2& s, const Vect2& v) const {
  double time_in = T;
  double time_out = 0;
  double sqs = s.sqv();
  double sqv = v.sqv();
  double sdotv = s.dot(v);
  double sqD = Util::sq(table.getDTHR());
  if (Util::almost_equals(sqv,0) && sqs <= sqD) { // [CAM] Changed from == to almost_equals to mitigate numerical problems
    time_in = 0;
    time_out = T;
    return LossData(time_in,time_out);
  }
  if (Util::almost_equals(sqv,0)) // [CAM] Changed from == to almost_equals to mitigate numerical problems
    return LossData(time_in,time_out);
  if (sqs <= sqD) {
    time_in = 0;
    time_out = Util::min(T,Horizontal::Theta_D(s,v,1,table.getDTHR()));
    return LossData(time_in,time_out);
  }
  if (sdotv > 0 || Horizontal::Delta(s,v,table.getDTHR()) < 0)
    return LossData(time_in,time_out);
  double tep = Horizontal::Theta_D(s,v,-1,table.getDTHR());
  if (tep-table.getTTHR() > T)
    return LossData(time_in,time_out);
  time_in = Util::max(0.0,tep-table.getTTHR());
  time_out = Util::min(T,Horizontal::Theta_D(s,v,1,table.getDTHR()));
  return LossData(time_in,time_out);
}
Exemple #3
0
void TCAS2D::RA2D_interval(double DMOD, double Tau, double B, double T, Vect2 s, Vect2 vo, Vect2 vi) {
  t_in = B;
  t_out = T;
  Vect2 v = vo.Sub(vi);
  double sqs = s.sqv();
  double sdotv = s.dot(v);
  double sqD = Util::sq(DMOD);
  if (vo.almostEquals(vi) && sqs <= sqD)
    return;
  double sqv = v.sqv();
  if (sqs <= sqD) {
    t_out = Util::root2b(sqv,sdotv,sqs-sqD,1);
    return;
  }
  double b = 2*sdotv+Tau*sqv;
  double c = sqs+Tau*sdotv-sqD;
  if (sdotv >= 0 || Util::discr(sqv,b,c) < 0) {
    t_in = T+1;
    t_out = 0;
    return;
  }
  t_in = Util::root(sqv,b,c,-1);
  if (Horizontal::Delta(s,v,DMOD) >= 0)
    t_out = Horizontal::Theta_D(s,v,1,DMOD);
  else
    t_out = Util::root(sqv,b,c,1);
}
Exemple #4
0
// Compute modified tau
double TCAS2D::tau_mod(double DMOD, const Vect2& s, const Vect2& v) {
  double sdotv = s.dot(v);

  if (Util::almost_equals(sdotv,0)) // [CAM] Changed from == to almost_equals to mitigate numerical problems
    return 0;
  return (Util::sq(DMOD)-s.sqv())/sdotv;
}
Exemple #5
0
double WCV_TEP::horizontal_tvar(const Vect2& s, const Vect2& v) const {
  // Time variable is Modified Tau
  double TEP = -1;
  double sdotv = s.dot(v);
  if (sdotv < 0)
    return (Util::sq(table.getDTHR())-s.sqv())/sdotv;
  return TEP;
}
Exemple #6
0
double TCAS2D::time_of_min_tau(double DMOD, double B, double T, const Vect2& s, const Vect2& v) {
  if (v.ScalAdd(B,s).dot(v) >= 0)
    return B;
  double d = Horizontal::Delta(s,v,DMOD);
  double rr = 0;
  if (d < 0)
    rr = 2*std::sqrt(-d) / v.sqv();
  if (v.ScalAdd(T,s).dot(v) < 0)
    return T;
  return nominal_tau(B,T,s,v,rr);
}
Exemple #7
0
/**
 * Return time to closest point approach
 * if time is negative or velocities are parallel returns 0
 */
double Vect2::tcpa (const Vect2& so, const Vect2& vo, const Vect2& si, const Vect2& vi){
	double t;
	Vect2 s = so.Sub(si);
	Vect2 v = vo.Sub(vi);
	double nv = v.sqv();
	if (nv > 0)
		t = std::max(0.0,-s.dot(v)/nv);
	else
		t = 0;
	return t;
}
Exemple #8
0
Vect2 TangentLine::Q(const Vect2& s, const double D, const int eps) {
  double sq_s = s.sqv();
  double sq_D = sq(D);
  double delta = sq_s -sq_D;
  if (delta >= 0) { 
    double alpha = sq_D/sq_s;
    double beta  = D*sqrt_safe(delta)/sq_s;
    return Vect2(alpha*s.x+eps*beta*s.y,
		 alpha*s.y-eps*beta*s.x);   
    }
    return Vect2();
}
Exemple #9
0
LossData WCV_TCPA::horizontal_WCV_interval(double T, const Vect2& s, const Vect2& v) const {
  double time_in = T;
  double time_out = 0;
  double sqs = s.sqv();
  double sqv = v.sqv();
  double sdotv = s.dot(v);
  double sqD = Util::sq(table.getDTHR());
  if (Util::almost_equals(sqv,0) && sqs <= sqD) { // [CAM] Changed from == to almost_equals to mitigate numerical problems
    time_in = 0;
    time_out = T;
    return LossData(time_in,time_out);
  }
  if (Util::almost_equals(sqv,0)) // [CAM] Changed from == to almost_equals to mitigate numerical problems
    return LossData(time_in,time_out);
  if (sqs <= sqD) {
    time_in = 0;
    time_out = std::min(T,Horizontal::Theta_D(s,v,1,table.getDTHR()));
    return LossData(time_in,time_out);
  }
  if (sdotv > 0)
    return LossData(time_in,time_out);
  double tcpa = Horizontal::tcpa(s,v);
  if (v.ScalAdd(tcpa, s).norm() > table.getDTHR())
    return LossData(time_in,time_out);
  double Delta = Horizontal::Delta(s,v,table.getDTHR());
  if (Delta < 0 && tcpa - table.getTTHR() > T)
    return LossData(time_in,time_out);
  if (Delta < 0) {
    time_in = std::max(0.0,tcpa-table.getTTHR());
    time_out = std::min(T,tcpa);
    return LossData(time_in,time_out);
  }
  double tmin = std::min(Horizontal::Theta_D(s,v,-1,table.getDTHR()),tcpa-table.getTTHR());
  if (tmin > T)
    return LossData(time_in,time_out);
  time_in = std::max(0.0,tmin);
  time_out = std::min(T,Horizontal::Theta_D(s,v,1,table.getDTHR()));
  return LossData(time_in,time_out);
}
Exemple #10
0
double InitTangentLineY (const Vect2& s, const double D, const int eps) {
//  double x;
  double y;
  double sq_s  = s.sqv();
  double sq_D  = sq(D);
  if (Util::almost_equals(sq_s,sq_D))  {
//    x = eps*s.y;
    y = -eps*s.x;
  } else {
    Vect2 q = TangentLine::Q(s,D,eps);
//    x = q.x;
    y = q.y;
    if (!q.isZero()) {
//      x -= s.x;
      y -= s.y;
    }
  }
  return y;
}
Exemple #11
0
double TCAS2D::nominal_tau(double B, double T, const Vect2& s, const Vect2& v, double rr) {
  if (v.isZero())
    return B;
  return std::max(B,std::min(T,-s.dot(v) / v.sqv()-rr/2));
}
Exemple #12
0
/**
 * returns the perpendicular distance between line defined vy s,v and point q.
 * @param s
 * @param v
 * @param q
 */
double Vect2::distAlong(const Vect2& s, const Vect2& v, const Vect2& q) {
	double tp = q.Sub(s).dot(v)/v.sqv();
	//f.pln(" $$$ distAlong: tp = "+tp);
	return Util::sign(tp)*v.Scal(tp).norm();

}
Exemple #13
0
/**
 * returns the perpendicular distance between line defined vy s,v and point q.
 * @param s
 * @param v
 * @param q
 */
double Vect2::distPerp(const Vect2& s, const Vect2& v, const Vect2& q) {
	double tp = q.Sub(s).dot(v)/v.sqv();
	return s.Add(v.Scal(tp)).Sub(q).norm();

}