Skip to content
This repository has been archived by the owner on Feb 3, 2025. It is now read-only.

SphericalCoordinates: support Lunar calculations #3250

Merged
merged 6 commits into from
Sep 3, 2022
Merged

SphericalCoordinates: support Lunar calculations #3250

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
c08edc0
Added support for MOON_SCS in spherical coords
adityapande-1995 Aug 9, 2022
0b84f61
Minor refactoring
adityapande-1995 Aug 16, 2022
9bb78fc
Updated tests
adityapande-1995 Aug 16, 2022
1ba9fea
Minor bugfix
adityapande-1995 Aug 17, 2022
2793687
Use SurfaceRadius method in distance calculation
adityapande-1995 Sep 1, 2022
e1109c1
Fixed double free error in test
adityapande-1995 Sep 2, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
93 changes: 93 additions & 0 deletions gazebo/common/SphericalCoordinates.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,12 +39,30 @@ const double g_EarthWGS84Flattening = 1.0/298.257223563;
// Radius of the Earth (meters).
const double g_EarthRadius = 6371000.0;

// Radius of the Moon (meters).
// Source: https://lunar.gsfc.nasa.gov/library/451-SCI-000958.pdf
const double g_MoonRadius = 1737400.0;

// a: Equatorial radius of the Moon.
// Source : https://nssdc.gsfc.nasa.gov/planetary/factsheet/moonfact.html
const double g_MoonAxisEquatorial = 1738100.0;

// b: Polar radius of the Moon.
// Source : https://nssdc.gsfc.nasa.gov/planetary/factsheet/moonfact.html
const double g_MoonAxisPolar = 1736000.0;

// if: Unitless flattening parameter for the Moon.
// Source : https://nssdc.gsfc.nasa.gov/planetary/factsheet/moonfact.html
const double g_MoonFlattening = 0.0012;

