Made EventQueue into a binary heap (for efficiency and sanity) dev
authorDmitriy Morozov <dmitriy@mrzv.org>
Sun, 07 Feb 2010 21:10:54 -0800
branchdev
changeset 201 4759535221ee
parent 200 73e8dce642be
child 202 a91083de7386
Made EventQueue into a binary heap (for efficiency and sanity)
.issues/ebda8db3f9908e33/new/1265605942.M759644P4728Q1.vine
include/geometry/kinetic-sort.hpp
include/geometry/simulator.h
include/geometry/simulator.hpp
include/utilities/binaryheap.h
include/utilities/eventqueue.h
tests/geometry/test-eventqueue.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/.issues/ebda8db3f9908e33/new/1265605942.M759644P4728Q1.vine	Sun Feb 07 21:10:54 2010 -0800
@@ -0,0 +1,9 @@
+From: Dmitriy Morozov <dmitriy@mrzv.org>
+Date: Sun, 07 Feb 2010 21:11:12
+Subject: Binary heap
+Message-Id: <ebda8db3f9908e33-e780beb990f99293-artemis@vine>
+References: <ebda8db3f9908e33-0-artemis@metatron>
+In-Reply-To: <ebda8db3f9908e33-0-artemis@metatron>
+
+Changed EventQueue representation to a binary heap. Used code developed by Danny
+Tarlow and Yanbin Lu.
--- a/include/geometry/kinetic-sort.hpp	Sun Feb 07 10:46:23 2010 -0800
+++ b/include/geometry/kinetic-sort.hpp	Sun Feb 07 21:10:54 2010 -0800
@@ -127,8 +127,6 @@
 	typedef 		typename Simulator::Function		        Function;
 	typedef 		typename Simulator::Time					Time;
 	
-    AssertMsg(simulator->audit_queue(), "Simulator's queue should be properly ordered");
-
 	Time t = simulator->audit_time();
 	rLog(rlKineticSortAudit, "Auditing at %s", tostring(t).c_str());
 
--- a/include/geometry/simulator.h	Sun Feb 07 10:46:23 2010 -0800
+++ b/include/geometry/simulator.h	Sun Feb 07 21:10:54 2010 -0800
@@ -58,8 +58,6 @@
         unsigned                    size() const                                { return queue_.size(); }
         unsigned                    event_count() const                         { return count_; }
 
-        bool                        audit_queue() const;
-
         std::ostream&               operator<<(std::ostream& out) const;
 
     private:
--- a/include/geometry/simulator.hpp	Sun Feb 07 10:46:23 2010 -0800
+++ b/include/geometry/simulator.hpp	Sun Feb 07 21:10:54 2010 -0800
@@ -71,11 +71,13 @@
 update(Key k, const Function& f)
 {
 	Event* ee = *k;
-	ee->root_stack() = RootStack();								// no clear() in std::stack
-	FunctionKernel::solve(f, ee->root_stack());
-	while (!ee->root_stack().empty() && ee->root_stack().top() < current_time())
-		ee->root_stack().pop();
-	queue_.update(k);
+    Event* e = new Event;
+	e->root_stack() = RootStack();								// no clear() in std::stack
+	FunctionKernel::solve(f, e->root_stack());
+	while (!e->root_stack().empty() && e->root_stack().top() < current_time())
+		e->root_stack().pop();
+	queue_.replace(k, e);
+    delete ee;
 }
 
 template<class FuncKernel_, template<class Event> class EventComparison_>
@@ -91,7 +93,7 @@
     rLog(rlSimulator, "Processing event: %s", intostring(*e).c_str());
 	
 	current_ = e->root_stack().top(); e->root_stack().pop();
-    queue_.update(top);
+    queue_.demoted(top);
 
     // Get the top element out of the queue, put it back depending on what process() says
 	if (!(e->process(this)))            { queue_.remove(top);  delete e; }
@@ -112,27 +114,6 @@
 }
 
 template<class FuncKernel_, template<class Event> class EventComparison_>
