/// <summary> /// 删除重复顶点 /// </summary> /// <returns></returns> public void DelRepeatPoint() { for (int i = 0; i < this.m_lstPoints.Count; i++) { for (int j = i + 1; j < this.m_lstPoints.Count; j++) { if (NMath.IsEqualZero(this.m_lstPoints[i] - this.m_lstPoints[j])) { this.m_lstPoints.Remove(this.m_lstPoints[j]); j = i; } } } }
/// <summary> /// 查找point是否在节点列表里面 /// </summary> /// <param name="nodeList">节点列表</param> /// <param name="point">用于查找的节点</param> /// <param name="pIndex">返回节点索引</param> /// <returns>if inside,return sucess,else return not inside</returns> public static PolyResCode NavNodeGetNodeIndex(List <NavNode> nodeList, Vector2 point, out int pIndex) { pIndex = -1; for (int i = 0; i < nodeList.Count; i++) { NavNode node = nodeList[i]; if (NMath.Equals(node.vertex, point)) { pIndex = i; return(PolyResCode.Success); } } return(PolyResCode.ErrNotInside); }
/// <summary> /// Checks the line cross. /// </summary> /// <returns><c>true</c>, if line cross was checked, <c>false</c> otherwise.</returns> public bool CheckLineCross() { Vector2 p1s, p1e, p2s, p2e; int iPointCount = this.m_lstPoints.Count; // 如果点的列表中只有两个点, 可以添加新点. if (iPointCount <= 2) { return(true); } for (int i = 0; i < iPointCount - 2; i++) { p1s.x = this.m_lstPoints[i].transform.position.x; p1s.y = this.m_lstPoints[i].transform.position.z; p1e.x = this.m_lstPoints[i + 1].transform.position.x; p1e.y = this.m_lstPoints[i + 1].transform.position.z; for (int j = i + 2; j < iPointCount; j++) { if (j != iPointCount - 1) { p2s.x = this.m_lstPoints[j].transform.position.x; p2s.y = this.m_lstPoints[j].transform.position.z; p2e.x = this.m_lstPoints[j + 1].transform.position.x; p2e.y = this.m_lstPoints[j + 1].transform.position.z; } else { p2s.x = this.m_lstPoints[j].transform.position.x; p2s.y = this.m_lstPoints[j].transform.position.z; p2e.x = this.m_lstPoints[0].transform.position.x; p2e.y = this.m_lstPoints[0].transform.position.z; if (0 == i) { continue; } } if (NMath.CheckCross(p1s, p1e, p2s, p2e)) { return(false); } } } return(true); }
/// <summary> /// 生成最终的路径点 /// </summary> /// <param name="startPos">起始点</param> /// <param name="endPos">终点</param> /// <param name="triPathList">三角形路径列表</param> /// <param name="wayPoints">路径点</param> /// <param name="offSet">移动物体宽度</param> /// <returns></returns> private PathResCode CreateWayPoints(Vector2 startPos, Vector2 endPos , List <NavTriangle> triPathList, out List <Vector2> wayPoints, int offSet) { wayPoints = new List <Vector2>(); if (triPathList.Count == 0 || startPos == null || endPos == null) { return(PathResCode.Failed); } // 保证从起点到终点的顺序 triPathList.Reverse(); // 保存出边编号 for (int i = 0; i < triPathList.Count; i++) { NavTriangle tri = triPathList[i]; if (i != triPathList.Count - 1) { NavTriangle nextTri = triPathList[i + 1]; tri.SetOutWallIndex(tri.GetWallIndex(nextTri.GetID())); } } wayPoints.Add(startPos); //起点与终点在同一三角形中 if (triPathList.Count == 1) { wayPoints.Add(endPos); return(PathResCode.Success); } WayPoint way = new WayPoint(startPos, triPathList[0]); while (!NMath.IsEqualZero(way.GetPoint() - endPos)) { way = GetFurthestWayPoint(way, triPathList, endPos, offSet); if (way == null) { return(PathResCode.CanNotGetNextWayPoint); } wayPoints.Add(way.GetPoint()); } return(PathResCode.Success); }
/// <summary> /// 两条线段是否相等 /// </summary> /// <param name="lineTemp">判断对象</param> /// <returns>是否相等</returns> public bool Equals(Line2D line) { //只是一个点 if (NMath.IsEqualZero(line.m_cStartPoint - line.m_cEndPoint) || NMath.IsEqualZero(m_cStartPoint - m_cEndPoint)) { return(false); } //whatever the direction bool bEquals = NMath.IsEqualZero(m_cStartPoint - line.m_cStartPoint) ? true : NMath.IsEqualZero(m_cStartPoint - line.m_cEndPoint); if (bEquals) { bEquals = NMath.IsEqualZero(m_cEndPoint - line.m_cStartPoint) ? true : NMath.IsEqualZero(m_cEndPoint - line.m_cEndPoint); } return(bEquals); }
/// <summary> /// 判断这条线段是否没有和其他的边相交 /// </summary> /// <param name="sPnt"></param> /// <param name="point"></param> /// <returns></returns> private bool IsVisibleIn2Point(Vector2 sPnt, Vector2 ePnt) { Line2D line = new Line2D(sPnt, ePnt); Vector2 interPos; foreach (Line2D edge in allEdges) { if (edge.Intersection(line, out interPos) == LineCrossState.CROSS) { if (!NMath.IsEqualZero(sPnt - interPos) && !NMath.IsEqualZero(ePnt - interPos)) { return(false); } } } return(true); }
/// <summary> /// 判断是否是顺时针 /// </summary> /// <returns></returns> public bool IsCW() { if (this.m_lstPoints.Count <= 2) { return(false); } //最上(y最小)最左(x最小)点, 肯定是一个凸点 //寻找最上点 Vector2 topPoint = this.m_lstPoints[0]; int topIndex = 0; for (int i = 1; i < this.m_lstPoints.Count; i++) { Vector2 currPoint = this.m_lstPoints[i]; if ((topPoint.y > currPoint.y) || ((topPoint.y == currPoint.y) && (topPoint.x > currPoint.x))) { topPoint = currPoint; topIndex = i; } } //寻找左右邻居 int preIndex = (topIndex - 1) >= 0 ? (topIndex - 1) : (this.m_lstPoints.Count - 1); int nextIndex = (topIndex + 1) < this.m_lstPoints.Count ? (topIndex + 1) : 0; Vector2 prePoint = this.m_lstPoints[preIndex]; Vector2 nextPoint = this.m_lstPoints[nextIndex]; //三点共线情况不存在,若三点共线则说明必有一点的y(斜线)或x(水平线)小于topPt float r = NMath.CrossProduct((prePoint - topPoint), (nextPoint - topPoint)); if (r > 0) { return(true); } return(false); }
/// <summary> /// 返回三角形的外接圆 /// </summary> /// <param name="p1"></param> /// <param name="p2"></param> /// <param name="p3"></param> /// <returns></returns> public static Circle CreateCircle(Vector2 p1, Vector2 p2, Vector2 p3) { if (!NMath.IsEqualZero(p1.y - p2.y) || !NMath.IsEqualZero(p2.y - p3.y)) { double yc, xc; float m1 = -(p2.x - p1.x) / (p2.y - p1.y); float m2 = -(p3.x - p2.x) / (p3.y - p2.y); double mx1 = (p1.x + p2.x) / 2.0; double mx2 = (p2.x + p3.x) / 2.0; double my1 = (p1.y + p2.y) / 2.0; double my2 = (p2.y + p3.y) / 2.0; if (NMath.IsEqualZero(p1.y - p2.y)) { xc = (p2.x + p1.x) / 2.0; yc = m2 * (xc - mx2) + my2; } else if (NMath.IsEqualZero(p3.y - p2.y)) { xc = (p3.x + p2.x) / 2.0; yc = m1 * (xc - mx1) + my1; } else { xc = (m1 * mx1 - m2 * mx2 + my2 - my1) / (m1 - m2); yc = m1 * (xc - mx1) + my1; } double dx = p2.x - xc; double dy = p2.y - yc; double rsqr = dx * dx + dy * dy; double r = Math.Sqrt(rsqr); return(new Circle(new Vector2((float)xc, (float)yc), (float)r)); } return(new Circle(new Vector2(0, 0), 0)); }
/// <summary> /// 计算两条二维线段的交点 /// </summary> /// <param name="other">Other line</param> /// <param name="intersectPoint">输出的线段交点</param> /// <returns>返回值说明了两条线段的位置关系(COLINE,PARALLEL,CROSS,NOT_CROSS) </returns> public LineCrossState Intersection(Line2D other, out Vector2 intersectPoint) { intersectPoint.x = intersectPoint.y = float.NaN; if (!NMath.CheckCross(this.m_cStartPoint, this.m_cEndPoint, other.m_cStartPoint, other.m_cEndPoint)) { return(LineCrossState.NOT_CROSS); } double A1, B1, C1, A2, B2, C2; A1 = this.m_cEndPoint.y - this.m_cStartPoint.y; B1 = this.m_cStartPoint.x - this.m_cEndPoint.x; C1 = this.m_cEndPoint.x * this.m_cStartPoint.y - this.m_cStartPoint.x * this.m_cEndPoint.y; A2 = other.m_cEndPoint.y - other.m_cStartPoint.y; B2 = other.m_cStartPoint.x - other.m_cEndPoint.x; C2 = other.m_cEndPoint.x * other.m_cStartPoint.y - other.m_cStartPoint.x * other.m_cEndPoint.y; if (NMath.IsEqualZero(A1 * B2 - B1 * A2)) { if (NMath.IsEqualZero((A1 + B1) * C2 - (A2 + B2) * C1)) { return(LineCrossState.COLINE); } else { return(LineCrossState.PARALLEL); } } else { intersectPoint.x = (float)((B2 * C1 - B1 * C2) / (A2 * B1 - A1 * B2)); intersectPoint.y = (float)((A1 * C2 - A2 * C1) / (A2 * B1 - A1 * B2)); return(LineCrossState.CROSS); } }
/// <summary> /// 根据原始点计算出正确的落在导航区内的位置,修复无效点和三角形 /// </summary> /// <param name="orgTarTri">起源三角形</param> /// <param name="orgTarPos">起源点</param> /// <param name="otherTri">参考方向三角形</param> /// <param name="otherPos">参考方向点</param> /// <returns>结果</returns> private PathResCode FixPos(ref NavTriangle orgTarTri, ref Vector2 orgTarPos, NavTriangle otherTri, Vector2 otherPos, int extendLength) { Vector2 tarPos = orgTarPos; ////////////////////////////////////////////////////////////////////////// //为了精确判断,需要逆向延长线段一定长度 if (extendLength > 0) { Vector2 newTarPos = NMath.ExtendPos(otherPos, tarPos, extendLength); tarPos = newTarPos; } ////////////////////////////////////////////////////////////////////////// Line2D linePath = new Line2D(tarPos, otherPos); //参考线段 Rect lineRect = NMath.LineRect(linePath); //获取线段矩形包围盒 //1)找到所有与参考线段矩形相交的三角形,并判断是否groupID相等 List <NavTriangle> crossNavTris = new List <NavTriangle>(); foreach (NavTriangle tri in this.m_lstTriangle) { if (NMath.CheckCross(lineRect, tri.GetBoxCollider())) { if (otherTri != null && otherTri.GetGroupID() != tri.GetGroupID()) { continue; } crossNavTris.Add(tri); } } //2)找出所有与参考线段相交的三角形,并记录相交点 List <Vector2> crossPoints = new List <Vector2>(); //相交点列表 List <int> triIndex = new List <int>(); //相交三角形索引列表 for (int index = 0; index < crossNavTris.Count; index++) { NavTriangle crossTri = crossNavTris[index]; Line2D triLine; for (int i = 0; i < 3; i++) { Vector2 insPoint; triLine = new Line2D(crossTri.GetPoint(i), crossTri.GetPoint((i + 1) % 3)); if (linePath.Intersection(triLine, out insPoint) == LineCrossState.CROSS) { crossPoints.Add(insPoint); triIndex.Add(index); } } } if (crossPoints.Count == 0) { return(PathResCode.NoCrossPoint); } //3)找到最接近起源点的点 Vector2 lastPos = crossPoints[0]; int lastTriIndex = triIndex[0]; double lastLength = Math.Pow(lastPos.x - orgTarPos.x, 2.0) + Math.Pow(lastPos.y - orgTarPos.y, 2.0); for (int i = 1; i < crossPoints.Count; i++) { double newLength = Math.Pow(crossPoints[i].x - orgTarPos.x, 2.0) + Math.Pow(crossPoints[i].y - orgTarPos.y, 2.0); if (newLength < lastLength) { lastPos = crossPoints[i]; lastTriIndex = triIndex[i]; lastLength = newLength; } } //4)保存目标 orgTarPos = lastPos; orgTarTri = crossNavTris[lastTriIndex]; return(PathResCode.Success); }
/// <summary> /// 找到指定边的约束边DT /// </summary> /// <param name="line"></param> /// <returns></returns> private bool FindDT(Line2D line, out Vector2 dtPoint) { dtPoint = new Vector2(); if (line == null) { return(false); } Vector2 ptA = line.GetStartPoint(); Vector2 ptB = line.GetEndPoint(); List <Vector2> visiblePnts = new List <Vector2>(); foreach (Vector2 point in allPoints) { if (IsPointVisibleOfLine(line, point)) { visiblePnts.Add(point); } } if (visiblePnts.Count == 0) { return(false); } bool bContinue = false; dtPoint = visiblePnts[0]; do { bContinue = false; //Step1.构造三角形的外接圆,以及外接圆的包围盒 Circle circle = NMath.CreateCircle(ptA, ptB, dtPoint); Rect boundBox = NMath.GetCircleBoundBox(circle); //Step2. 依次访问网格包围盒内的每个网格单元: //若某个网格单元中存在可见点 p, 并且 ∠p1pp2 > ∠p1p3p2,则令 p3=p,转Step1; //否则,转Step3. float angOld = (float)Math.Abs(NMath.LineRadian(ptA, dtPoint, ptB)); foreach (Vector2 pnt in visiblePnts) { if (pnt == ptA || pnt == ptB || pnt == dtPoint) { continue; } if (!boundBox.Contains(pnt)) { continue; } float angNew = (float)Math.Abs(NMath.LineRadian(ptA, pnt, ptB)); if (angNew > angOld) { dtPoint = pnt; bContinue = true; break; } } //false 转Step3 } while (bContinue); //Step3. 若当前网格包围盒内所有网格单元都已被处理完, // 也即C(p1,p2,p3)内无可见点,则 p3 为的 p1p2 的 DT 点 return(true); }