From b496605a1276cc1ed22c9d7656d2a58f43c152e9 Mon Sep 17 00:00:00 2001
From: Brandon Lai-Cheong <blaicheo@uwaterloo.ca>
Date: Mon, 2 Dec 2024 14:41:12 -0500
Subject: [PATCH] fixed interpolate
---
include/interpolate_z.h | 3 ++-
src/utilities/interpolate_z.cpp | 28 +++++++++++-----------------
2 files changed, 13 insertions(+), 18 deletions(-)
diff --git a/include/interpolate_z.h b/include/interpolate_z.h
index 914b763..473f253 100644
--- a/include/interpolate_z.h
+++ b/include/interpolate_z.h
@@ -1,5 +1,6 @@
#pragma once
#include <vector>
struct Point;
+struct Edge;
-float interpolateZ(const std::vector<Point> &shape, const Point &p);
\ No newline at end of file
+float interpolateZ(const Edge &e, const Point &p);
diff --git a/src/utilities/interpolate_z.cpp b/src/utilities/interpolate_z.cpp
index b822762..d375900 100644
--- a/src/utilities/interpolate_z.cpp
+++ b/src/utilities/interpolate_z.cpp
@@ -1,26 +1,20 @@
#include <interpolate_z.h>
#include <limits>
#include <point.h>
+#include <edge.h>
#include <cmath>
+float dist2D(const Point &p) {
+ return sqrt(p.x * p.x + p.y * p.y);
+}
-float interpolateZ(const std::vector<Point> &shape, const Point &p) {
- // create direction vectors
- const Point v1 = -(shape[0] - shape[1]);
- const Point v2 = -(shape[0] - shape[2]);
-
- const Point &origin = shape[0];
-
- float t = (p.y * v1.x - origin.y * v1.x - p.x * v1.y + origin.x * v1.y) / (v1.x * v2.y - v1.y * v2.x);
- float s = (p.y - origin.y - t * v2.y) / v1.y;
-
- if (s == - std::numeric_limits<float>::infinity() || std::isnan(s)) {
- s = 0;
- }
+float interpolateZ(const Edge &e, const Point &p) {
+ const float p1Dist = dist2D(e.p1);
+ const float p2Dist = dist2D(e.p2);
- if (t == - std::numeric_limits<float>::infinity() || std::isnan(t)) {
- t = 0;
- }
+ const float b = (p2Dist * e.p1.z - p1Dist * e.p2.z) / (p2Dist - p1Dist);
+
+ const float m = (e.p2.z - b) / p2Dist;
- return origin.z + v1.z * s + v2.z * t;
+ return m * dist2D(p) + b;
}
--
GitLab