-bool
-Simulator<FuncKernel_, EventComparison_>::
-audit_queue() const
-{
-	const_Key next = queue_.top();
-    const_Key cur = next++;
-	        
-    while (next != queue_.end())
-    {
-        if (IndirectEventComparison()(*next, *cur))
-        {
-            rError("Events [%s] and [%s] not in order", intostring(**cur).c_str(), intostring(**next).c_str());
-            queue_.print(std::cout, "  ");
-            return false;
-        }
-        cur = next++;
-    }
-    return true;
-}
-		
-template<class FuncKernel_, template<class Event> class EventComparison_>
 std::ostream&
 Simulator<FuncKernel_, EventComparison_>::
 operator<<(std::ostream& out) const
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/utilities/binaryheap.h	Sun Feb 07 21:10:54 2010 -0800
@@ -0,0 +1,302 @@
+// Heap Plus implementation 1.01alpha
+// ChangeLog:
+//  2009.09.26  - Updated __down_heap, added _updatepos (Yanbin Lu)
+//  2009.08.10  - Added update_heap_pos functions so that we can adjust
+//                heap values outside of update_heap functions -- e.g., if
+//                we have external pointers into the heap entries -- then
+//                call update_heap on the position only, regardless of the value.
+//                (Danny Tarlow: dtarlow@cs.toronto.edu)
+//  2006.12.18  - Initially created (lihw)
+ 
+#ifndef HEAPPLUS_H_
+#define HEAPPLUS_H_
+ 
+#include <iterator>
+ 
+namespace std {
+  template<typename _RandomAccessIterator, typename _Distance, typename _Tp,
+	typename _Compare, typename _Updatepos>
+    void
+    __push_heap(_RandomAccessIterator __first, _Distance __holeIndex,
+				_Distance __topIndex, _Tp __value, _Compare __comp, _Updatepos __updatepos)
+    {
+      _Distance __parent = (__holeIndex - 1) / 2;
+      while (__holeIndex > __topIndex
+			 && __comp(*(__first + __parent), __value))
+		{
+		  *(__first + __holeIndex) = *(__first + __parent);
+		  __updatepos(*(__first + __holeIndex), __holeIndex); /* yanbin */
+		  __holeIndex = __parent;
+		  __parent = (__holeIndex - 1) / 2;
+		}
+      *(__first + __holeIndex) = __value;
+	  __updatepos(*(__first + __holeIndex), __holeIndex); /* yanbin */
+    }
+
+  template<typename _RandomAccessIterator, typename _Compare, typename _Updatepos>
+    inline void
+    push_heap(_RandomAccessIterator __first, _RandomAccessIterator __last,
+	      _Compare __comp, _Updatepos __updatepos)
+    {
+      typedef typename iterator_traits<_RandomAccessIterator>::value_type
+	  _ValueType;
+      typedef typename iterator_traits<_RandomAccessIterator>::difference_type
+	  _DistanceType;
+
+      // concept requirements
+      __glibcxx_function_requires(_Mutable_RandomAccessIteratorConcept<
+	    _RandomAccessIterator>)
+      __glibcxx_requires_valid_range(__first, __last);
+      __glibcxx_requires_heap_pred(__first, __last - 1, __comp);
+
+      std::__push_heap(__first, _DistanceType((__last - __first) - 1),
+		       _DistanceType(0), _ValueType(*(__last - 1)), __comp, __updatepos);
+    }
+
+  template<typename _RandomAccessIterator, typename _Distance,
+	typename _Tp, typename _Compare, typename _Updatepos>
+    void
+    __adjust_heap(_RandomAccessIterator __first, _Distance __holeIndex,
+				  _Distance __len, _Tp __value, _Compare __comp, _Updatepos __updatepos)
+  {
+	const _Distance __topIndex = __holeIndex;
+	_Distance __secondChild = 2 * __holeIndex + 2;
+	while (__secondChild < __len)
+	  {
+		if (__comp(*(__first + __secondChild),
+				   *(__first + (__secondChild - 1))))
+		  __secondChild--;
+		*(__first + __holeIndex) = *(__first + __secondChild);
+		__updatepos(*(__first + __holeIndex), __holeIndex); /* yanbin */
+		__holeIndex = __secondChild;
+		__secondChild = 2 * (__secondChild + 1);
+	  }
+	if (__secondChild == __len)
+	  {
+		*(__first + __holeIndex) = *(__first + (__secondChild - 1));
+		__updatepos(*(__first + __holeIndex), __holeIndex); /* yanbin */
+		__holeIndex = __secondChild - 1;
+	  }
+	std::__push_heap(__first, __holeIndex, __topIndex, __value, __comp, __updatepos);
+  }
+
+  template<typename _RandomAccessIterator, typename _Tp, typename _Compare, typename _Updatepos>
+    inline void
+    __pop_heap(_RandomAccessIterator __first, _RandomAccessIterator __last,
+	       _RandomAccessIterator __result, _Tp __value, _Compare __comp, _Updatepos __updatepos)
+    {
+      typedef typename iterator_traits<_RandomAccessIterator>::difference_type
+	_Distance;
+      *__result = *__first;
+      std::__adjust_heap(__first, _Distance(0), _Distance(__last - __first),
+			 __value, __comp, __updatepos);
+    }
+
+  template<typename _RandomAccessIterator, typename _Compare, typename _Updatepos>
+    inline void
+    pop_heap(_RandomAccessIterator __first,
+			 _RandomAccessIterator __last, _Compare __comp, _Updatepos __updatepos)
+    {
+      // concept requirements
+      __glibcxx_function_requires(_Mutable_RandomAccessIteratorConcept<
+	    _RandomAccessIterator>)
+      __glibcxx_requires_valid_range(__first, __last);
+      __glibcxx_requires_heap_pred(__first, __last, __comp);
+
+      typedef typename iterator_traits<_RandomAccessIterator>::value_type
+	_ValueType;
+      std::__pop_heap(__first, __last - 1, __last - 1,
+		      _ValueType(*(__last - 1)), __comp, __updatepos);
+    }
+
+  template<typename _RandomAccessIterator, typename _Compare, typename _Updatepos>
+    inline void
+    make_heap(_RandomAccessIterator __first, _RandomAccessIterator __last,
+			  _Compare __comp, _Updatepos __updatepos)
+  {
+	typedef typename iterator_traits<_RandomAccessIterator>::value_type
+	  _ValueType;
+	typedef typename iterator_traits<_RandomAccessIterator>::difference_type
+	  _DistanceType;
+
+	// concept requirements
+	__glibcxx_function_requires(_Mutable_RandomAccessIteratorConcept<
+								_RandomAccessIterator>)
+      __glibcxx_requires_valid_range(__first, __last);
+
+	if (__last - __first < 2)
+	  {
+		for (_DistanceType __len = 0; __len < __last - __first; __len ++)
+		  {
+			__updatepos(*(__first + __len), __len); /* yanbin */
+		  }
+
+		return;
+	  }
+	const _DistanceType __len = __last - __first;
+	_DistanceType __parent = (__len - 2) / 2;
+	while (true)
+	  {
+		std::__adjust_heap(__first, __parent, __len,
+						   _ValueType(*(__first + __parent)), __comp, __updatepos);
+		if (__parent == 0)
+		  {
+			for (_DistanceType __len = 0; __len < __last - __first; __len ++)
+			  {
+				__updatepos(*(__first + __len), __len); /* yanbin */
+			  }
+
+			return;
+		  }
+		__parent--;
+	  }
+
+  }
+
+
+  template<typename _RandomAccessIterator, typename _Distance, typename _Tp, typename _Compare, typename _Updatepos>
+inline void
+__up_heap(_RandomAccessIterator __first, _RandomAccessIterator __end, _RandomAccessIterator __pos,
+		  _Distance, _Tp __value,  _Compare __comp, _Updatepos __updatepos)
+{
+    _Distance __parent = (__pos - __first - 1) /  2;
+    _Distance __index = __pos - __first;
+    while (__index > 0 && __comp(*(__first + __parent), __value)) {
+        *(__first + __index) = *(__first + __parent);
+		__updatepos(*(__first + __index), __index); /* yanbin */
+
+        __index = __parent;
+        __parent = (__parent - 1) / 2;
+    }
+        
+    if (__pos != (__first + __index)) {
+        *(__first + __index) = __value;
+		__updatepos(*(__first + __index), __index); /* yanbin */
+	}
+}
+ 
+template<typename _RandomAccessIterator, typename _Distance, typename _Tp, typename _Compare, typename _Updatepos>
+inline void
+__down_heap(_RandomAccessIterator __first, _RandomAccessIterator __last, _RandomAccessIterator __pos,
+        _Distance, _Tp __value, _Compare __comp, _Updatepos __updatepos)
+{
+    _Distance __len = __last - __first;
+    _Distance __index = __pos - __first;
+    _Distance __left = __index * 2 + 1;
+    _Distance __right = __index * 2 + 2;
+    _Distance __largest;
+    while (__index < __len) 
+	  {
+		if (__right < __len)
+		  {
+			if (__comp(*(__first + __left), *(__first + __right)))
+			  {
+				__largest = __right;
+			  }
+			else
+			  {
+				__largest = __left;
+			  }
+		  }
+		else if (__left < __len)
+		  {
+			__largest = __left;
+		  }
+		else
+		  {
+			__largest = __index;
+		  }
+
+        if (__largest < __len && __comp(__value, *(__first + __largest))) 
+		  {
+            *(__first + __index) = *(__first + __largest);
+			__updatepos(*(__first + __index), __index); /* yanbin */
+            __index = __largest;
+            __left = __index * 2 + 1;
+            __right = __index * 2 + 2;
+		  } else
+		  break;
+	  }
+ 
+    if (__pos != (__first + __index)) {
+        *(__first + __index) = __value;
+		__updatepos(*(__first + __index), __index); /* yanbin */
+	}
+}
+ 
+template<typename _RandomAccessIterator, typename _Distance, typename _Tp,
+    typename _Compare, typename _Updatepos>
+inline void
+__update_heap(_RandomAccessIterator __first, _RandomAccessIterator __last,
+        _RandomAccessIterator __pos, _Distance, _Tp __value, _Compare __comp, _Updatepos __updatepos)
+{
+    *(__pos) = __value;
+     
+    _Distance __index = (__pos - __first);
+    _Distance __parent = (__index - 1) / 2;
+ 
+    if (__index > 0 && __comp(*(__first + __parent), __value))
+	  __up_heap(__first, __last, __pos, _Distance(), __value, __comp, __updatepos);
+    else
+	  __down_heap(__first, __last, __pos, _Distance(), __value, __comp, __updatepos);
+}
+ 
+template<typename _RandomAccessIterator, typename _Distance, typename _Compare, typename _Updatepos>
+inline void
+__update_heap(_RandomAccessIterator __first, _RandomAccessIterator __last,
+        _RandomAccessIterator __pos, _Distance, _Compare __comp, _Updatepos __updatepos)
+{
+    _Distance __index = (__pos - __first);
+    _Distance __parent = (__index - 1) / 2;
+    if (__index > 0 && __comp(*(__first + __parent), *(__pos)))
+      __up_heap(__first, __last, __pos, _Distance(), *(__pos), __comp, __updatepos);
+    else
+      __down_heap(__first, __last, __pos, _Distance(), *(__pos), __comp, __updatepos);
+}
+ 
+template<typename _RandomAccessIterator, typename _Tp, typename _Updatepos>
+inline void
+update_heap(_RandomAccessIterator __first, _RandomAccessIterator __last,
+        _RandomAccessIterator __pos, _Tp __value, _Updatepos __updatepos)
+{
+      typedef typename iterator_traits<_RandomAccessIterator>::value_type _ValueType;
+      typedef typename iterator_traits<_RandomAccessIterator>::difference_type _DistanceType;
+ 
+      __update_heap(__first, __last, __pos, _DistanceType(), __value, less<_ValueType>(), __updatepos);
+}
+ 
+template<typename _RandomAccessIterator, typename _Tp, typename _Compare, typename _Updatepos>
+inline void
+update_heap(_RandomAccessIterator __first, _RandomAccessIterator __last,
+        _RandomAccessIterator __pos, _Tp __value, _Compare __comp, _Updatepos __updatepos)
+{
+  __update_heap(__first, __last, __pos, __value, __comp, __updatepos);
+}
+ 
+ 
+ template<typename _RandomAccessIterator, typename _Updatepos>
+inline void
+update_heap_pos(_RandomAccessIterator __first, _RandomAccessIterator __last,
+  _RandomAccessIterator __pos, _Updatepos __updatepos) {
+      typedef typename iterator_traits<_RandomAccessIterator>::value_type _ValueType;
+      typedef typename iterator_traits<_RandomAccessIterator>::difference_type _DistanceType;
+ 
+      __update_heap(__first, __last, __pos, _DistanceType(), less<_ValueType>(), __updatepos);
+}
+ 
+ 
+template<typename _RandomAccessIterator, typename _Compare, typename _Updatepos>
+inline void
+update_heap_pos(_RandomAccessIterator __first, _RandomAccessIterator __last,
+  _RandomAccessIterator __pos, _Compare __comp, _Updatepos __updatepos) {
+      typedef typename iterator_traits<_RandomAccessIterator>::value_type _ValueType;
+      typedef typename iterator_traits<_RandomAccessIterator>::difference_type _DistanceType;
+
+      __update_heap(__first, __last, __pos, _DistanceType(), __comp, __updatepos);
+}
+ 
+ 
+ 
+}; // namespace std
+ 
+#endif // !HEAPPLUS_H_
--- a/include/utilities/eventqueue.h	Sun Feb 07 10:46:23 2010 -0800
+++ b/include/utilities/eventqueue.h	Sun Feb 07 21:10:54 2010 -0800
@@ -2,19 +2,21 @@
 #define __EVENTQUEUE_H__
 
 #include <list>