//////////////////////////////////////////////////
SphericalCoordinates::SurfaceType SphericalCoordinates::Convert(
const std::string &_str)
{
if ("EARTH_WGS84" == _str)
return EARTH_WGS84;
else if ("MOON_SCS" == _str)
return MOON_SCS;

gzerr << "SurfaceType string not recognized, "
<< "EARTH_WGS84 returned by default" << std::endl;
Expand Down Expand Up @@ -143,6 +161,30 @@ void SphericalCoordinates::SetSurfaceType(const SurfaceType &_type)
// Set the flattening parameter
this->dataPtr->ellF = g_EarthWGS84Flattening;

// Set the first eccentricity ellipse parameter
// https://en.wikipedia.org/wiki/Eccentricity_(mathematics)#Ellipses
this->dataPtr->ellE = sqrt(1.0 -
std::pow(this->dataPtr->ellB, 2) / std::pow(this->dataPtr->ellA, 2));

// Set the second eccentricity ellipse parameter
// https://en.wikipedia.org/wiki/Eccentricity_(mathematics)#Ellipses
this->dataPtr->ellP = sqrt(
std::pow(this->dataPtr->ellA, 2) / std::pow(this->dataPtr->ellB, 2) -
1.0);

break;
}
case MOON_SCS:
{
// Set the semi-major axis
this->dataPtr->ellA = g_MoonAxisEquatorial;

// Set the semi-minor axis
this->dataPtr->ellB = g_MoonAxisPolar;

// Set the flattening parameter
this->dataPtr->ellF = g_MoonFlattening;

// Set the first eccentricity ellipse parameter
// https://en.wikipedia.org/wiki/Eccentricity_(mathematics)#Ellipses
this->dataPtr->ellE = sqrt(1.0 -
Expand Down Expand Up @@ -246,6 +288,57 @@ double SphericalCoordinates::Distance(const ignition::math::Angle &_latA,
return d;
}

//////////////////////////////////////////////////
/// Based on Haversine formula (http://en.wikipedia.org/wiki/Haversine_formula).
/// This takes into account the surface type.
double SphericalCoordinates::DistanceBetweenPoints(
const ignition::math::Angle &_latA,
const ignition::math::Angle &_lonA,
const ignition::math::Angle &_latB,
const ignition::math::Angle &_lonB)
{
ignition::math::Angle dLat = _latB - _latA;
ignition::math::Angle dLon = _lonB - _lonA;

double a = sin(dLat.Radian() / 2) * sin(dLat.Radian() / 2) +
sin(dLon.Radian() / 2) * sin(dLon.Radian() / 2) *
cos(_latA.Radian()) * cos(_latB.Radian());

double c = 2 * atan2(sqrt(a), sqrt(1 - a));
if (this->dataPtr->surfaceType == MOON_SCS)
return g_MoonRadius * c;
else
return g_EarthRadius * c;
}

//////////////////////////////////////////////////
double SphericalCoordinates::SurfaceRadius() const
{
if (this->dataPtr->surfaceType ==
SphericalCoordinates::SurfaceType::MOON_SCS)
return g_MoonRadius;
else
return g_EarthRadius;
}

//////////////////////////////////////////////////
double SphericalCoordinates::SurfaceAxisEquatorial() const
{
return this->dataPtr->ellA;
}

//////////////////////////////////////////////////
double SphericalCoordinates::SurfaceAxisPolar() const
{
return this->dataPtr->ellB;
}

//////////////////////////////////////////////////
double SphericalCoordinates::SurfaceFlattening() const
{
return this->dataPtr->ellF;
}

//////////////////////////////////////////////////
void SphericalCoordinates::UpdateTransformationMatrix()
{
Expand Down
40 changes: 39 additions & 1 deletion gazebo/common/SphericalCoordinates.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,12 @@ namespace gazebo
{
/// \brief Model of reference ellipsoid for earth, based on
/// WGS 84 standard. see wikipedia: World_Geodetic_System
EARTH_WGS84 = 1
EARTH_WGS84 = 1,

/// \brief Model of the moon, based on the Selenographic
/// coordinate system, see wikipedia: Selenographic
/// Coordinate System.
MOON_SCS = 2
};

/// \enum CoordinateType
Expand Down Expand Up @@ -119,6 +124,23 @@ namespace gazebo
const ignition::math::Angle &_latB,
const ignition::math::Angle &_lonB);

/// \brief Get the distance between two points expressed in geographic
/// latitude and longitude. It assumes that both points are at sea level.
/// Example: _latA = 38.0016667 and _lonA = -123.0016667) represents
/// the point with latitude 38d 0'6.00"N and longitude 123d 0'6.00"W.
/// This is different from the static Distance() method as it
/// takes into account the set surface's radius.
/// \param[in] _latA Latitude of point A.
/// \param[in] _lonA Longitude of point A.
/// \param[in] _latB Latitude of point B.
/// \param[in] _lonB Longitude of point B.
/// \return Distance in meters.
public: double DistanceBetweenPoints(
const ignition::math::Angle &_latA,
const ignition::math::Angle &_lonA,
const ignition::math::Angle &_latB,
const ignition::math::Angle &_lonB);

/// \brief Get SurfaceType currently in use.
/// \return Current SurfaceType value.
public: SurfaceType GetSurfaceType() const;
Expand All @@ -131,6 +153,22 @@ namespace gazebo
/// \return Reference longitude.
public: ignition::math::Angle LongitudeReference() const;

/// \brief Get the radius of the surface.
/// \return radius of the surface in use.
public: double SurfaceRadius() const;

/// \brief Get the major axis of the surface.
/// \return Equatorial axis of the surface in use.
public: double SurfaceAxisEquatorial() const;

/// \brief Get the minor axis of the surface.
/// \return Polar axis of the surface in use.
public: double SurfaceAxisPolar() const;

/// \brief Get the flattening of the surface.
/// \return Flattening parameter of the surface in use.
public: double SurfaceFlattening() const;

/// \brief Get reference elevation in meters.
/// \return Reference elevation.
public: double GetElevationReference() const;
Expand Down
103 changes: 101 additions & 2 deletions gazebo/common/SphericalCoordinates_TEST.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,17 @@ class SphericalCoordinatesTest : public gazebo::testing::AutoLogFixture { };
// Test different constructors, default parameters
TEST_F(SphericalCoordinatesTest, Constructor)
{
// Constants
const double g_EarthWGS84AxisEquatorial = 6378137.0;
const double g_EarthWGS84AxisPolar = 6356752.314245;
const double g_EarthWGS84Flattening = 1.0/298.257223563;
const double g_EarthRadius = 6371000.0;

const double g_MoonRadius = 1737400.0;
const double g_MoonAxisEquatorial = 1738100.0;
const double g_MoonAxisPolar = 1736000.0;
const double g_MoonFlattening = 0.0012;

// Default surface type
common::SphericalCoordinates::SurfaceType st =
common::SphericalCoordinates::EARTH_WGS84;
Expand All @@ -41,6 +52,14 @@ TEST_F(SphericalCoordinatesTest, Constructor)
EXPECT_EQ(sc.LongitudeReference(), ignition::math::Angle());
EXPECT_EQ(sc.HeadingOffset(), ignition::math::Angle());
EXPECT_NEAR(sc.GetElevationReference(), 0.0, 1e-6);

EXPECT_NEAR(sc.SurfaceRadius(), g_EarthRadius, 1e-6);
EXPECT_NEAR(sc.SurfaceAxisEquatorial(),
g_EarthWGS84AxisEquatorial, 1e-6);
EXPECT_NEAR(sc.SurfaceAxisPolar(),
g_EarthWGS84AxisPolar, 1e-6);
EXPECT_NEAR(sc.SurfaceFlattening(),
g_EarthWGS84Flattening, 1e-6);
}

// SurfaceType argument, default parameters
Expand All @@ -51,6 +70,14 @@ TEST_F(SphericalCoordinatesTest, Constructor)
EXPECT_EQ(sc.LongitudeReference(), ignition::math::Angle());
EXPECT_EQ(sc.HeadingOffset(), ignition::math::Angle());
EXPECT_NEAR(sc.GetElevationReference(), 0.0, 1e-6);

EXPECT_NEAR(sc.SurfaceRadius(), g_EarthRadius, 1e-6);
EXPECT_NEAR(sc.SurfaceAxisEquatorial(),
g_EarthWGS84AxisEquatorial, 1e-6);
EXPECT_NEAR(sc.SurfaceAxisPolar(),
g_EarthWGS84AxisPolar, 1e-6);
EXPECT_NEAR(sc.SurfaceFlattening(),
g_EarthWGS84Flattening, 1e-6);
}

// All arguments
Expand All @@ -63,6 +90,57 @@ TEST_F(SphericalCoordinatesTest, Constructor)
EXPECT_EQ(sc.LongitudeReference(), lon);
EXPECT_EQ(sc.HeadingOffset(), heading);
EXPECT_NEAR(sc.GetElevationReference(), elev, 1e-6);

EXPECT_NEAR(sc.SurfaceRadius(), g_EarthRadius, 1e-6);
EXPECT_NEAR(sc.SurfaceAxisEquatorial(),
g_EarthWGS84AxisEquatorial, 1e-6);
EXPECT_NEAR(sc.SurfaceAxisPolar(),
g_EarthWGS84AxisPolar, 1e-6);
EXPECT_NEAR(sc.SurfaceFlattening(),
g_EarthWGS84Flattening, 1e-6);
}

