Astar Flight Route

astar_flight_route.cpp

#include <iostream>
#include <vector>
#include <queue>
#include <unordered_map>
#include <unordered_set>
#include <cmath>
#include <limits>
#include <tuple>
#include <set>
#include <algorithm>
#include <map>
#include <cassert>
#include <chrono>
#include <array>

#include <nanoflann.hpp>

constexpr double EARTH_RADIUS_KM = 6371.0;

struct Node {
    int id;
    double lat;
    double lon;
    bool is_airport = false;
};

struct Edge {
    int to;
    double distance;
};

class RouteGraph {
public:
    /**
 * Add a node to the graph.
 * @param id Unique node identifier.
 * @param lat Latitude in degrees.
 * @param lon Longitude in degrees.
 * @param is_airport Whether the node is an airport or a waypoint.
 */
void add_node(int id, double lat, double lon, bool is_airport = false) {
        Node node{id, lat, lon, is_airport};
        nodes_[id] = node;
        double rlat = deg2rad(lat);
        double rlon = deg2rad(lon);
        double x = cos(rlat) * cos(rlon);
        double y = cos(rlat) * sin(rlon);
        double z = sin(rlat);
        node_coords_.push_back({x, y, z});
        id_index_map_.push_back(id);
    }

    
                }
            }
        }
    }

    void disable_node(int id) { disabled_nodes_.insert(id); }
    void enable_node(int id) { disabled_nodes_.erase(id); }

    template <typename Iterable>
    void disable_nodes(const Iterable& ids) {
        for (const auto& id : ids) disabled_nodes_.insert(id);
    }
    template <typename Iterable>
    void enable_nodes(const Iterable& ids) {
        for (const auto& id : ids) disabled_nodes_.erase(id);
    }

    void disable_nodes_in_radius(double lat, double lon, double radius_km) {
        for (const auto& [id, node] : nodes_) {
            double d = haversine(lat, lon, node.lat, node.lon);
            if (d <= radius_km) disabled_nodes_.insert(id);
        }
    }

    void disable_nodes_in_rectangle(double lat0, double lon0, double lat1, double lon1) {
        double top = std::max(lat0, lat1);
        double bottom = std::min(lat0, lat1);
        double left = std::min(lon0, lon1);
        double right = std::max(lon0, lon1);
        for (const auto& [id, node] : nodes_) {
            if (node.lat <= top && node.lat >= bottom && node.lon >= left && node.lon <= right) {
                disabled_nodes_.insert(id);
            }
        }
    }

    std::vector<std::pair<int, double>> astar(int start_id, int goal_id) const {
        struct QueueItem {
            int node;
            double priority;
            bool operator>(const QueueItem& other) const {
                return priority > other.priority;
            }
        };

        std::priority_queue<QueueItem, std::vector<QueueItem>, std::greater<>> frontier;
        frontier.push({start_id, 0.0});

        std::unordered_map<int, std::pair<int, double>> came_from;
        std::unordered_map<int, double> cost_so_far;
        std::unordered_set<int> visited;
        cost_so_far[start_id] = 0.0;

        while (!frontier.empty()) {
            while (!frontier.empty() && disabled_nodes_.count(frontier.top().node)) {
                frontier.pop();
            }
            if (frontier.empty()) break;

            int current = frontier.top().node;
        frontier.pop();

        if (visited.count(current)) continue;
        visited.insert(current);

            if (current == goal_id) break;

            for (const auto& edge : graph_.at(current)) {
                if (disabled_nodes_.count(edge.to)) continue;
                // disallow visiting intermediate airports
                if (edge.to != goal_id && nodes_.at(edge.to).is_airport) continue;
                double new_cost = cost_so_far[current] + edge.distance;
                if (!cost_so_far.count(edge.to) || new_cost < cost_so_far[edge.to]) {
                    cost_so_far[edge.to] = new_cost;
                    double priority = new_cost + haversine(
                        nodes_.at(edge.to).lat, nodes_.at(edge.to).lon,
                        nodes_.at(goal_id).lat, nodes_.at(goal_id).lon);
                    frontier.push({edge.to, priority});
                    came_from[edge.to] = {current, edge.distance};
                }
            }
        }

        std::vector<std::pair<int, double>> path;
        if (!came_from.count(goal_id)) return path;

        int current = goal_id;
        while (came_from.count(current)) {
            auto [prev, dist] = came_from.at(current);
            path.emplace_back(current, dist);
            current = prev;
        }
        path.emplace_back(start_id, 0.0);
        std::reverse(path.begin(), path.end());
        return path;
    }