+#include <vector>
 #include <functional>
-#include <boost/utility.hpp>
 
-#include <boost/multi_index_container.hpp>
-#include <boost/multi_index/ordered_index.hpp>
-#include <boost/multi_index/sequenced_index.hpp>
-namespace bmi = boost::multi_index;
+#include <boost/utility.hpp>
+#include <boost/iterator/iterator_adaptor.hpp>
+namespace b = boost;
+
 
 #include <utilities/log.h>
 #ifdef LOGGING
 static rlog::RLogChannel* rlEventQueue =             DEF_CHANNEL("utilities/eventqueue", rlog::Log_Debug);
 #endif // LOGGING
 
+#include <utilities/binaryheap.h>
+
 #include <iostream>
 #include <string>
 #include <algorithm>
@@ -27,57 +29,188 @@
 		typedef					Event_											Event;
 		typedef					EventComparison_								EventComparison;
 
-        // Not actually a heap, but will work for now
-        typedef                 boost::multi_index_container<Event, 
-                                                             bmi::indexed_by<
-                                                                bmi::sequenced<>,
-                                                                bmi::ordered_non_unique<bmi::identity<Event>, EventComparison >
-                                                             > >
-                                                                                QueueRepresentation;
-		typedef					typename QueueRepresentation::iterator			iterator;
-		typedef					typename QueueRepresentation::const_iterator	const_iterator;
+        struct                  HeapNode;
+        struct                  CompareNodes;
+        struct                  MarkedCompareNodes;
+        struct                  UpdateNode;
+        class                   iterator;
+        class                   const_iterator;
+
+        typedef                 std::list<HeapNode>                             QueueRepresentation;
+        typedef                 std::vector<iterator>                           EventListHeap;
+
+		                        EventQueue()			    {}
 		
