static List <PathfindNode> GetAllBridges(int from, int to, WalkType walkType)
{
List <PathfindNode> tmpIntermediate = new List <PathfindNode>();
for (int bri = instance.Bridges.Count - 1; bri >= 0; bri--)
{
PathfindNode bridge = instance.Bridges [bri];
PathfindNodeGroupParameter groupParameter = bridge.GetParameter(walkType);
if (groupParameter.Group == from || groupParameter.Group == to)
{
for (int i = 0; i < groupParameter.GroupBridge.Count; i++)
{
int bridgeGroup = groupParameter.GroupBridge [i];
if (bridgeGroup == to || bridgeGroup == from)
{
tmpIntermediate.Add(bridge);
}
}
}
}
return(tmpIntermediate);
}
void OnDrawGizmos()
{
if (CompleteNodeArea == null || CompleteNodeArea.AllNodes == null)
{
return;
}
if (Editable == false)
{
return;
}
//CompleteNodeArea.AllNodes = CompleteArea;
// if(CompleteNodeArea.AllNodes == null )
// CompleteNodeArea.AllNodes = CompleteArea;
for (int i = 0; i < CompleteNodeArea.AllNodes.Count; i++)
{
PathfindNode node = CompleteNodeArea.AllNodes [i];
Color GizmoColor = WalkableColor;
if (drawWalks)
{
GizmoColor = GetColorWalk(node);
}
else
{
GizmoColor = GetColorGroup(node);
}
Gizmos.color = GizmoColor;
// node.Walkable?WalkableColor:NotWalkableColor;
float size = node.Selected ? 2 : 1.0f;
Gizmos.DrawCube(new Vector3(node.Point.X, node.Point.Y, 0) * tileSize + transform.position, Vector3.one * size * 0.5f);
}
// for(int i = 0;i<= AreaRect.width;i++)
// {
// for(int j = 0;j<= AreaRect.height;j++)
// {
// Gizmos.DrawCube(new Vector3(i+(int)AreaRect.x,0,j+(int)AreaRect.y) *2* tileSize,Vector3.one);
// }
// }
}
static bool BelongSameGroup(PathfindNode startPoint, PathfindNode endPoint, WalkType walktype)
{
List <int> StartsGroups = new List <int>();
foreach (int b in startPoint.GetParameter(walktype).GroupBridge)
{
StartsGroups.Add(b);
}
StartsGroups.Add(startPoint.GetParameter(walktype).Group);
if (StartsGroups.Contains(endPoint.GetParameter(walktype).Group))
{
return(true);
}
foreach (int b in endPoint.GetParameter(walktype).GroupBridge)
{
if (StartsGroups.Contains(b))
{
return(true);
}
}
return(false);
//return (startPoint.GetParameter(walkType).Group != endPoint.GetParameter (walkType).Group);
}
public NodesArea GetAllowedNode(List <WalkType> walksTypes)
{
NodesArea AllowArea = new NodesArea(CompleteNodeArea.Width, CompleteNodeArea.Height);
for (int i = 0; i < CompleteNodeArea.AllNodes.Count; i++)
{
PathfindNode node = CompleteNodeArea.AllNodes[i];
if (node.IsWalkableBy(walksTypes))
{
AllowArea.AddNode(node.Point, node);
}
}
return(AllowArea);
// Dictionary<Point,PathfindNode> nodes = new Dictionary<Point,PathfindNode>();
// for(int i =0 ;i< CompleteNodeArea.AllNodes.Count;i++)
// {
// PathfindNode node = CompleteNodeArea.AllNodes[i];
// if(node.IsWalkableBy(walksTypes))
// nodes.Add(node.Point,node);
//
// }
//
// return nodes;
}
public AStarNode(PathfindNode p)
{
fscore = 0;
gscore = 0;
Position = p;
}
public static Path AStartPathFinding(PathfindNode startPoint, PathfindNode endPoint, Dictionary <Point, PathfindNode> reachablePoints, int loopMaxCount = int.MaxValue)
{
if (reachablePoints.ContainsKey(endPoint.Point) == false)
{
return(null);
}
if (PathFindingManager.PrecalculatedPaths != null && PathFindingManager.PrecalculatedPaths.ContainsKey(new PointCouple(startPoint.Point, endPoint.Point)))
{
return(PathFindingManager.PrecalculatedPaths[new PointCouple(startPoint.Point, endPoint.Point)].Clone());
}
List <AStarNode> openSet = new List <AStarNode>();
Dictionary <Point, AStarNode> openSetDico = new Dictionary <Point, AStarNode>();
openSet.Add(new AStarNode(startPoint));
openSetDico.Add(startPoint.Point, openSet[openSet.Count - 1]);
Dictionary <Point, AStarNode> closedSet = new Dictionary <Point, AStarNode>();
float fscore; // = (endPoint-startPoint).MagnitudeZero();
fscore = Point.MagnitudeZero(endPoint.Point - startPoint.Point); //+(1 -Vector2.Dot(toNeighbor,toEnd))*20;
int loopLimit = loopMaxCount;
while (openSet.Count > 0 && loopLimit > 0)
{
//Debug.Log(openSet.Count);
loopLimit--;
openSet.Sort();
AStarNode current = openSet[0];
if (current.Position == endPoint)
{
Path path = new Path(endPoint);
while (current.Parent != null)
{
path.steps.Add(instance.GetNodeAt(current.Position.Point));
current = current.Parent;
}
path.steps.Reverse();
if (PathFindingManager.PrecalculatedPaths == null)
{
PathFindingManager.PrecalculatedPaths = new Dictionary <PointCouple, Path>();
}
PathFindingManager.PrecalculatedPaths.Add(new PointCouple(startPoint.Point, endPoint.Point), path);
return(path.Clone());
}
openSetDico.Remove(openSet[0].Position.Point);
openSet.RemoveAt(0);
if (closedSet.ContainsKey(current.Position.Point))
{
int fdfg = 4;
}
closedSet.Add(current.Position.Point, current);
for (int i = 0, NeighbourhoodLength = Neighbourhood.Length; i < NeighbourhoodLength; i++)
{
PathFindNeighbourg neighbourPath = Neighbourhood [i];
PathfindNode neighbour = new PathfindNode(neighbourPath.offset);
PathfindNode neigbourRealPosition = neighbour + current.Position;
if (reachablePoints.ContainsKey(neigbourRealPosition.Point) == false)
{
continue;
}
AStarNode AstarNeighbor = new AStarNode(neigbourRealPosition);
AstarNeighbor.Position = neigbourRealPosition;
//current.Position;
AstarNeighbor.Parent = current;
if (closedSet.ContainsKey(AstarNeighbor.Position.Point))
{
continue;
}
float tentativeGscore = current.gscore + neighbourPath.cost;
AStarNode InSetNeighbor = null;
//GetNodeInList(AstarNeighbor,openSet);
if (openSetDico.ContainsKey(AstarNeighbor.Position.Point))
{
InSetNeighbor = openSetDico [AstarNeighbor.Position.Point];
}
if (InSetNeighbor == null || tentativeGscore < InSetNeighbor.gscore)
{
bool needAddToSet = false;
if (InSetNeighbor == null)
{
InSetNeighbor = new AStarNode(neigbourRealPosition);
needAddToSet = true;
}
InSetNeighbor.Parent = current;
InSetNeighbor.gscore = tentativeGscore;
InSetNeighbor.fscore = InSetNeighbor.gscore + heuristique(AstarNeighbor, endPoint.Point, current.Position.Point);
if (needAddToSet)
{
AstarNeighbor.fscore = InSetNeighbor.fscore;
AstarNeighbor.gscore = InSetNeighbor.gscore;
openSet.Add(AstarNeighbor);
openSetDico.Add(AstarNeighbor.Position.Point, openSet [openSet.Count - 1]);
}
}
}
}
return(null);
}
static int GetBestBridgeIn(WalkType walkType, List <PathfindNode> tmpIntermediate, PathfindNode previousPoint, PathfindNode endPoint)
{
int bestBridge = -1;
float bestDistance = float.MaxValue;
for (int i = 0; i < tmpIntermediate.Count; i++)
{
//float dist = (tmpIntermediate [i].Point - previousPoint.Point).EuclidiantMagnitude ();
float dist = (tmpIntermediate[i].Point.ToVector2() - previousPoint.Point.ToVector2()).magnitude; //NawakTools.GetDistanceToLine(tmpIntermediate [i].Point.ToVector2(), previousPoint.Point.ToVector2(),endPoint.Point.ToVector2());//(tmpIntermediate [i].Point - previousPoint.Point).EuclidiantMagnitude ();
float weight = 0.3f;
//Debug.Log("befor: " + dist);
dist += weight * (previousPoint.Point - tmpIntermediate [i].Point).Magnitude();
//dist += weight*(endPoint.Point - tmpIntermediate [i].Point).Magnitude();
//Debug.Log("after: " + dist);
if (dist < bestDistance)
{
bestBridge = i;
bestDistance = dist;
}
}
if (bestBridge == -1)
{
Debug.Break();
}
//Debug.Log("bestbridgr :" + tmpIntermediate[bestBridge] + " : " + previousPoint + " : " + endPoint);
return(bestBridge);
}
//return succes
private static bool GetIntermediatesPoint(int startGroup, int endGroup, WalkType walkType, ref List <int> WalkedGroup, ref List <PathfindNode> intermediate, PathfindNode targetPoint)
{
List <PathfindNode> tmpIntermediate = new List <PathfindNode>();
List <PathfindNode> stepIntermediate = new List <PathfindNode>();
for (int bri = instance.Bridges.Count - 1; bri >= 0; bri--)
{
PathfindNode bridge = instance.Bridges [bri];
PathfindNodeGroupParameter groupParameter = bridge.GetParameter(walkType);
if (groupParameter.Group == startGroup || groupParameter.Group == endGroup)
{
for (int i = 0; i < groupParameter.GroupBridge.Count; i++)
{
int bridgeGroup = groupParameter.GroupBridge [i];
if (bridgeGroup == endGroup || bridgeGroup == startGroup)
{
groupParameter.GroupBridge [i] = endGroup;
groupParameter.Group = startGroup;
tmpIntermediate.Add(bridge);
}
}
}
}
if (tmpIntermediate.Count > 0)
{
int bestBridge = GetBestBridgeIn(walkType, tmpIntermediate, intermediate [intermediate.Count - 1], targetPoint);
intermediate.Add(tmpIntermediate [bestBridge]);
PathfindNodeGroupParameter NextPointIntermediate = tmpIntermediate [bestBridge].GetParameter(walkType);
return(true);
}
else
{
WalkedGroup.Add(startGroup);
for (int bri = instance.Bridges.Count - 1; bri >= 0; bri--)
{
PathfindNode bridge = instance.Bridges [bri];
PathfindNodeGroupParameter groupParameter = bridge.GetParameter(walkType);
List <int> bridgeGroups = new List <int>(groupParameter.GroupBridge);
bridgeGroups.Add(groupParameter.Group);
if (bridgeGroups.Contains(startGroup))
{
for (int i = 0; i < bridgeGroups.Count; i++)
{
if (WalkedGroup.Contains(bridgeGroups[i]) == false)
{
int newGroupIndex = bridgeGroups[i];
List <PathfindNode> localBridges = GetAllBridges(bridgeGroups[i], startGroup, walkType);
int bestBridgeIndex = GetBestBridgeIn(walkType, localBridges, intermediate [intermediate.Count - 1], targetPoint);
PathfindNode bestBridge = null;
if (bestBridgeIndex >= 0)
{
bestBridge = localBridges[bestBridgeIndex];
}
else
{
Debug.Log("can't find bestbrige");
Debug.Break();
}
intermediate.Add(bestBridge);
if (GetIntermediatesPoint(bridgeGroups[i], endGroup, walkType, ref WalkedGroup, ref intermediate, targetPoint))
{
return(true);
}
else
{
intermediate.Remove(bestBridge);
}
}
}
}
}
return(false);
}
}
private static IEnumerator AStartPathFinding(PathfindNode startPoint, PathfindNode endPoint, PathComplete Callback, Dictionary <Point, PathfindNode> walkableArea, WalkType walkType, int loopMaxCount = int.MaxValue)
{
if (startPoint.Point == endPoint.Point)
{
Callback.Invoke(null);
}
int startGroup = startPoint.GetParameter(walkType).Group;
if (PathFindingManager.PrecalculatedPaths != null)
{
// foreach(PointCouple couple in PathFindingManager.PrecalculatedPaths.Keys)
// {
// //Debug.Log(couple);
// if(couple.StartPoint == startPoint.Point && couple.EndPoint == endPoint.Point)
// {
// // Debug.Log("FIND COUPLE");
// }
// }
if (PathFindingManager.PrecalculatedPaths.ContainsKey(new PointCouple(startPoint.Point, endPoint.Point)))
{
//instance.Processing = false;
Callback.Invoke(PathFindingManager.PrecalculatedPaths[new PointCouple(startPoint.Point, endPoint.Point)].Clone());
yield break;
}
}
Dictionary <Point, PathfindNode> reachablePoints = walkableArea; // s_instance.CompleteArea;
if (reachablePoints.ContainsKey(endPoint.Point) == false)
{
//Debug.Log("Node not reachable: "+endPoint.Point);
//instance.Processing = false;
Callback.Invoke(null);
yield break;
}
// if (BelongSameGroup (startPoint,endPoint,walkType) == false) {
//
// int endGroup = endPoint.GetParameter (walkType).Group;
// List<int> walkedGroup = new List<int>();
// List<PathfindNode> intermediates = new List<PathfindNode>();
// intermediates.Insert(0,startPoint);
// bool succes = GetIntermediatesPoint (startGroup, endGroup, walkType,ref walkedGroup,ref intermediates,endPoint);
// if(succes == false)
// {
// //instance.Processing = false;
// Callback.Invoke(null);
// yield break;
// }
// intermediates.Add(endPoint);
// Enemy PathOwner = Callback.Target as Enemy;
// PathOwner.ClearStepedPath();
// for(int i =0;i<intermediates.Count-1;i++)
// {
// yield return instance.StartCoroutine (AStartPathFinding (intermediates [i], intermediates [i+1], PathOwner.BuildPathFromStepCallback, walkableArea, walkType, loopMaxCount));
// }
//
// //instance.Processing = false;
// Callback.Invoke(PathOwner.GetStepedPath());
// yield break;
// }
List <AStarNode> openSet = new List <AStarNode>();
Dictionary <Point, AStarNode> openSetDico = new Dictionary <Point, AStarNode>();
openSet.Add(new AStarNode(startPoint));
openSetDico.Add(startPoint.Point, openSet[openSet.Count - 1]);
Dictionary <Point, AStarNode> closedSet = new Dictionary <Point, AStarNode>();
float fscore; // = (endPoint-startPoint).MagnitudeZero();
fscore = Point.MagnitudeZero(endPoint.Point - startPoint.Point); //+(1 -Vector2.Dot(toNeighbor,toEnd))*20;
int loopLimit = 0;
while (openSet.Count > 0 && loopMaxCount > 0)
{
//Debug.Log(openSet.Count);
loopMaxCount--;
loopLimit++;
openSet.Sort();
AStarNode current = openSet[0];
if (current.Position == endPoint)
{
Path path = new Path(endPoint);
while (current.Parent != null)
{
path.steps.Add(current.Position);
current = current.Parent;
}
path.steps.Reverse();
if (PathFindingManager.PrecalculatedPaths == null)
{
PathFindingManager.PrecalculatedPaths = new Dictionary <PointCouple, Path>();
}
PointCouple couple = new PointCouple(startPoint.Point, endPoint.Point);
if (PathFindingManager.PrecalculatedPaths.ContainsKey(couple))
{
Debug.Log("Try add existing couple: " + couple);
}
else
{
PathFindingManager.PrecalculatedPaths.Add(couple, path);
}
//instance.Processing = false;
Callback.Invoke(path.Clone());
yield break;
}
openSetDico.Remove(openSet[0].Position.Point);
openSet.RemoveAt(0);
closedSet.Add(current.Position.Point, current);
if (loopLimit > 10)
{
yield return(new WaitForEndOfFrame());
loopLimit = 0;
}
for (int i = 0; i < Neighbourhood.Length; i++)
{
PathFindNeighbourg neighbourPath = Neighbourhood [i];
PathfindNode neighbour = new PathfindNode(neighbourPath.offset);
PathfindNode neigbourRealPosition = neighbour + current.Position;
if (reachablePoints.ContainsKey(neigbourRealPosition.Point) == false)
{
continue;
}
AStarNode AstarNeighbor = new AStarNode(neigbourRealPosition);
AstarNeighbor.Position = neigbourRealPosition;
//current.Position;
AstarNeighbor.Parent = current;
if (closedSet.ContainsKey(AstarNeighbor.Position.Point))
{
continue;
}
bool containsStartGroup = instance.CompleteNodeArea.GetNodeAt(neigbourRealPosition.Point).GetParameter(walkType).ContainsGroupOf(endPoint, walkType);
if (containsStartGroup == false)
{
continue;
}
float tentativeGscore = current.gscore + neighbourPath.cost;
AStarNode InSetNeighbor = null;
//GetNodeInList(AstarNeighbor,openSet);
if (openSetDico.ContainsKey(AstarNeighbor.Position.Point))
{
InSetNeighbor = openSetDico [AstarNeighbor.Position.Point];
}
if (InSetNeighbor == null || tentativeGscore < InSetNeighbor.gscore)
{
bool needAddToSet = false;
if (InSetNeighbor == null)
{
InSetNeighbor = new AStarNode(neigbourRealPosition);
needAddToSet = true;
}
InSetNeighbor.Parent = current;
InSetNeighbor.gscore = tentativeGscore;
InSetNeighbor.fscore = InSetNeighbor.gscore + heuristique(AstarNeighbor, endPoint.Point, current.Position.Point);
if (needAddToSet)
{
AstarNeighbor.fscore = InSetNeighbor.fscore;
AstarNeighbor.gscore = InSetNeighbor.gscore;
openSet.Add(AstarNeighbor);
openSetDico.Add(AstarNeighbor.Position.Point, openSet [openSet.Count - 1]);
}
}
}
}
//instance.Processing = false;
Callback.Invoke(null);
//return null;
}
public Path(PathfindNode p) : this()
{
Target = p;
}