Android กับ Map หาระยะทางจากตำแหน่งปัจจุบันไปยังที่เราปักหมุดไว้ ได้ยังไง

public double CalculationByDistance(GeoPoint StartP, GeoPoint EndP) {
    int Radius=6371;//radius of earth in Km         
    double lat1 = StartP.getLatitudeE6()/1E6;
    double lat2 = EndP.getLatitudeE6()/1E6;
    double lon1 = StartP.getLongitudeE6()/1E6;
    double lon2 = EndP.getLongitudeE6()/1E6;
    double dLat = Math.toRadians(lat2-lat1);
    double dLon = Math.toRadians(lon2-lon1);
    double a = Math.sin(dLat/2) * Math.sin(dLat/2) +
    Math.cos(Math.toRadians(lat1)) * Math.cos(Math.toRadians(lat2)) *
    Math.sin(dLon/2) * Math.sin(dLon/2);
    double c = 2 * Math.asin(Math.sqrt(a));
    double valueResult= Radius*c;
    double km=valueResult/1;
    DecimalFormat newFormat = new DecimalFormat("####");
    kmInDec =  Integer.valueOf(newFormat.format(km));
    meter=valueResult%1000;
    meterInDec= Integer.valueOf(newFormat.format(meter));
    Log.i("Radius Value",""+valueResult+"   KM  "+kmInDec+" Meter   "+meterInDec);

    return Radius * c;
 }