1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
//! Rays against the ellipsoid: where a line meets the surface, line of sight and horizon distance.
//! The surface is smooth — no terrain, no atmospheric refraction.
use crate::ellipsoid::Ellipsoid;
use crate::position::Ecef;
use crate::vector::Vec3;
impl Ellipsoid {
/// Where the ray `origin + t·dir` (t ≥ 0) first meets the ellipsoid surface, or `None` if it
/// misses or the surface is behind it. `dir` need not be unit length.
///
/// For a pointing from an observer (azimuth and elevation) use
/// [`LocalFrame::ground_strike`](crate::LocalFrame::ground_strike).
pub fn ray_intersect(self, origin: Ecef, dir: Vec3) -> Option<Ecef> {
let (t1, t2) = self.ray_roots(origin, dir)?;
let t = if t1 >= 0.0 {
t1
} else if t2 >= 0.0 {
t2
} else {
return None;
};
Some(origin.offset(dir * t))
}
/// The roots `t1 ≤ t2` where the line `origin + t·dir` crosses the ellipsoid surface, or `None`
/// when it misses, `dir` is zero, or a coordinate is not finite. Scales space so the ellipsoid
/// becomes the unit sphere and solves the ray–sphere quadratic in its cancellation-free form,
/// so the near root stays accurate when the origin is close to the surface.
pub(crate) fn ray_roots(self, origin: Ecef, dir: Vec3) -> Option<(f64, f64)> {
let s = Vec3::new(1.0 / self.a(), 1.0 / self.a(), 1.0 / self.b());
let o = Vec3::new(origin.x * s.x, origin.y * s.y, origin.z * s.z);
let d = Vec3::new(dir.x * s.x, dir.y * s.y, dir.z * s.z);
let (qa, qb, qc) = (d.dot(d), 2.0 * o.dot(d), o.dot(o) - 1.0);
let disc = qb * qb - 4.0 * qa * qc;
if disc.is_nan() || disc < 0.0 || qa == 0.0 {
return None;
}
// q = −(b + sign(b)·√disc)/2 never subtracts nearly equal numbers; the roots are q/a and c/q.
let q = -0.5 * (qb + disc.sqrt().copysign(qb));
if q == 0.0 {
return Some((0.0, 0.0)); // tangent at the origin, which is on the surface
}
let (r1, r2) = (q / qa, qc / q);
Some((r1.min(r2), r1.max(r2)))
}
/// Whether the straight line between two points clears the ellipsoid — can `a` see `b`? Both
/// points should be at or above the surface; a point on the surface sees another only if the
/// line does not dip below it.
pub fn line_of_sight(self, a: Ecef, b: Ecef) -> bool {
let d = b.vec() - a.vec();
let dd = d.dot(d);
if dd == 0.0 {
return true;
}
// Reuse the ray solve but reject a hit that is just numerical noise at `a` itself.
match self.ray_intersect(a, d) {
None => true,
Some(hit) => {
let t = (hit.vec() - a.vec()).dot(d) / dd;
t >= 1.0 - 1e-12 || t <= 1e-9
}
}
}
/// How far you can see from `height_m` above the surface before the Earth curves away, metres
/// (on a sphere of the mean radius). Negative heights count as zero.
#[inline]
pub fn horizon_distance_m(self, height_m: f64) -> f64 {
let r = self.mean_radius();
let h = height_m.max(0.0);
(2.0 * r * h + h * h).sqrt()
}
}
#[cfg(test)]
mod tests {
use crate::test_util::close;
use crate::{Ecef, Ellipsoid, Geodetic, Vec3, WGS84};
#[test]
fn ray_roots_have_no_cancellation() {
// Vieta: the roots' product is c/a. The textbook formula loses the small root to
// cancellation when the origin is near the surface; the stable form keeps it.
let ell = Ellipsoid::new(1.0, 0.0);
let o = Ecef::new(1.0 + 1e-9, 0.0, 0.0);
for deg in [10.0f64, 60.0, 89.0, 120.0, 179.0] {
let d = Vec3::new(deg.to_radians().cos(), deg.to_radians().sin(), 0.0);
let (t1, t2) = ell.ray_roots(o, d).unwrap();
let c = o.vec().dot(o.vec()) - 1.0;
assert!(close(t1 * t2 / c, 1.0, 1e-13), "{deg}°: product {} vs {c}", t1 * t2);
assert!(t1 <= t2);
}
// Tangent from a point on the surface: a double root at 0.
assert_eq!(
ell.ray_roots(Ecef::new(1.0, 0.0, 0.0), Vec3::new(0.0, 1.0, 0.0)),
Some((0.0, 0.0))
);
}
#[test]
fn ray_intersect_finds_the_near_surface_or_misses() {
// Straight down from 10 km over the origin hits (a, 0, 0).
let o = WGS84.to_ecef(Geodetic::new(0.0, 0.0, 10_000.0));
let hit = WGS84.ray_intersect(o, Vec3::new(-1.0, 0.0, 0.0)).unwrap();
assert!(close(hit.x, WGS84.a(), 1e-6) && close(hit.y, 0.0, 1e-9));
// Pointing away: the surface is behind the ray.
assert!(WGS84.ray_intersect(o, Vec3::new(1.0, 0.0, 0.0)).is_none());
// Sideways from far out in space: misses.
assert!(WGS84
.ray_intersect(Ecef::new(1e8, 0.0, 0.0), Vec3::new(0.0, 1.0, 0.0))
.is_none());
// Zero direction: no ray.
assert!(WGS84.ray_intersect(o, Vec3::ZERO).is_none());
}
#[test]
fn horizon_and_line_of_sight() {
// ~357 km from 10 km up; zero from the surface; negative heights clamp to zero.
assert!(close(WGS84.horizon_distance_m(10_000.0), 357_000.0, 1_000.0));
assert_eq!(WGS84.horizon_distance_m(0.0), 0.0);
assert_eq!(WGS84.horizon_distance_m(-5.0), 0.0);
let a = WGS84.to_ecef(Geodetic::new(0.0, 0.0, 10_000.0));
let near = WGS84.to_ecef(Geodetic::new(0.0, 1.0, 10_000.0)); // 111 km: well within the horizon
let far = WGS84.to_ecef(Geodetic::new(0.0, 10.0, 10_000.0)); // 1113 km: over the horizon
assert!(WGS84.line_of_sight(a, near) && WGS84.line_of_sight(near, a));
assert!(!WGS84.line_of_sight(a, far) && !WGS84.line_of_sight(far, a));
assert!(WGS84.line_of_sight(a, a));
// Two satellites on opposite sides of the Earth are blocked; two close together are not.
let s1 = Ecef::new(2.0e7, 0.0, 0.0);
assert!(!WGS84.line_of_sight(s1, Ecef::new(-2.0e7, 0.0, 0.0)));
assert!(WGS84.line_of_sight(s1, Ecef::new(0.0, 2.0e7, 0.0)));
// From the surface, a point 1 km up 5 km away is visible.
let ground = WGS84.to_ecef(Geodetic::new(0.0, 0.0, 0.0));
assert!(WGS84.line_of_sight(ground, WGS84.to_ecef(Geodetic::new(0.0, 0.045, 1000.0))));
}
}