public List <PositionAndOrientation> GetTrackFromPreparedPlanner(PositionAndOrientation target)
{
if (preparedMap_ == null)
{
throw new ApplicationException("Prepare tracks first!");
}
if (seen_.Count == 0)
{
throw new ApplicationException();
}
double smallestError = Double.MaxValue;
BFSNode nodeClosestToTarget = null;
foreach (BFSNode x in seen_)
{
double currError = Math.Pow(x.position.x - target.x, 2.0d) +
Math.Pow(x.position.y - target.y, 2.0d) +
Math.Pow(x.position.angle - target.angle, 2.0d);
if (currError < smallestError)
{
smallestError = currError;
nodeClosestToTarget = x;
}
}
return(GetPathBetweenPoints(startPoint_, nodeClosestToTarget));
}
public List <List <TimedMove> > bfsToStartPositions(Move goalState)
{
ReducerOpenList <BFSNode> openList = new ReducerOpenList <BFSNode>();
HashSet <BFSNode> closedList = new HashSet <BFSNode>();
List <List <TimedMove> > paths = new List <List <TimedMove> >();
openList.Enqueue(new BFSNode(goalState));
int startPositionsFound = 0;
while (openList.Count != 0)
{
BFSNode node = openList.Dequeue();
if (closedList.Contains(node))
{
continue;
}
if (findStartPosition(node.position) != null)
{
addPath(instance, paths, node);
startPositionsFound++;
if (startPositionsFound == this.instance.m_vAgents.Length)
{
break;
}
}
GetSons(node, openList, instance);
closedList.Add(node);
}
return(paths);
}
public void PrepareTracks(Map map)
{
seen_.Clear();
preparedMap_ = map; // hack a bit - we r holding state here
obstacles_ = map.obstacles;
Queue <BFSNode> frontier = new Queue <BFSNode>();
startPoint_ = new BFSNode(map.car, null);
frontier.Enqueue(startPoint_);
PositionAndOrientation target = map.parking;
while (frontier.Count != 0)
{
BFSNode curr = frontier.Dequeue();
List <BFSNode> succesors = generateSuccessors(curr);
succesors.ForEach(x => frontier.Enqueue(x));
succesors.ForEach(x => seen_.Add(x));
}
}
void SolveWithOrderPuzzle(BFSNode orderSolve)
{
clock.Interval = 500;
var direction = new[] { new { rd = -1, cd = 0 }, new { rd = 0, cd = 1 }
, new { rd = 1, cd = 0 }, new { rd = 0, cd = -1 } };
foreach (var item in orderSolve.order)
{
int r = rowZero + direction[(int)item].rd, c = colZero + direction[(int)item].cd, rz = rowZero, cz = colZero;
Puzzle[rowZero, colZero] = Puzzle[r, c];
Puzzle[r, c] = 0;
rowZero = r;
colZero = c;
Monitor.Enter("lock");
Monitor.Pulse("lock");
Monitor.Wait("lock");
Dispatcher.Invoke(() =>
{
GridPuzzle.Children[9 + rz * 3 + cz].Visibility = Visibility.Visible;
(GridPuzzle.Children[9 + rz * 3 + cz] as Button).Content = Puzzle[rz, cz].ToString();
GridPuzzle.Children[9 + rowZero * 3 + colZero].Visibility = Visibility.Hidden;
});
Monitor.Exit("lock");
}
clock.Interval = 100;
}
protected override void VisitNode(BFSQueue queue, BFSNode node, int sourceUser, int neighbourhoodDistance)
{
Dictionary <int, float> indexes = new Dictionary <int, float>();
foreach (int user in this.originalTrustMatrix[node.value])
{
if (!this.estimatedTrustMatrix[sourceUser].Contains(user))
{
indexes[user] = ResourceAllocationIndex(sourceUser, user);
}
}
List <KeyValuePair <int, float> > indexesList = indexes.ToList();
if (topN.HasValue)
{
indexesList.Sort((pair1, pair2) => pair1.Value.CompareTo(pair2.Value));
if (topN < indexesList.Count)
{
indexesList.RemoveRange(0, indexesList.Count - topN.Value);
}
}
int distance = node.distance + 1;
foreach (KeyValuePair <int, float> pair in indexesList)
{
int user = pair.Key;
estimatedTrustMatrix[sourceUser, user] = BasicTrustMetric(neighbourhoodDistance, distance);
queue.Enqueue(new BFSNode(pair.Key, distance));
}
}
IEnumerable <BFSNode> expandNodeGraph(BFSNode node, bool isRoot)
{
var direction = new[] { new { rd = -1, cd = 0, dir = Dir.up }, new { rd = 0, cd = 1, dir = Dir.right }
, new { rd = 1, cd = 0, dir = Dir.down }, new { rd = 0, cd = -1, dir = Dir.left } };
BFSNode tempNode;
foreach (var d in direction)
{
if (isRoot || d.dir != reverseDir(node.order.Last()))
{
if (node.rowZ + d.rd > -1 && node.rowZ + d.rd < 3 &&
node.colZ + d.cd > -1 && node.colZ + d.cd < 3)
{
tempNode = node;
tempNode.order = new Queue <Dir>(node.order.ToArray());
tempNode.rowZ += (short)d.rd;
tempNode.colZ += (short)d.cd;
tempNode.order.Enqueue(d.dir);
if (checkVisit(tempNode))
{
continue;
}
yield return(tempNode);
}
}
}
}
public BFSNode AddChild(string name)
{
BFSNode child = new BFSNode(name);
children.Add(child);
return(this);
}
private List <BFSNode> generateSuccessors(BFSNode predecessor)
{
List <BFSNode> successors = new List <BFSNode>();
double oldAngleInRadians = predecessor.position.angle / 180.0f * Math.PI;
double angleStepInRadians = angleStep_ / 180.0f * Math.PI;
foreach (int i in new List <int>()
{
0, -1, 1
})
{
double newAngleInRadians = oldAngleInRadians + angleStepInRadians * i;
double newX = predecessor.position.x + Math.Cos(newAngleInRadians) * positionStep_;
double newY = predecessor.position.y + Math.Sin(newAngleInRadians) * positionStep_;
if (newX < 0 || newX > mapSizeX_ || newY < 0 || newY > mapSizeY_)
{
continue;
}
double newAngle = predecessor.position.angle + angleStep_ * i;
PositionAndOrientation newPosition = new PositionAndOrientation(newX, newY, newAngle);
if (!IsPositionSeen(newPosition) && !(IsPositionObstacled(newPosition)))
{
BFSNode newNode = new BFSNode(newPosition, predecessor);
successors.Add(new BFSNode(newPosition, predecessor));
}
}
return(successors);
}
bool checkVisit(BFSNode orderNode)
{
orderNode.order = new Queue <Dir>(orderNode.order.ToArray());
int[,] p = Puzzle.Clone() as int[, ];
int rowz = rowZero, colz = colZero;
var direction = new[] { new { rd = -1, cd = 0 }, new { rd = 0, cd = 1 }
, new { rd = 1, cd = 0 }, new { rd = 0, cd = -1 } };
for (int i = orderNode.order.Count; i > 0; i--)
{
var dir = orderNode.order.Dequeue();
int r = rowz + direction[(int)dir].rd, c = colz + direction[(int)dir].cd;
p[rowz, colz] = p[r, c];
p[r, c] = 0;
rowz = r;
colz = c;
}
int numOfNode = 0, e = 1;
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
numOfNode += p[i, j] * e;
e *= 10;
}
}
if (visitedNode.Contains(numOfNode))
{
return(true);
}
visitedNode.Add(numOfNode);
return(false);
}
public override void FindPath(Vector2 startPosition, Vector2 targetPosition)
{
// Start stopwatch.
stopwatch.Restart();
stopwatch.Start();
// Find start and end node positions.
BFSNode startNode = (BFSNode)grid.NodeFromWorldPoint(startPosition);
BFSNode targetNode = (BFSNode)grid.NodeFromWorldPoint(targetPosition);
// Initalise Lists & HastSets.
List <BFSNode> visitedSet = new List <BFSNode>();
List <BFSNode> order = new List <BFSNode>();
Queue <BFSNode> openSet = new Queue <BFSNode>();
startNode.History = new List <BFSNode>();
visitedSet.Add(startNode);
openSet.Enqueue(startNode); // Add starting node to open set, we start searching from here.
while (openSet.Count > 0) // While there are nodes in the open set, loop.
{
BFSNode currentNode = openSet.Dequeue();
if (!order.Contains(currentNode))
{
order.Add(currentNode);
}
foreach (BFSNode neighbourNode in grid.GetNeighbours(currentNode)) // Loop over each neighbour of the current node.
{
if (!neighbourNode.Walkable || visitedSet.Contains(neighbourNode)) // If the neighbour is not walkable, or has already been sorted into the closed set...
{
continue;
}
neighbourNode.History = new List <BFSNode>(currentNode.History);
neighbourNode.History.Add(currentNode);
visitedSet.Add(neighbourNode);
openSet.Enqueue(neighbourNode);
if (!order.Contains(neighbourNode))
{
order.Add(neighbourNode);
}
if (neighbourNode == targetNode) // If we have reached the target node...
{
neighbourNode.History.Add(neighbourNode);
RetracePath(targetNode, order.ConvertAll(x => (Node)x));
openSet.Clear();
stopwatch.Stop();
break;
}
}
}
waypoints = RetracePath(targetNode, order.ConvertAll(x => (Node)x));
unit.PathToPosition(startPosition, targetPosition, waypoints);
}
private void CollectSon(ReducerOpenList <BFSNode> openList, BFSNode node, int x, int y)
{
BFSNode son = new BFSNode(new TimedMove(x, y, Move.Direction.NO_DIRECTION, 0), node);
if (!openList.Contains(son))
{
openList.Enqueue(son);
}
}
public static Matrix <float> BuildRABasedTrust(Matrix <float> trustMatrix, int neighbourhoodDistance)
{
Matrix <float> estimatedTrustMatrix = trustMatrix.Clone();
Matrix <float> raMatrix = new Matrix <float>();
foreach (int sourceUser in trustMatrix.Rows)
{
BFSQueue queue = new BFSQueue(neighbourhoodDistance);
foreach (int trustedUser in trustMatrix[sourceUser])
{
queue.Enqueue(new BFSNode(trustedUser, 1));
}
while (queue.Count() > 0)
{
BFSNode node = queue.Deque();
int user = node.value;
int distance = node.distance;
if (distance == neighbourhoodDistance)
{
break;
}
if (trustMatrix.HasRow(user))
{
Dictionary <int, float> RAIndexes = new Dictionary <int, float>();
foreach (int trustedUser in trustMatrix[user])
{
if (!trustMatrix[sourceUser].Contains(trustedUser))
{
if (!(raMatrix.HasRow(sourceUser) && raMatrix[sourceUser].Contains(trustedUser)))
{
raMatrix[sourceUser, trustedUser] = ResourceAllocationIndex(trustMatrix, sourceUser, trustedUser);
}
RAIndexes[trustedUser] = raMatrix[sourceUser, trustedUser];
}
}
List <KeyValuePair <int, float> > RAList = RAIndexes.ToList();
RAList.Sort((pair1, pair2) => pair1.Value.CompareTo(pair2.Value));
for (int i = 0; i < 15 && i < RAList.Count; i++)
{
queue.Enqueue(new BFSNode(RAList[i].Key, distance + 1));
estimatedTrustMatrix[sourceUser, RAList[i].Key] = BasicTrustMetric(neighbourhoodDistance, distance + 1);
}
}
}
}
return(estimatedTrustMatrix);
}
public static Matrix <float> BuildCommonNeighboursBasedTrust(Matrix <float> trustMatrix, int neighbourhoodDistance)
{
Matrix <float> estimatedTrustMatrix = trustMatrix.Clone();
Matrix <float> cnMatrix = new Matrix <float>();
foreach (int sourceUser in trustMatrix.Rows)
{
BFSQueue queue = new BFSQueue(neighbourhoodDistance);
foreach (int trustedUser in trustMatrix[sourceUser])
{
queue.Enqueue(new BFSNode(trustedUser, 1));
}
while (queue.Count() != 0)
{
BFSNode node = queue.Deque();
int user = node.value;
int distance = node.distance;
if (distance == neighbourhoodDistance)
{
break;
}
if (trustMatrix.HasRow(user))
{
Dictionary <int, float> commonNeighbours = new Dictionary <int, float>();
foreach (int trustedUser in trustMatrix[user])
{
if (!trustMatrix[sourceUser].Contains(trustedUser))
{
if (!(cnMatrix.HasRow(sourceUser) && cnMatrix[sourceUser].Contains(trustedUser)))
{
cnMatrix[sourceUser, trustedUser] = CommonNeighbours(trustMatrix, sourceUser, trustedUser);
}
commonNeighbours[trustedUser] = cnMatrix[sourceUser, trustedUser];
}
}
List <KeyValuePair <int, float> > CNList = commonNeighbours.ToList();
CNList.Sort((pair1, pair2) => pair1.Value.CompareTo(pair2.Value));
for (int i = 0; i < 15 && i < CNList.Count; i++)
{
queue.Enqueue(new BFSNode(CNList[i].Key, distance + 1));
estimatedTrustMatrix[sourceUser, CNList[i].Key] = BasicTrustMetric(neighbourhoodDistance, distance + 1);
}
}
}
}
return(estimatedTrustMatrix);
}
private bool StoreInDatabase(BFSNode node, int partition)
{
UInt32 key = node.GetHashKey(partition);
if (!database[partition].ContainsKey(key))
{
database[partition].Add(key, node.distance);
return(true);
}
return(false);
}
/// <summary>
/// Traces the solved path and builds the internal BFS tree.
/// </summary>
private IEnumerable <IWalledMazeNode> TraceSolvedPath(IWalledMaze maze, BFSNode endNode)
{
BFSNode currentNode = endNode;
ICollection <IWalledMazeNode> pathTrace = new List <IWalledMazeNode>();
while (currentNode != null)
{
pathTrace.Add(GetMazeNode(maze, currentNode));
currentNode = currentNode.Predecessor;
}
return(pathTrace);
}
private void ExamineNeighbors(List <BFSNode> neighbors, BFSNode current)
{
for (int i = 0; i < neighbors.Count; i++)
{
BFSNode neighbor = neighbors[i];
if (!nodes.Contains(neighbor))
{
nodes.Add()
}
}
}
private void addPath(ProblemInstance problemInstance, List <List <TimedMove> > paths, BFSNode node)
{
List <TimedMove> newPath = new List <TimedMove>();
BFSNode currNode = node;
while (currNode != null)
{
newPath.Add(currNode.position);
currNode = currNode.lastMove;
}
paths.Add(newPath);
}
protected override void VisitNode(BFSQueue queue, BFSNode node, int sourceUser, int neighbourhoodDistance)
{
foreach (int user in this.originalTrustMatrix[node.value])
{
if (!this.estimatedTrustMatrix[sourceUser].Contains(user))
{
int distance = node.distance + 1;
this.estimatedTrustMatrix[sourceUser, user] = BasicTrustMetric(neighbourhoodDistance, distance);
queue.Enqueue(new BFSNode(user, distance));
}
}
}
private void BreadthFirstSearchGenerator(int partition) // expand all possible states and store the move count estimates (if larger than Manhattan distance)
{
// init - save the final state (solved board)
BFSNode node = new BFSNode(this), next = null;
for (int p = 0; p < parts; p++)
{
StoreInDatabase(node, p);
}
// run bfs
Queue <BFSNode> queue = new Queue <BFSNode>();
queue.Enqueue(node);
while (queue.Count > 0)
{
node = queue.Dequeue();
if (node.CanGoLeft())
{
next = node.MoveLeft(partition);
if (StoreInDatabase(next, partition))
{
queue.Enqueue(next);
}
}
if (node.CanGoRight())
{
next = node.MoveRight(partition);
if (StoreInDatabase(next, partition))
{
queue.Enqueue(next);
}
}
if (node.CanGoUp())
{
next = node.MoveUp(partition);
if (StoreInDatabase(next, partition))
{
queue.Enqueue(next);
}
}
if (node.CanGoDown())
{
next = node.MoveDown(partition);
if (StoreInDatabase(next, partition))
{
queue.Enqueue(next);
}
}
}
}
public bool NextIteration()
{
// Solution was already found.
if (status == SearchStatus.Complete || status == SearchStatus.NotFound)
{
return(true);
}
List <uint> nextNodes = graph.NextNodes(workingBranch[workingIndex].Node);
for (int i = 0; i < nextNodes.Count; i++)
{
// We check if it is goal.
BFSNode nextNode = new BFSNode(workingBranch[workingIndex], nextNodes[i]);
if (isGoal(nextNode.Node))
{
results.Add(nextNode);
status = SearchStatus.RunningWithSolution;
}
nextBranch.Add(nextNode);
}
// We check if we need to go to next level.
if (++workingIndex >= workingBranch.Count)
{
workingIndex = 0;
// Check if results were found.
if (results.Count > 0)
{
status = SearchStatus.Complete;
return(true);
}
if (++level >= maxBranch || nextBranch.Count == 0)
{
status = SearchStatus.NotFound;
return(false);
}
// If results were not found, we go to new level.
workingBranch = nextBranch;
nextBranch = new List <BFSNode>(workingBranch.Count);
}
return(false);
}
private static void QueueNewNeighbors(Dictionary <Vector2, BFSNode> nodes, Queue <BFSNode> queue, BFSNode current, Vector2 neighborCoordinate)
{
BFSNode neighbor = nodes[neighborCoordinate];
if (neighbor.explored || queue.Contains(neighbor))
{
//do nothing
}
else
{
neighbor.parent = current;
queue.Enqueue(neighbor);
}
}
public static Matrix <float> BuildJaccardIndexTrustMatrix2(Matrix <float> trustMatrix, int neighbourhoodDistance)
{
Matrix <float> estimatedTrustMatrix = trustMatrix.Clone();
Matrix <float> jaccardMatrix = new Matrix <float>();
foreach (int sourceUser in trustMatrix.Rows)
{
BFSQueue queue = new BFSQueue(neighbourhoodDistance);
foreach (int trustedUser in trustMatrix[sourceUser])
{
queue.Enqueue(new BFSNode(trustedUser, 1));
}
while (queue.Count() != 0)
{
BFSNode node = queue.Deque();
int user = node.value;
int distance = node.distance;
if (distance == neighbourhoodDistance)
{
break;
}
if (trustMatrix.HasRow(user))
{
foreach (int trustedUser in trustMatrix[user])
{
if (!(jaccardMatrix.HasRow(sourceUser) && jaccardMatrix[sourceUser].Contains(trustedUser)))
{
jaccardMatrix[sourceUser, trustedUser] = jaccardMatrix[trustedUser, sourceUser] = JaccardIndex(trustMatrix, sourceUser, trustedUser);
}
float jaccardIndex = jaccardMatrix[sourceUser, trustedUser];
if (jaccardIndex > _Threshold)
{
estimatedTrustMatrix[sourceUser, trustedUser] = BasicTrustMetric(neighbourhoodDistance, distance + 1);
queue.Enqueue(new BFSNode(trustedUser, distance + 1));
}
}
}
}
}
return(estimatedTrustMatrix);
}
private List <PositionAndOrientation> GetPathBetweenPoints(BFSNode start, BFSNode target)
{
List <PositionAndOrientation> path = new List <PositionAndOrientation>();
BFSNode curr = target;
while (curr != null)
{
path.Add(curr.position);
curr = curr.predecessor;
}
path.Reverse();
return(path);
}
/// <summary>
/// Retrace the found path from the start to end node, including the order that the path was found in.
/// </summary>
protected override Vector2[] RetracePath(Node endNode, List <Node> order)
{
List <Node> path = new List <Node>();
BFSNode currentNode = (BFSNode)endNode;
path = currentNode.History.ConvertAll(x => (Node)x);
path[0].IsStartNode = true;
endNode.IsEndNode = true;
grid.Path = path;
grid.Order = order;
grid.ShowFinalPath();
nodesVisitedText.text = "Nodes Visited: " + order.Count + " in " + stopwatch.ElapsedMilliseconds + "ms";
Vector2[] waypoints = ConvertToWaypoints(path);
return(waypoints);
}
private static List <Vector2> GetPath(Dictionary <Vector2, BFSNode> nodes, Vector2 start, Vector2 end)
{
List <Vector2> path = new List <Vector2> ();
path.Add(end);
BFSNode previous = nodes[end].parent;
//Debug.Log(previous.parent);
while (previous != nodes[start] && previous != null)
{
path.Add(previous.position);
previous = previous.parent;
}
path.Add(nodes[start].position);
path.Reverse();
return(path);
}
public static Stack <VacuumAgent.VacuumAction> ExecuteFor(Problem problem)
{
Stack <VacuumAgent.VacuumAction> vacuumActions = new Stack <VacuumAgent.VacuumAction>();
// Lancement de l'algorithme pour trouver un noeud solution du problème
BFSNode lastNode = RunAlgo(problem);
if (lastNode == null)
{
return(new Stack <VacuumAgent.VacuumAction>());
}
// Récupération de toutes les actions conduisant à ce noeud
while (lastNode._parentNode != null)
{
vacuumActions.Push(lastNode._action);
lastNode = lastNode._parentNode;
}
return(vacuumActions);
}
// O(v + e) time | O(v) space
public List <string> BreadthFirstSearch(List <string> array)
{
List <BFSNode> queue = new List <BFSNode>();
queue.Add(this);
var index = 0;
while (index < queue.Count)
{
BFSNode current = queue[index];
index += 1;
array.Add(current.name);
for (int i = 0; i < current.children.Count; i++)
{
queue.Add(current.children[i]);
}
}
return(array);
}
public static Matrix <float> BuildSaltonIndexTrustMatrix2(Matrix <float> trustMatrix, int neighbourhoodDistance)
{
Matrix <float> estimatedTrustMatrix = new Matrix <float>();
Matrix <float> saltonMatrix = new Matrix <float>();
foreach (int sourceUser in trustMatrix.Rows)
{
BFSQueue queue = new BFSQueue(neighbourhoodDistance);
estimatedTrustMatrix.AddRow(sourceUser);
foreach (int trustedUser in trustMatrix[sourceUser])
{
queue.Enqueue(new BFSNode(trustedUser, 1));
}
while (queue.Count() != 0)
{
BFSNode node = queue.Deque();
int user = node.value;
int distance = node.distance;
if (!estimatedTrustMatrix[sourceUser].Contains(user))
{
saltonMatrix[sourceUser, user] = saltonMatrix[user, sourceUser] = SaltonIndex(trustMatrix, sourceUser, user);
if (saltonMatrix[sourceUser, user] > _Threshold)
{
estimatedTrustMatrix[sourceUser, user] = BasicTrustMetric(neighbourhoodDistance, distance);
if (trustMatrix.Rows.Contains(user))
{
foreach (int trustedUser in trustMatrix[user])
{
queue.Enqueue(new BFSNode(trustedUser, distance + 1));
}
}
}
}
}
}
return(estimatedTrustMatrix);
}
// return move with shortest open path from a start point to any goal (using BFS - considered A* but doesn't fit easily)
private float SearchDist(Vector3 start, HashSet <Vector3> goals)
{
// if there are no goals, return arbitrary value
if (goals.Count() == 0)
{
return(0);
}
if (goals.Contains(start))
{
return(0);
}
Vector3[] moves = new[] { Vector3.left, Vector3.right, Vector3.up, Vector3.down, Vector3.forward, Vector3.back };
// use queue-based bfs to find distance to nearest goal
HashSet <Vector3> visited = new HashSet <Vector3>();
Queue <BFSNode> queue = new Queue <BFSNode>();
queue.Enqueue(new BFSNode(start, 0));
while (queue.Count > 0)
{
BFSNode node = queue.Dequeue();
Vector3 pos = node.Position;
float dist = node.Cost;
visited.Add(pos);
foreach (Vector3 move in moves)
{
//Debug.Log(counter);
Vector3 newPos = pos + move;
if (goals.Contains(newPos))
{
return(dist + 1);
}
if (IsOpen(newPos) && IsSafe(newPos) && !visited.Contains(newPos))
{
visited.Add(newPos);
queue.Enqueue(new BFSNode(newPos, dist + 1));
}
}
}
//Debug.Log("no goals reachable");
return(99999); // no goals reachable
}
private void GetSons(BFSNode node, ReducerOpenList <BFSNode> openList, ProblemInstance problem)
{
bool[][] problemGrid = problem.m_vGrid;
if (node.position.x != problemGrid.Length - 1 && !problemGrid[node.position.x + 1][node.position.y])
{
CollectSon(openList, node, node.position.x + 1, node.position.y);
}
if (node.position.x != 0 && !problemGrid[node.position.x - 1][node.position.y])
{
CollectSon(openList, node, node.position.x - 1, node.position.y);
}
if (node.position.y != problemGrid[node.position.x].Length - 1 && !problemGrid[node.position.x][node.position.y + 1])
{
CollectSon(openList, node, node.position.x, node.position.y + 1);
}
if (node.position.y != 0 && !problemGrid[node.position.x][node.position.y - 1])
{
CollectSon(openList, node, node.position.x, node.position.y - 1);
}
}
public void PrepareTracks(Map map)
{
seen_.Clear();
preparedMap_ = map; // hack a bit - we r holding state here
obstacles_ = map.obstacles;
Queue<BFSNode> frontier = new Queue<BFSNode>();
startPoint_ = new BFSNode(map.car, null);
frontier.Enqueue(startPoint_);
PositionAndOrientation target = map.parking;
while (frontier.Count != 0)
{
BFSNode curr = frontier.Dequeue();
List<BFSNode> succesors = generateSuccessors(curr);
succesors.ForEach(x => frontier.Enqueue(x));
succesors.ForEach(x => seen_.Add(x));
}
}
//TODO: add version with recalc from 80% with better accuaracy
/// <summary>
/// it finishes when gets to target
/// </summary>
/// <param name="map"></param>
/// <returns></returns>
public List<PositionAndOrientation> PlanTrack(Map map)
{
if (seen_.GetAllObjects().Count != 0)
throw new ApplicationException();
obstacles_ = map.obstacles;
Queue<BFSNode> frontier = new Queue<BFSNode>();
BFSNode startPoint = new BFSNode(map.car, null);
frontier.Enqueue(startPoint);
PositionAndOrientation target = map.parking;
while (frontier.Count != 0)
{
BFSNode curr = frontier.Dequeue();
List<BFSNode> succesors = generateSuccessors(curr);
foreach (BFSNode s in succesors)
{
if (ArePointsSame(s.position, target))
return GetPathBetweenPoints(startPoint, s);
}
succesors.ForEach(x => frontier.Enqueue(x));
succesors.ForEach(x => seen_.Add(x));
}
return new List<PositionAndOrientation>(); // couldn't find result - returning empty list
}
public BFSNode(PositionAndOrientation _position, BFSNode _predecessor)
{
position = _position;
predecessor = _predecessor;
Rect = new Rectangle((int)position.x, (int)position.y, 1, 1);
}
private List<PositionAndOrientation> GetPathBetweenPoints(BFSNode start, BFSNode target)
{
List<PositionAndOrientation> path = new List<PositionAndOrientation>();
BFSNode curr = target;
while (curr != null)
{
path.Add(curr.position);
curr = curr.predecessor;
}
path.Reverse();
return path;
}
private List<BFSNode> generateSuccessors(BFSNode predecessor)
{
List<BFSNode> successors = new List<BFSNode>();
double oldAngleInRadians = predecessor.position.angle / 180.0f * Math.PI;
double angleStepInRadians = angleStep_ / 180.0f * Math.PI;
foreach (int i in new List<int>(){0, -1, 1})
{
double newAngleInRadians = oldAngleInRadians + angleStepInRadians * i;
double newX = predecessor.position.x + Math.Cos(newAngleInRadians) * positionStep_;
double newY = predecessor.position.y + Math.Sin(newAngleInRadians) * positionStep_;
if (newX < 0 || newX > mapSizeX_ || newY < 0 || newY > mapSizeY_)
continue;
double newAngle = predecessor.position.angle + angleStep_ * i;
PositionAndOrientation newPosition = new PositionAndOrientation(newX, newY, newAngle);
if (!IsPositionSeen(newPosition) && !(IsPositionObstacled(newPosition)))
{
BFSNode newNode = new BFSNode(newPosition, predecessor);
successors.Add(new BFSNode(newPosition, predecessor));
}
}
return successors;
}
