问题描述
假设 A1&A2 是构成线段的两个 3D 点。
T1,T2,T3 是 三个 3D 点，它们在 3D 空间中构成三角形多边形。
设 P1 为线段上的一个点，设 P2 为三角形多边形上的一个点
P1 和 P2 彼此最接近
现在，我该如何计算 P1 和 P2 我应该使用哪种方法？
现在我知道如何从一个点找到线段上最近的点。
和两点线段之间最近的两点。
我使用这个下面的函数来找到两条线段之间最近的线段
std::pair<Vector3D,Vector3D>
shortest_connection_segment_to_segment(const Vector3D A,const Vector3D B,const Vector3D C,const Vector3D D)
{
Vector3D u = B - A;
Vector3D v = D - C;
Vector3D w = A - C;
double a = u*u; // always >= 0
double b = u*v;
double c = v*v; // always >= 0
double d = u*w;
double e = v*w;
double sc,sN,sD = a*c - b*b; // sc = sN / sD,sD >= 0
double tc,tN,tD = a*c - b*b; // tc = tN / tD,tD >= 0
double tol = 1e-15;
// compute the line parameters of the two closest points
if (sD < tol) { // the lines are almost parallel
sN = 0.0; // force using point A on segment AB
sD = 1.0; // to prevent possible division by 0.0 later
tN = e;
tD = c;
}
else { // get the closest points on the infinite lines
sN = (b*e - c*d);
tN = (a*e - b*d);
if (sN < 0.0) { // sc < 0 => the s=0 edge is visible
sN = 0.0; // compute shortest connection of A to segment CD
tN = e;
tD = c;
}
else if (sN > sD) { // sc > 1 => the s=1 edge is visible
sN = sD; // compute shortest connection of B to segment CD
tN = e + b;
tD = c;
}
}
if (tN < 0.0) { // tc < 0 => the t=0 edge is visible
tN = 0.0;
// recompute sc for this edge
if (-d < 0.0) // compute shortest connection of C to segment AB
sN = 0.0;
else if (-d > a)
sN = sD;
else {
sN = -d;
sD = a;
}
}
else if (tN > tD) { // tc > 1 => the t=1 edge is visible
tN = tD;
// recompute sc for this edge
if ((-d + b) < 0.0) // compute shortest connection of D to segment AB
sN = 0;
else if ((-d + b) > a)
sN = sD;
else {
sN = (-d + b);
sD = a;
}
}
// finally do the division to get sc and tc
sc = (fabs(sN) < tol ? 0.0 : sN / sD);
tc = (fabs(tN) < tol ? 0.0 : tN / tD);
Vector3D P1 = A + (sc * u);
Vector3D P2 = C + (tc * v);
return {P1,P2}; // return the closest distance
}
解决方法
我想如果您“手动”执行此操作，您可能会花费大量时间来编写和调试大量代码。更好的方法是将其表述为一般问题的实例，然后寻找可以解决此问题的库。
在这种情况下，问题是“约束线性最小二乘法”，这很常见。
首先要做的是引入参数，例如：
直线上的点 P 由下式给出
P = P1 + l*(P2-P1) where 0<=l<=1
三角形中的点 T 由下式给出
T = T1 + s*(T2-T1) + t*(T3-T1)
where 0<=s<=1 and 0<=t<=1 and s+t<=1
目标函数是P和T之间的距离平方，即
d2 = ||P(l) - T(s,t)||^2
一点代数将其转换为标准的最小二乘形式：
d2 = ||A*x - b ||^2 where
x = (l,s,t)'
A = (P2-P1 T1-T2 T1-T3)
b = T1-P1
and the constraints,as above,are
0<=l and l<=1
0<=s and s<=1
0<=t and t<=1
s+t <= 1
此函数在下面获取三角形（由 3 个点定义）和线段（由 2 个点定义）之间最近的 2 个 3D 点（线段）。这些数学函数基于 @StefanKssmr 's Answer 和 @Spektre 's line closest(line l0,triangle t0)。这是带有示例的完整代码 Online Compiler。 这是示例的 Visual Representation
void ClosestPointBetweenTriangleAndLineSegment(Vector3D LineStart,Vector3D LineEnd,Vector3D Triangle0,Vector3D Triangle1,Vector3D Triangle2,Vector3D& ClosestPointOnLine,Vector3D& ClosestPointOnTriangle)
{
ClosestPointOnLine = LineStart;//For FirstPart Calculation Only
ClosestPointOnTriangle;
Vector3D Return1 = LineEnd;//For FirstPart Calculation Only
Vector3D Return2;
NearestPointBetweenPointAndPlane(ClosestPointOnLine,Triangle0,Triangle1,Triangle2,ClosestPointOnTriangle);
NearestPointBetweenPointAndPlane(Return1,Return2);
if (Magnitude(VectorMinus(Return1,Return2)) < Magnitude(VectorMinus(ClosestPointOnLine,ClosestPointOnTriangle)))
{
ClosestPointOnLine = Return1;
ClosestPointOnTriangle = Return2;
}
if (FastNoCheckIsPointWithinTraingle(ClosestPointOnTriangle,Triangle2))
{
return;
}
else
{
NearestPointBetweenTwoLineSegment(LineStart,LineEnd,ClosestPointOnLine,ClosestPointOnTriangle);// Only For First
NearestPointBetweenTwoLineSegment(LineStart,Return1,Return2);
if (Magnitude(VectorMinus(Return1,ClosestPointOnTriangle)))
{
ClosestPointOnLine = Return1;
ClosestPointOnTriangle = Return2;
}
NearestPointBetweenTwoLineSegment(LineStart,ClosestPointOnTriangle)))
{
ClosestPointOnLine = Return1;
ClosestPointOnTriangle = Return2;
}
}
}
以下是上述函数的辅助函数，全部基于普通数学
#include <iostream>
#include <cmath>
struct Vector3D
{
float x = 0;
float y = 0;
float z = 0;
};
struct Vector4D
{
float x = 0;
float y = 0;
float z = 0;
float d = 0;
};
Vector3D VectorAdd(Vector3D V1,Vector3D V2)
{
return { V1.x + V2.x,V1.y + V2.y,V1.z + V2.z };
}
Vector3D VectorMinus(Vector3D V1,Vector3D V2)
{
return { V1.x - V2.x,V1.y - V2.y,V1.z - V2.z };
}
Vector3D VectorMultiply(Vector3D V1,float Multiplier)
{
return { (V1.x * Multiplier),(V1.y * Multiplier),(V1.z * Multiplier) };
}
Vector3D VectorMultiplyVector(Vector3D V1,Vector3D V2)
{
return { (V1.x * V2.x),(V1.y * V2.y),(V1.z * V2.z) };
}
Vector3D VectorDivide(Vector3D V1,float Divider)
{
return{ (V1.x / Divider),(V1.y / Divider),(V1.z / Divider) };
}
float DotProduct(Vector3D V1,Vector3D V2)
{
return (float)(V1.x * V2.x) + (V1.y * V2.y) + (V1.z * V2.z);
}
Vector3D CrossProduct(Vector3D V1,Vector3D V2)
{
return { (V1.y * V2.z) - (V1.z * V2.y),(V1.z * V2.x) - (V1.x * V2.z),(V1.x * V2.y) - (V1.y * V2.x) };
}
Vector3D VectorPower(Vector3D V1,float Power)
{
return { pow(V1.x,Power),pow(V1.y,pow(V1.z,Power) };
}
float VectorTotal(Vector3D V1)
{
return DotProduct(V1,{ 1,1,1 });
}
float Magnitude(Vector3D Vector)
{
float CalculationFloat = (float)(sqrt(pow(Vector.x,2.0) + pow(Vector.y,2.0) + pow(Vector.z,2.0)));
if (std::isnan(CalculationFloat) || std::isinf(CalculationFloat))
{
return 0;
}
return CalculationFloat;
}
float AreaOfTriangle3D(Vector3D VA,Vector3D VB,Vector3D VC)
{
return Magnitude(CrossProduct(VectorMinus(VB,VA),VectorMinus(VC,VA))) / 2;
}
void NearestPointBetweenTwoLineSegment(Vector3D AB1,Vector3D AB2,Vector3D CD1,Vector3D CD2,Vector3D& resultSegmentPoint1,Vector3D& resultSegmentPoint2)
{
Vector3D u = VectorMinus(AB2,AB1);
Vector3D v = VectorMinus(CD2,CD1);
Vector3D w = VectorMinus(AB1,CD1);
double a = DotProduct(u,u); // always >= 0
double b = DotProduct(u,v);
double c = DotProduct(v,v); // always >= 0
double d = DotProduct(u,w);
double e = DotProduct(v,w);
double sN,sD = (a * c) - (b * b); // sc = sN / sD,default sD = D >= 0
double tN,tD = (a * c) - (b * b); // tc = tN / tD,default tD = D >= 0
float Temp1;
float Temp2;
float Temp3;// Unfortuantely i have no choice but to use this...
//Part 1
Temp1 = (sD < 1e-6f) ? 1.0f : 0.0f;
sN = (1.0f - Temp1) * (b * e - c * d);
sD = ((1.0f - Temp1) * sD) + Temp1;
tN = (Temp1 * e) + ((1.0f - Temp1) * ((a * e) - (b * d)));
tD = (Temp1 * c) + ((1.0f - Temp1) * tD);
Temp2 = (sN < 0.0f) ? 1.0f : 0.0f;
Temp2 = Temp2 * (1.0f - Temp1);
sN = ((1.0f - Temp2) * sN);
tN = ((1.0f - Temp2) * tN) + (Temp2 * e);
tD = ((1.0f - Temp2) * tD) + (Temp2 * c);
Temp2 = ((sN > sD) ? 1.0f : 0.0f) * (1.0f - Temp2);
Temp2 = Temp2 * (1.0f - Temp1);
sN = ((1.0f - Temp2) * sN) + (Temp2 * sD);
tN = ((1.0f - Temp2) * tN) + (Temp2 * (e + b));
tD = ((1.0f - Temp2) * tD) + (Temp2 * c);
//Part 2.1
Temp1 = (tN < 0.0f) ? 1.0f : 0.0f;
tN = tN * (1.0f - Temp1);
Temp2 = (((-d) < 0.0) ? 1.0f : 0.0f) * Temp1;
sN = (1.0f - Temp2) * sN;//sN = (Temp2 * 0) + ((1.0f - Temp2) * sN);
Temp3 = ((((-d) > a) ? 1.0f : 0.0f) * (1.0f - Temp2)) * (Temp1);
sN = (Temp3 * sD) + ((1.0f - Temp3) * (sN));
Temp2 = (1.0f - Temp3) * (1.0f - Temp2) * (Temp1);
sN = (Temp2 * (-d)) + ((1.0f - Temp2) * (sN));
sD = (Temp2 * a) + ((1.0f - Temp2) * (sD));
//Part 2.2
Temp1 = ((tN > tD) ? 1.0f : 0.0f) * (1.0f - Temp1);
tN = ((1.0f - Temp1) * tN) + (Temp1 * tD);
Temp2 = (((-d + b) < 0.0) ? 1.0f : 0.0f) * Temp1;
sN = (1.0f - Temp2) * sN;//sN = (Temp2 * 0) + ((1.0f - Temp2) * sN);
Temp3 = ((((-d + b) > a) ? 1.0f : 0.0f) * (1.0f - Temp2)) * (Temp1);
sN = (Temp3 * sD) + ((1.0f - Temp3) * (sN));
Temp2 = (1.0f - Temp3) * (1.0f - Temp2) * (Temp1);
sN = (Temp2 * (-d)) + ((1.0f - Temp2) * (sN));
sD = (Temp2 * a) + ((1.0f - Temp2) * (sD));
resultSegmentPoint1 = VectorAdd(AB1,VectorMultiply(u,(fabs(sN) < 1e-6f ? 0.0 : sN / sD)));
resultSegmentPoint2 = VectorAdd(CD1,VectorMultiply(v,(fabs(tN) < 1e-6f ? 0.0 : tN / tD)));
}
bool FastNoCheckIsPointWithinTraingle(Vector3D Point,Vector3D Triangle2)
{
return fabsf((AreaOfTriangle3D(Point,Triangle2) + AreaOfTriangle3D(Triangle0,Point,Point)) - AreaOfTriangle3D(Triangle0,Triangle2)) < 1.0f;
}
Vector4D equation_plane(float x1,float y1,float z1,float x2,float y2,float z2,float x3,float y3,float z3)
{
float a1 = x2 - x1;
float b1 = y2 - y1;
float c1 = z2 - z1;
float a2 = x3 - x1;
float b2 = y3 - y1;
float c2 = z3 - z1;
float a = b1 * c2 - b2 * c1;
float b = a2 * c1 - a1 * c2;
float c = a1 * b2 - b1 * a2;
float d = (-a * x1 - b * y1 - c * z1);
//std::cout << std::fixed;
//std::cout << std::setprecision(2);
std::cout << "\nequation of plane is " << a << " x + " << b << " y + " << c << " z + ";
printf("%f",d);
std::cout << " = 0.";
return { a,b,c,d };
}
void NearestPointBetweenPointAndPlane(Vector3D Point,Vector3D& resultSegmentPoint)
{
//Note: Plane equation is x + y + z + d = 0 format//
Vector4D Plane = equation_plane(Triangle0.x,Triangle0.y,Triangle0.z,Triangle1.x,Triangle1.y,Triangle1.z,Triangle2.x,Triangle2.y,Triangle2.z);
Vector3D PlanePartial;
PlanePartial.x = Plane.x;
PlanePartial.y = Plane.y;
PlanePartial.z = Plane.z;
resultSegmentPoint = VectorAdd(Point,VectorMultiply(PlanePartial,-((VectorTotal(VectorMultiplyVector(Point,PlanePartial)) + Plane.d) / VectorTotal(VectorPower(PlanePartial,2)))));
}
可选调试功能
int Clamp(double Number,double Min,double Max)
{
if (Number > Max)
{
return Max;
}
if (Number < Min)
{
return Min;
}
return Number;
}
void PrintVector3Dfor(Vector3D Point,std::string Name,bool InvertXY)
{
int SpaceBar = 50;
SpaceBar = Clamp(SpaceBar - Name.length(),50);
std::cout << "\n" << Name << " =";
for (int i = 0; i < SpaceBar; ++i)
{
std::cout << " ";
}
std::cout << "(";
if (InvertXY)
{
printf("%f",Point.y);
printf(",%f",Point.x);
}
else
{
printf("%f",Point.x);
printf(",Point.y);
}
printf(",%f )",Point.z);
}
void Printfloatfor(float val,std::string Name)
{
int SpaceBar = 50;
SpaceBar = Clamp(SpaceBar - Name.length(),50);
std::cout << "\n" << Name << " =";
for (int i = 0; i < SpaceBar; ++i)
{
std::cout << " ";
}
printf(" %f",val);
}