예제 #1
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;
}
예제 #2
0
파일: WCV_TEP.cpp 프로젝트: nasa/WellClear
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);
}
예제 #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);
}
예제 #4
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);
}
예제 #5
0
파일: WCV_TEP.cpp 프로젝트: nasa/WellClear
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;
}
예제 #6
0
파일: VectFuns.cpp 프로젝트: nasa/WellClear
Vect2 VectFuns::closestPoint(const Vect2& a, const Vect2& b, const Vect2& so) {
	// translate a to origin, then project so onto the line defined by ab, then translate back to a
	Vect2 ab = b.Sub(a);
	return ab.Scal(so.Sub(a).dot(ab)/ab.dot(ab)).Add(a);
//	if (collinear(a,b,so)) return so;
//	Vect2 v = a.Sub(b).PerpL().Hat(); // perpendicular vector to line
//	Vect2 s2 = so.AddScal(100, v);
//	Vect2 cp = intersection(so,s2,100,a,b).first;
//	return cp;
}
예제 #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;
}
예제 #8
0
/**
 * Return actual time of closest point approach (return negative infinity if parallel)
 */
double Vect2::actual_tcpa (const Vect2& so, const Vect2& vo, const Vect2& si,  const Vect2& vi){
	double rtn;
	Vect2 s = so - si;
	Vect2 v = vo - vi;
	double nv = v.norm();
	if (nv > 0) {
		rtn = -s.dot(v)/(nv*nv);
	} else {
		rtn = NINFINITY;;
	}
	return rtn;
}
예제 #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);
}
예제 #10
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));
}
예제 #11
0
bool TCAS2D::horizontal_RA(double DMOD, double Tau, const Vect2& s, const Vect2& v) {
  if (s.dot(v) >= 0) return s.norm() <= DMOD;
  else return s.norm() <= DMOD || tau_mod(DMOD,s,v) <= Tau;
}
예제 #12
0
파일: VectFuns.cpp 프로젝트: nasa/WellClear
bool VectFuns::divergentHorizGt(const Vect2& s, const Vect2& vo, const Vect2& vi, double minRelSpeed) {
	Vect2 v = vo.Sub(vi);
	bool rtn = s.dot(v) > 0 && v.norm() > minRelSpeed;
	return rtn;
}
예제 #13
0
파일: VectFuns.cpp 프로젝트: nasa/WellClear
double VectFuns::angle_between(const Vect2& a, const Vect2& b, const Vect2& c) {
	Vect2 A = b.Sub(a);
	Vect2 B = b.Sub(c);
	return Util::acos_safe(A.dot(B)/(A.norm()*B.norm()));
}