446 lines
16 KiB
Rust
446 lines
16 KiB
Rust
//! Geo Algorithms
|
|
//!
|
|
//! Since the `geo` crate is not `no_std`, we need to implement the most important algorithms on our own
|
|
|
|
use alloc::string::{String, ToString};
|
|
use core::ops::{Mul, Sub};
|
|
|
|
use log::warn;
|
|
#[cfg(not(feature = "std"))]
|
|
use num_traits::float::Float;
|
|
|
|
const EARTH_RADIUS: f64 = 6_371_008.8; // mean radius of GRS80 ellipsoid
|
|
const EARTH_CIRCUMFERENCE: f64 = 40_030_228.884; // calculated from `EARTH_RADIUS * 2 * PI`
|
|
|
|
/// Haversine distance between two geo coordinates (in meters)
|
|
#[allow(unused)]
|
|
pub fn haversine_dist(a: &geo_types::Point, b: &geo_types::Point) -> f64 {
|
|
let theta1 = a.y().to_radians();
|
|
let theta2 = b.y().to_radians();
|
|
let delta_theta = (b.y() - a.y()).to_radians();
|
|
let delta_lambda = (b.x() - a.x()).to_radians();
|
|
let a = (delta_theta / 2.).sin().powi(2)
|
|
+ theta1.cos() * theta2.cos() * (delta_lambda / 2.).sin().powi(2);
|
|
let c = 2. * a.sqrt().asin();
|
|
EARTH_RADIUS * c
|
|
}
|
|
|
|
/// Haversine bearing from point `a` to point `b`
|
|
#[allow(unused)]
|
|
pub fn haversine_bearing(a: &geo_types::Point, b: &geo_types::Point) -> f32 {
|
|
let (lng_a, lat_a) = (a.x().to_radians(), a.y().to_radians());
|
|
let (lng_b, lat_b) = (b.x().to_radians(), b.y().to_radians());
|
|
let delta_lng = lng_b - lng_a;
|
|
|
|
#[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
|
|
let s = (lat_b.cos() * delta_lng.sin()) as f32;
|
|
#[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
|
|
let c = (lat_a.cos() * lat_b.sin() - lat_a.sin() * lat_b.cos() * delta_lng.cos()) as f32;
|
|
|
|
let degrees = f32::atan2(s, c).to_degrees();
|
|
(degrees + 360.0) % 360.0
|
|
}
|
|
|
|
/// Cartesian distance between to X/Y coordinates (in unit of the coordinates)
|
|
#[allow(unused)]
|
|
pub fn cartesian_dist(a: geo_types::Point, b: geo_types::Point) -> f64 {
|
|
let vector = b.sub(a);
|
|
cartesian_norm(vector)
|
|
}
|
|
|
|
/// Cartesian bearing from X/Y point `a` to X/Y point `b`
|
|
#[allow(unused)]
|
|
pub fn cartesian_bearing(a: geo_types::Point, b: geo_types::Point) -> f64 {
|
|
let vector = b.sub(a);
|
|
cartesian_theta(vector)
|
|
}
|
|
|
|
/// Cartesian vector norm (length)
|
|
#[allow(unused)]
|
|
pub fn cartesian_norm(vec: geo_types::Point) -> f64 {
|
|
f64::sqrt(vec.x() * vec.x() + vec.y() * vec.y())
|
|
}
|
|
|
|
/// Cartesian vector direction (polar coordinate angle)
|
|
#[allow(unused)]
|
|
pub fn cartesian_theta(vec: geo_types::Point) -> f64 {
|
|
let theta_rad = f64::atan2(vec.y(), vec.x());
|
|
theta_rad * 180. / core::f64::consts::PI
|
|
}
|
|
|
|
/// Normalizes an angle (in degrees) to (-180, 180] interval
|
|
#[allow(unused)]
|
|
pub fn deg_180(angle_deg: f32) -> f32 {
|
|
let full_circle_deg = 360.0;
|
|
|
|
let angle = angle_deg % full_circle_deg;
|
|
if angle > full_circle_deg / 2. {
|
|
return angle - full_circle_deg;
|
|
}
|
|
if angle <= -full_circle_deg / 2. {
|
|
return angle + full_circle_deg;
|
|
}
|
|
angle
|
|
}
|
|
|
|
/// Normalizes an angle (in degrees) to [0, 360) interval
|
|
#[allow(unused)]
|
|
pub fn deg_360(angle_deg: f32) -> f32 {
|
|
let full_circle_deg = 360.0;
|
|
|
|
let angle = angle_deg % full_circle_deg;
|
|
if (angle < 0.) {
|
|
return angle + full_circle_deg;
|
|
}
|
|
angle
|
|
}
|
|
|
|
/// Converts a geo-position (lon/lat) to cartesian coordinates relative to a reference positions
|
|
///
|
|
/// Both `ref_pos` and `position` are expected to be lon/lat in degrees.
|
|
/// The output will be in a local X/Y coordinate system with X pointing north and Y pointing east.
|
|
#[allow(unused)]
|
|
pub fn lonlat_to_dcart(
|
|
ref_pos: &geo_types::Point,
|
|
position: &geo_types::Point,
|
|
) -> geo_types::Point {
|
|
let pos_diff = position.sub(*ref_pos);
|
|
let dlat_deg = pos_diff.y();
|
|
let dlon_deg = pos_diff.x();
|
|
|
|
// latitude (north/south) degree per meter is independent from longitude
|
|
let dx = dlat_deg / 360. * EARTH_CIRCUMFERENCE;
|
|
|
|
// longitude (east/west) degree per meter has a different value depending on the latitude
|
|
#[allow(clippy::cast_possible_truncation)]
|
|
let ref_lat_rad = ref_pos.to_radians().y();
|
|
let dy = dlon_deg / 360. * (EARTH_CIRCUMFERENCE * ref_lat_rad.cos());
|
|
|
|
geo_types::Point::new(dx, dy)
|
|
}
|
|
|
|
/// Converts a X/Y coordinate (NED relative to a reference position) to a geo-position
|
|
///
|
|
/// The `ref_pos` is expected to be lon/lat in degrees and the output will be the same.
|
|
/// The `position` is expected to be in a local X/Y coordinate system with X pointing north and Y pointing east.
|
|
#[allow(unused)]
|
|
pub fn dcart_to_lonlat(
|
|
ref_pos: &geo_types::Point,
|
|
position: &geo_types::Point,
|
|
) -> geo_types::Point {
|
|
// note that we're using NED while c-its-parser uses ITS-G5 style ENU X/Y coordinates
|
|
// so we need to swap X and Y coordinates
|
|
|
|
#[allow(clippy::cast_possible_truncation)]
|
|
let dx = position.y() as f32;
|
|
#[allow(clippy::cast_possible_truncation)]
|
|
let dy = position.x() as f32;
|
|
|
|
c_its_parser::geo_utils::point_from_dxy(dx, dy, ref_pos)
|
|
}
|
|
|
|
/// Describes how a position is related to a line segment
|
|
#[derive(Debug, Default)]
|
|
#[allow(unused)]
|
|
pub struct LinePosition {
|
|
/// Own position relative to the line segment
|
|
pub semantic: LinePositionType,
|
|
/// Distance to center line (always positive)
|
|
pub parallel_dist: f64,
|
|
/// Distance to first node (negative if `BeforeStart`)
|
|
pub perpendicular_dist: f64,
|
|
}
|
|
|
|
#[derive(Debug, Default, PartialEq, Eq, PartialOrd, Ord)]
|
|
#[allow(unused)]
|
|
pub enum LinePositionType {
|
|
#[default]
|
|
Undefined,
|
|
BeforeStart,
|
|
InBetween,
|
|
AfterEnd,
|
|
}
|
|
|
|
#[allow(unused)]
|
|
impl LinePosition {
|
|
pub fn new(parallel_dist: f64, perpendicular_dist: f64, after_end: bool) -> Self {
|
|
let semantic = if after_end {
|
|
LinePositionType::AfterEnd
|
|
} else if parallel_dist < 0. {
|
|
LinePositionType::BeforeStart
|
|
} else {
|
|
LinePositionType::InBetween
|
|
};
|
|
Self {
|
|
semantic,
|
|
parallel_dist,
|
|
perpendicular_dist,
|
|
}
|
|
}
|
|
|
|
/// Determines `point` position relative to a line segment (in X/Y coordinates)
|
|
///
|
|
/// The `ls` shall consist of 2 points!
|
|
///
|
|
/// # Algorithm
|
|
/// The line segment can be converted to a vector which describes an infinite line.
|
|
/// So we can calculate the (perpendicular) projection of the point on that line:
|
|
/// ```text
|
|
/// [Case A] (point) | [Case B] (point)
|
|
/// | | |
|
|
/// | | |
|
|
/// (node1)---(node2)-----(q) | (node1)------(q)------(node2)
|
|
/// ```
|
|
///
|
|
/// This will already give us the perpendicular and parallel distance.
|
|
/// Since we came from a finite line segment and want to know if the point is bejond the segment,
|
|
/// we also need to compare the parallel distance with the length of the vector.
|
|
/// So both case are the same mathematically.
|
|
///
|
|
/// # Errors
|
|
/// Fails if points are the same
|
|
pub fn for_ls(point: geo_types::Point, line: &geo_types::Line) -> Result<Self, String> {
|
|
let node1 = line.start_point();
|
|
let node2 = line.end_point();
|
|
|
|
let line_vec = node2.sub(node1);
|
|
let line_vec_length = cartesian_norm(line_vec);
|
|
|
|
// input validation
|
|
if line_vec_length < f64::EPSILON {
|
|
return Err("Points are the same".to_string());
|
|
}
|
|
|
|
// calculate projection of `p` onto the line
|
|
// ```math
|
|
// # n needs to be a unit vector
|
|
// n = (node2 - node1) / |node2 - node1|
|
|
// # vector from node1 to the point
|
|
// p = point - node1
|
|
// # projection is the unit-vector multipled by the dot-product of p and n
|
|
// q = (p {dot} n) * n
|
|
// # perpendicular length is calculated from the vector q->p
|
|
// QP = q - p
|
|
// ```
|
|
let p = point.sub(node1);
|
|
let n = line_vec.mul(1. / line_vec_length);
|
|
let q = n.mul(p.dot(n));
|
|
let qp_length = cartesian_norm(q.sub(p));
|
|
|
|
// calculate parallel distance of `q` to `node1`.
|
|
// This can be achieved by the dot-product since `n` is a unit vector and
|
|
// the dot-product of two paralle vectors is the same as the multiplication of the vector norms.
|
|
let q_dist = n.dot(q);
|
|
|
|
// generate output
|
|
let after_end = q_dist > line_vec_length;
|
|
let line_pos = Self::new(q_dist, qp_length, after_end);
|
|
|
|
Ok(line_pos)
|
|
}
|
|
|
|
/// Determines `point` position relative to nodes of an ingress `Lane`
|
|
///
|
|
/// Keep in mind, that the lane's node list starts at the stop line and is extending away from the intersection.
|
|
/// But we are driving the other way around!
|
|
///
|
|
/// # Errors
|
|
/// Returns a human-readable string if position can't be determined
|
|
pub fn for_ingress_lane(
|
|
point: geo_types::Point,
|
|
nodes: &geo_types::LineString,
|
|
) -> Result<Self, String> {
|
|
let mut result = Self::default();
|
|
|
|
let mut lines = nodes.lines();
|
|
|
|
// check, that the stop line is not behind us
|
|
let line = lines
|
|
.next()
|
|
.ok_or("Lane has less than two nodes".to_string())?;
|
|
|
|
let pos = Self::for_ls(point, &line)?;
|
|
if pos.semantic == LinePositionType::BeforeStart {
|
|
// we have crossed the stop line -> abort search for position inside the lane
|
|
return Ok(pos);
|
|
}
|
|
if pos.semantic == LinePositionType::InBetween {
|
|
// we're at this segment -> search finished
|
|
return Ok(pos);
|
|
}
|
|
// otherwise: continue with next segment (and store this segment's length)
|
|
let mut lane_length = cartesian_norm(line.delta().into());
|
|
let mut last_pos = LinePosition::default();
|
|
|
|
// continue with remaining lines
|
|
for line in lines {
|
|
let line_len = cartesian_norm(line.delta().into());
|
|
|
|
// ignored line, if points are too close <10cm
|
|
if line_len < 0.1 {
|
|
warn!("Lane nodes are just {line_len:.1} m apart -> ignoring");
|
|
continue;
|
|
}
|
|
|
|
let pos = Self::for_ls(point, &line)?;
|
|
match pos.semantic {
|
|
LinePositionType::BeforeStart => {
|
|
// we're at the outer arc of the lane right at the border between line segments
|
|
// -> set parallel distance to zero and finish search
|
|
return Ok(LinePosition::new(
|
|
lane_length,
|
|
pos.perpendicular_dist,
|
|
false,
|
|
));
|
|
}
|
|
LinePositionType::InBetween => {
|
|
// we're at this segment
|
|
// -> add segment's distance to accumulated lane length and finish search
|
|
return Ok(LinePosition::new(
|
|
lane_length + pos.parallel_dist,
|
|
pos.perpendicular_dist,
|
|
false,
|
|
));
|
|
}
|
|
_ => {
|
|
// continue searching, add segment's distance to lane length accumulator
|
|
lane_length += line_len;
|
|
last_pos = pos;
|
|
}
|
|
}
|
|
}
|
|
|
|
// if we came this far, we're after the lane's end (actually the start in driving direction)
|
|
// -> add parallel distance to accumulated lane length
|
|
Ok(LinePosition::new(
|
|
lane_length + last_pos.parallel_dist,
|
|
last_pos.perpendicular_dist,
|
|
true,
|
|
))
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use crate::geo_alg::{LinePosition, cartesian_bearing};
|
|
|
|
#[test]
|
|
fn test_cart_bearing() {
|
|
let start = geo_types::Point::new(0., 0.);
|
|
|
|
assert_float_eq::assert_float_absolute_eq!(
|
|
0.,
|
|
cartesian_bearing(start, geo_types::Point::new(10., 0.))
|
|
);
|
|
|
|
assert_float_eq::assert_float_absolute_eq!(
|
|
90.,
|
|
cartesian_bearing(start, geo_types::Point::new(0., 10.))
|
|
);
|
|
|
|
assert_float_eq::assert_float_absolute_eq!(
|
|
45.,
|
|
cartesian_bearing(start, geo_types::Point::new(10., 10.))
|
|
);
|
|
|
|
assert_float_eq::assert_float_absolute_eq!(
|
|
135.,
|
|
cartesian_bearing(start, geo_types::Point::new(-10., 10.))
|
|
);
|
|
|
|
assert_float_eq::assert_float_absolute_eq!(
|
|
-135.,
|
|
cartesian_bearing(start, geo_types::Point::new(-10., -10.))
|
|
);
|
|
}
|
|
|
|
#[test]
|
|
fn linepos_for_ls_validation() {
|
|
// identical nodes shall result in an error
|
|
|
|
let point = geo_types::Point::new(0., 0.);
|
|
let node1 = geo_types::Point::new(0., 0.);
|
|
let node2 = geo_types::Point::new(10., -5.);
|
|
|
|
let ls = geo_types::Line::new(node1, node1);
|
|
assert!(LinePosition::for_ls(point, &ls).is_err());
|
|
|
|
let ls = geo_types::Line::new(node2, node2);
|
|
assert!(LinePosition::for_ls(point, &ls).is_err());
|
|
}
|
|
|
|
#[test]
|
|
fn linepos_for_ls_test() {
|
|
use crate::geo_alg::LinePositionType;
|
|
|
|
// test on X axis
|
|
{
|
|
let node1 = geo_types::Point::new(0., 0.);
|
|
let node2 = geo_types::Point::new(5., 0.);
|
|
|
|
let ls = geo_types::Line::new(node1, node2);
|
|
|
|
// test some positions
|
|
let test_tuples = vec![
|
|
(node1.x() - 1000., LinePositionType::BeforeStart),
|
|
(node1.x() - 1., LinePositionType::BeforeStart),
|
|
(node1.x(), LinePositionType::InBetween),
|
|
(node1.x() + 1., LinePositionType::InBetween),
|
|
(node2.x() - 1., LinePositionType::InBetween),
|
|
(node2.x(), LinePositionType::InBetween),
|
|
(node2.x() + 1., LinePositionType::AfterEnd),
|
|
(node2.x() + 1000., LinePositionType::AfterEnd),
|
|
];
|
|
let y_values = vec![(node1.y() - 1.), (node1.y()), (node1.y() + 1.)];
|
|
|
|
// y position should only affect the perpendicular distance
|
|
for y in &y_values {
|
|
// x position affects the position type and parallel distance
|
|
for (x, semantic) in &test_tuples {
|
|
let point = geo_types::Point::new(*x, *y);
|
|
|
|
let result = LinePosition::for_ls(point, &ls).unwrap();
|
|
|
|
assert_eq!(semantic, &result.semantic);
|
|
assert_eq!((y - node1.y()).abs(), result.perpendicular_dist);
|
|
assert_eq!(x - node1.x(), result.parallel_dist);
|
|
}
|
|
}
|
|
}
|
|
|
|
// test on Y axis
|
|
{
|
|
let node1 = geo_types::Point::new(3., 5.);
|
|
let node2 = geo_types::Point::new(3., 100.);
|
|
|
|
let ls = geo_types::Line::new(node1, node2);
|
|
|
|
// test some positions
|
|
let test_tuples = vec![
|
|
(node1.y() - 1000., LinePositionType::BeforeStart),
|
|
(node1.y() - 1., LinePositionType::BeforeStart),
|
|
(node1.y(), LinePositionType::InBetween),
|
|
(node1.y() + 1., LinePositionType::InBetween),
|
|
(node2.y() - 1., LinePositionType::InBetween),
|
|
(node2.y(), LinePositionType::InBetween),
|
|
(node2.y() + 1., LinePositionType::AfterEnd),
|
|
(node2.y() + 1000., LinePositionType::AfterEnd),
|
|
];
|
|
let x_values = vec![(node1.x() - 1.), (node1.x()), (node1.x() + 1.)];
|
|
|
|
// x position should only affect the perpendicular distance
|
|
for x in &x_values {
|
|
// y position affects the position type and parallel distance
|
|
for (y, semantic) in &test_tuples {
|
|
let point = geo_types::Point::new(*x, *y);
|
|
|
|
let result = LinePosition::for_ls(point, &ls).unwrap();
|
|
|
|
assert_eq!(semantic, &result.semantic);
|
|
assert_eq!((x - node1.x()).abs(), result.perpendicular_dist);
|
|
assert_eq!(y - node1.y(), result.parallel_dist);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|