// For Moon surface
{
common::SphericalCoordinates sc(
common::SphericalCoordinates::MOON_SCS);
EXPECT_EQ(sc.GetSurfaceType(),
common::SphericalCoordinates::MOON_SCS);
EXPECT_EQ(sc.LatitudeReference(), ignition::math::Angle());
EXPECT_EQ(sc.LongitudeReference(), ignition::math::Angle());
EXPECT_EQ(sc.HeadingOffset(), ignition::math::Angle());
EXPECT_NEAR(sc.GetElevationReference(), 0.0, 1e-6);

EXPECT_NEAR(sc.SurfaceRadius(), g_MoonRadius, 1e-6);
EXPECT_NEAR(sc.SurfaceAxisEquatorial(),
g_MoonAxisEquatorial, 1e-6);
EXPECT_NEAR(sc.SurfaceAxisPolar(),
g_MoonAxisPolar, 1e-6);
EXPECT_NEAR(sc.SurfaceFlattening(),
g_MoonFlattening, 1e-6);
}

// Moon surface with arguments
{
ignition::math::Angle lat(0.3), lon(-1.2), heading(0.5);
double elev = 354.1;
common::SphericalCoordinates sc(
common::SphericalCoordinates::MOON_SCS,
lat, lon, elev, heading);
EXPECT_EQ(sc.GetSurfaceType(),
common::SphericalCoordinates::MOON_SCS);
EXPECT_EQ(sc.LatitudeReference(), lat);
EXPECT_EQ(sc.LongitudeReference(), lon);
EXPECT_EQ(sc.HeadingOffset(), heading);
EXPECT_NEAR(sc.GetElevationReference(), elev, 1e-6);

EXPECT_NEAR(sc.SurfaceRadius(), g_MoonRadius, 1e-6);
EXPECT_NEAR(sc.SurfaceAxisEquatorial(),
g_MoonAxisEquatorial, 1e-6);
EXPECT_NEAR(sc.SurfaceAxisPolar(),
g_MoonAxisPolar, 1e-6);
EXPECT_NEAR(sc.SurfaceFlattening(),
g_MoonFlattening, 1e-6);
}
}

