diff --git a/Applications/DataExplorer/DataView/MeshElementRemovalDialog.cpp b/Applications/DataExplorer/DataView/MeshElementRemovalDialog.cpp
index e4538238281ac71e7b83b1ef67ae5e2919dce46d..ec37e986c9cc876788c734af873f35b64e0614ea 100644
--- a/Applications/DataExplorer/DataView/MeshElementRemovalDialog.cpp
+++ b/Applications/DataExplorer/DataView/MeshElementRemovalDialog.cpp
@@ -17,6 +17,7 @@
 #include <QList>
 #include <QListWidgetItem>
 #include <algorithm>
+#include<Eigen/StdVector>
 
 #include "Applications/DataExplorer/Base/OGSError.h"
 #include "Applications/DataHolderLib/Project.h"
@@ -142,9 +143,8 @@ void MeshElementRemovalDialog::accept()
             (aabb_edits[4]) ? this->zMinEdit->text().toDouble() : (minAABB[2]);
         maxAABB[2] =
             (aabb_edits[5]) ? this->zMaxEdit->text().toDouble() : (maxAABB[2]);
-        std::vector<MathLib::Point3d> extent;
-        extent.push_back(minAABB);
-        extent.push_back(maxAABB);
+        std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
+            extent{minAABB, maxAABB};
         const GeoLib::AABB updated_aabb(extent.begin(), extent.end());
         ex.searchByBoundingBox(updated_aabb);
         anything_checked = true;
diff --git a/Applications/Utils/FileConverter/Mesh2Raster.cpp b/Applications/Utils/FileConverter/Mesh2Raster.cpp
index 27ad44ee22b1e735c53cbb0c60d1b71aeb302afa..6e9e8acc213ee3124ab0b4f514bfa35619459b15 100644
--- a/Applications/Utils/FileConverter/Mesh2Raster.cpp
+++ b/Applications/Utils/FileConverter/Mesh2Raster.cpp
@@ -68,8 +68,8 @@ int main(int argc, char* argv[])
 
     std::vector<MeshLib::Node*> const& nodes_vec(mesh->getNodes());
     GeoLib::AABB const bounding_box(nodes_vec.begin(), nodes_vec.end());
-    MathLib::Point3d const& min(bounding_box.getMinPoint());
-    MathLib::Point3d const& max(bounding_box.getMaxPoint());
+    auto const& min(bounding_box.getMinPoint());
+    auto const& max(bounding_box.getMaxPoint());
     auto const n_cols =
         static_cast<std::size_t>(std::ceil((max[0] - min[0]) / cellsize));
     auto const n_rows =
diff --git a/Applications/Utils/MeshEdit/AddFaultToVoxelGrid.cpp b/Applications/Utils/MeshEdit/AddFaultToVoxelGrid.cpp
index 45e387e87438038b2371168d9267a3b81bc1387c..81d668e41f7b920d89cffa82c52df0ff441cda5b 100644
--- a/Applications/Utils/MeshEdit/AddFaultToVoxelGrid.cpp
+++ b/Applications/Utils/MeshEdit/AddFaultToVoxelGrid.cpp
@@ -110,7 +110,7 @@ void markFaults(MeshLib::Mesh& mesh, MeshLib::Mesh const& fault,
         min_pnt[i] -= half_cell_size[i];
         max_pnt[i] += half_cell_size[i];
     }
-    std::array<MathLib::Point3d, 2> const fault_extent{{min_pnt, max_pnt}};
+    std::array<Eigen::Vector3d, 2> const fault_extent{{min_pnt, max_pnt}};
     GeoLib::AABB const fault_aabb_ext(fault_extent.cbegin(),
                                       fault_extent.cend());
 
diff --git a/GeoLib/AABB.h b/GeoLib/AABB.h
index efef8bb1b69d1b5cbd6b155948bce728adf3361a..c63875b42c0ec957356678df8289c02a6a4bf84e 100644
--- a/GeoLib/AABB.h
+++ b/GeoLib/AABB.h
@@ -24,9 +24,9 @@
 #include <tuple>
 #include <vector>
 
+#include <Eigen/Eigen>
+
 #include "BaseLib/Error.h"
