/**
* Zooms the camera around the specified focus coordinates.
*
* @param focusX the x coordinate to focus on
* @param focusY the y coordinate to focus on
* @param factor the zoom will be scaled by this factor
*/
public void zoom(double focusX, double focusY, double factor) {
synchronized (mutex) {
Transform focus = Transform.translation(toMetricCoordinates((int) focusX, (int) focusY));
double zoom = RosMath.clamp(getZoom() * factor, MINIMUM_ZOOM, MAXIMUM_ZOOM) / getZoom();
transform = transform.multiply(focus).scale(zoom).multiply(focus.invert());
}
}
@Override
public boolean onTouchEvent(VisualizationView view, MotionEvent event) {
if (visible) {
Preconditions.checkNotNull(pose);
if (event.getAction() == MotionEvent.ACTION_MOVE) {
Vector3 orientationVector =
camera
.toWorldCoordinates(new Point((int) event.getX(), (int) event.getY()))
.subtract(pose.getTranslation());
if (orientationVector.length() > 0) {
pose.setRotation(
Quaternion.rotationBetweenVectors(new Vector3(1, 0, 0), orientationVector));
} else {
pose.setRotation(Quaternion.newIdentityQuaternion());
}
shape.setTransform(pose);
requestRender();
return true;
} else if (event.getAction() == MotionEvent.ACTION_UP) {
posePublisher.publish(
pose.toPoseStampedMessage(
camera.getFixedFrame(),
connectedNode.getCurrentTime(),
posePublisher.newMessage()));
visible = false;
requestRender();
return true;
}
}
gestureDetector.onTouchEvent(event);
return false;
}
/**
* Rotates the camera round the specified coordinates.
*
* @param focusX the x coordinate to focus on
* @param focusY the y coordinate to focus on
* @param deltaAngle the camera will be rotated by {@code deltaAngle} radians
*/
public void rotate(double focusX, double focusY, double deltaAngle) {
synchronized (mutex) {
Transform focus = Transform.translation(toMetricCoordinates((int) focusX, (int) focusY));
transform =
transform
.multiply(focus)
.multiply(Transform.zRotation(deltaAngle))
.multiply(focus.invert());
}
}
/**
* Changes the camera frame to the specified frame and aligns the camera with the new frame.
*
* @param frame the new camera frame
*/
public void jumpToFrame(GraphName frame) {
synchronized (mutex) {
this.frame = frame;
double zoom = getZoom();
resetTransform();
transform = transform.scale(zoom / getZoom());
}
}
/**
* Changes the camera frame to the specified frame.
*
* <p>If possible, the camera will avoid jumping on the next frame.
*
* @param frame the new camera frame
*/
public void setFrame(GraphName frame) {
Preconditions.checkNotNull(frame);
synchronized (mutex) {
if (this.frame != null && this.frame != frame) {
FrameTransform frameTransform = frameTransformTree.transform(frame, this.frame);
if (frameTransform != null) {
// Best effort to prevent the camera from jumping. If we don't have
// the transform yet, we can't help matters.
transform = transform.multiply(frameTransform.getTransform());
}
}
this.frame = frame;
}
}
/**
* Translates the camera.
*
* @param deltaX distance to move in x in pixels
* @param deltaY distance to move in y in pixels
*/
public void translate(double deltaX, double deltaY) {
synchronized (mutex) {
transform = Transform.translation(deltaX, deltaY, 0).multiply(transform);
}
}
private void resetTransform() {
// Rotate coordinate system to match ROS standard (x is forward, y is left).
transform = Transform.zRotation(Math.PI / 2).scale(DEFAULT_ZOOM);
}
/**
* @return the metric coordinates of the provided pixel coordinates where the origin is the top
* left corner of the view
*/
public Vector3 toMetricCoordinates(int x, int y) {
double centeredX = x - viewport.getWidth() / 2.0d;
double centeredY = viewport.getHeight() / 2.0d - y;
return transform.invert().apply(new Vector3(centeredX, centeredY, 0));
}
/** @return the current zoom factor */
public double getZoom() {
return transform.getScale();
}
