diff --git a/CHANGELOG.md b/CHANGELOG.md
index ba20431916..2f2ced4e40 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -42,6 +42,7 @@
    * FIXED: CostMatrix for trivial routes with oneways [#4626](https://github.com/valhalla/valhalla/pull/4626)
    * FIXED: some entry points to creating geotiff isochrones output did not register the geotiff driver before attempting to use it [#4628](https://github.com/valhalla/valhalla/pull/4628)
    * FIXED: libgdal wasn't installed in docker image, so it never worked in docker [#4629](https://github.com/valhalla/valhalla/pull/4629)
+   * FIXED: CostMatrix shapes for routes against trivial oneways [#4633](https://github.com/valhalla/valhalla/pull/4633)
 * **Enhancement**
    * UPDATED: French translations, thanks to @xlqian [#4159](https://github.com/valhalla/valhalla/pull/4159)
    * CHANGED: -j flag for multithreaded executables to override mjolnir.concurrency [#4168](https://github.com/valhalla/valhalla/pull/4168)
diff --git a/src/thor/costmatrix.cc b/src/thor/costmatrix.cc
index 1a1cb17986..db67b68210 100644
--- a/src/thor/costmatrix.cc
+++ b/src/thor/costmatrix.cc
@@ -1243,9 +1243,10 @@ std::string CostMatrix::RecostFormPath(GraphReader& graphreader,
   auto source_vertex = PointLL{source_edge.ll().lng(), source_edge.ll().lat()};
   auto target_vertex = PointLL{target_edge.ll().lng(), target_edge.ll().lat()};
   std::vector<PointLL> points;
-  for (const auto& path_edge : path_edges) {
-    auto is_first_edge = path_edge == path_edges.front();
-    auto is_last_edge = path_edge == path_edges.back();
+  for (uint32_t i = 0; i < path_edges.size(); i++) {
+    auto& path_edge = path_edges[i];
+    auto is_first_edge = i == 0;
+    auto is_last_edge = i == (path_edges.size() - 1);
 
     const auto* de = graphreader.directededge(path_edge, tile);
     auto edge_shp = tile->edgeinfo(de).shape();
diff --git a/test/gurka/test_matrix.cc b/test/gurka/test_matrix.cc
index b0e3cfcf88..3500fcdaec 100644
--- a/test/gurka/test_matrix.cc
+++ b/test/gurka/test_matrix.cc
@@ -504,8 +504,9 @@ TEST(StandAlone, CostMatrixShapes) {
   /*
   EXPECT_EQ(result.matrix().shapes(0), encoded);
   EXPECT_EQ(res_doc.Parse(res.c_str())["sources_to_targets"].GetArray()[0].GetArray()[0].GetObject()["shape"],
-  encoded); res.erase();
+  encoded);
   */
+  res.erase();
 
   // trivial route reverse
   // has a bug: https://github.com/valhalla/valhalla/issues/4433, but it's band-aided for now
@@ -517,8 +518,9 @@ TEST(StandAlone, CostMatrixShapes) {
   /*
   EXPECT_EQ(result.matrix().shapes(0), encoded);
   EXPECT_EQ(res_doc.Parse(res.c_str())["sources_to_targets"].GetArray()[0].GetArray()[0].GetObject()["shape"],
-  encoded); res.erase();
+  encoded);
   */
+  res.erase();
 
   // timedistancematrix
 
@@ -788,19 +790,33 @@ TEST(StandAlone, CostMatrixTrivialRoutes) {
       {"CD", {{"highway", "residential"}}}, {"BE", {{"highway", "residential"}}},
       {"EF", {{"highway", "residential"}}}, {"FC", {{"highway", "residential"}}},
   };
-  const auto layout = gurka::detail::map_to_coordinates(ascii_map, 100);
+  auto layout = gurka::detail::map_to_coordinates(ascii_map, 100);
   auto map =
       gurka::buildtiles(layout, ways, {}, {}, VALHALLA_BUILD_DIR "test/data/costmatrix_trivial");
 
+  std::unordered_map<std::string, std::string> options = {{"/shape_format", "polyline6"}};
+
   // test the against-oneway case
   {
-    auto matrix = gurka::do_action(valhalla::Options::sources_to_targets, map, {"1"}, {"2"}, "auto");
+    auto matrix =
+        gurka::do_action(valhalla::Options::sources_to_targets, map, {"1"}, {"2"}, "auto", options);
     EXPECT_EQ(matrix.matrix().distances(0), 2200);
+
+    std::vector<PointLL> oneway_vertices;
+    for (auto& node : {"1", "C", "F", "E", "B", "2"}) {
+      oneway_vertices.push_back(layout[node]);
+    }
+    auto encoded = encode<std::vector<PointLL>>(oneway_vertices, 1e6);
+    EXPECT_EQ(matrix.matrix().shapes(0), encoded);
   }
 
   // test the normal trivial case
   {
-    auto matrix = gurka::do_action(valhalla::Options::sources_to_targets, map, {"3"}, {"4"}, "auto");
+    auto matrix =
+        gurka::do_action(valhalla::Options::sources_to_targets, map, {"3"}, {"4"}, "auto", options);
     EXPECT_EQ(matrix.matrix().distances(0), 400);
+
+    auto encoded = encode<std::vector<PointLL>>({layout["3"], layout["4"]}, 1e6);
+    EXPECT_EQ(matrix.matrix().shapes(0), encoded);
   }
 }