DEV: Merge from default.

--HG-- branch : stable
USNavalResearchLaboratory · Jun 10, 2020 · f9fe10f · f9fe10f
2 parents d05b8d6 + 361941f
commit f9fe10f
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 16 deletions.
diff --git a/SDK/simCore/Calc/CoordinateConverter.cpp b/SDK/simCore/Calc/CoordinateConverter.cpp
@@ -908,7 +908,7 @@ int CoordinateConverter::convert(const Coordinate &inCoord, Coordinate &outCoord
 
 /// convert geodetic projection (LLA) to flat earth projection (NED/NWU/ENU)
 ///@pre flatCoord valid, ref origin set, in coord is LLA, system is NED/NWU/ENU, llaCoord does not alias flatCoord
-int CoordinateConverter::convertGeodeticToFlat_(const Coordinate &llaCoord, Coordinate &flatCoord, CoordinateSystem system) const
+int CoordinateConverter::convertGeodeticToFlat_(const Coordinate& llaCoord, Coordinate& flatCoord, CoordinateSystem system) const
 {
   // Test for same input/output -- this function cannot handle case of llaCoord == flatCoord
   if (&llaCoord == &flatCoord)
@@ -953,14 +953,14 @@ int CoordinateConverter::convertGeodeticToFlat_(const Coordinate &llaCoord, Coor
   // input lat and lon in radians, alt in meters, output values in meters
   if (system == COORD_SYS_NED)
   {
-    Vec3 llaPos(llaCoord.position());
-    //  (North East Down system)
+    const Vec3& llaPos(llaCoord.position());
+    // meters (North East Down system)
     // +X is North, Latitude is North-South
-    double x = angFixPI2(llaPos.lat() - referenceOrigin_.lat()) * latRadius_;
+    const double x = angFixPI(llaPos.lat() - referenceOrigin_.lat()) * latRadius_;
     // +Y is East, Longitude is East-West
-    double y = angFixPI(llaPos.lon() - referenceOrigin_.lon()) * lonRadius_;
+    const double y = angFixPI(llaPos.lon() - referenceOrigin_.lon()) * lonRadius_;
     // +Z is down, Altitude (+Z) is up
-    double z = -(llaPos.alt() - referenceOrigin_.alt());
+    const double z = -(llaPos.alt() - referenceOrigin_.alt());
 
     flatCoord.setPosition(x, y, z);
 
@@ -982,14 +982,14 @@ int CoordinateConverter::convertGeodeticToFlat_(const Coordinate &llaCoord, Coor
   }
   else if (system == COORD_SYS_ENU)
   {
-    Vec3 llaPos(llaCoord.position());
+    const Vec3& llaPos(llaCoord.position());
     // meters (East North Up system)
     // +X is East, Longitude is East-West
-    double x = angFixPI(llaPos.lon() - referenceOrigin_.lon()) * lonRadius_;
+    const double x = angFixPI(llaPos.lon() - referenceOrigin_.lon()) * lonRadius_;
     // +Y is North, Latitude is North-South
-    double y = angFixPI2(llaPos.lat() - referenceOrigin_.lat()) * latRadius_;
+    const double y = angFixPI(llaPos.lat() - referenceOrigin_.lat()) * latRadius_;
     // +Z is up, Altitude (+Z) is up
-    double z =  llaPos.alt() - referenceOrigin_.alt();
+    const double z = llaPos.alt() - referenceOrigin_.alt();
 
     flatCoord.setPosition(x, y, z);
 
@@ -1006,14 +1006,14 @@ int CoordinateConverter::convertGeodeticToFlat_(const Coordinate &llaCoord, Coor
   }
   else if (system == COORD_SYS_NWU)
   {
-    Vec3 llaPos(llaCoord.position());
+    const Vec3& llaPos(llaCoord.position());
     // meters (North West Up system)
     // +X is North, Latitude is North-South
-    double x =  angFixPI2(llaPos.lat() - referenceOrigin_.lat()) * latRadius_;
+    const double x = angFixPI(llaPos.lat() - referenceOrigin_.lat()) * latRadius_;
     // +Y is West, Longitude is East-West
-    double y = -angFixPI(llaPos.lon() - referenceOrigin_.lon()) * lonRadius_;
+    const double y = -angFixPI(llaPos.lon() - referenceOrigin_.lon()) * lonRadius_;
     // +Z is up, Altitude (+Z) is up
-    double z =   llaPos.alt() - referenceOrigin_.alt();
+    const double z = llaPos.alt() - referenceOrigin_.alt();
 
     flatCoord.setPosition(x, y, z);
 
@@ -1041,7 +1041,7 @@ int CoordinateConverter::convertGeodeticToFlat_(const Coordinate &llaCoord, Coor
 ///@param[out] llaCoord output
 ///@return 0 on success, !0 on failure
 ///@pre llaCoord valid, ref origin calculated, in coord is NED/NWU/ENU, llaCoord does not alias flatCoord
-int CoordinateConverter::convertFlatToGeodetic_(const Coordinate &flatCoord, Coordinate &llaCoord) const
+int CoordinateConverter::convertFlatToGeodetic_(const Coordinate& flatCoord, Coordinate& llaCoord) const
 {
   // Test for same input/output -- this function cannot handle case of llaCoord == flatCoord
   if (&llaCoord == &flatCoord)

diff --git a/Testing/SimCore/CoordConvertLibTest.cpp b/Testing/SimCore/CoordConvertLibTest.cpp
@@ -886,7 +886,7 @@ int testScaledFlatEarth()
   int rv = 0;
   simCore::CoordinateConverter cc;
   cc.setReferenceOriginDegrees(22.119439197, -159.91949881, 0.);
-  simCore::Coordinate ecefPos;
+  simCore::Coordinate enuPos;
   simCore::Coordinate llaPos;
   simCore::Coordinate sfePos;
 
@@ -966,6 +966,21 @@ int testScaledFlatEarth()
   rv += ccAqm.convert(llaPos, sfePos, simCore::COORD_SYS_NWU);
   rv += SDK_ASSERT(almostEqualCoord(posa, sfePos));
 
+
+  // SIM-11596, data fabricated to demonstrate issue
+  simCore::CoordinateConverter ccsim11596;
+  ccsim11596.setReferenceOriginDegrees(45, 161, 0.);
+  const simCore::Coordinate sim11596(simCore::COORD_SYS_LLA, simCore::Vec3(-45.1 * simCore::DEG2RAD, -162. * simCore::DEG2RAD, 100.));
+  // Validate conversion from and back to LLA works
+  rv += ccsim11596.convert(sim11596, enuPos, simCore::COORD_SYS_ENU);
+  rv += ccsim11596.convert(enuPos, sfePos, simCore::COORD_SYS_LLA);
+
+  // position-only comparison
+  rv += SDK_ASSERT(simCore::areAnglesEqual(sim11596.lat(), sfePos.lat()));
+  rv += SDK_ASSERT(simCore::areAnglesEqual(sim11596.lon(), sfePos.lon()));
+  rv += SDK_ASSERT(simCore::areEqual(sim11596.alt(), sfePos.alt()));
+
+
   std::cout << std::endl << "Scaled Flat Earth test case: ";
   std::cout << (rv==0 ? "PASSED" : "FAILED") << std::endl;
   return rv;