pointTriangleDistance (à tester)

CMakeLists.txt

@@ -52,7 +52,9 @@ target_sources(pfe PRIVATE
   src/kd_tree.cc
   src/kd_tree.h
   src/mesh_fit_filter.cc
-  src/mesh_fit_filter.h)
+  src/mesh_fit_filter.h
+  src/point_tris_dist.cc
+  src/point_tris_dist.h)
 
 target_link_libraries(pfe PRIVATE ${VTK_COMPONENTS})
 

src/point_tris_dist.cc

@@ -0,0 +1,109 @@
+#include "point_tris_dist.h"
+
+#include <vtkPoints.h>
+#include <vtkMath.h>
+
+double pointSegmentDistance(double *p, double *a, double *b, double* normal) {
+    double segSqrLength = vtkMath::Distance2BetweenPoints(a, b);
+
+    double ap[3]; vtkMath::Subtract(p, a, ap);
+    double ab[3]; vtkMath::Subtract(b, a, ab);
+
+	if(segSqrLength <= 0) {
+        normal[0] = ap[0];
+        normal[1] = ap[1];
+        normal[2] = ap[2];
+        vtkMath::Normalize(normal);
+        return vtkMath::Norm(ap, 3);
+    }
+	
+    double h = vtkMath::ClampValue(vtkMath::Dot(ap, ab) / segSqrLength, 0., 1.);
+
+    vtkMath::MultiplyScalar(ab, h);
+    double v[3]; vtkMath::Subtract(ap, ab, v);
+
+    normal[0] = v[0];
+    normal[1] = v[1];
+    normal[2] = v[2];
+    vtkMath::Normalize(normal);
+	return vtkMath::Norm(v);
+}
+
+double pointTriangleDistance(double *p, vtkTriangle *triangle, double *normal) {
+    double a[3]; triangle->GetPoints()->GetPoint(0, a);
+    double b[3]; triangle->GetPoints()->GetPoint(0, b);
+    double c[3]; triangle->GetPoints()->GetPoint(0, c);
+
+    double ab[3]; vtkMath::Subtract(b, a, ab);
+    double bc[3]; vtkMath::Subtract(c, b, bc);
+    double ca[3]; vtkMath::Subtract(a, c, ca);
+
+    double n[3]; vtkTriangle::ComputeNormal(a, b, c, n);
+    double vecTA[3]; vtkMath::Cross(ab, n, vecTA);
+    double vecTB[3]; vtkMath::Cross(bc, n, vecTB);
+    double vecTC[3]; vtkMath::Cross(ca, n, vecTC);
+
+    double ap[3]; vtkMath::Subtract(p, a, ap);
+    double bp[3]; vtkMath::Subtract(p, b, bp);
+    double cp[3]; vtkMath::Subtract(p, c, cp);
+
+    double da = vtkMath::Dot(vecTA, ap);
+	double db = vtkMath::Dot(vecTB, bp);
+	double dc = vtkMath::Dot(vecTC, cp);
+
+	if(da <= 0 && db <= 0 && dc <= 0) {
+        double na[3] = {
+            n[0] * a[0],
+            n[1] * a[1],
+            n[2] * a[2],
+        };
+
+        double np[3] = {
+            n[0] * p[0],
+            n[1] * p[1],
+            n[2] * p[2],
+        };
+
+        double t[3]; vtkMath::Subtract(na, np, t);
+
+        double nt[3] = {
+            n[0] * t[0],
+            n[1] * t[1],
+            n[2] * t[2],
+        };
+
+
+        normal[0] = np[0];
+        normal[1] = np[1];
+        normal[2] = np[2];
+        vtkMath::Normalize(normal);
+        return vtkMath::Norm(nt);
+
+	} else {
+        double normalA[3];
+        double normalB[3];
+        double normalC[3];
+
+		double lab = pointSegmentDistance(p, a, b, normalA);
+		double lbc = pointSegmentDistance(p, b, c, normalB);
+		double lca = pointSegmentDistance(p, c, a, normalC);
+
+		double min = vtkMath::Min(lab, vtkMath::Min(lbc, lca));
+
+        if(min == lab) {
+            normal[0] = normalA[0];
+            normal[1] = normalA[1];
+            normal[2] = normalA[2];
+        } else if(min == lbc) {
+            normal[0] = normalB[0];
+            normal[1] = normalB[1];
+            normal[2] = normalB[2];
+        } else {
+            normal[0] = normalC[0];
+            normal[1] = normalC[1];
+            normal[2] = normalC[2];
+        }
+
+        return min;
+	}
+}

src/point_tris_dist.h

@@ -0,0 +1,9 @@
+#ifndef POINT_TRIS_DIST_H
+#define POINT_TRIS_DIST_H
+
+#include <vtkTriangle.h>
+#include <utility>
+
+double pointTriangleDistance(double *point, vtkTriangle *triangle, double *normal);
+
+#endif