added current code

src/gridgraph.rs

@@ -0,0 +1,682 @@
+use crate::coordinates::{DegreeCoordinate, RadianCoordinate};
+use crate::polygonset::PolygonSet;
+use geojson::{Feature, FeatureCollection, Geometry, Position, Value};
+use serde::ser::{SerializeStruct, Serializer};
+use std::cmp::Ordering;
+use std::collections::{BinaryHeap, HashMap};
+use std::f64::consts::{FRAC_PI_2, PI, TAU};
+use std::fs::File;
+use std::io::{BufRead, BufReader, Write};
+use std::vec::Vec;
+use crate::utils::EARTH_RADIUS;
+use rand::seq::SliceRandom;
+use serde::{Serialize, Deserialize};
+
+/// Type for all edge costs
+/// Allows for easy adjustments for bigger/smaller number types if that is
+/// neccessary/sufficient.
+/// All distance measurements on the grid should use this type.
+pub type EdgeCost = u64;
+
+pub type NodeId = usize;
+
+#[derive(Debug, Clone, Default)]
+pub struct GridGraph {
+    pub nodes: Vec<GraphNode>,
+    pub edges: Vec<GraphEdge>,
+    pub edge_offsets: Vec<usize>,
+    lookup_grid: LookupGrid,
+}
+
+#[derive(Debug, Clone, Default)]
+pub struct LookupGrid {
+    lat_size: usize,
+    lon_size: usize,
+    grid: Vec<Vec<Vec<NodeId>>>,
+}
+
+#[derive(Debug, Copy, Clone, PartialEq, Serialize, Deserialize)]
+pub struct GraphNode {
+    pub index: u32,
+    pub position: RadianCoordinate,
+}
+
+#[derive(Debug, Copy, Clone, PartialEq)]
+pub struct GraphEdge {
+    // index of the neighbor in the data structure
+    pub neighbor: u32,
+    pub cost: EdgeCost,
+}
+
+#[derive(Debug)]
+pub enum FmiParsingError {
+    FormatError(String),
+    NotAnInteger(String),
+    NotAFloat(String),
+    WrongNodeAmount,
+    WrongEdgeAmount,
+}
+
+#[derive(Debug)]
+pub struct Route {
+    pub nodes: Vec<RadianCoordinate>,
+    pub cost: EdgeCost,
+}
+
+impl Serialize for Route {
+    fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
+    where
+        S: Serializer,
+    {
+        let mut state = serializer.serialize_struct("Route", 2)?;
+        state.serialize_field("geojson", &self.to_geojson())?;
+        state.serialize_field("cost", &self.cost)?;
+        state.end()
+    }
+}
+
+impl LookupGrid {
+    /// returns the latitude index and longitude index of a position
+    pub fn get_grid_cell(&self, position: RadianCoordinate) -> (usize, usize) {
+        let lat_index = ((position.lat + 90.0) / 180.0 * self.lat_size as f64).floor() as usize;
+        let lon_index = ((position.lon + 180.0) / 360.0 * self.lon_size as f64).floor() as usize;
+
+        (lat_index, lon_index)
+    }
+
+    /// inserts the nodes in to the grid
+    pub fn insert_nodes(&mut self, nodes: &[GraphNode]) {
+        for node in nodes.iter() {
+            let (lat, lon) = self.get_grid_cell(node.position);
+
+            self.grid[lat][lon].push(node.index as usize);
+        }
+    }
+
+    /// creates a new lookup grid with the given size and adds the given nodes
+    pub fn new(lat_size: usize, lon_size: usize, nodes: &[GraphNode]) -> LookupGrid {
+        let mut instance = LookupGrid {
+            lat_size,
+            lon_size,
+            grid: vec![vec!(Vec::new(); lon_size); lat_size],
+        };
+        instance.insert_nodes(nodes);
+
+        instance
+    }
+}
+
+impl Route {
+    pub fn to_geojson(&self) -> Box<FeatureCollection> {
+        let mut features: Box<FeatureCollection> = Box::new(FeatureCollection {
+            bbox: None,
+            features: vec![],
+            foreign_members: None,
+        });
+        let mut points = geojson::LineStringType::new();
+
+        for node in self.nodes.iter() {
+            let position: Position = (*node).into();
+            points.push(position);
+        }
+        let geometry = Geometry::new(Value::LineString(points));
+
+        features.features.push(Feature {
+            bbox: None,
+            geometry: Some(geometry),
+            id: None,
+            properties: None,
+            foreign_members: None,
+        });
+
+        features
+    }
+}
+
+impl GridGraph {
+
+
+    /// selects a single random graph node.
+    pub fn get_random_node(&self) -> Option<&GraphNode> {
+        let mut rng = rand::thread_rng();
+
+        self.nodes.choose(&mut rng)
+    }
+
+
+    /// selects up to `amount` random but distinct nodes from the graph.
+    pub fn get_random_nodes(&self, amount: usize) ->  Vec<&GraphNode>{
+
+        let mut rng = rand::thread_rng();
+
+        self.nodes.choose_multiple(&mut rng, amount).collect()
+
+    }
+
+    /// calculates the shortest path from the start node to the end node.
+    /// start and end nodes have to be references to the nodes contained in the
+    /// grid graph.
+    /// `start` and `end` have to be different nodes.
+    /// Returns the route.
+    pub fn shortest_path(&self, start: &GraphNode, end: &GraphNode) -> Option<Route> {
+        #[derive(Eq, PartialEq, Debug)]
+        struct DijkstraElement {
+            index: u32,
+            cost: EdgeCost,
+        }
+
+        impl Ord for DijkstraElement {
+            // inverted cmp function, such that the Max-Heap provided by Rust
+            // can be used as a Min-Heap
+            fn cmp(&self, other: &Self) -> Ordering {
+                other.cost.cmp(&self.cost).then_with(|| self.index.cmp(&other.index))
+            }
+        }
+
+        impl PartialOrd for DijkstraElement {
+            fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
+                Some(self.cmp(other))
+            }
+        }
+
+        let mut heap = BinaryHeap::new();
+        heap.push(DijkstraElement {
+            cost: 0,
+            index: start.index,
+        });
+
+        let mut distance = vec![EdgeCost::MAX; self.nodes.len()];
+        let mut ancestor: Vec<Option<u32>> = vec![None; self.nodes.len()];
+
+        let mut popcount = 0;
+
+        while let Some(DijkstraElement {
+            index,
+            cost,
+        }) = heap.pop()
+        {
+            popcount += 1;
+
+            if index == end.index {
+                // println!("found the end");
+                break;
+            }
+            // the heap does not support "update" operations, so we
+            // insert elements again and if they come out of the heap but have
+            // been processed earlier we simply skip them.
+            if cost > distance[index as usize] {
+                //println!("skipping {} because of cost {}, it can be reached with {}", index, cost, distance[index as usize]);
+                continue;
+            };
+
+            // println!("working on node {} with cost {}", index, cost);
+
+            let edge_start = self.edge_offsets[index as usize] as usize;
+            let edge_end = self.edge_offsets[(index + 1) as usize] as usize;
+
+            for edge in self.edges[edge_start..edge_end].iter() {
+                let new_cost = cost + edge.cost;
+
+                if new_cost < distance[edge.neighbor as usize] {
+                    // println!("found cheaper edge {:?} with cost {} than previous best {}", edge, new_cost, distance[edge.neighbor as usize]);
+                    distance[edge.neighbor as usize] = new_cost;
+                    ancestor[edge.neighbor as usize] = Some(index);
+                    //println!("adding new element to heap");
+                    heap.push(DijkstraElement {
+                        index: edge.neighbor,
+                        cost: new_cost,
+                    });
+                } else {
+                    // println!("edge {:?} is more expensive ({}) than the previous best {}", edge, new_cost, distance[edge.neighbor as usize]);
+                }
+            }
+        }
+
+        println!("popped {} elements from the heap", popcount);
+
+        // now the route calculation is done. If a route exist we can construct
+        // it from the ancestors.
+        if ancestor[end.index as usize].is_some() {
+            let mut route = Route {
+                cost: distance[end.index as usize],
+                nodes: Vec::new(),
+            };
+
+            let mut current = end.index;
+            while current != start.index {
+                route.nodes.push(self.nodes[current as usize].position);
+                current = ancestor[current as usize].unwrap();
+            }
+            route.nodes.push(self.nodes[current as usize].position);
+
+            route.nodes.reverse();
+
+            return Some(route);
+        }
+
+        None
+    }
+
+    /// returns the GraphNode nearest to that positon.
+    /// There might be cornercases where other nodes are actually closer due
+    /// to the spherical projection.
+    /// There also might be cases where no neares position is found because
+    /// the search only works on a 3x3 grid around the position.
+    pub fn get_nearest_node(&self, position: RadianCoordinate) -> Option<&GraphNode> {
+        let (lat, lon) = self.lookup_grid.get_grid_cell(position);
+        let lat = lat as isize;
+        let lon = lon as isize;
+
+        let lookup_distance = 1;
+        let mut best_node: Option<&GraphNode> = None;
+        let mut best_distance = f64::MAX;
+
+        // iterate over the grid and handle the wrap-aroung at 180 degrees
+        for lat_index in (lat - lookup_distance)..(lat + lookup_distance + 1) {
+            let lat_index = lat_index.rem_euclid(self.lookup_grid.lat_size as isize) as usize;
+            for lon_index in (lon - lookup_distance)..(lon + lookup_distance + 1) {
+                let lon_index = lon_index.rem_euclid(self.lookup_grid.lon_size as isize) as usize;
+
+                // actual check of the grid nodes in the cell
+                for node_id in self.lookup_grid.grid[lat_index][lon_index].iter() {
+                    let node = &(self.nodes[*node_id as usize]);
+                    let dist = node.position.distance_to(&position);
+                    if dist < best_distance {
+                        best_node = Some(node);
+                        best_distance = dist;
+                    }
+                }
+            }
+        }
+
+        best_node
+    }
+
+    /// reads a graph from an FMI file.
+    pub fn from_fmi_file(file: File) -> Result<Box<GridGraph>, FmiParsingError> {
+        let mut gridgraph = Box::new(GridGraph::default());
+
+        let reader = BufReader::new(file);
+
+        let lines = reader.lines();
+
+        let mut total_node_count: u32 = 0;
+        let mut total_edge_count: u32 = 0;
+        let mut node_count: u32 = 0;
+        let mut edge_count: u32 = 0;
+
+        let mut nodes: HashMap<u32, GraphNode> = HashMap::new();
+        let mut edges: HashMap<u32, Vec<TemporaryGraphEdge>> = HashMap::new();
+
+        enum ParserState {
+            NodeCount,
+            EdgeCount,
+            Nodes,
+            Edges,
+            Done,
+        }
+
+        struct TemporaryGraphEdge {
+            src: u32,
+            dst: u32,
+            cost: EdgeCost,
+        }
+
+        fn parse_int(line: &str) -> Result<u32, FmiParsingError> {
+            match line.parse::<u32>() {
+                Ok(result) => Ok(result),
+                Err(_) => Err(FmiParsingError::NotAnInteger(format!(
+                    "Line is not an integer: '{}'",
+                    line
+                ))),
+            }
+        }
+        fn parse_float(line: &str) -> Result<f64, FmiParsingError> {
+            match line.parse::<f64>() {
+                Ok(result) => Ok(result),
+                Err(_) => Err(FmiParsingError::NotAFloat(format!(
+                    "Line is not an float: '{}'",
+                    line
+                ))),
+            }
+        }
+
+        fn parse_node(line: &str) -> Result<GraphNode, FmiParsingError> {
+            let splits = line.split(" ").collect::<Vec<&str>>();
+            if splits.len() < 3 {
+                return Err(FmiParsingError::FormatError(format!(
+                    "Line '{}' does not have at least 3 parts",
+                    line
+                )));
+            }
+
+            let index = parse_int(splits[0])?;
+            let lat = parse_float(splits[1])?;
+            let lon = parse_float(splits[2])?;
+
+            Ok(GraphNode {
+                index,
+                position: RadianCoordinate::from_degrees(lat, lon),
+            })
+        }
+
+        fn parse_edge(line: &str) -> Result<TemporaryGraphEdge, FmiParsingError> {
+            let splits = line.split(" ").collect::<Vec<&str>>();
+            if splits.len() < 3 {
+                return Err(FmiParsingError::FormatError(format!(
+                    "Line '{}' does not have at least 3 parts",
+                    line
+                )));
+            }
+
+            let src = parse_int(splits[0])?;
+            let dst = parse_int(splits[1])?;
+            let cost = parse_int(splits[2])? as EdgeCost;
+
+            Ok(TemporaryGraphEdge { src, dst, cost })
+        }
+
+        let mut state = ParserState::NodeCount {};
+
+        for line in lines {
+            let line = line.unwrap();
+            let line = line.trim();
+            if line.is_empty() || line.starts_with("#") {
+                continue;
+            }
+
+            match state {
+                ParserState::NodeCount => {
+                    total_node_count = parse_int(line)?;
+                    state = ParserState::EdgeCount {};
+                }
+                ParserState::EdgeCount => {
+                    total_edge_count = parse_int(line)?;
+                    state = ParserState::Nodes {};
+                }
+                ParserState::Nodes => {
+                    let node = parse_node(line)?;
+                    nodes.entry(node.index).or_insert(node);
+                    node_count += 1;
+                    if node_count == total_node_count {
+                        state = ParserState::Edges {};
+                    }
+                }
+                ParserState::Edges => {
+                    let edge = parse_edge(line)?;
+                    edges.entry(edge.src).or_default().push(edge);
+                    edge_count += 1;
+                    if edge_count == total_edge_count {
+                        state = ParserState::Done;
+                    }
+                }
+                ParserState::Done => {
+                    break;
+                }
+            }
+        }
+
+        if node_count != total_node_count {
+            return Err(FmiParsingError::WrongNodeAmount);
+        }
+        if edge_count != total_edge_count {
+            return Err(FmiParsingError::WrongEdgeAmount);
+        }
+
+        // add the nodes to the gridgraph
+        for (_, mut item) in nodes.iter_mut() {
+            item.index = gridgraph.nodes.len() as u32;
+            gridgraph.nodes.push(item.clone());
+        }
+
+        // add the edges
+        gridgraph.edge_offsets.push(0);
+        for index in nodes.keys() {
+            for edge in edges.entry(*index).or_default() {
+                if !nodes.contains_key(&edge.dst) {
+                    println!(
+                        "An edge refers to the non-existing destination node {} skipping.",
+                        edge.dst
+                    );
+                    continue;
+                }
+                let dst_index = nodes[&edge.dst].index;
+                gridgraph.edges.push(GraphEdge {
+                    cost: edge.cost,
+                    neighbor: dst_index,
+                });
+            }
+            gridgraph.edge_offsets.push(gridgraph.edges.len());
+        }
+
+        gridgraph.lookup_grid = LookupGrid::new(100, 100, &gridgraph.nodes);
+
+        Ok(gridgraph)
+    }
+
+    pub fn write_fmi_file(&self, mut file: File) -> std::io::Result<()> {
+        writeln!(file, "{}", self.nodes.len())?;
+        writeln!(file, "{}", self.edges.len())?;
+        for node in self.nodes.iter() {
+            let deg_pos = DegreeCoordinate::from(node.position);
+            writeln!(file, "{} {} {}", node.index, deg_pos.lat, deg_pos.lon,)?;
+        }
+
+        for node in self.nodes.iter() {
+            for edge in self.get_edges(node.index as NodeId) {
+                let neighbor = self.nodes[edge.neighbor as usize];
+                writeln!(file, "{} {} {}", node.index, neighbor.index, edge.cost)?;
+            }
+        }
+
+        Ok(())
+    }
+
+    pub fn get_edges(&self, node: NodeId) -> &[GraphEdge] {
+        let start = self.edge_offsets[node];
+        let end = self.edge_offsets[node + 1];
+
+        &self.edges[start..end]
+    }
+
+    /// generates a regular grid over the globe with the given grid size.
+    pub fn generate_regular_grid(
+        lat_resolution: usize,
+        lon_resolution: usize,
+        polygons: Option<&PolygonSet>,
+    ) -> Box<GridGraph> {
+        
+        #[derive(Debug)]
+        struct TemporaryGraphNode {
+            final_index: u32,
+            position: RadianCoordinate,
+            lat_index: usize,
+            lon_index: usize,
+
+            // specifies whether or not the node will be in the final graph or
+            // not, because it might be on land.
+            used: bool,
+        }
+
+        impl TemporaryGraphNode {
+            pub fn left_neighbor_index(&self, lon_resolution: usize) -> usize {
+                let left_lon_index =
+                    (self.lon_index as isize - 1).rem_euclid(lon_resolution as isize) as usize;
+                left_lon_index + (self.lat_index * lon_resolution)
+            }
+
+            pub fn right_neighbor_index(&self, lon_resolution: usize) -> usize {
+                let left_lon_index = (self.lon_index + 1).rem_euclid(lon_resolution);
+                left_lon_index + (self.lat_index * lon_resolution)
+            }
+
+            pub fn bottom_neighbor_index(&self, lon_resolution: usize) -> Option<usize> {
+                if self.lat_index <= 0 {
+                    return None;
+                };
+
+                Some(self.lon_index + (self.lat_index - 1) * lon_resolution)
+            }
+
+            pub fn top_neighbor_index(&self, lat_resolution: usize, lon_resolution: usize) -> Option<usize> {
+                if self.lat_index >= lat_resolution - 1 {
+                    return None;
+                };
+
+                Some(self.lon_index + (self.lat_index + 1) * lon_resolution)
+            }
+        }
+
+        let mut temp_nodes = Vec::new();
+
+        for lat_index in 0..lat_resolution {
+            let lat = ((lat_index as f64 / lat_resolution as f64) * PI) - FRAC_PI_2;
+
+            for lon_index in 0..lon_resolution {
+                let lon = ((lon_index as f64 / lon_resolution as f64) * TAU) - PI;
+
+                let position = RadianCoordinate { lat, lon };
+
+                let used = match polygons {
+                    None => true,
+                    Some(p) => !p.in_any(position),
+                };
+
+                temp_nodes.push(TemporaryGraphNode {
+                    final_index: 0,
+                    position,
+                    used,
+                    lat_index,
+                    lon_index,
+                })
+            }
+        }
+
+        let mut final_graph: Box<GridGraph> = Box::new(GridGraph::default());
+
+        // add all the nodes to the final graph
+        for mut node in temp_nodes.iter_mut() {
+            if !node.used {
+                continue;
+            };
+
+            let new_node = GraphNode {
+                index: final_graph.nodes.len() as u32,
+                position: node.position,
+            };
+
+            node.final_index = new_node.index;
+
+            final_graph.nodes.push(new_node);
+        }
+
+        // add all edges to the final graph
+        for node in temp_nodes.iter() {
+
+            println!("working on edges for node {:?}", node);
+            if !node.used {
+                continue;
+            };
+
+            final_graph.edge_offsets.push(final_graph.edges.len());
+
+            fn add_neighbor(
+                node: &TemporaryGraphNode,
+                neighbor: &TemporaryGraphNode,
+                grid: &mut GridGraph,
+            ) {
+                if neighbor.used {
+                    grid.edges.push(GraphEdge {
+                        neighbor: neighbor.final_index,
+                        cost: (node.position.distance_to(&(neighbor.position)) * EARTH_RADIUS)
+                            as EdgeCost,
+                    });
+                }
+            }
+
+            // add neighbors
+            let left_neighbor = &temp_nodes[node.left_neighbor_index(lon_resolution)];
+
+            add_neighbor(&node, &left_neighbor, &mut final_graph);
+            let right_neighbor = &temp_nodes[node.right_neighbor_index(lon_resolution)];
+            add_neighbor(&node, &right_neighbor, &mut final_graph);
+
+            let top_index = node.top_neighbor_index(lat_resolution, lon_resolution);
+            match top_index {
+                Some(index) => {
+                    println!("top neighbor is {}", index);
+                    add_neighbor(&node, &temp_nodes[index], &mut final_graph);
+                }
+                None => (),
+            }
+
+            let bottom_index = node.bottom_neighbor_index(lon_resolution);
+            match bottom_index {
+                Some(index) => {
+                    add_neighbor(&node, &temp_nodes[index], &mut final_graph);
+                }
+                None => (),
+            }
+        }
+
+        // pushing the end offset
+        final_graph.edge_offsets.push(final_graph.edges.len());
+
+        // making the cells of the lookup grid 4 times the size of the regular
+        // grid is a heuristic that worked well.
+        final_graph.lookup_grid = LookupGrid::new(
+            (lat_resolution / 4).max(1),
+            (lon_resolution / 4).max(1),
+            &final_graph.nodes,
+        );
+
+        final_graph
+    }
+
+    /// Returns a GeoJSON representation of the grid.
+    /// This gets very large very soon and might be hard to render.
+    pub fn to_geojson(&self) -> Box<FeatureCollection> {
+        let mut features: Box<FeatureCollection> = Box::new(FeatureCollection {
+            bbox: None,
+            features: vec![],
+            foreign_members: None,
+        });
+
+        for node in self.nodes.iter() {
+            let value = Value::from(node.position);
+
+            features.features.push(Feature::from(value));
+        }
+
+        for node in self.nodes.iter() {
+            for edge_index in
+                self.edge_offsets[node.index as usize]..self.edge_offsets[(node.index + 1) as usize]
+            {
+                let edge = self.edges[edge_index as usize];
+
+                // the edges are stored as directed edges, but we only want to
+                // draw one of them
+                if node.index < edge.neighbor {
+                    continue;
+                }
+
+                let neighbor = self.nodes[edge.neighbor as usize];
+
+                let mut points = geojson::LineStringType::new();
+                points.push(Position::from(node.position));
+                points.push(Position::from(neighbor.position));
+
+                let geometry = Geometry::new(Value::LineString(points));
+                features.features.push(Feature {
+                    bbox: None,
+                    geometry: Some(geometry),
+                    id: None,
+                    properties: None,
+                    foreign_members: None,
+                })
+            }
+        }
+
+        features
+    }
+}