125 lines
4.8 KiB
C++
125 lines
4.8 KiB
C++
#include <iostream>
|
|
|
|
#include <arc_utilities/dijkstras.hpp>
|
|
#include <arc_utilities/pretty_print.hpp>
|
|
|
|
int main(int argc, char* argv[])
|
|
{
|
|
(void)argc;
|
|
(void)argv;
|
|
|
|
std::cout << "Creating graph with nodes indices:\n\n";
|
|
std::cout << " 2 | 5 | 8\n"
|
|
<< " --+---+--\n"
|
|
<< " 1 | 4 | 7\n"
|
|
<< " --+---+--\n"
|
|
<< " 0 | 3 | 6\n\n";
|
|
|
|
arc_dijkstras::Graph<Eigen::Vector2d> graph(9);
|
|
for (double x = -1; x <= 1; x += 1)
|
|
{
|
|
for (double y = -1; y <= 1; y += 1)
|
|
{
|
|
graph.AddNode(Eigen::Vector2d(x, y));
|
|
}
|
|
}
|
|
|
|
// Y-direction edges
|
|
graph.AddEdgeBetweenNodes(0, 1, 1.0);
|
|
graph.AddEdgesBetweenNodes(1, 2, 1.0);
|
|
graph.AddEdgesBetweenNodes(3, 4, 1.0);
|
|
graph.AddEdgesBetweenNodes(4, 5, 1.0);
|
|
graph.AddEdgesBetweenNodes(6, 7, 1.0);
|
|
graph.AddEdgesBetweenNodes(7, 8, 1.0);
|
|
|
|
// X-direction edges
|
|
graph.AddEdgesBetweenNodes(0, 3, 1.0);
|
|
graph.AddEdgesBetweenNodes(3, 6, 1.0);
|
|
graph.AddEdgesBetweenNodes(1, 4, 1.0);
|
|
graph.AddEdgesBetweenNodes(4, 7, 1.0);
|
|
graph.AddEdgesBetweenNodes(2, 5, 1.0);
|
|
graph.AddEdgesBetweenNodes(5, 8, 1.0);
|
|
|
|
assert(graph.CheckGraphLinkage());
|
|
|
|
|
|
auto dijkstras_result_4connected = arc_dijkstras::SimpleDijkstrasAlgorithm<Eigen::Vector2d, std::allocator<Eigen::Vector2d>>::PerformDijkstrasAlgorithm(graph, 0);
|
|
|
|
std::cout << "4-connected edges\n"
|
|
<< "Node index : 0, 1, 2, 3, 4, 5, 6, 7, 8\n";
|
|
std::cout << "Previous graph indices: " << PrettyPrint::PrettyPrint(dijkstras_result_4connected.second.first) << std::endl;
|
|
std::cout << "Distance : " << PrettyPrint::PrettyPrint(dijkstras_result_4connected.second.second) << std::endl;
|
|
|
|
// Diagonal edges
|
|
graph.AddEdgesBetweenNodes(0, 4, std::sqrt(2));
|
|
graph.AddEdgesBetweenNodes(1, 5, std::sqrt(2));
|
|
graph.AddEdgesBetweenNodes(3, 7, std::sqrt(2));
|
|
graph.AddEdgesBetweenNodes(4, 8, std::sqrt(2));
|
|
|
|
graph.AddEdgesBetweenNodes(1, 3, std::sqrt(2));
|
|
graph.AddEdgesBetweenNodes(2, 4, std::sqrt(2));
|
|
graph.AddEdgesBetweenNodes(4, 6, std::sqrt(2));
|
|
graph.AddEdgesBetweenNodes(5, 7, std::sqrt(2));
|
|
|
|
assert(graph.CheckGraphLinkage());
|
|
auto dijkstras_result_8connected = arc_dijkstras::SimpleDijkstrasAlgorithm<Eigen::Vector2d, std::allocator<Eigen::Vector2d>>::PerformDijkstrasAlgorithm(graph, 0);
|
|
|
|
std::cout << "\n8-connected edges\n"
|
|
<< "Node index : 0, 1, 2, 3, 4, 5, 6, 7, 8\n";
|
|
std::cout << "Previous graph indices: " << PrettyPrint::PrettyPrint(dijkstras_result_8connected.second.first) << std::endl;
|
|
std::cout << "Distance : " << PrettyPrint::PrettyPrint(dijkstras_result_8connected.second.second) << std::endl;
|
|
|
|
std::cout << "\nSerialization test... ";
|
|
|
|
arc_dijkstras::Graph<Eigen::Vector2d> serialization_test_graph(2);
|
|
serialization_test_graph.AddNode(Eigen::Vector2d(0,0));
|
|
serialization_test_graph.AddNode(Eigen::Vector2d(1,1));
|
|
serialization_test_graph.AddEdgesBetweenNodes(0, 1, 1.0);
|
|
|
|
// Define the graph value serialization function
|
|
const auto value_serializer_fn = [] (const Eigen::Vector2d& value, std::vector<uint8_t>& buffer)
|
|
{
|
|
const uint64_t start_buffer_size = buffer.size();
|
|
uint64_t running_total = 0;
|
|
|
|
running_total += arc_utilities::SerializeFixedSizePOD<double>(value(0), buffer);
|
|
running_total += arc_utilities::SerializeFixedSizePOD<double>(value(1), buffer);
|
|
|
|
const uint64_t end_buffer_size = buffer.size();
|
|
const uint64_t bytes_written = end_buffer_size - start_buffer_size;
|
|
|
|
assert(running_total == bytes_written);
|
|
|
|
return bytes_written;
|
|
};
|
|
|
|
// Define the graph value serialization function
|
|
const auto value_deserializer_fn = [] (const std::vector<uint8_t>& buffer, const uint64_t current)
|
|
{
|
|
uint64_t current_position = current;
|
|
|
|
// Deserialze 2 doubles
|
|
std::pair<double, uint64_t> x = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
|
|
current_position += x.second;
|
|
std::pair<double, uint64_t> y = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
|
|
current_position += y.second;
|
|
|
|
const Eigen::Vector2d deserialized(x.first, y.first);
|
|
|
|
// Figure out how many bytes were read
|
|
const uint64_t bytes_read = current_position - current;
|
|
return std::make_pair(deserialized, bytes_read);
|
|
};
|
|
|
|
// Serialze the graph
|
|
std::vector<uint8_t> buffer;
|
|
serialization_test_graph.SerializeSelf(buffer, value_serializer_fn);
|
|
|
|
auto deserialized_result = arc_dijkstras::Graph<Eigen::Vector2d>::Deserialize(buffer, 0, value_deserializer_fn);
|
|
assert(deserialized_result.first.CheckGraphLinkage());
|
|
|
|
std::cout << "passed" << std::endl;
|
|
|
|
return 0;
|
|
}
|