private void OnDrawGizmos() { if (!Application.isPlaying) { return; } if (_path == null) { return; } /* * if (_nextCorner != Fix64Vec3.zero) * { * Gizmos.color = Color.red; * Gizmos.DrawWireSphere((Vector3)_nextCorner, 0.1f); * } */ Gizmos.color = Color.red; if (_path.Status == ParallelNavMeshPathStatus.Valid) { if (_path.startIndex != -1) { PNavPolygon previousPolygon = null; for (int index = _path.startIndex; index < 128; index++) { int polygonIndex = _path.polygonIndexes[index]; PNavPolygon currentPolygon = _path.island.graph.polygons[polygonIndex]; if (previousPolygon != null) { Gizmos.DrawLine((Vector3)previousPolygon.Centroid3D, (Vector3)currentPolygon.Centroid3D); } previousPolygon = currentPolygon; } if (_minLeftVector != Fix64Vec2.zero) { Fix64Vec2 pos = _waypoints[_waypoints.Count - 1]; Vector3 pos3D = new Vector3((float)pos.x, 0, (float)pos.y); Gizmos.color = Color.blue; Vector3 left3D = new Vector3((float)_minLeft.x, 0, (float)_minLeft.y); Gizmos.DrawLine(pos3D, left3D); Gizmos.DrawWireSphere(left3D, 0.1f); Gizmos.color = Color.magenta; Vector3 right3D = new Vector3((float)_minRight.x, 0, (float)_minRight.y); Gizmos.DrawLine(pos3D, right3D); Gizmos.DrawWireSphere(right3D, 0.1f); } } Gizmos.color = Color.white; for (int i = 0; i < _waypoints.Count; i++) { Fix64Vec2 wp = _waypoints[i]; Vector3 wp3D = new Vector3((float)wp.x, 0, (float)wp.y); Gizmos.DrawWireSphere(wp3D, 0.1f); if (i == 0) { continue; } else { Fix64Vec2 prev = _waypoints[i - 1]; Vector3 prevWp3D = new Vector3((float)prev.x, 0, (float)prev.y); Gizmos.DrawLine(prevWp3D, wp3D); } } } }
public static void Process(PNavMesh pNavMesh) { const int INDICE_COUNT = 1024; using (new SProfiler("Triangulation")) { int[] indices = new int[INDICE_COUNT]; int[] indiceCounts = new int[INDICE_COUNT]; Fix64Vec2[] verts = new Fix64Vec2[INDICE_COUNT]; int[] indexes = new int[INDICE_COUNT]; int index = 0; foreach (PNavIsland island in pNavMesh.islands) { //build island with island boundary if (island.boundaryEdgeLoopIndex < 0) { continue; } Debug.Log($"Generating Polygon graph for island={index}"); index++; PNavEdgeLoop boundary = island.edgeLoops[island.boundaryEdgeLoopIndex]; int boundaryCornersCount = PrepareCornerVerts(boundary, island, verts, indexes); PolyIsland polyIsland = Parallel3D.CreatePolyIsland(verts, indexes, boundaryCornersCount); //add other edgeloops as holes int edgeLoopIndex = -1; foreach (PNavEdgeLoop edgeLoop in island.edgeLoops) { edgeLoopIndex++; if (edgeLoopIndex == island.boundaryEdgeLoopIndex) { continue; } int holeCornersCount = PrepareCornerVerts(edgeLoop, island, verts, indexes); Parallel3D.AddHolePolyIsland(verts, indexes, holeCornersCount, polyIsland); } int polygonCount = 0; int totalIndicsCount = 0; bool ok = Parallel3D.TriangulatePolyIsland(indices, indiceCounts, ref polygonCount, ref totalIndicsCount, 2, polyIsland); int[] indicesCopy = new int[totalIndicsCount]; Array.Copy(indices, 0, indicesCopy, 0, totalIndicsCount); int[] indiceCountsCopy = new int[polygonCount]; Array.Copy(indiceCounts, 0, indiceCountsCopy, 0, polygonCount); island.indices = indicesCopy; island.indiceCountsOfPolygons = indiceCountsCopy; island.indicsCount = totalIndicsCount; island.polygonCount = polygonCount; Parallel3D.DestroyPolyIsland(polyIsland); } } }
//http://digestingduck.blogspot.com/2010/03/simple-stupid-funnel-algorithm.html void ProcessNextPolygon() { //Debug.Log($"ProcessNextPolygon {_pathPolygonIndex}"); Fix64Vec2 pos = _waypoints[_waypoints.Count - 1]; if (_pathPolygonIndex == 128) { //Debug.Log("LAST POLYGON"); if (_minLeftVector == Fix64Vec2.zero) { //Debug.Log("GO to destination directly"); _waypoints.Add(_path.Destination2D); ResetPathValues(); return; } Fix64Vec2 destinationVector = _path.Destination2D - pos; bool destinationLeftOfMinLeft = Fix64Vec2.Cross(_minLeftVector, destinationVector) > Fix64.zero; bool destinationRightOfMinRight = Fix64Vec2.Cross(_minRightVector, destinationVector) < Fix64.zero; bool lastRecalculate = false; bool useLeft = true; if (destinationLeftOfMinLeft && destinationRightOfMinRight) { //when both are too be added //use the one that is closer to pos Fix64 leftDis = Fix64Vec2.Distance(_minLeft, pos); Fix64 rightDis = Fix64Vec2.Distance(_minRight, pos); if (leftDis > rightDis) { useLeft = false; } } if (destinationLeftOfMinLeft && useLeft) { _waypoints.Add(_minLeft); pos = _minLeft; _pathPolygonIndex = _leftPolygonIndex; _previousPolygon = _leftPreviousPolygon; lastRecalculate = true; } if (destinationRightOfMinRight && !lastRecalculate) { _waypoints.Add(_minRight); pos = _minRight; _pathPolygonIndex = _rightPolygonIndex; _previousPolygon = _rightPreviousPolygon; lastRecalculate = true; } if (!lastRecalculate) { //Debug.Log("GO to destination directly"); _waypoints.Add(_path.Destination2D); ResetPathValues(); } else { _minLeftVector = Fix64Vec2.zero; } return; } if (_finishedProcessing) { return; } int polygonIndex = _path.polygonIndexes[_pathPolygonIndex]; PNavPolygon currentPolygon = _path.island.graph.polygons[polygonIndex]; PNavEdge edge = _path.FindEdge(_previousPolygon, currentPolygon, Fix64.zero); if (edge == null) { Debug.LogError("No Edge found"); return; } _previousPolygon = currentPolygon; _pathPolygonIndex++; Fix64Vec2 ab = edge.pointB - edge.pointA; Fix64Vec2 ba = edge.pointA - edge.pointB; Fix64Vec2 pa = edge.pointA + ab.normalized * error; Fix64Vec2 pb = edge.pointB + ba.normalized * error; Fix64Vec2 vA = pa - pos; Fix64Vec2 vB = pb - pos; Fix64 c = Fix64Vec2.Cross(vA, vB); Fix64Vec2 left = pb; Fix64Vec2 right = pa; if (c > Fix64.zero) { //Debug.Log($"POSITVE: pointA is on the right. pA={pa} pB={pb} pos={pos}"); } else { //Debug.Log($"NEGTIVE: pointA is on the left. pA={pa} pB={pb} pos={pos}"); right = pb; left = pa; } if (_minLeftVector == Fix64Vec2.zero) { _minLeft = left; _minRight = right; _minLeftVector = left - pos; _minRightVector = right - pos; _leftPolygonIndex = _pathPolygonIndex; _rightPolygonIndex = _pathPolygonIndex; _leftPreviousPolygon = _previousPolygon; _rightPreviousPolygon = _previousPolygon; return; } Fix64Vec2 newLeftVector = left - pos; Fix64Vec2 newRightVector = right - pos; bool newLeftIsRightToOldRight = Fix64Vec2.Cross(_minRightVector, newLeftVector) < Fix64.zero; bool newRightIsLeftToOldLeft = Fix64Vec2.Cross(_minLeftVector, newRightVector) > Fix64.zero; bool recalculateNewVectors = false; if (newLeftIsRightToOldRight) { //move to old right and recalculate _waypoints.Add(_minRight); pos = _minRight; _pathPolygonIndex = _rightPolygonIndex; _previousPolygon = _rightPreviousPolygon; recalculateNewVectors = true; } if (newRightIsLeftToOldLeft && !recalculateNewVectors) { //move to old left and recalculate _waypoints.Add(_minLeft); pos = _minLeft; _pathPolygonIndex = _leftPolygonIndex; _previousPolygon = _leftPreviousPolygon; recalculateNewVectors = true; } if (recalculateNewVectors) { _minLeftVector = Fix64Vec2.zero; return; } else { bool newLeftIsOk = Fix64Vec2.Cross(_minLeftVector, newLeftVector) <= Fix64.zero; bool newRightIsOk = Fix64Vec2.Cross(_minRightVector, newRightVector) >= Fix64.zero; if (newLeftIsOk) { _minLeft = left; _minLeftVector = newLeftVector; _leftPolygonIndex = _pathPolygonIndex; _leftPreviousPolygon = _previousPolygon; } if (newRightIsOk) { _minRight = right; _minRightVector = newRightVector; _rightPolygonIndex = _pathPolygonIndex; _rightPreviousPolygon = _previousPolygon; } } }
public Fix64Vec2 FindNearestPointOnEdge(Fix64Vec2 testPosition, Fix64Vec2 pa, Fix64Vec2 pb, Fix64 width) { Fix64Vec2 pointOnEdge = Fix64Math.FindNearestPointOnLine(testPosition, pa, pb); Fix64 distanceToA = Fix64Vec2.Distance(pointOnEdge, pa); Fix64 edgeWidth = Fix64Vec2.Distance(pa, pb); Fix64 distanceToB = edgeWidth - distanceToA; Fix64Vec2 ba = pb - pa; ba = ba.normalized; Fix64 halfWidth = width / Fix64.two; if (distanceToA < halfWidth) { pointOnEdge = pa + ba * halfWidth; } else if (distanceToB < halfWidth) { pointOnEdge = pb - ba * halfWidth; } return(pointOnEdge); }
public bool NextCorner1(Fix64Vec3 position, Fix64 width, ref Fix64Vec3 corner, ref int index) { //if corner not found stay at current position corner = position; if (Status != ParallelNavMeshPathStatus.Valid) { return(false); } if (index < 0) { index = startIndex; } if (index == MAX_CORNER_COUNT) { //reached last polygon, invalidate path index = -1; Status = ParallelNavMeshPathStatus.Invalid; return(false); } Fix64Vec2 testPosition = new Fix64Vec2(position.x, position.z); int polygonIndex = polygonIndexes[index]; PNavPolygon currentPolygon = island.graph.polygons[polygonIndex]; int nextIndex = index + 1; if (nextIndex >= MAX_CORNER_COUNT) { //reached final polygon index = MAX_CORNER_COUNT; corner = Destination; return(false); } int nextPolygonIndex = polygonIndexes[nextIndex]; PNavPolygon nextPolygon = island.graph.polygons[nextPolygonIndex]; PNavEdge edge = FindEdge(currentPolygon, nextPolygon, width); if (edge != null) { Fix64Vec2 pointOnEdge = Fix64Math.FindNearestPointOnLine(testPosition, edge.pointA, edge.pointB); Fix64 distanceToA = Fix64Vec2.Distance(pointOnEdge, edge.pointA); Fix64 distanceToB = edge.width - distanceToA; Fix64Vec2 ba = edge.pointB - edge.pointA; ba = ba.normalized; Fix64 halfWidth = width / Fix64.two; if (distanceToA < halfWidth) { pointOnEdge = edge.pointA + ba * halfWidth; } else if (distanceToB < halfWidth) { pointOnEdge = edge.pointB - ba * halfWidth; } corner = new Fix64Vec3(pointOnEdge.x, position.y, pointOnEdge.y); index = nextIndex; return(true); } return(false); }
public bool NextCorner(Fix64Vec3 position, Fix64 width, ref Fix64Vec3 corner, ref int index) { //if corner not found stay at current position corner = position; if (Status != ParallelNavMeshPathStatus.Valid) { return(false); } index = BoundaryCheckIndex(index); if (index == -1) { Status = ParallelNavMeshPathStatus.Invalid; return(false); } Fix64Vec2 testPosition = new Fix64Vec2(position.x, position.z); Fix64Vec2 pa = Fix64Vec2.zero; Fix64Vec2 pb = Fix64Vec2.zero; bool pointABSet = false; if (polygonIndexes == null) { //destination polygon and start polygon is the same index = MAX_CORNER_COUNT; } while (index <= MAX_CORNER_COUNT) { if (index == MAX_CORNER_COUNT) { //reached final polygon if (pointABSet) { //check if destination is in the span of pa and pb Fix64Vec2 des = new Fix64Vec2(Destination.x, Destination.z); Fix64Vec2 vd = des - testPosition; Fix64Vec2 va = pa - testPosition; Fix64Vec2 vb = pb - testPosition; bool desInSpan = Fix64Math.InSpan(vd, va, vb); if (!desInSpan) { Fix64Vec2 pointOnEdge = FindNearestPointOnEdge(testPosition, pa, pb, width); index--; corner = new Fix64Vec3(pointOnEdge.x, position.y, pointOnEdge.y); return(true); } } corner = Destination; break; } int nextIndex = index + 1; int polygonIndex = polygonIndexes[index]; PNavPolygon currentPolygon = island.graph.polygons[polygonIndex]; int nextPolygonIndex = polygonIndexes[nextIndex]; PNavPolygon nextPolygon = island.graph.polygons[nextPolygonIndex]; PNavEdge edge = FindEdge(currentPolygon, nextPolygon, width); if (edge != null) { if (!pointABSet) { pa = edge.pointA; pb = edge.pointB; pointABSet = true; } else { Fix64Vec2 va = pa - testPosition; Fix64Vec2 vb = pb - testPosition; Fix64 AXB = Fix64Vec2.Cross(va, vb); Fix64 BXA = Fix64Vec2.Cross(vb, va); Fix64Vec2 pa1 = edge.pointA; Fix64Vec2 pb1 = edge.pointB; Fix64Vec2 va1 = pa1 - testPosition; Fix64Vec2 vb1 = pb1 - testPosition; Fix64 A1XB1 = Fix64Vec2.Cross(va1, vb1); bool sameDirection = A1XB1 * AXB > Fix64.zero; bool va1InsideOfSpan = Fix64Math.InSpan(va1, va, vb); bool vb1InsideOfSpan = Fix64Math.InSpan(vb1, va, vb); if (va1InsideOfSpan) { if (sameDirection) { bool valid = IsValidEdgePoint(testPosition, pb, pa1, width); if (valid) { pa = pa1; } else { va1InsideOfSpan = false; } } else { bool valid = IsValidEdgePoint(testPosition, pa, pa1, width); if (valid) { pb = pa1; } else { va1InsideOfSpan = false; } } } if (vb1InsideOfSpan) { if (sameDirection) { bool valid = IsValidEdgePoint(testPosition, pa, pb1, width); if (valid) { pb = pb1; } else { vb1InsideOfSpan = false; } } else { bool valid = IsValidEdgePoint(testPosition, pb, pb1, width); if (valid) { pa = pb1; } else { vb1InsideOfSpan = false; } } } if (!va1InsideOfSpan && !vb1InsideOfSpan) { Fix64Vec2 pointOnEdge = FindNearestPointOnEdge(testPosition, pa, pb, width); corner = new Fix64Vec3(pointOnEdge.x, position.y, pointOnEdge.y); index--; return(true); } } } else { break; } index = BoundaryCheckIndex(nextIndex); } return(false); }
public PNavMeshPath CalculatePath(Fix64Vec3 start, Fix64Vec3 end) { Fix64Vec2 startPosition = new Fix64Vec2(start.x, start.z); Fix64Vec2 endPosition = new Fix64Vec2(end.x, end.z); PNavPolygon startPolygon = null; PNavPolygon endPolygon = null; bool sameIsland = false; NavMeshAStart astart = null; PNavMeshPath result = new PNavMeshPath(); result.Destination = end; result.Destination2D = endPosition; result.Status = ParallelNavMeshPathStatus.Invalid; result.navMesh = navMesh; PNavIsland endIsland = null; PNavIsland startIsland = null; foreach (PNavIsland island in navMesh.islands) { bool foundStart = false; bool foundEnd = false; foreach (PNavPolygon polygon in island.graph.polygons) { bool isStart = polygon.TestPoint(startPosition); bool isEnd = polygon.TestPoint(endPosition); if (isStart && isEnd) { result.Status = ParallelNavMeshPathStatus.Valid; result.startIndex = -1; return(result); } if (isStart) { startPolygon = polygon; startIsland = island; foundStart = true; } if (isEnd) { endPolygon = polygon; endIsland = island; foundEnd = true; } } if (foundStart && foundEnd) { sameIsland = true; astart = astartDictionary[island]; result.island = island; break; } if (sameIsland) { break; } } if (startPolygon != null && endPolygon == null) { //we find the polygon that is closest to the end position in the start position island sameIsland = true; astart = astartDictionary[startIsland]; result.island = startIsland; endPolygon = startIsland.FindNearestPolygong(endPosition); } else if (startPolygon == null && endPolygon != null) { //we find the polygon that is closest to the start position in the end position island sameIsland = true; astart = astartDictionary[endIsland]; result.island = endIsland; startPolygon = endIsland.FindNearestPolygong(startPosition); } else if (startPolygon == null && endPolygon == null) { //we find the polygon that is closest to the start position //we then find the polygon that is closest to the end position in the same island Fix64 minStart = Fix64.FromDivision(1000, 1); PNavPolygon minStartPolygon = null; PNavIsland minStartIsland = null; foreach (PNavIsland island in navMesh.islands) { foreach (PNavPolygon polygon in island.graph.polygons) { Fix64 dis = Fix64Vec2.Distance(polygon.centroid, startPosition); if (dis < minStart) { minStart = dis; minStartPolygon = polygon; minStartIsland = island; } } } sameIsland = true; astart = astartDictionary[minStartIsland]; result.island = minStartIsland; startPolygon = minStartPolygon; endPolygon = minStartIsland.FindNearestPolygong(endPosition); } if (sameIsland) { if (astart != null && startPolygon != null && endPolygon != null) { NavMeshAStarNode startNode = astart.FindNode(startPolygon.index); NavMeshAStarNode endNode = astart.FindNode(endPolygon.index); NavMeshAStarNode lastNode = null; using (new SProfiler("Pathfinding")) { astart.PrePathFinding(); startNode.startPoint = startPosition; endNode.isLastNode = true; endNode.endPoint = endPosition; lastNode = astart.FindPath(startNode, endNode); } result.polygonIndexes = new int[PNavMeshPath.MAX_CORNER_COUNT]; int startIndex = 127; while (lastNode != null) { result.polygonIndexes[startIndex] = lastNode.UserObject.index; lastNode = (NavMeshAStarNode)lastNode.Parent; startIndex--; } result.startIndex = startIndex + 1; result.Status = ParallelNavMeshPathStatus.Valid; } } return(result); }