Fixed computation of hazard volumes (for display only)

C++/RELEASE-NOTES.txt

@@ -10,6 +10,10 @@ Rights Reserved.
 
 Release DAIDALUS-v2.0.1:
 ------------------
+* December 11, 2020
+- Fixed computation of TCPA hazard volume when TTHR*||v|| < DTHR and
+other volumes when relative velocity is zero (these fixes are only for display)
+
 * October 9, 2020
 - Added performance metrics methods to Daidalus class
 - Fixed bug in Daidalus class, where numberOfAircraft was used instead

C++/include/Horizontal.h

@@ -220,10 +220,10 @@ class Horizontal: public Vect2 {
   static Horizontal opt_vertical(const Vect3& s,const Vect3& vo,const Vect3& vi,const TangentLine& l,
       const int epsv,const double D,const double H);
 
-  /* Point in relative coordinates that is tangent to the circle of radius D, around
-   * so, in the direction of the relative velocity vo-vi.
+  /*
+   * Unit left perpendicular vector to v
    */
-  static Vect3 hmd_tangent_point(double D, const Vect3& v);
+  static Vect3 unit_perpL(const Vect3& v);
 
 };
 

C++/include/WCV_TAUMOD.h

@@ -82,7 +82,7 @@ class WCV_TAUMOD : public WCV_tvar {
   static double TAU_radius(const Velocity& v, double DTHR, double TTHR);
 
   virtual void hazard_zone_far_end(std::vector<Position>& haz,
-      const Position& po, const Velocity& v, const Velocity& vD, double T) const;
+      const Position& po, const Velocity& v, const Vect3& pu, double T) const;
 
 };
 

C++/include/WCV_TCPA.h

@@ -50,7 +50,7 @@ class WCV_TCPA : public WCV_tvar {
   bool contains(const Detection3D* cd) const;
 
   virtual void hazard_zone_far_end(std::vector<Position>& haz,
-      const Position& po, const Velocity& v, const Velocity& vD, double T) const;
+      const Position& po, const Velocity& v, const Vect3& pu, double T) const;
 
 };
 }

C++/include/WCV_TEP.h

@@ -50,7 +50,7 @@ class WCV_TEP : public WCV_tvar {
   bool contains(const Detection3D* cd) const;
 
   virtual void hazard_zone_far_end(std::vector<Position>& haz,
-      const Position& po, const Velocity& v, const Velocity& vD, double T) const;
+      const Position& po, const Velocity& v, const Vect3& pu, double T) const;
 
 };
 }

C++/include/WCV_tvar.h

@@ -100,7 +100,7 @@ class WCV_tvar : public Detection3D {
       double T) const;
 
   virtual void hazard_zone_far_end(std::vector<Position>& haz,
-      const Position& po, const Velocity& v, const Velocity& vD, double T) const {}
+      const Position& po, const Velocity& v, const Vect3& pu, double T) const {}
 
   virtual bool equals(Detection3D* o) const;
 

C++/src/CDCylinder.cpp

