/// <summary>
/// 创建从src到dest的边, 如果它已经存在, 直接返回它.
/// </summary>
public HalfEdge CreateEdge(Vertex src, Vertex dest)
{
HalfEdge self = GetRays(src).Find(item => { return item.Dest == dest; });
// 如果该边不存在, 进行创建.
if (self == null)
{
self = new HalfEdge();
self.Dest = dest;
HalfEdge other = new HalfEdge();
other.Dest = src;
self.Pair = other;
other.Pair = self;
/*src.Edge = self;
dest.Edge = other;
*/
AddUnserializedEdge(self);
AddUnserializedEdge(other);
}
return self;
}
/// <summary>
/// 获取可以经过a, b边的物体的最大半径(a, b必须有交点).
/// </summary>
public float GetWidth(HalfEdge a, HalfEdge b)
{
Vertex v = GetIntersectVertex(a, b);
if (v == A)
{
if (float.IsNaN(widthA)) { widthA = CalculateWidth(a, b); }
return widthA;
}
if (v == B)
{
if (float.IsNaN(widthB)) { widthB = CalculateWidth(a, b); }
return widthB;
}
if (v == C)
{
if (float.IsNaN(widthC)) { widthC = CalculateWidth(a, b); }
return widthC;
}
Utility.Verify(false, "Invalid edges {0} and {1}", a, b);
return float.NaN;
}
/// <summary>
/// 查找ray的环中, 与src->dest的边相交的边.
/// </summary>
bool FindCrossedEdge(out Tuple2<HalfEdge, CrossState> answer, HalfEdge ray, Vector3 src, Vector3 dest)
{
List<HalfEdge> cycle = ray.Cycle;
answer = new Tuple2<HalfEdge, CrossState>();
foreach (HalfEdge edge in cycle)
{
Vector3 point;
CrossState crossState = MathUtility.GetLineCrossPoint(out point,
edge.Src.Position, edge.Dest.Position,
src, dest
);
if (crossState == CrossState.FullyOverlaps
|| (crossState == CrossState.CrossOnSegment && !point.equals2(edge.Src.Position) && !point.equals2(edge.Dest.Position)))
{
answer.Second = crossState;
answer.First = edge;
if (crossState == CrossState.FullyOverlaps
&& (dest.equals2(edge.Src.Position) || src.equals2(edge.Dest.Position)))
{
answer.First = answer.First.Pair;
}
return true;
}
}
return false;
}
/// <summary>
/// 将点插入到old的边上.
/// </summary>
void InsertOnEdge(Vertex v, Triangle old, HalfEdge hitEdge)
{
// 连接v和v所在的边正对的顶点, 将old一分为二.
Triangle split1 = geomManager.CreateTriangle();
Triangle split2 = geomManager.CreateTriangle();
Vertex opositeVertex = hitEdge.Next.Dest;
HalfEdge ov = geomManager.CreateEdge(opositeVertex, v);
HalfEdge v1 = geomManager.CreateEdge(v, hitEdge.Dest);
HalfEdge v2 = geomManager.CreateEdge(v, hitEdge.Pair.Dest);
HalfEdge sp1Edge0 = hitEdge.Next;
HalfEdge sp2Edge0 = hitEdge.Next.Next;
HalfEdge sp2Edge1 = v2.Pair;
HalfEdge sp2Edge2 = ov.Pair;
// 更新Face.
sp1Edge0.Face = ov.Face = v1.Face = split1;
sp2Edge0.Face = sp2Edge1.Face = sp2Edge2.Face = split2;
// 释放旧三角形.
geomManager.ReleaseTriangle(old);
// 连接边.
split1.Edge = sp1Edge0.CycleLink(ov, v1);
split2.Edge = sp2Edge0.CycleLink(sp2Edge1, sp2Edge2);
// 将新三角形映射到格子地图上.
geomManager.RasterizeTriangle(split1);
geomManager.RasterizeTriangle(split2);
Utility.Verify(ov.Face == split1);
Utility.Verify(ov.Pair.Face == split2);
Triangle other = hitEdge.Pair.Face;
Triangle oposite1 = null;
Triangle oposite2 = null;
// 分割另一侧的三角形.
if (other != null)
{
Vertex p = hitEdge.Pair.Next.Dest;
HalfEdge vp = geomManager.CreateEdge(v, p);
oposite1 = geomManager.CreateTriangle();
oposite2 = geomManager.CreateTriangle();
HalfEdge hpn = hitEdge.Pair.Next;
HalfEdge op1Edge0 = hpn.Next;
HalfEdge op1Edge1 = v1.Pair;
HalfEdge op1Edge2 = vp;
hpn.Face = vp.Pair.Face = v2.Face = oposite2;
op1Edge0.Face = op1Edge1.Face = op1Edge2.Face = oposite1;
geomManager.ReleaseTriangle(other);
oposite2.Edge = hpn.CycleLink(vp.Pair, v2);
oposite1.Edge = op1Edge0.CycleLink(op1Edge1, op1Edge2);
geomManager.RasterizeTriangle(oposite1);
geomManager.RasterizeTriangle(oposite2);
Utility.Verify(vp.Face == oposite1);
Utility.Verify(vp.Pair.Face == oposite2);
}
Utility.Verify(v1.Face == split1);
Utility.Verify(v1.Pair.Face == oposite1);
Utility.Verify(v2.Face == oposite2);
Utility.Verify(v2.Pair.Face == split2);
// 维护delaunay特性.
FlipTriangles(split1.Edge);
FlipTriangles(split2.Edge);
if (other != null)
{
FlipTriangles(oposite1.Edge);
FlipTriangles(oposite2.Edge);
}
}
/// <summary>
/// 构造src到dest的约束边. crossedEdge为第一条相交边.
/// </summary>
HalfEdge OnConstrainedEdgeCrossEdges(ref Vertex src, Vertex dest, HalfEdge crossedEdge)
{
List<Vertex> upper = new List<Vertex>();
List<Vertex> lower = new List<Vertex>();
List<HalfEdge> edges = new List<HalfEdge>();
// 收集相交的非约束边.
Vertex newSrc = CollectCrossedUnconstrainedEdges(edges, lower, upper, src, dest, crossedEdge);
// 清理非约束边两侧的三角形.
for (int i = 0; i < edges.Count; ++i)
{
HalfEdge edge = edges[i];
if (edge.Face != null)
{
geomManager.ReleaseTriangle(edge.Face);
}
if (edge.Pair.Face != null)
{
geomManager.ReleaseTriangle(edge.Pair.Face);
}
}
// 对清理过后的区域三角形化(该区域不一定是多边形).
CreateTriangles(PolygonTriangulation.Triangulate(lower, dest, src));
CreateTriangles(PolygonTriangulation.Triangulate(upper, src, dest));
// 标记约束边.
HalfEdge constrainedEdge = geomManager.GetRays(src).Find(edge => { return edge.Dest == dest; });
Utility.Verify(constrainedEdge != null);
constrainedEdge.Constrained = true;
src = newSrc;
return constrainedEdge;
}
/// <summary>
/// 收集与src到dest相交的约束边.
/// </summary>
Vertex CollectCrossedUnconstrainedEdges(List<HalfEdge> answer, List<Vertex> lowerVertices, List<Vertex> upperVertices, Vertex src, Vertex dest, HalfEdge start)
{
// src->dest向量.
Vector3 srcDest = dest.Position - src.Position;
Vertex current = src;
// 遍历到dest所在的三角形为止.
for (; !start.Face.Contains(dest.Position); )
{
Utility.Verify(!start.Constrained, "Crossed constrained edge");
// 收集相交的边.
answer.Add(start);
HalfEdge opposedTriangle = start.Face.GetOpposite(current);
Utility.Verify(opposedTriangle != null);
Vertex opposedVertex = opposedTriangle.Next.Dest;
if ((opposedVertex.Position - src.Position).cross2(srcDest) < 0)
{
current = opposedTriangle.Src;
}
else
{
current = opposedTriangle.Dest;
}
float cr = (opposedTriangle.Dest.Position - src.Position).cross2(srcDest);
Utility.Verify(!MathUtility.Approximately(0, cr), "Not implement");
List<Vertex> activeContainer = ((cr < 0) ? upperVertices : lowerVertices);
if (!activeContainer.Contains(opposedTriangle.Dest)) { activeContainer.Add(opposedTriangle.Dest); }
cr = (opposedTriangle.Src.Position - src.Position).cross2(srcDest);
activeContainer = ((cr < 0) ? upperVertices : lowerVertices);
if (!activeContainer.Contains(opposedTriangle.Src)) { activeContainer.Add(opposedTriangle.Src); }
if (MathUtility.PointOnSegment(opposedVertex.Position, src.Position, dest.Position))
{
answer.Add(opposedTriangle);
src = opposedVertex;
break;
}
start = opposedTriangle;
}
return src;
}
/// <summary>
/// 判断半径为radius的物体是否可以通过prevPortal和currentPortal.
/// </summary>
static bool CheckCorridorWidthLimit(HalfEdge prevPortal, HalfEdge currentPortal, float radius)
{
if (prevPortal == null)
{
return true;
}
return currentPortal.Pair.Face.GetWidth(prevPortal, currentPortal.Pair) >= (radius * 2f);
}
/// <summary>
/// from在edge上, 查找半径为radius的物体从from到to可达的最远位置.
/// </summary>
Vector3 RaycastFromEdge(HalfEdge edge, Vector3 from, Vector3 to, float radius)
{
// 起点==终点, 或者edge为约束边.
if (from.equals2(to) || edge.Constrained || edge.Pair.Constrained)
{
return from;
}
// from位置无效.
if (!IsValidPosition(from, radius))
{
return from;
}
// 根据from->to的方向, 确定下一个要访问的三角形是edge.Face还是edge.Pair.Face.
if ((to - from).cross2(edge.Dest.Position - edge.Src.Position) > 0)
{
edge = edge.Pair;
}
// 不存在另一边的三角形.
if (edge.Face == null)
{
return from;
}
// 检查另外两条边.
return RaycastWithEdges(new HalfEdge[] { edge.Next, edge.Next.Next }, from, to, radius);
}
/// <summary>
/// 获取ea和eb的交点.
/// </summary>
Vertex GetIntersectVertex(HalfEdge ea, HalfEdge eb)
{
if (ea.Src == eb.Dest) { return ea.Src; }
if (ea.Dest == eb.Src) { return ea.Dest; }
Utility.Verify(false, "Edge {0} and {1} has no intersection", ea, eb);
return null;
}
float SearchWidth(Vertex c, Triangle t, HalfEdge e, float d)
{
Vertex u = e.Src, v = e.Dest;
if (c.Position.dot2(v.Position, u.Position) <= 0
|| c.Position.dot2(u.Position, v.Position) <= 0)
{
return d;
}
float d2 = MathUtility.MinDistance2Segment(c.Position, e.Src.Position, e.Dest.Position);
if (d2 > d)
{
return d;
}
if (e.Constrained)
{
return d2;
}
Triangle t2 = e.Pair.Face;
if (t2 == null)
{
Debug.LogError("Invalid t2");
return d;
}
HalfEdge e2 = t2.GetOpposite(e.Src);
HalfEdge e3 = t2.GetOpposite(e.Dest);
d = SearchWidth(c, t2, e2, d);
return SearchWidth(c, t2, e3, d);
}
void ReleaseHalfEdge(HalfEdge edge)
{
List<HalfEdge> list = halfEdgeContainer[edge.Src];
Utility.Verify(list.Remove(edge));
if (list.Count == 0)
{
halfEdgeContainer.Remove(edge.Src);
}
}
/// <summary>
/// 反序列化边.
/// </summary>
public void ReadBinary(BinaryReader reader, List<Vertex> vertices, IDictionary<int, HalfEdge> container)
{
container[ID] = this;
int destVertexID = reader.ReadInt32();
Dest = vertices.Find(item => { return item.ID == destVertexID; });
Utility.Verify(Dest != null);
int nextEdge = reader.ReadInt32();
HalfEdge edge = null;
if (nextEdge != -1 && !container.TryGetValue(nextEdge, out edge))
{
container.Add(nextEdge, edge = new HalfEdge());
edge.ID = nextEdge;
}
Next = edge;
int pairEdge = reader.ReadInt32();
if (!container.TryGetValue(pairEdge, out edge))
{
container.Add(pairEdge, edge = new HalfEdge());
edge.ID = pairEdge;
}
Pair = edge;
Utility.Verify(Pair != null);
Constrained = reader.ReadBoolean();
// Face字段由Triangle来更新.
}
void AddEdge(HalfEdge edge)
{
List<HalfEdge> list = null;
Utility.Verify(edge.Pair != null, "Invalid Edge, ID = " + edge.ID);
if (!halfEdgeContainer.TryGetValue(edge.Src, out list))
{
halfEdgeContainer.Add(edge.Src, list = new List<HalfEdge>());
}
list.Add(edge);
}
/// <summary>
/// 向管理器加入一条反序列化后的边.
/// <para>仅供反序列化时使用!</para>
/// </summary>
public void AddUnserializedEdge(HalfEdge edge)
{
AddEdge(edge);
}
/// <summary>
/// 释放edge及edge.Pair.
/// </summary>
public void ReleaseEdge(HalfEdge edge)
{
ReleaseHalfEdge(edge.Pair);
ReleaseHalfEdge(edge);
}
/// <summary>
/// 重置寻路使用的数据.
/// </summary>
public void ClearPathfinding()
{
G = H = float.PositiveInfinity;
Flag = -1;
Portal = null;
}
/// <summary>
/// 获取圆心在portal1和portal2的交点, 角度为二者的夹角, 半径为radius的圆弧的长度.
/// </summary>
public static float CalculateArcLengthBetweenPortals(HalfEdge portal1, HalfEdge portal2, float radius)
{
Vector3 v1 = Vector3.zero, v2 = Vector3.zero;
if (portal1.Src == portal2.Src)
{
v1 = portal1.Dest.Position - portal1.Src.Position;
v2 = portal2.Dest.Position - portal1.Src.Position;
}
else if (portal1.Dest == portal2.Dest)
{
v1 = portal1.Src.Position - portal1.Dest.Position;
v2 = portal2.Src.Position - portal1.Dest.Position;
}
else
{
Utility.Verify(false, "failed to find common vertex, portals are {0} and {1}", portal1, portal2);
}
return Mathf.Acos(v1.dot2(v2) / (v1.magnitude2() * v2.magnitude2())) * radius;
}
/// <summary>
/// 获取边from"正对"的顶点.
/// </summary>
public Vertex GetOpposite(HalfEdge from)
{
if (AB.ID == from.ID) { return C; }
if (BC.ID == from.ID) { return A; }
if (CA.ID == from.ID) { return B; }
Utility.Verify(false, "Invalid argument");
return null;
}
/// <summary>
/// 从reader中创建/查找边, 并初始化.
/// </summary>
public HalfEdge CreateEdge(BinaryReader reader, List<Vertex> vertices, IDictionary<int, HalfEdge> container)
{
HalfEdge answer = null;
int edgeID = reader.ReadInt32();
if (!container.TryGetValue(edgeID, out answer))
{
container.Add(edgeID, answer = new HalfEdge());
answer.ID = edgeID;
}
answer.ReadBinary(reader, vertices, container);
return answer;
}
/// <summary>
/// 计算可以经过a, b边的物体的最大半径(a, b必须有交点).
/// </summary>
float CalculateWidth(HalfEdge ea, HalfEdge eb)
{
Vertex vc = GetIntersectVertex(ea, eb);
Utility.Verify(vc != null);
HalfEdge ec = GetOpposite(vc);
Vertex va = GetOpposite(ea);
Vertex vb = GetOpposite(eb);
float d = (ea.Src.Position - ea.Dest.Position).magnitude2();
d = Mathf.Min(d, (eb.Src.Position - eb.Dest.Position).magnitude2());
if (vc.Position.dot2(vb.Position, va.Position) <= 0
|| vc.Position.dot2(va.Position, vb.Position) <= 0)
{
return d;
}
if (ec.Constrained)
{
return MathUtility.MinDistance2Segment(vc.Position, ec.Src.Position, ec.Dest.Position);
}
return SearchWidth(vc, this, ec, d);
}
/// <summary>
/// 检查halfEdge两侧的三角形是否满足delaunay性质. 如果不满足, 进行翻转.
/// </summary>
void FlipTriangles(HalfEdge halfEdge)
{
Stack<HalfEdge> stack = new Stack<HalfEdge>();
stack.Push(halfEdge);
for (; stack.Count != 0; )
{
halfEdge = stack.Pop();
// 如果该边是约束边, 不翻转.
if (halfEdge.Constrained || halfEdge.Pair.Constrained) { continue; }
Triangle x = halfEdge.Face;
Triangle y = halfEdge.Pair.Face;
if (x == null || y == null) { continue; }
Utility.Verify(x.Walkable && y.Walkable, "Can not flip unwalkable triangle");
// 检查是否满足delaunay性质.
if (!MathUtility.PointInCircumCircle(x.A.Position, x.B.Position, x.C.Position, halfEdge.Pair.Next.Dest.Position))
{
continue;
}
// 创建新的边.
HalfEdge ab = geomManager.CreateEdge(halfEdge.Next.Dest, halfEdge.Pair.Next.Dest);
HalfEdge bEdges0 = halfEdge.Pair.Next.Next;
HalfEdge bEdges1 = halfEdge.Next;
HalfEdge bEdges2 = ab;
// 去掉映射.
// 注意这里不删除三角形, 而是通过设置边, 将旧三角形改造为新的.
geomManager.UnrasterizeTriangle(x);
geomManager.UnrasterizeTriangle(y);
// 连接新的边.
x.Edge = halfEdge.Next.Next.CycleLink(halfEdge.Pair.Next, ab.Pair);
y.Edge = bEdges0.CycleLink(bEdges1, bEdges2);
// 映射新三角形.
geomManager.RasterizeTriangle(x);
geomManager.RasterizeTriangle(y);
x.BoundingEdges.ForEach(item => { item.Face = x; });
y.BoundingEdges.ForEach(item => { item.Face = y; });
// 翻转之后, 检查这2个三角形的另外2条边, 是否满足delaunay性质.
if (GuardedPushStack(stack, halfEdge.Pair.Next.Next)
&& GuardedPushStack(stack, halfEdge.Next)
&& GuardedPushStack(stack, halfEdge.Pair.Next)
&& GuardedPushStack(stack, halfEdge.Next.Next))
{
}
halfEdge.Face = halfEdge.Pair.Face = null;
// 该边已不存在, 将他删除.
geomManager.ReleaseEdge(halfEdge);
}
}
/// <summary>
/// 入栈.
/// </summary>
bool GuardedPushStack(Stack<HalfEdge> stack, HalfEdge item)
{
if (stack.Count < EditorConstants.kMaxStackCapacity)
{
stack.Push(item);
return true;
}
return false;
}
/// <summary>
/// 判断半径为radius的物体, 可否通过portal进入currentNode.
/// </summary>
static bool CheckEntryAndExitWidthLimit(PathfindingNode currentNode, PathfindingNode destNode, HalfEdge portal, float radius)
{
// 如果已经到达最终节点, 只判断可否经由此边进入.
if (currentNode == destNode)
{
return (portal.Dest.Position - portal.Src.Position).magnitude2() >= radius * 2;
}
// 否则, 除了判断, 是否可以经由次边进入外, 仍需判断该节点是否存在其他的边, 供离开.
HalfEdge other1 = portal.Face.AB, other2 = portal.Face.BC;
if (portal == portal.Face.AB)
{
other1 = portal.Face.BC;
other2 = portal.Face.CA;
}
else if (portal == portal.Face.BC)
{
other1 = portal.Face.AB;
other2 = portal.Face.CA;
}
// 其他两条边是否可供离开.
float diameter = radius * 2f;
if (portal.Face.GetWidth(portal, other1) >= diameter
|| portal.Face.GetWidth(portal, other2) >= diameter)
{
return true;
}
return false;
}