Expand All @@ -75,6 +153,10 @@ TEST_F(SphericalCoordinatesTest, Convert)
common::SphericalCoordinates::EARTH_WGS84;

EXPECT_EQ(common::SphericalCoordinates::Convert("EARTH_WGS84"), st);

// For Moon surface type
st = common::SphericalCoordinates::MOON_SCS;
EXPECT_EQ(common::SphericalCoordinates::Convert("MOON_SCS"), st);
}

//////////////////////////////////////////////////
Expand Down Expand Up @@ -261,9 +343,26 @@ TEST_F(SphericalCoordinatesTest, Distance)
longA.Degree(-122.249972);
latB.Degree(46.124953);
longB.Degree(-122.251683);
double d = common::SphericalCoordinates::Distance(latA, longA, latB, longB);

EXPECT_NEAR(14002, d, 20);
// Calculating distance using the static method.
double d1 = common::SphericalCoordinates::Distance(latA, longA, latB, longB);
EXPECT_NEAR(14002, d1, 20);

// Using the non static method. The default surface type is EARTH_WGS84.
common::SphericalCoordinates earthSC = common::SphericalCoordinates();
double d2 = earthSC.DistanceBetweenPoints(latA, longA, latB, longB);
EXPECT_NEAR(d1, d2, 0.1);

earthSC = common::SphericalCoordinates(
common::SphericalCoordinates::EARTH_WGS84);
double d3 = earthSC.DistanceBetweenPoints(latA, longA, latB, longB);
EXPECT_NEAR(d2, d3, 0.1);

// Setting the surface type as Moon.
common::SphericalCoordinates moonSC = common::SphericalCoordinates(
common::SphericalCoordinates::MOON_SCS);
double d4 = moonSC.DistanceBetweenPoints(latA, longA, latB, longB);
EXPECT_NEAR(3820, d4, 5);
}

/////////////////////////////////////////////////
Expand Down