Navigation broken for geometry with many skinny triangles in the navigation mesh

[Ozaq]

Our current navigation is broken for geometries with many skinny triangles.

This is a severe issue with no workaround and we will not be able to provide a quick fix. @JetteSchumann / @chraibi / @schroedtert we need to discuss what to do here.

Thanks @ytipocmk631 for finding this issue!

This was found on 1.2.x

Discussed in #1404

^{Originally posted by ytipocmk631 April 30, 2024}
I set up some queuing areas and connected to the waypoint behind the queue, but when the agent arrived at the waypoint, it did not move as expected (the agents route plan did not seem to be updated)

bandicam.2024-04-30.10-33-54-636.-.Trim.mp4

example.zip
geo.wkt.zip

[Ozaq]

So far I was able to find the following issues:

• Calculating g/h values used the wrong edge for minimum distance calculation
• Algorithm terminated on first found path and did not continue to evaluate alternative paths

Still this does not fix all path calculations.

[behrisch]

Hi @Ozaq Can you elaborate a little which problems persist? Are they visible in the given example? Is there a general problem with the algorithm? Is the source for the algorithm the same as in #1373. so that we can maybe check the implementation together?

[Ozaq]

@behrisch See discussion #1404 for a description of the problem. The algorithm is still TA*. Feel free to have a look at

jupedsim/libsimulator/src/RoutingEngine.cpp

Lines 276 to 420 in 5746156

	std::vector<std::vector<Point>>
	RoutingEngine::ComputeAllConsideredPaths(Point currentPosition, Point destination)
	{
	const auto from_pos = CDT::Point{currentPosition.x, currentPosition.y};
	const auto to_pos = CDT::Point{destination.x, destination.y};
	const auto from = find_face(from_pos);
	const auto to = find_face(to_pos);
	if(from == to) {
	return {{currentPosition, destination}};
	}
	using SearchStatePtr = std::shared_ptr<SearchState>;
	std::vector<SearchStatePtr> sss{};
	std::vector<SearchStatePtr> open_states{};
	known_faces.try_emplace(from, known_faces.size() + 1);
	sss.emplace_back(new SearchState{0.0, Distance(currentPosition, destination), from, nullptr});
	open_states.emplace_back(sss.back());
	// std::map<CDT::Face_handle, SearchStatePtr> closed_states{};
	std::vector<std::vector<Point>> paths{};
	double path_length = std::numeric_limits<double>::infinity();
	while(!open_states.empty()) {
	LOG_DEBUG("============================================================");
	std::make_heap(std::begin(open_states), std::end(open_states), CompareSearchStatesGt);
	std::pop_heap(std::begin(open_states), std::end(open_states));
	auto current_state = open_states.back();
	LOG_DEBUG("popped \t\t{}", *current_state);
	open_states.pop_back();
	// closed_states.insert(std::make_pair(current_state->id, current_state));
	if(current_state->id == to) {
	LOG_DEBUG("FOUND PATH");
	// Unlike in A* this is only a first candidate solution
	// Now compute the actual path length via funnel algorithm
	// store path and length if this variant is the shortest found so far
	const auto vertex_ids = current_state->path();
	const auto found_path_strict = straightenPath(currentPosition, destination, vertex_ids);
	const auto found_path_relaxed =
	straightenPath(currentPosition, destination, vertex_ids);
	const double found_path_length = length_of_path(found_path_strict);
	paths.push_back(found_path_relaxed);
	if(found_path_length < path_length) {
	path_length = found_path_length;
	}
	}
	if(current_state->f_value() > path_length) {
	LOG_DEBUG("DONE");
	// This search nodes f-value already excedes our paths length, and since the f-value is
	// underestimation of the path length the excat path cannot be shorter than what we have
	return paths;
	}
	// Generate successors
	for(int idx = 0; idx < 3; ++idx) {
	LOG_DEBUG("Evaluating neighbor {}", idx);
	const auto target = current_state->id->neighbor(idx);
	if(!target->get_in_domain()) {
	// Not a neighboring triangle.
	LOG_DEBUG("Out of domain");
	continue;
	}
	// Do not add search nodes for nodes already in the ancestor list of this path
	if(current_state->parents_contain(target)) {
	LOG_DEBUG("Already in path to source");
	continue;
	}
	const auto edge = cdt.segment(current_state->id, idx);
	// For all remaining nodes compute g/h values
	// The h-value is the distance between the goal and the closts point on the edge
	// between the current triangle and this successor
	const double h_value = sqrt(CGAL::to_double((CGAL::squared_distance(to_pos, edge))));
	// The g-value is the maximum of:
	// "The first and simplest is the distance between the start point and the closest
	// point to it on the entry edge of the corresponding triangle."
	const double g_value_1 =
	sqrt(CGAL::to_double((CGAL::squared_distance(from_pos, edge))));
	// The second is g(s) plus the distance between the triangles associated with s and
	// s′. We assume that the g-value of s is a lower bound, and so we wish to add the
	// shortest such distance to achieve another lower bound. Again, we take this
	// measurement using the edges by which the triangles were first reached by search.
	// Since the triangles are adjacent, this is the distance of moving through the
	// triangle associated with s. In Theorem 4.2.12, we proved that the shortest
	// distance between two edges in a triangle was an arc path around the vertex shared
	// by these edges. Thus, if the entry edges of the triangles corresponding to s′ and
	// s form an angle θ, this estimate is calculated as g(s) + rθ. NOTE: Right now this
	// is always g(s) + zero as we asume point size agents (for now)
	const double g_value_2 = current_state->g_value + 0;
	// Another lower bound value for g(s′) is g(s)+(h(s)−h(s′)), or the parent state’s
	// g-value plus the difference between its h-value and that of the child state.
	// This is an underes- timate because the Euclidean distance metric used for the
	// heuristic is consistent.
	const double g_value_3 = current_state->g_value + current_state->h_value - h_value;
	const double g_value = std::max(g_value_1, std::max(g_value_2, g_value_3));
	LOG_DEBUG("g-values 1: {} 2: {} 3: {}", g_value_1, g_value_2, g_value_3);
	// NOTE(kkratz): Clang16 seems to be confused with capturing a structured binding
	// and emits a warnign when capturing 'target' directly As of C++20 this SHOULD(TM)
	// be valid code but see:
	// https://stackoverflow.com/questions/46114214/lambda-implicit-capture-fails-with-variable-declared-from-structured-binding
	auto t2 = target;
	// TODO(kkratz): replace this find on unsorted vector with something with a better
	// runtime
	if(auto iter = std::find_if(
	std::begin(open_states),
	std::end(open_states),
	[t2](const auto& s) { return s->id == t2; });
	iter != std::end(open_states)) {
	if(auto& s = *iter; s->g_value > g_value) {
	s->g_value = g_value;
	s->parent = current_state.get();
	LOG_DEBUG("updated \t{}", *s);
	}
	// } else if(auto iter = closed_states.find(target); iter !=
	// std::end(closed_states)) {
	// if(auto& [_, s] = *iter; s->g_value >= g_value) {
	// s->g_value = g_value;
	// s->parent = current_state.get();
	// open_states.push_back(s);
	// closed_states.erase(s->id);
	// }
	} else {
	known_faces.try_emplace(target, known_faces.size() + 1);
	sss.emplace_back(new SearchState{g_value, h_value, target, current_state.get()});
	open_states.emplace_back(sss.back());
	}
	}
	}
	return paths;
	}