-#include "MathLib/MathTools.h"
-#include "MathLib/Point3d.h"
 
 namespace GeoLib
 {
@@ -171,14 +171,14 @@ public:
      * for the given point set
      * @return a point
      */
-    MathLib::Point3d const& getMinPoint() const { return _min_pnt; }
+    Eigen::Vector3d const& getMinPoint() const { return _min_pnt; }
 
     /**
      * returns a point that coordinates are maximal for each dimension
      * within the given point set
      * @return a point
      */
-    MathLib::Point3d const& getMaxPoint() const { return _max_pnt; }
+    Eigen::Vector3d const& getMaxPoint() const { return _max_pnt; }
 
     /**
      * Method checks if the given AABB object is contained within the
@@ -193,14 +193,13 @@ public:
                containsPoint(other_aabb.getMaxPoint(), 0);
     }
 
-protected:
-    MathLib::Point3d _min_pnt = MathLib::Point3d{std::array<double, 3>{
-        {std::numeric_limits<double>::max(), std::numeric_limits<double>::max(),
-         std::numeric_limits<double>::max()}}};
-    MathLib::Point3d _max_pnt = MathLib::Point3d{
-        std::array<double, 3>{{std::numeric_limits<double>::lowest(),
-                               std::numeric_limits<double>::lowest(),
-                               std::numeric_limits<double>::lowest()}}};
+private:
+    Eigen::Vector3d _min_pnt{std::numeric_limits<double>::max(),
+                             std::numeric_limits<double>::max(),
+                             std::numeric_limits<double>::max()};
+    Eigen::Vector3d _max_pnt{std::numeric_limits<double>::lowest(),
+                             std::numeric_limits<double>::lowest(),
+                             std::numeric_limits<double>::lowest()};
 
 private:
     /// Enlarge the bounding box the smallest possible amount (modifying the
diff --git a/GeoLib/Grid.h b/GeoLib/Grid.h
index 49dc22e51e8f274a0e68aea7027690580a340218..0d6900a6df8654248c8f0602a8ba5ba05ee08a70 100644
--- a/GeoLib/Grid.h
+++ b/GeoLib/Grid.h
@@ -14,6 +14,7 @@
 
 #pragma once
 
+#include <array>
 #include <bitset>
 #include <vector>
 
@@ -21,16 +22,14 @@
 
 // GeoLib
 #include "AABB.h"
+#ifndef NDEBUG
 #include "GEOObjects.h"
-
-// MathLib
-#include "MathLib/MathTools.h"
-#include "MathLib/Point3d.h"
+#endif
 
 namespace GeoLib
 {
 template <typename POINT>
-class Grid : public GeoLib::AABB
+class Grid final : public GeoLib::AABB
 {
 public:
     /**
@@ -97,8 +96,8 @@ public:
                                           double half_len) const;
 
     void getPntVecsOfGridCellsIntersectingCuboid(
-        MathLib::Point3d const& min_pnt,
-        MathLib::Point3d const& max_pnt,
+        Eigen::Vector3d const& min_pnt,
+        Eigen::Vector3d const& max_pnt,
         std::vector<std::vector<POINT*> const*>& pnts) const;
 
 #ifndef NDEBUG
@@ -197,9 +196,10 @@ Grid<POINT>::Grid(InputIterator first, InputIterator last,
 {
     auto const n_pnts(std::distance(first, last));
 
-    std::array<double, 3> delta = {{_max_pnt[0] - _min_pnt[0],
-                                    _max_pnt[1] - _min_pnt[1],
-                                    _max_pnt[2] - _min_pnt[2]}};
+    auto const& max{getMaxPoint()};
+    auto const& min{getMinPoint()};
+    std::array<double, 3> delta = {
+        {max[0] - min[0], max[1] - min[1], max[2] - min[2]}};
 
     // enlarge delta
     for (auto& d : delta)
@@ -254,8 +254,8 @@ Grid<POINT>::getPntVecsOfGridCellsIntersectingCube(P const& center,
                                                    double half_len) const
 {
     std::vector<std::vector<POINT*> const*> pnts;
-    MathLib::Point3d tmp_pnt{{{center[0] - half_len, center[1] - half_len,
-                               center[2] - half_len}}};  // min
+    POINT tmp_pnt{center[0] - half_len, center[1] - half_len,
+                  center[2] - half_len};  // min
     std::array<std::size_t, 3> min_coords(getGridCoords(tmp_pnt));
 
     tmp_pnt[0] = center[0] + half_len;
@@ -285,8 +285,8 @@ Grid<POINT>::getPntVecsOfGridCellsIntersectingCube(P const& center,
 
 template <typename POINT>
 void Grid<POINT>::getPntVecsOfGridCellsIntersectingCuboid(
-    MathLib::Point3d const& min_pnt,
-    MathLib::Point3d const& max_pnt,
+    Eigen::Vector3d const& min_pnt,
+    Eigen::Vector3d const& max_pnt,
     std::vector<std::vector<POINT*> const*>& pnts) const
 {
     std::array<std::size_t, 3> min_coords(getGridCoords(min_pnt));
@@ -419,23 +419,25 @@ template <typename POINT>
 template <typename T>
 std::array<std::size_t, 3> Grid<POINT>::getGridCoords(T const& pnt) const
 {
+    auto const& min_point{getMinPoint()};
+    auto const& max_point{getMinPoint()};
     std::array<std::size_t, 3> coords{};
     for (std::size_t k(0); k < 3; k++)
     {
-        if (pnt[k] < _min_pnt[k])
+        if (pnt[k] < min_point[k])
         {
             coords[k] = 0;
         }
         else
         {
-            if (pnt[k] >= _max_pnt[k])
+            if (pnt[k] >= max_point[k])
             {
                 coords[k] = _n_steps[k] - 1;
             }
             else
             {
                 coords[k] = static_cast<std::size_t>(
-                    std::floor((pnt[k] - _min_pnt[k])) /
+                    std::floor((pnt[k] - min_point[k])) /
                     std::nextafter(_step_sizes[k],
                                    std::numeric_limits<double>::max()));
             }
@@ -449,20 +451,21 @@ template <typename P>
 std::array<double, 6> Grid<POINT>::getPointCellBorderDistances(
     P const& pnt, std::array<std::size_t, 3> const& coords) const
 {
+    auto const& min_point{getMinPoint()};
     std::array<double, 6> dists{};
     dists[0] =
-        std::abs(pnt[2] - _min_pnt[2] + coords[2] * _step_sizes[2]);  // bottom
-    dists[5] = std::abs(pnt[2] - _min_pnt[2] +
+        std::abs(pnt[2] - min_point[2] + coords[2] * _step_sizes[2]);  // bottom
+    dists[5] = std::abs(pnt[2] - min_point[2] +
                         (coords[2] + 1) * _step_sizes[2]);  // top
 
     dists[1] =
-        std::abs(pnt[1] - _min_pnt[1] + coords[1] * _step_sizes[1]);  // front
-    dists[3] = std::abs(pnt[1] - _min_pnt[1] +
+        std::abs(pnt[1] - min_point[1] + coords[1] * _step_sizes[1]);  // front
+    dists[3] = std::abs(pnt[1] - min_point[1] +
                         (coords[1] + 1) * _step_sizes[1]);  // back
 
     dists[4] =
-        std::abs(pnt[0] - _min_pnt[0] + coords[0] * _step_sizes[0]);  // left
-    dists[2] = std::abs(pnt[0] - _min_pnt[0] +
+        std::abs(pnt[0] - min_point[0] + coords[0] * _step_sizes[0]);  // left
+    dists[2] = std::abs(pnt[0] - min_point[0] +
                         (coords[0] + 1) * _step_sizes[0]);  // right
     return dists;
 }
@@ -472,8 +475,10 @@ template <typename P>
 POINT* Grid<POINT>::getNearestPoint(P const& pnt) const
 {
     std::array<std::size_t, 3> coords(getGridCoords(pnt));
+    auto const& min_point{getMinPoint()};
+    auto const& max_point{getMaxPoint()};
 
-    double sqr_min_dist(MathLib::sqrDist(_min_pnt, _max_pnt));
+    double sqr_min_dist = (max_point - min_point).squaredNorm();
     POINT* nearest_pnt(nullptr);
 
     std::array<double, 6> dists(getPointCellBorderDistances(pnt, coords));
@@ -536,7 +541,10 @@ POINT* Grid<POINT>::getNearestPoint(P const& pnt) const
         }  // end while
     }      // end else
 
-    double len(std::sqrt(MathLib::sqrDist(pnt, *nearest_pnt)));
+    auto to_eigen = [](auto const& point)
+    { return Eigen::Map<Eigen::Vector3d const>(point.getCoords()); };
+
+    double len((to_eigen(pnt) - to_eigen(*nearest_pnt)).norm());
     // search all other grid cells within the cube with the edge nodes
     std::vector<std::vector<POINT*> const*> vecs_of_pnts(
         getPntVecsOfGridCellsIntersectingCube(pnt, len));
@@ -548,7 +556,8 @@ POINT* Grid<POINT>::getNearestPoint(P const& pnt) const
         const std::size_t n_pnts(pnts.size());
         for (std::size_t k(0); k < n_pnts; k++)
         {
-            const double sqr_dist(MathLib::sqrDist(pnt, *pnts[k]));
+            const double sqr_dist(
+                (to_eigen(pnt) - to_eigen(*pnts[k])).squaredNorm());
             if (sqr_dist < sqr_min_dist)
             {
                 sqr_min_dist = sqr_dist;
@@ -652,12 +661,16 @@ bool Grid<POINT>::calcNearestPointInGridCell(
     if (pnts.empty())
         return false;
 
+    auto to_eigen = [](auto const& point)
+    { return Eigen::Map<Eigen::Vector3d const>(point.getCoords()); };
+
     const std::size_t n_pnts(pnts.size());
-    sqr_min_dist = MathLib::sqrDist(*pnts[0], pnt);
+    sqr_min_dist = (to_eigen(*pnts[0]) - to_eigen(pnt)).squaredNorm();
     nearest_pnt = pnts[0];
     for (std::size_t i(1); i < n_pnts; i++)
     {
-        const double sqr_dist(MathLib::sqrDist(*pnts[i], pnt));
+        const double sqr_dist(
+            (to_eigen(*pnts[i]) - to_eigen(pnt)).squaredNorm());
         if (sqr_dist < sqr_min_dist)
         {
             sqr_min_dist = sqr_dist;
@@ -677,12 +690,15 @@ std::vector<std::size_t> Grid<POINT>::getPointsInEpsilonEnvironment(
 
     double const sqr_eps(eps * eps);
 
+    auto to_eigen = [](auto const& point)
+    { return Eigen::Map<Eigen::Vector3d const>(point.getCoords()); };
+
     std::vector<std::size_t> pnts;
     for (auto vec : vec_pnts)
     {
         for (auto const p : *vec)
         {
-            if (MathLib::sqrDist(*p, pnt) <= sqr_eps)
+            if ((to_eigen(*p) - to_eigen(pnt)).squaredNorm() <= sqr_eps)
             {
                 pnts.push_back(p->getID());
             }
diff --git a/GeoLib/OctTree-impl.h b/GeoLib/OctTree-impl.h
index e044e7609e6ecb80bac497a7ba292f76dcff285c..45ea89d9feba8c4b2645a05f732f8f13beb973ba 100644
--- a/GeoLib/OctTree-impl.h
+++ b/GeoLib/OctTree-impl.h
@@ -13,9 +13,8 @@
 namespace GeoLib
 {
 template <typename POINT, std::size_t MAX_POINTS>
-template <typename T>
 OctTree<POINT, MAX_POINTS>* OctTree<POINT, MAX_POINTS>::createOctTree(
-    T ll, T ur, double eps)
+    Eigen::Vector3d ll, Eigen::Vector3d ur, double eps)
 {
     // compute an axis aligned cube around the points ll and ur
     const double dx(ur[0] - ll[0]);
@@ -60,7 +59,9 @@ OctTree<POINT, MAX_POINTS>* OctTree<POINT, MAX_POINTS>::createOctTree(
             ur[k] += eps;
         }
     }
-    return new OctTree<POINT, MAX_POINTS>(ll, ur, eps);
+    Eigen::Vector3d lower_left{ll[0], ll[1], ll[2]};
+    Eigen::Vector3d upper_right{ur[0], ur[1], ur[2]};
+    return new OctTree<POINT, MAX_POINTS>(lower_left, upper_right, eps);
 }
 
 template <typename POINT, std::size_t MAX_POINTS>
@@ -77,15 +78,19 @@ bool OctTree<POINT, MAX_POINTS>::addPoint(POINT* pnt, POINT*& ret_pnt)
 {
     // first do a range query using a epsilon box around the point pnt
     std::vector<POINT*> query_pnts;
-    MathLib::Point3d min(std::array<double, 3>{
-        {(*pnt)[0] - _eps, (*pnt)[1] - _eps, (*pnt)[2] - _eps}});
-    MathLib::Point3d max(std::array<double, 3>{
-        {(*pnt)[0] + _eps, (*pnt)[1] + _eps, (*pnt)[2] + _eps}});
+    Eigen::Vector3d const min =
+        Eigen::Map<Eigen::Vector3d>(pnt->getCoords()).array() - _eps;
+    Eigen::Vector3d const max =
+        Eigen::Map<Eigen::Vector3d>(pnt->getCoords()).array() + _eps;
     getPointsInRange(min, max, query_pnts);
-    auto const it =
-        std::find_if(query_pnts.begin(), query_pnts.end(),
-                     [pnt, this](auto const* p)
-                     { return MathLib::sqrDist(*p, *pnt) < _eps * _eps; });
+    auto const it = std::find_if(
+        query_pnts.begin(), query_pnts.end(),
+        [pnt, this](auto const* p)
+        {
+            return (Eigen::Map<Eigen::Vector3d const>(p->getCoords()) -
+                    Eigen::Map<Eigen::Vector3d const>(pnt->getCoords()))
+                       .squaredNorm() < _eps * _eps;
+        });
     if (it != query_pnts.end())
     {
         ret_pnt = *it;
@@ -164,8 +169,8 @@ void OctTree<POINT, MAX_POINTS>::getPointsInRange(
 }
 
 template <typename POINT, std::size_t MAX_POINTS>
-OctTree<POINT, MAX_POINTS>::OctTree(MathLib::Point3d const& ll,
-                                    MathLib::Point3d const& ur, double eps)
+OctTree<POINT, MAX_POINTS>::OctTree(Eigen::Vector3d const& ll,
+                                    Eigen::Vector3d const& ur, double eps)
     : _ll(ll), _ur(ur), _is_leaf(true), _eps(eps)
 {
     _children.fill(nullptr);
@@ -235,8 +240,8 @@ void OctTree<POINT, MAX_POINTS>::splitNode(POINT* pnt)
     const double x_mid((_ur[0] + _ll[0]) / 2.0);
     const double y_mid((_ur[1] + _ll[1]) / 2.0);
     const double z_mid((_ur[2] + _ll[2]) / 2.0);
-    MathLib::Point3d p0(std::array<double, 3>{{x_mid, y_mid, _ll[2]}});
-    MathLib::Point3d p1(std::array<double, 3>{{_ur[0], _ur[1], z_mid}});
+    Eigen::Vector3d p0{x_mid, y_mid, _ll[2]};
+    Eigen::Vector3d p1{_ur[0], _ur[1], z_mid};
 
     // create child NEL
     _children[static_cast<std::int8_t>(Quadrant::NEL)] =
diff --git a/GeoLib/OctTree.h b/GeoLib/OctTree.h
index 3b1d17ff42a7cffe2e472449a87cb863c296ae94..b780945234ea329bf6dd788d980574e5df87263a 100644
--- a/GeoLib/OctTree.h
+++ b/GeoLib/OctTree.h
@@ -14,13 +14,11 @@
 
 #pragma once
 
+#include <Eigen/Eigen>
 #include <cstdint>
 #include <limits>
 #include <vector>
 
-#include "MathLib/MathTools.h"
-#include "MathLib/Point3d.h"
-
 namespace GeoLib
 {
 /// @tparam POINT point data type the OctTree will use
@@ -45,9 +43,9 @@ public:
     /// the memory requirements for the OctTree may be lower but the search
     /// inside a OctTree leaf may be more expensive. The value should be
     /// chosen application dependent. [default 8]
-    template <typename T>
     static OctTree<POINT, MAX_POINTS>* createOctTree(
-        T ll, T ur, double eps = std::numeric_limits<double>::epsilon());
+        Eigen::Vector3d ll, Eigen::Vector3d ur,
+        double eps = std::numeric_limits<double>::epsilon());
 
     /// Destroys the children of this node. @attention Does not destroy the
     /// pointers to the managed objects.
@@ -76,8 +74,8 @@ public:
                           std::vector<POINT*>& pnts) const;
 
 #ifndef NDEBUG
-    MathLib::Point3d const& getLowerLeftCornerPoint() const { return _ll; }
-    MathLib::Point3d const& getUpperRightCornerPoint() const { return _ur; }
+    Eigen::Vector3d const& getLowerLeftCornerPoint() const { return _ll; }
+    Eigen::Vector3d const& getUpperRightCornerPoint() const { return _ur; }
     OctTree<POINT, MAX_POINTS> const* getChild(std::size_t i) const
     {
         return _children[i];
@@ -90,7 +88,7 @@ private:
     /// @param ll lower left point
     /// @param ur upper right point
     /// @param eps the euclidean distance as a threshold to make objects unique
-    OctTree(MathLib::Point3d const& ll, MathLib::Point3d const& ur, double eps);
+    OctTree(Eigen::Vector3d const& ll, Eigen::Vector3d const& ur, double eps);
 
     enum class Quadrant : std::int8_t
     {
@@ -140,9 +138,9 @@ private:
     ///   _children[7] is south east upper child
     std::array<OctTree<POINT, MAX_POINTS>*, 8> _children;
     /// lower left front face point of the cube
-    MathLib::Point3d const _ll;
+    Eigen::Vector3d const _ll;
     /// upper right back face point of the cube
-    MathLib::Point3d const _ur;
+    Eigen::Vector3d const _ur;
     /// vector of pointers to POINT objects
     std::vector<POINT*> _pnts;
     /// flag if this OctTree is a leaf
diff --git a/GeoLib/PointVec.cpp b/GeoLib/PointVec.cpp
index 6822171f434cd1f1a421e3b177deed1a24649ff9..15b428ba699cb6c824b0599ac51f938465d74efc 100644
--- a/GeoLib/PointVec.cpp
+++ b/GeoLib/PointVec.cpp
@@ -28,8 +28,7 @@ PointVec::PointVec(
     : TemplateVec<Point>(name, std::move(points), std::move(name_id_map)),
       _type(type),
       _aabb(_data_vec->begin(), _data_vec->end()),
-      _rel_eps(rel_eps * std::sqrt(MathLib::sqrDist(_aabb.getMinPoint(),
-                                                    _aabb.getMaxPoint()))),
+      _rel_eps(rel_eps * (_aabb.getMaxPoint() - _aabb.getMinPoint()).norm()),
       _oct_tree(OctTree<GeoLib::Point, 16>::createOctTree(
           _aabb.getMinPoint(), _aabb.getMaxPoint(), _rel_eps))
 {
@@ -257,14 +256,13 @@ void PointVec::setNameForElement(std::size_t id, std::string const& name)
 
 void PointVec::resetInternalDataStructures()
 {
-    MathLib::Point3d const& min(_aabb.getMinPoint());
-    MathLib::Point3d const& max(_aabb.getMaxPoint());
-    double const rel_eps(_rel_eps / std::sqrt(MathLib::sqrDist(min, max)));
+    auto const& min(_aabb.getMinPoint());
+    auto const& max(_aabb.getMaxPoint());
+    double const rel_eps(_rel_eps / (max-min).norm());
 
     _aabb = GeoLib::AABB(_data_vec->begin(), _data_vec->end());
 
-    _rel_eps = rel_eps * std::sqrt(MathLib::sqrDist(_aabb.getMinPoint(),
-                                                    _aabb.getMaxPoint()));
+    _rel_eps = rel_eps * (_aabb.getMaxPoint() - _aabb.getMinPoint()).norm();
 
     _oct_tree.reset(OctTree<GeoLib::Point, 16>::createOctTree(
         _aabb.getMinPoint(), _aabb.getMaxPoint(), _rel_eps));
diff --git a/GeoLib/Polygon.cpp b/GeoLib/Polygon.cpp
index 36bb3dab438b33748c0d4c0b715c09fa435e3ea7..f4b3c43440a9088088ac37b66f44b5fb3d00efd8 100644
--- a/GeoLib/Polygon.cpp
+++ b/GeoLib/Polygon.cpp
@@ -70,8 +70,8 @@ bool Polygon::initialise()
 
 bool Polygon::isPntInPolygon(GeoLib::Point const& pnt) const
 {
-    MathLib::Point3d const& min_aabb_pnt(_aabb.getMinPoint());
-    MathLib::Point3d const& max_aabb_pnt(_aabb.getMaxPoint());
+    auto const& min_aabb_pnt(_aabb.getMinPoint());
+    auto const& max_aabb_pnt(_aabb.getMaxPoint());
 
     if (pnt[0] < min_aabb_pnt[0] || max_aabb_pnt[0] < pnt[0] ||
         pnt[1] < min_aabb_pnt[1] || max_aabb_pnt[1] < pnt[1])
diff --git a/GeoLib/Polyline.cpp b/GeoLib/Polyline.cpp
index c6feec89399eed3fbc1b38cb42507de0cbc62824..ac6aa949b515b80630741cc77da6ea328c6ff02e 100644
--- a/GeoLib/Polyline.cpp
+++ b/GeoLib/Polyline.cpp
@@ -20,6 +20,7 @@
 #include "BaseLib/Error.h"
 #include "BaseLib/Logging.h"
 #include "MathLib/GeometricBasics.h"
+#include "MathLib/MathTools.h"
 
 namespace GeoLib
 {
diff --git a/GeoLib/SurfaceGrid.cpp b/GeoLib/SurfaceGrid.cpp
index c904e991d979ca1186f88fd83eef9772d5951f2e..a2677bfb1237aab0fb3ac638c4cec9afeceb0369 100644
--- a/GeoLib/SurfaceGrid.cpp
+++ b/GeoLib/SurfaceGrid.cpp
@@ -25,56 +25,53 @@ namespace GeoLib
 SurfaceGrid::SurfaceGrid(Surface const* const sfc)
     : AABB(sfc->getAABB()), _n_steps({{1, 1, 1}})
 {
+    auto min_point{getMinPoint()};
+    auto max_point{getMaxPoint()};
     // enlarge the bounding, such that the points with maximal coordinates
     // fits into the grid
     for (std::size_t k(0); k < 3; ++k)
     {
-        _max_pnt[k] += std::abs(_max_pnt[k]) * 1e-6;
-        if (std::abs(_max_pnt[k]) < std::numeric_limits<double>::epsilon())
+        max_point[k] += std::abs(max_point[k]) * 1e-6;
+        if (std::abs(max_point[k]) < std::numeric_limits<double>::epsilon())
         {
-            _max_pnt[k] = (_max_pnt[k] - _min_pnt[k]) * (1.0 + 1e-6);
+            max_point[k] = (max_point[k] - min_point[k]) * (1.0 + 1e-6);
         }
     }
 
-    std::array<double, 3> delta{{_max_pnt[0] - _min_pnt[0],
-                                 _max_pnt[1] - _min_pnt[1],
-                                 _max_pnt[2] - _min_pnt[2]}};
+    Eigen::Vector3d delta = max_point - min_point;
 
     if (delta[0] < std::numeric_limits<double>::epsilon())
     {
         const double max_delta(std::max(delta[1], delta[2]));
-        _min_pnt[0] -= max_delta * 0.5e-3;
-        _max_pnt[0] += max_delta * 0.5e-3;
-        delta[0] = _max_pnt[0] - _min_pnt[0];
+        min_point[0] -= max_delta * 0.5e-3;
+        max_point[0] += max_delta * 0.5e-3;
+        delta[0] = max_point[0] - min_point[0];
     }
 
     if (delta[1] < std::numeric_limits<double>::epsilon())
     {
         const double max_delta(std::max(delta[0], delta[2]));
-        _min_pnt[1] -= max_delta * 0.5e-3;
-        _max_pnt[1] += max_delta * 0.5e-3;
-        delta[1] = _max_pnt[1] - _min_pnt[1];
+        min_point[1] -= max_delta * 0.5e-3;
+        max_point[1] += max_delta * 0.5e-3;
+        delta[1] = max_point[1] - min_point[1];
     }
 
     if (delta[2] < std::numeric_limits<double>::epsilon())
     {
         const double max_delta(std::max(delta[0], delta[1]));
-        _min_pnt[2] -= max_delta * 0.5e-3;
-        _max_pnt[2] += max_delta * 0.5e-3;
-        delta[2] = _max_pnt[2] - _min_pnt[2];
+        min_point[2] -= max_delta * 0.5e-3;
+        max_point[2] += max_delta * 0.5e-3;
+        delta[2] = max_point[2] - min_point[2];
     }
 
+    update(min_point);
+    update(max_point);
+
     const std::size_t n_tris(sfc->getNumberOfTriangles());
     const std::size_t n_tris_per_cell(5);
 
-    std::bitset<3> dim;  // all bits are set to zero.
-    for (std::size_t k(0); k < 3; ++k)
-    {
-        if (std::abs(delta[k]) >= std::numeric_limits<double>::epsilon())
-        {
-            dim[k] = true;
-        }
-    }
+    Eigen::Matrix<bool, 3, 1> dim =
+        delta.array() >= std::numeric_limits<double>::epsilon();
 
     // *** condition: n_tris / n_cells < n_tris_per_cell
     //                where n_cells = _n_steps[0] * _n_steps[1] * _n_steps[2]
@@ -83,9 +80,8 @@ SurfaceGrid::SurfaceGrid(Surface const* const sfc)
     // 1/3.)));
     //          _n_steps[1] = _n_steps[0] * delta[1]/delta[0],
     //          _n_steps[2] = _n_steps[0] * delta[2]/delta[0]
-    auto sc_ceil = [](double v) {
-        return static_cast<std::size_t>(std::ceil(v));
-    };
+    auto sc_ceil = [](double v)
+    { return static_cast<std::size_t>(std::ceil(v)); };
     switch (dim.count())
     {
         case 3:  // 3d case
@@ -133,7 +129,14 @@ SurfaceGrid::SurfaceGrid(Surface const* const sfc)
     for (std::size_t k(0); k < 3; k++)
     {
         _step_sizes[k] = delta[k] / _n_steps[k];
-        _inverse_step_sizes[k] = 1.0 / _step_sizes[k];
+        if (delta[k] > std::numeric_limits<double>::epsilon())
+        {
+            _inverse_step_sizes[k] = 1.0 / _step_sizes[k];
+        }
+        else
+        {
+            _inverse_step_sizes[k] = 0;
+        }
     }
 
     _triangles_in_grid_box.resize(_n_steps[0] * _n_steps[1] * _n_steps[2]);
@@ -149,11 +152,14 @@ void SurfaceGrid::sortTrianglesInGridCells(Surface const* const sfc)
             Point const& p0(*((*sfc)[l]->getPoint(0)));
             Point const& p1(*((*sfc)[l]->getPoint(1)));
             Point const& p2(*((*sfc)[l]->getPoint(2)));
-            ERR("Sorting triangle {:d} [({:f},{:f},{:f}), ({:f},{:f},{:f}), "
-                "({:f},{:f},{:f}) into grid.",
+            auto const& min{getMinPoint()};
+            auto const& max{getMaxPoint()};
+            OGS_FATAL(
+                "Sorting triangle {:d} [({:f},{:f},{:f}), ({:f},{:f},{:f}), "
+                "({:f},{:f},{:f}) into grid. Bounding box is [{:f}, {:f}] x "
+                "[{:f}, {:f}] x [{:f}, {:f}].",
                 l, p0[0], p0[1], p0[2], p1[0], p1[1], p1[2], p2[0], p2[1],
-                p2[2]);
-            OGS_FATAL("");
+                p2[2], min[0], max[0], min[1], max[1], min[2], max[2]);
         }
     }
 }
@@ -215,12 +221,13 @@ bool SurfaceGrid::sortTriangleInGridCells(Triangle const* const triangle)
 std::optional<std::array<std::size_t, 3>> SurfaceGrid::getGridCellCoordinates(
     MathLib::Point3d const& p) const
 {
+    auto const& min_point{getMinPoint()};
     std::array<std::size_t, 3> coords{
-        {static_cast<std::size_t>((p[0] - _min_pnt[0]) *
+        {static_cast<std::size_t>((p[0] - min_point[0]) *
                                   _inverse_step_sizes[0]),
-         static_cast<std::size_t>((p[1] - _min_pnt[1]) *
+         static_cast<std::size_t>((p[1] - min_point[1]) *
                                   _inverse_step_sizes[1]),
-         static_cast<std::size_t>((p[2] - _min_pnt[2]) *
+         static_cast<std::size_t>((p[2] - min_point[2]) *
                                   _inverse_step_sizes[2])}};
 
     if (coords[0] >= _n_steps[0] || coords[1] >= _n_steps[1] ||
diff --git a/GeoLib/SurfaceGrid.h b/GeoLib/SurfaceGrid.h
index c72914a528ee57cd14539538c2e6c0af6480fcd9..6066a714bf3a06baf51315cf0734da2676c544a8 100644
--- a/GeoLib/SurfaceGrid.h
+++ b/GeoLib/SurfaceGrid.h
@@ -26,7 +26,7 @@ namespace GeoLib {
 class Triangle;
 class Surface;
 
-class SurfaceGrid : public AABB {
+class SurfaceGrid final : public AABB {
 public:
     explicit SurfaceGrid(GeoLib::Surface const*const sfc);
     bool isPointInSurface(MathLib::Point3d const & pnt,
diff --git a/MeshGeoToolsLib/ConstructMeshesFromGeometries.cpp b/MeshGeoToolsLib/ConstructMeshesFromGeometries.cpp
index 7989e3f3380fdcca49fe7ac28489e3bafe1f5aa8..9035a7ff84f3ad9b08a0f25ce079bc4c6018a3e4 100644
--- a/MeshGeoToolsLib/ConstructMeshesFromGeometries.cpp
+++ b/MeshGeoToolsLib/ConstructMeshesFromGeometries.cpp
@@ -10,6 +10,7 @@
 #include "ConstructMeshesFromGeometries.h"
 
 #include "BaseLib/Logging.h"
+#include "GeoLib/GEOObjects.h"
 #include "BoundaryElementsSearcher.h"
 #include "MeshLib/Elements/Element.h"
 #include "MeshLib/MeshEditing/DuplicateMeshComponents.h"
diff --git a/MeshGeoToolsLib/GeoMapper.cpp b/MeshGeoToolsLib/GeoMapper.cpp
index 1785814235579f30e3d767ade724fb5ba9b9ddc7..f54ea043cf4c74a5df123bdfd30ab00b5e888021 100644
--- a/MeshGeoToolsLib/GeoMapper.cpp
+++ b/MeshGeoToolsLib/GeoMapper.cpp
@@ -479,8 +479,14 @@ getCandidateElementsForLineSegmentIntersection(
         {seg_deep_copy.getBeginPoint(), seg_deep_copy.getEndPoint()}};
     GeoLib::AABB aabb(pnts.cbegin(), pnts.cend());
 
-    auto candidate_elements = mesh_element_grid.getElementsInVolume(
-        aabb.getMinPoint(), aabb.getMaxPoint());
+    // TODO TF: remove after getElementsInVolume interface change
+    auto convert_to_Point3d = [](Eigen::Vector3d const& v) {
+        return MathLib::Point3d{std::array{v[0], v[1], v[2]}};
+    };
+
+    auto const min = convert_to_Point3d(aabb.getMinPoint());
+    auto const max = convert_to_Point3d(aabb.getMaxPoint());
+    auto candidate_elements = mesh_element_grid.getElementsInVolume(min, max);
 
     // make candidate elements unique
     BaseLib::makeVectorUnique(candidate_elements);
diff --git a/MeshGeoToolsLib/GeoMapper.h b/MeshGeoToolsLib/GeoMapper.h
index fe97ff83317e63f7cc10af09af20d94cdc0742f3..82757ec60dae0cb07e1bb877e7d03067883b4500 100644
--- a/MeshGeoToolsLib/GeoMapper.h
+++ b/MeshGeoToolsLib/GeoMapper.h
@@ -17,9 +17,9 @@
 #include <cstddef>
 #include <vector>
 
-#include "GeoLib/Point.h"
+#include "GeoLib/GEOObjects.h"
 #include "GeoLib/Grid.h"
-
+#include "GeoLib/Point.h"
 #include "MathLib/Point3d.h"
 
 namespace MeshLib {
diff --git a/MeshGeoToolsLib/MeshNodeSearcher.cpp b/MeshGeoToolsLib/MeshNodeSearcher.cpp
index 7af112eb6b31b40c5927849d7483002534d572ef..9f578e0e016d4e60089ce3b99dcbd0a4604dbd45 100644
--- a/MeshGeoToolsLib/MeshNodeSearcher.cpp
+++ b/MeshGeoToolsLib/MeshNodeSearcher.cpp
@@ -17,6 +17,7 @@
 #include "BaseLib/Logging.h"
 #include "GeoLib/Point.h"
 #include "GeoLib/Polyline.h"
+#include "GeoLib/Surface.h"
 #include "HeuristicSearchLength.h"
 #include "MeshLib/Elements/Element.h"
 #include "MeshLib/Mesh.h"
diff --git a/MeshGeoToolsLib/MeshNodeSearcher.h b/MeshGeoToolsLib/MeshNodeSearcher.h
index 9caea2e8f3e4a36306982b84f2b00cf66f6ae0d1..e85aef00adc6497fc480a52212f79a70dce8bdb1 100644
--- a/MeshGeoToolsLib/MeshNodeSearcher.h
+++ b/MeshGeoToolsLib/MeshNodeSearcher.h
@@ -13,12 +13,10 @@
 #include <memory>
 #include <vector>
 
-// GeoLib
 #include "GeoLib/Grid.h"
-
-// MeshGeoToolsLib
-#include "MeshGeoToolsLib/SearchLength.h"
+#include "MathLib/Point3dWithID.h"
 #include "MeshGeoToolsLib/SearchAllNodes.h"
+#include "MeshGeoToolsLib/SearchLength.h"
 
 // forward declaration
 namespace GeoLib
diff --git a/MeshLib/CoordinateSystem.cpp b/MeshLib/CoordinateSystem.cpp
index f50feec578f1b11459e04253eface2949041c4a7..83aecfcb4ba0604d2d4616a9bc9d1c8f19ebe8f8 100644
--- a/MeshLib/CoordinateSystem.cpp
+++ b/MeshLib/CoordinateSystem.cpp
@@ -45,10 +45,8 @@ unsigned char CoordinateSystem::getCoordinateSystem(
 {
     unsigned char coords = 0;
 
-    auto const bbox_min =
-        Eigen::Map<Eigen::Vector3d const>(bbox.getMinPoint().getCoords());
-    auto const bbox_max =
-        Eigen::Map<Eigen::Vector3d const>(bbox.getMaxPoint().getCoords());
+    auto const& bbox_min = bbox.getMinPoint();
+    auto const& bbox_max = bbox.getMaxPoint();
     Eigen::Vector3d const pt_diff = bbox_max - bbox_min;
 
     // The axis aligned bounding box is a from the right half-open interval.
diff --git a/MeshLib/MeshSearch/MeshElementGrid.cpp b/MeshLib/MeshSearch/MeshElementGrid.cpp
index 1599a65ff50fe006b34da9b5ec00bc56eab8a688..5179afa35fe58c3ba894b16ba88f9ba5175e38cf 100644
--- a/MeshLib/MeshSearch/MeshElementGrid.cpp
+++ b/MeshLib/MeshSearch/MeshElementGrid.cpp
@@ -26,8 +26,8 @@ MeshElementGrid::MeshElementGrid(MeshLib::Mesh const& mesh)
     : _aabb{mesh.getNodes().cbegin(), mesh.getNodes().cend()},
       _n_steps({{1, 1, 1}})
 {
-    auto getDimensions = [](MathLib::Point3d const& min,
-                            MathLib::Point3d const& max) {
+    auto getDimensions = [](auto const& min, auto const& max)
+    {
         std::bitset<3> dim;  // all bits are set to zero.
         for (std::size_t k(0); k < 3; ++k)
         {
@@ -42,8 +42,8 @@ MeshElementGrid::MeshElementGrid(MeshLib::Mesh const& mesh)
         return dim;
     };
 
-    MathLib::Point3d const& min_pnt(_aabb.getMinPoint());
-    MathLib::Point3d const& max_pnt(_aabb.getMaxPoint());
+    auto const& min_pnt(_aabb.getMinPoint());
+    auto const& max_pnt(_aabb.getMaxPoint());
     auto const dim = getDimensions(min_pnt, max_pnt);
 
     std::array<double, 3> delta{{max_pnt[0] - min_pnt[0],
@@ -119,12 +119,12 @@ MeshElementGrid::MeshElementGrid(MeshLib::Mesh const& mesh)
     sortElementsInGridCells(mesh);
 }
 
-MathLib::Point3d const& MeshElementGrid::getMinPoint() const
+Eigen::Vector3d const& MeshElementGrid::getMinPoint() const
 {
     return _aabb.getMinPoint();
 }
 
-MathLib::Point3d const& MeshElementGrid::getMaxPoint() const
+Eigen::Vector3d const& MeshElementGrid::getMaxPoint() const
 {
     return _aabb.getMaxPoint();
 }
diff --git a/MeshLib/MeshSearch/MeshElementGrid.h b/MeshLib/MeshSearch/MeshElementGrid.h
index bd546ffee627db920d944a78963c9a250d03b607..0303c10eb98d46c0d61780ea780a6d63fa216aa1 100644
--- a/MeshLib/MeshSearch/MeshElementGrid.h
+++ b/MeshLib/MeshSearch/MeshElementGrid.h
@@ -69,10 +69,10 @@ public:
 
     /// Returns the min point of the internal AABB. The method is a wrapper for
     /// GeoLib::AABB::getMinPoint().
-    MathLib::Point3d const& getMinPoint() const;
+    Eigen::Vector3d const& getMinPoint() const;
     /// Returns the max point of the internal AABB. The method is a wrapper for
     /// AABB::getMaxPoint().
-    MathLib::Point3d const& getMaxPoint() const;
+    Eigen::Vector3d const& getMaxPoint() const;
 
 private:
     void sortElementsInGridCells(MeshLib::Mesh const& mesh);
diff --git a/Tests/GeoLib/TestAABB.cpp b/Tests/GeoLib/TestAABB.cpp
index e0c522edffcd9e3b15bc82a747b20b6f3b005038..50b181cd38441690c9e76a0fff4b78acbbd44dbd 100644
--- a/Tests/GeoLib/TestAABB.cpp
+++ b/Tests/GeoLib/TestAABB.cpp
@@ -45,8 +45,8 @@ TEST(GeoLibAABB, RandomNumberOfPointersToRandomPoints)
     // construct from list points a axis aligned bounding box
     GeoLib::AABB aabb(pnts_list.begin(), pnts_list.end());
 
-    MathLib::Point3d const& min_pnt(aabb.getMinPoint());
-    MathLib::Point3d const& max_pnt(aabb.getMaxPoint());
+    auto const& min_pnt(aabb.getMinPoint());
+    auto const& max_pnt(aabb.getMaxPoint());
 
     ASSERT_LE(minus_half_box_size, min_pnt[0])
         << "coordinate 0 of min_pnt is smaller than " << minus_half_box_size;
@@ -92,8 +92,8 @@ TEST(GeoLibAABB, RandomNumberOfPointsRandomPointInAList)
     // construct from list points a axis aligned bounding box
     GeoLib::AABB aabb(pnts_list.begin(), pnts_list.end());
 
-    MathLib::Point3d const& min_pnt(aabb.getMinPoint());
-    MathLib::Point3d const& max_pnt(aabb.getMaxPoint());
+    auto const& min_pnt(aabb.getMinPoint());
+    auto const& max_pnt(aabb.getMaxPoint());
 
     ASSERT_LE(minus_half_box_size, min_pnt[0])
         << "coordinate 0 of min_pnt is smaller than " << minus_half_box_size;
@@ -134,8 +134,8 @@ TEST(GeoLibAABB, RandomNumberOfPointersToRandomPointsInAVector)
     // construct from list points a axis aligned bounding box
     GeoLib::AABB aabb(pnts.begin(), pnts.end());
 
-    MathLib::Point3d const& min_pnt(aabb.getMinPoint());
-    MathLib::Point3d const& max_pnt(aabb.getMaxPoint());
+    auto const& min_pnt(aabb.getMinPoint());
+    auto const& max_pnt(aabb.getMaxPoint());
 
     ASSERT_LE(minus_half_box_size, min_pnt[0])
         << "coordinate 0 of min_pnt is smaller than " << minus_half_box_size;
@@ -181,8 +181,8 @@ TEST(GeoLibAABB, RandomNumberOfPointsRandomPointInAVector)
     // construct from list points a axis aligned bounding box
     GeoLib::AABB aabb(pnts.begin(), pnts.end());
 
-    MathLib::Point3d const& min_pnt(aabb.getMinPoint());
-    MathLib::Point3d const& max_pnt(aabb.getMaxPoint());
+    auto const& min_pnt(aabb.getMinPoint());
+    auto const& max_pnt(aabb.getMaxPoint());
 
     ASSERT_LE(minus_half_box_size, min_pnt[0])
         << "coordinate 0 of min_pnt is smaller than " << minus_half_box_size;
@@ -229,8 +229,8 @@ TEST(GeoLibAABB, RandomNumberOfPointsRandomBox)
     // construct from list points a axis aligned bounding box
     GeoLib::AABB aabb(pnts.begin(), pnts.end());
 
-    MathLib::Point3d const& min_pnt(aabb.getMinPoint());
-    MathLib::Point3d const& max_pnt(aabb.getMaxPoint());
+    auto const& min_pnt(aabb.getMinPoint());
+    auto const& max_pnt(aabb.getMaxPoint());
 
     ASSERT_LE(minus_half_box_size_x, min_pnt[0])
         << "coordinate 0 of min_pnt is smaller than " << minus_half_box_size_x;
@@ -265,7 +265,7 @@ TEST(GeoLib, AABBAllPointsWithNegativeCoordinatesI)
     ids.push_back(1);
     GeoLib::AABB aabb(pnts, ids);
 
-    MathLib::Point3d const& max_pnt(aabb.getMaxPoint());
+    auto const& max_pnt(aabb.getMaxPoint());
 
     ASSERT_NEAR(-1.0, max_pnt[0], std::numeric_limits<double>::epsilon());
     ASSERT_NEAR(-1.0, max_pnt[1], std::numeric_limits<double>::epsilon());
@@ -287,7 +287,7 @@ TEST(GeoLib, AABBAllPointsWithNegativeCoordinatesII)
     // construct from points of the vector a axis aligned bounding box
     GeoLib::AABB aabb(pnts.begin(), pnts.end());
 
-    MathLib::Point3d const& max_pnt(aabb.getMaxPoint());
+    auto const& max_pnt(aabb.getMaxPoint());
 
     ASSERT_NEAR(-1.0, max_pnt[0], std::numeric_limits<double>::epsilon());
     ASSERT_NEAR(-1.0, max_pnt[1], std::numeric_limits<double>::epsilon());
diff --git a/Tests/GeoLib/TestOctTree.cpp b/Tests/GeoLib/TestOctTree.cpp
index 62cb593011801e914b9adf5232471c75e442f3f9..ec836fdfa14a65a3c3bece67413eef6722784965 100644
--- a/Tests/GeoLib/TestOctTree.cpp
+++ b/Tests/GeoLib/TestOctTree.cpp
@@ -17,6 +17,14 @@
 #include "GeoLib/OctTree.h"
 #include "GeoLib/Point.h"
 
+namespace
+{
+Eigen::Vector3d convertToEigen(MathLib::Point3d point3d)
+{
+    return Eigen::Vector3d{point3d[0], point3d[1], point3d[2]};
+}
+}  // namespace
+
 class GeoLibOctTree : public testing::Test
 {
 public:
@@ -89,12 +97,13 @@ TEST_F(GeoLibOctTree, TestWithEquidistantPoints3d)
     generateEquidistantPoints3d();
     double const eps(10 * std::numeric_limits<double>::epsilon());
     std::unique_ptr<GeoLib::OctTree<GeoLib::Point, 2>> oct_tree(
-        GeoLib::OctTree<GeoLib::Point, 2>::createOctTree(*ps_ptr.front(),
-                                                         *ps_ptr.back(), eps));
+        GeoLib::OctTree<GeoLib::Point, 2>::createOctTree(
+            convertToEigen(*ps_ptr.front()), convertToEigen(*ps_ptr.back()),
+            eps));
 
 #ifndef NDEBUG
-    MathLib::Point3d const& ll(oct_tree->getLowerLeftCornerPoint());
-    MathLib::Point3d const& ur(oct_tree->getUpperRightCornerPoint());
+    Eigen::Vector3d const& ll(oct_tree->getLowerLeftCornerPoint());
+    Eigen::Vector3d const& ur(oct_tree->getUpperRightCornerPoint());
 
     EXPECT_EQ((*ps_ptr.front())[0], ll[0]);
     EXPECT_EQ((*ps_ptr.front())[1], ll[1]);
@@ -277,8 +286,8 @@ TEST_F(GeoLibOctTree, TestWithAlternatingPoints3d)
     ps_ptr.push_back(new GeoLib::Point(5 * small_displacement, 1, 0, 6));
 
     GeoLib::AABB const aabb(ps_ptr.cbegin(), ps_ptr.cend());
-    const MathLib::Point3d& min(aabb.getMinPoint());
-    const MathLib::Point3d& max(aabb.getMaxPoint());
+    auto const& min(aabb.getMinPoint());
+    auto const& max(aabb.getMaxPoint());
     std::unique_ptr<GeoLib::OctTree<GeoLib::Point, 8>> oct_tree(
         GeoLib::OctTree<GeoLib::Point, 8>::createOctTree(min, max, eps));
 
@@ -332,8 +341,8 @@ TEST_F(GeoLibOctTree, TestSmallDistanceDifferentLeaves)
 
     // create OctTree
     GeoLib::AABB const aabb(ps_ptr.cbegin(), ps_ptr.cend());
-    const MathLib::Point3d& min(aabb.getMinPoint());
-    const MathLib::Point3d& max(aabb.getMaxPoint());
+    auto const& min(aabb.getMinPoint());
+    auto const& max(aabb.getMaxPoint());
     std::unique_ptr<GeoLib::OctTree<GeoLib::Point, 2>> oct_tree(
         GeoLib::OctTree<GeoLib::Point, 2>::createOctTree(min, max, eps));
 
@@ -364,9 +373,10 @@ TEST_F(GeoLibOctTree, TestOctTreeWithTwoEqualPoints)
     double const eps(0.0);
 
     GeoLib::AABB aabb(ps_ptr.begin(), ps_ptr.end());
+    auto const& min(aabb.getMinPoint());
+    auto const& max(aabb.getMaxPoint());
     std::unique_ptr<GeoLib::OctTree<GeoLib::Point, 2>> oct_tree(
-        GeoLib::OctTree<GeoLib::Point, 2>::createOctTree(
-            aabb.getMinPoint(), aabb.getMaxPoint(), eps));
+        GeoLib::OctTree<GeoLib::Point, 2>::createOctTree(min, max, eps));
 
     GeoLib::Point* ret_pnt(nullptr);
     ASSERT_TRUE(oct_tree->addPoint(ps_ptr[0], ret_pnt));
@@ -382,9 +392,10 @@ TEST_F(GeoLibOctTree, TestOctTreeWithTwoEqualPointsOne)
     double const eps(0.0);
 
     GeoLib::AABB aabb(ps_ptr.begin(), ps_ptr.end());
+    auto const& min(aabb.getMinPoint());
+    auto const& max(aabb.getMaxPoint());
     std::unique_ptr<GeoLib::OctTree<GeoLib::Point, 2>> oct_tree(
-        GeoLib::OctTree<GeoLib::Point, 2>::createOctTree(
-            aabb.getMinPoint(), aabb.getMaxPoint(), eps));
+        GeoLib::OctTree<GeoLib::Point, 2>::createOctTree(min, max, eps));
 
     GeoLib::Point* ret_pnt(nullptr);
     ASSERT_TRUE(oct_tree->addPoint(ps_ptr[0], ret_pnt));
@@ -400,9 +411,10 @@ TEST_F(GeoLibOctTree, TestOctTreeOnCubicDomain)
     double const eps(0.0);
 
     GeoLib::AABB aabb(ps_ptr.begin(), ps_ptr.end());
+    auto const& min(aabb.getMinPoint());
+    auto const& max(aabb.getMaxPoint());
     std::unique_ptr<GeoLib::OctTree<GeoLib::Point, 2>> oct_tree(
-        GeoLib::OctTree<GeoLib::Point, 2>::createOctTree(
-            aabb.getMinPoint(), aabb.getMaxPoint(), eps));
+        GeoLib::OctTree<GeoLib::Point, 2>::createOctTree(min, max, eps));
 
     GeoLib::Point* ret_pnt(nullptr);
     ASSERT_TRUE(oct_tree->addPoint(ps_ptr[0], ret_pnt));