void build_graph(double max_connection_km, int k_neighbors, bool bidirectional) {
        using KDTree = nanoflann::KDTreeSingleIndexAdaptor<
            nanoflann::L2_Simple_Adaptor<double, RouteGraph>,
            RouteGraph, 3, size_t>;

        KDTree tree(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams());
        tree.buildIndex();

        for (size_t i = 0; i < node_coords_.size(); ++i) {
            std::vector<size_t> ret_indexes(k_neighbors);
            std::vector<double> out_dists_sq(k_neighbors);

            double query_pt[3] = {node_coords_[i][0], node_coords_[i][1], node_coords_[i][2]};
            size_t found = tree.knnSearch(query_pt, k_neighbors, ret_indexes.data(), out_dists_sq.data());

            int from_id = id_index_map_[i];
            for (size_t j = 0; j < found; ++j) {
                int to_id = id_index_map_[ret_indexes[j]];
                if (from_id != to_id) {
                    double dist_km = haversine(
                        nodes_.at(from_id).lat, nodes_.at(from_id).lon,
                        nodes_.at(to_id).lat, nodes_.at(to_id).lon);
                    if (dist_km <= max_connection_km) {
                        graph_[from_id].push_back({to_id, dist_km});
                        if (bidirectional) {
                            graph_[to_id].push_back({from_id, dist_km});
                        }
                    }
                }
            }
        }
    }

public:
    void build_graph_directional(double max_connection_km, int k_neighbors = 30, int sectors = 8, bool bidirectional = true) {
        using KDTree = nanoflann::KDTreeSingleIndexAdaptor<
            nanoflann::L2_Simple_Adaptor<double, RouteGraph>,
            RouteGraph, 3, size_t>;

        KDTree tree(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams());
        tree.buildIndex();

        for (size_t i = 0; i < node_coords_.size(); ++i) {
            std::vector<size_t> ret_indexes(k_neighbors);
            std::vector<double> out_dists_sq(k_neighbors);
            double query_pt[3] = {node_coords_[i][0], node_coords_[i][1], node_coords_[i][2]};
            size_t found = tree.knnSearch(query_pt, k_neighbors, ret_indexes.data(), out_dists_sq.data());

            int from_id = id_index_map_[i];
            std::map<int, std::pair<int, double>> sector_best;

            for (size_t j = 0; j < found; ++j) {
                int to_id = id_index_map_[ret_indexes[j]];
                if (from_id == to_id) continue;

                double bearing = compute_bearing(
                    nodes_.at(from_id).lat, nodes_.at(from_id).lon,
                    nodes_.at(to_id).lat, nodes_.at(to_id).lon);

                int sector = static_cast<int>(std::floor(bearing / (360.0 / sectors))) % sectors;

                double dist_km = haversine(
                    nodes_.at(from_id).lat, nodes_.at(from_id).lon,
                    nodes_.at(to_id).lat, nodes_.at(to_id).lon);
                if (dist_km > max_connection_km) continue;

                if (!sector_best.count(sector) || dist_km < sector_best[sector].second) {
                    sector_best[sector] = {to_id, dist_km};
                }
            }

            for (const auto& [sector, pair] : sector_best) {
                int to_id = pair.first;
                double dist_km = pair.second;
                graph_[from_id].push_back({to_id, dist_km});
                if (bidirectional) {
                    graph_[to_id].push_back({from_id, dist_km});
                }
            }
        }
    }

private:
    inline size_t kdtree_get_point_count() const { return node_coords_.size(); }
    inline double kdtree_get_pt(const size_t idx, const size_t dim) const { return node_coords_[idx][dim]; }
    template <class BBOX> bool kdtree_get_bbox(BBOX&) const { return false; }

    static double deg2rad(double deg) { return deg * M_PI / 180.0; }
    static double compute_bearing(double lat1, double lon1, double lat2, double lon2) {
        double phi1 = deg2rad(lat1);
        double phi2 = deg2rad(lat2);
        double dlon = deg2rad(lon2 - lon1);
        double y = sin(dlon) * cos(phi2);
        double x = cos(phi1) * sin(phi2) - sin(phi1) * cos(phi2) * cos(dlon);
        double bearing = atan2(y, x) * 180.0 / M_PI;
        return fmod((bearing + 360.0), 360.0);
    }

    static double haversine(double lat1, double lon1, double lat2, double lon2) {
        double dlat = deg2rad(lat2 - lat1);
        double dlon = deg2rad(lon2 - lon1);
        lat1 = deg2rad(lat1);
        lat2 = deg2rad(lat2);
        double a = sin(dlat / 2) * sin(dlat / 2) +
                   cos(lat1) * cos(lat2) * sin(dlon / 2) * sin(dlon / 2);
        double c = 2 * atan2(sqrt(a), sqrt(1 - a));
        return EARTH_RADIUS_KM * c;
    }

    std::unordered_map<int, Node> nodes_;
    std::unordered_set<int> disabled_nodes_;
    std::unordered_map<int, std::vector<Edge>> graph_;
    std::vector<std::array<double, 3>> node_coords_;
    std::vector<int> id_index_map_;
};

