/**
* @param source the source frame
* @return the {@link Transform} from {@code source} to root
*/
@VisibleForTesting
FrameTransform transformToRoot(GraphName source) {
FrameTransform result = getLatest(source);
if (result == null) {
return null;
}
while (true) {
FrameTransform resultToParent = lookUp(result.getTargetFrame(), result.getTime());
if (resultToParent == null) {
return result;
}
// Now resultToParent.getSourceFrame() == result.getTargetFrame()
Transform transform = resultToParent.getTransform().multiply(result.getTransform());
GraphName target = resultToParent.getTargetFrame();
result = new FrameTransform(transform, source, target, result.getTime());
}
}
/**
* @return the {@link FrameTransform} from source the frame to the target frame, or {@code null}
* if no {@link FrameTransform} could be found
*/
public FrameTransform transform(GraphName source, GraphName target) {
Preconditions.checkNotNull(source);
Preconditions.checkNotNull(target);
// This adds support for tf2 while maintaining backward compatibility with tf.
GraphName relativeSource = source.toRelative();
GraphName relativeTarget = target.toRelative();
if (relativeSource.equals(relativeTarget)) {
return new FrameTransform(Transform.identity(), relativeSource, relativeTarget, null);
}
FrameTransform sourceToRoot = transformToRoot(relativeSource);
FrameTransform targetToRoot = transformToRoot(relativeTarget);
if (sourceToRoot == null && targetToRoot == null) {
return null;
}
if (sourceToRoot == null) {
if (targetToRoot.getTargetFrame().equals(relativeSource)) {
// relativeSource is root.
return targetToRoot.invert();
} else {
return null;
}
}
if (targetToRoot == null) {
if (sourceToRoot.getTargetFrame().equals(relativeTarget)) {
// relativeTarget is root.
return sourceToRoot;
} else {
return null;
}
}
if (sourceToRoot.getTargetFrame().equals(targetToRoot.getTargetFrame())) {
// Neither relativeSource nor relativeTarget is root and both share the
// same root.
Transform transform =
targetToRoot.getTransform().invert().multiply(sourceToRoot.getTransform());
return new FrameTransform(transform, relativeSource, relativeTarget, sourceToRoot.getTime());
}
// No known transform.
return null;
}