jtbx/esp32-c_its-companion
0
0
Fork 0
Code Activity

Evaluate remaining red/green time for upcoming intersection

Browse source
This commit is contained in:
Jannik Beyerstedt 2026-05-30 23:34:06 +02:00
commit 7463adb038
12 changed files with 2979 additions and 103 deletions
Download patch file Download diff file Expand all files Collapse all files

446
src/geo_alg.rs Normal file
View file

@ -0,0 +1,446 @@
//! 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);
}
}
}
}
}