fixed z interpolation

src/utilities/interpolate_z.cpp

@@ -11,10 +11,15 @@ float dist2D(const Point &p) {
 float interpolateZ(const Edge &e, const Point &p) {
 	const float p1Dist = dist2D(e.p1);
 	const float p2Dist = dist2D(e.p2);
+    const float pDist = dist2D(p);
 
-	const float b = (p2Dist * e.p1.z - p1Dist * e.p2.z) / (p2Dist - p1Dist);
-	
-	const float m = (e.p2.z - b) / p2Dist;
+    if (p1Dist - p2Dist == 0) {
+        return e.p1.z;
+    }
 
-	return m * dist2D(p) + b;
+    const float m = (e.p1.z - e.p2.z) / (p1Dist - p2Dist);
+    const float b = e.p1.z - p1Dist * m;
+
+
+	return m * pDist + b;
 }

src/utilities/intersections.cpp

@@ -62,6 +62,8 @@ std::optional<Point> intersectionFirstSide(const Edge &e1, const Edge &e2) {
     auto candPoint = intersection(e1, e2);
 
     if (candPoint.has_value() && withinEdge(e2, candPoint.value())) {
+        float z = interpolateZ(e2, candPoint.value());
+        candPoint.value().z = z;
         return candPoint;
     }
     return {};
@@ -172,9 +174,6 @@ std::vector<Point> intersections(const std::vector<Point> &points, const Edge &l
         auto cand = intersectionFirstSide(line, es[i]);
         if (cand.has_value())
         {
-            // add depth information
-            float z = interpolateZ(es[i], cand.value());
-            cand.value().z = z;
             result.push_back(cand.value());
         }
     }

tests/unit_tests/interpolate_z_tests.cpp

 #include <gtest/gtest.h>
 #include <interpolate_z.h>
 #include <point.h>
+#include <edge.h>
 
 struct InterpolateParams {
-    std::vector<Point> points;
+    Edge e;
+    Point point;
     float expected_z;
 };
 
+std::ostream &operator<<(std::ostream & out, const Edge &e) {
+    return out << e.p1 << ", " << e.p2;
+}
+
+std::ostream &operator<<(std::ostream & out, const InterpolateParams &p) {
+    return out << "Edge: " << p.e << "Point: " << p.point;
+}
+
 class InstantiateInterpolateZTests : public testing::TestWithParam<InterpolateParams>{
 };
 
 TEST_P (InstantiateInterpolateZTests, Interpolate) {
     
-	std::vector<Point> points = GetParam().points;
+	Edge e = GetParam().e;
+    Point point = GetParam().point;
 
-    float z = interpolateZ(points, {0,1,0});
+    float z = interpolateZ(e, point);
 
     EXPECT_EQ(z,GetParam().expected_z);
 }
 
 INSTANTIATE_TEST_SUITE_P(Interpolation, InstantiateInterpolateZTests, testing::Values(
-    InterpolateParams{
-        {{1,1,3}, {2,2,3}, {4,5,3}}
-        , 3},
-    InterpolateParams{{{1,1,5}, {1,1,5}, {1,1,5}}, 5}));
+    InterpolateParams{{{1,1,3}, {2,2,3}}, {1.5,1.5},3},
+    InterpolateParams{{{1,1,5}, {1,1,5}}, {1,1},5},
+    InterpolateParams{{{1,1,10}, {2,3,15}}, {2,3},15},
+    InterpolateParams{{{1,1,10}, {2,2,0}}, {1,1},10},
\ No newline at end of file
+    InterpolateParams{{{1,1,10}, {2,2,0}}, {2,2},0},
+    InterpolateParams{{{5, 3,3}, {2.5, 6,3}}, {0.540541,3.64865},3}
+    ));
+