Use terrain elevation for lookAt gesture origin

worldwind/src/main/java/gov/nasa/worldwind/BasicFrameController.java

@@ -65,7 +65,6 @@ protected void renderTerrainPickedObject(RenderContext rc) {
         // picked object ID and the intersection position.
         if (rc.pickRay != null && rc.terrain.intersect(rc.pickRay, this.pickPoint)) {
             rc.globe.cartesianToGeographic(this.pickPoint.x, this.pickPoint.y, this.pickPoint.z, this.pickPos);
-            this.pickPos.altitude = 0; // report the actual altitude, which may not lie on the terrain's surface
             rc.offerPickedObject(PickedObject.fromTerrain(pickedObjectId, this.pickPos));
         }
     }

worldwind/src/main/java/gov/nasa/worldwind/BasicWorldWindowController.java

@@ -10,6 +10,7 @@
 import java.util.Arrays;
 import java.util.List;
 
+import gov.nasa.worldwind.geom.Camera;
 import gov.nasa.worldwind.geom.Location;
 import gov.nasa.worldwind.geom.LookAt;
 import gov.nasa.worldwind.gesture.GestureListener;
@@ -33,6 +34,8 @@ public class BasicWorldWindowController implements WorldWindowController, Gestur
 
     protected LookAt beginLookAt = new LookAt();
 
+    protected Camera camera = new Camera();
+
     protected int activeGestures;
 
     protected GestureRecognizer panRecognizer = new PanRecognizer();
