100 lines
3.4 KiB
C#
100 lines
3.4 KiB
C#
using System.Collections;
|
||
using System.Collections.Generic;
|
||
using UnityEngine;
|
||
|
||
namespace BITKit.Physics
|
||
{
|
||
public static class GeometryUtils
|
||
{
|
||
/// <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;
|
||
}
|
||
/// <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);
|
||
}
|
||
}
|
||
|
||
}
|
||
|