/** * 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; }