void scanSelf()
{
if (this.Visible)
{
PointGraph navg = (PointGraph)AstarPath.active.graphs[0];
AstarPath.active.AddWorkItem(new AstarPath.AstarWorkItem(delegate() {
//float f = Time.realtimeSinceStartup;
PointNode p = navg.AddNode((Int3)pathLocation);
if (p != null)
{
Dictionary <GraphNode, float> cons = new Dictionary <GraphNode, float>();
navg.GetNodes((delegate(GraphNode node) {
float distance;
if (navg.IsValidConnection(p, node, out distance))
{
//Debug.Log(distance);
cons[node] = distance;
}
return(true);
}));
foreach (GraphNode g in cons.Keys)
{
p.Area = 0;
g.Area = 0;
p.AddConnection(g, (uint)cons[g]);
g.AddConnection(p, (uint)cons[g]);
}
//Debug.Log("Found " + cons.Count + " valid connections");
//Debug.Log("Scan time: " + (Time.realtimeSinceStartup - f));
}
}, null));
}
}
void BuildPointNode(Transform obstacle)
{
float xSize = obstacle.transform.localScale.x / 2 + 0.35f;
float zSize = obstacle.transform.localScale.z / 2 + 0.35f;
Vector3 forwardDirection = obstacle.forward * zSize;
Vector3 rightDirection = obstacle.right * xSize;
//Build four point around obstacle
for (int i = -1; i <= 1; i = i + 2)
{
for (int j = -1; j <= 1; j = j + 2)
{
Vector3 pos = i * forwardDirection + j * rightDirection + obstacle.position;
//Check the new point is Walkable
if (CheckIsWalkabel(pos))
{
PointNode pointNode = new PointNode(pos);
grid.Add(pointNode);
}
}
}
}
public override IEnumerable <Progress> ScanInternal()
{
PointNode[][] graph = new PointNode[width][];
for (int i = 0; i < width; i++)
{
graph[width] = new PointNode[height];
for (int j = 0; j < height; j++)
{
Vector3 nodePos = CalculateNodePosition(i, j);
RaycastHit2D hit = Physics2D.Raycast(nodePos, Vector2.down, scale);
if (hit.collider != null)
{
Vector2 groundNormal = hit.normal;
float slopeAngle = Vector2.Angle(groundNormal, Vector2.up);
if (slopeAngle <= maxAngle)
{
CreateNode(nodePos);
}
}
}
}
yield break;
}
private static void UpdateFreeNodes(PointNode node, NodeStates state) {
if (state == NodeStates.Free)
ActiveMap.FreeNodes.Add(node);
else
RemoveNodeFromFreeNodes(node);
}
public bool NodeIsFree(PointNode node) => node.State == NodeStates.Free;
private void CreatePointNode(Position pos) {
PointNode node = new PointNode(pos);
nodes[pos.X, pos.Y, pos.Z] = node;
FreeNodes.Add(node);
}
public PointEdge(PointNode start, PointNode end, int index)
{
this.Start = start;
this.End = end;
this.Index = index;
}
public virtual void Move(PointNode newPointNode)
{
ChangePosition(newPointNode.Position);
ChangeCurrentNode(newPointNode);
}
public void Add(PointNode p)
{
controlPoints.Add(p);
}
public override void Move(PointNode newNode)
{
base.Move(newNode);
ChangeNodeState(newNode, NodeStates.OccupiedByItem);
}
protected void ChangePreviousNode(PointNode node) => PreviousNode = node;
private void curveIntersectionToolStripMenuItem_Click(object sender, EventArgs e)
{
TopoShape ellipseArc = GlobalInstance.BrepTools.MakeEllipseArc(Vector3.ZERO, 100, 50, 0, 90, Vector3.UNIT_Z);
TopoShape circle = GlobalInstance.BrepTools.MakeCircle(Vector3.ZERO, 60, Vector3.UNIT_Z);
IntersectionLineCurve intersector = new IntersectionLineCurve();
intersector.SetCurve(ellipseArc);
if (intersector.Perform(circle))
{
PointStyle ps = new PointStyle();
ps.SetMarker("plus");
ps.SetPointSize(10);
int nCount = intersector.GetPointCount();
for (int ii = 1; ii <= nCount; ++ii)
{
if (intersector.GetSquareDistance(ii) < 0.001)
{
Vector3 pt = intersector.GetPoint(ii);
PointNode pn = new PointNode();
pn.SetPoint(pt);
pn.SetPointStyle(ps);
renderView.ShowSceneNode(pn);
}
}
}
renderView.ShowGeometry(ellipseArc, ++shapeId);
renderView.ShowGeometry(circle, ++shapeId);
renderView.RequestDraw();
}
private void pointToolStripMenuItem_Click(object sender, EventArgs e)
{
PointNode pn = new PointNode();
pn.SetPoint(new Vector3(100, 100, 100));
PointStyle ps = new PointStyle();
ps.SetMarker("plus");
ps.SetPointSize(10);
pn.SetPointStyle(ps);
renderView.ShowSceneNode(pn);
renderView.RequestDraw();
}
/// <summary>
/// ��ȡVCT��ʵ��ڵ�
/// </summary>
public override EntityNode GetEntityNode()
{
try
{
m_PointNode = new PointNode();
IFeature pFeature = this.Feature as IFeature;
///��ʶ�븳ֵ
int dBSMIndex = -1;
dBSMIndex = this.Feature.Fields.FindField(m_strEntityIDFiled);
if (dBSMIndex != -1)
m_PointNode.EntityID=Convert.ToInt32( this.Feature.get_Value(dBSMIndex));
///Ҫ�ش��븳ֵ
//int dSYDMIndex = -1;
//dSYDMIndex = this.Feature.Fields.FindField("YSDM");
//if (dSYDMIndex != -1)
// m_PointNode.FeatureCode = this.Feature.get_Value(dSYDMIndex).ToString();
//string sAttriTableName = (pFeature.Class as IDataset).Name;
//m_PointNode.FeatureCode = MetaDataFile.GetFeatureCodeByName(sAttriTableName);
m_PointNode.FeatureCode = this.FeatureCode;
///�����������ͺ�ͼ�α��ֱ��븳ֵ
m_PointNode.PointCount = 1;
//ͨ���������õ����ͣ��ֱ�ͳһ����Ϊ�����㣩
m_PointNode.PointType =Convert.ToInt32( Metadata.MetaDataFile.GraphConfig.GetGraphSymbol("POINTFEATURETYPE", "SinglePoint"));
//m_PointNode.Representation = pFeature.Class.AliasName;
//��ȡ�ռ���������ֵ
IPoint pPoint = pFeature.Shape as IPoint;
PointInfoNode pInfoNode =new PointInfoNode(pPoint.X,pPoint.Y);
m_PointNode.PointInfoNode = pInfoNode;
return m_PointNode;
}
catch(Exception ex)
{
LogAPI.WriteErrorLog(ex);
return null;
}
}
/// <summary>
/// ��ȡVCT��ʵ��ڵ�
/// </summary>
private PointNode ReadPointNode()
{
if (this.m_streamReader != null)
{
string strLine = ""; //��ȡ�������ֻ���
if (m_bFoundPointBegin == false)
{
strLine = this.ReadLine(true);
//���ҵ�ʵ��ڵ�Ŀ�ʼ��ǡ�PointBegin��
if (strLine.IndexOf("PointBegin", System.StringComparison.OrdinalIgnoreCase) > -1)
{
m_bFoundPointBegin = true;
}
}
if (m_bFoundPointBegin == true)
{
strLine = this.ReadLine(true);
//���ҵ�ʵ��ڵ�Ľ�����ǡ�PointEnd��
if (strLine.IndexOf("PointEnd", System.StringComparison.OrdinalIgnoreCase) > -1)
return null;
else
{
PointNode pointNode = new PointNode();
//�Ȼ�ȡ��ʵ���ʶ��
int nEntityID = -1;
if (VCTFile.ConvertToInt32(strLine, out nEntityID))
pointNode.EntityID = nEntityID;
else
{
LogAPI.WriteLog("��ȡ��ʵ��ı�ʶ��ʧ�ܣ����������Ƿ���ϱ���");
LogAPI.WriteLog("��ȡ���������������" + m_nReadLineCount + "��");
return null;
}
//�ٻ�ȡҪ�ر���
pointNode.FeatureCode = this.ReadLine(false);
//��ȡͼ�α��ֱ���
pointNode.Representation = this.ReadLine(false);
//��ȡ�����������
int nPointType = -1;
if (VCTFile.ConvertToInt32(this.ReadLine(false),out nPointType))
pointNode.PointType = nPointType;
else
{
LogAPI.WriteLog("��ȡ��ʵ�����������ʧ�ܣ����������Ƿ���ϱ���");
LogAPI.WriteLog("��ȡ���������������" + m_nReadLineCount + "��");
return null;
}
//��ȡ��ĸ���
this.ReadLine(false);
//��ȡ�������
PointInfoNode pointInfoNode = new PointInfoNode(this.ReadLine(false));
pointNode.PointInfoNode = pointInfoNode;
return pointNode;
}
}
}
return null;
}
protected void ToFreeNode(PointNode node) => ChangeNodeState(node, NodeStates.Free);
public override void Move(PointNode newPointNode)
{
ChangePreviousNode(Node);
base.Move(newPointNode);
}
internal void AddPoint(PointNode point)
{
_points.ForEach(p => p.AddConnection(point));
_points.Add(point);
}
private IEnumerable <PointNode> GetNextPoints(PointNode start, IDictionary <Point, HashSet <EquipmentType> > visited)
{
var neighbourPoints = new HashSet <Point>
{
new Point(start.P.X + 1, start.P.Y),
new Point(start.P.X - 1, start.P.Y),
new Point(start.P.X, start.P.Y + 1),
new Point(start.P.X, start.P.Y - 1)
};
var currentTerrain = this._terrain[start.P];
var nextPoints = new HashSet <PointNode>();
foreach (var neighbour in neighbourPoints)
{
if (neighbour.X < 0 || neighbour.X > this._furthestPoint.X || neighbour.Y < 0 ||
neighbour.Y > this._furthestPoint.Y)
{
continue;
}
var neighbourTerrain = this._terrain[neighbour];
var nextPoint = new PointNode
{
Equipment = start.Equipment,
P = neighbour,
Time = start.Time + 1
};
switch (currentTerrain)
{
case TerrainType.Narrow:
switch (neighbourTerrain)
{
case TerrainType.Rocky:
nextPoint.Equipment = EquipmentType.Torch;
break;
case TerrainType.Wet:
nextPoint.Equipment = EquipmentType.Neither;
break;
case TerrainType.Narrow:
break;
default:
throw new ArgumentOutOfRangeException();
}
break;
case TerrainType.Rocky:
switch (neighbourTerrain)
{
case TerrainType.Rocky:
break;
case TerrainType.Wet:
nextPoint.Equipment = EquipmentType.ClimbingGear;
break;
case TerrainType.Narrow:
nextPoint.Equipment = EquipmentType.Torch;
break;
default:
throw new ArgumentOutOfRangeException();
}
break;
case TerrainType.Wet:
switch (neighbourTerrain)
{
case TerrainType.Rocky:
nextPoint.Equipment = EquipmentType.ClimbingGear;
break;
case TerrainType.Wet:
break;
case TerrainType.Narrow:
nextPoint.Equipment = EquipmentType.Neither;
break;
default:
throw new ArgumentOutOfRangeException();
}
break;
default:
throw new ArgumentOutOfRangeException();
}
if (nextPoint.Equipment != start.Equipment)
{
// Changing tools to get to this point, add tool change time
nextPoint.Time += 7;
}
if (!visited.ContainsKey(nextPoint.P) || !visited[nextPoint.P].Contains(nextPoint.Equipment))
{
// Not visited this point before, or not visited it with this equipment
nextPoints.Add(nextPoint);
}
}
return(nextPoints);
}
public void Insert(PointNode p, int idx)
{
controlPoints.Insert(idx + 1, p);
}
public static Position GetNextPosition(PointNode node, DirectionOption directionOption)
{
sbyte[] coords = node.Position.Coordinates;
coords[directionOption.AxisNumber] += directionOption.Direction;
return(new Position(coords));
}
protected void ChangeCurrentNode(PointNode newPointNode) => Node = newPointNode;
private static void Initialize(PointNode start, PointNode finish)
{
endNode = finish;
ZeroizeNode(start);
InitializeNodeSets(start);
}
protected override IEnumerable <Progress> ScanInternal()
{
TilemapNavigator navigator = TilemapNavigator.Instance;
BoundsInt mapBounds = navigator.GetTilemapBounds();
Dictionary <int, Dictionary <int, PointNode> > gridNodes = new Dictionary <int, Dictionary <int, PointNode> >();
// Find all tiles
for (int xPos = mapBounds.xMin; xPos <= mapBounds.xMax; xPos++)
{
for (int yPos = mapBounds.yMin; yPos <= mapBounds.yMax; yPos++)
{
Vector3Int cellPos = GetCellPos(xPos, yPos);
if (navigator.HasTile(cellPos))
{
if (!gridNodes.ContainsKey(xPos))
{
gridNodes.Add(xPos, new Dictionary <int, PointNode>());
}
gridNodes[xPos].Add(yPos, CreateNode(cellPos));
}
}
}
// Connect adjacent tiles
for (int xPos = mapBounds.xMin; xPos <= mapBounds.xMax; xPos++)
{
for (int yPos = mapBounds.yMin; yPos <= mapBounds.yMax; yPos++)
{
PointNode node = GetNodeInDict(gridNodes, xPos, yPos);
if (node != null && node.Walkable)
{
Vector3Int cellPos = GetCellPos(xPos, yPos);
LevelTile tile = navigator.GetTile(cellPos);
List <Connection> connections = new List <Connection>();
Vector2Int[] neighbors =
{
Vector2Int.up,
Vector2Int.down,
Vector2Int.left,
Vector2Int.right,
};
node.Penalty = tile.Cost;
foreach (Vector2Int offset in neighbors)
{
PointNode neighborNode = GetNodeInDict(gridNodes, xPos + offset.x, yPos + offset.y);
if (neighborNode != null && neighborNode.Walkable)
{
connections.Add(new Connection(neighborNode, 0));
}
}
node.connections = connections.ToArray();
}
}
}
List <PointNode> allNodes = new List <PointNode>();
foreach (KeyValuePair <int, Dictionary <int, PointNode> > column in gridNodes)
{
foreach (KeyValuePair <int, PointNode> cell in column.Value)
{
allNodes.Add(cell.Value);
}
}
nodes = allNodes.ToArray();
yield break;
}
private static void ZeroizeNode(PointNode node)
{
node.PreviousPoint = null;
node.PathLengthFromStart = 0;
node.HeuristicPathLength = 0;
}
public static void RemoveItem(PointNode itemNode) {
Item item = FindItem(itemNode);
item.PutInPool();
ActiveMap.Items.Remove(item);
}
private static bool NextNodeIsEnd()
{
currentNode = pendingNodes.OrderBy(node => node.EstimateFullPathLength).First();
return(currentNode == endNode);
}
public static void ChangeNodeState(PointNode node, NodeStates state) {
UpdateFreeNodes(node, state);
node.State = state;
}
private void HashEdge(PointNode e)
{
hash[HashKey(e.Point)] = e;
}
private static void RemoveNodeFromFreeNodes(PointNode node) {
int index = ActiveMap.FreeNodes.IndexOf(node);
if (index >= 0)
ActiveMap.FreeNodes.RemoveAt(index);
}
/// <summary>
/// Calculates Delaunay triangulation using the more efficient Sweep-Circle algorithm.
/// Considered one of the fastest algorithms in empirical tests.
/// Expected runtime is O(n log n).
/// </summary>
/// <remarks>
/// http://cglab.ca/~biniaz/papers/Sweep%20Circle.pdf
/// </remarks>
public override void CalculateDelaunay()
{
if (!CheckDelaunayConditions())
{
return;
}
// Calculate origin point and seed triangle
(Point origin, Triangle seed) = CalculateOriginSeedTriangle(out int i0, out int i1, out int i2);
center = origin;
if (i0 == -1)
{
SetDelaunayResult(new List <Triangle>());
return; // No Delaunay triangulation exists
}
var len = Points.Length;
polarPoints = new PolarPoint[len];
var seed1 = new PolarPoint();
var seed2 = new PolarPoint();
var seed3 = new PolarPoint();
for (var i = 0; i < len; i++)
{
var p = Points[i];
var pp = new PolarPoint(
p,
GeoMath.EuclideanDistance2(center, p),
GeoMath.PolarPseudoAngle(center, p)
);
if (p.Equals(seed.A))
{
seed1 = pp;
}
else if (p.Equals(seed.B))
{
seed2 = pp;
}
else if (p.Equals(seed.C))
{
seed3 = pp;
}
polarPoints[i] = pp;
}
// Sort polar points by radius in ascending order
QuickSortPoints(polarPoints, 0, len - 1);
// Hashtable for edges of convex hull
hashSize = (int)Math.Sqrt(len);
hash = new PointNode[hashSize + 2];
// Circular doubly-linked list which will hold the convex hull
var e = hull = InsertNode(seed1);
HashEdge(e);
e.t = 0;
e = InsertNode(seed2, e);
HashEdge(e);
e.t = 1;
e = InsertNode(seed3, e);
HashEdge(e);
e.t = 2;
var maxTriangles = 2 * len - 5;
triangles = new PolarPoint[maxTriangles * 3];
halfedges = new int[maxTriangles * 3];
AddTriangle(seed1, seed2, seed3, -1, -1, -1);
var prev = new PolarPoint(new Point(-1, -1), -1, -1);
for (var i = 0; i < len; i++)
{
var pp = polarPoints[i];
if (seed.HasVertex(pp.CartesianPoint))
{
continue; // Skip seed triangle points
}
if (prev.CartesianPoint.Equals(pp.CartesianPoint))
{
continue; // Ignore duplicate point
}
// Find a visible edge on the convex hull using edge hash
var startKey = HashKey(pp);
var key = startKey;
PointNode start;
do
{
start = hash[key];
key = (key + 1) % hashSize;
}while ((start == null || start.Removed) && key != startKey);
e = start;
while (GeoMath.TriangleAreaDeterminant(pp.CartesianPoint, e.Point.CartesianPoint, e.Next.Point.CartesianPoint) <= 0)
{
e = e.Next;
#if DEBUG
if (e == start)
{
throw new Exception("Processing error. Input points invalid or seed triangle wrongly oriented.");
}
#endif
}
var walkBack = e == start;
// Add first triangle
var t = AddTriangle(e.Point, pp, e.Next.Point, -1, -1, e.t);
e.t = t; // Keep track of boundary triangles on hull
e = InsertNode(pp, e);
// Recursively flip triangles from the point until they satisfy the Delaunay condition
e.t = Legalize(t + 2);
if (e.Previous.Previous.t == halfedges[t + 1])
{
e.Previous.Previous.t = t + 2;
}
// Walk forward through the hull, adding more triangles and flipping recursively
var q = e.Next;
while (GeoMath.TriangleAreaDeterminant(pp.CartesianPoint, q.Point.CartesianPoint, q.Next.Point.CartesianPoint) > 0)
{
t = AddTriangle(q.Point, pp, q.Next.Point, q.Previous.t, -1, q.t);
q.Previous.t = Legalize(t + 2); // Flipping
hull = RemoveNode(q);
q = q.Next;
}
if (walkBack)
{
// Walk backward from the other side, adding more triangles and flipping recursively
q = e.Previous;
while (GeoMath.TriangleAreaDeterminant(pp.CartesianPoint, q.Previous.Point.CartesianPoint, q.Point.CartesianPoint) > 0)
{
t = AddTriangle(q.Previous.Point, pp, q.Point, -1, q.t, q.Previous.t);
Legalize(t + 2); // Flipping
q.Previous.t = t;
hull = RemoveNode(q);
q = q.Previous;
}
}
// Save the two new edges in the hashtable
HashEdge(e);
HashEdge(e.Previous);
prev = pp;
}
// Create Delaunay triangles from points
var delaunay = new List <Triangle>(trianglesPointIndex / 3);
for (var i = 0; i < trianglesPointIndex;)
{
delaunay.Add(new Triangle(
triangles[i++].CartesianPoint,
triangles[i++].CartesianPoint,
triangles[i++].CartesianPoint
));
}
SetDelaunayResult(delaunay);
}
/// <summary>
/// д��VCT��ʵ��ڵ�
/// </summary>
/// <param name="pointNode">VCT��ʵ��ڵ�</param>
public bool WritePointNode(PointNode pointNode)
{
if (this.m_streamWriter != null)
{
if (this.m_bFoundPointBegin == false)
{
this.m_streamWriter.WriteLine("PointBegin");
this.m_bFoundPointBegin = true;
}
this.m_streamWriter.WriteLine(pointNode);
return true;
}
return false;
}
//Calculate The Cost from current node To target node
float CalculateMovementCostToTargetNode(PointNode _node, PointNode _targetNode)
{
return(Vector3.Distance(_node.position, _targetNode.position));
}
public void AncestorMoveHappened(PointNode nextNode)
{
Move(nextNode);
MovePreviousSectionOrToFreeNode();
}
public void FindingPath(PathRequest request, Action <PathResponse> callBack)
{
if (_pointGrid.grid == null)
{
return;
}
Heap <PointNode> openList = new Heap <PointNode>();
List <PointNode> closeList = new List <PointNode>();
PointNode agentNode = _pointGrid.GetPointNodeFromGridByPosition(request.startNode);
PointNode targetNode = _pointGrid.GetPointNodeFromGridByPosition(request.targetNode);
if (targetNode == null)
{
return;
}
openList.Add(agentNode);
while (openList.Count > 0)
{
PointNode currentNode = openList.Pop();
if (currentNode == null)
{
return;
}
//Check If Reach The Goal
if (_pointGrid.CheckIfPointsLinkedTogether(currentNode, targetNode))
{
targetNode.parent = currentNode;
break;
}
closeList.Add(currentNode);
List <PointNode> neighboursNode = currentNode.neighbors;
for (int i = 0; i < neighboursNode.Count; i++)
{
if (closeList.Contains(neighboursNode[i]))
{
continue;
}
else
{
float gCost = currentNode.gCost + CalculateMovementCostToTargetNode(currentNode, neighboursNode[i]);
float hCost = CalculateMovementCostToTargetNode(neighboursNode[i], targetNode);
if (openList.Contains(neighboursNode[i]) == false)
{
//Set fValue
neighboursNode[i].fCost = hCost + gCost;
neighboursNode[i].parent = currentNode;
openList.Add(neighboursNode[i]);
}
else
{
float new_fCost = gCost + hCost;
//Check if new fvalue is best than the older
if (neighboursNode[i].fCost > new_fCost)
{
//Update fValue
neighboursNode[i].fCost = new_fCost;
neighboursNode[i].parent = currentNode;
}
}
}
}
}
// Create The Path
Vector3[] path = CreatePath(agentNode, targetNode);
//Create Response To Call Back it
PathResponse pathResponse = new PathResponse(path, true, request.callBack);
//Call Back
callBack(pathResponse);
return;
}
