Skip to content

Commit

Permalink
Fix for issue #1
Browse files Browse the repository at this point in the history
  • Loading branch information
@kenba
kenba committed Apr 26, 2024
1 parent b5ceff2 commit 8e5eadb
Show file tree
Hide file tree
Showing 3 changed files with 130 additions and 77 deletions.
66 changes: 28 additions & 38 deletions src/geodesic.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,11 +32,7 @@ use crate::ellipsoid::{calculate_epsilon, calculate_parametric_latitude, Metres}
use crate::Ellipsoid;
use angle_sc::trig::{cosine_from_sine, UnitNegRange};
use angle_sc::{is_small, Angle, Radians};
use unit_sphere::great_circle;
use unit_sphere::LatLong;

/// The maximum precision, in Radians.
pub const MAX_PRECISION: Radians = Radians(2.0 * core::f64::EPSILON);
use unit_sphere::{great_circle, LatLong};

/// Estimate omega12 by solving the astroid problem.
/// Solve k^4+2*k^3-(x^2+y^2-1)*k^2-2*y^2*k-y^2 = 0 for positive root k.
Expand Down Expand Up @@ -259,9 +255,11 @@ fn find_azimuth_and_aux_length(
gc_length: Radians,
ellipsoid: &Ellipsoid,
) -> (Angle, Radians) {
/// The maximum precision, in Radians.
const MAX_PRECISION: Radians = Radians(great_circle::MIN_VALUE);

// The maximum number of iterations to attempt.
const MAX_ITERS: u32 = 20;
let antipodal_arc_threshold: f64 = core::f64::consts::PI * ellipsoid.one_minus_f();

// Start at the latitude furthest from the Equator
let swap_latitudes = libm::fabs(lat_a.sin().0) < libm::fabs(lat_b.sin().0);
Expand All @@ -286,6 +284,7 @@ fn find_azimuth_and_aux_length(
let abs_lambda12 = lambda12.abs();

// Estimate the azimuth at the start of the geodesic
let antipodal_arc_threshold: f64 = core::f64::consts::PI * ellipsoid.one_minus_f();
let mut alpha1 = if antipodal_arc_threshold < gc_length.0 {
estimate_antipodal_initial_azimuth(beta1, beta2, abs_lambda12, ellipsoid)
} else {
Expand Down Expand Up @@ -396,41 +395,31 @@ pub fn aux_sphere_azimuth_length(
delta_long: Angle,
ellipsoid: &Ellipsoid,
) -> (Angle, Radians) {
const MIN_VALUE: UnitNegRange = UnitNegRange(2.0 * core::f64::EPSILON);
const MAX_LENGTH: Radians = Radians(core::f64::consts::PI - 2.0 * MIN_VALUE.0);

// Determine whether on a meridian, i.e. a great circle which passes through the North and South poles
let gc_azimuth = great_circle::calculate_gc_azimuth(lat1, lat2, delta_long);
let gc_length = great_circle::calculate_gc_distance(lat1, lat2, delta_long);
// Ensure that the points are far enough apart
if gc_length.0 <= MIN_VALUE.0 {
return (gc_azimuth, Radians(0.0));
}

// Determine whether a meridional path
let abs_delta_long = Radians::from(delta_long.abs());
if (abs_delta_long.0 <= MIN_VALUE.0)
|| (MAX_LENGTH <= abs_delta_long)
|| (lat1.cos() <= MIN_VALUE)
|| (lat2.cos() <= MIN_VALUE)
{
let meridian_length = if MAX_LENGTH <= gc_length {
Radians(core::f64::consts::PI)
// gc_azimuth is 0° or 180°
if is_small(gc_azimuth.abs().sin().0, great_circle::MIN_VALUE) {
// Calculate the meridian distance on the auxillary sphere
let beta1 = calculate_parametric_latitude(lat1, ellipsoid.one_minus_f());
let beta2 = calculate_parametric_latitude(lat2, ellipsoid.one_minus_f());
let meridian_length = great_circle::calculate_gc_distance(beta1, beta2, delta_long);
(gc_azimuth, meridian_length)
} else {
// Determine whether on an equatorial path, i.e. the circle around the equator.
let gc_length = great_circle::calculate_gc_distance(lat1, lat2, delta_long);
// gc_azimuth is +/-90° and both latitudes are very close to the equator
if is_small(gc_azimuth.cos().0, great_circle::MIN_VALUE)
&& is_small(lat1.abs().sin().0, core::f64::EPSILON)
&& is_small(lat2.abs().sin().0, core::f64::EPSILON)
{
// Calculate the distance around the equator on the auxillary sphere
let equatorial_length = Radians(gc_length.0 * ellipsoid.recip_one_minus_f());
(gc_azimuth, equatorial_length)
} else {
let beta1 = calculate_parametric_latitude(lat1, ellipsoid.one_minus_f());
let beta2 = calculate_parametric_latitude(lat2, ellipsoid.one_minus_f());
great_circle::calculate_gc_distance(beta1, beta2, delta_long)
};
return (gc_azimuth, meridian_length);
}

// Determine whether an equitorial path, i.e. both latitudes on the equator.
if (lat1.abs().sin() <= MIN_VALUE) && (lat2.abs().sin() <= MIN_VALUE) {
let equator_length = Radians(gc_length.0 * ellipsoid.recip_one_minus_f());
return (gc_azimuth, equator_length);
// Iterate to find the azimuth and length on the auxillary sphere
find_azimuth_and_aux_length(lat1, lat2, delta_long, gc_length, ellipsoid)
}
}

// Iterate using Newton's method to find the azimuth and length
find_azimuth_and_aux_length(lat1, lat2, delta_long, gc_length, ellipsoid)
}

/// Calculate the geodesic azimuth and great circle length on the auxiliary sphere
Expand Down Expand Up @@ -606,6 +595,7 @@ mod tests {
assert_eq!(0.0, Degrees::from(result.0).0);
assert_eq!(0.3502163200513691, (result.1).0);
}

#[test]
fn test_calculate_azimuth_aux_length_equator() {
let wgs84_ellipsoid = Ellipsoid::wgs84();
Expand Down
109 changes: 83 additions & 26 deletions src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,7 +112,7 @@ pub use icao_units::si::Metres;
pub use unit_sphere::LatLong;

use angle_sc::{is_small, trig};
use unit_sphere::Vector3d;
use unit_sphere::{great_circle, Vector3d};

/// The parameters of an Ellipsoid.
/// The default value is the WGS84 Ellipsoid.
Expand Down Expand Up @@ -442,7 +442,20 @@ impl<'a> Geodesic<'a> {
/// * `ellipsoid` - a reference to the Ellipsoid.
#[must_use]
pub fn between_positions(a: &LatLong, b: &LatLong, ellipsoid: &'a Ellipsoid) -> Self {
Self::from((a, b, ellipsoid))
let (azimuth, aux_length) = geodesic::calculate_azimuth_aux_length(a, b, ellipsoid);
let a_lat = Angle::from(a.lat());
// if a is at the North or South pole
if is_small(a_lat.cos().0, great_circle::MIN_VALUE) {
// use b's longitude
Self::from_lat_lon_azi_aux_length(
&LatLong::new(a.lat(), b.lon()),
azimuth,
aux_length,
ellipsoid,
)
} else {
Self::from_lat_lon_azi_aux_length(a, azimuth, aux_length, ellipsoid)
}
}

/// Accessor for the start latitude on the auxiliary sphere.
Expand Down Expand Up @@ -489,7 +502,7 @@ impl<'a> Geodesic<'a> {
/// returns the distance in radians on the auxiliary sphere.
#[must_use]
pub fn metres_to_radians(&self, distance_m: Metres) -> Radians {
if is_small(libm::fabs(distance_m.0), core::f64::EPSILON) {
if is_small(libm::fabs(distance_m.0), great_circle::MIN_VALUE) {
Radians(0.0)
} else {
let tau12 = Radians(distance_m.0 / (self.ellipsoid.b().0 * self.a1));
Expand All @@ -508,7 +521,7 @@ impl<'a> Geodesic<'a> {
/// returns the distance in metres on the ellipsoid.
#[must_use]
pub fn radians_to_metres(&self, gc_distance: Radians) -> Metres {
if is_small(libm::fabs(gc_distance.0), core::f64::EPSILON) {
if is_small(libm::fabs(gc_distance.0), great_circle::MIN_VALUE) {
Metres(0.0)
} else {
let sigma_sum = self.sigma1 + Angle::from(gc_distance);
Expand Down Expand Up @@ -564,9 +577,9 @@ impl<'a> Geodesic<'a> {
/// return the azimuth of the geodesic/great circle at `gc_length`.
#[must_use]
pub fn aux_azimuth(&self, gc_length: Radians) -> Angle {
const MAX_LAT: f64 = 1.0 - 2.0 * core::f64::EPSILON;
const MAX_LAT: f64 = 1.0 - great_circle::MIN_VALUE;

if is_small(libm::fabs(gc_length.0), 2.0 * core::f64::EPSILON) {
if is_small(libm::fabs(gc_length.0), great_circle::MIN_VALUE) {
self.azi
} else {
let length = Angle::from(gc_length);
Expand Down Expand Up @@ -600,24 +613,25 @@ impl<'a> Geodesic<'a> {
#[allow(clippy::similar_names)]
#[must_use]
pub fn delta_longitude(&self, gc_length: Radians) -> Angle {
if libm::fabs(gc_length.0) <= core::f64::EPSILON {
return Angle::default();
}
// The great circle distance from Northward Equator crossing.
let sigma_sum = self.sigma1 + Angle::from(gc_length);
if is_small(libm::fabs(gc_length.0), great_circle::MIN_VALUE) {
Angle::default()
} else {
// The great circle distance from Northward Equator crossing.
let sigma_sum = self.sigma1 + Angle::from(gc_length);

// The longitude difference on the auxiliary sphere, omega12.
let omega12 = Angle::from_y_x(self.azi0.sin().0 * sigma_sum.sin().0, sigma_sum.cos().0)
- Angle::from_y_x(
self.azi0.sin().0 * self.beta.sin().0,
geodesic::calculate_cos_omega(self.beta, self.azi.cos()).0,
);
// The longitude difference on the auxiliary sphere, omega12.
let omega12 = Angle::from_y_x(self.azi0.sin().0 * sigma_sum.sin().0, sigma_sum.cos().0)
- Angle::from_y_x(
self.azi0.sin().0 * self.beta.sin().0,
geodesic::calculate_cos_omega(self.beta, self.azi.cos()).0,
);

let c3 = self.ellipsoid.calculate_c3y(self.eps);
let b31 = ellipsoid::coefficients::sin_cos_series(&c3, self.sigma1);
let b32 = ellipsoid::coefficients::sin_cos_series(&c3, sigma_sum);
let c3 = self.ellipsoid.calculate_c3y(self.eps);
let b31 = ellipsoid::coefficients::sin_cos_series(&c3, self.sigma1);
let b32 = ellipsoid::coefficients::sin_cos_series(&c3, sigma_sum);

omega12 - Angle::from(Radians(self.a3c * (gc_length.0 + (b32.0 - b31.0))))
omega12 - Angle::from(Radians(self.a3c * (gc_length.0 + (b32.0 - b31.0))))
}
}

/// Calculate the geodesic longitude at the great circle length along
Expand Down Expand Up @@ -672,7 +686,7 @@ impl<'a> Geodesic<'a> {
let point = unit_sphere::vector::to_point(self.beta, self.lon);
let pole = unit_sphere::vector::calculate_pole(self.beta, self.lon, self.azi);
// If at the start of the geodesic
if is_small(libm::fabs(gc_length.0), 2.0 * core::f64::EPSILON) {
if is_small(libm::fabs(gc_length.0), great_circle::MIN_VALUE) {
(point, pole)
} else {
let length = Angle::from(gc_length);
Expand All @@ -681,7 +695,7 @@ impl<'a> Geodesic<'a> {
let point = unit_sphere::vector::to_point(beta, lon);

// if point is on a meridional Geodesic use auxiliary sphere point and pole
if is_small(libm::fabs(self.azi0.sin().0), 2.0 * core::f64::EPSILON) {
if is_small(libm::fabs(self.azi0.sin().0), great_circle::MIN_VALUE) {
(point, pole)
} else {
// Note: point cannot be at North pole, since it is not on a meridional Geodesic
Expand Down Expand Up @@ -861,9 +875,7 @@ impl<'a> From<(&LatLong, &LatLong, &'a Ellipsoid)> for Geodesic<'a> {
/// * `ellipsoid` - a reference to the Ellipsoid.
#[must_use]
fn from(params: (&LatLong, &LatLong, &'a Ellipsoid)) -> Geodesic<'a> {
let (azimuth, aux_length) =
geodesic::calculate_azimuth_aux_length(params.0, params.1, params.2);
Geodesic::from((params.0, azimuth, aux_length, params.2))
Self::between_positions(params.0, params.1, params.2)
}
}

Expand Down Expand Up @@ -1135,6 +1147,7 @@ mod tests {
fn test_geodesicarc_direct_constructors() {
let wgs84_ellipsoid = Ellipsoid::wgs84();
let length = Metres(9_000_000.0);
let aux_length = Radians(core::f64::consts::FRAC_PI_2);

// Ensure that two Geodesics can fit on a cache line.
assert_eq!(128, size_of::<Geodesic>());
Expand All @@ -1157,6 +1170,18 @@ mod tests {

let len0 = geodesic1.length();
assert!(is_within_tolerance(length.0, len0.0, 1.0e-8));

let geodesic2 = Geodesic::from((&a, azimuth, aux_length, &wgs84_ellipsoid));
assert!(geodesic2.is_valid());
let azi0 = geodesic2.azimuth(Metres(0.0));
assert!(is_within_tolerance(
Radians::from(azimuth).0,
Radians::from(azi0).0,
2.0 * std::f64::EPSILON
));

let len0 = geodesic2.aux_length();
assert!(is_within_tolerance(aux_length.0, len0.0, 1.0e-8));
}
}

Expand Down Expand Up @@ -1280,6 +1305,38 @@ mod tests {
assert_eq!(pole0, pole1);
}

#[test]
fn test_geodesicarc_90n_0n_0e() {
let wgs84_ellipsoid = Ellipsoid::wgs84();

let a = LatLong::new(Degrees(90.0), Degrees(0.0));
let b = LatLong::new(Degrees(0.0), Degrees(0.0));
let g1 = Geodesic::from((&a, &b, &wgs84_ellipsoid));

assert!(is_within_tolerance(
core::f64::consts::FRAC_PI_2,
g1.aux_length().0,
core::f64::EPSILON
));
assert_eq!(180.0, Degrees::from(g1.azi()).0);
}

#[test]
fn test_geodesicarc_90s_0n_50e() {
let wgs84_ellipsoid = Ellipsoid::wgs84();

let a = LatLong::new(Degrees(-90.0), Degrees(0.0));
let b = LatLong::new(Degrees(0.0), Degrees(50.0));
let g1 = Geodesic::from((&a, &b, &wgs84_ellipsoid));

assert!(is_within_tolerance(
core::f64::consts::FRAC_PI_2,
g1.aux_length().0,
core::f64::EPSILON
));
assert_eq!(0.0, Degrees::from(g1.azi()).0);
}

#[test]
fn test_calculate_atd_and_xtd() {
let wgs84_ellipsoid = Ellipsoid::wgs84();
Expand Down
32 changes: 19 additions & 13 deletions tests/integration_test.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,15 +65,17 @@ fn test_geodesic_examples() {
let result = geodesic::calculate_azimuth_aux_length(&a, &b, &geoid);

let delta_azimuth = libm::fabs(azi1.0 - Degrees::from(result.0).0);
if 5.5e-5 < delta_azimuth {
// reduce tolerance for entries running between or close to vertices
let azimuth_tolerance = if line_number <= 400000 { 5.5e-5 } else { 0.28 };
if azimuth_tolerance < delta_azimuth {
panic!(
"azimuth, line: {:?} delta: {:?} azimuth: {:?} delta_long: {:?} ",
line_number, delta_azimuth, azi1, lon2
);
}

let delta_length = libm::fabs(d_degrees.0.to_radians() - (result.1).0);
if 2.0e-11 < delta_length {
if 3.0e-10 < delta_length {
panic!(
"length, line: {:?} delta: {:?} length: {:?} delta_long: {:?} ",
line_number, delta_length, d_degrees, lon2
Expand All @@ -84,29 +86,33 @@ fn test_geodesic_examples() {
let result_m = geodesic::convert_radians_to_metres(beta1, result.0, result.1, &geoid);

let delta_length_m = libm::fabs(d_metres.0 - result_m.0);
if line_number <= 150000 {
let delta_length_m_ratio = delta_length_m / d_metres.0;
if 1.7e-11 < delta_length_m_ratio {
// if a short geodesic, test delta length, not delta length ratio
if line_number >= 150000 && line_number < 200000 {
if 9.0e-5 < delta_length_m {
panic!(
"length, line: {:?} delta: {:?} length: {:?} delta_long: {:?} ",
line_number, delta_length_m_ratio, d_metres, result_m
"length, line: {:?} delta: {:?} length: {:?} result: {:?} ",
line_number, delta_length_m, d_metres, result_m
);
}
} else {
if 9.0e-5 < delta_length_m {
let delta_length_m_ratio = delta_length_m / d_metres.0;
if 2.5e-9 < delta_length_m_ratio {
panic!(
"length, line: {:?} delta: {:?} length: {:?} delta_long: {:?} ",
line_number, delta_length_m, d_metres, result_m
"length, line: {:?} delta ratio: {:?} length: {:?} result: {:?} ",
line_number, delta_length_m_ratio, d_metres, result_m
);
}
}

// random_df = tests_df[:100000]
// antipodal_df = tests_df[100000:150000]
// short_df = tests_df[150000:200000]
// one_pole_df = tests_df[200000:250000]
// two_poles_df = tests_df[250000:300000]
// near_meridional_df = tests_df[300000:350000]
// near_equatorial_df = tests_df[350000:400000]
// between_vertices_df = tests_df[400000:450000]
// end_by_vertices_df = tests_df[450000:500000]
line_number += 1;
if 200000 < line_number {
break;
}
}
}

0 comments on commit 8e5eadb

Please sign in to comment.