-		EventQueue()			{}
-		
-		const_iterator 			top() const					{ AssertMsg(!empty(), "Queue must not be empty"); return queue_.begin(); }
-		iterator 				top()						{ AssertMsg(!empty(), "Queue must not be empty"); return queue_.begin(); }
-		// iterator 				push(Event e)				{ return queue_.insert(insert_position(e), e).first; }
-		iterator 				push(Event e)				{ iterator i = queue_.push_front(e).first; update(i); return i; }
-		void 					pop()						{ AssertMsg(!empty(), "Queue must not be empty"); queue_.erase(queue_.begin()); }
-		void					remove(iterator i)			{ queue_.erase(i); }
-        void                    replace(iterator i,
-                                        Event e)            { queue_.replace(i, e); update(i); }
+		const_iterator 			top() const					{ AssertMsg(!empty(), "Queue must not be empty"); return heap_.front(); }
+		iterator 				top()						{ AssertMsg(!empty(), "Queue must not be empty"); return heap_.front(); }
+		iterator 				push(Event e);
+		void 					pop()						{ AssertMsg(!empty(), "Queue must not be empty"); std::pop_heap(heap_.begin(), heap_.end(), CompareNodes(), UpdateNode()); queue_.erase(heap_.back().base()); heap_.pop_back(); }
+        void					remove(iterator i);
+        void                    replace(iterator i, Event e);
+        void                    promoted(iterator i)        { std::update_heap_pos(heap_.begin(), heap_.end(), heap_.begin() + i.base()->heap_position_, CompareNodes(), UpdateNode()); }
+        void                    demoted(iterator i);
 
         iterator                begin()                     { return queue_.begin(); }
         const_iterator          begin() const               { return queue_.begin(); }
 		iterator 				end()						{ return queue_.end(); }
 		const_iterator 			end() const					{ return queue_.end(); }
 		bool					empty() const				{ return queue_.empty(); }