on branch 1411-navigation-broken-for-geometry-with-many-skinny-triangles-in-the-navigation-mesh

Right now the branch contains modifications to display all expanded (i.e. straightened paths) that have been considered by TA*.

examples/geometry contains the geometry shown above.

If you have a suggestion where the implementation is not following the Algorithm described in #1373 I would be very glad to know.

[behrisch]

I am not entirely sure but I suspect it might be necessary to add a search state not only for every face but for every combination of face and "entry edge". The reason to suspect that is that a) the thesis never talks about updating old states it only adds new ones and b) updating a g-value where the h-value actually refers to a different entry edge looks suspicious. To test that out, one could start with really only adding new states and never update old ones.

[behrisch]

@Ozaq Good news! I just tested it and yes if I remove the whole update part in

jupedsim/libsimulator/src/RoutingEngine.cpp

Lines 392 to 401 in 5746156

	if(auto iter = std::find_if(
	std::begin(open_states),
	std::end(open_states),
	[t2](const auto& s) { return s->id == t2; });
	iter != std::end(open_states)) {
	if(auto& s = *iter; s->g_value > g_value) {
	s->g_value = g_value;
	s->parent = current_state.get();
	LOG_DEBUG("updated \t{}", *s);
	}

, I get the right path. Shall I do a pull request here or against the rls branch (or not at all)?

[Ozaq]

@behrisch can you please make a PR with your changes with 1411-navigation-broken-for-geometry-with-many-skinny-triangles-in-the-navigation-mesh I think I have some time on Friday to do some testing and get the changes in if all works out.

[behrisch]

@Ozaq see #1422

[Ozaq]

Hmm ok, still cannot merge this. I have been doing some testing with an arena example from @JetteSchumann and there are way to many paths created and expanded

In the screenshot below you see the start and goal marked with the pink circle and blue denoting alternative routes, green optimal route. In total 2122 paths are computed

At the moment It is unclear to me if this is a general problem in the algorithm or if there is an issue somewhere else. Some of the alternate paths though look odd on first glance at I might very well be that there are more paths computed than it should but still the number of computed paths is computationally prohibitive.

[behrisch]

Can you share this scenario or is it already in the examples? And how do you do the drawing? I added two little optimizations to the branch but I am not sure it will improve things a lot. The paths do not look very implausible to me. What is the effect on runtime or how many paths would you deem acceptable?

[behrisch]

Looking at the Demyen thesis the example above might be an instance of the problematic cases mentioned in chapter 8.2. I think there are basically three options (which can be combined of course):

1. Implementing the full TRA* (it simplifies the search tree so it may reduce the number of paths but probably will not help with many small obstacles)
2. Simplifying the geometry (at least reducing the complexity (number of surrounding triangles) of small obstacles by using their bounding box)
3. Sacrificing optimality (discarding new candidates if they are not at least 5% better than the current best path or abort after a fixed number of extended paths). This can go together with the optimization mentioned in chapter 8.2 to find a first solution fast by using a secondary queue for faces which have been expanded before.