public void reset()
{
score = 0;
parent = null;
free = true;
weight = uint.MaxValue;
used = false;
}
public NodeTriangle GetNodeTriangle(PathFinderNode node)
{
foreach (var triangle in allTriangle)
{
if (PointInTriangle(node.position, triangle.p1.position, triangle.p2.position, triangle.p3.position))
{
return(triangle);
}
}
return(new NodeTriangle());
}
public List <PathFinderNode> GetNodeOnNode(PathFinderNode node)
{
foreach (var n in Nodes)
{
var distance = Math.Sqrt(Math.Pow(node.position.X - n.position.X, 2) + Math.Pow(node.position.Y - n.position.Y, 2));
if (distance < 0.5)
{
Console.WriteLine("distance");
return(n.neighbors);
}
}
return(null);
}
public PathFinderNode GetEquivalentNode(PathFinderNode node)
{
if (node == null)
{
return(null);
}
foreach (var n in Nodes)
{
if (n.NavId == node.NavId)
{
return(n);
}
}
return(null);
}
private uint getDjikstraWeight(PathFinderNode current, PathFinderNode neighbour)
{
int manathanDistX = ((int)current.position.X * 100) - ((int)neighbour.position.X * 100);
int manathanDistY = ((int)current.position.Y * 100) - ((int)neighbour.position.Y * 100);
if (manathanDistX < 0)
{
manathanDistX *= -1;
}
if (manathanDistY < 0)
{
manathanDistY *= -1;
}
uint manathanDist = (uint)(manathanDistY + manathanDistX);
return(manathanDist);
}
private bool AddNodeToNavMesh(NavMesh nav, PathFinderNode node, bool throwEvent = false)
{
NodeTriangle triangle;
triangle = nav.GetNodeTriangle(node);
if (throwEvent == true)
{
node.NodeTriangleEvent = triangle;
}
else
{
node.NodeTriangle = triangle;
}
if (triangle.p1 == null || triangle.p2 == null || triangle.p3 == null)
{
return(false);
}
nav.graph.AddVertex(node);
if (NavMesh.NodeToNodeRayCast(node, navMeshes[0].GetEquivalentNode(triangle.p1)))
{
triangle.p1.neighbors.Add(node);
node.neighbors.Add(triangle.p1);
nav.graph.AddEdge(new Edge <PathFinderNode>(node, triangle.p1));
}
if (NavMesh.NodeToNodeRayCast(node, navMeshes[0].GetEquivalentNode(triangle.p2)))
{
triangle.p2.neighbors.Add(node);
node.neighbors.Add(triangle.p2);
nav.graph.AddEdge(new Edge <PathFinderNode>(node, triangle.p2));
}
if (NavMesh.NodeToNodeRayCast(node, navMeshes[0].GetEquivalentNode(triangle.p3)))
{
triangle.p3.neighbors.Add(node);
node.neighbors.Add(triangle.p3);
nav.graph.AddEdge(new Edge <PathFinderNode>(node, triangle.p3));
}
return(true);
}
private List <PathFinderNode> createPath(PathFinderNode currentNode, PathFinderNode startNode)
{
var resultPath = new List <PathFinderNode>();
PathFinderNode backPathNode = currentNode;
while (backPathNode.Equals(startNode) == false)
{
resultPath.Add(backPathNode);
backPathNode = backPathNode.parent;
if (backPathNode == null)
{
break;
}
}
resultPath.Add(startNode);
resultPath.Reverse();
return(resultPath);
}
public static bool isWayClearToNode(PhysicObj startObj, PathFinderNode node)
{
Vertices objVertices = ((PolygonShape)startObj.getBoundsFixture()?.Shape)?.Vertices;
if (((AdditionalFixtureData)startObj.body.FixtureList[0].UserData).isTouching == true)
{
return(false);
}
if (objVertices == null)
{
return(false);
}
bool isVisible = true;
foreach (Vector2 vertex in objVertices)
{
Bloodbender.ptr.world.RayCast((fixture, point, normal, fraction) =>
{
if (fixture.UserData == null)
{
//isVisible = false;
return(-1);
}
if (fixture.IsSensor || ((AdditionalFixtureData)fixture.UserData).physicParent.pathNodeType == PathFinderNodeType.CENTER)
{
return(-1);
}
if (((AdditionalFixtureData)fixture.UserData).physicParent.Equals(startObj))
{
return(-1);
}
isVisible = false;
return(0);
}, vertex + startObj.body.Position, node.position);
if (isVisible == false)
{
return(false);
}
}
return(true);
}
public List <PathFinderNode> runDjikstra(PathFinderNode startNode, PathFinderNode endNode)
{
Clear();
startNode.weight = 0;
PathFinderNode currentNode = startNode;
PathFinderNode bestNeighbour;
while (currentNode != null && currentNode != endNode)
{
currentNode.used = true;
bestNeighbour = null;
foreach (PathFinderNode neighbour in currentNode.neighbors)
{
if (neighbour.used == false)
{
uint linkWeight;
//if (currentNode.Equals(startNode))
// linkWeight = uint.MaxValue;
//else
linkWeight = getDjikstraWeight(currentNode, neighbour);
if (currentNode.weight + linkWeight < neighbour.weight)
{
neighbour.weight = currentNode.weight + linkWeight;
neighbour.parent = currentNode;
}
if (bestNeighbour == null || neighbour.weight < bestNeighbour.weight)
{
bestNeighbour = neighbour;
}
}
}
currentNode = bestNeighbour;
}
if (endNode != null)
{
return(createPath(endNode, startNode));
}
return(null);
}
private void RemoveNodeFromNavMesh(NavMesh nav, PathFinderNode node)
{
if (!nav.graph.ContainsVertex(node))
{
return;
}
foreach (var neighbor in node.neighbors)
{
Edge <PathFinderNode> edge = null;
if (nav.graph.TryGetEdge(node, neighbor, out edge))
{
nav.graph.RemoveEdge(edge);
}
neighbor.neighbors.Remove(node);
}
if (nav.graph.ContainsVertex(node))
{
nav.graph.RemoveVertex(node);
}
node.neighbors.Clear();
}
public static bool NodeToNodeRayCast(PathFinderNode node1, PathFinderNode node2)
{
bool isVisible = true;
Bloodbender.ptr.world.RayCast((fixture, point, normal, fraction) =>
{
if (fixture.UserData == null)
{
isVisible = false;
return(0);
}
if (fixture.IsSensor || ((AdditionalFixtureData)fixture.UserData).physicParent.pathNodeType == PathFinderNodeType.CENTER)
{
return(-1);
}
isVisible = false;
return(0);
}, node2.position, node1.position);
return(isVisible);
}
public void GenerateNavMesh()
{
List <Triangulator.Geometry.Point> vertices = new List <Triangulator.Geometry.Point>();
Nodes.ForEach(i => vertices.Add(new Triangulator.Geometry.Point(i.position.X * Bloodbender.meterToPixel, i.position.Y * Bloodbender.meterToPixel)));
var list = Triangulator.Delauney.Triangulate(vertices);
foreach (var triangle in list)
{
PathFinderNode node1 = GetNodeFromPosition(vertices[triangle.p1]);
PathFinderNode node2 = GetNodeFromPosition(vertices[triangle.p2]);
PathFinderNode node3 = GetNodeFromPosition(vertices[triangle.p3]);
graph.AddVertex(node1);
graph.AddVertex(node2);
graph.AddVertex(node3);
node1.neighbors.Add(node2);
node1.neighbors.Add(node3);
graph.AddEdge(new Edge <PathFinderNode>(node1, node2));
graph.AddEdge(new Edge <PathFinderNode>(node1, node3));
node2.neighbors.Add(node1);
node2.neighbors.Add(node3);
graph.AddEdge(new Edge <PathFinderNode>(node2, node3));
node3.neighbors.Add(node1);
node3.neighbors.Add(node2);
NodeTriangle nodeTriangle = new NodeTriangle();
nodeTriangle.p1 = node1;
nodeTriangle.p2 = node2;
nodeTriangle.p3 = node3;
allTriangle.Add(nodeTriangle);
}
DeleteInvalidLink();
ThickenessCorrection();
}
public void UpdateTriangleForNode(PathFinderNode node)
{
}
public void AddNode(PathFinderNode node)
{
navMeshes.ForEach(item => item.AddNode(node.Copy()));
}
public void AddNode(PathFinderNode nodePosition)
{
Nodes.Add(nodePosition);
graph.AddVertex(nodePosition);
}