@@ -252,12 +252,12 @@ void CDCylinder::horizontalHazardZone(std::vector<Position>& haz,
   haz.clear();
   const Position& po = ownship.getPosition();
   Velocity v = Velocity::make(ownship.getVelocity().Sub(intruder.getVelocity()));
-  Vect3 sD = Horizontal::hmd_tangent_point(D_,v);
-  Velocity vD = Velocity::make(sD);
-  Velocity vnD = Velocity::make(sD.Neg());
-  if (T == 0) {
-    circular_arc(haz,po,vD,2*Pi,false);
+  if (Util::almost_equals(T,0) || Util::almost_equals(v.norm2D(),0)) {
+    circular_arc(haz,po,Velocity::mkVxyz(D_,0,0),2*Pi,false);
   } else {
+    Vect3 sD = Horizontal::unit_perpL(v).Scal(D_);
+    Velocity vD = Velocity::make(sD);
+    Velocity vnD = Velocity::make(sD.Neg());
     circular_arc(haz,po,vD,Pi,true);
     circular_arc(haz,po.linear(v,T),vnD,Pi,true);
   }

C++/src/Horizontal.cpp

@@ -369,11 +369,11 @@ Horizontal Horizontal::opt_vertical(const Vect3& s,const Vect3& vo,const Vect3&
   return NoHorizontalSolution();
 }
 
-/* Point in relative coordinates that is tangent to the circle of radius D, around
- * so, in the direction of the relative velocity vo-vi.
+/*
+ * Unit left perpendicular vector to v
  */
-Vect3 Horizontal::hmd_tangent_point(double D, const Vect3& v) {
-  return v.Hat2D().PerpL().Scal(D);
+Vect3 Horizontal::unit_perpL(const Vect3& v) {
+  return v.Hat2D().PerpL();
 }
 
 

C++/src/TCAS3D.cpp

@@ -452,40 +452,40 @@ void TCAS3D::horizontalHazardZone(std::vector<Position>& haz, const TrafficState
   haz.clear();
   Position po = ownship.getPosition();
   Velocity v = Velocity::make(ownship.getVelocity().Sub(intruder.getVelocity()));
-  Vect3 sD = Horizontal::hmd_tangent_point(DMOD,v);
-  Velocity vD = Velocity::make(sD);
-  if (TAUMOD+T == 0) {
-      CDCylinder::circular_arc(haz,po,vD,2*Pi,false);
+  if (Util::almost_equals(TAUMOD+T,0) || Util::almost_equals(v.norm2D(),0)) {
+    CDCylinder::circular_arc(haz,po,Velocity::mkVxyz(DMOD,0,0),2*Pi,false);
   } else {
-      CDCylinder::circular_arc(haz,po,vD,Pi,usehmdf);
-      Position TAU_center = WCV_TAUMOD::TAU_center(po,v,TAUMOD,T);
-      Vect3 vC = v.Scal(0.5*TAUMOD);     // TAUMOD Center (relative)
-      if (usehmdf) {
-          Vect3 vDC = vC.Sub(vD); // Far end point opposite to -vD (vC-relative);
-          Vect3 nvDC = vC.Add(vD); // Far end point opposite to vD (vC-relative);
-          double sqa = vDC.sqv2D();
-          double alpha = Util::atan2_safe(vDC.det2D(nvDC)/sqa,vDC.dot2D(nvDC)/sqa);
-          Velocity velDC = Velocity::make(vDC);
-          CDCylinder::circular_arc(haz,TAU_center,velDC,alpha,true);
-      } else {
-          Vect3 nsCD=sD.Neg().Sub(vC);
-          Vect3 sCD=sD.Sub(vC);
-          double sqa = sCD.sqv2D();
-          Velocity nvCD = Velocity::make(nsCD);
-          if (T==0) { // Two circles: DMOD and TAUMO. They intersect at +/-vD
-              double alpha = Util::atan2_safe(nsCD.det2D(sCD)/sqa,nsCD.dot2D(sCD)/sqa);
-              CDCylinder::circular_arc(haz,TAU_center,nvCD,alpha,false);
-          } else { // Two circles: DMOD and TAUMOD. They intersect at +/- vD.
-              Vect3 sT = Horizontal::hmd_tangent_point(std::sqrt(sqa),v);
-              Velocity vT = Velocity::make(sT);
-              Vect3 nsT = sT.Neg();
-              Velocity nvT = Velocity::make(nsT);
-              double alpha = Util::atan2_safe(nsCD.det2D(nsT)/sqa,nsCD.dot2D(nsT)/sqa);
-              Position TAU_center_0 = WCV_TAUMOD::TAU_center(po,v,TAUMOD,0);
-              CDCylinder::circular_arc(haz,TAU_center_0,nvCD,alpha,true);
-              CDCylinder::circular_arc(haz,TAU_center,nvT,Pi,true);
-              CDCylinder::circular_arc(haz,TAU_center_0,vT,alpha,false);                }
-      }
+    Vect3 sD = Horizontal::unit_perpL(v).Scal(DMOD);
+    Velocity vD = Velocity::make(sD);
+    CDCylinder::circular_arc(haz,po,vD,Pi,usehmdf);
+    Position TAU_center = WCV_TAUMOD::TAU_center(po,v,TAUMOD,T);
+    Vect3 vC = v.Scal(0.5*TAUMOD);     // TAUMOD Center (relative)
+    if (usehmdf) {
+      Vect3 vDC = vC.Sub(vD); // Far end point opposite to -vD (vC-relative);
+      Vect3 nvDC = vC.Add(vD); // Far end point opposite to vD (vC-relative);
+      double sqa = vDC.sqv2D();
+      double alpha = Util::atan2_safe(vDC.det2D(nvDC)/sqa,vDC.dot2D(nvDC)/sqa);
+      Velocity velDC = Velocity::make(vDC);
+      CDCylinder::circular_arc(haz,TAU_center,velDC,alpha,true);
+    } else {
+      Vect3 nsCD=sD.Neg().Sub(vC);
+      Vect3 sCD=sD.Sub(vC);
+      double sqa = sCD.sqv2D();
+      Velocity nvCD = Velocity::make(nsCD);
+      if (Util::almost_equals(T,0)) { // Two circles: DMOD and TAUMO. They intersect at +/-vD
+        double alpha = Util::atan2_safe(nsCD.det2D(sCD)/sqa,nsCD.dot2D(sCD)/sqa);
+        CDCylinder::circular_arc(haz,TAU_center,nvCD,alpha,false);
+      } else { // Two circles: DMOD and TAUMOD. They intersect at +/- vD.
+        Vect3 sT = Horizontal::unit_perpL(v).Scal(std::sqrt(sqa));
+        Velocity vT = Velocity::make(sT);
+        Vect3 nsT = sT.Neg();
+        Velocity nvT = Velocity::make(nsT);
+        double alpha = Util::atan2_safe(nsCD.det2D(nsT)/sqa,nsCD.dot2D(nsT)/sqa);
+        Position TAU_center_0 = WCV_TAUMOD::TAU_center(po,v,TAUMOD,0);
+        CDCylinder::circular_arc(haz,TAU_center_0,nvCD,alpha,true);
+        CDCylinder::circular_arc(haz,TAU_center,nvT,Pi,true);
+        CDCylinder::circular_arc(haz,TAU_center_0,vT,alpha,false);                }
+    }
   }
 }
 

C++/src/WCV_TAUMOD.cpp

@@ -160,7 +160,8 @@ double WCV_TAUMOD::TAU_radius(const Velocity& v, double DTHR, double TTHR) {
 }
 
 void WCV_TAUMOD::hazard_zone_far_end(std::vector<Position>& haz,
-    const Position& po, const Velocity& v, const Velocity& vD, double T) const {
+    const Position& po, const Velocity& v, const Vect3& pu, double T) const {
+  Vect3 vD = pu.Scal(getDTHR());
   Vect3 vC = v.Scal(0.5*getTTHR());     // TAUMOD Center (relative)
   Vect3 vDC = vC.Sub(vD); // Far end point opposite to -vD (vC-relative);
   Vect3 nvDC = vC.Add(vD); // Far end point opposite to vD (vC-relative);

C++/src/WCV_TCPA.cpp

@@ -5,6 +5,7 @@
  * Rights Reserved.
  */
 
+#include "CDCylinder.h"
 #include "WCV_TCPA.h"
 #include "WCV_TCOA.h"
 #include "Vect3.h"
@@ -113,10 +114,20 @@ bool WCV_TCPA::contains(const Detection3D* cd) const {
 }
 
 void WCV_TCPA::hazard_zone_far_end(std::vector<Position>& haz,
-    const Position& po, const Velocity& v, const Velocity& vD, double T) const {
+    const Position& po, const Velocity& v, const Vect3& pu, double T) const {
   Position npo = po.linear(v,getTTHR()+T);
-  haz.push_back(npo.linear(vD,-1));
-  haz.push_back(npo.linear(vD,1));
+  Velocity vu = Velocity::make(pu);
+  haz.push_back(npo.linear(vu,-getDTHR()));
+  double b = v.norm2D()*getTTHR();
+  if (Util::almost_greater(getDTHR(),b)) {
+      // Far end has the form of a cap
+      double a = Util::sqrt_safe(Util::sq(getDTHR())-Util::sq(b));
+      double alpha = Util::acos_safe(b/getDTHR());
+      Vect3 vD = pu.ScalAdd(-a,v.Hat().Scal(b));
+      CDCylinder::circular_arc(haz,po.linear(v,T),
+              Velocity::make(vD),2*alpha,true);
+  }
+  haz.push_back(npo.linear(vu,getDTHR()));
 }
 
 

C++/src/WCV_TEP.cpp

@@ -107,8 +107,9 @@ bool WCV_TEP::contains(const Detection3D* cd) const {
 }
 
 void WCV_TEP::hazard_zone_far_end(std::vector<Position>& haz,
-    const Position& po, const Velocity& v, const Velocity& vD, double T) const {
-  CDCylinder::circular_arc(haz,po.linear(v,getTTHR()+T),Velocity::make(vD.Neg()),Pi,true);
+    const Position& po, const Velocity& v, const Vect3& pu, double T) const {
+  CDCylinder::circular_arc(haz,po.linear(v,getTTHR()+T),
+      Velocity::make(pu.Scal(-getDTHR())),Pi,true);
 }
 
 }

C++/src/WCV_tvar.cpp

@@ -199,13 +199,13 @@ void WCV_tvar::horizontalHazardZone(std::vector<Position>& haz, const TrafficSta
   haz.clear();
   const Position& po = ownship.getPosition();
   Velocity v = Velocity::make(ownship.getVelocity().Sub(intruder.getVelocity()));
-  Vect3 sD = Horizontal::hmd_tangent_point(getDTHR(),v);
-  Velocity vD = Velocity::make(sD);
-  if (getTTHR()+T == 0) {
-      CDCylinder::circular_arc(haz,po,vD,2*Pi,false);
+  if (Util::almost_equals(getTTHR()+T,0) || Util::almost_equals(v.norm2D(),0)) {
+    CDCylinder::circular_arc(haz,po,Velocity::mkVxyz(getDTHR(),0,0),2*Pi,false);
   } else {
-      CDCylinder::circular_arc(haz,po,vD,Pi,true);
-      hazard_zone_far_end(haz,po,v,vD,T);
+    Vect3 pu = Horizontal::unit_perpL(v);
+    Velocity vD = Velocity::make(pu.Scal(getDTHR()));
+    CDCylinder::circular_arc(haz,po,vD,Pi,true);
+    hazard_zone_far_end(haz,po,v,pu,T);
   }
 }
 

Java/RELEASE-NOTES.txt

@@ -10,6 +10,10 @@ Rights Reserved.
 
 Release DAIDALUS-v2.0.1:
 ------------------
+* December 11, 2020
+- Fixed computation of TCPA hazard volume when TTHR*||v|| < DTHR and
+other volumes when relative velocity is zero (these fixes are only for display)
+
 * October 9, 2020
 - Added performance metrics methods to Daidalus class
 - Fixed bug in Daidalus class, where numberOfAircraft was used instead

Java/src/gov/nasa/larcfm/ACCoRD/CDCylinder.java

@@ -387,11 +387,11 @@ public void horizontalHazardZone(List<Position> haz,
 		haz.clear();
 		Position po = ownship.getPosition();
 		Velocity v = ownship.getVelocity().Sub(intruder.getVelocity());
-		Vect3 sD = Horizontal.hmd_tangent_point(D_,v);
-		Velocity vD = Velocity.make(sD);
-		if (T == 0) {
-			circular_arc(haz,po,vD,2*Math.PI,false);
+		if (Util.almost_equals(T,0) || Util.almost_equals(v.norm2D(),0)) {
+			circular_arc(haz,po,Velocity.mkVxyz(D_,0,0),2*Math.PI,false);
 		} else {
+			Vect3 sD = Horizontal.unit_perpL(v).Scal(D_);
+			Velocity vD = Velocity.make(sD);
 			circular_arc(haz,po,vD,Math.PI,true);	
 			circular_arc(haz,po.linear(v,T),vD.Neg(),Math.PI,true);
 		}

Java/src/gov/nasa/larcfm/ACCoRD/Horizontal.java

@@ -460,11 +460,11 @@ public static double epsilonCriticalPointGS(Vect2 s, Vect2 vo, Vect2 vi) {
 		return nvo.norm();
 	}
 
-	/* Point in relative coordinates that is tangent to the circle of radius D, around
-	 * so, in the direction of the relative velocity vo-vi. 
+	/* 
+	 * Unit left perpendicular vector to v
 	 */
-	public static Vect3 hmd_tangent_point(double D, Vect3 v) {
-		return v.Hat2D().PerpL().Scal(D);
+	public static Vect3 unit_perpL(Vect3 v) {
+		return v.Hat2D().PerpL();
 	}
 
 	public String toString() {

Java/src/gov/nasa/larcfm/ACCoRD/TCAS3D.java

@@ -457,11 +457,11 @@ public void horizontalHazardZone(List<Position>haz, TrafficState ownship, Traffi
 		haz.clear();
 		Position po = ownship.getPosition();
 		Velocity v = ownship.getVelocity().Sub(intruder.getVelocity());
-		Vect3 sD = Horizontal.hmd_tangent_point(DMOD,v);
-		Velocity vD = Velocity.make(sD);
-		if (TAUMOD+T == 0) {
-			CDCylinder.circular_arc(haz,po,vD,2*Math.PI,false);
+		if (Util.almost_equals(TAUMOD+T,0) || Util.almost_equals(v.norm2D(),0)) {
+			CDCylinder.circular_arc(haz,po,Velocity.mkVxyz(DMOD,0,0),2*Math.PI,false);
 		} else {
+			Vect3 sD = Horizontal.unit_perpL(v).Scal(DMOD);
+			Velocity vD = Velocity.make(sD);
 			CDCylinder.circular_arc(haz,po,vD,Math.PI,usehmdf);	
 			Position TAU_center = WCV_TAUMOD.TAU_center(po,v,TAUMOD,T);
 			Vect3 vC = v.Scal(0.5*TAUMOD);     // TAUMOD Center (relative)
@@ -477,11 +477,11 @@ public void horizontalHazardZone(List<Position>haz, TrafficState ownship, Traffi
 				Vect3 sCD=sD.Sub(vC);
 				double sqa = sCD.sqv2D();
 				Velocity nvCD = Velocity.make(nsCD);
-				if (T==0) { // Two circles: DMOD and TAUMO. They intersect at +/-vD
+				if (Util.almost_equals(T,0)) { // Two circles: DMOD and TAUMO. They intersect at +/-vD
 					double alpha = Util.atan2_safe(nsCD.det2D(sCD)/sqa,nsCD.dot2D(sCD)/sqa);	
 					CDCylinder.circular_arc(haz,TAU_center,nvCD,alpha,false);		
 				} else { // Two circles: DMOD and TAUMOD. They intersect at +/- vD. 
-					Vect3 sT = Horizontal.hmd_tangent_point(Math.sqrt(sqa),v);
+					Vect3 sT = Horizontal.unit_perpL(v).Scal(Math.sqrt(sqa));
 					Velocity vT = Velocity.make(sT);
 					Vect3 nsT = sT.Neg();
 					Velocity nvT = Velocity.make(nsT);

Java/src/gov/nasa/larcfm/ACCoRD/WCV_TAUMOD.java

@@ -144,7 +144,8 @@ public static double TAU_radius(Velocity v, double DTHR, double TTHR) {
 	}
 
 	public void hazard_zone_far_end(List<Position> haz,
-			Position po, Velocity v, Velocity vD, double T) {
+			Position po, Velocity v, Vect3 pu, double T) {
+		Vect3 vD = pu.Scal(getDTHR());
 		Vect3 vC = v.Scal(0.5*getTTHR());     // TAUMOD Center (relative)
 		Vect3 vDC = vC.Sub(vD); // Far end point opposite to -vD (vC-relative);
 		Vect3 nvDC = vC.Add(vD); // Far end point opposite to vD (vC-relative);

Java/src/gov/nasa/larcfm/ACCoRD/WCV_TCPA.java

@@ -97,12 +97,22 @@ public boolean contains(Detection3D cd) {
 		}
 		return false;
 	}
-	
+
 	public void hazard_zone_far_end(List<Position> haz,
-			Position po, Velocity v, Velocity vD, double T) {
-		Position npo = po.linear(v,getTTHR()+T);		
-		haz.add(npo.linear(vD,-1));
-		haz.add(npo.linear(vD,1));
+			Position po, Velocity v, Vect3 pu, double T) {
+		Position npo = po.linear(v,getTTHR()+T);
+		Velocity vu = Velocity.make(pu);
+		haz.add(npo.linear(vu,-getDTHR()));
+		double b = v.norm2D()*getTTHR();
+		if (Util.almost_greater(getDTHR(),b)) {
+			// Far end has the form of a cap
+			double a = Util.sqrt_safe(Util.sq(getDTHR())-Util.sq(b));
+			double alpha = Util.acos_safe(b/getDTHR());
+			Vect3 vD = pu.ScalAdd(-a,v.Hat().Scal(b));
+			CDCylinder.circular_arc(haz,po.linear(v,T),
+					Velocity.make(vD),2*alpha,true);
+		}
+		haz.add(npo.linear(vu,getDTHR()));
 	}
 
 }

Java/src/gov/nasa/larcfm/ACCoRD/WCV_TEP.java

@@ -90,10 +90,11 @@ public boolean contains(Detection3D cd) {
 		}
 		return false;
 	}
-	
+
 	public void hazard_zone_far_end(List<Position> haz,
-			Position po, Velocity v, Velocity vD, double T) {
-		CDCylinder.circular_arc(haz,po.linear(v,getTTHR()+T),vD.Neg(),Math.PI,true);
+			Position po, Velocity v, Vect3 pu, double T) {
+		CDCylinder.circular_arc(haz,po.linear(v,getTTHR()+T),
+				Velocity.make(pu.Scal(-getDTHR())),Math.PI,true);
 	}
 
 }

Java/src/gov/nasa/larcfm/ACCoRD/WCV_tvar.java

@@ -207,18 +207,18 @@ public void horizontalHazardZone(List<Position> haz,
 		haz.clear();
 		Position po = ownship.getPosition();
 		Velocity v = ownship.getVelocity().Sub(intruder.getVelocity());
-		Vect3 sD = Horizontal.hmd_tangent_point(getDTHR(),v);
-		Velocity vD = Velocity.make(sD);
-		if (getTTHR()+T == 0) {
-			CDCylinder.circular_arc(haz,po,vD,2*Math.PI,false);
+		if (Util.almost_equals(getTTHR()+T,0) || Util.almost_equals(v.norm2D(),0)) {
+			CDCylinder.circular_arc(haz,po,Velocity.mkVxyz(getDTHR(),0,0),2*Math.PI,false);
 		} else {
+			Vect3 pu = Horizontal.unit_perpL(v);
+			Velocity vD = Velocity.make(pu.Scal(getDTHR()));
 			CDCylinder.circular_arc(haz,po,vD,Math.PI,true);	
-			hazard_zone_far_end(haz,po,v,vD,T);
+			hazard_zone_far_end(haz,po,v,pu,T);
 		}
 	}
 
 	public void hazard_zone_far_end(List<Position> haz,
-			Position po, Velocity v, Velocity vD, double T) {}
+			Position po, Velocity v, Vect3 pu, double T) {}
 
 	@Override
 	public boolean equals(Object obj) {