GeoMapping Bearing and Coordinate Calculation for GoogleMaps markers

Question

I'm writing an Android app and integrating GoogleMapsV2 API. I have a series of markers on the map at various locations around an anchor.

I want those markers to converge on the anchor's position incrementally.

I've got a loop running that will call each marker B and from B's position calculate the bearing to the anchor A. I then calculate the destination coordinate for a fixed distance along that bearing and update.

Here are the two functions (taken from an amalgamation of stack posts and a GeoMapping site, for full disclosure) I'm using:

public double calcBearing(double lat1, double lon1, double lat2, double lon2){
    double longitude1 = lon1;
    double longitude2 = lon2;
    double latitude1 = Math.toRadians(lat1);
    double latitude2 = Math.toRadians(lat2);
    double longDiff= Math.toRadians(longitude2-longitude1);
    double y= Math.sin(longDiff)*Math.cos(latitude2);
    double x=Math.cos(latitude1)*Math.sin(latitude2)-Math.sin(latitude1)*Math.cos(latitude2)*Math.cos(longDiff);

    double calcBearing =  (Math.toDegrees(Math.atan2(y, x))+360)%360;
    return calcBearing;
}

public Coordinate calcCoordFromPointBearing(double lat1, double lon1, double bearing, double distance){
    double rEarth = 6371.01; // Earth's average radius in km
    double epsilon = 0.000001; // threshold for floating-point equality

    double rLat1 = deg2rad(lat1);
    double rLon1 = deg2rad(lon1);
    double rbearing = deg2rad(bearing);
    double rdistance = distance / rEarth;

    double rlat = Math.asin( Math.sin(rLat1) * Math.cos(rdistance) + Math.cos(rLat1) * Math.sin(rdistance) * Math.cos(rbearing) );
    double rlon;
    if (Math.cos(rlat) == 0 || Math.abs(Math.cos(rlat)) < epsilon) // Endpoint a pole
            rlon=rLon1;
    else
        rlon = ( (rLon1 - Math.asin( Math.sin(rbearing)* Math.sin(rdistance) / Math.cos(rlat) ) + Math.PI ) % (2*Math.PI) ) - Math.PI;

    double lat = rad2deg(rlat);
    double lon = rad2deg(rlon);
    return new Coordinate(lat,lon);
}

private double deg2rad(double deg) {
    return (deg * Math.PI / 180.0);
}

private double rad2deg(double rad) {
    return (rad * 180.0 / Math.PI);
}

In short, I've screwed up the above calculations I believe. The behavior I'm seeing is the markers moving erratically and with a high frequency ending up heading towards two bearings: 90 and 270. As a result, they tend to move away from my anchor instead of towards it.

Can someone help me spot the mistake? I am passing in degrees to both the bearing function and the coordinate calculation function, but I'm converting them immediately to radians for the algorithm and back to degrees for usage elsewhere.

