C++ Velocity::vect2方法代码示例

// Assumes 0 <= B < T
LossData WCV_tvar::WCV_interval(const Vect3& so, const Velocity& vo, const Vect3& si, const Velocity& vi, double B, double T) const {
  double time_in = T;
  double time_out = B;

  Vect2 so2 = so.vect2();
  Vect2 si2 = si.vect2();
  Vect2 s2 = so2.Sub(si2);
  Vect2 vo2 = vo.vect2();
  Vect2 vi2 = vi.vect2();
  Vect2 v2 = vo2.Sub(vi2);
  double sz = so.z-si.z;
  double vz = vo.z-vi.z;

  Interval ii = wcv_vertical->vertical_WCV_interval(table.getZTHR(),table.getTCOA(),B,T,sz,vz);

  if (ii.low > ii.up) {
    return LossData(time_in,time_out);
  }
  Vect2 step = v2.ScalAdd(ii.low,s2);
  if (Util::almost_equals(ii.low,ii.up)) { // [CAM] Changed from == to almost_equals to mitigate numerical problems
    if (horizontal_WCV(step,v2)) {
      time_in = ii.low;
      time_out = ii.up;
    }
    return LossData(time_in,time_out);
  }
  LossData ld = horizontal_WCV_interval(ii.up-ii.low,step,v2);
  time_in = ld.getTimeIn() + ii.low;
  time_out = ld.getTimeOut() + ii.low;
  return LossData(time_in,time_out);
}

bool WCV_tvar::violation(const Vect3& so, const Velocity& vo, const Vect3& si, const Velocity& vi) const {
  Vect2 so2 = so.vect2();
  Vect2 si2 = si.vect2();
  Vect2 s2 = so2.Sub(si2);
  Vect2 vo2 = vo.vect2();
  Vect2 vi2 = vi.vect2();
  Vect2 v2 = vo2.Sub(vi2);
  return horizontal_WCV(s2,v2) &&
      wcv_vertical->vertical_WCV(table.getZTHR(),table.getTCOA(),so.z-si.z,vo.z-vi.z);
}

/**
 * Computes 2D intersection point of two lines, but also finds z component (projected by time from line 1)
 * @param s0 starting point of line 1
 * @param v0 direction vector for line 1
 * @param s1 starting point of line 2
 * @param v1 direction vector of line 2
 * @return Pair (2-dimensional point of intersection with 3D projection, relative time of intersection, relative to the so3)
 * If the lines are parallel, this returns the pair (0,NaN).
 */
double  VectFuns::timeOfIntersection(const Vect3& so3, const Velocity& vo3, const Vect3& si3, const Velocity& vi3) {
	Vect2 so = so3.vect2();
	Vect2 vo = vo3.vect2();
	Vect2 si = si3.vect2();
	Vect2 vi = vi3.vect2();
	Vect2 ds = si.Sub(so);
	if (vo.det(vi) == 0) {
		//f.pln(" $$$ intersection: lines are parallel");
		return NaN;
	}
	double tt = ds.det(vi)/vo.det(vi);
	//f.pln(" $$$ intersection: tt = "+tt);
	return tt;
}

/**
 * Computes 2D intersection point of two lines, but also finds z component (projected by time from line 1)
 * @param s0 starting point of line 1
 * @param v0 direction vector for line 1
 * @param s1 starting point of line 2
 * @param v1 direction vector of line 2
 * @return Pair (2-dimensional point of intersection with 3D projection, relative time of intersection, relative to the so3)
 * If the lines are parallel, this returns the pair (0,NaN).
 */
std::pair<Vect3,double> VectFuns::intersection(const Vect3& so3, const Velocity& vo3, const Vect3& si3, const Velocity& vi3) {
	Vect2 so = so3.vect2();
	Vect2 vo = vo3.vect2();
	Vect2 si = si3.vect2();
	Vect2 vi = vi3.vect2();
	Vect2 ds = si.Sub(so);
	if (vo.det(vi) == 0) {
		//f.pln(" $$$ intersection: lines are parallel");
		return std::pair<Vect3,double>(Vect3::ZERO(),  NaN);
	}
	double tt = ds.det(vi)/vo.det(vi);
	//f.pln(" $$$ intersection: tt = "+tt);
	Vect3 intersec = so3.Add(vo3.Scal(tt));
	double nZ = intersec.z;
	double maxZ = Util::max(so3.z,si3.z);
	double minZ = Util::min(so3.z,si3.z);
	if (nZ > maxZ) nZ = maxZ;
	if (nZ < minZ) nZ = minZ;
	return std::pair<Vect3,double>(intersec.mkZ(nZ),tt);
}

/**
 * Returns values indicating whether the ownship state will pass in front of or behind the intruder (from a horizontal perspective)
 * @param so ownship position
 * @param vo ownship velocity
 * @param si intruder position
 * @param vi intruder velocity
 * @return 1 if ownship will pass in front (or collide, from a horizontal sense), -1 if ownship will pass behind, 0 if divergent or parallel
 */
int VectFuns::passingDirection(const Vect3& so, const Velocity& vo, const Vect3& si, const Velocity& vi) {
	double toi = timeOfIntersection(so,vo,si,vi);
	double tii = timeOfIntersection(si,vi,so,vo); // these values may have opposite sign!
//fpln("toi="+toi);
//fpln("int = "+	intersection(so,vo,si,vi));
	if (ISNAN(toi) || toi < 0 || tii < 0) return 0;
	Vect3 so3 = so.linear(vo, toi);
	Vect3 si3 = si.linear(vi, toi);
//fpln("so3="+so3);
//fpln("si3="+si3);
	if (behind(so3.vect2(), si3.vect2(), vi.vect2())) return -1;
	return 1;
}

int VectFuns::dirForBehind(const Vect3& so, const Velocity& vo, const Vect3& si, const Velocity& vi) {
     return dirForBehind(so.vect2(),vo.vect2(),si.vect2(),vi.vect2());
}