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