int main() {
    RouteGraph rg;
    rg.add_node(0, 49.0, -123.0); // Vancouver
    rg.add_node(1, 37.6, -122.4); // San Francisco
    rg.add_node(2, 45.0, -110.0); // Midpoint
    rg.add_node(3, 40.0, -120.0); // Another waypoint

    rg.build_graph(2000.0, 3);

    std::vector<std::pair<int, double>> path = rg.astar(0, 1);
    for (const auto& [id, dist] : path) std::cout << id << "(" << dist << "km) ";
    std::cout << "\n";
    return 0;
}

#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include "main.cpp" // or route_graph.hpp if separated later

namespace py = pybind11;

PYBIND11_MODULE(routegraph, m) {
    py::class_<RouteGraph>(m, "RouteGraph")
        .def(py::init<>())
        .def("add_node", &RouteGraph::add_node, "Add a node by ID, lat, lon")
        .def("build_graph", &RouteGraph::build_graph,
             py::arg("max_connection_km"), py::arg("k_neighbors") = 10)
        .def("astar", &RouteGraph::astar)
        .def("disable_node", &RouteGraph::disable_node)
        .def("enable_node", &RouteGraph::enable_node)
        .def("disable_nodes", &RouteGraph::disable_nodes<std::vector<int>>)
        .def("enable_nodes", &RouteGraph::enable_nodes<std::vector<int>>)
        .def("disable_nodes_in_radius", &RouteGraph::disable_nodes_in_radius)
        .def("disable_nodes_in_rectangle", &RouteGraph::disable_nodes_in_rectangle);

    py::class_<Node>(m, "Node")
        .def_readonly("id", &Node::id)
        .def_readonly("lat", &Node::lat)
        .def_readonly("lon", &Node::lon);
}

import random
import plotly.graph_objects as go
from routegraph import RouteGraph

# Generate sample airports and waypoints
random.seed(42)
rg = RouteGraph()

# 10 airports (IDs 0-9), spread over North America
for i in range(10):
    lat = random.uniform(25, 60)
    lon = random.uniform(-130, -60)
    rg.add_node(i, lat, lon)

# 20 waypoints (IDs 10–29)
for i in range(10, 30):
    lat = random.uniform(20, 70)
    lon = random.uniform(-140, -50)
    rg.add_node(i, lat, lon)

# Build connections
rg.build_graph(max_connection_km=1500, k_neighbors=5, search_checks=-1)

# Pick two airports
start, goal = 0, 1
path = rg.astar(start, goal)

# Extract lat/lon for plotting
def get_coords(path):
    return [ (rg._RouteGraph__nodes[i].lat, rg._RouteGraph__nodes[i].lon) for i in path ]

latlon_path = get_coords(path)

# Plot
fig = go.Figure()
fig.add_trace(go.Scattergeo(
    lon=[lon for lat, lon in latlon_path],
    lat=[lat for lat, lon in latlon_path],
    mode='lines+markers',
    line=dict(width=2, color='blue'),
    name='Initial Path'))

# Disable region (rectangle)
rg.disable_nodes_in_rectangle(35, -120, 50, -100)
new_path = rg.astar(start, goal)
latlon_path2 = get_coords(new_path)
fig.add_trace(go.Scattergeo(
    lon=[lon for lat, lon in latlon_path2],
    lat=[lat for lat, lon in latlon_path2],
    mode='lines+markers',
    line=dict(width=2, color='red'),
    name='Path After Disable'))

fig.update_layout(
    title='Flight Paths Before and After Regional Disable',
    geo=dict(
        projection_type='natural earth',
        showland=True,
        landcolor='rgb(243, 243, 243)',
        countrycolor='rgb(204, 204, 204)',
    )
)

fig.show()

from setuptools import setup, Extension
from setuptools.command.build_ext import build_ext
import sys
import setuptools

class get_pybind_include(object):
    def __str__(self):
        import pybind11
        return pybind11.get_include()

ext_modules = [
    Extension(
        'routegraph',
        ['bindings.cpp'],
        include_dirs=[
            get_pybind_include(),
            '.',  # in case routegraph code is in current directory
        ],
        language='c++',
        extra_compile_args=['-std=c++17']
    ),
]

setup(
    name='routegraph',
    version='0.1',
    author='Your Name',
    author_email='[email protected]',
    description='Flight routing with A* and geospatial KD-tree in C++ via pybind11',
    ext_modules=ext_modules,
    cmdclass={'build_ext': build_ext},
    zip_safe=False,
    install_requires=['pybind11>=2.6.0'],
    python_requires='>=3.7',
    classifiers=[
        'Programming Language :: Python :: 3',
        'License :: OSI Approved :: BSD License',
        'Operating System :: OS Independent',
    ],
)