Camera.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.rosjava_geometry.FrameTransform;
import org.ros.rosjava_geometry.FrameTransformTree;
import org.ros.rosjava_geometry.FrameName;
import org.ros.rosjava_geometry.Transform;
import org.ros.rosjava_geometry.Vector3;

import javax.microedition.khronos.opengles.GL10;

public class Camera {

  private static final double DEFAULT_ZOOM = 100.0;

  private static final float MINIMUM_ZOOM = 10;

  private static final float MAXIMUM_ZOOM = 500;

  private final FrameTransformTree frameTransformTree;
  private final Object mutex;

  private Viewport viewport;
  private Transform transform;

  private FrameName frame;

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

  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);
  }

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

    public boolean applyFrameTransform(GL10 gl, FrameName 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) {
      transform = Transform.translation(deltaX, deltaY, 0).multiply(transform);
    }
  }

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

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

  public double getZoom() {
    return transform.getScale();
  }

  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));
  }

  public FrameName getFrame() {
    return frame;
  }

  public void setFrame(FrameName 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;
    }
  }

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

  public void jumpToFrame(FrameName frame) {
    synchronized (mutex) {
      this.frame = frame;
      double zoom = getZoom();
      resetTransform();
      transform = transform.scale(zoom / getZoom());
    }
  }

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

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