diff --git a/src/linkgraph/mcf.cpp b/src/linkgraph/mcf.cpp
index ecdf792..20bebbc 100644
--- a/src/linkgraph/mcf.cpp
+++ b/src/linkgraph/mcf.cpp
@@ -10,12 +10,27 @@
 typedef std::map<NodeID, Path *> PathViaMap;
 
 /**
+ * This is a wrapper around Tannotation* which also stores a cache of GetAnnotation() and GetNode()
+ * to remove the need dereference the Tannotation* pointer when sorting/inseting/erasing in MultiCommodityFlow::Dijkstra::AnnoSet
+ */
+template<typename Tannotation>
+class AnnoSetItem {
+public:
+	Tannotation *anno_ptr;
+	typename Tannotation::AnnotationValueType cached_annotation;
+	NodeID node_id;
+
+	AnnoSetItem(Tannotation *anno) : anno_ptr(anno), cached_annotation(anno->GetAnnotation()), node_id(anno->GetNode()) {}
+};
+
+/**
  * Distance-based annotation for use in the Dijkstra algorithm. This is close
  * to the original meaning of "annotation" in this context. Paths are rated
  * according to the sum of distances of their edges.
  */
 class DistanceAnnotation : public Path {
 public:
+	typedef uint AnnotationValueType;
 
 	/**
 	 * Constructor.
@@ -41,7 +56,7 @@ public:
 	 * Comparator for std containers.
 	 */
 	struct Comparator {
-		bool operator()(const DistanceAnnotation *x, const DistanceAnnotation *y) const;
+		bool operator()(const AnnoSetItem<DistanceAnnotation> &x, const AnnoSetItem<DistanceAnnotation> &y) const;
 	};
 };
 
@@ -53,8 +68,8 @@ public:
  */
 class CapacityAnnotation : public Path {
 	int cached_annotation;
-
 public:
+	typedef int AnnotationValueType;
 
 	/**
 	 * Constructor.
@@ -83,7 +98,7 @@ public:
 	 * Comparator for std containers.
 	 */
 	struct Comparator {
-		bool operator()(const CapacityAnnotation *x, const CapacityAnnotation *y) const;
+		bool operator()(const AnnoSetItem<CapacityAnnotation> &x, const AnnoSetItem<CapacityAnnotation> &y) const;
 	};
 };
 
@@ -247,6 +262,17 @@ bool CapacityAnnotation::IsBetter(const CapacityAnnotation *base, uint cap,
 }
 
 /**
+ * Annotation wrapper class which also stores an iterator to the AnnoSet node which points to this annotation
+ * This is to enable erasing the AnnoSet node when calling Path::Fork without having to search the set
+ */
+template<class Tannotation>
+struct AnnosWrapper : public Tannotation {
+	typename std::set<AnnoSetItem<Tannotation>, typename Tannotation::Comparator>::iterator self_iter;
+
+	AnnosWrapper(NodeID n, bool source = false) : Tannotation(n, source) {}
+};
+
+/**
  * A slightly modified Dijkstra algorithm. Grades the paths not necessarily by
  * distance, but by the value Tannotation computes. It uses the max_saturation
  * setting to artificially decrease capacities.
@@ -258,21 +284,22 @@ bool CapacityAnnotation::IsBetter(const CapacityAnnotation *base, uint cap,
 template<class Tannotation, class Tedge_iterator>
 void MultiCommodityFlow::Dijkstra(NodeID source_node, PathVector &paths)
 {
-	typedef std::set<Tannotation *, typename Tannotation::Comparator> AnnoSet;
+	typedef std::set<AnnoSetItem<Tannotation>, typename Tannotation::Comparator> AnnoSet;
+	AnnoSet annos = AnnoSet(typename Tannotation::Comparator());
 	Tedge_iterator iter(this->job);
 	uint size = this->job.Size();
-	AnnoSet annos;
 	paths.resize(size, NULL);
 	for (NodeID node = 0; node < size; ++node) {
-		Tannotation *anno = new Tannotation(node, node == source_node);
+		AnnosWrapper<Tannotation> *anno = new AnnosWrapper<Tannotation>(node, node == source_node);
 		anno->UpdateAnnotation();
-		annos.insert(anno);
+		anno->self_iter = annos.insert(AnnoSetItem<Tannotation>(anno)).first;
 		paths[node] = anno;
 	}
 	while (!annos.empty()) {
 		typename AnnoSet::iterator i = annos.begin();
-		Tannotation *source = *i;
+		AnnosWrapper<Tannotation> *source = static_cast<AnnosWrapper<Tannotation> *>(i->anno_ptr);
 		annos.erase(i);
+		source->self_iter = annos.end();
 		NodeID from = source->GetNode();
 		iter.SetNode(source_node, from);
 		for (NodeID to = iter.Next(); to != INVALID_NODE; to = iter.Next()) {
@@ -286,12 +313,12 @@ void MultiCommodityFlow::Dijkstra(NodeID source_node, PathVector &paths)
 			}
 			/* punish in-between stops a little */
 			uint distance = DistanceMaxPlusManhattan(this->job[from].XY(), this->job[to].XY()) + 1;
-			Tannotation *dest = static_cast<Tannotation *>(paths[to]);
+			AnnosWrapper<Tannotation> *dest = static_cast<AnnosWrapper<Tannotation> *>(paths[to]);
 			if (dest->IsBetter(source, capacity, capacity - edge.Flow(), distance)) {
-				annos.erase(dest);
+				if (dest->self_iter != annos.end()) annos.erase(dest->self_iter);
 				dest->Fork(source, capacity, capacity - edge.Flow(), distance);
 				dest->UpdateAnnotation();
-				annos.insert(dest);
+				dest->self_iter = annos.insert(AnnoSetItem<Tannotation>(dest)).first;
 			}
 		}
 	}
@@ -579,11 +606,11 @@ bool Greater(T x_anno, T y_anno, NodeID x, NodeID y)
  * @param y Second capacity annotation.
  * @return If x is better than y.
  */
-bool CapacityAnnotation::Comparator::operator()(const CapacityAnnotation *x,
-		const CapacityAnnotation *y) const
+bool CapacityAnnotation::Comparator::operator()(const AnnoSetItem<CapacityAnnotation> &x,
+		const AnnoSetItem<CapacityAnnotation> &y) const
 {
-	return x != y && Greater<int>(x->GetAnnotation(), y->GetAnnotation(),
-			x->GetNode(), y->GetNode());
+	return x.anno_ptr != y.anno_ptr && Greater<int>(x.cached_annotation, y.cached_annotation,
+			x.node_id, y.node_id);
 }
 
 /**
@@ -592,9 +619,9 @@ bool CapacityAnnotation::Comparator::operator()(const CapacityAnnotation *x,
  * @param y Second distance annotation.
  * @return If x is better than y.
  */
-bool DistanceAnnotation::Comparator::operator()(const DistanceAnnotation *x,
-		const DistanceAnnotation *y) const
+bool DistanceAnnotation::Comparator::operator()(const AnnoSetItem<DistanceAnnotation> &x,
+		const AnnoSetItem<DistanceAnnotation> &y) const
 {
-	return x != y && !Greater<uint>(x->GetAnnotation(), y->GetAnnotation(),
-			x->GetNode(), y->GetNode());
+	return x.anno_ptr != y.anno_ptr && !Greater<uint>(x.cached_annotation, y.cached_annotation,
+			x.node_id, y.node_id);
 }