@@ -225,10 +228,25 @@ protected void applyLimits(LookAt lookAt) {
     protected void gestureDidBegin() {
         if (this.activeGestures++ == 0) {
             this.wwd.getNavigator().getAsLookAt(this.wwd.getGlobe(), this.beginLookAt);
+            this.trySetBeginLookAtToTerrain();
             this.lookAt.set(this.beginLookAt);
         }
     }
 
+    protected void trySetBeginLookAtToTerrain() {
+        if (this.wwd.getWidth() <= 0 || this.wwd.getHeight() <= 0) {
+            return;
+        }
+
+        PickedObject terrain = this.wwd.pick(this.wwd.getWidth() / 2f, this.wwd.getHeight() / 2f).terrainPickedObject();
+        if (terrain == null || terrain.getTerrainPosition() == null) {
+            return;
+        }
+
+        this.wwd.getNavigator().getAsCamera(this.wwd.getGlobe(), this.camera);
+        this.wwd.getNavigator().cameraToLookAt(this.wwd.getGlobe(), this.camera, terrain.getTerrainPosition(), this.beginLookAt);
+    }
+
     protected void gestureDidEnd() {
         if (this.activeGestures > 0) { // this should always be the case, but we check anyway
             this.activeGestures--;

worldwind/src/main/java/gov/nasa/worldwind/Navigator.java

@@ -192,16 +192,27 @@ public Matrix4 getAsViewingMatrix(Globe globe, Matrix4 result) {
     }
 
     protected LookAt cameraToLookAt(Globe globe, Camera camera, LookAt result) {
+        return this.cameraToLookAt(globe, camera, null, result);
+    }
+
+    protected LookAt cameraToLookAt(Globe globe, Camera camera, Position lookAtPosition, LookAt result) {
         this.cameraToViewingMatrix(globe, camera, this.modelview);
-        this.modelview.extractEyePoint(this.forwardRay.origin);
-        this.modelview.extractForwardVector(this.forwardRay.direction);
 
-        if (!globe.intersect(this.forwardRay, this.originPoint)) {
-            double horizon = globe.horizonDistance(camera.altitude);
-            this.forwardRay.pointAt(horizon, this.originPoint);
+        if (lookAtPosition != null) {
+            this.originPos.set(lookAtPosition);
+            globe.geographicToCartesian(this.originPos.latitude, this.originPos.longitude, this.originPos.altitude, this.originPoint);
+        } else {
+            this.modelview.extractEyePoint(this.forwardRay.origin);
+            this.modelview.extractForwardVector(this.forwardRay.direction);
+
+            if (!globe.intersect(this.forwardRay, this.originPoint)) {
+                double horizon = globe.horizonDistance(camera.altitude);
+                this.forwardRay.pointAt(horizon, this.originPoint);
+            }
+
+            globe.cartesianToGeographic(this.originPoint.x, this.originPoint.y, this.originPoint.z, this.originPos);
         }
 
-        globe.cartesianToGeographic(this.originPoint.x, this.originPoint.y, this.originPoint.z, this.originPos);
         globe.cartesianToLocalTransform(this.originPoint.x, this.originPoint.y, this.originPoint.z, this.origin);
         this.modelview.multiplyByMatrix(this.origin);
 

worldwind/src/test/java/gov/nasa/worldwind/NavigatorTest.java

@@ -0,0 +1,82 @@
+/*
+ * Copyright (c) 2016 United States Government as represented by the Administrator of the
+ * National Aeronautics and Space Administration. All Rights Reserved.
+ */
+
+package gov.nasa.worldwind;
+
+import org.junit.Before;
+import org.junit.Test;
+import org.junit.runner.RunWith;
+import org.powermock.api.mockito.PowerMockito;
+import org.powermock.core.classloader.annotations.PrepareForTest;
+import org.powermock.modules.junit4.PowerMockRunner;
+
+import gov.nasa.worldwind.geom.Camera;
+import gov.nasa.worldwind.geom.LookAt;
+import gov.nasa.worldwind.geom.Position;
+import gov.nasa.worldwind.globe.Globe;
+import gov.nasa.worldwind.globe.ProjectionWgs84;
+import gov.nasa.worldwind.util.Logger;
+
+import static org.junit.Assert.assertEquals;
+import static org.junit.Assert.assertTrue;
+
+@RunWith(PowerMockRunner.class)
+@PrepareForTest(Logger.class)
+public class NavigatorTest {
+
+    private static final double ANGLE_DELTA = 1.0e-7;
+
+    private static final double ALTITUDE_DELTA = 1.0e-6;
+
+    private Globe globe;
+
+    private Navigator navigator;
+
+    @Before
+    public void setUp() {
+        PowerMockito.mockStatic(Logger.class);
+
+        this.globe = new Globe(WorldWind.WGS84_ELLIPSOID, new ProjectionWgs84());
+        this.navigator = new Navigator();
+        this.navigator.setLatitude(34.2);
+        this.navigator.setLongitude(-118.2);
+        this.navigator.setAltitude(20000);
+        this.navigator.setHeading(35);
+        this.navigator.setTilt(55);
+        this.navigator.setRoll(3);
+    }
+
+    @Test
+    public void testCameraToLookAt_UsesSuppliedLookAtPosition() {
+        LookAt baseline = this.navigator.getAsLookAt(this.globe, new LookAt());
+        Camera camera = this.navigator.getAsCamera(this.globe, new Camera());
+
+        Position terrainPosition = new Position(baseline.latitude, baseline.longitude, 1500);
+        LookAt terrainLookAt = this.navigator.cameraToLookAt(this.globe, camera, terrainPosition, new LookAt());
+
+        assertEquals("latitude", terrainPosition.latitude, terrainLookAt.latitude, ANGLE_DELTA);
+        assertEquals("longitude", terrainPosition.longitude, terrainLookAt.longitude, ANGLE_DELTA);
+        assertEquals("altitude", terrainPosition.altitude, terrainLookAt.altitude, ALTITUDE_DELTA);
+        assertEquals("heading", baseline.heading, terrainLookAt.heading, ANGLE_DELTA);
+        assertEquals("tilt", baseline.tilt, terrainLookAt.tilt, ANGLE_DELTA);
+        assertTrue("range", terrainLookAt.range < baseline.range);
+    }
+
+    @Test
+    public void testCameraToLookAt_WithNullPositionMatchesLegacyPath() {
+        Camera camera = this.navigator.getAsCamera(this.globe, new Camera());
+
+        LookAt legacyLookAt = this.navigator.cameraToLookAt(this.globe, camera, new LookAt());
+        LookAt nullPositionLookAt = this.navigator.cameraToLookAt(this.globe, camera, (Position) null, new LookAt());
+
+        assertEquals("latitude", legacyLookAt.latitude, nullPositionLookAt.latitude, ANGLE_DELTA);
+        assertEquals("longitude", legacyLookAt.longitude, nullPositionLookAt.longitude, ANGLE_DELTA);
+        assertEquals("altitude", legacyLookAt.altitude, nullPositionLookAt.altitude, ALTITUDE_DELTA);
+        assertEquals("range", legacyLookAt.range, nullPositionLookAt.range, ALTITUDE_DELTA);
+        assertEquals("heading", legacyLookAt.heading, nullPositionLookAt.heading, ANGLE_DELTA);
+        assertEquals("tilt", legacyLookAt.tilt, nullPositionLookAt.tilt, ANGLE_DELTA);
+        assertEquals("roll", legacyLookAt.roll, nullPositionLookAt.roll, ANGLE_DELTA);
+    }
+}