diff --git a/src/Algorithms/IndexMap.cpp b/src/Algorithms/IndexMap.cpp
index 9fcc9767cf4a92012de09adcd3417d37e9a64443..b6a97d256ff7cf39bc6cdada88f975de6fa1dba4 100644
--- a/src/Algorithms/IndexMap.cpp
+++ b/src/Algorithms/IndexMap.cpp
@@ -1,3 +1,25 @@
+//
+// Class IndexMap
+//
+// This class stores and prints the sequence of elements that the referenc particle passes.
+// Each time the reference particle enters or leaves an element an entry is added to the map.
+// With help of this map one can determine which element can be found at a given position.
+//
+// Copyright (c) 2016,       Christof Metzger-Kraus, Helmholtz-Zentrum Berlin, Germany
+//               2017 - 2020 Christof Metzger-Kraus
+//
+// All rights reserved
+//
+// This file is part of OPAL.
+//
+// OPAL is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// You should have received a copy of the GNU General Public License
+// along with OPAL. If not, see <https://www.gnu.org/licenses/>.
+//
 #include <map>
 #include <limits>
 #include <iostream>
@@ -128,8 +150,7 @@ void IndexMap::tidyUp(double zstop) {
 
     if (rit != mapRange2Element_m.rend() &&
         (*rit).second.size() == 0 &&
-        zstop > (*rit).first.first &&
-        zstop < (*rit).first.second) {
+        zstop > (*rit).first.first) {
 
         key_t key((*rit).first.first, zstop);
         value_t val;
diff --git a/src/Algorithms/IndexMap.h b/src/Algorithms/IndexMap.h
index 2b24769c4cb05bbe9368ec3d2f95fa76bcfe6627..f2a3ee9ef756adc80ac1264103006dfa13b96568 100644
--- a/src/Algorithms/IndexMap.h
+++ b/src/Algorithms/IndexMap.h
@@ -1,3 +1,25 @@
+//
+// Class IndexMap
+//
+// This class stores and prints the sequence of elements that the referenc particle passes.
+// Each time the reference particle enters or leaves an element an entry is added to the map.
+// With help of this map one can determine which element can be found at a given position.
+//
+// Copyright (c) 2016,       Christof Metzger-Kraus, Helmholtz-Zentrum Berlin, Germany
+//               2017 - 2020 Christof Metzger-Kraus
+//
+// All rights reserved
+//
+// This file is part of OPAL.
+//
+// OPAL is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// You should have received a copy of the GNU General Public License
+// along with OPAL. If not, see <https://www.gnu.org/licenses/>.
+//
 #ifndef OPAL_INDEXMAP_H
 #define OPAL_INDEXMAP_H
 
diff --git a/src/Algorithms/OrbitThreader.cpp b/src/Algorithms/OrbitThreader.cpp
index a1a8e5f67befb81d0ffffef5f5e5112dd085e883..893fedd44150af66777d27f97ea1bd7fb81fd5ec 100644
--- a/src/Algorithms/OrbitThreader.cpp
+++ b/src/Algorithms/OrbitThreader.cpp
@@ -112,6 +112,7 @@ void OrbitThreader::execute() {
     std::set<std::string> visitedElements;
 
     trackBack();
+    updateBoundingBoxWithCurrentPosition();
 
     Vector_t nextR = r_m / (Physics::c * dt_m);
     integrator_m.push(nextR, p_m, dt_m);
@@ -414,6 +415,7 @@ void OrbitThreader::computeBoundingBox() {
     FieldList allElements = itsOpalBeamline_m.getElementByType(ElementBase::ANY);
     FieldList::iterator it = allElements.begin();
     const FieldList::iterator end = allElements.end();
+
     globalBoundingBox_m.lowerLeftCorner = Vector_t(std::numeric_limits<double>::max());
     globalBoundingBox_m.upperRightCorner = Vector_t(-std::numeric_limits<double>::max());
 
@@ -424,6 +426,15 @@ void OrbitThreader::computeBoundingBox() {
         ElementBase::BoundingBox other = it->getBoundingBoxInLabCoords();
         globalBoundingBox_m.getCombinedBoundingBox(other);
     }
+
+    updateBoundingBoxWithCurrentPosition();
+}
+
+
+void OrbitThreader::updateBoundingBoxWithCurrentPosition() {
+    Vector_t dR = Physics::c * dt_m * p_m / Util::getGamma(p_m);
+    ElementBase::BoundingBox pos = ElementBase::BoundingBox::getBoundingBox({r_m - 10 * dR, r_m + 10 * dR});
+    globalBoundingBox_m.getCombinedBoundingBox(pos);
 }
 
 double OrbitThreader::computeDriftLengthToBoundingBox(const std::set<std::shared_ptr<Component>> & elements,
diff --git a/src/Algorithms/OrbitThreader.h b/src/Algorithms/OrbitThreader.h
index 72d9407e7ffce7234dce9091f8e5531ba4fadd2c..37d985042a30b2e79a191db24acca5e64d49cffd 100644
--- a/src/Algorithms/OrbitThreader.h
+++ b/src/Algorithms/OrbitThreader.h
@@ -108,7 +108,7 @@ private:
     void processElementRegister();
     void setDesignEnergy(FieldList &allElements, const std::set<std::string> &visitedElements);
     void computeBoundingBox();
-    // double computeMaximalImplicitDrift();
+    void updateBoundingBoxWithCurrentPosition();
     double computeDriftLengthToBoundingBox(const std::set<std::shared_ptr<Component>> & elements,
                                            const Vector_t & position,
                                            const Vector_t & direction) const;
diff --git a/src/Classic/AbsBeamline/ElementBase.cpp b/src/Classic/AbsBeamline/ElementBase.cpp
index b81bcb39be4d061c61e1e12a4dd36e1b8d582cb8..8613c5197572e2bb7e5a4d6e0ecc59971b95cf60 100644
--- a/src/Classic/AbsBeamline/ElementBase.cpp
+++ b/src/Classic/AbsBeamline/ElementBase.cpp
@@ -309,6 +309,17 @@ bool ElementBase::isInsideTransverse(const Vector_t &r) const
     }
 }
 
+ElementBase::BoundingBox ElementBase::BoundingBox::getBoundingBox(const std::vector<Vector_t> & points) {
+    const Vector_t & point = points.front();
+    BoundingBox result{point, point};
+    for (const Vector_t & point: points) {
+        BoundingBox tmp{point, point};
+        result.getCombinedBoundingBox(tmp);
+    }
+
+    return result;
+}
+
 bool ElementBase::BoundingBox::isInside(const Vector_t & position) const {
     Vector_t relativePosition = position - lowerLeftCorner;
     Vector_t diagonal = upperRightCorner - lowerLeftCorner;
@@ -392,4 +403,26 @@ ElementBase::BoundingBox ElementBase::getBoundingBoxInLabCoords() const {
     }
 
     return bb;
+}
+
+void ElementBase::BoundingBox::print(std::ostream & out) const {
+    const Vector_t & ll = lowerLeftCorner;
+    const Vector_t & ur = upperRightCorner;
+    Vector_t diagonal = ur - ll;
+    Vector_t dX(diagonal(0), 0, 0), dY(0, diagonal(1), 0), dZ(0, 0, diagonal(2));
+
+    std::vector<Vector_t> corners{ll, ll + dX, ll + dX + dY, ll + dY, ur, ur - dX, ur - dX - dY, ur - dY};
+    std::vector<std::vector<unsigned int>> paths{{0, 1, 2, 3}, {0, 1, 7, 6}, {1, 2, 4, 7}, {2, 3, 5, 4}, {3, 0, 6, 5}, {4, 5, 6, 7}};
+
+    out << std::setprecision(8);
+    for (const std::vector<unsigned int> path: paths) {
+        for (unsigned int i : {0, 1, 2, 3, 0}) {
+            const Vector_t & corner = corners[path[i]];
+            out << std::setw(16) << corner(0)
+                << std::setw(16) << corner(1)
+                << std::setw(16) << corner(2)
+                << std::endl;
+        }
+        out << std::endl;
+    }
 }
\ No newline at end of file
diff --git a/src/Classic/AbsBeamline/ElementBase.h b/src/Classic/AbsBeamline/ElementBase.h
index 7cf48099982bf5ad407dfda2a6cad3d0a907d0c6..4c813e16cc359263b4bf3f0f9452d5a5ca40c6ef 100644
--- a/src/Classic/AbsBeamline/ElementBase.h
+++ b/src/Classic/AbsBeamline/ElementBase.h
@@ -345,6 +345,8 @@ public:
         Vector_t lowerLeftCorner;
         Vector_t upperRightCorner;
 
+        static BoundingBox getBoundingBox(const std::vector<Vector_t> & points);
+
         void getCombinedBoundingBox(const BoundingBox & other) {
             for (unsigned int d = 0; d < 3; ++ d) {
                 lowerLeftCorner[d] = std::min(lowerLeftCorner[d], other.lowerLeftCorner[d]);
@@ -354,6 +356,8 @@ public:
 
         bool isInside(const Vector_t &) const;
 
+        void print(std::ostream &) const;
+
         /*! Computes the intersection point between a bounding box and the ray which
          *  has the direction 'direction' and starts at the position 'position'. If
          *  the position is inside the box then the algorithm should find an inter-
@@ -609,4 +613,4 @@ inline
 bool ElementBase::isElementPositionSet() const
 { return elemedgeSet_m; }
 
-#endif // CLASSIC_ElementBase_HH
+#endif // CLASSIC_ElementBase_HH
\ No newline at end of file