binarybaddies/CollisionAvoidance.cpp at main · a-porche/binarybaddies · GitHub

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
#include "CollisionAvoidance.h"
#include <algorithm>
#include <iostream>

CollisionAvoidance::CollisionAvoidance(std::shared_ptr<RailNetwork> net, std::shared_ptr<PathFinder> pf)
    : network(net), pathfinder(pf), collision_count(0), reroute_count(0) {}

bool CollisionAvoidance::isTrackAvailable(int track_id, double start_time, double end_time, int train_id) const {
    for (const auto& res : reservations) {
        if (res.track_id == track_id && res.train_id != train_id) {
            if (!(end_time <= res.start_time || start_time >= res.end_time)) {
                return false;
            }
        }
    }
    return true;
}

bool CollisionAvoidance::checkRouteConflict(int train_id, const std::vector<int>& route, double start_time) {
    if (route.size() < 2) return false;

    double current_time = start_time;
    for (size_t i = 0; i < route.size() - 1; ++i) {
        auto track = network->getTrackBetween(route[i], route[i + 1]);
        if (!track) continue;

        double travel_time = track->distance / track->speed_limit;
        double end_time = current_time + travel_time;

        if (!isTrackAvailable(track->id, current_time, end_time, train_id)) {
            return true;
        }

        current_time = end_time;
    }
    return false;
}

bool CollisionAvoidance::reserveRoute(int train_id, const std::vector<int>& route, double current_time) {
    if (route.size() < 2) return true;

    clearReservationsForTrain(train_id);

    if (checkRouteConflict(train_id, route, current_time)) {
        return false;
    }

    double time = current_time;
    for (size_t i = 0; i < route.size() - 1; ++i) {
        auto track = network->getTrackBetween(route[i], route[i + 1]);
        if (!track) continue;

        double travel_time = track->distance / track->speed_limit;
        double end_time = time + travel_time;

        reservations.emplace_back(train_id, track->id, time, end_time);
        time = end_time;
    }

    train_routes[train_id] = route;
    return true;
}

void CollisionAvoidance::clearReservationsForTrain(int train_id) {
    reservations.erase(
        std::remove_if(reservations.begin(), reservations.end(),
            [train_id](const TrainReservation& res) { return res.train_id == train_id; }),
        reservations.end()
    );
}

bool CollisionAvoidance::validateTrainMovement(std::shared_ptr<Train> train1, std::shared_ptr<Train> train2) const {
    if (train1->getCurrentStation() == train2->getCurrentStation() &&
        train1->getStatus() == TrainStatus::MOVING &&
        train2->getStatus() == TrainStatus::MOVING) {
        return false;
    }
    return true;
}

std::vector<int> CollisionAvoidance::findSafeRoute(int train_id, int start_station, int end_station, double current_time) {
    auto route = pathfinder->findOptimalPath(start_station, end_station);

    if (route.empty()) {
        return {};
    }

    if (!checkRouteConflict(train_id, route, current_time)) {
        return route;
    }

    reroute_count++;

    std::vector<int> alternative_route;
    const auto& adjacent = network->getAdjacentStations(start_station);

    for (int waypoint : adjacent) {
        if (waypoint == end_station) continue;

        auto route1 = pathfinder->findOptimalPath(start_station, waypoint);
        auto route2 = pathfinder->findOptimalPath(waypoint, end_station);

        if (!route1.empty() && !route2.empty()) {
            std::vector<int> combined = route1;
            combined.insert(combined.end(), route2.begin() + 1, route2.end());

            if (!checkRouteConflict(train_id, combined, current_time)) {
                return combined;
            }
        }
    }

    return route;
}

bool CollisionAvoidance::detectPotentialCollision(const std::vector<std::shared_ptr<Train>>& trains) const {
    for (size_t i = 0; i < trains.size(); ++i) {
        for (size_t j = i + 1; j < trains.size(); ++j) {
            if (!validateTrainMovement(trains[i], trains[j])) {
                return true;
            }
        }
    }
    return false;
}