通用的一个方法GetDistance,计算两个点的距离,不开平方
/// <summary>
/// 获取两个点的距离,不开平方
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static float GetDistance(Vector3 a, Vector3 b)
{
float num = a.x - b.x;
float num2 = a.y - b.y;
float num3 = a.z - b.z;
return num * num + num2 * num2 + num3 * num3;
}
1.计算一条线段上到一个点距离最短的点
/// <summary>
/// 获取一个点到一条线段的最近点
/// </summary>
/// <param name="point"></param>
/// <param name="lineStart"></param>
/// <param name="lineEnd"></param>
/// <returns></returns>
public static Vector3 PointToLineSegmentDistance(Vector3 point, Vector3 lineStart, Vector3 lineEnd)
{
Vector3 lineDirection = lineEnd - lineStart;
Vector3 pointDirection = point - lineStart;
float lineLength = lineDirection.magnitude;
lineDirection.Normalize();
float dotProduct = Vector3.Dot(pointDirection, lineDirection);
dotProduct = Mathf.Clamp(dotProduct, 0f, lineLength);
Vector3 closestPoint = lineStart + lineDirection * dotProduct;
return closestPoint;
}
2.计算一个三角形内距离一个点最短的点
/// <summary>
/// 获取一个点到一个三角形内最短距离的点
/// </summary>
/// <param name="a">三角形顶点a</param>
/// <param name="b">三角形顶点b</param>
/// <param name="c">三角形顶点c</param>
/// <param name="pos"></param>
/// <returns></returns>
public static Vector3 GetPosInTriangle(Vector3 a, Vector3 b, Vector3 c, Vector3 pos)
{
Vector3 normal = Vector3.Cross(b - a, c - a).normalized;
Vector3 toPoint = pos - a;
float distance = Vector3.Dot(toPoint, normal);
Vector3 targetPos = pos - distance * normal;
if(PointInTriangle(targetPos, a, b, c))
return targetPos;
else
{
Vector3 p1 = PointToLineSegmentDistance(pos, a, b);
Vector3 p2 = PointToLineSegmentDistance(pos, a, c);
Vector3 p3 = PointToLineSegmentDistance(pos, b, c);
float d1 = Vector3.Distance(p1, pos);
float d2 = Vector3.Distance(p2, pos);
float d3 = Vector3.Distance(p3, pos);
if (d1 <= d2 && d1 <= d3)
return p1;
else if (d2 <= d3 && d2 <= d1)
return p2;
else /*if(d3 <= d1 && d3 <= d2)*/
return p3;
//return default;
}
}
/// <summary>
/// 判断一个点是否在三角形内
/// </summary>
/// <param name="pos"></param>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="c"></param>
/// <returns></returns>
public static bool PointInTriangle(Vector3 pos, Vector3 a, Vector3 b, Vector3 c)
{
var v0 = c - a;
var v1 = b - a;
var v2 = pos - a;
var dot00 = Vector3.Dot(v0, v0);
var dot01 = Vector3.Dot(v0, v1);
var dot02 = Vector3.Dot(v0, v2);
var dot11 = Vector3.Dot(v1, v1);
var dot12 = Vector3.Dot(v1, v2);
var invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
var u = (dot11 * dot02 - dot01 * dot12) * invDenom;
var v = (dot00 * dot12 - dot01 * dot02) * invDenom;
// 如果u和v都在[0,1]的范围内,那么点P在三角形ABC内
return (u >= 0) && (v >= 0) && (u + v < 1);
}
3.计算两条线段间距离最短的两个点,此方法不是最优方法,还需修改
/// <summary>
/// 计算两个线段的最近点
/// 此方法需要修改,现在实现的方法不是最好的方法,存在误差
/// </summary>
/// <param name="line1Point1"></param>
/// <param name="line1Point2"></param>
/// <param name="line2Point1"></param>
/// <param name="lin2Point2"></param>
/// <param name="value1"></param>
/// <param name="value2"></param>
public static float LineToLine(Vector3 line1Point1, Vector3 line1Point2, Vector3 line2Point1, Vector3 lin2Point2, out Vector3 value1, out Vector3 value2)
{
List<Vector3> points = new List< Vector3>();
value1 = Vector3.zero ;
value2 = Vector3.zero ;
//这边的方法需要修改,目前是将线段1按每距离0.1米加一个点(包括两个端点)
float dis = Vector3.Distance(line1Point1, line1Point2);
int count = (int)(dis * 10);
Vector3 dir = (line1Point2 - line1Point1).normalized;
points.Add(line1Point1);
for (int i = 1; i <= count; i++)
{
points.Add(dir * 0.1f * i + line1Point1);
}
points.Add(line1Point2);
float minDis = float.MaxValue;
//然后使用每个点计算到另一线段距离最短的点,最后比较得到距离最短的点
foreach (var item in points)
{
Vector3 p = PointToLineSegmentDistance(item, line2Point1, lin2Point2);
float d = GetDistance(p, item);
if(d < minDis)
{
minDis = d;
value1 = item;
value2 = p;
}
}
return minDis;
}
4.计算一条线段到一个三角面距离最短的两个点
/// <summary>
/// 计算一条线段到一个三角面距离最短的两个点
/// </summary>
/// <param name="linePoints"></param>
/// <param name="triPoints"></param>
/// <param name="value1"></param>
/// <param name="value2"></param>
public static void LineToTriangle(List<Vector3> linePoints, List<Vector3> triPoints, out Vector3 value1, out Vector3 value2)
{
value1 = Vector3.zero;
value2 = Vector3.zero;
Vector3 pos = default;
//判断线段与三角面是否相交,若相交,则距离最短的点为交点
if (LineIntersectsTriangle(linePoints[0], linePoints[1], triPoints[0], triPoints[1], triPoints[2],out pos))
{
value1 = pos;
value2 = pos;
return;
}
//不相交的话,分别计算这条边与三角形三条边距离最短的点
Vector3 p1 = default;
Vector3 p2 = default;
float dis1 = LineToLine(linePoints[0], linePoints[1], triPoints[0], triPoints[1], out p1, out p2);
Vector3 p3 = default;
Vector3 p4 = default;
float dis2 = LineToLine(linePoints[0], linePoints[1], triPoints[0], triPoints[2], out p3, out p4);
Vector3 p5 = default;
Vector3 p6 = default;
float dis3 = LineToLine(linePoints[0], linePoints[1], triPoints[2], triPoints[1], out p5, out p6);
//分别计算线段的端点到三角形距离最短的点
Vector3 p7 = GetPosInTriangle(triPoints[0], triPoints[1], triPoints[2], linePoints[0]);
Vector3 p8 = GetPosInTriangle(triPoints[0], triPoints[1], triPoints[2], linePoints[1]);
float dis4 = GetDistance(p7, linePoints[0]);
float dis5 = GetDistance(p8, linePoints[1]);
//比较得到最短距离的点
float min = Mathf.Min(dis1, dis2, dis3, dis4, dis5);
if(min == dis1)
{
value1 = p1;
value2 = p2;
return;
}
else if(min == dis2)
{
value1 = p3;
value2 = p4;
return;
}
else if(min == dis3)
{
value1 = p5;
value2 = p6;
return;
}
else if(min == dis4)
{
value1 = p7;
value2 = linePoints[0];
return;
}
else
{
value1 = p8;
value2 = linePoints[1];
return;
}
}
/// <summary>
/// 判断线段是否与一个三角面有交点,并得到这个交点
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="v0"></param>
/// <param name="v1"></param>
/// <param name="v2"></param>
/// <param name="pos"></param>
/// <returns></returns>
public static bool LineIntersectsTriangle(Vector3 a, Vector3 b, Vector3 v0, Vector3 v1, Vector3 v2, out Vector3 pos)
{
// 计算三角形的法线
Vector3 triangleNormal = Vector3.Cross(v1 - v0, v2 - v0).normalized;
pos = default;
// 计算线段的方向
Vector3 lineDirection = b - a;
// 计算线段与平面的交点
float denominator = Vector3.Dot(triangleNormal, lineDirection);
if (denominator != 0)
{
// 线段与平面不平行,计算交点
float t = Vector3.Dot(v0 - a, triangleNormal) / denominator;
if (t >= 0 && t <= 1)
{
// 交点在线段内部
Vector3 intersectionPoint = a + t * lineDirection;
// 检查交点是否在三角形内部
if (PointInTriangle(intersectionPoint, v0, v1, v2))
{
pos = intersectionPoint;
return true;
}
else
{
return false;
}
}
else
{
return false;
}
}
else
{
return false;
}
}
5.计算两个三角形间距离最短的两个点
/// <summary>
/// 计算两个三角形间距离最短的两个点
/// </summary>
/// <param name="tri1"></param>
/// <param name="tri2"></param>
/// <param name="value1"></param>
/// <param name="value2"></param>
public static void GetPosInTriangles(List<Vector3> tri1, List<Vector3> tri2, out Vector3 value1, out Vector3 value2)
{
//分别计算三角形1的三条边到三角形2的最短距离
Vector3 p1 = default;
Vector3 p2 = default;
LineToTriangle(new List<Vector3>() { tri1[0], tri1[1] }, tri2, out p1, out p2);
Vector3 p3 = default;
Vector3 p4 = default;
LineToTriangle(new List<Vector3>() { tri1[0], tri1[2] }, tri2, out p3, out p4);
Vector3 p5 = default;
Vector3 p6 = default;
LineToTriangle(new List<Vector3>() { tri1[1], tri1[2] }, tri2, out p5, out p6);
//Vector3 p11 = default;
//Vector3 p21 = default;
//LineToTriangle(new List<Vector3>() { tri2[0], tri2[1] }, tri1, out p11, out p21);
//Vector3 p31 = default;
//Vector3 p41 = default;
//LineToTriangle(new List<Vector3>() { tri2[0], tri2[2] }, tri1, out p31, out p41);
//Vector3 p51 = default;
//Vector3 p61 = default;
//LineToTriangle(new List<Vector3>() { tri2[1], tri2[2] }, tri1, out p51, out p61);
float dis1 = GetDistance(p1, p2);
float dis2 = GetDistance(p3, p4);
float dis3 = GetDistance(p5, p6);
//float dis4 = GetDistance(p11, p21);
//float dis5 = GetDistance(p31, p41);
//float dis6 = GetDistance(p51, p61);
//比较得到面之间距离最短的点
float min = Mathf.Min(dis1, dis2, dis3/*, dis4, dis5, dis6*/);
if(min == dis1)
{
value1 = p1;
value2 = p2;
return;
}
else if(min == dis2)
{
value1 = p3;
value2 = p4;
return;
}
else/* if(min == dis3)*/
{
value1 = p5;
value2 = p6;
return;
}
//else if(min == dis4)
//{
// value1 = p11;
// value2 = p21;
// return;
//}
//else if(min == dis5)
//{
// value1 = p31;
// value2 = p41;
// return;
//}
//else
//{
// value1 = p51;
// value2 = p61;
// return;
//}
}