//Search in the central floor object to find out what is behind a certain node
//TODO add stoachastic variable for observing --> approximated edge length and
public void ObserveRoom(int nodeID, CentralFloor floor)
{
List <int> incident = floor.Graph.Node(nodeID).GetIncident;
foreach (int i in incident)
{
FloorGraphEdge e = floor.Graph.Edge(i);
//Add edge
if (!(fGraph.Edges.ContainsKey(i)))
{
fGraph.AddEdge(e.Id, e.HardCopy());
fGraph.Node(nodeID).GetIncident.Add(i);
}
//Add node
int dest = fGraph.Edge(i).GetDestination;
if (!fGraph.Nodes.ContainsKey(dest))
{
fGraph.AddNode(dest, new List <int> {
i
});
}
//Add references
if (!fGraph.Node(dest).GetIncident.Contains(i))
{
fGraph.Node(dest).GetIncident.Add(i);
}
}
}
public void AddEdge(int id, FloorGraphEdge edge)
{
edges.Add(id, edge);
}
public List <FloorGraphNode> FindPathGraph(FloorGraphNode start, FloorGraphNode end)
{
FloorGraphNode parentNode = start;
bool found = false;
mStop = false;
mStopped = false;
mOpen.Clear();
mClose.Clear();
parentNode.G = 0;
parentNode.H = mHEstimate;
parentNode.F = parentNode.G + parentNode.H;
parentNode.parent = parentNode.id;
mGraphOpen.Push(parentNode);
while (mGraphOpen.Count > 0 && !mStop)
{
parentNode = mGraphOpen.Pop();
if (parentNode == end)
{
mGraphClose.Add(parentNode);
found = true;
break;
}
if (mClose.Count > mSearchLimit)
{
mStopped = true;
return(null);
}
for (int i = 0; i < parentNode.GetIncident.Count; i++)
{
FloorGraphNode newNode = mGraph.GetFloorGraphNode(parentNode.GetIncident[i]);
//find corresponding edge
FloorGraphEdge e = null;
List <int> edges = parentNode.GetEdges;
bool foundMathcingEdge = false;
foreach (int E in edges)
{
if (!foundMathcingEdge)
{
e = mGraph.GetFloorGraphEdge(E);
if ((e.GetDestination == parentNode.id && e.GetOrigin == newNode.id) || (e.GetDestination == newNode.id && e.GetOrigin == parentNode.id))
{
foundMathcingEdge = true;
}
}
else
{
break;
}
}
float newG = e.expectedLength;
if (newG == parentNode.G)
{
//Unbrekeable
continue;
}
int foundInOpenIndex = -1;
for (int j = 0; j < mGraphOpen.Count; j++)
{
if (mGraphOpen[j] == newNode)
{
foundInOpenIndex = j;
break;
}
}
if (foundInOpenIndex != -1 && mGraphOpen[foundInOpenIndex].G <= newG)
{
continue;
}
int foundInCloseIndex = -1;
for (int j = 0; j < mGraphClose.Count; j++)
{
if (mGraphClose[j] == newNode)
{
foundInCloseIndex = j;
break;
}
}
if (foundInCloseIndex != -1 && mGraphClose[foundInCloseIndex].G <= newG)
{
continue;
}
newNode.parent = parentNode.id;
newNode.G = newG;
newNode.H = mHEstimate * (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y));
newNode.F = newNode.G + newNode.H;
//It is faster if we leave the open node in the priority queue
//When it is removed, all nodes around will be closed, it will be ignored automatically
//if (foundInOpenIndex != -1)
// mOpen.RemoveAt(foundInOpenIndex);
//if (foundInOpenIndex == -1)
mGraphOpen.Push(newNode);
}
mGraphClose.Add(parentNode);
}
mCompletedTime = HighResolutionTime.GetTime();
if (found)
{
FloorGraphNode fNode = mGraphClose[mGraphClose.Count - 1];
for (int i = mGraphClose.Count - 1; i >= 0; i--)
{
if (fNode == mGraphClose[i] || i == mClose.Count - 1)
{
fNode = mGraphClose[i];
}
else
{
mClose.RemoveAt(i);
}
}
mStopped = true;
return(mGraphClose);
}
mStopped = true;
return(null);
}