-		size_t					size() const				{ return queue_.size(); }
+		size_t					size() const				{ return heap_.size(); }
 
 		std::ostream&			print(std::ostream& out, const std::string& prefix) const;
 
-        iterator                update(iterator i)
-        { 
-            iterator bg = bmi::project<0>(queue_, bmi::get<1>(queue_).lower_bound(*i)); 
-            iterator pos = std::find_if(bg, queue_.end(), std::bind1st(EventComparison(), *i));
-            queue_.replace(i, *i);
-            queue_.relocate(pos, i);
-            return pos; 
-        }
-
 	private:
 		QueueRepresentation		queue_;
+		EventListHeap           heap_;
+};
+
+template<class Event_, class EventComparison_>
+struct EventQueue<Event_, EventComparison_>::HeapNode
+{
+                        HeapNode(const Event& e): e_(e)     {}
+
+    size_t              heap_position_;
+    Event               e_;
+};
+
+template<class Event_, class EventComparison_>
+class EventQueue<Event_, EventComparison_>::iterator: 
+    public boost::iterator_adaptor<iterator, 
+                                   typename QueueRepresentation::iterator,
+                                   Event>
+{
+    public:
+        iterator():     
+            iterator::iterator_adaptor_(0)                              {}
+
+        iterator(typename QueueRepresentation::iterator i):
+            iterator::iterator_adaptor_(i)                              {}
+
+        using iterator::iterator_adaptor_::base;
+
+        typename iterator::iterator_adaptor_::reference
+                        dereference() const                             { return base()->e_; }
+};
+
+template<class Event_, class EventComparison_>
+class EventQueue<Event_, EventComparison_>::const_iterator: 
+    public boost::iterator_adaptor<const_iterator, 
+                                   typename QueueRepresentation::const_iterator,
+                                   Event,
+                                   b::use_default,
+                                   const Event&>
+{
+    public:
+        const_iterator():
+            const_iterator::iterator_adaptor_(0)                        {}
+
+        const_iterator(iterator i):
+            const_iterator::iterator_adaptor_(i.base())                 {}
+
+        const_iterator(typename QueueRepresentation::const_iterator i):
+            const_iterator::iterator_adaptor_(i)                        {}
+
+        using const_iterator::iterator_adaptor_::base;
+
+        typename const_iterator::iterator_adaptor_::reference
+                        dereference() const                             { return base()->e_; }
+};
+
+template<class Event_, class EventComparison_>
+struct EventQueue<Event_, EventComparison_>::UpdateNode
+{
+    void operator()(iterator i, size_t pos) const                       { i.base()->heap_position_ = pos; }
+};
+        
+template<class Event_, class EventComparison_>
+struct EventQueue<Event_, EventComparison_>::CompareNodes
+{
+    bool operator()(iterator i, iterator j) const                       { return EventComparison()(*j, *i); }
 };
 
