If of the previous commit: Set the MutablePriorityQueue indices
to size_t(-1) when removed from the queue.
This commit is contained in:
parent
9c4dc80057
commit
2b17e81f13
3 changed files with 44 additions and 32 deletions
|
@ -319,7 +319,7 @@ Polyline MotionPlannerGraph::shortest_path(size_t node_start, size_t node_end) c
|
|||
std::vector<size_t> map_node_to_queue_id(m_adjacency_list.size(), size_t(-1));
|
||||
distance[node_start] = 0.;
|
||||
|
||||
auto queue = make_mutable_priority_queue<node_t>(
|
||||
auto queue = make_mutable_priority_queue<node_t, false>(
|
||||
[&map_node_to_queue_id](const node_t node, size_t idx) { map_node_to_queue_id[node] = idx; },
|
||||
[&distance](const node_t node1, const node_t node2) { return distance[node1] < distance[node2]; });
|
||||
queue.reserve(m_adjacency_list.size());
|
||||
|
|
|
@ -3,7 +3,7 @@
|
|||
|
||||
#include <assert.h>
|
||||
|
||||
template<typename T, typename IndexSetter, typename LessPredicate>
|
||||
template<typename T, typename IndexSetter, typename LessPredicate, const bool ResetIndexWhenRemoved = false>
|
||||
class MutablePriorityQueue
|
||||
{
|
||||
public:
|
||||
|
@ -42,26 +42,30 @@ private:
|
|||
LessPredicate m_less_predicate;
|
||||
};
|
||||
|
||||
template<typename T, typename IndexSetter, typename LessPredicate>
|
||||
MutablePriorityQueue<T, IndexSetter, LessPredicate> make_mutable_priority_queue(IndexSetter &&index_setter, LessPredicate &&less_predicate)
|
||||
template<typename T, const bool ResetIndexWhenRemoved, typename IndexSetter, typename LessPredicate>
|
||||
MutablePriorityQueue<T, IndexSetter, LessPredicate, ResetIndexWhenRemoved> make_mutable_priority_queue(IndexSetter &&index_setter, LessPredicate &&less_predicate)
|
||||
{
|
||||
return MutablePriorityQueue<T, IndexSetter, LessPredicate>(
|
||||
return MutablePriorityQueue<T, IndexSetter, LessPredicate, ResetIndexWhenRemoved>(
|
||||
std::forward<IndexSetter>(index_setter), std::forward<LessPredicate>(less_predicate));
|
||||
}
|
||||
|
||||
template<class T, class LessPredicate, class IndexSetter>
|
||||
inline void MutablePriorityQueue<T, LessPredicate, IndexSetter>::clear()
|
||||
template<class T, class LessPredicate, class IndexSetter, const bool ResetIndexWhenRemoved>
|
||||
inline void MutablePriorityQueue<T, LessPredicate, IndexSetter, ResetIndexWhenRemoved>::clear()
|
||||
{
|
||||
#ifndef NDEBUG
|
||||
#ifdef NDEBUG
|
||||
// Only mark as removed from the queue in release mode, if configured so.
|
||||
if (ResetIndexWhenRemoved)
|
||||
#endif /* NDEBUG */
|
||||
{
|
||||
for (size_t idx = 0; idx < m_heap.size(); ++ idx)
|
||||
// Mark as removed from the queue.
|
||||
m_index_setter(m_heap[idx], std::numeric_limits<size_t>::max());
|
||||
#endif /* NDEBUG */
|
||||
}
|
||||
m_heap.clear();
|
||||
}
|
||||
|
||||
template<class T, class LessPredicate, class IndexSetter>
|
||||
inline void MutablePriorityQueue<T, LessPredicate, IndexSetter>::push(const T &item)
|
||||
template<class T, class LessPredicate, class IndexSetter, const bool ResetIndexWhenRemoved>
|
||||
inline void MutablePriorityQueue<T, LessPredicate, IndexSetter, ResetIndexWhenRemoved>::push(const T &item)
|
||||
{
|
||||
size_t idx = m_heap.size();
|
||||
m_heap.emplace_back(item);
|
||||
|
@ -69,8 +73,8 @@ inline void MutablePriorityQueue<T, LessPredicate, IndexSetter>::push(const T &i
|
|||
update_heap_up(0, idx);
|
||||
}
|
||||
|
||||
template<class T, class LessPredicate, class IndexSetter>
|
||||
inline void MutablePriorityQueue<T, LessPredicate, IndexSetter>::push(T &&item)
|
||||
template<class T, class LessPredicate, class IndexSetter, const bool ResetIndexWhenRemoved>
|
||||
inline void MutablePriorityQueue<T, LessPredicate, IndexSetter, ResetIndexWhenRemoved>::push(T &&item)
|
||||
{
|
||||
size_t idx = m_heap.size();
|
||||
m_heap.emplace_back(std::move(item));
|
||||
|
@ -78,14 +82,18 @@ inline void MutablePriorityQueue<T, LessPredicate, IndexSetter>::push(T &&item)
|
|||
update_heap_up(0, idx);
|
||||
}
|
||||
|
||||
template<class T, class LessPredicate, class IndexSetter>
|
||||
inline void MutablePriorityQueue<T, LessPredicate, IndexSetter>::pop()
|
||||
template<class T, class LessPredicate, class IndexSetter, const bool ResetIndexWhenRemoved>
|
||||
inline void MutablePriorityQueue<T, LessPredicate, IndexSetter, ResetIndexWhenRemoved>::pop()
|
||||
{
|
||||
assert(! m_heap.empty());
|
||||
#ifndef NDEBUG
|
||||
#ifdef NDEBUG
|
||||
// Only mark as removed from the queue in release mode, if configured so.
|
||||
if (ResetIndexWhenRemoved)
|
||||
#endif /* NDEBUG */
|
||||
{
|
||||
// Mark as removed from the queue.
|
||||
m_index_setter(m_heap.front(), std::numeric_limits<size_t>::max());
|
||||
#endif /* NDEBUG */
|
||||
}
|
||||
if (m_heap.size() > 1) {
|
||||
m_heap.front() = m_heap.back();
|
||||
m_heap.pop_back();
|
||||
|
@ -95,14 +103,18 @@ inline void MutablePriorityQueue<T, LessPredicate, IndexSetter>::pop()
|
|||
m_heap.clear();
|
||||
}
|
||||
|
||||
template<class T, class LessPredicate, class IndexSetter>
|
||||
inline void MutablePriorityQueue<T, LessPredicate, IndexSetter>::remove(size_t idx)
|
||||
template<class T, class LessPredicate, class IndexSetter, const bool ResetIndexWhenRemoved>
|
||||
inline void MutablePriorityQueue<T, LessPredicate, IndexSetter, ResetIndexWhenRemoved>::remove(size_t idx)
|
||||
{
|
||||
assert(idx < m_heap.size());
|
||||
#ifndef NDEBUG
|
||||
#ifdef NDEBUG
|
||||
// Only mark as removed from the queue in release mode, if configured so.
|
||||
if (ResetIndexWhenRemoved)
|
||||
#endif /* NDEBUG */
|
||||
{
|
||||
// Mark as removed from the queue.
|
||||
m_index_setter(m_heap[idx], std::numeric_limits<size_t>::max());
|
||||
#endif /* NDEBUG */
|
||||
}
|
||||
if (idx + 1 == m_heap.size()) {
|
||||
m_heap.pop_back();
|
||||
return;
|
||||
|
@ -114,8 +126,8 @@ inline void MutablePriorityQueue<T, LessPredicate, IndexSetter>::remove(size_t i
|
|||
update_heap_up(0, idx);
|
||||
}
|
||||
|
||||
template<class T, class LessPredicate, class IndexSetter>
|
||||
inline void MutablePriorityQueue<T, LessPredicate, IndexSetter>::update_heap_up(size_t top, size_t bottom)
|
||||
template<class T, class LessPredicate, class IndexSetter, const bool ResetIndexWhenRemoved>
|
||||
inline void MutablePriorityQueue<T, LessPredicate, IndexSetter, ResetIndexWhenRemoved>::update_heap_up(size_t top, size_t bottom)
|
||||
{
|
||||
size_t childIdx = bottom;
|
||||
T *child = &m_heap[childIdx];
|
||||
|
@ -138,8 +150,8 @@ inline void MutablePriorityQueue<T, LessPredicate, IndexSetter>::update_heap_up(
|
|||
}
|
||||
}
|
||||
|
||||
template<class T, class LessPredicate, class IndexSetter>
|
||||
inline void MutablePriorityQueue<T, LessPredicate, IndexSetter>::update_heap_down(size_t top, size_t bottom)
|
||||
template<class T, class LessPredicate, class IndexSetter, const bool ResetIndexWhenRemoved>
|
||||
inline void MutablePriorityQueue<T, LessPredicate, IndexSetter, ResetIndexWhenRemoved>::update_heap_down(size_t top, size_t bottom)
|
||||
{
|
||||
size_t parentIdx = top;
|
||||
T *parent = &m_heap[parentIdx];
|
||||
|
|
|
@ -191,7 +191,7 @@ std::vector<std::pair<size_t, bool>> chain_segments_greedy_constrained_reversals
|
|||
}
|
||||
|
||||
// Initialize a heap of end points sorted by the lowest distance to the next valid point of a path.
|
||||
auto queue = make_mutable_priority_queue<EndPoint*>(
|
||||
auto queue = make_mutable_priority_queue<EndPoint*, false>(
|
||||
[](EndPoint *ep, size_t idx){ ep->heap_idx = idx; },
|
||||
[](EndPoint *l, EndPoint *r){ return l->distance_out < r->distance_out; });
|
||||
queue.reserve(end_points.size() * 2 - 1);
|
||||
|
@ -687,7 +687,7 @@ std::vector<std::pair<size_t, bool>> chain_segments_greedy_constrained_reversals
|
|||
}
|
||||
|
||||
// Initialize a heap of end points sorted by the lowest distance to the next valid point of a path.
|
||||
auto queue = make_mutable_priority_queue<EndPoint*>(
|
||||
auto queue = make_mutable_priority_queue<EndPoint*, true>(
|
||||
[](EndPoint *ep, size_t idx){ ep->heap_idx = idx; },
|
||||
[](EndPoint *l, EndPoint *r){ return l->distance_out < r->distance_out; });
|
||||
queue.reserve(end_points.size() * 2);
|
||||
|
@ -1150,7 +1150,7 @@ static inline void improve_ordering_by_segment_flipping(Polylines &polylines, bo
|
|||
#endif /* NDEBUG */
|
||||
|
||||
// Initialize a MutablePriorityHeap of connections between polylines.
|
||||
auto queue = make_mutable_priority_queue<Connection*>(
|
||||
auto queue = make_mutable_priority_queue<Connection*, false>(
|
||||
[](Connection *connection, size_t idx){ connection->heap_idx = idx; },
|
||||
// Sort by decreasing connection distance.
|
||||
[&polylines, &connections](Connection *l, Connection *r){ return l->squaredNorm(polylines, connections) > r->squaredNorm(polylines, connections); });
|
||||
|
|
Loading…
Reference in a new issue