96 lines
2.8 KiB
C++
96 lines
2.8 KiB
C++
#include "point_tris_dist.h"
|
|
|
|
#include <vtkPoints.h>
|
|
#include <vtkTriangle.h>
|
|
#include <vtkMath.h>
|
|
|
|
double pointSegmentDistance(double *p, double *a, double *b, double* direction) {
|
|
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) {
|
|
direction[0] = ap[0];
|
|
direction[1] = ap[1];
|
|
direction[2] = ap[2];
|
|
return vtkMath::Normalize(direction);
|
|
}
|
|
|
|
double h = vtkMath::ClampValue(vtkMath::Dot(ap, ab) / segSqrLength, 0., 1.);
|
|
|
|
vtkMath::MultiplyScalar(ab, h);
|
|
double v[3]; vtkMath::Subtract(ap, ab, v);
|
|
|
|
direction[0] = v[0];
|
|
direction[1] = v[1];
|
|
direction[2] = v[2];
|
|
return vtkMath::Normalize(direction);
|
|
}
|
|
|
|
double pointTriangleDistance(double *p, vtkCell *triangle, double *direction) {
|
|
double a[3]; triangle->GetPoints()->GetPoint(0, a);
|
|
double b[3]; triangle->GetPoints()->GetPoint(1, b);
|
|
double c[3]; triangle->GetPoints()->GetPoint(2, 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 d = vtkMath::Dot(n, ap);
|
|
double n2 = vtkMath::Dot(n, n);
|
|
|
|
if(d >= 0) {
|
|
direction[0] = n[0];
|
|
direction[1] = n[1];
|
|
direction[2] = n[2];
|
|
} else {
|
|
direction[0] = -n[0];
|
|
direction[1] = -n[1];
|
|
direction[2] = -n[2];
|
|
}
|
|
|
|
return sqrt(d * d / n2);
|
|
} 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) {
|
|
direction[0] = normalA[0];
|
|
direction[1] = normalA[1];
|
|
direction[2] = normalA[2];
|
|
} else if(min == lbc) {
|
|
direction[0] = normalB[0];
|
|
direction[1] = normalB[1];
|
|
direction[2] = normalB[2];
|
|
} else {
|
|
direction[0] = normalC[0];
|
|
direction[1] = normalC[1];
|
|
direction[2] = normalC[2];
|
|
}
|
|
|
|
return min;
|
|
}
|
|
}
|