コード例 #1
0
ファイル: VectFuns.cpp プロジェクト: nasa/WellClear
double VectFuns::distAtTau(const Vect3& s, const Vect3& vo, const Vect3& vi, bool futureOnly) {
	double tau = VectFuns::tau(s,vo,vi);
	if (tau < 0 && futureOnly)
		return s.norm();                 // return distance now
	else {
		Vect3 v = vo.Sub(vi);
		Vect3 sAtTau = s.Add(v.Scal(tau));
		return sAtTau.norm();
	}
}
コード例 #2
0
ファイル: VectFuns.cpp プロジェクト: nasa/WellClear
// time of closest approach
double VectFuns::tau(const Vect3& s, const Vect3& vo, const Vect3& vi) {
	double rtn;
	Vect3 v = vo.Sub(vi);
	double nv = v.norm();
	if (Util::almost_equals(nv,0.0)) {
		rtn = std::numeric_limits<double>::max();        // pseudo infinity
	} else
		rtn = -s.dot(v)/(nv*nv);
	return rtn;
}// tau
コード例 #3
0
ファイル: VectFuns.cpp プロジェクト: nasa/WellClear
Vect3 VectFuns::closestPoint(const Vect3& a, const Vect3& b, const Vect3& so) {
	if (a.almostEquals(b)) return Vect3::INVALID();
	Vect2 c = closestPoint(a.vect2(), b.vect2(), so.vect2());
	Vect3 v = b.Sub(a);
	double d1 = v.vect2().norm();
	double d2 = c.Sub(a.vect2()).norm();
	double d3 = c.Sub(b.vect2()).norm();
	double f = d2/d1;
	if (d3 > d1 && d3 > d2) { // negative direction
		f = -f;
	}
	return a.AddScal(f, v);


//	Vect3 v = a.Sub(b).PerpL().Hat2D(); // perpendicular vector to line
//	Vect3 s2 = so.AddScal(100, v);
//	std::pair<Vect3, double> i = intersectionAvgZ(a,b,100,so,s2);
//	// need to fix altitude to be along the a-b line
//	return interpolate(a,b,i.second/100.0);
}
コード例 #4
0
TrafficState DCPAUrgencyStrategy::mostUrgentAircraft(Detection3D* detector, const TrafficState& ownship, const std::vector<TrafficState>& traffic, double T) {
  TrafficState repac = TrafficState::INVALID;
  if (!ownship.isValid() || traffic.empty()) {
    return repac;
  }
  double mindcpa = 0;
  double mintcpa = 0;
  double D = ACCoRDConfig::NMAC_D;
  double H = ACCoRDConfig::NMAC_H;
  Vect3 so = ownship.get_s();
  Velocity vo = ownship.get_v();
  for (TrafficState::nat ac = 0; ac < traffic.size(); ++ac) {
    Vect3 si = traffic[ac].get_s();
    Velocity vi = traffic[ac].get_v();
    Vect3 s = so.Sub(si);
    Velocity v = vo.Sub(vi);
    ConflictData det = detector->conflictDetection(so,vo,si,vi,0,T);
    if (det.conflict()) {
      double tcpa = CD3D::tccpa(s,vo,vi,D,H);
      double dcpa = v.ScalAdd(tcpa,s).cyl_norm(D,H);
      // If aircraft have almost same tcpa, select the one with smallest dcpa
      // Otherwise,  select aircraft with smallest tcpa
      bool tcpa_strategy = Util::almost_equals(tcpa,mintcpa,PRECISION5) ? dcpa < mindcpa : tcpa < mintcpa;
      // If aircraft have almost same dcpa, select the one with smallest tcpa
      // Otherwise,  select aircraft with smallest dcpa
      bool dcpa_strategy = Util::almost_equals(dcpa,mindcpa,PRECISION5) ? tcpa < mintcpa : dcpa < mindcpa;
      // If aircraft are both in a min recovery trajectory, follows tcpa strategy. Otherwise follows dcpa strategy
      if (!repac.isValid() || // There are no candidates
          (dcpa <= 1 ? mindcpa > 1 || tcpa_strategy : dcpa_strategy)) {
        repac = traffic[ac];
        mindcpa = dcpa;
        mintcpa = tcpa;
      }
    }
  }
  return repac;
}
コード例 #5
0
ConflictData WCV_tvar::conflictDetection(const Vect3& so, const Velocity& vo, const Vect3& si, const Velocity& vi, double B, double T) const {
  LossData ret = WCV3D(so,vo,si,vi,B,T);
  double t_tca = (ret.getTimeIn() + ret.getTimeOut())/2;
  double dist_tca = so.linear(vo, t_tca).Sub(si.linear(vi, t_tca)).cyl_norm(table.getDTHR(),table.getZTHR());
  return ConflictData(ret, t_tca,dist_tca,so.Sub(si),vo.Sub(vi));
}
コード例 #6
0
ファイル: VectFuns.cpp プロジェクト: nasa/WellClear
// f should be between 0 and 1 to interpolate
Vect3 VectFuns::interpolate(const Vect3& v1, const Vect3& v2, double f) {
	Vect3 dv = v2.Sub(v1);
	return v1.Add(dv.Scal(f));
}
コード例 #7
0
ファイル: VectFuns.cpp プロジェクト: nasa/WellClear
bool VectFuns::collinear(const Vect3& p0, const Vect3& p1, const Vect3& p2) {
	Vect3 v01 = p0.Sub(p1);
	Vect3 v02 = p1.Sub(p2);
	return v01.parallel(v02);
}
コード例 #8
0
ファイル: VectFuns.cpp プロジェクト: nasa/WellClear
bool VectFuns::LoS(const Vect3& so, const Vect3& si, double D, double H) {
	Vect3 s = so.Sub(si);
	return s.x*s.x + s.y*s.y < D*D && std::abs(s.z) < H;
}
コード例 #9
0
ファイル: VectFuns.cpp プロジェクト: nasa/WellClear
double VectFuns::distanceH(const Vect3& soA, const Vect3& soB) {
	return soA.Sub(soB).vect2().norm();
}
コード例 #10
0
ファイル: VectFuns.cpp プロジェクト: nasa/WellClear
bool VectFuns::divergent(const Vect3& so, const Velocity& vo, const Vect3& si, const Velocity& vi) {
	  return so.Sub(si).dot(vo.Sub(vi)) > 0;
}
コード例 #11
-1
bool KinematicIntegerBands::vert_repul_at(double tstep, bool trajdir, int k, const TrafficState& ownship,
    const TrafficState& repac, int epsv) const {
  // repac is not NULL at this point and k >= 0
  if (k==0) {
    return true;
  }
  std::pair<Vect3,Velocity> sovo = trajectory(ownship,0,trajdir);
  Vect3 so = sovo.first;
  Vect3 vo = sovo.second;
  Vect3 si = repac.get_s();
  Vect3 vi = repac.get_v();
  bool rep = true;
  if (k==1) {
    rep = CriteriaCore::vertical_new_repulsive_criterion(so.Sub(si),vo,vi,linvel(ownship,tstep,trajdir,0),epsv);
  }
  if (rep) {
    std::pair<Vect3,Velocity> sovot = trajectory(ownship,k*tstep,trajdir);
    Vect3 sot = sovot.first;
    Vect3 vot = sovot.second;
    Vect3 sit = vi.ScalAdd(k*tstep,si);
    Vect3 st = sot.Sub(sit);
    Vect3 vop = linvel(ownship,tstep,trajdir,k-1);
    Vect3 vok = linvel(ownship,tstep,trajdir,k);
    return CriteriaCore::vertical_new_repulsive_criterion(st,vop,vi,vot,epsv) &&
        CriteriaCore::vertical_new_repulsive_criterion(st,vot,vi,vok,epsv) &&
        CriteriaCore::vertical_new_repulsive_criterion(st,vop,vi,vok,epsv);
  }
  return false;
}
コード例 #12
-1
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);
}