[UPDATE:

Most of the code came from this example:
Calculating coordinates given a bearing and a distance

It looks to me that the output longitude is being normalized to -180 to 180, which I'm plotting on a 360 degree space causing the outputs to head to the bearings 90 and 270. Any suggestions on the trig math change required to fix this?]

Answer 1

probably needs 360.0

 double calcBearing =  (Math.toDegrees(Math.atan2(y, x))+360.0)%360.0;

This was kindof answered here

You still have another issue. Your not considering any tilt in the map. Why not just animate with the pixels. There won't be too much distortion of curvature. What you have to do is get the pixel position of the marker. You'll have to save the latlon when adding the marker or you have to add the markers with .setAnchor which gives you an offset in pixels. If you have the latlon of the marker placement then you get the point by.

LatLon ll;
Point p = mMap.getProjection().toScreenLocation(ll);

Then you can use code like this to animate the markers. I'm making a marker bounce below by interpolating the y axis. You'll have to interpolate both axi.

    final Handler handler = new Handler();
    final long start = SystemClock.uptimeMillis();
    final long duration = 2500;

    final Interpolator interpolator = new BounceInterpolator();

    handler.post(new Runnable() {
        @Override
        public void run() {
            long elapsed = SystemClock.uptimeMillis() - start;
            float t = Math.max(
                    1 - interpolator.getInterpolation((float) elapsed
                            / duration), 0);

            marker.setAnchor(0.5f, 1.0f + 6 * t);

            if (t > 0.0) {
                // Post again 16ms later.
                handler.postDelayed(this, 16);
            }
        }
    });

The above code is from this question. I apologize for any performance issues you have when you use the above method. But you could still use the pixel positions for a more traditional animation approach.

I've got almost the same formulas as you working in another program where I animate a map to move to the expected location based on a location bearing and speed. The formula is slightly different at the end than yours. I lifted it from here and changed to longer names.

    // Define the callback method that receives location updates
@Override
public void onLocationChanged(Location location) {

    // Given the bearing, speed, and current location
    // calculate what the expected location is traveling for an
    // interval that is slightly larger than two times fastest interval of
    // the location provider and animate the map movement to the
    // expected location over the same slightly larger interval.

    // In Theory by using an interval that is slightly larger
    // than two times fastest interval of the location provider for the
    // animation length a new animation will start before the
    // currently running animation finishes. This should ensure a
    // smooth animation of the map while traveling under most
    // circumstances.

    // Negative acceleration (braking)
    // should have acceptable map animation because the map
    // animation in theory never finishes.

    // Note longer intervals, large negative accelerations, just
    // braking at the start of an interval may result in the map moving
    // backwards. But it will still be animated.

    // Some handhelds might not be able to keep up

    // TODO CHECK THE age of the location

    // location.getSpeed() =meters/second
    // interval 1/1000 seconds
    // distance in radians km/6371

    // changed.
    // (location.getSpeed()m/s)(1/1000 interval seconds)( 1/1000 km/m)
    // (1/6371 radians/km) = radians/6371000000.0
    double expectedDistance = location.getSpeed() * expectedDistMultiplier;
    // latitude in Radians
    double currentLatitude = Math.toRadians(location.getLatitude());
    // longitude in Radians
    double longitude1 = Math.toRadians(location.getLongitude());
    double bearing;
    bearing = (location.hasBearing()) ? Math.toRadians(location
            .getBearing()) : 0;

    // calculate the expected latitude and longitude based on staring
    // location
    // , bearing, and distance

    double expectedLatitude = Math.asin(Math.sin(currentLatitude)
            * Math.cos(expectedDistance) + Math.cos(currentLatitude)
            * Math.sin(expectedDistance) * Math.cos(bearing));
    double a = Math.atan2(
            Math.sin(bearing) * Math.sin(expectedDistance)
                    * Math.cos(currentLatitude),
            Math.cos(expectedDistance) - Math.sin(currentLatitude)
                    * Math.sin(expectedLatitude));
    double expectedLongitude = longitude1 + a;
    expectedLongitude = (expectedLongitude + 3 * Math.PI) % (2 * Math.PI)
            - Math.PI;

    // convert to degrees for the expected destination
    double expectedLongitudeDestination = Math.toDegrees(expectedLongitude);
    double expectedLatitudeDestination = Math.toDegrees(expectedLatitude);

    // log everything for testing.
    Log.d("Location", "Bearing in radians" + bearing);
    Log.d("Location", "distance in km" + expectedDistance);
    Log.d("Location", "Current Latitude = " + location.getLatitude()
            + " Current Longitude = " + location.getLongitude());
    Log.d("Location", "New Latitude = " + expectedLatitudeDestination
            + " New Longitude = " + expectedLongitudeDestination);

    // build a camera update to animate positioning map to the expected
    // destination
    LatLng ll = new LatLng(expectedLatitudeDestination,
            expectedLongitudeDestination);
    CameraPosition.Builder cb = CameraPosition.builder()
            .zoom(mMap.getCameraPosition().zoom)
            .bearing(mMap.getCameraPosition().bearing)
            .tilt(mMap.getCameraPosition().tilt).target(ll);
    if (location.hasBearing()) {
        cb.bearing(location.getBearing());
    }
    CameraPosition camera = cb.build();
    CameraUpdate update = CameraUpdateFactory.newCameraPosition(camera);
    mMap.animateCamera(update, interval, this);
}