If of the previous commit: Set the MutablePriorityQueue indices

to size_t(-1) when removed from the queue.
This commit is contained in:
bubnikv 2019-11-22 19:09:39 +01:00
parent 9c4dc80057
commit 2b17e81f13
3 changed files with 44 additions and 32 deletions

View file

@ -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());

View file

@ -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];

View file

@ -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); });