mirror of
https://gitee.com/jisol/jisol-game/
synced 2025-09-27 10:46:17 +00:00
提交
This commit is contained in:
PC-20230316NUNE\Administrator
@@ -0,0 +1,8 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 34971619b0c1f46b4b29e418d346d483
|
||||
folderAsset: yes
|
||||
DefaultImporter:
|
||||
externalObjects: {}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,131 @@
|
||||
|
||||
// 定义BspTree的Unity可视化扩展
|
||||
|
||||
//#define SHOW_BSP_TREE_GIZOMS
|
||||
#if UNITY_EDITOR // && SHOW_BSP_TREE_GIZOMS
|
||||
|
||||
using System.Collections.Generic;
|
||||
using JNGame.Math;
|
||||
using UnityEngine;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
// BspTreeNode的调试信息
|
||||
public partial class BspTreeNode
|
||||
{
|
||||
public static int s_AutoIncretID = 0;
|
||||
|
||||
public int debugNodeId;
|
||||
public BspTreeNode()
|
||||
{
|
||||
debugNodeId = ++s_AutoIncretID;
|
||||
}
|
||||
}
|
||||
|
||||
// BspTree的可视化绘制
|
||||
public partial class BspTree
|
||||
{
|
||||
#region Debug Gizoms
|
||||
public static Transform s_DebugTrans;
|
||||
public static Material s_DebugMat;
|
||||
|
||||
public void DebugDraw()
|
||||
{
|
||||
var tran = DrawNode(m_RootNode, 0, s_DebugTrans);
|
||||
tran.transform.position += Vector3.up * 0.05f;
|
||||
}
|
||||
|
||||
private Vector3 Hash13(int id)
|
||||
{
|
||||
var val = (Mathf.Sin(15741.254f * id) + 1) / 2;
|
||||
var val2 = (Mathf.Sin(7331.5147f * id) + 1) / 2;
|
||||
var val3 = (Mathf.Sin(24317.433f * id) + 1) / 2;
|
||||
return new Vector3(val, val2, val3);
|
||||
}
|
||||
|
||||
private Transform DrawNode(BspTreeNode node, int depth, Transform parent)
|
||||
{
|
||||
var val = Hash13(node.debugNodeId);
|
||||
Color color = new Color(val.x, val.y, val.z, 1);
|
||||
if (node.IsLeaf)
|
||||
{
|
||||
var tran = CreateGo(node.debugNodeId.ToString(), node.tris, color).transform;
|
||||
tran.SetParent(parent, true);
|
||||
return tran;
|
||||
}
|
||||
else
|
||||
{
|
||||
var tran = CreateGo(node.debugNodeId.ToString(), node.splitLine, color).transform;
|
||||
tran.SetParent(parent, true);
|
||||
|
||||
DrawNode(node.LeftChild, depth + 1, tran);
|
||||
DrawNode(node.RightChild, depth + 1, tran);
|
||||
return tran;
|
||||
}
|
||||
}
|
||||
|
||||
private GameObject CreateGo(string name, in SplitLine plane, in Color color)
|
||||
{
|
||||
return new GameObject(name);
|
||||
#if false
|
||||
var dir = plane.Direction.Normalized;
|
||||
var or = plane.vertexA;
|
||||
var s = or - dir * worldSize;
|
||||
var e = or + dir * worldSize;
|
||||
var perp = new LVector2(-dir.y, dir.x);
|
||||
const float lineSize = 0.3f;
|
||||
var s1 = perp * lineSize.ToLFloat() + s;
|
||||
var e1 = perp * lineSize.ToLFloat() + e;
|
||||
List<Vector3> vertices = new List<Vector3>();
|
||||
List<int> tirs = new List<int>();
|
||||
vertices.Add(s.ToVector3XZ());
|
||||
vertices.Add(e.ToVector3XZ());
|
||||
vertices.Add(e1.ToVector3XZ());
|
||||
vertices.Add(s1.ToVector3XZ());
|
||||
int triIdx = 0;
|
||||
tirs.Add(0);
|
||||
tirs.Add(1);
|
||||
tirs.Add(2);
|
||||
tirs.Add(0);
|
||||
tirs.Add(2);
|
||||
tirs.Add(3);
|
||||
return CreateGo(name, vertices, tirs, color);
|
||||
#endif
|
||||
}
|
||||
|
||||
private GameObject CreateGo(string name, List<TriangleRef> allRawTriangle, in Color color)
|
||||
{
|
||||
List<Vector3> vertices = new List<Vector3>();
|
||||
List<int> tirs = new List<int>();
|
||||
int triIdx = 0;
|
||||
foreach (var tri in allRawTriangle)
|
||||
{
|
||||
vertices.Add(tri.vertexA.ToVector3XZ());
|
||||
vertices.Add(tri.vertexB.ToVector3XZ());
|
||||
vertices.Add(tri.vertexC.ToVector3XZ());
|
||||
tirs.Add(triIdx++);
|
||||
tirs.Add(triIdx++);
|
||||
tirs.Add(triIdx++);
|
||||
}
|
||||
|
||||
return CreateGo(name, vertices, tirs, color);
|
||||
}
|
||||
|
||||
private GameObject CreateGo(string name, List<Vector3> vertices, List<int> tirs, in Color color)
|
||||
{
|
||||
var go = new GameObject(name);
|
||||
var render = go.AddComponent<MeshRenderer>();
|
||||
var mat = new Material(s_DebugMat) { color = color };
|
||||
render.material = mat;
|
||||
|
||||
var tempMesh = new Mesh();
|
||||
tempMesh.vertices = vertices.ToArray();
|
||||
tempMesh.triangles = tirs.ToArray();
|
||||
go.AddComponent<MeshFilter>().mesh = tempMesh;
|
||||
return go;
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: ba3cb62d3cc64ae4a94be3cbd1bf6ef8
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,63 @@
|
||||
using System.Collections.Generic;
|
||||
using JNGame.Math;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 二叉空间分割树
|
||||
/// </summary>
|
||||
public partial class BspTree
|
||||
{
|
||||
private readonly List<TriangleRef> m_AllTriangle = new List<TriangleRef>();
|
||||
|
||||
/// <summary>
|
||||
/// 根节点
|
||||
/// </summary>
|
||||
private BspTreeNode m_RootNode;
|
||||
|
||||
/// <summary>
|
||||
/// 查找点邻接三角形的缓存，每次查找都会清理，外部不可缓存此列表，只可一次性使用
|
||||
/// </summary>
|
||||
private readonly List<int> m_TempNeareastTrianges = new List<int>(16);
|
||||
|
||||
/// <summary>
|
||||
/// 构造二叉空间分割树
|
||||
/// </summary>
|
||||
/// <param name="rawTriangles">原始的三角形数据</param>
|
||||
public void Init(List<NavTriangle> rawTriangles)
|
||||
{
|
||||
foreach (var tri in rawTriangles)
|
||||
{
|
||||
m_AllTriangle.Add(new TriangleRef(tri));
|
||||
}
|
||||
|
||||
m_RootNode = new BspTreeNode();
|
||||
m_RootNode.Init(m_AllTriangle);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据位置检索BSP树，快速定位点所在三角形区域
|
||||
/// </summary>
|
||||
/// <param name="pos"></param>
|
||||
/// <param name="nearTriangleIndices"></param>
|
||||
/// <returns></returns>
|
||||
public int GetTriangle(in LVector3 pos, out List<int> nearTriangleIndices)
|
||||
{
|
||||
m_TempNeareastTrianges.Clear();
|
||||
var pos2d = pos.ToLVector2XZ();
|
||||
int triangleIndex = m_RootNode.GetTriangle(pos2d, out _, out List<TriangleRef> nearTriangles);
|
||||
if (triangleIndex == -1)
|
||||
{ // 未找到落位的三角形，说明在阻挡内，获取阻挡周边的一些三角形
|
||||
for (int i = 0; i < nearTriangles.Count; ++i)
|
||||
{
|
||||
if (!m_TempNeareastTrianges.Contains(nearTriangles[i].index))
|
||||
{
|
||||
m_TempNeareastTrianges.Add(nearTriangles[i].index);
|
||||
}
|
||||
}
|
||||
}
|
||||
nearTriangleIndices = m_TempNeareastTrianges;
|
||||
return triangleIndex;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 477985406605c4a0c9d9718247233d1d
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,315 @@
|
||||
|
||||
using System.Collections.Generic;
|
||||
using JNGame.Math;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 二叉空间分割树节点
|
||||
/// </summary>
|
||||
public partial class BspTreeNode
|
||||
{
|
||||
/// <summary>
|
||||
/// 划分到三角形数小于此则停止，可以性能测试下，选择一个合适的值
|
||||
/// </summary>
|
||||
private const int kMinTriangleCount = 3;
|
||||
|
||||
/// <summary>
|
||||
/// 找到树叶节点，向上取三层，获取包含的三角形，用于查找落位阻挡点的临近三角形
|
||||
/// </summary>
|
||||
private const int kNearestDeepSearch = 3;
|
||||
|
||||
/// <summary>
|
||||
/// 当前节点中包含的三角形数据
|
||||
/// </summary>
|
||||
public List<TriangleRef> tris;
|
||||
/// <summary>
|
||||
/// 当前节点的子节点
|
||||
/// </summary>
|
||||
private BspTreeNode[] m_Children;
|
||||
/// <summary>
|
||||
/// 当前节点的树深
|
||||
/// </summary>
|
||||
public int depth = 0;
|
||||
/// <summary>
|
||||
/// 节点的切割线
|
||||
/// </summary>
|
||||
public SplitLine splitLine;
|
||||
/// <summary>
|
||||
/// 是否是叶子节点
|
||||
/// </summary>
|
||||
public bool IsLeaf => m_Children == null;
|
||||
/// <summary>
|
||||
/// 左子树
|
||||
/// </summary>
|
||||
public BspTreeNode LeftChild => m_Children?[0];
|
||||
/// <summary>
|
||||
/// 右子树
|
||||
/// </summary>
|
||||
public BspTreeNode RightChild => m_Children?[1];
|
||||
|
||||
/// <summary>
|
||||
/// 获取指定坐标所在的三角形索引
|
||||
/// </summary>
|
||||
/// <param name="pos"></param>
|
||||
/// <returns></returns>
|
||||
public int GetTriangle(in LVector2 pos, out int searchDeep, out List<TriangleRef> triangeGroup)
|
||||
{
|
||||
if (IsLeaf)
|
||||
{
|
||||
searchDeep = -1;
|
||||
triangeGroup = null;
|
||||
foreach (var tri in tris)
|
||||
{
|
||||
if (tri.Contain(pos))
|
||||
{
|
||||
return tri.index;
|
||||
}
|
||||
}
|
||||
searchDeep = kNearestDeepSearch;
|
||||
return -1;
|
||||
}
|
||||
else
|
||||
{
|
||||
var isLeft = LVector2.Cross(splitLine.vertexB - splitLine.vertexA, pos - splitLine.vertexA) >= 0;
|
||||
if (isLeft)
|
||||
{
|
||||
int targetIndex = LeftChild.GetTriangle(pos, out searchDeep, out triangeGroup);
|
||||
if (targetIndex == -1)
|
||||
{
|
||||
--searchDeep;
|
||||
if (searchDeep == 0)
|
||||
{
|
||||
triangeGroup = tris;
|
||||
}
|
||||
}
|
||||
return targetIndex;
|
||||
}
|
||||
else
|
||||
{
|
||||
int targetIndex = RightChild.GetTriangle(pos, out searchDeep, out triangeGroup);
|
||||
if (targetIndex == -1)
|
||||
{
|
||||
--searchDeep;
|
||||
if (searchDeep == 0)
|
||||
{
|
||||
triangeGroup = tris;
|
||||
}
|
||||
}
|
||||
return targetIndex;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 初始化二叉空间树的节点
|
||||
/// </summary>
|
||||
/// <param name="tris"></param>
|
||||
/// <param name="depth"></param>
|
||||
public void Init(List<TriangleRef> tris, int depth = 0)
|
||||
{
|
||||
this.depth = depth;
|
||||
this.tris = tris;
|
||||
|
||||
if (tris.Count <= kMinTriangleCount)
|
||||
{ // 三角形数少于指标，停止
|
||||
return;
|
||||
}
|
||||
if (depth > 20)
|
||||
{ // 树深超过20层，停止
|
||||
return;
|
||||
}
|
||||
// 选择分割线
|
||||
splitLine = PickSplitLine(tris);
|
||||
m_Children = new BspTreeNode[] { new BspTreeNode(), new BspTreeNode() };
|
||||
var lTris = new List<TriangleRef>();
|
||||
var rTris = new List<TriangleRef>();
|
||||
foreach (var tri in tris)
|
||||
{
|
||||
var val = SplitLine.GetSplitResult(in splitLine, tri);
|
||||
if (val == EnumSplitType.Left)
|
||||
{ // 三角形位于切割线左侧，加入左子树三角形列表
|
||||
lTris.Add(tri);
|
||||
}
|
||||
else if (val == EnumSplitType.Right)
|
||||
{ // 三角形位于切割线右侧，加入右子树三角形列表
|
||||
rTris.Add(tri);
|
||||
}
|
||||
else
|
||||
{ // 切割线经过三角形，将三角形分为几个多边形
|
||||
SplitTriangle(lTris, rTris, tri);
|
||||
}
|
||||
}
|
||||
|
||||
LeftChild.Init(lTris, depth + 1);
|
||||
RightChild.Init(rTris, depth + 1);
|
||||
}
|
||||
|
||||
private readonly List<LVector2> m_RightVerts = new List<LVector2>();
|
||||
private readonly List<LVector2> m_LeftVerts = new List<LVector2>();
|
||||
|
||||
/// <summary>
|
||||
/// 将三角形按切割线切割为若干个三角形
|
||||
/// </summary>
|
||||
/// <param name="lTris"></param>
|
||||
/// <param name="rTris"></param>
|
||||
/// <param name="tri"></param>
|
||||
private void SplitTriangle(List<TriangleRef> lTris, List<TriangleRef> rTris, TriangleRef tri)
|
||||
{
|
||||
int numVerts = 3;
|
||||
m_RightVerts.Clear();
|
||||
m_LeftVerts.Clear();
|
||||
var a = tri[2];
|
||||
var aSide = SplitLine.ClassifyPointToPlane(splitLine, a);
|
||||
for (int i = 0; i < numVerts; i++)
|
||||
{
|
||||
var b = tri[i];
|
||||
var bSide = SplitLine.ClassifyPointToPlane(splitLine, b);
|
||||
if (bSide == EnumSplitType.Right)
|
||||
{
|
||||
if (aSide == EnumSplitType.Left)
|
||||
{
|
||||
var p = SplitLine.GetIntersectPoint(a, b, splitLine.vertexA, splitLine.vertexB);
|
||||
m_RightVerts.Add(p);
|
||||
m_LeftVerts.Add(p);
|
||||
}
|
||||
else if (aSide == EnumSplitType.OnPlane)
|
||||
{
|
||||
if (m_RightVerts.Count == 0 || a != m_RightVerts[m_RightVerts.Count - 1])
|
||||
{
|
||||
m_RightVerts.Add(a);
|
||||
}
|
||||
}
|
||||
|
||||
m_RightVerts.Add(b);
|
||||
}
|
||||
else if (bSide == EnumSplitType.Left)
|
||||
{
|
||||
if (aSide == EnumSplitType.Right)
|
||||
{
|
||||
var p = SplitLine.GetIntersectPoint(a, b, splitLine.vertexA, splitLine.vertexB);
|
||||
m_RightVerts.Add(p);
|
||||
m_LeftVerts.Add(p);
|
||||
}
|
||||
else if (aSide == EnumSplitType.OnPlane)
|
||||
{
|
||||
if (m_LeftVerts.Count == 0 || a != m_LeftVerts[m_LeftVerts.Count - 1])
|
||||
{
|
||||
m_LeftVerts.Add(a);
|
||||
}
|
||||
}
|
||||
|
||||
m_LeftVerts.Add(b);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (aSide == EnumSplitType.Right)
|
||||
{
|
||||
if (!(m_RightVerts.Count == 3 && b == m_RightVerts[0]))
|
||||
{
|
||||
m_RightVerts.Add(b);
|
||||
}
|
||||
}
|
||||
else if (aSide == EnumSplitType.Left)
|
||||
{
|
||||
if (!(m_LeftVerts.Count == 3 && b == m_LeftVerts[0]))
|
||||
{
|
||||
m_LeftVerts.Add(b);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
a = b;
|
||||
aSide = bSide;
|
||||
}
|
||||
|
||||
//push into tri list
|
||||
if (m_LeftVerts.Count >= 3)
|
||||
{
|
||||
if (m_LeftVerts.Count == 3)
|
||||
{
|
||||
AddTriangle(lTris, m_LeftVerts[0], m_LeftVerts[1], m_LeftVerts[2], tri);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Debug.Assert(m_LeftVerts.Count == 4);
|
||||
AddTriangle(lTris, m_LeftVerts[0], m_LeftVerts[1], m_LeftVerts[2], tri);
|
||||
AddTriangle(lTris, m_LeftVerts[0], m_LeftVerts[2], m_LeftVerts[3], tri);
|
||||
}
|
||||
}
|
||||
|
||||
if (m_RightVerts.Count >= 3)
|
||||
{
|
||||
if (m_RightVerts.Count == 3)
|
||||
{
|
||||
AddTriangle(rTris, m_RightVerts[0], m_RightVerts[1], m_RightVerts[2], tri);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Debug.Assert(m_RightVerts.Count == 4);
|
||||
AddTriangle(rTris, m_RightVerts[0], m_RightVerts[1], m_RightVerts[2], tri);
|
||||
AddTriangle(rTris, m_RightVerts[0], m_RightVerts[2], m_RightVerts[3], tri);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private void AddTriangle(List<TriangleRef> triangles, in LVector2 vertexA, in LVector2 vertexB, in LVector2 vertexC, TriangleRef tri)
|
||||
{
|
||||
if (vertexA == vertexB || vertexB == vertexC || vertexC == vertexA) { return; }
|
||||
triangles.Add(new TriangleRef(vertexA, vertexB, vertexC, tri));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 选择一条合适的切割线
|
||||
/// </summary>
|
||||
/// <param name="tris">待切割的三角形集合</param>
|
||||
/// <returns></returns>
|
||||
private static SplitLine PickSplitLine(List<TriangleRef> tris)
|
||||
{
|
||||
int minScore = int.MinValue;
|
||||
int minLCount = 0;
|
||||
int minRCount = 0;
|
||||
SplitLine bestLine = new SplitLine();
|
||||
int[] splitCounter = new int[(int)EnumSplitType.EnumCount]; // 每次切割，三角形位于切割线左右和线上的次数统计
|
||||
var tirCount = tris.Count;
|
||||
foreach (var tri in tris)
|
||||
{ // 遍历所有三角形的边框线，作为切割线来切割空间，计算每次切割的分数，从而选取最优的分割线
|
||||
foreach (var borderLine in tri.borders)
|
||||
{
|
||||
for (int i = 0; i < (int)EnumSplitType.EnumCount; i++)
|
||||
{
|
||||
splitCounter[i] = 0;
|
||||
}
|
||||
foreach (var otherTri in tris)
|
||||
{
|
||||
var val = (int)SplitLine.GetSplitResult(borderLine, otherTri);
|
||||
splitCounter[val]++;
|
||||
}
|
||||
|
||||
// Clock wise =>Left ++ CCW=>Right++// self tri is on the left
|
||||
var leftCount = splitCounter[(int)EnumSplitType.Left];
|
||||
var rightCount = splitCounter[(int)EnumSplitType.Right];
|
||||
var splitCount = splitCounter[(int)EnumSplitType.OnPlane];
|
||||
if ((leftCount == 0 || rightCount == 0) && (leftCount + rightCount) == tirCount)
|
||||
{
|
||||
// 所有的三角形都在同一侧，忽略
|
||||
continue;
|
||||
}
|
||||
// 根据切割线左右的三角形数，以及切割的三角形数，计算本次分割的得分
|
||||
var balanceVal = LMath.Abs(leftCount - rightCount);
|
||||
var sameVal = LMath.Min(leftCount, rightCount);
|
||||
int score = sameVal * 3 - balanceVal - splitCount * 2;
|
||||
if (score > minScore)
|
||||
{ // 选取得分最高的作为切割线
|
||||
minLCount = leftCount;
|
||||
minRCount = rightCount;
|
||||
minScore = score;
|
||||
bestLine = borderLine;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return bestLine;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 133bf2d5724b64ac0945954588c88155
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,10 @@
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
public enum EnumSplitType
|
||||
{
|
||||
Left,
|
||||
Right,
|
||||
OnPlane,
|
||||
EnumCount
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: b9b5e74642ed142cd83c3453ce2d5894
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,115 @@
|
||||
using JNGame.Math;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 二叉空间树的切割线
|
||||
/// </summary>
|
||||
public struct SplitLine
|
||||
{
|
||||
/// <summary>
|
||||
/// 切割线起点
|
||||
/// </summary>
|
||||
public LVector2 vertexA;
|
||||
/// <summary>
|
||||
/// 切割线终点
|
||||
/// </summary>
|
||||
public LVector2 vertexB;
|
||||
/// <summary>
|
||||
/// 切割线方向
|
||||
/// </summary>
|
||||
public readonly LVector2 Direction => vertexB - vertexA;
|
||||
|
||||
/// <summary>
|
||||
/// 构造切割线
|
||||
/// </summary>
|
||||
/// <param name="vertexA"></param>
|
||||
/// <param name="vertexB"></param>
|
||||
public SplitLine(in LVector2 vertexA, in LVector2 vertexB)
|
||||
{
|
||||
this.vertexA = vertexA;
|
||||
this.vertexB = vertexB;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取三角形的切割结果
|
||||
/// </summary>
|
||||
/// <param name="line">切割线</param>
|
||||
/// <param name="tri">切割三角形</param>
|
||||
/// <returns>切割线左侧、右侧或经过切割线</returns>
|
||||
public static EnumSplitType GetSplitResult(in SplitLine line, TriangleRef tri)
|
||||
{
|
||||
var lineDir = line.Direction;
|
||||
// 三角形三个顶点与切割线的位置关系
|
||||
var valA = LVector2.Cross(lineDir, tri.vertexA - line.vertexA);
|
||||
var valB = LVector2.Cross(lineDir, tri.vertexB - line.vertexA);
|
||||
var valC = LVector2.Cross(lineDir, tri.vertexC - line.vertexA);
|
||||
|
||||
var isRight = false;
|
||||
if (valA != 0) isRight = valA < 0;
|
||||
if (valB != 0) isRight = valB < 0;
|
||||
if (valC != 0) isRight = valC < 0;
|
||||
|
||||
var isA = valA <= 0;
|
||||
var isB = valB <= 0;
|
||||
var isC = valC <= 0;
|
||||
if (isA == isB && isB == isC)
|
||||
{
|
||||
return isRight ? EnumSplitType.Right : EnumSplitType.Left;
|
||||
}
|
||||
|
||||
isA = valA >= 0;
|
||||
isB = valB >= 0;
|
||||
isC = valC >= 0;
|
||||
if (isA == isB && isB == isC)
|
||||
{
|
||||
return isRight ? EnumSplitType.Right : EnumSplitType.Left;
|
||||
}
|
||||
|
||||
return EnumSplitType.OnPlane;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取顶点和切割线的位置关系
|
||||
/// </summary>
|
||||
/// <param name="line">切割线</param>
|
||||
/// <param name="vertex">顶点</param>
|
||||
/// <returns></returns>
|
||||
public static EnumSplitType ClassifyPointToPlane(in SplitLine line, in LVector2 vertex)
|
||||
{
|
||||
var val = LVector2.Cross(line.Direction, vertex - line.vertexA);
|
||||
if (val == 0)
|
||||
{
|
||||
return EnumSplitType.OnPlane;
|
||||
}
|
||||
else
|
||||
{
|
||||
return val < 0 ? EnumSplitType.Right : EnumSplitType.Left;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算p0p1,p2p3的交点
|
||||
/// </summary>
|
||||
/// <param name="p0"></param>
|
||||
/// <param name="p1"></param>
|
||||
/// <param name="p2"></param>
|
||||
/// <param name="p3"></param>
|
||||
/// <returns></returns>
|
||||
public static LVector2 GetIntersectPoint(in LVector2 p0, in LVector2 p1, in LVector2 p2, in LVector2 p3)
|
||||
{
|
||||
var diff = p2 - p0;
|
||||
var d1 = p1 - p0;
|
||||
var d2 = p3 - p2;
|
||||
var demo = LMath.Cross(d1, d2); //det
|
||||
if (LMath.Abs(demo) < LFloat.EPSILON)
|
||||
{
|
||||
// 叉积结果为0，说明两个向量平行
|
||||
return p0;
|
||||
}
|
||||
|
||||
var t1 = LMath.Cross(diff, d2) / demo; // Cross2D(diff,-d2)
|
||||
return p0 + (p1 - p0) * t1;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 6a160827a7e254845beb3b10a0644f26
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,137 @@
|
||||
using System;
|
||||
// using JNGame.Collision2D;
|
||||
using JNGame.Math;
|
||||
// using Debug = JNGame.Logging.Debug;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 导航三角形在二叉空间分割树中的引用
|
||||
/// <para>一个导航三角形可能会被树分割为多个三角形引用</para>
|
||||
/// </summary>
|
||||
public class TriangleRef
|
||||
{
|
||||
/// <summary>
|
||||
/// 当前三角形引用关联的导航三角形索引
|
||||
/// </summary>
|
||||
public int index;
|
||||
/// <summary>
|
||||
/// 三角形顶点A
|
||||
/// </summary>
|
||||
public LVector2 vertexA;
|
||||
/// <summary>
|
||||
/// 三角形顶点B
|
||||
/// </summary>
|
||||
public LVector2 vertexB;
|
||||
/// <summary>
|
||||
/// 三角形顶点C
|
||||
/// </summary>
|
||||
public LVector2 vertexC;
|
||||
|
||||
/// <summary>
|
||||
/// 是否是分割的三角形
|
||||
/// </summary>
|
||||
public bool IsSplit { get; private set; } = false;
|
||||
/// <summary>
|
||||
/// 三角形的边框
|
||||
/// </summary>
|
||||
public SplitLine[] borders;
|
||||
|
||||
/// <summary>
|
||||
/// 根据下标获取三角形的顶点，合法下标0/1/2
|
||||
/// </summary>
|
||||
/// <param name="index"></param>
|
||||
/// <returns></returns>
|
||||
/// <exception cref="IndexOutOfRangeException"></exception>
|
||||
public LVector2 this[int index]
|
||||
{
|
||||
get
|
||||
{
|
||||
return index switch
|
||||
{
|
||||
0 => vertexA,
|
||||
1 => vertexB,
|
||||
2 => vertexC,
|
||||
_ => throw new IndexOutOfRangeException("vector idx invalid" + index),
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据原始导航三角形创建引用
|
||||
/// <para>由此构造的三角形引用认为是未切割过的三角形</para>
|
||||
/// </summary>
|
||||
/// <param name="srcTriangle">原始的导航三角形</param>
|
||||
public TriangleRef(NavTriangle srcTriangle)
|
||||
: this(srcTriangle.vertexA.ToLVector2XZ(), srcTriangle.vertexB.ToLVector2XZ(), srcTriangle.vertexC.ToLVector2XZ(), srcTriangle.index)
|
||||
{ }
|
||||
|
||||
/// <summary>
|
||||
/// 根据分割的三个顶点和原始导航三角形创建引用
|
||||
/// <para>由此构造的三角形引用认为是切割过的三角形</para>
|
||||
/// </summary>
|
||||
/// <param name="vertexA">三角形顶点A</param>
|
||||
/// <param name="vertexB">三角形顶点B</param>
|
||||
/// <param name="vertexC">三角形顶点C</param>
|
||||
/// <param name="srcTriangle">原始的导航三角形</param>
|
||||
public TriangleRef(in LVector2 vertexA, in LVector2 vertexB, in LVector2 vertexC, TriangleRef srcTriangle)
|
||||
: this(vertexA, vertexB, vertexC, srcTriangle.index)
|
||||
{
|
||||
IsSplit = true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 私有构造
|
||||
/// </summary>
|
||||
/// <param name="vertexA">三角形顶点A</param>
|
||||
/// <param name="vertexB">三角形顶点B</param>
|
||||
/// <param name="vertexC">三角形顶点C</param>
|
||||
/// <param name="idx">引用的导航三角形索引</param>
|
||||
private TriangleRef(in LVector2 vertexA, in LVector2 vertexB, in LVector2 vertexC, int idx)
|
||||
{
|
||||
this.vertexA = vertexA;
|
||||
this.vertexB = vertexB;
|
||||
this.vertexC = vertexC;
|
||||
index = idx;
|
||||
borders = new SplitLine[]
|
||||
{
|
||||
new SplitLine(vertexA, vertexB),
|
||||
new SplitLine(vertexB, vertexC),
|
||||
new SplitLine(vertexC, vertexA)
|
||||
};
|
||||
//check valid
|
||||
CheckValid();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 三角形三点不可重合
|
||||
/// </summary>
|
||||
private void CheckValid()
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
if (borders[i].Direction == LVector2.Zero)
|
||||
{
|
||||
// Debug.Assert(false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断点是否在此三角形内
|
||||
/// </summary>
|
||||
/// <param name="pos"></param>
|
||||
/// <returns></returns>
|
||||
public bool Contain(in LVector2 pos)
|
||||
{
|
||||
// ABC三个顶点绕序为顺时针，三个叉积都需要为负，则点在三角形内
|
||||
var isRightA = LVector2.Cross(vertexB - vertexA, pos - vertexA) > 0;
|
||||
if (isRightA) return false;
|
||||
var isRightB = LVector2.Cross(vertexC - vertexB, pos - vertexB) > 0;
|
||||
if (isRightB) return false;
|
||||
var isRightC = LVector2.Cross(vertexA - vertexC, pos - vertexC) > 0;
|
||||
if (isRightC) return false;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 63961ae30f8cb4ca7a519fa7a7cdc394
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,83 @@
|
||||
|
||||
using UnityEngine;
|
||||
using JNGame.Math;
|
||||
|
||||
namespace JNGame.Map
|
||||
{
|
||||
/// <summary>
|
||||
/// 地图数据
|
||||
/// </summary>
|
||||
[System.Serializable]
|
||||
public class StaticMapData
|
||||
{
|
||||
/// <summary>
|
||||
/// 地图ID
|
||||
/// </summary>
|
||||
public int mapID;
|
||||
/// <summary>
|
||||
/// 地图名
|
||||
/// </summary>
|
||||
public string mapName;
|
||||
/// <summary>
|
||||
/// 地图导航网格数据
|
||||
/// </summary>
|
||||
public PathFinding.NavMeshData navMeshData;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 生成地图配置逻辑数据的脚步，仅作为编辑器导出地图场景数据的入口，运行时无作用
|
||||
/// </summary>
|
||||
public class GenMapConfigHelper : MonoBehaviour
|
||||
{
|
||||
/// <summary>
|
||||
/// 地图ID
|
||||
/// </summary>
|
||||
public int mapID;
|
||||
/// <summary>
|
||||
/// 地图名
|
||||
/// </summary>
|
||||
public string mapName;
|
||||
/// <summary>
|
||||
/// Unity的Navigation组件
|
||||
/// </summary>
|
||||
public Unity.AI.Navigation.NavMeshSurface navMeshSurface;
|
||||
|
||||
|
||||
private void OnDrawGizmos()
|
||||
{
|
||||
// if (m_MapConfig == null) { return; }
|
||||
// DrawNavMesh();
|
||||
}
|
||||
|
||||
// private void DrawNavMesh()
|
||||
// {
|
||||
// if (m_MapConfig == null || m_MapConfig.navMeshData == null || m_MapConfig.navMeshData.pathTriangles == null) { return; }
|
||||
// int i = 0;
|
||||
// Gizmos.color = Color.green;
|
||||
// while (i < m_MapConfig.navMeshData.pathTriangles.Length)
|
||||
// {
|
||||
// int aIndex = m_MapConfig.navMeshData.pathTriangles[i++];
|
||||
// int bIndex = m_MapConfig.navMeshData.pathTriangles[i++];
|
||||
// int cIndex = m_MapConfig.navMeshData.pathTriangles[i++];
|
||||
|
||||
// Vector3 vertexA = m_MapConfig.navMeshData.pathVertices[aIndex].ToVector3();
|
||||
// Vector3 vertexB = m_MapConfig.navMeshData.pathVertices[bIndex].ToVector3();
|
||||
// Vector3 vertexC = m_MapConfig.navMeshData.pathVertices[cIndex].ToVector3();
|
||||
|
||||
// Gizmos.DrawLine(vertexA, vertexB);
|
||||
// Gizmos.DrawLine(vertexB, vertexC);
|
||||
// Gizmos.DrawLine(vertexC, vertexA);
|
||||
// }
|
||||
|
||||
// Gizmos.color = Color.blue;
|
||||
// Vector3 bottomLeft = new LVector3(m_MapConfig.navMeshData.xMin, 0.ToLFloat(), m_MapConfig.navMeshData.zMin).ToVector3();
|
||||
// Vector3 bottomRight = new LVector3(m_MapConfig.navMeshData.xMax, 0.ToLFloat(), m_MapConfig.navMeshData.zMin).ToVector3();
|
||||
// Vector3 topLeft = new LVector3(m_MapConfig.navMeshData.xMin, 0.ToLFloat(), m_MapConfig.navMeshData.zMax).ToVector3();
|
||||
// Vector3 topRight = new LVector3(m_MapConfig.navMeshData.xMax, 0.ToLFloat(), m_MapConfig.navMeshData.zMax).ToVector3();
|
||||
// Gizmos.DrawLine(bottomLeft, bottomRight);
|
||||
// Gizmos.DrawLine(bottomLeft, topLeft);
|
||||
// Gizmos.DrawLine(bottomRight, topRight);
|
||||
// Gizmos.DrawLine(topLeft, topRight);
|
||||
// }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,3 @@
|
||||
fileFormatVersion: 2
|
||||
guid: c7bfc1bbc6a14b83a73b6c061d4f8227
|
||||
timeCreated: 1715151863
|
||||
@@ -0,0 +1,8 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 7f7bde9205575458d94afea935c783d8
|
||||
folderAsset: yes
|
||||
DefaultImporter:
|
||||
externalObjects: {}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,8 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 6fa8733a350b745f291097ea3eb01303
|
||||
folderAsset: yes
|
||||
DefaultImporter:
|
||||
externalObjects: {}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,242 @@
|
||||
using JNGame.Math;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
public static class GeometryUtil
|
||||
{
|
||||
/// <summary>
|
||||
/// Projects a point to a line segment.
|
||||
/// <para>This implementation is thread-safe.</para>
|
||||
/// </summary>
|
||||
/// <param name="nearest">输出：点point在线段的投影点</param>
|
||||
/// <param name="start">线段起点</param>
|
||||
/// <param name="end">线段终点</param>
|
||||
/// <param name="point">被投影的原始点</param>
|
||||
/// <returns>投影点与原始点的距离平方</returns>
|
||||
public static LFloat NearestSegmentPointSquareDistance(ref LVector3 nearest, in LVector3 start, in LVector3 end, in LVector3 point)
|
||||
{
|
||||
nearest = start;
|
||||
// 线段AB
|
||||
var abX = end.x - start.x;
|
||||
var abY = end.y - start.y;
|
||||
var abZ = end.z - start.z;
|
||||
// 线段AB的长度平方
|
||||
var abLen2 = abX * abX + abY * abY + abZ * abZ;
|
||||
if (abLen2 > 0)
|
||||
{ // Avoid NaN due to the indeterminate form 0/0
|
||||
// 投影点在线段上的比例位置
|
||||
var t = ((point.x - start.x) * abX + (point.y - start.y) * abY + (point.z - start.z) * abZ) / abLen2;
|
||||
var s = LMath.Clamp01(t);
|
||||
nearest.x += abX * s;
|
||||
nearest.y += abY * s;
|
||||
nearest.z += abZ * s;
|
||||
}
|
||||
return LVector3.Distance2(nearest, point);
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// Find the closest point on the triangle, given a measure point.
|
||||
/// This is the optimized algorithm taken from the book "Real-Time Collision Detection".
|
||||
/// <para>This implementation is NOT thread-safe.</para>
|
||||
/// </summary>
|
||||
/// <param name="vertexA"></param>
|
||||
/// <param name="vertexB"></param>
|
||||
/// <param name="vertexC"></param>
|
||||
/// <param name="p"></param>
|
||||
/// <param name="_out"></param>
|
||||
/// <returns></returns>
|
||||
public static LFloat GetClosestPointOnTriangle(in LVector3 vertexA, in LVector3 vertexB, in LVector3 vertexC, in LVector3 p, ref LVector3 _out)
|
||||
{
|
||||
// Check if P in vertex region outside A
|
||||
var ab = vertexB - vertexA;
|
||||
var ac = vertexC - vertexA;
|
||||
var ap = p - vertexA;
|
||||
var d1 = LVector3.Dot(ab, ap);
|
||||
var d2 = LVector3.Dot(ac, ap);
|
||||
if (d1 <= 0 && d2 <= 0)
|
||||
{
|
||||
_out = vertexA;
|
||||
return LVector3.Distance2(p, vertexA);
|
||||
}
|
||||
|
||||
// Check if P in vertex region outside B
|
||||
var bp = p - vertexB;
|
||||
var d3 = LVector3.Dot(ab, bp);
|
||||
var d4 = LVector3.Dot(ac, bp);
|
||||
if (d3 >= 0 && d4 <= d3)
|
||||
{
|
||||
_out = vertexB;
|
||||
return LVector3.Distance2(p, vertexB);
|
||||
}
|
||||
|
||||
// Check if P in edge region of AB, if so return projection of P onto AB
|
||||
var vc = d1 * d4 - d3 * d2;
|
||||
if (vc <= 0 && d1 >= 0 && d3 <= 0)
|
||||
{
|
||||
var v = d1 / (d1 - d3);
|
||||
_out = vertexA + ab * v; // barycentric coordinates (1-v,v,0)
|
||||
return LVector3.Distance2(p, _out);
|
||||
}
|
||||
|
||||
// Check if P in vertex region outside C
|
||||
var cp = p - vertexC;
|
||||
var d5 = LVector3.Dot(ab, cp);
|
||||
var d6 = LVector3.Dot(ac, cp);
|
||||
if (d6 >= 0 && d5 <= d6)
|
||||
{
|
||||
_out = vertexC;
|
||||
return LVector3.Distance2(p, vertexC);
|
||||
}
|
||||
|
||||
// Check if P in edge region of AC, if so return projection of P onto AC
|
||||
var vb = d5 * d2 - d1 * d6;
|
||||
if (vb <= 0 && d2 >= 0 && d6 <= 0)
|
||||
{
|
||||
var w = d2 / (d2 - d6);
|
||||
_out = vertexA + ac * w; // barycentric coordinates (1-w,0,w)
|
||||
return LVector3.Distance2(_out, p);
|
||||
}
|
||||
|
||||
// Check if P in edge region of BC, if so return projection of P onto BC
|
||||
var va = d3 * d6 - d5 * d4;
|
||||
if (va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0)
|
||||
{
|
||||
var w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
|
||||
_out = vertexB + (vertexC - vertexB) * w; // barycentric coordinates (0,1-w,w)
|
||||
return LVector3.Distance2(_out, p);
|
||||
}
|
||||
|
||||
// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
|
||||
var denom = 1 / (va + vb + vc);
|
||||
{
|
||||
LFloat v = vb * denom;
|
||||
LFloat w = vc * denom;
|
||||
_out = vertexA + ab * v + ac * w;
|
||||
}
|
||||
return LVector3.Distance2(_out, p);
|
||||
}
|
||||
|
||||
|
||||
public static bool IntersectRayTriangle(Ray ray, in LVector3 vertexA, in LVector3 vertexB, in LVector3 vertexC, out LVector3 intersection)
|
||||
{
|
||||
intersection = LVector3.Zero;
|
||||
// AB边向量
|
||||
LVector3 edgeAB = vertexB - vertexA;
|
||||
// AC边向量
|
||||
LVector3 edgeAC = vertexC - vertexA;
|
||||
|
||||
LVector3 pvec = LVector3.Cross(ray.direction, edgeAC);
|
||||
LFloat det = LVector3.Dot(edgeAB, pvec);
|
||||
if (LMath.Abs(det) <= LFloat.EPSILON)
|
||||
{
|
||||
var p = new Plane(vertexA, vertexB, vertexC);
|
||||
if (p.TestPoint(ray.origin) == EnumPlaneSide.OnPlane && IsPointInTriangle(ray.origin, vertexA, vertexB, vertexC))
|
||||
{
|
||||
intersection = ray.origin;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
det = 1 / det;
|
||||
|
||||
LVector3 tvec = ray.origin - vertexA;
|
||||
LFloat u = LVector3.Dot(tvec, pvec) * det;
|
||||
if (u < 0 || u > 1)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
LVector3 qvec = LVector3.Cross(tvec, edgeAB);
|
||||
LFloat v = LVector3.Dot(ray.direction, qvec) * det;
|
||||
if (v < 0 || u + v > 1)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
LFloat t = LVector3.Dot(edgeAC, qvec) * det;
|
||||
if (t < 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (LMath.Abs(t) <= LFloat.EPSILON)
|
||||
{
|
||||
intersection = ray.origin;
|
||||
}
|
||||
else
|
||||
{
|
||||
intersection = ray.GetEndPoint(t);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检测点point是否在三角形ABC内（重心法）
|
||||
/// </summary>
|
||||
/// <param name="point"></param>
|
||||
/// <param name="vertexA"></param>
|
||||
/// <param name="vertexB"></param>
|
||||
/// <param name="vertexC"></param>
|
||||
/// <returns></returns>
|
||||
public static bool IsPointInTriangle(in LVector3 point, in LVector3 vertexA, in LVector3 vertexB, in LVector3 vertexC)
|
||||
{
|
||||
var vPA = vertexA - point;
|
||||
var vPB = vertexB - point;
|
||||
var vPC = vertexC - point;
|
||||
|
||||
var ab = LVector3.Dot(vPA, vPB);
|
||||
var ac = LVector3.Dot(vPA, vPC);
|
||||
var bc = LVector3.Dot(vPB, vPC);
|
||||
var cc = LVector3.Dot(vPC, vPC);
|
||||
|
||||
if (bc * ac - cc * ab < 0)
|
||||
return false;
|
||||
var bb = LVector3.Dot(vPB, vPB);
|
||||
if (ab * bc - ac * bb < 0)
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算点P是否在有向直线AB的左侧，返回值>0s说明在左侧
|
||||
/// <para>Computes the signed distance from a line connecting the
|
||||
/// specified points to a specified point.</para>
|
||||
/// </summary>
|
||||
/// <param name="linePointA">直线上一点A</param>
|
||||
/// <param name="linePointB">直线上一点B</param>
|
||||
/// <param name="p">查询点P</param>
|
||||
/// <returns>Positive when the point p lies to the left of the line ab.</returns>
|
||||
public static LFloat IsLineLeft(in LVector2 linePointA, in LVector2 linePointB, in LVector2 p)
|
||||
{
|
||||
return LMath.Cross(linePointA - p, linePointB - linePointA);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 返回点P到直线AB的距离平方
|
||||
/// </summary>
|
||||
/// <param name="linePointA">直线上一点A</param>
|
||||
/// <param name="linePointB">直线上一点B</param>
|
||||
/// <param name="pointP">点P</param>
|
||||
/// <returns></returns>
|
||||
public static LFloat SqrDistPointLineSegment(in LVector2 linePointA, in LVector2 linePointB, in LVector2 pointP)
|
||||
{
|
||||
LFloat r = LMath.Dot(pointP - linePointA, linePointB - linePointA) / (linePointB - linePointA).SqrMagnitude;
|
||||
|
||||
if (r < LFloat.Zero)
|
||||
{
|
||||
return (pointP - linePointA).SqrMagnitude;
|
||||
}
|
||||
|
||||
if (r > LFloat.One)
|
||||
{
|
||||
return (pointP - linePointB).SqrMagnitude;
|
||||
}
|
||||
|
||||
return (pointP - (linePointA + r * (linePointB - linePointA))).SqrMagnitude;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 36c93c611b97d4495b67967418da8216
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,158 @@
|
||||
using JNGame.Math;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 平面的数据结构
|
||||
/// </summary>
|
||||
public class Plane
|
||||
{
|
||||
/// <summary>
|
||||
/// 平面法向量，单位向量，法向量方向为面的正面
|
||||
/// </summary>
|
||||
public LVector3 normal = LVector3.Zero;
|
||||
/// <summary>
|
||||
/// 平面到原点的距离
|
||||
/// </summary>
|
||||
public LFloat distFromOrigin = LFloat.Zero;
|
||||
|
||||
/// <summary>
|
||||
/// 默认构造
|
||||
/// </summary>
|
||||
public Plane() { }
|
||||
|
||||
/// <summary>
|
||||
/// 拷贝构造
|
||||
/// </summary>
|
||||
/// <param name="plane"></param>
|
||||
public Plane(Plane plane)
|
||||
{
|
||||
this.normal = plane.normal;
|
||||
this.distFromOrigin = plane.distFromOrigin;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 方向向量和平面到原点的距离构成平面
|
||||
/// </summary>
|
||||
/// <param name="normal">平面的方向向量</param>
|
||||
/// <param name="distance">平面到原点的距离</param>
|
||||
public Plane(in LVector3 normal, LFloat distance)
|
||||
{
|
||||
this.normal = normal.Normalized;
|
||||
this.distFromOrigin = distance;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 方向向量和平面内一点构成平面
|
||||
/// </summary>
|
||||
/// <param name="normal">平面的方向向量</param>
|
||||
/// <param name="point">平面内一点，也可视为原点到平面内一点的向量</param>
|
||||
public Plane(in LVector3 normal, in LVector3 point)
|
||||
{
|
||||
this.normal = normal.Normalized;
|
||||
this.distFromOrigin = -LVector3.Dot(this.normal, point);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 三个点构成一个平面
|
||||
/// </summary>
|
||||
/// <param name="point1"></param>
|
||||
/// <param name="point2"></param>
|
||||
/// <param name="point3"></param>
|
||||
public Plane(in LVector3 point1, in LVector3 point2, in LVector3 point3)
|
||||
{
|
||||
Set(point1, point2, point3);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据三个点，设置平面信息
|
||||
/// </summary>
|
||||
/// <param name="point1"></param>
|
||||
/// <param name="point2"></param>
|
||||
/// <param name="point3"></param>
|
||||
public void Set(in LVector3 point1, in LVector3 point2, in LVector3 point3)
|
||||
{
|
||||
LVector3 vector12 = point2 - point1; // 点1指向点2的向量
|
||||
LVector3 vector23 = point3 - point2; // 点2指向点3的向量
|
||||
normal = LVector3.Cross(vector12, vector23).Normalized;
|
||||
// 规定原点在面的背面，因此距离为负
|
||||
distFromOrigin = -LVector3.Dot(point1, normal);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据点和法线，设置平面信息
|
||||
/// </summary>
|
||||
/// <param name="point"></param>
|
||||
/// <param name="normal"></param>
|
||||
public void Set(in LVector3 point, in LVector3 normal)
|
||||
{
|
||||
this.normal = normal.Normalized;
|
||||
distFromOrigin = -LVector3.Dot(point, normal);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据点和法线，设置平面信息
|
||||
/// </summary>
|
||||
/// <param name="pointX"></param>
|
||||
/// <param name="pointY"></param>
|
||||
/// <param name="pointZ"></param>
|
||||
/// <param name="norX"></param>
|
||||
/// <param name="norY"></param>
|
||||
/// <param name="norZ"></param>
|
||||
public void Set(LFloat pointX, LFloat pointY, LFloat pointZ, LFloat norX, LFloat norY, LFloat norZ)
|
||||
{
|
||||
this.normal = new LVector3(norX, norY, norZ).Normalized;
|
||||
distFromOrigin = -(pointX * norX + pointY * norY + pointZ * norZ);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算平面到点的距离
|
||||
/// </summary>
|
||||
/// <param name="point"></param>
|
||||
/// <returns></returns>
|
||||
public LFloat Distance(in LVector3 point)
|
||||
{
|
||||
// 点在normal方向的投影距离 + 面到原点的距离。根据符号，可判断点在面的哪一侧
|
||||
return LVector3.Dot(normal, point) + distFromOrigin;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 测试点位于平面的哪一面
|
||||
/// </summary>
|
||||
/// <param name="point"></param>
|
||||
/// <returns></returns>
|
||||
public EnumPlaneSide TestPoint(in LVector3 point)
|
||||
{
|
||||
LFloat dist = Distance(in point);
|
||||
|
||||
if (dist == 0)
|
||||
return EnumPlaneSide.OnPlane;
|
||||
else if (dist < 0)
|
||||
return EnumPlaneSide.Back;
|
||||
else
|
||||
return EnumPlaneSide.Front;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 是否位于正面
|
||||
/// </summary>
|
||||
/// <param name="direction"></param>
|
||||
/// <returns></returns>
|
||||
public bool IsFrontFacing(in LVector3 direction)
|
||||
{
|
||||
LFloat dot = LVector3.Dot(normal, direction);
|
||||
return dot <= 0;
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
normal = LVector3.Zero;
|
||||
distFromOrigin = LFloat.Zero;
|
||||
}
|
||||
|
||||
public override string ToString()
|
||||
{
|
||||
return normal.ToString() + ", " + distFromOrigin;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 555aac364a42943c4993c540358381df
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,50 @@
|
||||
using JNGame.Math;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 射线数据结构
|
||||
/// </summary>
|
||||
public class Ray
|
||||
{
|
||||
/// <summary>
|
||||
/// 射线起点
|
||||
/// </summary>
|
||||
public LVector3 origin = LVector3.Zero;
|
||||
/// <summary>
|
||||
/// 射线方向，单位向量
|
||||
/// </summary>
|
||||
public LVector3 direction = LVector3.Zero;
|
||||
|
||||
public Ray() { }
|
||||
|
||||
public Ray(Ray ray)
|
||||
{
|
||||
this.origin = ray.origin;
|
||||
this.direction = ray.direction;
|
||||
}
|
||||
|
||||
public Ray(in LVector3 origin, in LVector3 direction)
|
||||
{
|
||||
this.origin = origin;
|
||||
this.direction = direction.Normalized;
|
||||
}
|
||||
|
||||
public LVector3 GetEndPoint(LFloat distance)
|
||||
{
|
||||
return origin + direction * distance;
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
origin = LVector3.Zero;
|
||||
direction = LVector3.Zero;
|
||||
}
|
||||
|
||||
/** {@inheritDoc} */
|
||||
public override string ToString()
|
||||
{
|
||||
return "ray [" + origin + ":" + direction + "]";
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 07d36ae82dda2434da808db57664e965
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,8 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 884e8e78a03eb4ca1bcdcc561b57c387
|
||||
folderAsset: yes
|
||||
DefaultImporter:
|
||||
externalObjects: {}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,32 @@
|
||||
using JNGame.Math;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 节点的边邻接关系接口定义
|
||||
/// </summary>
|
||||
/// <typeparam name="TNode">图元节点类型</typeparam>
|
||||
public interface IConnection<TNode>
|
||||
{
|
||||
/// <summary>
|
||||
/// 邻接关系的移动消耗
|
||||
/// <para>Returns the non-negative cost of this connection</para>
|
||||
/// </summary>
|
||||
/// <returns></returns>
|
||||
LFloat GetCost();
|
||||
|
||||
/// <summary>
|
||||
/// 邻接关系的起始节点
|
||||
/// <para>Returns the node that this connection came from</para>
|
||||
/// </summary>
|
||||
/// <returns>返回图元节点</returns>
|
||||
TNode GetFromNode();
|
||||
|
||||
/// <summary>
|
||||
/// 邻接关系指向的节点
|
||||
/// <para>Returns the node that this connection leads to</para>
|
||||
/// </summary>
|
||||
/// <returns>返回图元节点</returns>
|
||||
TNode GetToNode();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 7fb05d24110ca4f9091409c8ff0b0220
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,20 @@
|
||||
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 图接口，图是一系列节点的集合，每个节点有邻接关系的列表
|
||||
/// <para> A graph is a collection of nodes, each one having a collection of outgoing {@link Connection connections}. </para>
|
||||
/// </summary>
|
||||
/// <typeparam name="TNode">图元节点类型</typeparam>
|
||||
public interface IGraph<TNode>
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取当前节点的邻接关系
|
||||
/// </summary>
|
||||
/// <param name="fromNode">the node whose outgoing connections will be returned</param>
|
||||
/// <returns>the array of connections outgoing from the given node.</returns>
|
||||
List<IConnection<TNode>> GetConnections(TNode fromNode);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 086d1303386de40d395759867571b2a9
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,32 @@
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// A graph for the {@link IndexedAStarPathFinder}.
|
||||
/// </summary>
|
||||
/// <typeparam name="TNode">Type of node</typeparam>
|
||||
public interface IIndexedGraph<TNode> : IGraph<TNode>
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取节点在图中的索引
|
||||
/// <para>Returns the unique index of the given node.</para>
|
||||
/// </summary>
|
||||
/// <param name="node">the node whose index will be returned</param>
|
||||
/// <returns>return the unique index of the given node.</returns>
|
||||
int GetIndex(TNode node);
|
||||
|
||||
/// <summary>
|
||||
/// 获取图中的节点总数
|
||||
/// <para>Returns the number of nodes in this graph.</para>
|
||||
/// </summary>
|
||||
/// <returns></returns>
|
||||
int GetNodeCount();
|
||||
|
||||
/// <summary>
|
||||
/// 根据节点索引获取图元节点
|
||||
/// </summary>
|
||||
/// <param name="index"></param>
|
||||
/// <returns></returns>
|
||||
TNode GetNode(int index);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 46150180c48a24416979310fdc726856
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,161 @@
|
||||
|
||||
using System.Collections.Generic;
|
||||
using JNGame.Math;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// <para> 导航网格数据 </para>
|
||||
/// <para> 从Unity的NavMesh中导出，经过定点库转化 </para>
|
||||
/// </summary>
|
||||
[System.Serializable]
|
||||
public class NavMeshData
|
||||
{
|
||||
/// <summary>
|
||||
/// NavMeshData关联的关卡地图ID
|
||||
/// </summary>
|
||||
public int mapID;
|
||||
|
||||
/// <summary>
|
||||
/// 移动单位的包围盒半径，在烘焙导航网格时判断是否可通过
|
||||
/// <para> 如果有包围盒半径差别特别大的，需要生成多份的导航网格数据供不同体型的可移动单位使用 </para>
|
||||
/// </summary>
|
||||
public LFloat radius = LFloat.Half;
|
||||
|
||||
/// <summary>
|
||||
/// X轴方向最小位置坐标
|
||||
/// </summary>
|
||||
public LFloat xMin;
|
||||
/// <summary>
|
||||
/// Z轴方向最小位置坐标
|
||||
/// </summary>
|
||||
public LFloat zMin;
|
||||
|
||||
/// <summary>
|
||||
/// X轴方向最大位置坐标
|
||||
/// </summary>
|
||||
public LFloat xMax;
|
||||
/// <summary>
|
||||
/// Z轴方向最大位置坐标
|
||||
/// </summary>
|
||||
public LFloat zMax;
|
||||
|
||||
/// <summary>
|
||||
/// 宽，X轴方向最小位置到最大位置的距离
|
||||
/// </summary>
|
||||
public LFloat Width => LMath.Abs(xMax - xMin);
|
||||
/// <summary>
|
||||
/// 高，Z轴方向最小位置到最大位置的距离
|
||||
/// </summary>
|
||||
public LFloat Height => LMath.Abs(zMax - zMin);
|
||||
|
||||
/// <summary>
|
||||
/// 行走区域的顶点坐标
|
||||
/// </summary>
|
||||
public LVector3[] pathVertices;
|
||||
|
||||
/// <summary>
|
||||
/// 构成行走区域的三角形索引
|
||||
/// </summary>
|
||||
public int[] pathTriangles;
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 数据检测，客户端的顶点坐标和三角形数据有可能是重复的
|
||||
/// <para>TODO 小三角形合并成大三角形或多边形；判断顶点是否在寻路层中，寻路层中的顶点不能作为路径点；两点所连线段是否穿过阻挡区，不穿过，直接获取坐标点</para>
|
||||
/// </summary>
|
||||
/// <param name="scale">缩放比例</param>
|
||||
public void Check(int scale)
|
||||
{
|
||||
FixSameVertex(pathTriangles, pathVertices);
|
||||
ScaleVertex(pathVertices, scale);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 缩放顶点坐标，进行地图的缩放
|
||||
/// </summary>
|
||||
/// <param name="vertices">顶点集合</param>
|
||||
/// <param name="scale">缩放比例</param>
|
||||
private void ScaleVertex(LVector3[] vertices, int scale)
|
||||
{
|
||||
if (vertices == null || scale == 1)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
var lscale = scale.ToLFloat();
|
||||
for (int i = 0; i < vertices.Length; i++)
|
||||
{
|
||||
vertices[i].x += (-this.xMin); // 缩放移动
|
||||
vertices[i].z += (-this.zMin);
|
||||
vertices[i] = vertices[i] * lscale;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 重复顶点坐标，坐标相同的下标修改为一致
|
||||
/// <para>unity的NavMeshData有一些共边的三角形，共边的三角形其实不是连通关系，共边的三角形只是他们共同构成一个凸多边形，并且这种共边的三角形，全部都是扇形排列。</para>
|
||||
/// </summary>
|
||||
/// <param name="indexs">索引数据</param>
|
||||
/// <param name="vertices">顶点数据</param>
|
||||
private void FixSameVertex(int[] indexs, LVector3[] vertices)
|
||||
{
|
||||
if (indexs == null || vertices == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
// 缓存顶点-索引映射
|
||||
Dictionary<LVector3, int> cacheVertices = new Dictionary<LVector3, int>();
|
||||
// 检测路径重复点
|
||||
for (int i = 0; i < vertices.Length; ++i)
|
||||
{
|
||||
// 重复出现的坐标
|
||||
if (cacheVertices.ContainsKey(vertices[i]))
|
||||
{
|
||||
for (int j = 0; j < indexs.Length; j++)
|
||||
{
|
||||
if (indexs[j] == i)
|
||||
{ // 修正重复的坐标
|
||||
indexs[j] = cacheVertices[vertices[i]];
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
cacheVertices.Add(vertices[i], i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
public int[] GetPathTriangles()
|
||||
{
|
||||
return pathTriangles;
|
||||
}
|
||||
|
||||
public void SetPathTriangles(int[] pathTriangles)
|
||||
{
|
||||
this.pathTriangles = pathTriangles;
|
||||
}
|
||||
|
||||
public LVector3[] GetPathVertices()
|
||||
{
|
||||
return pathVertices;
|
||||
}
|
||||
|
||||
public void SetPathVertices(LVector3[] pathVertices)
|
||||
{
|
||||
this.pathVertices = pathVertices;
|
||||
}
|
||||
|
||||
public int GetMapID()
|
||||
{
|
||||
return mapID;
|
||||
}
|
||||
|
||||
public void SetMapID(int mapID)
|
||||
{
|
||||
this.mapID = mapID;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: d4d0cc4c1d59a45f488d5ab7693a8572
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,23 @@
|
||||
using JNGame.Math;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
public static class NavTriangleExtension
|
||||
{
|
||||
/// <summary>
|
||||
/// 基于r，f的值，获取三角形内的一点
|
||||
/// </summary>
|
||||
/// <param name="triangle"></param>
|
||||
/// <param name="r">点在三角形AB边的比值[0, 1]之间</param>
|
||||
/// <param name="f">点在三角形BC边的比值[0, 1]之间</param>
|
||||
/// <returns></returns>
|
||||
public static LVector3 GetVertex(this NavTriangle triangle, LFloat r, LFloat f)
|
||||
{
|
||||
LVector3 vecAB = triangle.vertexB - triangle.vertexA;
|
||||
LVector3 vecBC = triangle.vertexC - triangle.vertexB;
|
||||
LFloat x = vecAB.x * r + vecBC.x * r * f + triangle.vertexA.x;
|
||||
LFloat z = vecAB.z * r + vecBC.z * r * f + triangle.vertexA.z;
|
||||
return new LVector3(x, LFloat.Zero, z);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 49e7da5e90cc551448aa1c8cdedb2148
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,127 @@
|
||||
|
||||
using System.Collections.Generic;
|
||||
using JNGame.Math;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 导航网格中的三角形数据结构
|
||||
/// <para>三个顶点ABC,按照A->B->C->A的绕序。是顺时针绕序</para>
|
||||
/// </summary>
|
||||
public class NavTriangle
|
||||
{
|
||||
/// <summary>
|
||||
/// 三角形序号
|
||||
/// </summary>
|
||||
public int index;
|
||||
|
||||
/// <summary>
|
||||
/// 三角形顶点A
|
||||
/// </summary>
|
||||
public LVector3 vertexA;
|
||||
/// <summary>
|
||||
/// 三角形顶点B
|
||||
/// </summary>
|
||||
public LVector3 vertexB;
|
||||
/// <summary>
|
||||
/// 三角形顶点C
|
||||
/// </summary>
|
||||
public LVector3 vertexC;
|
||||
|
||||
/// <summary>
|
||||
/// 三角形Y轴高度，取三个顶点的平均高度
|
||||
/// </summary>
|
||||
public LFloat y;
|
||||
|
||||
/// <summary>
|
||||
/// 三角形中心点
|
||||
/// </summary>
|
||||
public LVector3 center;
|
||||
|
||||
// /// <summary>
|
||||
// /// 三角形顶点的索引
|
||||
// /// </summary>
|
||||
// public int[] vectorIndices;
|
||||
|
||||
/// <summary>
|
||||
/// 三角形的邻接边关系集合
|
||||
/// </summary>
|
||||
public List<IConnection<NavTriangle>> connections;
|
||||
|
||||
public NavTriangle(in LVector3 vertexA, in LVector3 vertexB, in LVector3 vertexC, int index)
|
||||
{
|
||||
this.vertexA = vertexA;
|
||||
this.vertexB = vertexB;
|
||||
this.vertexC = vertexC;
|
||||
this.y = (vertexA.y + vertexB.y + vertexC.y) / 3;
|
||||
this.index = index;
|
||||
this.center = (vertexA + vertexB + vertexC) / 3;
|
||||
this.connections = new List<IConnection<NavTriangle>>();
|
||||
// this.vectorIndices = vectorIndex;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断点是否在三角形内
|
||||
/// </summary>
|
||||
/// <param name="point"></param>
|
||||
/// <returns></returns>
|
||||
public bool IsInnerPoint(LVector3 point)
|
||||
{
|
||||
bool res = PointInLineLeft(vertexA, vertexB, point);
|
||||
if (res != PointInLineLeft(vertexB, vertexC, point))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (res != PointInLineLeft(vertexC, vertexA, point))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (Cross2D(vertexA, vertexB, vertexC) == 0)
|
||||
{
|
||||
//三点共线
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public static LFloat Cross2D(LVector3 fromPoint, LVector3 toPoint, LVector3 p)
|
||||
{
|
||||
return (toPoint.x - fromPoint.x) * (p.z - fromPoint.z) - (toPoint.z - fromPoint.z) * (p.x - fromPoint.x);
|
||||
}
|
||||
|
||||
public static bool PointInLineLeft(LVector3 fromPoint, LVector3 toPoint, LVector3 p)
|
||||
{
|
||||
return Cross2D(fromPoint, toPoint, p) > 0;
|
||||
}
|
||||
|
||||
public override int GetHashCode()
|
||||
{
|
||||
int prime = 31;
|
||||
int result = 1;
|
||||
result = prime * result + index;
|
||||
return result;
|
||||
}
|
||||
|
||||
public override bool Equals(object obj)
|
||||
{
|
||||
if (this == obj)
|
||||
return true;
|
||||
if (obj == null)
|
||||
return false;
|
||||
if (GetType() != obj.GetType())
|
||||
return false;
|
||||
NavTriangle other = (NavTriangle)obj;
|
||||
if (index != other.index)
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
public override string ToString()
|
||||
{
|
||||
return "Triangle [index=" + index + ", vertexA=" + vertexA + ", vertexB=" + vertexB + ", vertexC=" + vertexC + ", center=" + center + ", connections=" + connections.Count + "]";
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 1b4e6ea59290a47898b80b6d06b31483
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,111 @@
|
||||
using System.Text;
|
||||
using JNGame.Math;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 三角形节点的共享(邻接)边关系
|
||||
/// </summary>
|
||||
public class TriangleEdge : IConnection<NavTriangle>
|
||||
{
|
||||
/// <summary>
|
||||
/// 源三角形
|
||||
/// </summary>
|
||||
public NavTriangle fromNode;
|
||||
/// <summary>
|
||||
/// 指向的三角形，可为空，为空说明该边没有邻接三角形
|
||||
/// </summary>
|
||||
public NavTriangle toNode;
|
||||
|
||||
/// <summary>
|
||||
/// 三角形邻接边的顶点A，边起点，绕序同三角形的绕序
|
||||
/// </summary>
|
||||
public LVector3 vertexA;
|
||||
/// <summary>
|
||||
/// 三角形邻接边的顶点B，边终点，绕序同三角形的绕序
|
||||
/// </summary>
|
||||
public LVector3 vertexB;
|
||||
|
||||
public TriangleEdge()
|
||||
{
|
||||
this.fromNode = null;
|
||||
this.toNode = null;
|
||||
this.vertexA = LVector3.Zero;
|
||||
this.vertexB = LVector3.Zero;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构建三角形节点的邻接边
|
||||
/// </summary>
|
||||
/// <param name="fromNode">边起点三角形</param>
|
||||
/// <param name="toNode">边终点三角形</param>
|
||||
/// <param name="vertexA">边连接的两个三角形的共享边的顶点</param>
|
||||
/// <param name="vertexB">边连接的两个三角形的共享边的顶点</param>
|
||||
public TriangleEdge(NavTriangle fromNode, NavTriangle toNode, in LVector3 vertexA, in LVector3 vertexB)
|
||||
{
|
||||
this.fromNode = fromNode;
|
||||
this.toNode = toNode;
|
||||
this.vertexA = vertexA;
|
||||
this.vertexB = vertexB;
|
||||
}
|
||||
|
||||
public LFloat GetCost()
|
||||
{
|
||||
// 邻边的消耗固定为1
|
||||
return LFloat.One;
|
||||
}
|
||||
|
||||
public NavTriangle GetFromNode()
|
||||
{
|
||||
return fromNode;
|
||||
}
|
||||
|
||||
public NavTriangle GetToNode()
|
||||
{
|
||||
return toNode;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 更新成员属性
|
||||
/// </summary>
|
||||
/// <param name="fromNode"></param>
|
||||
/// <param name="toNode"></param>
|
||||
/// <param name="vertexA"></param>
|
||||
/// <param name="vertexB"></param>
|
||||
public void Set(NavTriangle fromNode, NavTriangle toNode, in LVector3 vertexA, in LVector3 vertexB)
|
||||
{
|
||||
this.fromNode = fromNode;
|
||||
this.toNode = toNode;
|
||||
this.vertexA = vertexA;
|
||||
this.vertexB = vertexB;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 清理成员属性
|
||||
/// </summary>
|
||||
public void Clear()
|
||||
{
|
||||
this.fromNode = null;
|
||||
this.toNode = null;
|
||||
this.vertexA = LVector3.Zero;
|
||||
this.vertexB = LVector3.Zero;
|
||||
}
|
||||
|
||||
public bool IsValid()
|
||||
{
|
||||
return fromNode != null;
|
||||
}
|
||||
|
||||
public override string ToString()
|
||||
{
|
||||
StringBuilder sb = new StringBuilder("Edge{");
|
||||
sb.Append("fromNode=").Append(fromNode.index);
|
||||
//sb.Append(", toNode=").Append(toNode == null ? "null" : toNode.index);
|
||||
sb.Append(", VertexA=").Append(vertexA);
|
||||
sb.Append(", VertexB=").Append(vertexB);
|
||||
sb.Append('}');
|
||||
return sb.ToString();
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 23cebc1b22ecc4befabcaec093f97db5
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,557 @@
|
||||
using System.Collections.Generic;
|
||||
using JNGame.Math;
|
||||
using Profiler = JNGame.Util.Profiler;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 以三角形为图节点的索引图数据结构
|
||||
/// </summary>
|
||||
public partial class TriangleGraph : IIndexedGraph<NavTriangle>
|
||||
{
|
||||
/// <summary>
|
||||
/// 存储共边三角形共边关系
|
||||
/// <para>Class for storing the edge connection data between two adjacent triangles.</para>
|
||||
/// </summary>
|
||||
public class IndexConnection
|
||||
{
|
||||
/// <summary>
|
||||
/// <para>共边顶点索引1，绕序的起点</para>
|
||||
/// The vertex indices which makes up the edge shared between two triangles.
|
||||
/// </summary>
|
||||
public int edgeVertexIndex1;
|
||||
|
||||
/// <summary>
|
||||
/// <para>共边顶点索引2，绕序的终点</para>
|
||||
/// The vertex indices which makes up the edge shared between two triangles.
|
||||
/// </summary>
|
||||
public int edgeVertexIndex2;
|
||||
|
||||
/// <summary>
|
||||
/// <para>共边的三角形索引1</para>
|
||||
/// The indices of the two triangles sharing this edge.
|
||||
/// </summary>
|
||||
public int fromTriIndex;
|
||||
|
||||
/// <summary>
|
||||
/// <para>共边的三角形索引2</para>
|
||||
/// The indices of the two triangles sharing this edge.
|
||||
public int toTriIndex;
|
||||
|
||||
public IndexConnection(int sharedEdgeVertex1Index, int sharedEdgeVertex2Index, int fromTriIndex, int toTriIndex)
|
||||
{
|
||||
this.edgeVertexIndex1 = sharedEdgeVertex1Index;
|
||||
this.edgeVertexIndex2 = sharedEdgeVertex2Index;
|
||||
this.fromTriIndex = fromTriIndex;
|
||||
this.toTriIndex = toTriIndex;
|
||||
}
|
||||
|
||||
public override string ToString()
|
||||
{
|
||||
return "IndexConnection [edgeVertexIndex1=" + edgeVertexIndex1 + ", edgeVertexIndex2=" +
|
||||
edgeVertexIndex2
|
||||
+ ", fromTriIndex=" + fromTriIndex + ", toTriIndex=" + toTriIndex + "]";
|
||||
}
|
||||
|
||||
public override int GetHashCode()
|
||||
{
|
||||
int prime = 31;
|
||||
int result = 1;
|
||||
result = prime * result + edgeVertexIndex1;
|
||||
result = prime * result + edgeVertexIndex2;
|
||||
result = prime * result + fromTriIndex;
|
||||
result = prime * result + toTriIndex;
|
||||
return result;
|
||||
}
|
||||
|
||||
public override bool Equals(object obj)
|
||||
{
|
||||
if (this == obj)
|
||||
return true;
|
||||
if (obj == null)
|
||||
return false;
|
||||
if (GetType() != obj.GetType())
|
||||
return false;
|
||||
IndexConnection other = (IndexConnection)obj;
|
||||
if (edgeVertexIndex1 != other.edgeVertexIndex1)
|
||||
return false;
|
||||
if (edgeVertexIndex2 != other.edgeVertexIndex2)
|
||||
return false;
|
||||
if (fromTriIndex != other.fromTriIndex)
|
||||
return false;
|
||||
if (toTriIndex != other.toTriIndex)
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 导航网格数据，从文件读取
|
||||
/// </summary>
|
||||
private readonly NavMeshData m_NavMeshData;
|
||||
|
||||
/// <summary>
|
||||
/// 根据导航网格数据，生成的三角形
|
||||
/// </summary>
|
||||
private readonly List<NavTriangle> m_Triangles = new List<NavTriangle>();
|
||||
public List<NavTriangle> Triangles => m_Triangles;
|
||||
|
||||
/// <summary>
|
||||
/// 三角形的邻接边关系映射表 [Key, Value] = [三角形对象，共边的连接关系]
|
||||
/// <para>与m_IsolatedEdgesMap应该是严格互补的</para>
|
||||
/// </summary>
|
||||
private readonly Dictionary<NavTriangle, List<IConnection<NavTriangle>>> m_SharedEdges;
|
||||
|
||||
/// <summary>
|
||||
/// 三角形的独边关系映射表 [Key, Value] = [三角形对象，无邻边的连接关系]
|
||||
/// <para>与m_SharedEdges应该是严格互补的</para>
|
||||
/// </summary>
|
||||
private readonly Dictionary<NavTriangle, List<IConnection<NavTriangle>>> m_IsolatedEdgesMap;
|
||||
|
||||
/// <summary>
|
||||
/// BSP树，通过空间分割快速查找点所在的三角形
|
||||
/// </summary>
|
||||
private readonly BspTree m_BspTree;
|
||||
|
||||
/// <summary>
|
||||
/// 导航三角形中独边总数
|
||||
/// </summary>
|
||||
private readonly int m_NumDisconnectedEdges;
|
||||
|
||||
/// <summary>
|
||||
/// 导航三角形中共享边总数
|
||||
/// </summary>
|
||||
private readonly int m_NumConnectedEdges;
|
||||
|
||||
/// <summary>
|
||||
/// 导航三角形中所有的边总数
|
||||
/// </summary>
|
||||
private readonly int m_NumTotalEdges;
|
||||
|
||||
/// <summary>
|
||||
/// 构造以三角形为节点的图数据结构
|
||||
/// </summary>
|
||||
/// <param name="navMeshData">导航网格数据</param>
|
||||
/// <param name="scale">缩放比例，默认：1</param>
|
||||
public TriangleGraph(NavMeshData navMeshData, int scale = 1)
|
||||
{
|
||||
m_NavMeshData = navMeshData;
|
||||
// 检测导航网格数据中重复的顶点，并按需求对顶点进行缩放
|
||||
m_NavMeshData.Check(scale);
|
||||
// 根据导航网格数据构建三角形
|
||||
var pathTriangles = CreateTriangles();
|
||||
// 计算所有三角形的共边关系
|
||||
var pathIndexConnections = GetIndexConnections(navMeshData.pathTriangles);
|
||||
// 创建三角形邻边数据表
|
||||
m_SharedEdges = CreateSharedEdgesMap(pathIndexConnections, pathTriangles, navMeshData.pathVertices);
|
||||
// 创建三角形独边数据表
|
||||
m_IsolatedEdgesMap = CreateIsolatedEdgesMap(m_SharedEdges);
|
||||
// 创建BSP树，耗时
|
||||
m_BspTree = new BspTree();
|
||||
m_BspTree.Init(pathTriangles);
|
||||
// 统计独边的总数
|
||||
foreach (var edges in m_IsolatedEdgesMap.Values)
|
||||
{
|
||||
m_NumDisconnectedEdges += edges.Count;
|
||||
}
|
||||
// 统计共享边的总数
|
||||
foreach (var edges in m_SharedEdges.Values)
|
||||
{
|
||||
m_NumConnectedEdges += edges.Count;
|
||||
}
|
||||
m_NumConnectedEdges /= 2; // 相邻的三角形各有一条相互的共享边连接数据，因此需要/2
|
||||
// 导航三角形中所有边的总数
|
||||
m_NumTotalEdges = m_NumConnectedEdges + m_NumDisconnectedEdges;
|
||||
|
||||
// Debug.Log("CreateTriangleGraph Success! mapId{0} triangles{1} totalEdges{2} connEdges{3} disConnEdges{4}", navMeshData.mapID, m_Triangles.Count
|
||||
// , m_NumTotalEdges, m_NumConnectedEdges, m_NumDisconnectedEdges);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前三角形的邻接边数据
|
||||
/// </summary>
|
||||
/// <param name="fromNode">查询的三角形节点</param>
|
||||
/// <returns></returns>
|
||||
public List<IConnection<NavTriangle>> GetConnections(NavTriangle fromNode)
|
||||
{
|
||||
return m_SharedEdges.TryGetValue(fromNode, out var val) ? val : null;
|
||||
}
|
||||
|
||||
public int GetIndex(NavTriangle node)
|
||||
{
|
||||
return node.index;
|
||||
}
|
||||
|
||||
public int GetNodeCount()
|
||||
{
|
||||
return m_SharedEdges.Count;
|
||||
}
|
||||
|
||||
public NavTriangle GetNode(int index)
|
||||
{
|
||||
if (index < 0 || index >= m_Triangles.Count)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
return m_Triangles[index];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 遍历NavMeshData的顶点数组和索引数组，构建三角形
|
||||
/// </summary>
|
||||
/// <returns></returns>
|
||||
private List<NavTriangle> CreateTriangles()
|
||||
{
|
||||
var vertexIndexs = m_NavMeshData.GetPathTriangles();
|
||||
var vertices = m_NavMeshData.GetPathVertices();
|
||||
int triangleIndex = 0; // 三角形下标
|
||||
int length = vertexIndexs.Length - 3;
|
||||
for (int i = 0; i <= length;)
|
||||
{
|
||||
int aIndex = vertexIndexs[i++];
|
||||
int bIndex = vertexIndexs[i++];
|
||||
int cIndex = vertexIndexs[i++];
|
||||
NavTriangle triangle = new NavTriangle(vertices[aIndex], vertices[bIndex], vertices[cIndex], triangleIndex++);
|
||||
m_Triangles.Add(triangle);
|
||||
}
|
||||
return m_Triangles;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取所有三角形的共享边关系
|
||||
/// </summary>
|
||||
/// <param name="indices">顶点索引</param>
|
||||
/// <returns></returns>
|
||||
private static HashSet<IndexConnection> GetIndexConnections(int[] indices)
|
||||
{
|
||||
var indexConnections = new HashSet<IndexConnection>();
|
||||
int[] edge = { -1, -1 }; // 隐含三角形顶点绕序，0是绕序的起点，1是绕序的终点
|
||||
int i = 0;
|
||||
int j, a0, a1, a2, b0, b1, b2, triAIndex, triBIndex;
|
||||
while (i < indices.Length)
|
||||
{
|
||||
triAIndex = i / 3; // A三角形索引
|
||||
a0 = indices[i++]; // 构成A三角形的三个顶点索引
|
||||
a1 = indices[i++];
|
||||
a2 = indices[i++];
|
||||
j = i; // 第二个三角形从A三角形之后开始计算，A三角形之前的连接关系已经在之前的循环中计算过了
|
||||
while (j < indices.Length)
|
||||
{
|
||||
triBIndex = j / 3; // B三角形索引
|
||||
b0 = indices[j++]; // 构成B三角形的三个顶点索引
|
||||
b1 = indices[j++];
|
||||
b2 = indices[j++];
|
||||
|
||||
if (HasSharedEdgeIndices(a0, a1, a2, b0, b1, b2, edge))
|
||||
{
|
||||
// 两个有共享边的三角形，三角形A和三角形B共享边的绕序必定是相反的
|
||||
var indexConnection1 = new IndexConnection(edge[0], edge[1], triAIndex, triBIndex);
|
||||
var indexConnection2 = new IndexConnection(edge[1], edge[0], triBIndex, triAIndex);
|
||||
indexConnections.Add(indexConnection1);
|
||||
indexConnections.Add(indexConnection2);
|
||||
edge[0] = -1;
|
||||
edge[1] = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Debug.Log("连接个数：{0}", indexConnections.Count);
|
||||
return indexConnections;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检测两个三角形是否有共享边，有两个共享索引，说明两个三角形存在共边
|
||||
/// <para> Checks if the two triangles have shared vertex indices.</para>
|
||||
/// <para>The edge will always follow the vertex winding order of the triangle A. Since all
|
||||
/// triangles have the same winding order, triangle A should have the opposite
|
||||
/// edge direction to triangle B.</para>
|
||||
/// </summary>
|
||||
/// <param name="a0">三角形A顶点0索引</param>
|
||||
/// <param name="a1">三角形A顶点1索引</param>
|
||||
/// <param name="a2">三角形A顶点2索引</param>
|
||||
/// <param name="b0">三角形B顶点0索引</param>
|
||||
/// <param name="b1">三角形B顶点1索引</param>
|
||||
/// <param name="b2">三角形B顶点2索引</param>
|
||||
/// <param name="edge">输出:[共边顶点索引1, 共边顶点索引2]</param>
|
||||
/// <returns>bool: 是否存在共边</returns>
|
||||
private static bool HasSharedEdgeIndices(int a0, int a1, int a2, int b0, int b1, int b2, int[] edge)
|
||||
{
|
||||
bool match0 = (a0 == b0 || a0 == b1 || a0 == b2);
|
||||
bool match1 = (a1 == b0 || a1 == b1 || a1 == b2);
|
||||
if (!match0 && !match1)
|
||||
{ // 无两个共享点
|
||||
return false;
|
||||
}
|
||||
else if (match0 && match1)
|
||||
{
|
||||
edge[0] = a0; // 保证三角形绕序一致，0->1->2
|
||||
edge[1] = a1;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool match2 = (a2 == b0 || a2 == b1 || a2 == b2);
|
||||
if (match0 && match2)
|
||||
{
|
||||
edge[0] = a2; // 保证三角形绕序一致，0->1->2
|
||||
edge[1] = a0;
|
||||
return true;
|
||||
}
|
||||
else if (match1 && match2)
|
||||
{
|
||||
edge[0] = a1; // 保证三角形绕序一致，0->1->2
|
||||
edge[1] = a2;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建三角形共享边映射表 [Key, Value] = [三角形对象，共边的连接关系]
|
||||
/// </summary>
|
||||
/// <param name="indexConnections">共享边关系集合</param>
|
||||
/// <param name="triangles">三角形集合</param>
|
||||
/// <param name="vertices">顶点集合</param>
|
||||
/// <returns></returns>
|
||||
private static Dictionary<NavTriangle, List<IConnection<NavTriangle>>> CreateSharedEdgesMap(
|
||||
HashSet<IndexConnection> indexConnections, List<NavTriangle> triangles, LVector3[] vertices)
|
||||
{
|
||||
var connectionMap = new Dictionary<NavTriangle, List<IConnection<NavTriangle>>>();
|
||||
|
||||
foreach (NavTriangle triangle in triangles)
|
||||
{
|
||||
connectionMap.Add(triangle, new List<IConnection<NavTriangle>>());
|
||||
}
|
||||
|
||||
foreach (IndexConnection indexConnection in indexConnections)
|
||||
{
|
||||
var fromNode = triangles[indexConnection.fromTriIndex];
|
||||
var toNode = triangles[indexConnection.toTriIndex];
|
||||
var edgeVertexA = vertices[indexConnection.edgeVertexIndex1];
|
||||
var edgeVertexB = vertices[indexConnection.edgeVertexIndex2];
|
||||
var edge = new TriangleEdge(fromNode, toNode, edgeVertexA, edgeVertexB);
|
||||
connectionMap[fromNode].Add(edge);
|
||||
fromNode.connections.Add(edge);
|
||||
//Debug.Log("Triangle：{0}-->{1} {2}-->{3}", fromNode.index, toNode.index, fromNode, toNode);
|
||||
}
|
||||
|
||||
return connectionMap;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建三角形的没有邻接边的关系映射表
|
||||
/// </summary>
|
||||
/// <param name="connectionMap">邻边关系集合</param>
|
||||
/// <returns>无邻边关系映射表</returns>
|
||||
private static Dictionary<NavTriangle, List<IConnection<NavTriangle>>> CreateIsolatedEdgesMap(
|
||||
Dictionary<NavTriangle, List<IConnection<NavTriangle>>> connectionMap)
|
||||
{
|
||||
var disconnectionMap = new Dictionary<NavTriangle, List<IConnection<NavTriangle>>>();
|
||||
|
||||
foreach (NavTriangle tri in connectionMap.Keys)
|
||||
{
|
||||
var connectedEdges = connectionMap[tri];
|
||||
// 没有邻接边的关系集合
|
||||
var disconnectedEdges = new List<IConnection<NavTriangle>>();
|
||||
disconnectionMap.Add(tri, disconnectedEdges);
|
||||
|
||||
if (connectedEdges.Count < 3)
|
||||
{ // This triangle does not have all edges connected to other triangles
|
||||
bool ab = true; // 三角形顶点AB边没有邻接边
|
||||
bool bc = true; // 三角形顶点BC边没有邻接边
|
||||
bool ca = true; // 三角形顶点CA边没有邻接边
|
||||
foreach (var item in connectedEdges)
|
||||
{
|
||||
var edge = item as TriangleEdge;
|
||||
if (edge.vertexA == tri.vertexA && edge.vertexB == tri.vertexB)
|
||||
{
|
||||
ab = false; // AB边在邻接边关系集合中，标记置为false
|
||||
}
|
||||
else if (edge.vertexA == tri.vertexB && edge.vertexB == tri.vertexC)
|
||||
{
|
||||
bc = false; // BC边在邻接边关系集合中，标记置为false
|
||||
}
|
||||
else if (edge.vertexA == tri.vertexC && edge.vertexB == tri.vertexA)
|
||||
{
|
||||
ca = false; // CA边在邻接边关系集合中，标记置为false
|
||||
}
|
||||
}
|
||||
// 没有邻边三角形的，加入没有邻边集合中
|
||||
if (ab)
|
||||
{
|
||||
disconnectedEdges.Add(new TriangleEdge(tri, null, tri.vertexA, tri.vertexB));
|
||||
}
|
||||
if (bc)
|
||||
{
|
||||
disconnectedEdges.Add(new TriangleEdge(tri, null, tri.vertexB, tri.vertexC));
|
||||
}
|
||||
if (ca)
|
||||
{
|
||||
disconnectedEdges.Add(new TriangleEdge(tri, null, tri.vertexC, tri.vertexA));
|
||||
}
|
||||
}
|
||||
// 断言所有边关系的总和必定为三角形边数
|
||||
int totalEdges = connectedEdges.Count + disconnectedEdges.Count;
|
||||
// Debug.Assert(totalEdges == 3, "Wrong number of edges ({0}) in triangle {1}", totalEdges, tri.index);
|
||||
}
|
||||
|
||||
return disconnectionMap;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据点位置获取所在的三角形
|
||||
/// </summary>
|
||||
/// <param name="point"></param>
|
||||
/// <returns></returns>
|
||||
public NavTriangle GetTriangle(in LVector3 point)
|
||||
{
|
||||
Profiler.BeginSample("InternalGetTriangle");
|
||||
var ret = InternalGetTriangle(in point, out _);
|
||||
Profiler.EndSample();
|
||||
return ret;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据点位置获取所在的三角形，如果点不在，则获取邻接的三角形，并输出在三角形上的邻接点
|
||||
/// </summary>
|
||||
/// <param name="point"></param>
|
||||
/// <param name="pointOnTriangle">三角形上邻接point的点</param>
|
||||
/// <returns></returns>
|
||||
public NavTriangle GetTriangleOrNeareastTriangle(in LVector3 point, out LVector3 pointOnTriangle)
|
||||
{
|
||||
Profiler.BeginSample("GetTriangleOrNearestTriangle");
|
||||
var ret = InternalGetTriangle(in point, out List<int> nearTriangles);
|
||||
if (ret == null)
|
||||
{
|
||||
// 不在三角形内，找最近的一个三角形
|
||||
LFloat minDist = LFloat.FLT_MAX;
|
||||
LVector2 pointInVec2 = point.ToLVector2XZ();
|
||||
for (int i = 0; i < nearTriangles.Count; ++i)
|
||||
{
|
||||
int triangleIndex = nearTriangles[i];
|
||||
NavTriangle triangle = m_Triangles[triangleIndex];
|
||||
if (m_IsolatedEdgesMap.TryGetValue(triangle, out List<IConnection<NavTriangle>> isolatedEdges))
|
||||
{
|
||||
// 查找独边，找邻接三角形
|
||||
TriangleEdge edge = InternalGetNearestEdge(pointInVec2, isolatedEdges, out LFloat outSqrDistMin);
|
||||
if (outSqrDistMin < minDist)
|
||||
{
|
||||
ret = edge.fromNode;
|
||||
minDist = outSqrDistMin;
|
||||
}
|
||||
}
|
||||
}
|
||||
pointOnTriangle = LVector3.Zero;
|
||||
if (ret != null)
|
||||
{
|
||||
GeometryUtil.GetClosestPointOnTriangle(ret.vertexA, ret.vertexB, ret.vertexC, point, ref pointOnTriangle);
|
||||
// 稍微向三角形方向内部一点，避免严格在边上，三角形落位判断问题
|
||||
pointOnTriangle = point + (pointOnTriangle - point) * new LFloat(true, 1010000);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
pointOnTriangle = point;
|
||||
}
|
||||
Profiler.EndSample();
|
||||
return ret;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取最邻接目标点的共享边
|
||||
/// </summary>
|
||||
/// <param name="navTriangle"></param>
|
||||
/// <param name="position"></param>
|
||||
/// <returns></returns>
|
||||
public TriangleEdge GetNearestSharedEdge(NavTriangle navTriangle, in LVector3 position)
|
||||
{
|
||||
if (m_SharedEdges.TryGetValue(navTriangle, out List<IConnection<NavTriangle>> outConnections))
|
||||
{
|
||||
return InternalGetNearestEdge(position.ToLVector2XZ(), outConnections, out _);
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取最邻接目标点的阻挡边
|
||||
/// </summary>
|
||||
/// <param name="navTriangle"></param>
|
||||
/// <param name="position"></param>
|
||||
/// <returns></returns>
|
||||
public TriangleEdge GetNeareastIsolatedEdge(NavTriangle navTriangle, in LVector3 position)
|
||||
{
|
||||
if (m_IsolatedEdgesMap.TryGetValue(navTriangle, out List<IConnection<NavTriangle>> outConnections))
|
||||
{
|
||||
return InternalGetNearestEdge(position.ToLVector2XZ(), outConnections, out _);
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取最邻接目标点的三角形边，不考虑阻挡还是共享
|
||||
/// </summary>
|
||||
/// <param name="navTriangle"></param>
|
||||
/// <param name="position"></param>
|
||||
/// <returns></returns>
|
||||
public TriangleEdge GetNearestEdge(NavTriangle navTriangle, in LVector3 position)
|
||||
{
|
||||
// 最近的共享边
|
||||
TriangleEdge nearestSharedEdge = null;
|
||||
LFloat sharedEdgeSqrDist = LFloat.FLT_MAX;
|
||||
if (m_SharedEdges.TryGetValue(navTriangle, out List<IConnection<NavTriangle>> outSharedConnections))
|
||||
{
|
||||
nearestSharedEdge = InternalGetNearestEdge(position.ToLVector2XZ(), outSharedConnections, out sharedEdgeSqrDist);
|
||||
}
|
||||
// 最近的阻挡边
|
||||
TriangleEdge nearestIsolatedEdge = null;
|
||||
LFloat osolatedEdgeSqrDist = LFloat.FLT_MAX;
|
||||
if (m_IsolatedEdgesMap.TryGetValue(navTriangle, out List<IConnection<NavTriangle>> outIsolatedConnections))
|
||||
{
|
||||
nearestIsolatedEdge = InternalGetNearestEdge(position.ToLVector2XZ(), outIsolatedConnections, out osolatedEdgeSqrDist);
|
||||
}
|
||||
// 选取最近的返回
|
||||
return sharedEdgeSqrDist < osolatedEdgeSqrDist ? nearestSharedEdge : nearestIsolatedEdge;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取点落位的三角形，若为空，会附带点附近的一些三角形
|
||||
/// </summary>
|
||||
/// <param name="point"></param>
|
||||
/// <param name="nearTriangles"></param>
|
||||
/// <returns></returns>
|
||||
private NavTriangle InternalGetTriangle(in LVector3 point, out List<int> nearTriangles)
|
||||
{
|
||||
// 通过Bsp空间分割树查找点所在的三角形
|
||||
var index = m_BspTree.GetTriangle(point, out nearTriangles);
|
||||
if (index >= 0 && index < m_Triangles.Count)
|
||||
{
|
||||
return m_Triangles[index];
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取距离指定位置最近的边
|
||||
/// </summary>
|
||||
/// <param name="position">目标位置</param>
|
||||
/// <param name="edges">边集合</param>
|
||||
/// <param name="outSqrDistMin">输出最短距离</param>
|
||||
/// <returns></returns>
|
||||
private TriangleEdge InternalGetNearestEdge(in LVector2 position, List<IConnection<NavTriangle>> edges, out LFloat outSqrDistMin)
|
||||
{
|
||||
outSqrDistMin = LFloat.FLT_MAX;
|
||||
if (edges == null || edges.Count <= 0) { return null; }
|
||||
TriangleEdge targetEdge = null;
|
||||
foreach (IConnection<NavTriangle> connection in edges)
|
||||
{
|
||||
if (connection is not TriangleEdge edge) { continue; }
|
||||
LFloat sprDist = GeometryUtil.SqrDistPointLineSegment(edge.vertexA.ToLVector2XZ(), edge.vertexB.ToLVector2XZ(), position);
|
||||
if (sprDist < outSqrDistMin)
|
||||
{
|
||||
targetEdge = edge;
|
||||
outSqrDistMin = sprDist;
|
||||
}
|
||||
}
|
||||
return targetEdge;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 68a7ecd59eb0548cf9676b742a1855b8
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,69 @@
|
||||
#if UNITY_5_3_OR_NEWER
|
||||
|
||||
using JNGame.Math;
|
||||
using UnityEngine;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
public partial class TriangleNavMesh
|
||||
{
|
||||
#if UNITY_EDITOR
|
||||
|
||||
public void DebugVisualize()
|
||||
{
|
||||
m_Graph.DebugVisualize();
|
||||
}
|
||||
|
||||
public void DrawGizmos()
|
||||
{
|
||||
m_Graph.DrawGizmos();
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
public partial class TriangleGraph
|
||||
{
|
||||
#if UNITY_EDITOR
|
||||
|
||||
public void DebugVisualize()
|
||||
{
|
||||
m_BspTree.DebugDraw();
|
||||
}
|
||||
|
||||
public void DrawGizmos()
|
||||
{
|
||||
if (m_NavMeshData == null || m_NavMeshData.pathTriangles == null) { return; }
|
||||
int i = 0;
|
||||
Gizmos.color = Color.green;
|
||||
while (i < m_NavMeshData.pathTriangles.Length)
|
||||
{
|
||||
int aIndex = m_NavMeshData.pathTriangles[i++];
|
||||
int bIndex = m_NavMeshData.pathTriangles[i++];
|
||||
int cIndex = m_NavMeshData.pathTriangles[i++];
|
||||
|
||||
Vector3 vertexA = m_NavMeshData.pathVertices[aIndex].ToVector3();
|
||||
Vector3 vertexB = m_NavMeshData.pathVertices[bIndex].ToVector3();
|
||||
Vector3 vertexC = m_NavMeshData.pathVertices[cIndex].ToVector3();
|
||||
|
||||
Gizmos.DrawLine(vertexA, vertexB);
|
||||
Gizmos.DrawLine(vertexB, vertexC);
|
||||
Gizmos.DrawLine(vertexC, vertexA);
|
||||
}
|
||||
|
||||
Gizmos.color = Color.blue;
|
||||
Vector3 bottomLeft = new LVector3(m_NavMeshData.xMin, 0.ToLFloat(), m_NavMeshData.zMin).ToVector3();
|
||||
Vector3 bottomRight = new LVector3(m_NavMeshData.xMax, 0.ToLFloat(), m_NavMeshData.zMin).ToVector3();
|
||||
Vector3 topLeft = new LVector3(m_NavMeshData.xMin, 0.ToLFloat(), m_NavMeshData.zMax).ToVector3();
|
||||
Vector3 topRight = new LVector3(m_NavMeshData.xMax, 0.ToLFloat(), m_NavMeshData.zMax).ToVector3();
|
||||
Gizmos.DrawLine(bottomLeft, bottomRight);
|
||||
Gizmos.DrawLine(bottomLeft, topLeft);
|
||||
Gizmos.DrawLine(bottomRight, topRight);
|
||||
Gizmos.DrawLine(topLeft, topRight);
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: b08810f156faa684f92fa3cd9cb54c14
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,205 @@
|
||||
using System.Collections.Generic;
|
||||
using JNGame.Math;
|
||||
using JNGame.Util;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 边查找过滤枚举
|
||||
/// </summary>
|
||||
[System.Flags]
|
||||
public enum EnumEdgeFilter
|
||||
{
|
||||
/// <summary>
|
||||
/// 共享边
|
||||
/// </summary>
|
||||
kFilterShared = 1,
|
||||
/// <summary>
|
||||
/// 独边
|
||||
/// </summary>
|
||||
kFilterIsolated = 2,
|
||||
/// <summary>
|
||||
/// 全部边
|
||||
/// </summary>
|
||||
kFilterAll = kFilterShared | kFilterIsolated
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 以三角形为图元的导航网格
|
||||
/// </summary>
|
||||
public partial class TriangleNavMesh
|
||||
{
|
||||
/// <summary>
|
||||
/// 三角形图元节点组成的图
|
||||
/// </summary>
|
||||
private readonly TriangleGraph m_Graph;
|
||||
|
||||
/// <summary>
|
||||
/// 路径查找器
|
||||
/// </summary>
|
||||
private readonly IndexedAStarPathFinder<NavTriangle> m_PathFinder;
|
||||
|
||||
/// <summary>
|
||||
/// 寻路启发器
|
||||
/// </summary>
|
||||
private readonly TriangleHeuristic m_Heuristic;
|
||||
|
||||
/// <summary>
|
||||
/// 寻路坐标描述结果的缓存，每次寻路都会被清理和覆盖，外部不可直接使用本对象
|
||||
/// </summary>
|
||||
private readonly TrianglePointPath m_TempPointPath = new TrianglePointPath();
|
||||
|
||||
/// <summary>
|
||||
/// 寻路节点描述结果的缓存，每次寻路都会被清理和覆盖，外部不可直接使用本对象
|
||||
/// </summary>
|
||||
private readonly TriangleEdgeGraphPath m_TempGraphPath = new TriangleEdgeGraphPath();
|
||||
|
||||
/// <summary>
|
||||
/// 构造导航网格
|
||||
/// </summary>
|
||||
/// <param name="jsonNavMeshData">导航网格数据，Json格式字符串</param>
|
||||
/// <param name="scale"></param>
|
||||
public TriangleNavMesh(string jsonNavMeshData, int scale = 1)
|
||||
{
|
||||
var data = JsonUtil.ToObject<NavMeshData>(jsonNavMeshData);
|
||||
m_Graph = new TriangleGraph(data, scale);
|
||||
m_PathFinder = new IndexedAStarPathFinder<NavTriangle>(m_Graph, true);
|
||||
m_Heuristic = new TriangleHeuristic();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造导航网格
|
||||
/// </summary>
|
||||
/// <param name="navMeshData">导航网格数据</param>
|
||||
/// <param name="scale"></param>
|
||||
public TriangleNavMesh(NavMeshData navMeshData, int scale = 1)
|
||||
{
|
||||
m_Graph = new TriangleGraph(navMeshData, scale);
|
||||
m_PathFinder = new IndexedAStarPathFinder<NavTriangle>(m_Graph, true);
|
||||
m_Heuristic = new TriangleHeuristic();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 搜索从起点fromPoint到终点toPoint的路径
|
||||
/// </summary>
|
||||
/// <param name="fromPoint">起点位置</param>
|
||||
/// <param name="toPoint">终点位置</param>
|
||||
/// <param name="allowFixToPoint">是否运行修正终点坐标，如果终点在阻挡内，使用阻挡边的邻接点作为终点</param>
|
||||
/// <returns>路径经过的坐标点列表</returns>
|
||||
public List<LVector3> FindPath(in LVector3 fromPoint, in LVector3 toPoint, bool allowFixToPoint = false)
|
||||
{
|
||||
// 清理结果缓存
|
||||
m_TempPointPath.Clear();
|
||||
m_TempGraphPath.Clear();
|
||||
// 查找目标路径
|
||||
bool find = FindPath(fromPoint, toPoint, m_TempGraphPath, allowFixToPoint);
|
||||
if (!find)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
// 节点路径描述转化为坐标路径描述
|
||||
// m_TempPointPath.CalculateForGraphPath(m_TempGraphPath, false);
|
||||
// Profiler.BeginSample("PathTranslate");
|
||||
m_TempPointPath.CalculateForGraphPath(m_TempGraphPath, false);
|
||||
// Profiler.EndSample();
|
||||
return m_TempPointPath.GetVectors();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 搜索从fromPoint到toPoint的路径，路径结果用路径经过的图上的边或结点表示
|
||||
/// </summary>
|
||||
/// <param name="fromPoint">起点位置</param>
|
||||
/// <param name="toPoint">终点位置</param>
|
||||
/// <param name="allowFixToPoint">是否运行修正终点坐标，如果终点在阻挡内，使用阻挡边的邻接点作为终点</param>
|
||||
/// <param name="path">结果路径</param>
|
||||
/// <returns>是否成功找出路径</returns>
|
||||
private bool FindPath(in LVector3 fromPoint, in LVector3 toPoint, TriangleEdgeGraphPath path, bool allowFixToPoint)
|
||||
{
|
||||
// 获取起始点和目标点所在的三角形节点
|
||||
NavTriangle fromTriangle = m_Graph.GetTriangle(in fromPoint);
|
||||
LVector3 toPointFixed = toPoint;
|
||||
NavTriangle toTriangle = allowFixToPoint ? m_Graph.GetTriangleOrNeareastTriangle(in toPoint, out toPointFixed)
|
||||
: m_Graph.GetTriangle(in toPoint);
|
||||
// 验证输入合法性
|
||||
if (fromTriangle == null)
|
||||
{
|
||||
//Debug.LogWarning("TriangleNavMesh.FindPath Failed! fromPoint = {0} valid!", fromPoint);
|
||||
return false;
|
||||
}
|
||||
if (toTriangle == null)
|
||||
{
|
||||
//Debug.LogWarning("TriangleNavMesh.FindPath Failed! toPoint = {0} valid!", toPoint);
|
||||
return false;
|
||||
}
|
||||
// Profiler.BeginSample("AStarPathFinder");
|
||||
// 搜索路径
|
||||
bool result = m_PathFinder.SearchPath(fromTriangle, toTriangle, m_Heuristic, path);
|
||||
if (result)
|
||||
{
|
||||
path.startPoint = fromPoint;
|
||||
path.endPoint = toPointFixed;
|
||||
path.startTriangle = fromTriangle;
|
||||
}
|
||||
// Profiler.EndSample();
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 查找坐标落位的三角形，空说明在阻挡区内
|
||||
/// </summary>
|
||||
/// <param name="position"></param>
|
||||
/// <returns></returns>
|
||||
public NavTriangle GetTriangle(in LVector3 position)
|
||||
{
|
||||
return m_Graph.GetTriangle(position);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 查找坐标落位的三角形或是距离最近的三角形，返回最近三角形上的点
|
||||
/// </summary>
|
||||
/// <param name="position"></param>
|
||||
/// <param name="outNearestPosition"></param>
|
||||
/// <returns></returns>
|
||||
public NavTriangle GetTriangleOrNearestTriangle(in LVector3 position, out LVector3 outNearestPosition)
|
||||
{
|
||||
return m_Graph.GetTriangleOrNeareastTriangle(position, out outNearestPosition);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 查询距离指定位置最近的三角形网格边
|
||||
/// </summary>
|
||||
/// <param name="navTriangle">三角形</param>
|
||||
/// <param name="position">位置</param>
|
||||
/// <param name="filter">边查找过滤</param>
|
||||
/// <returns></returns>
|
||||
public TriangleEdge GetNearestEdge(NavTriangle navTriangle, in LVector3 position, EnumEdgeFilter filter)
|
||||
{
|
||||
return filter switch
|
||||
{
|
||||
EnumEdgeFilter.kFilterShared => m_Graph.GetNearestSharedEdge(navTriangle, in position),
|
||||
EnumEdgeFilter.kFilterIsolated => m_Graph.GetNeareastIsolatedEdge(navTriangle, in position),
|
||||
EnumEdgeFilter.kFilterAll => m_Graph.GetNearestEdge(navTriangle, in position),
|
||||
_ => null,
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据索引获取三角形网格
|
||||
/// </summary>
|
||||
/// <param name="index"></param>
|
||||
/// <returns></returns>
|
||||
public NavTriangle GetTriangle(int index)
|
||||
{
|
||||
return m_Graph.GetNode(index);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取全部的导航三角形网格数据
|
||||
/// </summary>
|
||||
/// <returns></returns>
|
||||
public List<NavTriangle> GetNavTriangles()
|
||||
{
|
||||
return m_Graph.Triangles;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 0de2ec35d68a342c292a00bb7f43e327
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,8 @@
|
||||
fileFormatVersion: 2
|
||||
guid: b8bb5a4c219ae4dcbbd5a7b6ff3b6b85
|
||||
folderAsset: yes
|
||||
DefaultImporter:
|
||||
externalObjects: {}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,62 @@
|
||||
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 描述图数据结构上的一条路径
|
||||
/// </summary>
|
||||
/// <typeparam name="TEdge">路径节点描述：可以是图数据结构中邻接边或节点</typeparam>
|
||||
public class DefaultGraphPath<TEdge> : IGraphPath<TEdge>
|
||||
{
|
||||
/// <summary>
|
||||
/// 路径节点的集合，节点可以用图的邻接边或节点描述
|
||||
/// </summary>
|
||||
public List<TEdge> nodes;
|
||||
|
||||
/// <summary>
|
||||
/// Creates a {@code DefaultGraphPath} with no nodes.
|
||||
/// </summary>
|
||||
public DefaultGraphPath() : this(new List<TEdge>()) { }
|
||||
|
||||
/// <summary>
|
||||
/// Creates a {@code DefaultGraphPath} with the given capacity and no nodes.
|
||||
/// </summary>
|
||||
/// <param name="capacity"></param>
|
||||
public DefaultGraphPath(int capacity) : this(new List<TEdge>(capacity)) { }
|
||||
|
||||
/// <summary>
|
||||
/// Creates a {@code DefaultGraphPath} with the given nodes.
|
||||
/// </summary>
|
||||
/// <param name="nodes"></param>
|
||||
public DefaultGraphPath(List<TEdge> nodes)
|
||||
{
|
||||
this.nodes = nodes;
|
||||
}
|
||||
|
||||
public void Clear()
|
||||
{
|
||||
nodes.Clear();
|
||||
}
|
||||
|
||||
public int GetCount()
|
||||
{
|
||||
return nodes.Count;
|
||||
}
|
||||
|
||||
public void Add(TEdge node)
|
||||
{
|
||||
nodes.Add(node);
|
||||
}
|
||||
|
||||
public TEdge Get(int index)
|
||||
{
|
||||
return nodes[index];
|
||||
}
|
||||
|
||||
public void Reverse()
|
||||
{
|
||||
nodes.Reverse();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 2c513670f2d7d435ab65402918015090
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,48 @@
|
||||
|
||||
using System.Collections.Generic;
|
||||
using JNGame.Math;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
public class EdgePoint
|
||||
{
|
||||
/**
|
||||
* Triangle which must be crossed to reach the next path point.
|
||||
*/
|
||||
public NavTriangle toNode;
|
||||
|
||||
/**
|
||||
* Triangle which was crossed to reach this point.
|
||||
*/
|
||||
public NavTriangle fromNode;
|
||||
|
||||
/**
|
||||
* Path edges connected to this point. Can be used for spline generation at some
|
||||
* point perhaps...
|
||||
*/
|
||||
public List<TriangleEdge> connectingEdges = new List<TriangleEdge>(8);
|
||||
|
||||
/**
|
||||
* The point where the path crosses an edge.
|
||||
*/
|
||||
public LVector3 point;
|
||||
|
||||
public EdgePoint()
|
||||
{
|
||||
point = LVector3.Zero;
|
||||
toNode = null;
|
||||
}
|
||||
|
||||
public void Set(in LVector3 point, NavTriangle toNode)
|
||||
{
|
||||
this.point = point;
|
||||
this.toNode = toNode;
|
||||
}
|
||||
|
||||
public EdgePoint(in LVector3 point, NavTriangle toNode)
|
||||
{
|
||||
this.point = point;
|
||||
this.toNode = toNode;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: a8be0d16ec4044fbc8b80bb7a236024b
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,75 @@
|
||||
using JNGame.Math;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 漏斗
|
||||
/// </summary>
|
||||
public class Funnel
|
||||
{
|
||||
/// <summary>
|
||||
/// 漏斗点，路径的起点或拐点
|
||||
/// </summary>
|
||||
public LVector3 pivot = LVector3.Zero;
|
||||
/// <summary>
|
||||
/// 漏斗的边界面A
|
||||
/// </summary>
|
||||
public Plane leftPlane = new Plane(); // 左平面，高度为y轴
|
||||
/// <summary>
|
||||
/// 漏斗的边界面B
|
||||
/// </summary>
|
||||
public Plane rightPlane = new Plane();
|
||||
/// <summary>
|
||||
/// 漏斗的边界面A与三角形平面的交点
|
||||
/// </summary>
|
||||
public LVector3 leftPortal = LVector3.Zero; // 路径左顶点，
|
||||
/// <summary>
|
||||
/// 漏斗的边界面B与三角形平面的交点
|
||||
/// </summary>
|
||||
public LVector3 rightPortal = LVector3.Zero; // 路径右顶点
|
||||
|
||||
public void SetLeftPlane(in LVector3 pivot, in LVector3 leftEdgeVertex)
|
||||
{
|
||||
leftPlane.Set(pivot, pivot + LVector3.Up, leftEdgeVertex);
|
||||
leftPortal = leftEdgeVertex;
|
||||
}
|
||||
|
||||
public void SetRightPlane(in LVector3 pivot, in LVector3 rightEdgeVertex)
|
||||
{
|
||||
rightPlane.Set(pivot, pivot + LVector3.Up, rightEdgeVertex); // 高度
|
||||
rightPlane.normal = -rightPlane.normal; // 平面方向取反
|
||||
rightPlane.distFromOrigin = -rightPlane.distFromOrigin;
|
||||
rightPortal = rightEdgeVertex;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 设置漏斗的两个边面
|
||||
/// </summary>
|
||||
/// <param name="pivot">漏斗点</param>
|
||||
/// <param name="edge">三角形的共享边</param>
|
||||
public void SetPlanes(in LVector3 pivot, TriangleEdge edge)
|
||||
{
|
||||
SetLeftPlane(pivot, edge.vertexB);
|
||||
SetRightPlane(pivot, edge.vertexA);
|
||||
}
|
||||
|
||||
public EnumPlaneSide SideLeftPlane(in LVector3 point)
|
||||
{
|
||||
return leftPlane.TestPoint(point);
|
||||
}
|
||||
|
||||
public EnumPlaneSide SideRightPlane(in LVector3 point)
|
||||
{
|
||||
return rightPlane.TestPoint(point);
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
pivot = LVector3.Zero;
|
||||
leftPlane.Reset();
|
||||
rightPlane.Reset();
|
||||
leftPortal = LVector3.Zero;
|
||||
rightPortal = LVector3.Zero;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 55922bc76371f4895b885c6663d8d181
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,44 @@
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 描述一条路径的接口
|
||||
/// </summary>
|
||||
/// <typeparam name="TEdge">路径节点描述：可以是图数据结构中邻接边或节点</typeparam>
|
||||
public interface IGraphPath<TEdge>
|
||||
{
|
||||
/// <summary>
|
||||
/// 返回路径节点总数
|
||||
/// <para> Returns the number of items of this path. </para>
|
||||
/// </summary>
|
||||
/// <returns></returns>
|
||||
int GetCount();
|
||||
|
||||
/// <summary>
|
||||
/// 获取指定索引路径节点
|
||||
/// <para> Returns the item of this path at the given index. </para>
|
||||
/// </summary>
|
||||
/// <param name="index"></param>
|
||||
/// <returns></returns>
|
||||
TEdge Get(int index);
|
||||
|
||||
/// <summary>
|
||||
/// 添加一个路径节点到列表尾部
|
||||
/// <para> Adds an item at the end of this path. </para>
|
||||
/// </summary>
|
||||
/// <param name="node"></param>
|
||||
void Add(TEdge node);
|
||||
|
||||
/// <summary>
|
||||
/// 清理所有的路径节点
|
||||
/// <para> Clears this path. </para>
|
||||
/// </summary>
|
||||
void Clear();
|
||||
|
||||
/// <summary>
|
||||
/// 翻转路径节点(节点A->节点B 变为 节点B->节点A)
|
||||
/// <para> Reverses this path. </para>
|
||||
/// </summary>
|
||||
void Reverse();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 3b81b57e2d2d1466eadfe89f96171b11
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,25 @@
|
||||
using JNGame.Math;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// <para> A {@code Heuristic} generates estimates of the cost to move from a given node to the goal. </para>
|
||||
/// With a heuristic function pathfinding algorithms can choose the node that is most likely to lead to the optimal path. The
|
||||
/// notion of "most likely" is controlled by a heuristic. If the heuristic is accurate, then the algorithm will be efficient. If
|
||||
/// the heuristic is terrible, then it can perform even worse than other algorithms that don't use any heuristic function such as
|
||||
/// Dijkstra.
|
||||
/// </summary>
|
||||
/// <typeparam name="TNode">Type of node</typeparam>
|
||||
public interface IHeuristic<TNode>
|
||||
{
|
||||
/// <summary>
|
||||
/// 启发式算法估价函数，计算从当前点到目标点的估价
|
||||
/// <para>A*启发函数的g(n)</para>
|
||||
/// <para>Calculates an estimated cost to reach the goal node from the given node.</para>
|
||||
/// </summary>
|
||||
/// <param name="node">the start node</param>
|
||||
/// <param name="endNode">the end node</param>
|
||||
/// <returns>the estimated cost</returns>
|
||||
LFloat Estimate(TNode node, TNode endNode);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 736f27d5a74ce43f8923b8bc96b225b3
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,30 @@
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 路径搜索器的接口定义
|
||||
/// </summary>
|
||||
/// <typeparam name="TNode">图数据结构的节点</typeparam>
|
||||
public interface IPathFinder<TNode>
|
||||
{
|
||||
/// <summary>
|
||||
/// 搜索从startNode到endNode的一条路径
|
||||
/// </summary>
|
||||
/// <param name="startNode">起始节点</param>
|
||||
/// <param name="endNode">目标节点</param>
|
||||
/// <param name="heuristic">启发函数</param>
|
||||
/// <param name="outPath">输出结果路径：路径用图的邻接边描述</param>
|
||||
/// <returns>是否成功搜索到一条路径</returns>
|
||||
bool SearchPath(TNode startNode, TNode endNode, IHeuristic<TNode> heuristic, IGraphPath<IConnection<TNode>> outPath);
|
||||
|
||||
/// <summary>
|
||||
/// 搜索从startNode到endNode的一条路径
|
||||
/// </summary>
|
||||
/// <param name="startNode">图中的起始节点</param>
|
||||
/// <param name="endNode">图中的目标节点</param>
|
||||
/// <param name="heuristic">启发函数</param>
|
||||
/// <param name="outPath">输出结果路径：路径用图的节点描述</param>
|
||||
/// <returns>是否成功搜索到一条路径</returns>
|
||||
bool SearchNodePath(TNode startNode, TNode endNode, IHeuristic<TNode> heuristic, IGraphPath<TNode> outPath);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 09b9a4d779c64410b8720c88bd5168e5
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,366 @@
|
||||
|
||||
using System.Collections.Generic;
|
||||
using JNGame.Math;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 基于A*算法的路径搜索器
|
||||
/// </summary>
|
||||
/// <typeparam name="TNode">图的节点类型</typeparam>
|
||||
public class IndexedAStarPathFinder<TNode> : IPathFinder<TNode>
|
||||
{
|
||||
/// <summary>
|
||||
/// 节点搜索状态 - 未遍历
|
||||
/// </summary>
|
||||
public const int UNVISITED = 0;
|
||||
/// <summary>
|
||||
/// 节点搜索状态 - 处理OpenList中
|
||||
/// </summary>
|
||||
public const int OPEN = 1;
|
||||
/// <summary>
|
||||
/// 节点搜索状态 - 处理完成
|
||||
/// </summary>
|
||||
public const int CLOSED = 2;
|
||||
|
||||
/// <summary>
|
||||
/// 寻路使用的数据源
|
||||
/// <para>以TNode为节点构成的图数据结构，包含TNode节点集合，以及TNode的邻接关系集合</para>
|
||||
/// </summary>
|
||||
private readonly IIndexedGraph<TNode> m_Graph;
|
||||
/// <summary>
|
||||
/// 节点的搜索记录集合，数组长度为图的节点总数
|
||||
/// </summary>
|
||||
private readonly NodeRecord<TNode>[] m_NodeRecords;
|
||||
/// <summary>
|
||||
/// OpenList，用二叉堆数据结构描述
|
||||
/// </summary>
|
||||
private readonly GenericBinaryHeap<NodeRecord<TNode>> m_OpenList;
|
||||
|
||||
/// <summary>
|
||||
/// 搜索ID，每次搜索自增，用于标记节点搜索记录
|
||||
/// The unique ID for each search run. Used to mark nodes.
|
||||
/// </summary>
|
||||
private int m_SearchId = 0;
|
||||
|
||||
/// <summary>
|
||||
/// 当前搜索的节点
|
||||
/// </summary>
|
||||
private NodeRecord<TNode> m_Current;
|
||||
|
||||
/// <summary>
|
||||
/// 性能评估指标，与搜索过程无关，记录A*的搜索次数，以此评估性能
|
||||
/// </summary>
|
||||
public Metrics metrics;
|
||||
|
||||
/// <summary>
|
||||
/// 基于A*算法的路径搜索器构造
|
||||
/// </summary>
|
||||
/// <param name="graph">路径搜索器使用的数据源</param>
|
||||
/// <param name="calculateMetrics">是否开启性能评估，默认:false</param>
|
||||
public IndexedAStarPathFinder(IIndexedGraph<TNode> graph, bool calculateMetrics = false)
|
||||
{
|
||||
m_Graph = graph;
|
||||
m_NodeRecords = new NodeRecord<TNode>[graph.GetNodeCount()];
|
||||
m_OpenList = new GenericBinaryHeap<NodeRecord<TNode>>();
|
||||
if (calculateMetrics)
|
||||
{
|
||||
metrics = new Metrics();
|
||||
}
|
||||
}
|
||||
|
||||
public bool SearchPath(TNode startNode, TNode endNode, IHeuristic<TNode> heuristic, IGraphPath<IConnection<TNode>> outPath)
|
||||
{
|
||||
// Perform AStar
|
||||
bool found = Search(startNode, endNode, heuristic);
|
||||
if (found)
|
||||
{
|
||||
// Create a path made of connections
|
||||
GeneratePath(startNode, outPath);
|
||||
}
|
||||
return found;
|
||||
}
|
||||
|
||||
public bool SearchNodePath(TNode startNode, TNode endNode, IHeuristic<TNode> heuristic, IGraphPath<TNode> outPath)
|
||||
{
|
||||
// Perform AStar
|
||||
bool found = Search(startNode, endNode, heuristic);
|
||||
if (found)
|
||||
{
|
||||
// Create a path made of nodes
|
||||
GenerateNodePath(startNode, outPath);
|
||||
}
|
||||
return found;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 开始执行路径搜索
|
||||
/// </summary>
|
||||
/// <param name="startNode">图上的起始节点</param>
|
||||
/// <param name="endNode">图上的目标节点</param>
|
||||
/// <param name="heuristic">启发函数</param>
|
||||
/// <returns>是否存在路径</returns>
|
||||
protected bool Search(TNode startNode, TNode endNode, IHeuristic<TNode> heuristic)
|
||||
{
|
||||
InitSearch(startNode, endNode, heuristic);
|
||||
// Iterate through processing each node
|
||||
do
|
||||
{
|
||||
// Retrieve the node with smallest estimated total cost from the open list
|
||||
m_Current = m_OpenList.Pop();
|
||||
m_Current.category = CLOSED;
|
||||
// Terminate if we reached the goal node
|
||||
if (m_Current.node.Equals(endNode)) { return true; }
|
||||
// 遍历当前节点的邻接节点，选取评估值
|
||||
VisitChildren(endNode, heuristic);
|
||||
}
|
||||
while (m_OpenList.size > 0);
|
||||
// We've run out of nodes without finding the goal, so there's no solution
|
||||
return false;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 初始化路径搜索
|
||||
/// </summary>
|
||||
/// <param name="startNode">图上的起始节点</param>
|
||||
/// <param name="endNode">图上的目标节点</param>
|
||||
/// <param name="heuristic">启发函数</param>
|
||||
protected void InitSearch(TNode startNode, TNode endNode, IHeuristic<TNode> heuristic)
|
||||
{
|
||||
// 清空性能指标
|
||||
metrics?.Reset();
|
||||
// 搜索ID自增
|
||||
if (++m_SearchId < 0) { m_SearchId = 1; }
|
||||
// 初始化OpenList
|
||||
m_OpenList.Clear();
|
||||
// Initialize the record for the start node and add it to the open list
|
||||
NodeRecord<TNode> startRecord = GetNodeRecord(startNode);
|
||||
startRecord.node = startNode;
|
||||
startRecord.connection = null; // 起点的连接口设置为null
|
||||
startRecord.costSoFar = 0.ToLFloat();
|
||||
// 起始节点添加到OpenList中
|
||||
AddToOpenList(startRecord, heuristic.Estimate(startNode, endNode));
|
||||
m_Current = null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 遍历子节点
|
||||
/// </summary>
|
||||
/// <param name="endNode">路径的目标节点</param>
|
||||
/// <param name="heuristic">启发函数</param>
|
||||
protected void VisitChildren(TNode endNode, IHeuristic<TNode> heuristic)
|
||||
{
|
||||
// Get current node's outgoing connections
|
||||
List<IConnection<TNode>> connections = m_Graph.GetConnections(m_Current.node);
|
||||
// Loop through each connection in turn
|
||||
for (int i = 0; i < connections.Count; i++)
|
||||
{
|
||||
if (metrics != null) { ++metrics.VisitedNodes; }
|
||||
|
||||
IConnection<TNode> connection = connections[i];
|
||||
// Get the cost estimate for the node
|
||||
TNode node = connection.GetToNode(); // 邻接边连接的目标节点
|
||||
LFloat nodeCost = m_Current.costSoFar + connection.GetCost(); // 节点到起点的消耗(g(n))
|
||||
LFloat nodeHeuristic;
|
||||
NodeRecord<TNode> nodeRecord = GetNodeRecord(node);
|
||||
if (nodeRecord.category == CLOSED)
|
||||
{ // The node is closed
|
||||
// If we didn't find a shorter route, skip 已经是消耗最小的目标点
|
||||
if (nodeRecord.costSoFar <= nodeCost) { continue; }
|
||||
// We can use the node's old cost values to calculate its heuristic
|
||||
// without calling the possibly expensive heuristic function
|
||||
nodeHeuristic = nodeRecord.GetEstimatedTotalCost() - nodeRecord.costSoFar;
|
||||
}
|
||||
else if (nodeRecord.category == OPEN)
|
||||
{ // The node is open
|
||||
// If our route is no better, then skip
|
||||
if (nodeRecord.costSoFar <= nodeCost) { continue; }
|
||||
// Remove it from the open list (it will be re-added with the new cost)
|
||||
m_OpenList.Remove(nodeRecord);
|
||||
// We can use the node's old cost values to calculate its heuristic
|
||||
// without calling the possibly expensive heuristic function
|
||||
nodeHeuristic = nodeRecord.GetEstimatedTotalCost() - nodeRecord.costSoFar;
|
||||
}
|
||||
else
|
||||
{ // the node is unvisited
|
||||
// We'll need to calculate the heuristic value using the function,
|
||||
// since we don't have a node record with a previously calculated value
|
||||
nodeHeuristic = heuristic.Estimate(node, endNode);
|
||||
}
|
||||
// Update node record's cost and connection
|
||||
nodeRecord.costSoFar = nodeCost;
|
||||
nodeRecord.connection = connection;
|
||||
// Add it to the open list with the estimated total cost
|
||||
AddToOpenList(nodeRecord, nodeCost + nodeHeuristic);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 生成用图的邻接边描述的路径
|
||||
/// </summary>
|
||||
/// <param name="startNode">图起点</param>
|
||||
/// <param name="outPath">输出的路径</param>
|
||||
protected void GeneratePath(TNode startNode, IGraphPath<IConnection<TNode>> outPath)
|
||||
{
|
||||
// outPath.clear();
|
||||
// Work back along the path, accumulating connections
|
||||
while (!m_Current.node.Equals(startNode))
|
||||
{ // 沿着变成搜索的连接口，搜索到起点
|
||||
outPath.Add(m_Current.connection);
|
||||
m_Current = m_NodeRecords[m_Graph.GetIndex(m_Current.connection.GetFromNode())];
|
||||
}
|
||||
// Reverse the path
|
||||
outPath.Reverse();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 生成用图的节点描述的路径
|
||||
/// </summary>
|
||||
/// <param name="startNode">图起点</param>
|
||||
/// <param name="outPath">输出的路径</param>
|
||||
protected void GenerateNodePath(TNode startNode, IGraphPath<TNode> outPath)
|
||||
{
|
||||
// outPath.clear();
|
||||
// Work back along the path, accumulating nodes
|
||||
while (m_Current.connection != null)
|
||||
{ // 沿着变成搜索的连接口，搜索到空连接
|
||||
outPath.Add(m_Current.node);
|
||||
m_Current = m_NodeRecords[m_Graph.GetIndex(m_Current.connection.GetFromNode())];
|
||||
}
|
||||
// 额外添加起点
|
||||
outPath.Add(startNode);
|
||||
// Reverse the path
|
||||
outPath.Reverse();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 添加节点到OpenList，节点搜索记录状态设置为Open
|
||||
/// </summary>
|
||||
/// <param name="nodeRecord">节点关联的搜索记录</param>
|
||||
/// <param name="estimatedTotalCost">当前节点的到路径终点的总消耗(g(n)+h(n))</param>
|
||||
protected void AddToOpenList(NodeRecord<TNode> nodeRecord, LFloat estimatedTotalCost)
|
||||
{
|
||||
m_OpenList.Add(nodeRecord, estimatedTotalCost);
|
||||
// 节点搜索状态更新为OPEN
|
||||
nodeRecord.category = OPEN;
|
||||
if (metrics != null)
|
||||
{
|
||||
// 更新性能指标
|
||||
++metrics.OpenListAdditions;
|
||||
if (m_OpenList.size > metrics.OpenListPeak)
|
||||
{
|
||||
metrics.OpenListPeak = m_OpenList.size;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取指定图节点的搜索记录
|
||||
/// </summary>
|
||||
/// <param name="node">图节点</param>
|
||||
/// <returns>图节点关联的搜索记录</returns>
|
||||
protected NodeRecord<TNode> GetNodeRecord(TNode node)
|
||||
{
|
||||
int index = m_Graph.GetIndex(node);
|
||||
NodeRecord<TNode> nr = m_NodeRecords[index];
|
||||
if (nr != null)
|
||||
{
|
||||
// 不同批次的搜索，重置节点状态和关联的搜索ID
|
||||
if (nr.searchId != m_SearchId)
|
||||
{
|
||||
nr.category = UNVISITED;
|
||||
nr.searchId = m_SearchId;
|
||||
}
|
||||
return nr;
|
||||
}
|
||||
// 从未搜索过，创建新的节点搜索记录
|
||||
nr = m_NodeRecords[index] = new NodeRecord<TNode>();
|
||||
nr.node = node;
|
||||
nr.searchId = m_SearchId;
|
||||
return nr;
|
||||
}
|
||||
|
||||
#region 二叉堆的节点数据结构定义
|
||||
/// <summary>
|
||||
/// 图节点搜索记录
|
||||
/// </summary>
|
||||
/// <typeparam name="TN"></typeparam>
|
||||
public class NodeRecord<TN> : BinaryHeapNode
|
||||
{
|
||||
/// <summary>
|
||||
/// 关联的图节点
|
||||
/// <para>The reference to the node.</para>
|
||||
/// </summary>
|
||||
public TN node;
|
||||
|
||||
/// <summary>
|
||||
/// 本次搜索走的邻接口
|
||||
/// <para>The incoming connection to the node.</para>
|
||||
/// </summary>
|
||||
public IConnection<TN> connection;
|
||||
|
||||
/// <summary>
|
||||
/// 当前节点到起点的实际消耗，A*启发函数的g(n)
|
||||
/// <para>The actual cost from the start node.</para>
|
||||
/// </summary>
|
||||
public LFloat costSoFar;
|
||||
|
||||
/// <summary>
|
||||
/// 节点的状态，UNVISITED / OPEN / OPEN 三者之一
|
||||
/// <para>The node category: {@link #UNVISITED}, {@link #OPEN} or {@link #OPEN}.</para>
|
||||
/// </summary>
|
||||
public int category;
|
||||
|
||||
/// <summary>
|
||||
/// 关联的搜索ID
|
||||
/// <para>ID of the current search.</para>
|
||||
/// </summary>
|
||||
public int searchId;
|
||||
|
||||
/// <summary>
|
||||
/// Creates a {@code NodeRecord}.
|
||||
/// </summary>
|
||||
public NodeRecord() : base(LFloat.Zero) { }
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前节点总的消耗：f(n) = g(n) + h(n)
|
||||
/// </summary>
|
||||
/// <returns>Returns the estimated total cost.</returns>
|
||||
public LFloat GetEstimatedTotalCost()
|
||||
{
|
||||
return value;
|
||||
}
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region 寻路指标定义，与寻路过程无关
|
||||
/// <summary>
|
||||
/// 寻路指标，评测寻路性能使用
|
||||
/// </summary>
|
||||
public class Metrics
|
||||
{
|
||||
/// <summary>
|
||||
/// 遍历节点的次数
|
||||
/// </summary>
|
||||
public int VisitedNodes;
|
||||
/// <summary>
|
||||
/// OpenList添加的次数
|
||||
/// </summary>
|
||||
public int OpenListAdditions;
|
||||
/// <summary>
|
||||
/// OpenList中最大的待处理节点数
|
||||
/// </summary>
|
||||
public int OpenListPeak;
|
||||
|
||||
public Metrics() { }
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
VisitedNodes = 0;
|
||||
OpenListAdditions = 0;
|
||||
OpenListPeak = 0;
|
||||
}
|
||||
}
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 5243578b0e4974e358512ec1e0c2ae05
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,317 @@
|
||||
|
||||
using System;
|
||||
using System.Text;
|
||||
using JNGame.Math;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 二叉堆的节点基类
|
||||
/// </summary>
|
||||
public class BinaryHeapNode
|
||||
{
|
||||
/// <summary>
|
||||
/// 节点排序比较值
|
||||
/// </summary>
|
||||
public LFloat value;
|
||||
|
||||
/// <summary>
|
||||
/// 节点索引
|
||||
/// </summary>
|
||||
public int index;
|
||||
|
||||
public BinaryHeapNode(LFloat value)
|
||||
{
|
||||
this.value = value;
|
||||
}
|
||||
public override string ToString()
|
||||
{
|
||||
return value.ToString();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 二叉堆数据结构
|
||||
/// <para>常规二叉堆不从索引0开始，而是从索引1开始</para>
|
||||
/// <para>索引为n的左孩子索引是2n，索引为n的右孩子索引是2n+1，索引为n的父节点索引是n/2</para>
|
||||
/// <para>本实现从索引0开始</para>
|
||||
/// <para>本实现索引为n的左孩子索引是2n+1，索引为n的右孩子索引是2(n+1)，索引为n的父节点索引是(n-1)/2</para>
|
||||
/// </summary>
|
||||
/// <typeparam name="T">二叉堆中的数据节点</typeparam>
|
||||
public class GenericBinaryHeap<T> where T : BinaryHeapNode
|
||||
{
|
||||
/// <summary>
|
||||
/// 二叉堆中的有效节点总数，同时是当前的可插入位置
|
||||
/// </summary>
|
||||
public int size;
|
||||
/// <summary>
|
||||
/// 二叉堆节点集合，用数组存储
|
||||
/// </summary>
|
||||
private BinaryHeapNode[] m_Nodes;
|
||||
/// <summary>
|
||||
/// 是否是最大堆
|
||||
/// <para>最大堆:任意节点权值不小于其子节点</para>
|
||||
/// <para>最小堆:任意节点权值不大于于其子节点</para>
|
||||
/// </summary>
|
||||
private readonly bool m_IsMaxHeap;
|
||||
|
||||
/// <summary>
|
||||
/// 二叉堆构造器
|
||||
/// </summary>
|
||||
public GenericBinaryHeap() : this(16, false) { }
|
||||
|
||||
/// <summary>
|
||||
/// 二叉堆构造器
|
||||
/// </summary>
|
||||
/// <param name="capacity">初始容量</param>
|
||||
/// <param name="isMaxHeap">是否是最大堆</param>
|
||||
public GenericBinaryHeap(int capacity, bool isMaxHeap)
|
||||
{
|
||||
m_IsMaxHeap = isMaxHeap;
|
||||
m_Nodes = new BinaryHeapNode[capacity];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 添加节点到二叉堆
|
||||
/// </summary>
|
||||
/// <param name="node"></param>
|
||||
/// <returns></returns>
|
||||
public T Add(T node)
|
||||
{
|
||||
// Expand if necessary.
|
||||
if (size == m_Nodes.Length)
|
||||
{
|
||||
BinaryHeapNode[] newNodes = new BinaryHeapNode[size << 1];
|
||||
Array.Copy(m_Nodes, 0, newNodes, 0, size);
|
||||
m_Nodes = newNodes;
|
||||
}
|
||||
// Insert at end.
|
||||
node.index = size;
|
||||
m_Nodes[size] = node;
|
||||
// bubble up
|
||||
Up(size++);
|
||||
return node;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 添加节点到二叉堆，并设置节点的排序权重为value
|
||||
/// </summary>
|
||||
/// <param name="node">添加到二叉堆的节点</param>
|
||||
/// <param name="value">节点的排序权重</param>
|
||||
/// <returns></returns>
|
||||
public T Add(T node, LFloat value)
|
||||
{
|
||||
node.value = value;
|
||||
return Add(node);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取二叉堆顶的节点
|
||||
/// </summary>
|
||||
/// <returns></returns>
|
||||
/// <exception cref="Exception"></exception>
|
||||
public T Peek()
|
||||
{
|
||||
if (size == 0)
|
||||
throw new Exception("The heap is empty.");
|
||||
return (T)m_Nodes[0];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 弹出二叉堆顶的节点
|
||||
/// </summary>
|
||||
/// <returns></returns>
|
||||
public T Pop()
|
||||
{
|
||||
return Remove(0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 弹出指定的二叉堆中节点
|
||||
/// </summary>
|
||||
/// <param name="node">移除的节点</param>
|
||||
/// <returns></returns>
|
||||
public T Remove(T node)
|
||||
{
|
||||
return Remove(node.index);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 移除二叉堆指定索引位置的节点
|
||||
/// </summary>
|
||||
/// <param name="index">移除位置索引</param>
|
||||
/// <returns></returns>
|
||||
private T Remove(int index)
|
||||
{
|
||||
BinaryHeapNode[] nodes = m_Nodes;
|
||||
BinaryHeapNode removed = nodes[index];
|
||||
// 用最后一个位置的节点填充移除的位置
|
||||
nodes[index] = nodes[--size];
|
||||
nodes[size] = null;
|
||||
if (size > 0 && index < size)
|
||||
{
|
||||
Down(index);
|
||||
}
|
||||
return (T)removed;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 清空二叉堆
|
||||
/// TODO 优化，可以不用对nodes进行清零
|
||||
/// </summary>
|
||||
public void Clear()
|
||||
{
|
||||
for (int i = 0, n = size; i < n; ++i)
|
||||
{
|
||||
m_Nodes[i] = null;
|
||||
}
|
||||
size = 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 修改二叉堆中一个节点的排序权重
|
||||
/// </summary>
|
||||
/// <param name="node">二叉堆中的一个节点</param>
|
||||
/// <param name="value">新的排序值</param>
|
||||
public void SetValue(T node, LFloat value)
|
||||
{
|
||||
LFloat oldValue = node.value;
|
||||
node.value = value;
|
||||
if ((value < oldValue) ^ m_IsMaxHeap)
|
||||
{
|
||||
Up(node.index);
|
||||
}
|
||||
else
|
||||
{
|
||||
Down(node.index);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 上浮索引位置的节点，直至满足二叉堆性质
|
||||
/// </summary>
|
||||
/// <param name="index">上浮的节点索引</param>
|
||||
private void Up(int index)
|
||||
{
|
||||
BinaryHeapNode[] nodes = this.m_Nodes;
|
||||
BinaryHeapNode node = nodes[index];
|
||||
LFloat value = node.value;
|
||||
while (index > 0)
|
||||
{
|
||||
// 获取父节点
|
||||
int parentIndex = (index - 1) >> 1;
|
||||
BinaryHeapNode parent = nodes[parentIndex];
|
||||
// 当前节点的权值与父节点比较，是否满足二叉堆性质(最大堆或最小堆)
|
||||
if ((value < parent.value) ^ m_IsMaxHeap)
|
||||
{ // 不满足，则将父节点的值写入到当前节点的位置，当前节点索引该为父节点索引
|
||||
nodes[index] = parent;
|
||||
parent.index = index;
|
||||
index = parentIndex;
|
||||
}
|
||||
else
|
||||
{
|
||||
// 满足，退出上浮
|
||||
break;
|
||||
}
|
||||
}
|
||||
// 当前节点写入正确位置，更新节点的索引
|
||||
nodes[index] = node;
|
||||
node.index = index;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 节点下沉，寻找符合二叉堆性质的
|
||||
/// </summary>
|
||||
/// <param name="index"></param>
|
||||
private void Down(int index)
|
||||
{
|
||||
BinaryHeapNode[] nodes = m_Nodes;
|
||||
int size = this.size;
|
||||
BinaryHeapNode node = nodes[index];
|
||||
LFloat value = node.value;
|
||||
// 当前节点循环下沉，寻找符合二叉堆性质的索引位置
|
||||
while (true)
|
||||
{
|
||||
// no child, finished
|
||||
int leftIndex = 1 + (index << 1);
|
||||
if (leftIndex >= size)
|
||||
{
|
||||
break;
|
||||
}
|
||||
// Always have a left child.
|
||||
BinaryHeapNode leftNode = nodes[leftIndex];
|
||||
LFloat leftValue = leftNode.value;
|
||||
// May have a right child.
|
||||
int rightIndex = leftIndex + 1;
|
||||
BinaryHeapNode rightNode;
|
||||
LFloat rightValue;
|
||||
if (rightIndex >= size)
|
||||
{
|
||||
rightNode = null;
|
||||
rightValue = m_IsMaxHeap ? LFloat.FLT_MIN : LFloat.FLT_MAX;
|
||||
}
|
||||
else
|
||||
{
|
||||
rightNode = nodes[rightIndex];
|
||||
rightValue = rightNode.value;
|
||||
}
|
||||
// The smallest of the three values is the parent.
|
||||
if ((leftValue < rightValue) ^ m_IsMaxHeap)
|
||||
{ // 最大堆，左节点为两个子节点的大值 || 最小堆，左节点为两个子节点的小值
|
||||
if (leftValue == value || ((leftValue > value) ^ m_IsMaxHeap))
|
||||
{ // 左节点与当前节点(父节点)权值相同，或 最大堆:左节点为权值小的 | 最小堆:左节点为权值大的。退出，下沉完成
|
||||
break;
|
||||
}
|
||||
// 最小堆:左节点为权值最小的 || 最大堆:左节点为权值最大的，交换当前(父)节点与左节点的位置
|
||||
nodes[index] = leftNode;
|
||||
leftNode.index = index;
|
||||
index = leftIndex;
|
||||
}
|
||||
else
|
||||
{ // 最大堆，右节点为两个子节点的大值 || 最小堆，右节点为两个子节点的小值
|
||||
if (rightValue == value || ((rightValue > value) ^ m_IsMaxHeap))
|
||||
{ // 右节点与当前节点(父节点)权值相同，或 最大堆右节点为权值小的 | 最小堆:右节点为权值大的。退出，下沉完成
|
||||
break;
|
||||
}
|
||||
// 最小堆:右节点为权值最小的 || 最大堆:右节点为权值最大的，交换当前(父)节点与右节点的位置
|
||||
nodes[index] = rightNode;
|
||||
rightNode.index = index;
|
||||
index = rightIndex;
|
||||
}
|
||||
}
|
||||
// 更新当前节点到最终的索引位置
|
||||
nodes[index] = node;
|
||||
node.index = index;
|
||||
}
|
||||
|
||||
public override int GetHashCode()
|
||||
{
|
||||
long h = 1;
|
||||
for (int i = 0, n = size; i < n; i++)
|
||||
{
|
||||
h = h * 31 + m_Nodes[i].value.rawValue;
|
||||
}
|
||||
return (int)h;
|
||||
}
|
||||
|
||||
public override string ToString()
|
||||
{
|
||||
if (size == 0)
|
||||
{
|
||||
return "[]";
|
||||
}
|
||||
BinaryHeapNode[] nodes = this.m_Nodes;
|
||||
StringBuilder buffer = new StringBuilder(32);
|
||||
buffer.Append('[');
|
||||
buffer.Append(nodes[0].value);
|
||||
for (int i = 1; i < size; i++)
|
||||
{
|
||||
buffer.Append(", ");
|
||||
buffer.Append(nodes[i].value);
|
||||
}
|
||||
|
||||
buffer.Append(']');
|
||||
return buffer.ToString();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 024aed145f1c9421faa2dcf00949e00d
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,38 @@
|
||||
using JNGame.Math;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 描述以三角形为图元节点的图上，从起点三角形到终点三角形的一条图上的路径。
|
||||
/// <para>路径描节点用图的邻接边描述</para>
|
||||
/// </summary>
|
||||
public class TriangleEdgeGraphPath : DefaultGraphPath<IConnection<NavTriangle>>
|
||||
{
|
||||
/// <summary>
|
||||
/// 路径的起点
|
||||
/// <para>The start point when generating a point path for this triangle path</para>
|
||||
/// </summary>
|
||||
public LVector3 startPoint;
|
||||
|
||||
/// <summary>
|
||||
/// 路径的终点
|
||||
/// <para>The end point when generating a point path for this triangle path</para>
|
||||
/// </summary>
|
||||
public LVector3 endPoint;
|
||||
|
||||
/// <summary>
|
||||
/// 路径的起始三角形
|
||||
/// </summary>
|
||||
public NavTriangle startTriangle;
|
||||
|
||||
/// <summary>
|
||||
/// 获取路径上的最后一个三角形
|
||||
/// </summary>
|
||||
/// <returns>Last triangle in the path.</returns>
|
||||
public NavTriangle GetEndTriangle()
|
||||
{
|
||||
// If the triangle path is empty, the point path will span this triangle
|
||||
return (GetCount() > 0) ? Get(GetCount() - 1).GetToNode() : startTriangle;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 274f209b145dc49a0b89a286ea968a06
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,97 @@
|
||||
using JNGame.Math;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 三角形节点寻路启发式算法
|
||||
/// </summary>
|
||||
public class TriangleHeuristic : IHeuristic<NavTriangle>
|
||||
{
|
||||
/// <summary>
|
||||
/// 三角形A的AB边中点
|
||||
/// </summary>
|
||||
private LVector3 m_MidPointAB_A = LVector3.Zero;
|
||||
/// <summary>
|
||||
/// 三角形A的BC边中点
|
||||
/// </summary>
|
||||
private LVector3 m_MidPointBC_A = LVector3.Zero;
|
||||
/// <summary>
|
||||
/// 三角形A的CA边中点
|
||||
/// </summary>
|
||||
private LVector3 m_MidPointCA_A = LVector3.Zero;
|
||||
/// <summary>
|
||||
/// 三角形B的AB边中点
|
||||
/// </summary>
|
||||
private LVector3 m_MidPointAB_B = LVector3.Zero;
|
||||
/// <summary>
|
||||
/// 三角形B的BC边中点
|
||||
/// </summary>
|
||||
private LVector3 m_MidPointBC_B = LVector3.Zero;
|
||||
/// <summary>
|
||||
/// 三角形B的CA边中点
|
||||
/// </summary>
|
||||
private LVector3 m_MidPointCA_B = LVector3.Zero;
|
||||
|
||||
/// <summary>
|
||||
/// 启发式算法估价函数：两个三角形节点三边中点的距离，获取最短的距离
|
||||
/// <para>A*启发函数的g(n)</para>
|
||||
/// </summary>
|
||||
/// <param name="node">当前节点</param>
|
||||
/// <param name="endNode">目标节点</param>
|
||||
/// <returns></returns>
|
||||
public LFloat Estimate(NavTriangle node, NavTriangle endNode)
|
||||
{
|
||||
LFloat minDst2 = LFloat.FLT_MAX;
|
||||
// 三角形A的三边中点
|
||||
m_MidPointAB_A = (node.vertexA + node.vertexB) * LFloat.Half;
|
||||
m_MidPointBC_A = (node.vertexB + node.vertexC) * LFloat.Half;
|
||||
m_MidPointCA_A = (node.vertexC + node.vertexA) * LFloat.Half;
|
||||
// 三角形B的三边中点
|
||||
m_MidPointAB_B = (endNode.vertexA + endNode.vertexB) * LFloat.Half;
|
||||
m_MidPointBC_B = (endNode.vertexB + endNode.vertexC) * LFloat.Half;
|
||||
m_MidPointCA_B = (endNode.vertexC + endNode.vertexA) * LFloat.Half;
|
||||
LFloat dst2;
|
||||
// 三角形A的边AB中点到三角形B的三边中点的距离平方
|
||||
if ((dst2 = LVector3.Distance2(m_MidPointAB_A, m_MidPointAB_B)) < minDst2)
|
||||
{
|
||||
minDst2 = dst2;
|
||||
}
|
||||
if ((dst2 = LVector3.Distance2(m_MidPointAB_A, m_MidPointBC_B)) < minDst2)
|
||||
{
|
||||
minDst2 = dst2;
|
||||
}
|
||||
if ((dst2 = LVector3.Distance2(m_MidPointAB_A, m_MidPointCA_B)) < minDst2)
|
||||
{
|
||||
minDst2 = dst2;
|
||||
}
|
||||
// 三角形A的边BC中点到三角形B的三边中点的距离平方
|
||||
if ((dst2 = LVector3.Distance2(m_MidPointBC_A, m_MidPointAB_B)) < minDst2)
|
||||
{
|
||||
minDst2 = dst2;
|
||||
}
|
||||
if ((dst2 = LVector3.Distance2(m_MidPointBC_A, m_MidPointBC_B)) < minDst2)
|
||||
{
|
||||
minDst2 = dst2;
|
||||
}
|
||||
if ((dst2 = LVector3.Distance2(m_MidPointBC_A, m_MidPointCA_B)) < minDst2)
|
||||
{
|
||||
minDst2 = dst2;
|
||||
}
|
||||
// 三角形A的边CA中点到三角形B的三边中点的距离平方
|
||||
if ((dst2 = LVector3.Distance2(m_MidPointCA_A, m_MidPointAB_B)) < minDst2)
|
||||
{
|
||||
minDst2 = dst2;
|
||||
}
|
||||
if ((dst2 = LVector3.Distance2(m_MidPointCA_A, m_MidPointBC_B)) < minDst2)
|
||||
{
|
||||
minDst2 = dst2;
|
||||
}
|
||||
if ((dst2 = LVector3.Distance2(m_MidPointCA_A, m_MidPointCA_B)) < minDst2)
|
||||
{
|
||||
minDst2 = dst2;
|
||||
}
|
||||
// 返回最短距离
|
||||
return LMath.Sqrt(minDst2);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 29ee51a11a00042b6bcd827f0d2d634f
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,476 @@
|
||||
|
||||
using System.Collections.Generic;
|
||||
using JNGame.Math;
|
||||
|
||||
namespace JNGame.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 位置平面关系枚举
|
||||
/// </summary>
|
||||
public enum EnumPlaneSide
|
||||
{
|
||||
/// <summary>
|
||||
/// 点在平面上
|
||||
/// </summary>
|
||||
OnPlane,
|
||||
/// <summary>
|
||||
/// 点在平面背面
|
||||
/// </summary>
|
||||
Back,
|
||||
/// <summary>
|
||||
/// 点在平面正面
|
||||
/// </summary>
|
||||
Front
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将图节点路径转换为一条最短路径
|
||||
/// <para>漏斗算法</para>
|
||||
/// </summary>
|
||||
public class TrianglePointPath
|
||||
{
|
||||
private readonly Plane m_CrossingPlane = new Plane(); // 横跨平面
|
||||
/// <summary>
|
||||
/// 用图边描述的路径
|
||||
/// </summary>
|
||||
private List<IConnection<NavTriangle>> m_EdgePaths; // 路径连接点
|
||||
/// <summary>
|
||||
/// 路径起点
|
||||
/// </summary>
|
||||
private LVector3 m_Start; // 起点
|
||||
/// <summary>
|
||||
/// 路径终点
|
||||
/// </summary>
|
||||
private LVector3 m_End; // 终点
|
||||
/// <summary>
|
||||
/// 起始三角形
|
||||
/// </summary>
|
||||
private NavTriangle m_StartTri; // 起始三角形
|
||||
/// <summary>
|
||||
/// 上次添加的边点
|
||||
/// </summary>
|
||||
private EdgePoint m_LastPointAdded;
|
||||
/// <summary>
|
||||
/// 最终结果路径坐标点
|
||||
/// </summary>
|
||||
private readonly List<LVector3> m_Vectors = new List<LVector3>();
|
||||
/// <summary>
|
||||
/// 最终结果边点描述
|
||||
/// </summary>
|
||||
private readonly List<EdgePoint> m_PathPoints = new List<EdgePoint>();
|
||||
|
||||
/// <summary>
|
||||
/// 路径计算的中间射线对象，每次计算清理并复用
|
||||
/// </summary>
|
||||
private readonly Ray m_TempRay = new Ray();
|
||||
/// <summary>
|
||||
/// 路径计算的中间漏斗对象，每次计算清理并复用
|
||||
/// </summary>
|
||||
private readonly Funnel m_TempFunnel = new Funnel();
|
||||
|
||||
/// <summary>
|
||||
/// 路径计算的最后一个边，每次计算清理并复用
|
||||
/// </summary>
|
||||
private TriangleEdge m_TempLastEdge = new TriangleEdge();
|
||||
|
||||
/// <summary>
|
||||
/// EdgePoint临时对象队列的初始数量
|
||||
/// </summary>
|
||||
private const int kDefaultEdgePointCount = 16;
|
||||
/// <summary>
|
||||
/// EdgePoint的临时对象缓存队列，作为对象池复用
|
||||
/// </summary>
|
||||
private readonly Queue<EdgePoint> m_TempEdgePointQueue = new Queue<EdgePoint>(kDefaultEdgePointCount);
|
||||
|
||||
public TrianglePointPath()
|
||||
{
|
||||
// 初始化一些EdgePoint的临时对象
|
||||
for (int i = 0; i < kDefaultEdgePointCount; ++i)
|
||||
{
|
||||
m_TempEdgePointQueue.Enqueue(new EdgePoint());
|
||||
}
|
||||
}
|
||||
|
||||
public void CalculateForGraphPath(TriangleEdgeGraphPath trianglePath)
|
||||
{
|
||||
Clear();
|
||||
m_EdgePaths = trianglePath.nodes;
|
||||
m_Start = trianglePath.startPoint;
|
||||
m_End = trianglePath.endPoint;
|
||||
m_StartTri = trianglePath.startTriangle;
|
||||
for (int i = 0; i < m_EdgePaths.Count; ++i)
|
||||
{
|
||||
IConnection<NavTriangle> connection = m_EdgePaths[i];
|
||||
NavTriangle to = connection.GetToNode();
|
||||
m_Vectors.Add(to.center);
|
||||
}
|
||||
m_Vectors.Add(m_End);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 通过图元中间路径转化为点路径
|
||||
/// </summary>
|
||||
/// <param name="trianglePath"></param>
|
||||
/// <param name="calculateCrossPoint"></param>
|
||||
public void CalculateForGraphPath(TriangleEdgeGraphPath trianglePath, bool calculateCrossPoint)
|
||||
{
|
||||
Clear();
|
||||
m_EdgePaths = trianglePath.nodes;
|
||||
m_Start = trianglePath.startPoint;
|
||||
m_End = trianglePath.endPoint;
|
||||
m_StartTri = trianglePath.startTriangle;
|
||||
// Check that the start point is actually inside the start triangle,
|
||||
// if not, project it to the closest triangle edge.
|
||||
// Otherwise the funnel calculation might generate spurious path segments.
|
||||
LVector3 rayOrigin = m_Start + LVector3.Up * 1000;
|
||||
// Ray ray = new Ray(rayOrigin, LVector3.Down);
|
||||
// 起始坐标从上向下的射线
|
||||
m_TempRay.origin = rayOrigin;
|
||||
m_TempRay.direction = LVector3.Down;
|
||||
if (!GeometryUtil.IntersectRayTriangle(m_TempRay, m_StartTri.vertexA, m_StartTri.vertexB, m_StartTri.vertexC, out var ss))
|
||||
{
|
||||
LFloat minDst = LFloat.FLT_MAX;
|
||||
LVector3 projection = new LVector3(); // 规划坐标
|
||||
LVector3 newStart = new LVector3(); // 新坐标
|
||||
LFloat dst;
|
||||
// A-B
|
||||
if ((dst = GeometryUtil.NearestSegmentPointSquareDistance(ref projection, m_StartTri.vertexA, m_StartTri.vertexB,
|
||||
m_Start)) < minDst)
|
||||
{
|
||||
minDst = dst;
|
||||
newStart = projection;
|
||||
}
|
||||
|
||||
// B-C
|
||||
if ((dst = GeometryUtil.NearestSegmentPointSquareDistance(ref projection, m_StartTri.vertexB, m_StartTri.vertexC,
|
||||
m_Start)) < minDst)
|
||||
{
|
||||
minDst = dst;
|
||||
newStart = projection;
|
||||
}
|
||||
|
||||
// C-A
|
||||
if ((dst = GeometryUtil.NearestSegmentPointSquareDistance(ref projection, m_StartTri.vertexC, m_StartTri.vertexA,
|
||||
m_Start)) < minDst)
|
||||
{
|
||||
minDst = dst;
|
||||
newStart = projection;
|
||||
}
|
||||
|
||||
m_Start = newStart;
|
||||
}
|
||||
|
||||
if (m_EdgePaths.Count == 0)
|
||||
{ // 起点终点在同一三角形中
|
||||
AddPoint(m_Start, m_StartTri);
|
||||
AddPoint(m_End, m_StartTri);
|
||||
}
|
||||
else
|
||||
{
|
||||
m_TempLastEdge.Set(m_EdgePaths[^1].GetToNode(), m_EdgePaths[^1].GetToNode(), m_End, m_End);
|
||||
CalculateEdgePoints(calculateCrossPoint);
|
||||
}
|
||||
}
|
||||
|
||||
public void Clear()
|
||||
{
|
||||
m_TempFunnel.Reset();
|
||||
m_TempRay.Reset();
|
||||
m_Vectors.Clear();
|
||||
// Debug.Log("RemainCount: {0}", m_TempEdgePointQueue.Count);
|
||||
for (int i = m_PathPoints.Count - 1; i >= 0; --i)
|
||||
{
|
||||
RecycleEdgePoint(m_PathPoints[i]);
|
||||
}
|
||||
// Debug.Log("TotalCount: {0}", m_TempEdgePointQueue.Count);
|
||||
m_PathPoints.Clear();
|
||||
m_StartTri = null;
|
||||
m_LastPointAdded = null;
|
||||
m_TempLastEdge.Clear();
|
||||
}
|
||||
|
||||
private TriangleEdge GetEdge(int index)
|
||||
{
|
||||
return (TriangleEdge)((index == m_EdgePaths.Count) ? m_TempLastEdge : m_EdgePaths[index]);
|
||||
}
|
||||
|
||||
private int NumEdges()
|
||||
{
|
||||
return m_EdgePaths.Count + 1;
|
||||
}
|
||||
|
||||
public LVector3 GetVector(int index)
|
||||
{
|
||||
return m_Vectors[index];
|
||||
}
|
||||
|
||||
public int GetSize()
|
||||
{
|
||||
return m_Vectors.Count;
|
||||
}
|
||||
|
||||
/** All vectors in the path */
|
||||
public List<LVector3> GetVectors()
|
||||
{
|
||||
return m_Vectors;
|
||||
}
|
||||
|
||||
/** The triangle which must be crossed to reach the next path point.*/
|
||||
public NavTriangle GetToTriangle(int index)
|
||||
{
|
||||
return m_PathPoints[index].toNode;
|
||||
}
|
||||
|
||||
/** The triangle from which must be crossed to reach this point. */
|
||||
public NavTriangle GetFromTriangle(int index)
|
||||
{
|
||||
return m_PathPoints[index].fromNode;
|
||||
}
|
||||
|
||||
/** The navmesh edge(s) crossed at this path point. */
|
||||
public List<TriangleEdge> GetCrossedEdges(int index)
|
||||
{
|
||||
return m_PathPoints[index].connectingEdges;
|
||||
}
|
||||
|
||||
private void AddPoint(in LVector3 point, NavTriangle toNode)
|
||||
{
|
||||
// AddPoint(new EdgePoint(point, toNode));
|
||||
AddPoint(GetFreeEdgePoint(in point, toNode));
|
||||
}
|
||||
|
||||
private void AddPoint(EdgePoint edgePoint)
|
||||
{
|
||||
m_Vectors.Add(edgePoint.point);
|
||||
m_PathPoints.Add(edgePoint);
|
||||
m_LastPointAdded = edgePoint;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 基于漏斗算法，将图的边路径转换为最短点路径
|
||||
/// <para>point path through the path triangles, using the Simple Stupid Funnel Algorithm.</para>
|
||||
/// </summary>
|
||||
/// <param name="calculateCrossPoint"></param>
|
||||
private void CalculateEdgePoints(bool calculateCrossPoint)
|
||||
{
|
||||
TriangleEdge edge = GetEdge(0);
|
||||
AddPoint(m_Start, edge.fromNode);
|
||||
m_LastPointAdded.fromNode = edge.fromNode;
|
||||
// 初始化第一个漏斗
|
||||
m_TempFunnel.pivot = m_Start;
|
||||
m_TempFunnel.SetPlanes(m_TempFunnel.pivot, edge);// 路径起点为漏斗点，设置共享边的两个端点为漏斗边界平面
|
||||
int leftIndex = 0; // 左顶点索引
|
||||
int rightIndex = 0; // 右顶点索引
|
||||
int lastRestart = 0;
|
||||
for (int i = 1; i < NumEdges(); ++i)
|
||||
{
|
||||
edge = GetEdge(i); // 下一条边
|
||||
// 边的两个端点与漏斗边平面A的位置关系
|
||||
var leftPlaneLeftDP = m_TempFunnel.SideLeftPlane(edge.vertexB);
|
||||
var leftPlaneRightDP = m_TempFunnel.SideLeftPlane(edge.vertexA);
|
||||
// 边的两个端点与漏斗边平面B的位置关系
|
||||
var rightPlaneLeftDP = m_TempFunnel.SideRightPlane(edge.vertexB);
|
||||
var rightPlaneRightDP = m_TempFunnel.SideRightPlane(edge.vertexA);
|
||||
|
||||
// 右顶点在右平面里面
|
||||
if (rightPlaneRightDP != EnumPlaneSide.Front)
|
||||
{
|
||||
// 右顶点在左平面里面
|
||||
if (leftPlaneRightDP != EnumPlaneSide.Front)
|
||||
{
|
||||
// Tighten the funnel. 缩小漏斗
|
||||
m_TempFunnel.SetRightPlane(m_TempFunnel.pivot, edge.vertexA);
|
||||
rightIndex = i;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Right over left, insert left to path and restart scan from portal left point.
|
||||
// 右顶点在左平面外面，设置左顶点为漏斗顶点和路径点，从新已该漏斗开始扫描
|
||||
if (calculateCrossPoint)
|
||||
{
|
||||
CalculateEdgeCrossings(lastRestart, leftIndex, m_TempFunnel.pivot, m_TempFunnel.leftPortal);
|
||||
}
|
||||
else
|
||||
{
|
||||
m_Vectors.Add(m_TempFunnel.leftPortal);
|
||||
}
|
||||
|
||||
m_TempFunnel.pivot = m_TempFunnel.leftPortal;
|
||||
i = leftIndex;
|
||||
rightIndex = i;
|
||||
if (i < NumEdges() - 1)
|
||||
{
|
||||
lastRestart = i;
|
||||
m_TempFunnel.SetPlanes(m_TempFunnel.pivot, GetEdge(i + 1));
|
||||
continue;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// 左顶点在左平面里面
|
||||
if (leftPlaneLeftDP != EnumPlaneSide.Front)
|
||||
{
|
||||
// 左顶点在右平面里面
|
||||
if (rightPlaneLeftDP != EnumPlaneSide.Front)
|
||||
{
|
||||
// Tighten the funnel.
|
||||
m_TempFunnel.SetLeftPlane(m_TempFunnel.pivot, edge.vertexB);
|
||||
leftIndex = i;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Left over right, insert right to path and restart scan from portal right point.
|
||||
if (calculateCrossPoint)
|
||||
{
|
||||
CalculateEdgeCrossings(lastRestart, rightIndex, m_TempFunnel.pivot, m_TempFunnel.rightPortal);
|
||||
}
|
||||
else
|
||||
{
|
||||
m_Vectors.Add(m_TempFunnel.rightPortal);
|
||||
}
|
||||
|
||||
m_TempFunnel.pivot = m_TempFunnel.rightPortal;
|
||||
i = rightIndex;
|
||||
leftIndex = i;
|
||||
if (i < NumEdges() - 1)
|
||||
{
|
||||
lastRestart = i;
|
||||
m_TempFunnel.SetPlanes(m_TempFunnel.pivot, GetEdge(i + 1));
|
||||
continue;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (calculateCrossPoint)
|
||||
{
|
||||
CalculateEdgeCrossings(lastRestart, NumEdges() - 1, m_TempFunnel.pivot, m_End);
|
||||
}
|
||||
else
|
||||
{
|
||||
m_Vectors.Add(m_End);
|
||||
}
|
||||
|
||||
for (int i = 1; i < m_PathPoints.Count; i++)
|
||||
{
|
||||
EdgePoint p = m_PathPoints[i];
|
||||
p.fromNode = m_PathPoints[i - 1].toNode;
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/**
|
||||
* Store all edge crossing points between the start and end indices. If the path
|
||||
* crosses exactly the start or end points (which is quite likely), store the
|
||||
* edges in order of crossing in the EdgePoint data structure.
|
||||
* <p/>
|
||||
* Edge crossings are calculated as intersections with the plane from the start,
|
||||
* end and up vectors.
|
||||
*/
|
||||
private void CalculateEdgeCrossings(int startIndex, int endIndex, LVector3 startPoint, LVector3 endPoint)
|
||||
{
|
||||
if (startIndex >= NumEdges() || endIndex >= NumEdges())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
m_CrossingPlane.Set(startPoint, startPoint + LVector3.Up, endPoint);
|
||||
|
||||
EdgePoint previousLast = m_LastPointAdded;
|
||||
|
||||
var edge = GetEdge(endIndex);
|
||||
// EdgePoint end = new EdgePoint(endPoint, edge.toNode);
|
||||
EdgePoint end = GetFreeEdgePoint(in endPoint, edge.toNode);
|
||||
for (int i = startIndex; i < endIndex; i++)
|
||||
{
|
||||
edge = GetEdge(i);
|
||||
|
||||
if (edge.vertexA.Equals(startPoint) || edge.vertexB.Equals(startPoint))
|
||||
{
|
||||
previousLast.toNode = edge.toNode;
|
||||
if (!previousLast.connectingEdges.Contains(edge))
|
||||
{
|
||||
previousLast.connectingEdges.Add(edge);
|
||||
}
|
||||
}
|
||||
else if (edge.vertexB.Equals(endPoint) || edge.vertexA.Equals(endPoint))
|
||||
{
|
||||
if (!end.connectingEdges.Contains(edge))
|
||||
{
|
||||
end.connectingEdges.Add(edge);
|
||||
}
|
||||
}
|
||||
else if (IntersectSegmentPlane(edge.vertexB, edge.vertexA, m_CrossingPlane, out LVector3 intersection))
|
||||
{
|
||||
if (i != startIndex || i == 0)
|
||||
{
|
||||
m_LastPointAdded.toNode = edge.fromNode;
|
||||
// EdgePoint crossing = new EdgePoint(intersection, edge.toNode);
|
||||
EdgePoint crossing = GetFreeEdgePoint(in intersection, edge.toNode);
|
||||
crossing.connectingEdges.Add(edge);
|
||||
AddPoint(crossing);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (endIndex < NumEdges() - 1)
|
||||
{
|
||||
end.connectingEdges.Add(GetEdge(endIndex));
|
||||
}
|
||||
|
||||
if (!m_LastPointAdded.Equals(end))
|
||||
{
|
||||
AddPoint(end);
|
||||
}
|
||||
}
|
||||
|
||||
public static bool IntersectSegmentPlane(LVector3 start, LVector3 end, Plane plane, out LVector3 intersection)
|
||||
{
|
||||
intersection = LVector3.Zero;
|
||||
LVector3 dir = end - start;
|
||||
LFloat denom = LVector3.Dot(dir, plane.normal);
|
||||
LFloat t = -(LVector3.Dot(start, plane.normal) + plane.distFromOrigin) / denom;
|
||||
if (t < 0 || t > 1)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
intersection = start + dir * t;
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从临时队列中获取缓存的EdgePoint对象，并且初始化
|
||||
/// </summary>
|
||||
/// <param name="point"></param>
|
||||
/// <param name="toNode"></param>
|
||||
/// <returns></returns>
|
||||
private EdgePoint GetFreeEdgePoint(in LVector3 point, NavTriangle toNode)
|
||||
{
|
||||
if (m_TempEdgePointQueue.Count == 0)
|
||||
{
|
||||
return new EdgePoint(in point, toNode);
|
||||
}
|
||||
EdgePoint edgePoint = m_TempEdgePointQueue.Dequeue();
|
||||
edgePoint.Set(in point, toNode);
|
||||
return edgePoint;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 回收edgePoint临时对象，以备复用
|
||||
/// </summary>
|
||||
/// <param name="edgePoint"></param>
|
||||
private void RecycleEdgePoint(EdgePoint edgePoint)
|
||||
{
|
||||
if (edgePoint == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
m_TempEdgePointQueue.Enqueue(edgePoint);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 8f226701410f4424d8ac316660c1ccb0
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
Reference in New Issue
Block a user