/**
* Computes a node whose sequence of recursive parents corresponds to a
* sequence of actions which leads from the initial state of the original
* problem to the state of node1 and then to the initial state of the
* reverse problem, following reverse actions to parents of node2. Note that
* both nodes must be linked to the same state. Success is not guaranteed if
* some actions cannot be reversed.
*/
private Node <S, A> getSolution(IProblem <S, A> orgP, ExtendedNode node1, ExtendedNode node2)
{
if (!node1.getState().Equals(node2.getState()))
{
throw new NotSupportedException("states not equal");
}
Node <S, A> orgNode = node1.getProblemIndex() == ORG_P_IDX ? node1 : node2;
Node <S, A> revNode = node1.getProblemIndex() == REV_P_IDX ? node1 : node2;
while (revNode.getParent() != null)
{
A action = getReverseAction(orgP, revNode);
if (action != null)
{
S nextState = revNode.getParent().getState();
double stepCosts = orgP.getStepCosts(revNode.getState(), action, nextState);
orgNode = nodeExpander.createNode(nextState, orgNode, action, stepCosts);
revNode = revNode.getParent();
}
else
{
return(null);
}
}
metrics.set(METRIC_PATH_COST, orgNode.getPathCost());
return(orgNode);
}
private ExtendedNode getCorrespondingNodeFromOtherProblem(ExtendedNode node)
{
ExtendedNode result = explored.Get(1 - node.getProblemIndex()).Get(node.getState());
// Caution: The goal test of the original problem should always include
// the root node of the reverse problem as that node might not yet have
// been explored yet. This is important if the reverse problem does not
// provide reverse actions for all original problem actions.
if (result == null && node.getProblemIndex() == ORG_P_IDX && node.getState().Equals(goalStateNode.getState()))
{
result = goalStateNode;
}
return(result);
}
public override void NodeRemoved(ExtendedNode node)
{
var startNode = node as StartNode;
var endNode = node as FinalNode;
if (startNode != null)
{
StartNode = null;
}
else if (endNode != null)
{
FinalNode = null;
}
CurrentError = GetError();
}
public void SplitNode(LinkedList <Node> allNodes)
{
// first, count how many granules are involved
int nGranules = granules.Count;
// next, add (nGranules-1) new nodes
// and update connected elements
for (int i = 0; i < nGranules - 1; i++)
{
int currentGranule = granules[i];
ExtendedNode newNode = new ExtendedNode(this);
newNode.ll_node = allNodes.AddAfter(ll_node, newNode);
foreach (ExtendedElement elem in elementsOfNode)
{
if (elem.granule == currentGranule)
{
elem.SubstituteNode(this, newNode);
}
}
}
}
// populate elementsOfFace arrays for each Face; works on extended Mesh
public static void IdentifyParentsOfTriangles(this Mesh mg)
{
foreach (ExtendedNode nd in mg.nodes)
{
nd.elementsOfNode.Clear();
}
foreach (ExtendedFace f in mg.faces)
{
f.elementsOfFace.Clear();
}
foreach (Element elem in mg.elems)
{
foreach (ExtendedNode nd in elem.vrts)
{
Trace.Assert(!nd.elementsOfNode.Contains(elem), "nd.elementsOfNode.Contains");
nd.elementsOfNode.Add(elem);
}
}
foreach (ExtendedFace f in mg.faces)
{
ExtendedNode nd = (ExtendedNode)f.vrts[0];
foreach (ExtendedElement e in nd.elementsOfNode)
{
if (e.ContainsFace(f))
{
f.elementsOfFace.Add(e);
if (f.elementsOfFace.Count == 2)
{
break;
}
}
}
}
}
public abstract void NodeInitialized(ExtendedNode node);
public abstract void NodeRemoved(ExtendedNode node);
public static void InsertCohesiveElements(this Mesh mg)
{
Trace.Assert(mg.czs.Count == 0, "CZs can be inserted only once");
mg.Extend();
mg.IdentifyParentsOfTriangles();
// connectivity information for nodes
foreach (ExtendedNode nd in mg.nodes)
{
nd.elementsOfNode.Clear();
}
foreach (Element e in mg.elems)
{
foreach (ExtendedNode n in e.vrts)
{
n.elementsOfNode.Add(e);
if (!n.granules.Contains(e.granule))
{
n.granules.Add(e.granule);
}
}
}
// preserve the exposed faces
List <ExtendedFace> surface = new List <ExtendedFace>();
List <ExtendedFace> innerTris = new List <ExtendedFace>();
foreach (ExtendedFace f in mg.faces)
{
if (f.elementsOfFace.Count == 1)
{
surface.Add(f);
}
else if (f.elementsOfFace.Count == 2)
{
innerTris.Add(f);
}
}
foreach (Face f in surface)
{
foreach (Node nd in f.vrts)
{
nd.isSurface = true;
}
}
List <Tuple <ExtendedElement, int> > surfaceFaces = new List <Tuple <ExtendedElement, int> >(); // (element, faceIdx) format
foreach (ExtendedFace f in mg.faces)
{
foreach (ExtendedElement e in f.elementsOfFace)
{
Trace.Assert(e.ContainsFace(f), "error in .elementsOfFace");
int which = e.WhichFace(f);
// exposed faces are preserved in surfaceFaces
if (f.elementsOfFace.Count == 1)
{
surfaceFaces.Add(new Tuple <ExtendedElement, int>(e, which));
}
e.faces.Add(which);
}
}
// store all edges within extended elements
foreach (GranuleEdge ge in mg.edges)
{
ExtendedNode nd = (ExtendedNode)ge.vrts[0];
foreach (ExtendedElement elem in nd.elementsOfNode)
{
elem.AddEdgeIfContains(ge);
}
}
// convert inner triangles into cohesive elements
foreach (ExtendedFace f in innerTris)
{
ExtendedElement e0 = (ExtendedElement)f.elementsOfFace[0];
ExtendedElement e1 = (ExtendedElement)f.elementsOfFace[1];
ExtendedCZ ecz = new ExtendedCZ(f, e0, e1);
mg.czs.Add(ecz);
}
// list the nodes, which are connected to CZs
foreach (Face t in innerTris)
{
foreach (ExtendedNode n in t.vrts)
{
n._belongs_to_cz = true;
}
}
List <ExtendedNode> nczs = new List <ExtendedNode>();
foreach (ExtendedNode nd in mg.nodes)
{
if (nd._belongs_to_cz)
{
nczs.Add(nd);
}
}
// create linked list
LinkedList <Node> ll = new LinkedList <Node>(mg.nodes);
LinkedListNode <Node> lln = ll.First;
do
{
((ExtendedNode)lln.Value).ll_node = lln;
lln = lln.Next;
} while (lln != null);
// split the nodes, which belong to cohesive elements
foreach (ExtendedNode nd in nczs)
{
nd.SplitNode(ll);
}
// linked list becomes the new list of nodes; resequence
mg.nodes = new List <Node>(ll);
for (int i = 0; i < mg.nodes.Count; i++)
{
mg.nodes[i].id = i;
}
// infer cz.vrts[] from fidx
foreach (ExtendedCZ cz in mg.czs)
{
cz.ReinitializeVerticeArrays();
}
// restore the list of faces and the list of edges
mg.edges.Clear();
mg.faces.Clear();
foreach (ExtendedElement e in mg.elems)
{
foreach (int i in e.edges)
{
mg.edges.Add(e.GetEdge(i));
}
}
foreach (GranuleEdge ge in mg.edges)
{
ge.exposed = true;
}
// first, create exposed faces from surfaceFaces array
foreach (Tuple <ExtendedElement, int> tuple in surfaceFaces)
{
ExtendedElement elem = tuple.Item1;
int which = tuple.Item2;
Face fc = elem.GetFace(which);
fc.exposed = true;
fc.id = mg.faces.Count;
mg.faces.Add(fc);
}
// create non-exposed faces from CZs, record their references into cz.faces
foreach (ExtendedCZ cz in mg.czs)
{
Face fc = cz.e0.GetFace(cz.fidx0);
fc.elem = cz.e0;
cz.faces[0] = fc;
fc.granule = cz.e0.granule;
fc.exposed = false;
fc.id = mg.faces.Count;
mg.faces.Add(fc);
fc = cz.e1.GetFace(cz.fidx1);
fc.elem = cz.e1;
cz.faces[1] = fc;
fc.exposed = false;
fc.id = mg.faces.Count;
fc.granule = cz.e1.granule;
mg.faces.Add(fc);
}
// convert extended nodes back to regular
mg.ConvertBack();
mg.DetectSurfacesAfterLoadingMSH();
for (int i = 0; i < mg.czs.Count; i++)
{
mg.czs[i].immutableID = i;
}
}
/**
* Implements an approximation algorithm for bidirectional problems with
* exactly one initial and one goal state. The algorithm guarantees the
* following: If the queue is ordered by path costs (uniform cost search),
* the path costs of the solution will be less or equal to the costs of the
* best solution multiplied with two. Especially, if all step costs are
* equal and the reverse problem provides reverse actions for all actions of
* the original problem, the path costs of the result will exceed the
* optimal path by the costs of one step at maximum.
*
* @param problem
* a bidirectional search problem
* @param frontier
* the data structure to be used to decide which node to be
* expanded next
*
* @return a list of actions to the goal if the goal was found, a list
* containing a single NoOp Action if already at the goal, or an
* empty list if the goal could not be found.
*/
public override Node <S, A> findNode(IProblem <S, A> problem, ICollection <Node <S, A> > frontier)
{
if (!(problem is IBidirectionalProblem <S, A>))
{
throw new IllegalArgumentException("problem is not a BidirectionalProblem<S, A>");
}
this.isCancelled = false;
nodeExpander.useParentLinks(true); // bidirectional search needs parents!
this.frontier = frontier;
clearMetrics();
explored.Get(ORG_P_IDX).Clear();
explored.Get(REV_P_IDX).Clear();
IProblem <S, A> orgP = ((IBidirectionalProblem <S, A>)problem).getOriginalProblem();
IProblem <S, A> revP = ((IBidirectionalProblem <S, A>)problem).getReverseProblem();
ExtendedNode initStateNode;
initStateNode = new ExtendedNode(nodeExpander.createRootNode(orgP.getInitialState()), ORG_P_IDX);
goalStateNode = new ExtendedNode(nodeExpander.createRootNode(revP.getInitialState()), REV_P_IDX);
if (orgP.getInitialState().Equals(revP.getInitialState()))
{
return(getSolution(orgP, initStateNode, goalStateNode));
}
// initialize the frontier using the initial state of the problem
addToFrontier(initStateNode);
addToFrontier(goalStateNode);
while (!isFrontierEmpty() && !this.isCancelled)
{
// choose a leaf node and remove it from the frontier
ExtendedNode nodeToExpand = (ExtendedNode)removeFromFrontier();
ExtendedNode nodeFromOtherProblem;
// if the node contains a goal state then return the
// corresponding solution
if (!earlyGoalTest && (nodeFromOtherProblem = getCorrespondingNodeFromOtherProblem(nodeToExpand)) != null)
{
return(getSolution(orgP, nodeToExpand, nodeFromOtherProblem));
}
// expand the chosen node, adding the resulting nodes to the
// frontier
foreach (Node <S, A> s in nodeExpander.expand(nodeToExpand, problem))
{
ExtendedNode successor = new ExtendedNode(s, nodeToExpand.getProblemIndex());
if (!isReverseActionTestEnabled || nodeToExpand.getProblemIndex() == ORG_P_IDX ||
getReverseAction(orgP, successor) != null)
{
if (earlyGoalTest &&
(nodeFromOtherProblem = getCorrespondingNodeFromOtherProblem(successor)) != null)
{
return(getSolution(orgP, successor, nodeFromOtherProblem));
}
addToFrontier(successor);
}
}
}
// if the frontier is empty then return failure
return(null);
}
private void setExplored(Node <S, A> node)
{
ExtendedNode eNode = (ExtendedNode)node;
explored.Get(eNode.getProblemIndex()).Put(eNode.getState(), eNode);
}
private bool isExplored(Node <S, A> node)
{
ExtendedNode eNode = (ExtendedNode)node;
return(explored.Get(eNode.getProblemIndex()).ContainsKey(eNode.getState()));
}