+template<class Event_, class EventComparison_>
+struct EventQueue<Event_, EventComparison_>::MarkedCompareNodes
+{
+         MarkedCompareNodes(iterator i): i_(i)                          {}
+    bool operator()(iterator i, iterator j) const                       
+    { 
+        // i_ is less than everything else
+        if (i == i_)
+            return false;
+        if (j == i_)
+            return true;
+        return EventComparison()(*j, *i); 
+    }
+    iterator i_;
+};
+
+        
+template<class Event_, class EventComparison_>
+typename EventQueue<Event_, EventComparison_>::iterator
+EventQueue<Event_, EventComparison_>::
+push(Event e)
+{ 
+    queue_.push_back(e); 
+    iterator i = b::prior(queue_.end()); 
+    heap_.push_back(i); 
+    std::push_heap(heap_.begin(), heap_.end(), CompareNodes(), UpdateNode()); 
+
+    return i;
+}
+			
+template<class Event_, class EventComparison_>
+void
+EventQueue<Event_, EventComparison_>::
+remove(iterator i)
+{ 
+    MarkedCompareNodes mcmp(i);
+    std::update_heap_pos(heap_.begin(), heap_.end(), heap_.begin() + i.base()->heap_position_, mcmp, UpdateNode());
+    std::pop_heap(heap_.begin(), heap_.end(), mcmp, UpdateNode()); 
+    AssertMsg(heap_.back() == i, "i should be in the back");
+    queue_.erase(heap_.back().base()); 
+    heap_.pop_back();
+}
+        
+template<class Event_, class EventComparison_>
+void
+EventQueue<Event_, EventComparison_>::
+replace(iterator i, Event e)
+{ 
+    
+    if (EventComparison()(*i, e))
+    {
+        *i = e; 
+        promoted(i); 
+    } else
+    {
+        *i = e;
+        demoted(i);
+    }
+}
+
+template<class Event_, class EventComparison_>
+void
+EventQueue<Event_, EventComparison_>::
+demoted(iterator i)
+{ 
+    // Bring to front
+    MarkedCompareNodes mcmp(i);
+    std::update_heap_pos(heap_.begin(), heap_.end(), heap_.begin() + i.base()->heap_position_, mcmp, UpdateNode());
+
+    // Bring to back
+    std::pop_heap(heap_.begin(), heap_.end(), mcmp, UpdateNode()); 
+
+    // Find new place
+    std::update_heap_pos(heap_.begin(), heap_.end(), heap_.begin() + i.base()->heap_position_, CompareNodes(), UpdateNode()); 
+}
 
 template<class Event_, class EventComparison_>
 std::ostream&
 EventQueue<Event_, EventComparison_>::
 print(std::ostream& out, const std::string& prefix) const
 {
-	for (typename QueueRepresentation::const_iterator cur = queue_.begin(); cur != queue_.end(); ++cur)
-		(*cur)->operator<<(out << prefix) << std::endl;
+	for (typename EventListHeap::const_iterator cur = heap_.begin(); cur != heap_.end(); ++cur)
+		out << prefix << **cur << std::endl;
 	return out;
 }
 
