Calculate distance between two point of Latitude and Longitude

LatLongPoint.java

public class LatLongPoint {
	
	private double latitude;
	private double longitude;
	private double altitude;
	
	public LatLongPoint(double latitude, double longitude) {
		this(latitude, longitude, 0.0);
	}
	
	public LatLongPoint(double latitude, double longitude, double altitude) {
		this.latitude = latitude;
		this.longitude = longitude;
		this.altitude = altitude;
	}
	
	public double getLatitude() {
		return this.latitude;
	}
	
	public double getLongitude() {
		return this.longitude;
	}
	
	public double getAltitude() {
		return this.altitude;
	}
	
	public double distanceTo(LatLongPoint toPoint) {

	    //final int R = 6371000; // Radius of the earth
	    //final int R = 6378137;
	    final int R = 6370996;
	    
	    Double latDistance = deg2rad(toPoint.getLatitude() - this.getLatitude());
	    Double lonDistance = deg2rad(toPoint.getLongitude() - this.getLongitude());
	    Double a = Math.pow(Math.sin(latDistance / 2), 2)
	            + Math.cos(deg2rad(this.getLatitude())) * Math.cos(deg2rad(toPoint.getLatitude()))
	            * Math.pow(Math.sin(lonDistance / 2), 2);
	    Double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
	    double distance = R * c;

	    //return distance;
	    double height = this.getAltitude() - toPoint.getAltitude();
	    if (height > 0) {
		    distance = Math.pow(distance, 2) + Math.pow(height, 2);
		    distance = Math.sqrt(distance);
	    }
	    return distance;
	}
	
	private double deg2rad(double deg) {
	    return (deg * Math.PI / 180.0);
	}
	
	public static void main(String[] args) {
		//116.316473,39.990071
		LatLongPoint p = new LatLongPoint(39.990071, 116.316473);
		//116.317318,39.990071
		LatLongPoint tp = new LatLongPoint(39.990071, 116.317318, 0.0);
		double distance = p.distanceTo(tp);
		System.out.println("Distance: " + distance);
	}
}