diff --git a/data/test_assets/objects/axis.glb b/data/test_assets/objects/axis.glb
index d62ab2fa7a..b9c5bdb2f8 100644
Binary files a/data/test_assets/objects/axis.glb and b/data/test_assets/objects/axis.glb differ
diff --git a/examples/coordinate_system.py b/examples/coordinate_system.py
index 69e8730ee2..1d0742b901 100644
--- a/examples/coordinate_system.py
+++ b/examples/coordinate_system.py
@@ -55,7 +55,11 @@ def display_sensor_image(camera_sensor):
     rgb_image.show()
 
 
-if __name__ == "__main__":
+def main_coordinate_system_visual_examples():
+    """
+    Initializes a simulator and runs a set of incremental visual examples to demonstrate the default behavior of the system and cameras.
+    """
+
     # setup the settings configuration
     sim_settings: Dict[str, Any] = default_sim_settings
     # make camera coincident with agent scenenode
@@ -228,9 +232,7 @@ def display_sensor_image(camera_sensor):
         axis_template.render_asset_handle = "data/test_assets/objects/axis.glb"
         # don't let the CoM be computed from centroid of the shape
         axis_template.compute_COM_from_shape = False
-        axis_template_id = rotm.register_template(
-            axis_template, specified_handle="axis"
-        )
+        rotm.register_template(axis_template, specified_handle="axis")
         rom = sim.get_rigid_object_manager()
         axis_ro = rom.add_object_by_template_handle("axis")
         print(
@@ -261,7 +263,7 @@ def display_sensor_image(camera_sensor):
         display_sensor_image(camera_sensor)
 
         # add a spot robot
-        spot_urdf = aom.add_articulated_object_from_urdf(
+        aom.add_articulated_object_from_urdf(
             "data/robots/hab_spot_arm/urdf/hab_spot_arm.urdf",
             fixed_base=True,
         )
@@ -287,3 +289,79 @@ def display_sensor_image(camera_sensor):
             mn.Vector3(0, 0, -1), mn.Vector3(0, 1, -1), mn.Color4.magenta()
         )
         display_sensor_image(camera_sensor)
+    # exit initial sim instancing
+
+
+def main_fremont_robotics():
+    """
+    Load a Fremont scene configured for robotics coordinate system testing.
+    """
+    # setup the settings configuration
+    sim_settings: Dict[str, Any] = default_sim_settings
+    # make camera coincident with agent scenenode
+    sim_settings["sensor_height"] = 0
+    sim_settings[
+        "scene_dataset_config_file"
+    ] = "data/Fremont-Knuckles/fremont_knuckles.scene_dataset_config.json"
+    # NOTE: This loads the scan.stage_config.json
+    # NOTE: This file is modified to re-orient into robotics coordinate system.
+    sim_settings["scene"] = "scan"
+
+    cfg = make_cfg(sim_settings)
+
+    with habitat_sim.Simulator(cfg) as sim:
+        # set gravity to -Z direction
+        sim.set_gravity(mn.Vector3(0, 0, -9.8))
+        sim.navmesh_visualization = True
+
+        # agent local transform is identity
+        camera_agent = sim.get_agent(0)
+        # set the default agent transform to identity (it gets randomized when added upon initialization if there is a navmesh)
+        camera_agent.scene_node.transformation = mn.Matrix4()
+        print(f"Agent transform: {camera_agent.scene_node.transformation}")
+        # camera local transform is identity
+        camera_sensor = sim._Simulator__sensors[0]["color_sensor"]
+        print(f"Camera transform: {camera_sensor._sensor_object.node.transformation}")
+        # camera absolute transform is identity
+        print(
+            f"Camera absolute transform: {camera_sensor._sensor_object.node.absolute_transformation()}"
+        )
+
+        # add a spot robot
+        aom = sim.get_articulated_object_manager()
+        spot = aom.add_articulated_object_from_urdf(
+            "data/robots/hab_spot_arm/urdf/hab_spot_arm_no_rot.urdf",
+            fixed_base=True,
+        )
+        spot.translation = sim.pathfinder.snap_point(spot.translation) + mn.Vector3(
+            0, 0, 0.8
+        )
+
+        render_dist = 4
+        for diagonal in [
+            mn.Vector3(2, 2, 2),
+            mn.Vector3(-2, 2, 2),
+            mn.Vector3(-2, -2, 2),
+            mn.Vector3(2, -2, 2),
+        ]:
+            diagonal = diagonal.normalized() * render_dist
+            camera_sensor._sensor_object.node.transformation = mn.Matrix4.look_at(
+                eye=diagonal,  # from along the diagonal
+                target=mn.Vector3(),  # to the origin
+                up=mn.Vector3(0, 0, 1),  # up is robotics in +Z
+            )
+
+            draw_axes(sim)
+            display_sensor_image(camera_sensor)
+
+        stage_attributes = sim.get_stage_initialization_template()
+        print(stage_attributes)
+        # breakpoint()
+        pass
+
+
+if __name__ == "__main__":
+    # test the conventions and produce images
+    # main_coordinate_system_visual_examples()
+
+    main_fremont_robotics()
diff --git a/src/esp/bindings/ShortestPathBindings.cpp b/src/esp/bindings/ShortestPathBindings.cpp
index a1f4d27dad..e412d5f139 100644
--- a/src/esp/bindings/ShortestPathBindings.cpp
+++ b/src/esp/bindings/ShortestPathBindings.cpp
@@ -57,9 +57,8 @@ void initShortestPathBindings(py::module& m) {
       .def(py::init(&MultiGoalShortestPath::create<>))
       .def_readwrite("requested_start", &MultiGoalShortestPath::requestedStart,
                      R"(The starting point for the path.)")
-      .def_property("requested_ends", &MultiGoalShortestPath::getRequestedEnds,
-                    &MultiGoalShortestPath::setRequestedEnds,
-                    R"(The list of desired potential end points.)")
+      .def_readwrite("requested_ends", &MultiGoalShortestPath::requestedEnds,
+                     R"(The list of desired potential end points.)")
       .def_readwrite(
           "points", &MultiGoalShortestPath::points,
           R"(A list of points that specify the shortest path on the navigation mesh between requestedStart and the closest (by geodesic distance) point in requestedEnds. Will be empty if no path exists.)")
@@ -172,26 +171,17 @@ void initShortestPathBindings(py::module& m) {
           py::overload_cast<MultiGoalShortestPath&>(&PathFinder::findPath),
           "path"_a,
           R"(Finds the shortest path between a start point and the closest of a set of end points (in geodesic distance) on the navigation mesh using MultiGoalShortestPath module. Path variable is filled if successful. Returns boolean success.)")
-      .def("try_step", &PathFinder::tryStep<Magnum::Vector3>, "start"_a,
+      .def("try_step", &PathFinder::tryStep, "start"_a, "end"_a)
+      .def("try_step_no_sliding", &PathFinder::tryStepNoSliding, "start"_a,
            "end"_a)
-      .def("try_step", &PathFinder::tryStep<vec3f>, "start"_a, "end"_a)
-      .def("try_step_no_sliding",
-           &PathFinder::tryStepNoSliding<Magnum::Vector3>, "start"_a, "end"_a)
-      .def("try_step_no_sliding", &PathFinder::tryStepNoSliding<vec3f>,
-           "start"_a, "end"_a)
-      .def("snap_point", &PathFinder::snapPoint<Magnum::Vector3>, "point"_a,
+      .def("snap_point", &PathFinder::snapPoint, "point"_a,
            "island_index"_a = ID_UNDEFINED)
-      .def("snap_point", &PathFinder::snapPoint<vec3f>, "point"_a,
-           "island_index"_a = ID_UNDEFINED)
-      .def(
-          "get_island", &PathFinder::getIsland<Magnum::Vector3>, "point"_a,
-          R"(Query the island closest to a point. Snaps the point to the NavMesh first, so check the snap distance also if unsure.)")
       .def(
-          "get_island", &PathFinder::getIsland<vec3f>, "point"_a,
+          "get_island", &PathFinder::getIsland, "point"_a,
           R"(Query the island closest to a point. Snaps the point to the NavMesh first, so check the snap distance also if unsure.)")
       .def(
           "island_radius",
-          [](PathFinder& self, const vec3f& pt) {
+          [](PathFinder& self, const Magnum::Vector3& pt) {
             return self.islandRadius(pt);
           },
           "pt"_a,
diff --git a/src/esp/geo/CoordinateFrame.cpp b/src/esp/geo/CoordinateFrame.cpp
index bd04b12862..e8b69f96ea 100644
--- a/src/esp/geo/CoordinateFrame.cpp
+++ b/src/esp/geo/CoordinateFrame.cpp
@@ -6,6 +6,8 @@
 
 #include "esp/geo/Geo.h"
 
+#include <Magnum/Math/Matrix4.h>
+
 namespace esp {
 namespace geo {
 
@@ -21,9 +23,12 @@ CoordinateFrame::CoordinateFrame(const quatf& rotation,
     : CoordinateFrame(rotation * ESP_UP, rotation * ESP_FRONT, origin) {}
 
 quatf CoordinateFrame::rotationWorldToFrame() const {
-  const quatf R_frameUp_worldUp = quatf::FromTwoVectors(ESP_UP, up_);
-  return quatf::FromTwoVectors(R_frameUp_worldUp * ESP_FRONT, front_) *
-         R_frameUp_worldUp;
+  auto front = (Mn::Vector3(front_)).normalized();
+  auto right = Mn::Math::cross(Mn::Vector3(up_), front).normalized();
+  auto realUp = Mn::Math::cross(front, right);
+  auto myMat = Mn::Matrix4::from({front, right, realUp}, Mn::Vector3());
+  return Magnum::EigenIntegration::cast<quatf>(
+      Mn::Quaternion::fromMatrix(myMat.rotation()));
 }
 
 quatf CoordinateFrame::rotationFrameToWorld() const {
diff --git a/src/esp/geo/Geo.h b/src/esp/geo/Geo.h
index d3c2c58896..7be52299e4 100644
--- a/src/esp/geo/Geo.h
+++ b/src/esp/geo/Geo.h
@@ -46,11 +46,11 @@ enum class ColorSpace {
 };
 
 //! global/world up direction
-static const vec3f ESP_UP = vec3f::UnitY();
+static const vec3f ESP_UP = vec3f::UnitZ();
 //! global/world gravity (down) direction
 static const vec3f ESP_GRAVITY = -ESP_UP;
 //! global/world front direction
-static const vec3f ESP_FRONT = -vec3f::UnitZ();
+static const vec3f ESP_FRONT = vec3f::UnitX();
 //! global/world back direction
 static const vec3f ESP_BACK = -ESP_FRONT;
 
diff --git a/src/esp/nav/GreedyFollower.cpp b/src/esp/nav/GreedyFollower.cpp
index de5714635c..158c9a157a 100644
--- a/src/esp/nav/GreedyFollower.cpp
+++ b/src/esp/nav/GreedyFollower.cpp
@@ -39,8 +39,8 @@ GreedyGeodesicFollowerImpl::GreedyGeodesicFollowerImpl(
 
 float GreedyGeodesicFollowerImpl::geoDist(const Mn::Vector3& start,
                                           const Mn::Vector3& end) {
-  geoDistPath_.requestedStart = cast<vec3f>(start);
-  geoDistPath_.requestedEnd = cast<vec3f>(end);
+  geoDistPath_.requestedStart = start;
+  geoDistPath_.requestedEnd = end;
   pathfinder_->findPath(geoDistPath_);
   return geoDistPath_.geodesicDistance;
 }
@@ -56,7 +56,7 @@ GreedyGeodesicFollowerImpl::TryStepResult GreedyGeodesicFollowerImpl::tryStep(
 
   const float geoDistAfter = geoDist(newPose, end);
   const float distToObsAfter = pathfinder_->distanceToClosestObstacle(
-      cast<vec3f>(newPose), 1.1 * closeToObsThreshold_);
+      newPose, 1.1 * closeToObsThreshold_);
 
   return {geoDistAfter, distToObsAfter, didCollide};
 }
@@ -163,8 +163,8 @@ GreedyGeodesicFollowerImpl::CODES GreedyGeodesicFollowerImpl::nextActionAlong(
     const core::RigidState& start,
     const Mn::Vector3& end) {
   ShortestPath path;
-  path.requestedStart = cast<vec3f>(start.translation);
-  path.requestedEnd = cast<vec3f>(end);
+  path.requestedStart = start.translation;
+  path.requestedEnd = end;
   pathfinder_->findPath(path);
 
   CODES nextAction;
@@ -200,8 +200,8 @@ GreedyGeodesicFollowerImpl::findPath(const core::RigidState& start,
     core::RigidState state{findPathDummyNode_.rotation(),
                            findPathDummyNode_.MagnumObject::translation()};
     ShortestPath path;
-    path.requestedStart = cast<vec3f>(state.translation);
-    path.requestedEnd = cast<vec3f>(end);
+    path.requestedStart = state.translation;
+    path.requestedEnd = end;
     pathfinder_->findPath(path);
     const auto nextPrim = nextBestPrimAlong(state, path);
     if (nextPrim.empty()) {
diff --git a/src/esp/nav/PathFinder.cpp b/src/esp/nav/PathFinder.cpp
index 89bcb5865a..202be3ddc0 100644
--- a/src/esp/nav/PathFinder.cpp
+++ b/src/esp/nav/PathFinder.cpp
@@ -45,6 +45,16 @@ namespace Cr = Corrade;
 namespace esp {
 namespace nav {
 
+// declare vector conversion functions to convert between recast and habitat
+// coordinate systems.
+inline vec3f HabVecToRecast(const Magnum::Vector3& habVec) {
+  return vec3f(habVec[0], habVec[2], -habVec[1]);
+}
+
+inline Magnum::Vector3 RecastVecToHab(const vec3f& recastVec) {
+  return Magnum::Vector3(recastVec[0], -recastVec[2], recastVec[1]);
+}
+
 bool operator==(const NavMeshSettings& a, const NavMeshSettings& b) {
 #define CLOSE(name) (std::abs(a.name - b.name) < 1e-5)
 #define EQ(name) (a.name == b.name)
@@ -92,35 +102,6 @@ void NavMeshSettings::writeToJSON(const std::string& jsonFile) const {
   ESP_CHECK(ok, "writeSavedKeyframesToFile: unable to write to " << jsonFile);
 }
 
-struct MultiGoalShortestPath::Impl {
-  std::vector<vec3f> requestedEnds;
-
-  std::vector<dtPolyRef> endRefs;
-  //! Tracks whether an endpoint is valid or not as determined by setup to avoid
-  //! extra work or issues later.
-  std::vector<bool> endIsValid;
-  std::vector<vec3f> pathEnds;
-
-  std::vector<float> minTheoreticalDist;
-  vec3f prevRequestedStart = vec3f::Zero();
-};
-
-MultiGoalShortestPath::MultiGoalShortestPath()
-    : pimpl_{spimpl::make_unique_impl<Impl>()} {};
-
-void MultiGoalShortestPath::setRequestedEnds(
-    const std::vector<vec3f>& newEnds) {
-  pimpl_->endRefs.clear();
-  pimpl_->pathEnds.clear();
-  pimpl_->requestedEnds = newEnds;
-
-  pimpl_->minTheoreticalDist.assign(newEnds.size(), 0);
-}
-
-const std::vector<vec3f>& MultiGoalShortestPath::getRequestedEnds() const {
-  return pimpl_->requestedEnds;
-}
-
 namespace {
 template <typename T>
 std::tuple<dtStatus, dtPolyRef, vec3f> projectToPoly(
@@ -459,6 +440,121 @@ class IslandSystem {
 };
 }  // namespace impl
 
+/**
+ * @brief Struct for shortest path finding with Eigen. Used in conjunction with
+ * findPath.
+ */
+struct ShortestPathImpl {
+  ShortestPathImpl() { points = std::vector<vec3f>(); };
+  /**
+   * Constructor from a Magnum ShortestPath object.
+   */
+  ShortestPathImpl(ShortestPath sp) {
+    points = std::vector<vec3f>();
+    requestedStart = HabVecToRecast(sp.requestedStart);
+    requestedEnd = HabVecToRecast(sp.requestedEnd);
+    for (auto& point : sp.points) {
+      points.push_back(HabVecToRecast(point));
+    }
+    geodesicDistance = sp.geodesicDistance;
+  };
+
+  /**
+   * Convert this ShortestPathImpl back to a Habitat ShortestPath object.
+   */
+  ShortestPath getHabShortestPath() {
+    ShortestPath sp = ShortestPath();
+    sp.requestedStart = RecastVecToHab(requestedStart);
+    sp.requestedEnd = RecastVecToHab(requestedEnd);
+    for (auto& point : points) {
+      sp.points.push_back(RecastVecToHab(point));
+    }
+    sp.geodesicDistance = geodesicDistance;
+    return sp;
+  }
+
+  vec3f requestedStart;
+
+  vec3f requestedEnd;
+
+  std::vector<vec3f> points;
+
+  float geodesicDistance{};
+};
+
+/**
+ * @brief Struct for multi-goal shortest path finding. Used in conjunction with
+ * @ref PathFinder.findPath
+ */
+struct MultiGoalShortestPathImpl {
+  MultiGoalShortestPathImpl() {
+    points = std::vector<vec3f>();
+    requestedEnds = std::vector<vec3f>();
+    pathEnds = std::vector<vec3f>();
+    endRefs = std::vector<dtPolyRef>();
+  };
+
+  /**
+   * Constructor from a Magnum ShortestPath object.
+   */
+  MultiGoalShortestPathImpl(MultiGoalShortestPath sp) {
+    points = std::vector<vec3f>();
+    requestedStart = HabVecToRecast(sp.requestedStart);
+    for (auto& end : sp.requestedEnds) {
+      requestedEnds.push_back(HabVecToRecast(end));
+    }
+    minTheoreticalDist.assign(requestedEnds.size(), 0);
+    for (auto& point : sp.points) {
+      points.push_back(HabVecToRecast(point));
+    }
+    geodesicDistance = sp.geodesicDistance;
+  };
+
+  /**
+   * Convert this ShortestPathImpl back to a Habitat ShortestPath object.
+   */
+  MultiGoalShortestPath getHabMultiGoalShortestPath() {
+    MultiGoalShortestPath sp = MultiGoalShortestPath();
+    sp.requestedStart = RecastVecToHab(requestedStart);
+    for (auto& end : requestedEnds) {
+      sp.requestedEnds.push_back(RecastVecToHab(end));
+    }
+    for (auto& point : points) {
+      sp.points.push_back(RecastVecToHab(point));
+    }
+    sp.geodesicDistance = geodesicDistance;
+    sp.closestEndPointIndex = closestEndPointIndex;
+    return sp;
+  }
+
+  vec3f requestedStart;
+
+  std::vector<vec3f> requestedEnds;
+
+  std::vector<dtPolyRef> endRefs;
+  //! Tracks whether an endpoint is valid or not as determined by setup to avoid
+  //! extra work or issues later.
+  std::vector<bool> endIsValid;
+  std::vector<vec3f> pathEnds;
+
+  std::vector<float> minTheoreticalDist;
+  vec3f prevRequestedStart = vec3f::Zero();
+
+  /**
+   * @brief Set the list of desired potential end points
+   */
+  void setRequestedEnds(const std::vector<vec3f>& newEnds) {
+    endRefs.clear();
+    pathEnds.clear();
+    requestedEnds = newEnds;
+    minTheoreticalDist.assign(newEnds.size(), 0);
+  }
+
+  std::vector<vec3f> points;
+  float geodesicDistance{};
+  int closestEndPointIndex{};
+};
+
 struct PathFinder::Impl {
   Impl();
   ~Impl() = default;
@@ -483,8 +579,8 @@ struct PathFinder::Impl {
                                         int maxTries,
                                         int islandIndex /*= ID_UNDEFINED*/);
 
-  bool findPath(ShortestPath& path);
-  bool findPath(MultiGoalShortestPath& path);
+  bool findPath(ShortestPathImpl& path);
+  bool findPath(MultiGoalShortestPathImpl& path);
 
   template <typename T>
   T tryStep(const T& start, const T& end, bool allowSliding);
@@ -564,7 +660,7 @@ struct PathFinder::Impl {
                    dtPolyRef endRef,
                    const vec3f& pathEnd);
 
-  bool findPathSetup(MultiGoalShortestPath& path,
+  bool findPathSetup(MultiGoalShortestPathImpl& path,
                      dtPolyRef& startRef,
                      vec3f& pathStart);
 };
@@ -731,7 +827,7 @@ bool PathFinder::Impl::build(const NavMeshSettings& bs,
   //   vols[i].hmax, (unsigned char)vols[i].area, *ws.chf);
 
   // Partition the heightfield so that we can use simple algorithm later to
-  // triangulate the walkable areas. There are 3 martitioning methods, each with
+  // triangulate the walkable areas. There are 3 partitioning methods, each with
   // some pros and cons: 1) Watershed partitioning
   //   - the classic Recast partitioning
   //   - creates the nicest tessellation
@@ -1407,8 +1503,8 @@ float pathLength(const std::vector<vec3f>& points) {
 }
 }  // namespace
 
-bool PathFinder::Impl::findPath(ShortestPath& path) {
-  MultiGoalShortestPath tmp;
+bool PathFinder::Impl::findPath(ShortestPathImpl& path) {
+  MultiGoalShortestPathImpl tmp;
   tmp.requestedStart = path.requestedStart;
   tmp.setRequestedEnds({path.requestedEnd});
 
@@ -1463,7 +1559,7 @@ PathFinder::Impl::findPathInternal(const vec3f& start,
   return std::make_tuple(length, std::move(points));
 }
 
-bool PathFinder::Impl::findPathSetup(MultiGoalShortestPath& path,
+bool PathFinder::Impl::findPathSetup(MultiGoalShortestPathImpl& path,
                                      dtPolyRef& startRef,
                                      vec3f& pathStart) {
   path.geodesicDistance = std::numeric_limits<float>::infinity();
@@ -1479,26 +1575,26 @@ bool PathFinder::Impl::findPathSetup(MultiGoalShortestPath& path,
     return false;
   }
 
-  if (!path.pimpl_->endRefs.empty())
+  if (!path.endRefs.empty())
     return true;
 
   int numValidPoints = 0;
-  for (const auto& rqEnd : path.getRequestedEnds()) {
+  for (const auto& rqEnd : path.requestedEnds) {
     dtPolyRef endRef = 0;
     vec3f pathEnd;
     std::tie(status, endRef, pathEnd) =
         projectToPoly(rqEnd, navQuery_.get(), filter_.get());
 
     if (status != DT_SUCCESS || endRef == 0) {
-      path.pimpl_->endIsValid.emplace_back(false);
+      path.endIsValid.emplace_back(false);
       ESP_DEBUG() << "Can't project end-point to navmesh, skipping: " << rqEnd;
     } else {
-      path.pimpl_->endIsValid.emplace_back(true);
+      path.endIsValid.emplace_back(true);
       numValidPoints++;
     }
 
-    path.pimpl_->endRefs.emplace_back(endRef);
-    path.pimpl_->pathEnds.emplace_back(pathEnd);
+    path.endRefs.emplace_back(endRef);
+    path.pathEnds.emplace_back(pathEnd);
   }
   if (numValidPoints == 0) {
     ESP_DEBUG() << "Early abort, can't project any points to navmesh.";
@@ -1508,56 +1604,54 @@ bool PathFinder::Impl::findPathSetup(MultiGoalShortestPath& path,
   return true;
 }
 
-bool PathFinder::Impl::findPath(MultiGoalShortestPath& path) {
+bool PathFinder::Impl::findPath(MultiGoalShortestPathImpl& path) {
   dtPolyRef startRef = 0;
   vec3f pathStart;
   if (!findPathSetup(path, startRef, pathStart))
     return false;
 
-  if (path.pimpl_->requestedEnds.size() > 1) {
+  if (path.requestedEnds.size() > 1) {
     // Bound the minimum distance any point could be from the start by either
     // how close it use to be minus how much we moved from the last search point
     // or just the L2 distance.
 
-    ShortestPath prevPath;
+    ShortestPathImpl prevPath;
     prevPath.requestedStart = path.requestedStart;
-    prevPath.requestedEnd = path.pimpl_->prevRequestedStart;
+    prevPath.requestedEnd = path.prevRequestedStart;
     findPath(prevPath);
     const float movedAmount = prevPath.geodesicDistance;
 
-    for (int i = 0; i < path.pimpl_->requestedEnds.size(); ++i) {
-      path.pimpl_->minTheoreticalDist[i] = std::max(
-          path.pimpl_->minTheoreticalDist[i] - movedAmount,
-          (path.pimpl_->requestedEnds[i] - path.requestedStart).norm());
+    for (int i = 0; i < path.requestedEnds.size(); ++i) {
+      path.minTheoreticalDist[i] =
+          std::max(path.minTheoreticalDist[i] - movedAmount,
+                   (path.requestedEnds[i] - path.requestedStart).norm());
     }
 
-    path.pimpl_->prevRequestedStart = path.requestedStart;
+    path.prevRequestedStart = path.requestedStart;
   }
 
   // Explore possible goal points by their minimum theoretical distance.
-  std::vector<size_t> ordering(path.pimpl_->requestedEnds.size());
+  std::vector<size_t> ordering(path.requestedEnds.size());
   std::iota(ordering.begin(), ordering.end(), 0);
   std::sort(ordering.begin(), ordering.end(),
             [&path](const size_t a, const size_t b) -> bool {
-              return path.pimpl_->minTheoreticalDist[a] <
-                     path.pimpl_->minTheoreticalDist[b];
+              return path.minTheoreticalDist[a] < path.minTheoreticalDist[b];
             });
 
   for (size_t i : ordering) {
-    if (!path.pimpl_->endIsValid[i])
+    if (!path.endIsValid[i])
       continue;
 
-    if (path.pimpl_->minTheoreticalDist[i] > path.geodesicDistance)
+    if (path.minTheoreticalDist[i] > path.geodesicDistance)
       continue;
 
     const Cr::Containers::Optional<std::tuple<float, std::vector<vec3f>>>
-        findResult =
-            findPathInternal(path.requestedStart, startRef, pathStart,
-                             path.pimpl_->requestedEnds[i],
-                             path.pimpl_->endRefs[i], path.pimpl_->pathEnds[i]);
+        findResult = findPathInternal(path.requestedStart, startRef, pathStart,
+                                      path.requestedEnds[i], path.endRefs[i],
+                                      path.pathEnds[i]);
 
     if (findResult && std::get<0>(*findResult) < path.geodesicDistance) {
-      path.pimpl_->minTheoreticalDist[i] = std::get<0>(*findResult);
+      path.minTheoreticalDist[i] = std::get<0>(*findResult);
       path.geodesicDistance = std::get<0>(*findResult);
       path.points = std::get<1>(*findResult);
       path.closestEndPointIndex = i;
@@ -1722,7 +1816,7 @@ HitRecord PathFinder::Impl::closestObstacleSurfacePoint(
   std::tie(status, ptRef, polyPt) =
       projectToPoly(pt, navQuery_.get(), filter_.get());
   if (status != DT_SUCCESS || ptRef == 0) {
-    return {vec3f(0, 0, 0), vec3f(0, 0, 0),
+    return {Magnum::Vector3(0, 0, 0), Magnum::Vector3(0, 0, 0),
             std::numeric_limits<float>::infinity()};
   }
   vec3f hitPos, hitNormal;
@@ -1730,7 +1824,7 @@ HitRecord PathFinder::Impl::closestObstacleSurfacePoint(
   navQuery_->findDistanceToWall(ptRef, polyPt.data(), maxSearchRadius,
                                 filter_.get(), &hitDist, hitPos.data(),
                                 hitNormal.data());
-  return {std::move(hitPos), std::move(hitNormal), hitDist};
+  return {RecastVecToHab(hitPos), RecastVecToHab(hitNormal), hitDist};
 }
 
 bool PathFinder::Impl::isNavigable(const vec3f& pt,
@@ -1859,7 +1953,10 @@ assets::MeshData::ptr PathFinder::Impl::getNavMeshData(
 
         for (auto& tri : triangles) {
           for (int k = 0; k < 3; ++k) {
-            vbo.push_back(tri.v[k]);
+            // TODO: converting to Vector3 and back to vec3f to consistently
+            // apply the transform. Should be refactored when Eigen is removed.
+            vbo.push_back(
+                Mn::EigenIntegration::cast<vec3f>(RecastVecToHab(tri.v[k])));
             ibo.push_back(vbo.size() - 1);
           }
         }
@@ -1875,76 +1972,70 @@ assets::MeshData::ptr PathFinder::Impl::getNavMeshData(
 
 PathFinder::PathFinder() : pimpl_{spimpl::make_unique_impl<Impl>()} {};
 
-bool PathFinder::build(const NavMeshSettings& bs,
-                       const float* verts,
-                       const int nverts,
-                       const int* tris,
-                       const int ntris,
-                       const float* bmin,
-                       const float* bmax) {
-  return pimpl_->build(bs, verts, nverts, tris, ntris, bmin, bmax);
-}
 bool PathFinder::build(const NavMeshSettings& bs,
                        const esp::assets::MeshData& mesh) {
-  return pimpl_->build(bs, mesh);
+  // NOTE: we're copying the full mesh here to transform the verts, but at least
+  // we aren't modifying the passed in values directly
+  auto meshCopy = esp::assets::MeshData(mesh);
+  for (auto& vert : meshCopy.vbo) {
+    // TODO: we're converting to magnum and back to vec3f until Eigen is
+    // removed. Refactor this.
+    vert = HabVecToRecast(Mn::Vector3(vert));
+  }
+  return pimpl_->build(bs, meshCopy);
 }
 
-vec3f PathFinder::getRandomNavigablePoint(const int maxTries /*= 10*/,
-                                          int islandIndex /*= ID_UNDEFINED*/) {
-  return pimpl_->getRandomNavigablePoint(maxTries, islandIndex);
+Magnum::Vector3 PathFinder::getRandomNavigablePoint(
+    const int maxTries /*= 10*/,
+    int islandIndex /*= ID_UNDEFINED*/) {
+  return RecastVecToHab(pimpl_->getRandomNavigablePoint(maxTries, islandIndex));
 }
 
-vec3f PathFinder::getRandomNavigablePointAroundSphere(
-    const vec3f& circleCenter,
+Magnum::Vector3 PathFinder::getRandomNavigablePointAroundSphere(
+    const Magnum::Vector3& circleCenter,
     const float radius,
     const int maxTries,
     int islandIndex /*= ID_UNDEFINED*/) {
-  return pimpl_->getRandomNavigablePointInCircle(circleCenter, radius, maxTries,
-                                                 islandIndex);
+  return RecastVecToHab(pimpl_->getRandomNavigablePointInCircle(
+      HabVecToRecast(circleCenter), radius, maxTries, islandIndex));
 }
 
 bool PathFinder::findPath(ShortestPath& path) {
-  return pimpl_->findPath(path);
+  // NOTE: findPath modifies the input ShortestPath so we need to copy those
+  // changes back over before return
+  auto shortestPathImpl = ShortestPathImpl(path);
+  auto result = pimpl_->findPath(shortestPathImpl);
+  path = shortestPathImpl.getHabShortestPath();
+  return result;
 }
 
 bool PathFinder::findPath(MultiGoalShortestPath& path) {
-  return pimpl_->findPath(path);
+  // NOTE: findPath modifies the input ShortestPath so we need to copy those
+  // changes back over before return
+  auto shortestPathImpl = MultiGoalShortestPathImpl(path);
+  auto result = pimpl_->findPath(shortestPathImpl);
+  path = shortestPathImpl.getHabMultiGoalShortestPath();
+  return result;
 }
 
-template vec3f PathFinder::tryStep<vec3f>(const vec3f&, const vec3f&);
-template Mn::Vector3 PathFinder::tryStep<Mn::Vector3>(const Mn::Vector3&,
-                                                      const Mn::Vector3&);
-
-template <typename T>
-T PathFinder::tryStep(const T& start, const T& end) {
-  return pimpl_->tryStep(start, end, /*allowSliding=*/true);
+Magnum::Vector3 PathFinder::tryStep(const Magnum::Vector3& start,
+                                    const Magnum::Vector3& end) {
+  return RecastVecToHab(pimpl_->tryStep(
+      HabVecToRecast(start), HabVecToRecast(end), /*allowSliding=*/true));
 }
 
-template vec3f PathFinder::tryStepNoSliding<vec3f>(const vec3f&, const vec3f&);
-template Mn::Vector3 PathFinder::tryStepNoSliding<Mn::Vector3>(
-    const Mn::Vector3&,
-    const Mn::Vector3&);
-
-template <typename T>
-T PathFinder::tryStepNoSliding(const T& start, const T& end) {
-  return pimpl_->tryStep(start, end, /*allowSliding=*/false);
+Magnum::Vector3 PathFinder::tryStepNoSliding(const Magnum::Vector3& start,
+                                             const Magnum::Vector3& end) {
+  return RecastVecToHab(pimpl_->tryStep(
+      HabVecToRecast(start), HabVecToRecast(end), /*allowSliding=*/false));
 }
 
-template vec3f PathFinder::snapPoint<vec3f>(const vec3f& pt, int islandIndex);
-template Mn::Vector3 PathFinder::snapPoint<Mn::Vector3>(const Mn::Vector3& pt,
-                                                        int islandIndex);
-
-template int PathFinder::getIsland<vec3f>(const vec3f& pt);
-template int PathFinder::getIsland<Mn::Vector3>(const Mn::Vector3& pt);
-
-template <typename T>
-T PathFinder::snapPoint(const T& pt, int islandIndex) {
-  return pimpl_->snapPoint(pt, islandIndex);
+Mn::Vector3 PathFinder::snapPoint(const Mn::Vector3& pt, int islandIndex) {
+  return RecastVecToHab(pimpl_->snapPoint(HabVecToRecast(pt), islandIndex));
 }
 
-template <typename T>
-int PathFinder::getIsland(const T& pt) {
-  return pimpl_->getIsland(pt);
+int PathFinder::getIsland(const Mn::Vector3& pt) {
+  return pimpl_->getIsland(HabVecToRecast(pt));
 }
 
 bool PathFinder::loadNavMesh(const std::string& path) {
@@ -1963,8 +2054,8 @@ void PathFinder::seed(uint32_t newSeed) {
   return pimpl_->seed(newSeed);
 }
 
-float PathFinder::islandRadius(const vec3f& pt) const {
-  return pimpl_->islandRadius(pt);
+float PathFinder::islandRadius(const Magnum::Vector3& pt) const {
+  return pimpl_->islandRadius(HabVecToRecast(pt));
 }
 
 float PathFinder::islandRadius(int islandIndex) const {
@@ -1975,27 +2066,31 @@ int PathFinder::numIslands() const {
   return pimpl_->numIslands();
 }
 
-float PathFinder::distanceToClosestObstacle(const vec3f& pt,
+float PathFinder::distanceToClosestObstacle(const Magnum::Vector3& pt,
                                             const float maxSearchRadius) const {
-  return pimpl_->distanceToClosestObstacle(pt, maxSearchRadius);
+  return pimpl_->distanceToClosestObstacle(HabVecToRecast(pt), maxSearchRadius);
 }
 
 HitRecord PathFinder::closestObstacleSurfacePoint(
-    const vec3f& pt,
+    const Magnum::Vector3& pt,
     const float maxSearchRadius) const {
-  return pimpl_->closestObstacleSurfacePoint(pt, maxSearchRadius);
+  return pimpl_->closestObstacleSurfacePoint(HabVecToRecast(pt),
+                                             maxSearchRadius);
 }
 
-bool PathFinder::isNavigable(const vec3f& pt, const float maxYDelta) const {
-  return pimpl_->isNavigable(pt, maxYDelta);
+bool PathFinder::isNavigable(const Magnum::Vector3& pt,
+                             const float maxYDelta) const {
+  return pimpl_->isNavigable(HabVecToRecast(pt), maxYDelta);
 }
 
 float PathFinder::getNavigableArea(int islandIndex /*= ID_UNDEFINED*/) const {
   return pimpl_->getNavigableArea(islandIndex);
 }
 
-std::pair<vec3f, vec3f> PathFinder::bounds() const {
-  return pimpl_->bounds();
+std::pair<Magnum::Vector3, Magnum::Vector3> PathFinder::bounds() const {
+  return std::pair<Magnum::Vector3, Magnum::Vector3>(
+      RecastVecToHab(pimpl_->bounds().first),
+      RecastVecToHab(pimpl_->bounds().second));
 }
 
 Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> PathFinder::getTopDownView(
@@ -2014,6 +2109,8 @@ PathFinder::getTopDownIslandView(const float metersPerPixel,
 
 assets::MeshData::ptr PathFinder::getNavMeshData(
     int islandIndex /*= ID_UNDEFINED*/) {
+  // NOTE: The cached MeshDats in impl is already pre-converted to Habitat's
+  // coordinate system.
   return pimpl_->getNavMeshData(islandIndex);
 }
 
diff --git a/src/esp/nav/PathFinder.h b/src/esp/nav/PathFinder.h
index bf782cb232..91f6a34788 100644
--- a/src/esp/nav/PathFinder.h
+++ b/src/esp/nav/PathFinder.h
@@ -21,6 +21,12 @@ struct MeshData;
 //! NavMesh namespace
 namespace nav {
 
+// declare vector conversion functions to convert between recast and habitat
+// coordinate systems.
+inline vec3f HabVecToRecast(const Magnum::Vector3& habVec);
+
+inline Magnum::Vector3 RecastVecToHab(const vec3f& recastVec);
+
 class PathFinder;
 
 /**
@@ -28,9 +34,9 @@ class PathFinder;
  */
 struct HitRecord {
   //! World position of the closest obstacle.
-  vec3f hitPos;
+  Magnum::Vector3 hitPos;
   //! Normal of the navmesh at the obstacle in xz plane.
-  vec3f hitNormal;
+  Magnum::Vector3 hitNormal;
   //! Distance from query point to closest obstacle. Inf if no valid point was
   //! found.
   float hitDist{};
@@ -44,12 +50,12 @@ struct ShortestPath {
   /**
    * @brief The starting point for the path
    */
-  vec3f requestedStart;
+  Magnum::Vector3 requestedStart;
 
   /**
    * @brief The ending point for the path
    */
-  vec3f requestedEnd;
+  Magnum::Vector3 requestedEnd;
 
   /**
    * @brief A list of points that specify the shortest path on the navigation
@@ -57,7 +63,7 @@ struct ShortestPath {
    *
    * @note Will be empty if no path exists
    */
-  std::vector<vec3f> points;
+  std::vector<Magnum::Vector3> points;
 
   /**
    * @brief The geodesic distance between @ref requestedStart and @ref
@@ -75,19 +81,12 @@ struct ShortestPath {
  * @ref PathFinder.findPath
  */
 struct MultiGoalShortestPath {
-  MultiGoalShortestPath();
-
   /**
    * @brief The starting point for the path
    */
-  vec3f requestedStart;
+  Magnum::Vector3 requestedStart;
 
-  /**
-   * @brief Set the list of desired potential end points
-   */
-  void setRequestedEnds(const std::vector<vec3f>& newEnds);
-
-  const std::vector<vec3f>& getRequestedEnds() const;
+  std::vector<Magnum::Vector3> requestedEnds;
 
   /**
    * @brief A list of points that specify the shortest path on the navigation
@@ -96,7 +95,7 @@ struct MultiGoalShortestPath {
    *
    * Will be empty if no path exists
    */
-  std::vector<vec3f> points;
+  std::vector<Magnum::Vector3> points;
 
   /**
    * @brief The geodesic distance
@@ -113,9 +112,7 @@ struct MultiGoalShortestPath {
    */
   int closestEndPointIndex{};
 
-  friend class PathFinder;
-
-  ESP_SMART_POINTERS_WITH_UNIQUE_PIMPL(MultiGoalShortestPath)
+  ESP_SMART_POINTERS(MultiGoalShortestPath)
 };
 
 /**
@@ -328,6 +325,11 @@ bool operator!=(const NavMeshSettings& a, const NavMeshSettings& b);
  * surfaces of solid voxels where the cylinder would sit without intersection or
  * overhanging and respecting configured constraints such as maximum climbable
  * slope and step-height.
+ *
+ * Note that Recast Navigation uses a fixed internal right-hand coordinate
+ * system with +Y==UP. This exposed wrapper API assumes all points are in
+ * Habitat's coordinate system and therefore converts all points as they pass
+ * through to/from the impl layer.
  */
 class PathFinder {
  public:
@@ -337,30 +339,6 @@ class PathFinder {
   PathFinder();
   ~PathFinder() = default;
 
-  /**
-   * @brief Construct a NavMesh from @ref NavMeshSettings and mesh data
-   * pointers.
-   *
-   * @param bs Parameter settings for NavMesh construction.
-   * @param verts Vertex array of the mesh.
-   * @param nverts Number of verts in the array.
-   * @param tris Index array of the mesh triangles.
-   * @param ntris Number of triangle indices in the array.
-   * @param bmin Navigable region bounding box min corner. Could be mesh bb or
-   * user defined override for subset of the mesh.
-   * @param bmax Navigable region bounding box max corner. Could be mesh bb or
-   * user defined override for subset of the mesh.
-   *
-   * @return Whether or not construction was successful.
-   */
-  bool build(const NavMeshSettings& bs,
-             const float* verts,
-             int nverts,
-             const int* tris,
-             int ntris,
-             const float* bmin,
-             const float* bmax);
-
   /**
    * @brief Construct a NavMesh from @ref NavMeshSettings and a @ref MeshData
    * object.
@@ -386,8 +364,8 @@ class PathFinder {
    * the returned point will be `{NAN, NAN, NAN}`. Use @ref
    * isNavigable to check if the point is navigable.
    */
-  vec3f getRandomNavigablePoint(int maxTries = 10,
-                                int islandIndex = ID_UNDEFINED);
+  Magnum::Vector3 getRandomNavigablePoint(int maxTries = 10,
+                                          int islandIndex = ID_UNDEFINED);
 
   /**
    * @brief Returns a random navigable point within a specified radius about a
@@ -406,10 +384,11 @@ class PathFinder {
    * the returned point will be `{NAN, NAN, NAN}`. Use @ref
    * isNavigable to check if the point is navigable.
    */
-  vec3f getRandomNavigablePointAroundSphere(const vec3f& circleCenter,
-                                            float radius,
-                                            int maxTries = 10,
-                                            int islandIndex = ID_UNDEFINED);
+  Magnum::Vector3 getRandomNavigablePointAroundSphere(
+      const Magnum::Vector3& circleCenter,
+      float radius,
+      int maxTries = 10,
+      int islandIndex = ID_UNDEFINED);
 
   /**
    * @brief Finds the shortest path between two points on the navigation mesh
@@ -454,14 +433,14 @@ class PathFinder {
    *
    * @return The found end location.
    */
-  template <typename T>
-  T tryStep(const T& start, const T& end);
+  Magnum::Vector3 tryStep(const Magnum::Vector3& start,
+                          const Magnum::Vector3& end);
 
   /**
    * @brief Same as @ref tryStep but does not allow for sliding along walls
    */
-  template <typename T>
-  T tryStepNoSliding(const T& start, const T& end);
+  Magnum::Vector3 tryStepNoSliding(const Magnum::Vector3& start,
+                                   const Magnum::Vector3& end);
 
   /**
    * @brief Snaps a point to the navigation mesh.
@@ -473,8 +452,8 @@ class PathFinder {
    * @return The closest navigation point to @ref pt.  Will be `{NAN, NAN, NAN}`
    * if no navigable point was within a reasonable distance
    */
-  template <typename T>
-  T snapPoint(const T& pt, int islandIndex = ID_UNDEFINED);
+  Magnum::Vector3 snapPoint(const Magnum::Vector3& pt,
+                            int islandIndex = ID_UNDEFINED);
 
   /**
    * @brief Identifies the island closest to a point.
@@ -491,8 +470,7 @@ class PathFinder {
    *
    * @return The island for the point or ID_UNDEFINED (-1) if failed.
    */
-  template <typename T>
-  int getIsland(const T& pt);
+  int getIsland(const Magnum::Vector3& pt);
 
   /**
    * @brief Loads a navigation meshed saved by @ref saveNavMesh
@@ -543,7 +521,7 @@ class PathFinder {
    *
    * @return Heuristic size of the connected component.
    */
-  float islandRadius(const vec3f& pt) const;
+  float islandRadius(const Magnum::Vector3& pt) const;
 
   /**
    * @brief returns the size of the specified connected component.
@@ -570,14 +548,14 @@ class PathFinder {
    * @return The distance to the closest non-navigable location or @ref
    * maxSearchRadius if all locations within @ref maxSearchRadius are navigable
    */
-  float distanceToClosestObstacle(const vec3f& pt,
+  float distanceToClosestObstacle(const Magnum::Vector3& pt,
                                   float maxSearchRadius = 2.0) const;
 
   /**
    * @brief Same as @ref distanceToClosestObstacle but returns additional
    * information.
    */
-  HitRecord closestObstacleSurfacePoint(const vec3f& pt,
+  HitRecord closestObstacleSurfacePoint(const Magnum::Vector3& pt,
                                         float maxSearchRadius = 2.0) const;
 
   /**
@@ -593,7 +571,7 @@ class PathFinder {
    *
    * @return Whether or not @ref pt is navigable
    */
-  bool isNavigable(const vec3f& pt, float maxYDelta = 0.5) const;
+  bool isNavigable(const Magnum::Vector3& pt, float maxYDelta = 0.5) const;
 
   /**
    * Compute and return the total area of all NavMesh polygons.
@@ -608,7 +586,7 @@ class PathFinder {
   /**
    * @return The axis aligned bounding box containing the navigation mesh.
    */
-  std::pair<vec3f, vec3f> bounds() const;
+  std::pair<Magnum::Vector3, Magnum::Vector3> bounds() const;
 
   /**
    * @brief Get a 2D grid marking navigable and non-navigable cells at a
@@ -653,6 +631,9 @@ class PathFinder {
    * Theobject is generated and stored if this is the first query for constant
    * time access in subsequent calls.
    *
+   * NOTE: Vertices are already transformed to Habitat's coordinate system from
+   * RecastNavigation's local Y-up coordinate system.
+   *
    * Does nothing if the PathFinder is not loaded.
    *
    * @param[in] islandIndex Optionally limit results to a specific island.
diff --git a/src/tests/NavTest.cpp b/src/tests/NavTest.cpp
index 3c7280a0b0..0382b688da 100644
--- a/src/tests/NavTest.cpp
+++ b/src/tests/NavTest.cpp
@@ -18,9 +18,11 @@ namespace Cr = Corrade;
 
 namespace {
 
-void printPathPoint(int run, int step, const esp::vec3f& p, float distance) {
-  ESP_VERY_VERBOSE() << run << "," << step << "," << Magnum::Vector3{p} << ","
-                     << distance;
+void printPathPoint(int run,
+                    int step,
+                    const Magnum::Vector3& p,
+                    float distance) {
+  ESP_VERY_VERBOSE() << run << "," << step << "," << p << "," << distance;
 }
 
 struct NavTest : Cr::TestSuite::Tester {
@@ -59,11 +61,10 @@ void NavTest::PathFinderLoadTest() {
         CORRADE_COMPARE(pf.islandRadius(path.points[j]), islandSize);
       }
       CORRADE_COMPARE(pf.islandRadius(path.requestedEnd), islandSize);
-      const esp::vec3f& pathStart = path.points.front();
-      const esp::vec3f& pathEnd = path.points.back();
-      const esp::vec3f end = pf.tryStep(pathStart, pathEnd);
-      ESP_DEBUG() << "tryStep initial end=" << pathEnd.transpose()
-                  << ", final end=" << end.transpose();
+      const Magnum::Vector3& pathStart = path.points.front();
+      const Magnum::Vector3& pathEnd = path.points.back();
+      const Magnum::Vector3 end = pf.tryStep(pathStart, pathEnd);
+      ESP_DEBUG() << "tryStep initial end=" << pathEnd << ", final end=" << end;
       CORRADE_COMPARE_AS(path.geodesicDistance,
                          std::numeric_limits<float>::infinity(),
                          Cr::TestSuite::Compare::Less);
@@ -79,8 +80,10 @@ void printRandomizedPathSet(esp::nav::PathFinder& pf) {
   ESP_VERY_VERBOSE() << "run,step,x,y,z,geodesicDistance";
   for (int i = 0; i < 100; ++i) {
     const float r = 0.1;
-    esp::vec3f rv(random.uniform_float(-r, r), 0, random.uniform_float(-r, r));
-    esp::vec3f rv2(random.uniform_float(-r, r), 0, random.uniform_float(-r, r));
+    Magnum::Vector3 rv(random.uniform_float(-r, r), 0,
+                       random.uniform_float(-r, r));
+    Magnum::Vector3 rv2(random.uniform_float(-r, r), 0,
+                        random.uniform_float(-r, r));
     path.requestedStart += rv;
     path.requestedEnd += rv2;
     const bool foundPath = pf.findPath(path);
@@ -94,8 +97,7 @@ void printRandomizedPathSet(esp::nav::PathFinder& pf) {
                      path.geodesicDistance);
     } else {
       ESP_WARNING() << "Failed to find shortest path between start="
-                    << path.requestedStart.transpose()
-                    << "and end=" << path.requestedEnd.transpose();
+                    << path.requestedStart << "and end=" << path.requestedEnd;
     }
   }
 }
@@ -105,8 +107,8 @@ void NavTest::PathFinderTestCases() {
   pf.loadNavMesh(Cr::Utility::Path::join(
       SCENE_DATASETS, "habitat-test-scenes/skokloster-castle.navmesh"));
   esp::nav::ShortestPath testPath;
-  testPath.requestedStart = esp::vec3f(-6.493, 0.072, -3.292);
-  testPath.requestedEnd = esp::vec3f(-8.98, 0.072, -0.62);
+  testPath.requestedStart = Magnum::Vector3(-6.493, 0.072, -3.292);
+  testPath.requestedEnd = Magnum::Vector3(-8.98, 0.072, -0.62);
   pf.findPath(testPath);
   CORRADE_COMPARE(testPath.points.size(), 0);
   CORRADE_COMPARE(testPath.geodesicDistance,
@@ -114,14 +116,15 @@ void NavTest::PathFinderTestCases() {
 
   testPath.requestedStart = pf.getRandomNavigablePoint();
   // Jitter the point just enough so that it isn't exactly the same
-  testPath.requestedEnd = testPath.requestedStart + esp::vec3f(0.01, 0.0, 0.01);
+  testPath.requestedEnd =
+      testPath.requestedStart + Magnum::Vector3(0.01, 0.0, 0.01);
   pf.findPath(testPath);
   // There should be 2 points
   CORRADE_COMPARE(testPath.points.size(), 2);
   // The geodesicDistance should be almost exactly the L2 dist
   CORRADE_COMPARE_AS(
       std::abs(testPath.geodesicDistance -
-               (testPath.requestedStart - testPath.requestedEnd).norm()),
+               (testPath.requestedStart - testPath.requestedEnd).length()),
       0.001, Cr::TestSuite::Compare::LessOrEqual);
 }
 
@@ -130,12 +133,13 @@ void NavTest::PathFinderTestNonNavigable() {
   pf.loadNavMesh(Cr::Utility::Path::join(
       SCENE_DATASETS, "habitat-test-scenes/skokloster-castle.navmesh"));
 
-  const esp::vec3f nonNavigablePoint{1e2, 1e2, 1e2};
+  const Magnum::Vector3 nonNavigablePoint{1e2, 1e2, 1e2};
 
-  const esp::vec3f resultPoint = pf.tryStep<esp::vec3f>(
-      nonNavigablePoint, nonNavigablePoint + esp::vec3f{0.25, 0, 0});
+  const Magnum::Vector3 resultPoint = pf.tryStep(
+      nonNavigablePoint, nonNavigablePoint + Magnum::Vector3{0.25, 0, 0});
 
-  CORRADE_VERIFY(nonNavigablePoint.isApprox(resultPoint));
+  CORRADE_COMPARE_AS((nonNavigablePoint - resultPoint).length(), 1.0e-3,
+                     Cr::TestSuite::Compare::LessOrEqual);
 }
 
 void NavTest::PathFinderTestSeed() {
@@ -145,23 +149,23 @@ void NavTest::PathFinderTestSeed() {
 
   // The same seed should produce the same point
   pf.seed(1);
-  esp::vec3f firstPoint = pf.getRandomNavigablePoint();
+  Magnum::Vector3 firstPoint = pf.getRandomNavigablePoint();
   pf.seed(1);
-  esp::vec3f secondPoint = pf.getRandomNavigablePoint();
+  Magnum::Vector3 secondPoint = pf.getRandomNavigablePoint();
   CORRADE_COMPARE(firstPoint, secondPoint);
 
   // Different seeds should produce different points
   pf.seed(2);
-  esp::vec3f firstPoint2 = pf.getRandomNavigablePoint();
+  Magnum::Vector3 firstPoint2 = pf.getRandomNavigablePoint();
   pf.seed(3);
-  esp::vec3f secondPoint2 = pf.getRandomNavigablePoint();
+  Magnum::Vector3 secondPoint2 = pf.getRandomNavigablePoint();
   CORRADE_COMPARE_AS(firstPoint2, secondPoint2,
                      Cr::TestSuite::Compare::NotEqual);
 
   // One seed should produce different points when sampled twice
   pf.seed(4);
-  esp::vec3f firstPoint3 = pf.getRandomNavigablePoint();
-  esp::vec3f secondPoint3 = pf.getRandomNavigablePoint();
+  Magnum::Vector3 firstPoint3 = pf.getRandomNavigablePoint();
+  Magnum::Vector3 secondPoint3 = pf.getRandomNavigablePoint();
   CORRADE_COMPARE_AS(firstPoint3, secondPoint3,
                      Cr::TestSuite::Compare::NotEqual);
 }
diff --git a/src/tests/PathFinderTest.cpp b/src/tests/PathFinderTest.cpp
index 8d153d6ce0..ca2806d99b 100644
--- a/src/tests/PathFinderTest.cpp
+++ b/src/tests/PathFinderTest.cpp
@@ -66,7 +66,7 @@ void PathFinderTest::bounds() {
   Mn::Vector3 minExpected{-9.75916f, -0.390081f, 0.973853f};
   Mn::Vector3 maxExpected{8.56903f, 6.43441f, 25.5983f};
 
-  std::pair<esp::vec3f, esp::vec3f> bounds = pathFinder.bounds();
+  std::pair<Magnum::Vector3, Magnum::Vector3> bounds = pathFinder.bounds();
 
   CORRADE_COMPARE(Mn::Vector3{bounds.first}, minExpected);
   CORRADE_COMPARE(Mn::Vector3{bounds.second}, maxExpected);
@@ -80,12 +80,11 @@ void PathFinderTest::tryStepNoSliding() {
 
   Mn::Vector3 stepDir{0, 0, -1};
   for (int i = 0; i < 100; ++i) {
-    Mn::Vector3 pos{pathFinder.getRandomNavigablePoint()};
+    Mn::Vector3 pos = pathFinder.getRandomNavigablePoint();
     for (int j = 0; j < 10; ++j) {
       Mn::Vector3 targetPos = pos + stepDir;
       Mn::Vector3 actualEnd = pathFinder.tryStepNoSliding(pos, targetPos);
-      CORRADE_VERIFY(pathFinder.isNavigable(
-          Mn::EigenIntegration::cast<esp::vec3f>(actualEnd)));
+      CORRADE_VERIFY(pathFinder.isNavigable(actualEnd));
 
       // The test becomes unreliable if we moved a very small distance
       if (Mn::Math::gather<'x', 'z'>(actualEnd - pos).dot() < 1e-5)
@@ -108,7 +107,7 @@ void PathFinderTest::multiGoalPath() {
   pathFinder.seed(0);
 
   for (int __j = 0; __j < 1000; ++__j) {
-    std::vector<esp::vec3f> points;
+    std::vector<Magnum::Vector3> points;
     points.reserve(10);
     for (int i = 0; i < 10; ++i) {
       points.emplace_back(pathFinder.getRandomNavigablePoint());
@@ -116,7 +115,7 @@ void PathFinderTest::multiGoalPath() {
 
     esp::nav::MultiGoalShortestPath multiPath;
     multiPath.requestedStart = points[0];
-    multiPath.setRequestedEnds({points.begin() + 1, points.end()});
+    multiPath.requestedEnds = {points.begin() + 1, points.end()};
 
     CORRADE_VERIFY(pathFinder.findPath(multiPath));
 
@@ -124,7 +123,7 @@ void PathFinderTest::multiGoalPath() {
     path.requestedStart = points[0];
     float trueMinDist = 1e5;
     for (int i = 1; i < points.size(); ++i) {
-      path.setRequestedEnds({points[i]});
+      path.requestedEnds = {points[i]};
 
       CORRADE_VERIFY(pathFinder.findPath(path));
 
@@ -142,12 +141,12 @@ void PathFinderTest::testCaching() {
 
   esp::nav::MultiGoalShortestPath cachePath;
   {
-    std::vector<esp::vec3f> rqEnds;
+    std::vector<Magnum::Vector3> rqEnds;
     rqEnds.reserve(25);
     for (int i = 0; i < 25; ++i) {
       rqEnds.emplace_back(pathFinder.getRandomNavigablePoint());
     }
-    cachePath.setRequestedEnds(rqEnds);
+    cachePath.requestedEnds = rqEnds;
   }
 
   for (int i = 0; i < 1000; ++i) {
@@ -157,7 +156,7 @@ void PathFinderTest::testCaching() {
     pathFinder.findPath(cachePath);
 
     esp::nav::MultiGoalShortestPath noCachePath;
-    noCachePath.setRequestedEnds(cachePath.getRequestedEnds());
+    noCachePath.requestedEnds = cachePath.requestedEnds;
     noCachePath.requestedStart = cachePath.requestedStart;
     pathFinder.findPath(noCachePath);
 
@@ -195,12 +194,12 @@ void PathFinderTest::benchmarkMultiGoal() {
     path.requestedStart = pathFinder.getRandomNavigablePoint();
   } while (pathFinder.islandRadius(path.requestedStart) < 10.0);
 
-  std::vector<esp::vec3f> rqEnds;
+  std::vector<Magnum::Vector3> rqEnds;
   rqEnds.reserve(1000);
   for (int i = 0; i < 1000; ++i) {
     rqEnds.emplace_back(pathFinder.getRandomNavigablePoint());
   }
-  path.setRequestedEnds(rqEnds);
+  path.requestedEnds = rqEnds;
 
   if (data.cache) {
     pathFinder.findPath(path);
diff --git a/src/tests/SimTest.cpp b/src/tests/SimTest.cpp
index ffaf2531b3..8a5fbb778a 100644
--- a/src/tests/SimTest.cpp
+++ b/src/tests/SimTest.cpp
@@ -454,7 +454,7 @@ void SimTest::recomputeNavmeshWithStaticObjects() {
   simulator->recomputeNavMesh(*simulator->getPathFinder().get(),
                               navMeshSettings);
 
-  esp::vec3f randomNavPoint =
+  Magnum::Vector3 randomNavPoint =
       simulator->getPathFinder()->getRandomNavigablePoint();
   while (simulator->getPathFinder()->distanceToClosestObstacle(randomNavPoint) <
              1.0 ||
@@ -490,10 +490,10 @@ void SimTest::recomputeNavmeshWithStaticObjects() {
   int tmplateID = objectAttribsMgr->registerObject(objectTemplate);
 
   obj = rigidObjMgr->addObjectByHandle(objs[0]);
-  obj->setTranslation(Magnum::Vector3{randomNavPoint});
+  obj->setTranslation(randomNavPoint);
   obj->setTranslation(obj->getTranslation() + Magnum::Vector3{0, 0.5, 0});
   obj->setMotionType(esp::physics::MotionType::STATIC);
-  esp::vec3f offset(0.75, 0, 0);
+  Magnum::Vector3 offset(0.75, 0, 0);
   CORRADE_VERIFY(simulator->getPathFinder()->isNavigable(randomNavPoint, 0.1));
   CORRADE_VERIFY(
       simulator->getPathFinder()->isNavigable(randomNavPoint + offset, 0.2));
diff --git a/src/utils/viewer/viewer.cpp b/src/utils/viewer/viewer.cpp
index 69fd73797c..0e1fe0dc67 100644
--- a/src/utils/viewer/viewer.cpp
+++ b/src/utils/viewer/viewer.cpp
@@ -2239,9 +2239,8 @@ void Viewer::keyPressEvent(KeyEvent& event) {
       break;
     case KeyEvent::Key::Nine:
       if (simulator_->getPathFinder()->isLoaded()) {
-        const esp::vec3f position =
-            simulator_->getPathFinder()->getRandomNavigablePoint();
-        agentBodyNode_->setTranslation(Mn::Vector3(position));
+        auto position = simulator_->getPathFinder()->getRandomNavigablePoint();
+        agentBodyNode_->setTranslation(position);
       }
       break;
     case KeyEvent::Key::LeftBracket: