/**
* Performs a bi-directional best-first graph search on the tf graph to try to find a path from
* sourceFrame to targetFrame, at the given time. One priority queue is used to keep a sorted list
* of all search nodes (from both directions, ordered descending by their potential of
* contributing to a good solution). At the moment, the cost of the path from A to B is defined as
* the largest absolute difference between the time stamps of the transforms from A to B and the
* given time point. This corresponds to searching for a transform path that needs the least
* amount of inter- and extrapolation.
*
* <p>Note: often in search, if we talk about expanding a search node, we say that the node
* expands and its _children_ are added to the queue. Yet, the tf graph is stored by linking child
* frames to their _parent_ frames, not the other way around. So, if a search node is expanded,
* the _parent_ frames are added to the queue. This may be a bit confusing.
*/
protected boolean lookupLists(
Frame targetFrame,
Frame sourceFrame,
long time,
LinkedList<TransformStorage> inverseTransforms,
LinkedList<TransformStorage> forwardTransforms) {
// wrap the source and target frames in search nodes
SearchNode<Frame> sourceNode = new SearchNode<Frame>(sourceFrame);
SearchNode<Frame> targetNode = new SearchNode<Frame>(targetFrame);
// set beginning of forward path (from source)
sourceNode.backwardStep = sourceNode;
// set beginning of backward path (form target)
targetNode.forwardStep = targetNode;
// create a hash map that map frames to search nodes. This is necessary to keep track of
// which frames have already been visited (and from which direction).
HashMap<Frame, SearchNode<Frame>> frameToNode = new HashMap<Frame, SearchNode<Frame>>();
// add source and target search nodes to the map
frameToNode.put(sourceFrame, sourceNode);
frameToNode.put(targetFrame, targetNode);
// create a priority queue, which will hold the search nodes ordered by cost (descending)
PriorityQueue<SearchNode<Frame>> Q = new PriorityQueue<SearchNode<Frame>>();
// at the source and target search nodes to the queue
Q.add(sourceNode);
Q.add(targetNode);
// perform the search
while (!Q.isEmpty()) {
// poll most potential search node from queue
SearchNode<Frame> frameNode = Q.poll();
Frame frame = frameNode.content;
// if the node is both visited from the source and from the target node, a path has been found
if (frameNode.backwardStep != null && frameNode.forwardStep != null) {
// found the best path from source to target through FRAME.
// create inverse list (from source to FRAME)
SearchNode<Frame> node = frameNode;
while (node.content != sourceNode.content) {
inverseTransforms.addLast(node.backwardStep.content.getData(time, node.content));
node = node.backwardStep;
}
// create forward list (from FRAME to target)
node = frameNode;
while (node.content != targetNode.content) {
forwardTransforms.addLast(node.forwardStep.content.getData(time, node.content));
node = node.forwardStep;
}
return true;
}
// expand search node
for (Frame parentFrame : frame.getParentFrames()) {
SearchNode<Frame> parentFrameNode = frameToNode.get(parentFrame);
boolean addToQueue = false;
if (parentFrameNode == null) {
// node was not yet visited
parentFrameNode = new SearchNode<Frame>(parentFrame);
frameToNode.put(parentFrame, parentFrameNode);
addToQueue = true;
} else {
// node is already visited
if ((parentFrameNode.backwardStep == null && frameNode.forwardStep == null)
|| (parentFrameNode.forwardStep == null && frameNode.backwardStep == null)) {
// node was visited, but from other direction.
// create new search node that represents this frame, visited from both sides
// this allows the other search node of this frame to still be expanded first
parentFrameNode = new SearchNode<Frame>(parentFrameNode);
addToQueue = true;
}
}
// add search node belonging to parent frame to the queue
if (addToQueue) {
// determine cost (based on max absolute difference in time stamp)
TimeCache cache = frame.getTimeCache(parentFrame);
parentFrameNode.cost =
Math.max(
(double) cache.timeToNearestTransform(time),
Math.max(parentFrameNode.cost, frameNode.cost));
// if visiting forward (from source), set backward step to remember path
if (frameNode.backwardStep != null) parentFrameNode.backwardStep = frameNode;
// if visiting backward (from target), set forward step to remember path
if (frameNode.forwardStep != null) parentFrameNode.forwardStep = frameNode;
// add node to queue
Q.add(parentFrameNode);
}
}
}
// target and source frames are not connected.
return false;
}
