XYOrthographicCamera.java

Go to the documentation of this file.

/*
 * Copyright (C) 2011 Google Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */

package org.ros.android.view.visualization;

import com.google.common.base.Preconditions;

import org.ros.math.RosMath;
import org.ros.namespace.GraphName;
import org.ros.rosjava_geometry.FrameTransform;
import org.ros.rosjava_geometry.FrameTransformTree;
import org.ros.rosjava_geometry.Transform;
import org.ros.rosjava_geometry.Vector3;

import javax.microedition.khronos.opengles.GL10;

public class XYOrthographicCamera {

  private static final double PIXELS_PER_METER = 100.0;

  private static final Transform ROS_TO_SCREEN_TRANSFORM = Transform.zRotation(Math.PI / 2).scale(
      PIXELS_PER_METER);

  private static final float MINIMUM_ZOOM_FACTOR = 0.1f;

  private static final float MAXIMUM_ZOOM_FACTOR = 5.f;

  private final FrameTransformTree frameTransformTree;
  private final Object mutex;

  private Viewport viewport;

  private Transform cameraToRosTransform;

  private GraphName frame;

  public XYOrthographicCamera(FrameTransformTree frameTransformTree) {
    this.frameTransformTree = frameTransformTree;
    mutex = new Object();
    resetTransform();
  }

  private void resetTransform() {
    cameraToRosTransform = Transform.identity();
  }

  public void apply(GL10 gl) {
    synchronized (mutex) {
      OpenGlTransform.apply(gl, ROS_TO_SCREEN_TRANSFORM);
      OpenGlTransform.apply(gl, cameraToRosTransform);
    }
  }

  public boolean applyFrameTransform(GL10 gl, GraphName frame) {
    Preconditions.checkNotNull(frame);
    if (this.frame != null) {
      FrameTransform frameTransform = frameTransformTree.transform(frame, this.frame);
      if (frameTransform != null) {
        OpenGlTransform.apply(gl, frameTransform.getTransform());
        return true;
      }
    }
    return false;
  }

  public void translate(double deltaX, double deltaY) {
    synchronized (mutex) {
      cameraToRosTransform =
          ROS_TO_SCREEN_TRANSFORM.invert().multiply(Transform.translation(deltaX, deltaY, 0))
              .multiply(getCameraToScreenTransform());
    }
  }

  private Transform getCameraToScreenTransform() {
    return ROS_TO_SCREEN_TRANSFORM.multiply(cameraToRosTransform);
  }

  public Transform getScreenTransform(GraphName targetFrame) {
    FrameTransform frameTransform = frameTransformTree.transform(frame, targetFrame);
    return frameTransform.getTransform().multiply(getCameraToScreenTransform().invert());
  }

  public void rotate(double focusX, double focusY, double deltaAngle) {
    synchronized (mutex) {
      Transform focus = Transform.translation(toCameraFrame((int) focusX, (int) focusY));
      cameraToRosTransform =
          cameraToRosTransform.multiply(focus).multiply(Transform.zRotation(deltaAngle))
              .multiply(focus.invert());
    }
  }

  public void zoom(double focusX, double focusY, double factor) {
    synchronized (mutex) {
      Transform focus = Transform.translation(toCameraFrame((int) focusX, (int) focusY));
      double scale = cameraToRosTransform.getScale();
      double zoom = RosMath.clamp(scale * factor, MINIMUM_ZOOM_FACTOR, MAXIMUM_ZOOM_FACTOR) / scale;
      cameraToRosTransform =
          cameraToRosTransform.multiply(focus).scale(zoom).multiply(focus.invert());
    }
  }

  public double getZoom() {
    return cameraToRosTransform.getScale() * PIXELS_PER_METER;
  }

  public Vector3 toCameraFrame(int pixelX, int pixelY) {
    final double centeredX = pixelX - viewport.getWidth() / 2.0d;
    final double centeredY = viewport.getHeight() / 2.0d - pixelY;
    return getCameraToScreenTransform().invert().apply(new Vector3(centeredX, centeredY, 0));
  }

  public Transform toFrame(final int pixelX, final int pixelY, final GraphName frame) {
    final Transform translation = Transform.translation(toCameraFrame(pixelX, pixelY));
    final FrameTransform cameraToFrame =
        frameTransformTree.transform(this.frame, frame);
    return cameraToFrame.getTransform().multiply(translation);
  }

  public GraphName getFrame() {
    return 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.
          cameraToRosTransform = cameraToRosTransform.multiply(frameTransform.getTransform());
        }
      }
      this.frame = frame;
    }
  }

  public void setFrame(String frame) {
    setFrame(GraphName.of(frame));
  }

  public void jumpToFrame(GraphName frame) {
    synchronized (mutex) {
      this.frame = frame;
      final double scale = cameraToRosTransform.getScale();
      resetTransform();
      cameraToRosTransform = cameraToRosTransform.scale(scale / cameraToRosTransform.getScale());
    }
  }

  public void jumpToFrame(String frame) {
    jumpToFrame(GraphName.of(frame));
  }

  public void setViewport(Viewport viewport) {
    Preconditions.checkNotNull(viewport);
    this.viewport = viewport;
  }

  public Viewport getViewport() {
    Preconditions.checkNotNull(viewport);
    return viewport;
  }

  public Transform getCameraToRosTransform() {
    // Transforms are immutable. No need for a defensive copy.
    return cameraToRosTransform;
  }
}