diff --git a/src/dsm/dsm.hpp b/src/dsm/dsm.hpp index bc74293a5..2d587b259 100644 --- a/src/dsm/dsm.hpp +++ b/src/dsm/dsm.hpp @@ -6,7 +6,7 @@ static constexpr uint8_t DSM_VERSION_MAJOR = 2; static constexpr uint8_t DSM_VERSION_MINOR = 6; -static constexpr uint8_t DSM_VERSION_PATCH = 2; +static constexpr uint8_t DSM_VERSION_PATCH = 3; static auto const DSM_VERSION = std::format("{}.{}.{}", DSM_VERSION_MAJOR, DSM_VERSION_MINOR, DSM_VERSION_PATCH); diff --git a/src/dsm/headers/RoadDynamics.hpp b/src/dsm/headers/RoadDynamics.hpp index dae266975..cdbb2b5bd 100644 --- a/src/dsm/headers/RoadDynamics.hpp +++ b/src/dsm/headers/RoadDynamics.hpp @@ -1556,7 +1556,7 @@ namespace dsm { continue; } auto& spire = dynamic_cast(*pStreet); - file << separator << spire.code(); + file << separator << streetId; } file << std::endl; } diff --git a/src/dsm/headers/RoadNetwork.hpp b/src/dsm/headers/RoadNetwork.hpp index 12a6af8eb..af5ec35d6 100644 --- a/src/dsm/headers/RoadNetwork.hpp +++ b/src/dsm/headers/RoadNetwork.hpp @@ -85,6 +85,9 @@ namespace dsm { /// Street priorities are assigned considering the number of lanes and the speed limit. /// Traffic Lights with an input degree lower than 3 are converted to standard intersections. void initTrafficLights(); + /// @brief Automatically re-maps street lanes basing on network's topology + /// @details For example, if one street has the right turn forbidden, then the right lane becomes a straight one + void autoMapStreetLanes(); /// @brief Import the graph's adjacency matrix from a file. /// If the file is not of a supported format, it will read the file as a matrix with the first two elements being diff --git a/src/dsm/sources/RoadNetwork.cpp b/src/dsm/sources/RoadNetwork.cpp index 003e83a96..79b13328e 100644 --- a/src/dsm/sources/RoadNetwork.cpp +++ b/src/dsm/sources/RoadNetwork.cpp @@ -239,6 +239,77 @@ namespace dsm { }); } } + void RoadNetwork::autoMapStreetLanes() { + std::for_each(m_nodes.cbegin(), m_nodes.cend(), [this](auto const& pair) { + auto const& inNeighbours{m_adjacencyMatrix.getCol(pair.first)}; + auto const& outNeighbours{m_adjacencyMatrix.getRow(pair.first)}; + std::for_each( + inNeighbours.cbegin(), + inNeighbours.cend(), + [this, &pair, &outNeighbours](auto const& inNodeId) { + auto const& pInStreet{m_edges.at(inNodeId * m_nodes.size() + pair.first)}; + // auto const& laneMapping{pInStreet->laneMapping()}; + std::multiset allowedTurns; + std::for_each( + outNeighbours.cbegin(), + outNeighbours.cend(), + [this, &pair, &pInStreet, &allowedTurns](auto const& outNodeId) { + auto const& pOutStreet{ + m_edges.at(pair.first * m_nodes.size() + outNodeId)}; + auto const deltaAngle{pOutStreet->deltaAngle(pInStreet->angle())}; + // Logger::debug(std::format("Angle in {} - angle out {}", pInStreet->angle(), pOutStreet->angle())); + // Logger::debug(std::format("Delta: {}", deltaAngle)); + if (std::abs(deltaAngle) < 5 * std::numbers::pi / 6) { + if (deltaAngle < -std::numbers::pi / 6.) { + Logger::debug( + std::format("Street {} can turn RIGHT", pInStreet->id())); + allowedTurns.emplace(Direction::RIGHT); + } else if (deltaAngle > std::numbers::pi / 6.) { + Logger::debug( + std::format("Street {} can turn LEFT", pInStreet->id())); + allowedTurns.emplace(Direction::LEFT); + } else { + Logger::debug( + std::format("Street {} can go STRAIGHT", pInStreet->id())); + allowedTurns.emplace(Direction::STRAIGHT); + } + } + }); + auto const nLanes{pInStreet->nLanes()}; + while (allowedTurns.size() < static_cast(nLanes)) { + if (allowedTurns.contains(Direction::STRAIGHT)) { + allowedTurns.emplace(Direction::STRAIGHT); + } else if (allowedTurns.contains(Direction::RIGHT)) { + allowedTurns.emplace(Direction::RIGHT); + } else if (allowedTurns.contains(Direction::LEFT)) { + allowedTurns.emplace(Direction::LEFT); + } else { + allowedTurns.emplace(Direction::ANY); + } + } + switch (nLanes) { + case 1: + // Leaving Direction::ANY for one lane streets is the less painful option + break; + case 2: + if (allowedTurns.contains(Direction::STRAIGHT) && + allowedTurns.contains(Direction::RIGHT) && + allowedTurns.contains(Direction::LEFT)) { + break; + } + [[fallthrough]]; + default: + assert(allowedTurns.size() == static_cast(nLanes)); + std::vector newMapping(nLanes); + auto it{allowedTurns.cbegin()}; + for (size_t i{0}; i < allowedTurns.size(); ++i, ++it) { + newMapping[i] = *it; + } + pInStreet->setLaneMapping(newMapping); + } + }); + }); + } void RoadNetwork::buildAdj() { // find max values in streets node pairs diff --git a/src/dsm/sources/Street.cpp b/src/dsm/sources/Street.cpp index 833e8b4a5..06318fe8f 100644 --- a/src/dsm/sources/Street.cpp +++ b/src/dsm/sources/Street.cpp @@ -53,6 +53,13 @@ namespace dsm { void Street::setLaneMapping(std::vector const& laneMapping) { assert(laneMapping.size() == static_cast(m_nLanes)); m_laneMapping = laneMapping; + std::string strLaneMapping; + std::for_each( + laneMapping.cbegin(), laneMapping.cend(), [&strLaneMapping](auto const item) { + strLaneMapping += std::format("{} - ", static_cast(item)); + }); + Logger::debug( + std::format("New lane mapping for street {} is: {}", m_id, strLaneMapping)); } void Street::setQueue(dsm::queue> queue, size_t index) { assert(index < m_exitQueues.size()); diff --git a/test/Test_graph.cpp b/test/Test_graph.cpp index c79797ab5..9fb3ebcab 100644 --- a/test/Test_graph.cpp +++ b/test/Test_graph.cpp @@ -95,7 +95,49 @@ TEST_CASE("RoadNetwork") { CHECK(graph.adjacencyMatrix().contains(0, 2)); CHECK_FALSE(graph.adjacencyMatrix().contains(1, 3)); } - + SUBCASE("autoMapStreetLanes") { + GIVEN("A Graph object") { + RoadNetwork graph{}; + graph.addNode(0, std::make_pair(0., 0.)); + graph.addNode(1, std::make_pair(0., -1.)); + graph.addNode(2, std::make_pair(-1., 0.)); + graph.addNode(3, std::make_pair(1., 1.)); + graph.addEdge(1, std::make_pair(0, 1), 1., 1., 3); + graph.addEdge(4, std::make_pair(1, 0), 1., 1., 3); + graph.addEdge(2, std::make_pair(0, 2), 1.); + graph.addEdge(8, std::make_pair(2, 0), 1.); + graph.addEdge(3, std::make_pair(0, 3), 1., 1., 2); + graph.addEdge(12, std::make_pair(3, 0), 1., 1., 2); + graph.buildAdj(); + CHECK_EQ(graph.nEdges(), 6); + CHECK_EQ(graph.nNodes(), 4); + WHEN("We automatically map street lanes") { + // dsm::Logger::setLogLevel(dsm::log_level_t::DEBUG); + graph.autoMapStreetLanes(); + // dsm::Logger::setLogLevel(dsm::log_level_t::INFO); + THEN("The lanes are correctly mapped") { + CHECK_EQ(graph.edge(1)->laneMapping().size(), 3); + CHECK_EQ(graph.edge(1)->laneMapping()[0], dsm::Direction::ANY); + CHECK_EQ(graph.edge(1)->laneMapping()[1], dsm::Direction::ANY); + CHECK_EQ(graph.edge(1)->laneMapping()[2], dsm::Direction::ANY); + CHECK_EQ(graph.edge(4)->laneMapping().size(), 3); + CHECK_EQ(graph.edge(4)->laneMapping()[0], dsm::Direction::RIGHT); + CHECK_EQ(graph.edge(4)->laneMapping()[1], dsm::Direction::RIGHT); + CHECK_EQ(graph.edge(4)->laneMapping()[2], dsm::Direction::LEFT); + CHECK_EQ(graph.edge(2)->laneMapping().size(), 1); + CHECK_EQ(graph.edge(2)->laneMapping()[0], dsm::Direction::ANY); + CHECK_EQ(graph.edge(8)->laneMapping().size(), 1); + CHECK_EQ(graph.edge(8)->laneMapping()[0], dsm::Direction::ANY); + CHECK_EQ(graph.edge(3)->laneMapping().size(), 2); + CHECK_EQ(graph.edge(3)->laneMapping()[0], dsm::Direction::ANY); + CHECK_EQ(graph.edge(3)->laneMapping()[1], dsm::Direction::ANY); + CHECK_EQ(graph.edge(12)->laneMapping().size(), 2); + CHECK_EQ(graph.edge(12)->laneMapping()[0], dsm::Direction::RIGHT); + CHECK_EQ(graph.edge(12)->laneMapping()[1], dsm::Direction::LEFT); + } + } + } + } SUBCASE("importMatrix - dsm") { // This tests the importMatrix function over .dsm files // GIVEN: a graph