--- a/tests/geometry/test-eventqueue.cpp	Sun Feb 07 10:46:23 2010 -0800
+++ b/tests/geometry/test-eventqueue.cpp	Sun Feb 07 21:10:54 2010 -0800
@@ -4,19 +4,30 @@
 
 int main()
 {
-	typedef EventQueue<int, std::less<int> >		    EQ;
+	typedef EventQueue<int, std::less<int> >            EQ;
 	typedef EQ::iterator								iterator;
 	 
 	EQ queue;
 
-	iterator i = queue.push(4);
-	queue.push(2);
-	queue.push(7);
-	iterator j = queue.push(6);
-	queue.push(5);
+    std::cout << "Queue initialized" << std::endl;
+
+	iterator i1 = queue.push(4);
+	iterator i2 = queue.push(2);
+	iterator i3 = queue.push(9);
+	iterator i4 = queue.push(6);
+	iterator i5 = queue.push(5);
 
-	queue.replace(i,8);
-	queue.remove(j);
+    std::cout << "Values inserted" << std::endl;
+    queue.print(std::cout, "  ");
+
+    queue.replace(i1,1);
+    queue.remove(i4);
+    queue.replace(i5,10);
+
+    *i3 = 14;
+    queue.demoted(i3);
+    
+    std::cout << "Replaced and removed" << std::endl;
 
 	while (!queue.empty())
 	{