Matlab Version & new exact method mars2wgs (#51)

* Matlab Version & new exact methed mars2wgs

Added a fully vecorized version fo the code in matlab.

Also added new method mars2wgs that is a much faster version of gcj2wgs_exact from what I can tell it is roughly 10 x faster and much more accurate.
only 3x more time than the gcj2wgs
 non exact version.

* UTM

UTM Adds and tests

* updates based on Feedback for main merge

matlab/UTM/deg2utm.m

@@ -0,0 +1,63 @@
+function  [x,y,utmzone] = deg2utm(lat,long)
+% -------------------------------------------------------------------------
+% [x,y,utmzone] = deg2utm(Lat,Lon)
+%
+% Inputs:
+%    Lat: Latitude vector.   Degrees.  +ddd.ddddd  WGS84
+%    Lon: Longitude vector.  Degrees.  +ddd.ddddd  WGS84
+% Outputs:
+%    x, y , utmzone.   See example
+% Author: 
+%   Aaron Close
+%   Close Consulting
+%-------------------------------------------------------------------------
+% Argument checking
+%
+error(nargchk(2, 2, nargin));  %2 arguments required
+n1=length(lat);
+n2=length(long);
+if (n1~=n2)
+    error('Lat and Long vectors need to be the same length');
+end
+
+% set standards
+majorRadius = 6378137.000000;
+minorRadius = 6356752.314245;
+e2 = (((majorRadius^2) - (minorRadius^2))^0.5)/minorRadius;
+e2square = e2^2;
+meanRadius = (majorRadius^2)/minorRadius;
+alpha = (3/4)*e2square;
+beta = (5/3)*alpha^2;
+gamma = (35/27)*alpha^3;
+
+% Calculate Zone From Inital Point
+Zone = fix((long(1)/6) + 31);
+if (lat(1)>=0), hemisphear='N';
+else hemisphear='S';
+end
+utmzone=sprintf('%02d%c',Zone,hemisphear);
+%convert to radians
+
+S = ((Zone*6) - 183);
+lat = lat * (pi/180);
+long = long * (pi/180);
+deltaS = long - (S *(pi/180));
+
+% Vectorized main calculation
+a = cos(lat).*sin(deltaS);
+epsilon = 0.5.*log((1+a)./(1-a));
+nu = atan(tan(lat)./cos(deltaS))-lat;
+v = (meanRadius./((1 + (e2square.*(cos(lat)).^2))).^0.5).*0.9996;
+a1 = sin(2.*lat);
+a2 = a1.*(cos(lat)).^2;
+j2 = lat + (a1./2);
+j4 = ((3.*j2) + a2)./4;
+j6 = ((5.*j4) + (a2.*(cos(lat)).^2))./3;
+Bm = 0.9996*meanRadius*(lat-alpha.*j2 + beta.*j4 - gamma.*j6);
+ta = (e2square/2).*epsilon.^2.*(cos(lat)).^2;
+x = epsilon.*v.*(1 + (ta./3)) + 500000;
+y = nu.*v.*(1 + ta) + Bm;
+y(y<0)=9999999+y(y<0);
+
+end
+

matlab/UTM/utm2deg.m

@@ -0,0 +1,91 @@
+function  [Lat,Lon] = utm2deg(X,Y,utmzone)
+    % -------------------------------------------------------------------------
+    % [Lat,Lon] = utm2deg(x,y,utmzone)
+    % Description: Function to convert vectors of UTM coordinates into Lat/Lon vectors (WGS84).
+    % Some code has been extracted from UTMIP.m function by Gabriel Ruiz
+    % Martinez and utm2deg.m by Rafael Palacios
+    % This is a vectorized version based mostly of Rafael Palacios' version
+    % Inputs:
+    %    X vector or scaler,
+    %    Y vector or scaler,
+    %    utmzone of form '15N' or  '25S' no spaces and only north or south
+    %    designation
+    % Outputs:
+    %    Lat: Latitude vector or scaler.   Degrees.  +ddd.ddddd  WGS84
+    %    Lon: Longitude vector or scaler.  Degrees.  +ddd.ddddd  WGS84
+    %
+    % Example 1:
+    % x=[ 458731;  407653;  239027;  230253;  343898;  362850];
+    % y=[4462881; 5126290; 4163083; 3171843; 4302285; 2772478];
+    % utmzone='30N';
+    % [Lat, Lon]=utm2deg(X,Y,utmzone);
+    %
+    % Author:
+    %   Aaron Close
+    %   Close Consulting
+    %   Houston, TX
+    %%-------------------------------------------------------------------------
+    % Argument checking
+    %
+    error(nargchk(3, 3, nargin)); %3 arguments required
+    n1=length(X);
+    n2=length(X);
+    if (n1~=n2)
+        error('x,y vectors should have the same number or rows');
+    end
+
+    meanRadius=size(utmzone);
+    if (meanRadius~=3)
+        error('utmzone should be a vector of strings like "30N" or "15S" leading zero is required for zones less than 10' );
+    end
+
+    %% set zone info and check for errors
+    if ~(utmzone(3)=='N' || utmzone(3)=='S')
+        fprintf('utm2deg: Warning utmzone should be a vector of strings like "30N", not "30n"\n');
+        return;
+    end
+
+    if (utmzone(3)=='N')
+        hemisphere='N';   % Northern hemisphere
+    else
+        hemisphere='S';
+    end
+    Zone=str2double(utmzone(1:2));
+
+    %% one time calcs
+    majorRadius = 6378137.000000;
+    minorRadius = 6356752.314245;
+    e2 = (((majorRadius^2) - (minorRadius^2))^0.5)/minorRadius;
+    e2squared = e2^2;
+    meanRadius = (majorRadius^2)/minorRadius;
+    alpha = ( 3 / 4 ) * e2squared;
+    beta = ( 5 / 3 ) * alpha ^ 2;
+    gamma = ( 35 / 27 ) * alpha ^ 3;
+
+    X = X - 500000;
+    if hemisphere == 'S'
+        Y = Y - 10000000;
+    end
+    S = ( ( Zone * 6 ) - 183 );
+
+    % Vectorized Math
+    lat =  Y / ( 6366197.724 * 0.9996 );
+    v = ((meanRadius./(1+(e2squared*(cos(lat).^2))).^0.5)*0.9996);
+    a = X./ v;
+    a1 = sin( 2 * lat );
+    a2 = a1.* ( cos(lat) ).^2;
+    j2 = lat + ( a1 / 2 );
+    j4 = ( ( 3 * j2 ) + a2 ) / 4;
+    j6 = ( ( 5 * j4 ) + ( a2.*(cos(lat)).^ 2) ) / 3;
+    Bm = 0.9996 * meanRadius * ( lat - alpha * j2 + beta * j4 - gamma * j6 );
+    b = ( Y - Bm )./v;
+    Epsi = ((e2squared*a.^2 )./2 ).*(cos(lat)).^2;
+    Eps = a.*(1-(Epsi./3));
+    nab = (b.*(1-Epsi)) + lat;
+    senoheps = (exp(Eps) - exp(-Eps))./2;
+    Delt = atan(senoheps./cos(nab));
+    TaO = atan(cos(Delt).*tan(nab));
+    Lon = (Delt.*(180/pi))+S;
+    Lat = (lat+(1+e2squared.*(cos(lat).^2)-(3/2)*e2squared.*sin(lat).*cos(lat).*(TaO-lat)).*(TaO-lat)).*(180/pi);
+
+end

matlab/bd2gcj.m

@@ -0,0 +1,17 @@
+function [gcjLat, gcjLng]  = bd2gcj(bdLat, bdLng)
+    gcjLat = bdLat;
+    gcjLng = bdLng;
+
+    x_pi = pi * 3000.0 / 180.0;
+    inChina = ~outOfChina(bdLat, bdLng);
+    if ~any(inChina),return;end
+
+    x = bdLng(inChina) - 0.0065;
+    y = bdLat(inChina) - 0.006;
+
+    z = hypot(x, y) - 0.00002 * sin(y * x_pi);
+    theta = atan2(y, x) - 0.000003 * cos(x * x_pi);
+    gcjLng(inChina) = z.*cos(theta);
+    gcjLat(inChina) = z.*sin(theta);
+
+end

matlab/bd2wgs.m

@@ -0,0 +1,10 @@
+function [wgsLat, wgsLng] = bd2wgs(bdLat, bdLng)
+
+[gcjLat, gcjLng] = bd2gcj(bdLat, bdLng);
+[wgsLat, wgsLng] = gcj2wgs(gcjLat, gcjLng);
+
+end
+
+
+
+

matlab/delta.m

@@ -0,0 +1,18 @@
+
+function [dLat dLng] = delta(lat,lng)
+
+    earthRadius = 6378245 % Replace from because of differnet ellipsoid 6378137.0;
+    ee = 0.00669342162296594323;
+
+    [dLat, dLng] = transform(lat, lng);
+
+    radConv = pi/180;
+    radLat = lat * radConv;
+    magic = 1 - ee * sin(radLat).^2;
+    sqrtMagic = sqrt(magic);
+
+    dLat = (dLat ./((earthRadius * (1 - ee))./(magic.*sqrtMagic))./radConv);
+    dLng = (dLng ./(earthRadius./sqrtMagic.*cos(radLat))./radConv);
+
+
+end

matlab/distance.m

@@ -0,0 +1,18 @@
+function totalDistance =  distance(latA, lngA, latB, lngB)
+% Fully vectorized version of code
+
+    earthR = 6371000; % Updated from 6378137.0
+    pi180 = pi / 180;
+    radLatA = latA * pi180;
+    radLatB = latB * pi180;
+    x = (cos(radLatA).*cos(radLatB).*cos((lngA - lngB)*pi180));
+    y = sin(radLatA).*sin(radLatB);
+
+    s = x + y;
+    s(s > 1)=1;
+    s(s < -1) = -1;
+
+    alpha = acos(s);
+    totalDistance = alpha * earthR;
+
+end

matlab/gcj2bd.m

@@ -0,0 +1,21 @@
+function  [bdLat,bdLng]= gcj2bd(gcjLat, gcjLng)
+
+    bdLat = gcjLat;
+    bdLng = gcjLng;
+
+    x_pi = pi * 3000.0 / 180.0;
+    inChina = ~outOfChina(gcjLat, gcjLng);
+    if ~any(inChina),return;end
+
+    x = gcjLng(inChina);
+    y = gcjLat(inChina);
+    z = hypot(x, y) + 0.00002 * sin(y * x_pi);
+    theta = atan2(y, x) + 0.000003 * cos(x * x_pi);
+    bdLng(inChina) = z.* cos(theta) + 0.0065;
+    bdLat(inChina) = z.* sin(theta) + 0.006;
+
+
+
+end
+
+

matlab/gcj2wgs.m

@@ -0,0 +1,17 @@
+function [lat, long ] = gcj2wgs(gcjLat, gcjLng)
+% Not exact
+% Fully vectorized
+
+    lat = gcjLat;
+    long = gcjLng;
+
+    inChina = ~outOfChina(gcjLat, gcjLng);
+
+    [dlat, dlng] = delta(gcjLat(inChina), gcjLng(inChina));
+
+    lat(inChina) = gcjLat(inChina) - dlat;
+    long(inChina) = gcjLng(inChina) - dlng;
+
+end
+
+

matlab/gcj2wgs_exact.m

@@ -0,0 +1,33 @@
+function [lat, long ] = gcj2wgs_exact(gcjLat, gcjLng)
+% adapted from https://github.com/googollee/eviltransform
+
+    initDelta = 0.01;
+    threshold = 0.000001;
+    dLat = initDelta;
+    dLng = initDelta;
+    mLat = gcjLat - dLat;
+    mLng = gcjLng - dLng;
+    pLat = gcjLat + dLat;
+    pLng = gcjLng + dLng;
+
+    for i=1:30
+        wgsLat = (mLat + pLat) / 2;
+        wgsLng = (mLng + pLng) / 2;
+        [tmplat, tmplng] = wgs2gcj(wgsLat, wgsLng);
+        dLat = tmplat - gcjLat;
+        dLng = tmplng - gcjLng;
+        if all(abs(dLat) < threshold & abs(dLng) < threshold)
+            lat = wgsLat;
+            long =wgsLng;
+            return;
+        end
+        pLat(dLat>=0) = wgsLat(dLat>=0);
+        mLat(dLat<=0) = wgsLat(dLat<=0);
+        pLng(dLng>=0) = wgsLng(dLng>=0);
+        mLng(dLng<=0) = wgsLng(dLng<=0);
+    end
+
+   lat = wgsLat;
+   long =wgsLng;
+
+end

matlab/mars2wgs.m

@@ -0,0 +1,23 @@
+function [lat, long ] = mars2wgs(marsLat, marsLong)
+% assumption is that inital mars lat longs are close to actual
+% then solves for the local delta and estimates the original lat longs
+% then uses the estimated lat longs to try again
+% this method seems very stable
+% initialize estimates of lat and long
+
+lat = marsLat;
+long = marsLong;
+threshold = 0.0000001;
+
+for i=1:30
+    [tempLat,tempLong] = wgs2gcj(lat, long);
+    deltaLat = lat - tempLat;
+    deltaLong = long - tempLong;
+    lat = marsLat + deltaLat;
+    long = marsLong + deltaLong;
+    if all(abs(tempLat-marsLat)<threshold & abs(tempLong-marsLong)<threshold)
+        return;
+    end
+end
+
+end

matlab/outOfChina.m

@@ -0,0 +1,5 @@
+function result = outOfChina(lat,long)
+
+    result = [long < 72.004 | long > 137.8347 | lat < 0.8293 | lat > 55.8271];
+
+end