// EPS: Elgaard Positioning System: GPS navigation software. // Copyright (C) 1997 Niels Elgaard Larsen // This program is free software; you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation; either version 2 of the License, or // (at your option) any later version. // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // You should have received a copy of the GNU General Public License // along with this program; if not, write to the Free Software // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. // Niels Elgaard Larsen, // import java.awt.*; class Pos { static public Datum WGS84 = new Datum("WGS84", Elip.WGS84, 0d, 0d, 0d,100); static public Datum defaultDatum = new Datum("default", Elip.WGS84, 0d, 0d, 0d,-1); static public Datum datumList[] = { new Datum("Adindan", Elip.Clarke1880, -162.0, -12.0, 206.0,0), new Datum("Arc 1950", Elip.Clarke1880, -143.0, -90.0, -294.0,4), new Datum("Arc 1960", Elip.Clarke1880, -160.0, -8.0, -300.0,5), new Datum("Camp Area Astro", Elip.International, -104d, -129d, +239d,-1 ), // new Datum("European 1950", Elip.International, -87d, -96d, -120d,-1), new Datum("European 1950", Elip.International, -87d, -98d, -121d,28), new Datum("European 1979", Elip.International, -86d, -98d, -119d,29), new Datum("Finnish Nautical Chart", Elip.International, -78d, -231d, -97d,-1), new Datum("Geodetic Datum '49", Elip.International, +84d, -22d, +209d,32), new Datum("Hong Kong 1963", Elip.International, -156d, -271d, -189d,36), new Datum("Hu-Tzu-Shan", Elip.International, -634d, -549d, -201d,-1), new Datum("Indian Bangladesh", Elip.Everest1830, +289d, +734d, +257d,37), new Datum("NAD27 CONUS", Elip.Clarke1866, -8d, +160d, +176d,61), new Datum("NAD83", Elip.GRS1980, 0d, 0d, 0d,66), new Datum("Oman", Elip.Clarke1880, -346d, -1d, +224d, 74), new Datum("Ord Srvy Grt Britn", Elip.Airy1830, +375d, -111d, +431d,75), new Datum("Pulkovo1942", Elip.Krassovsky1940, +28d, -130d, -95d,-1), new Datum("Prov So Amrican `56", Elip.International, -288d, +175d, -376d,78), new Datum("South American '69", Elip.SouthAmerican1969, -57d, +1d, -41d,-1), new Datum("Tokyo", Elip.Bessel1841, -128, +481, +664,95), new Datum("WGS72", Elip.WGS72, 0d, 0d, +5d,99), WGS84, null }; Datum theDatum = WGS84; double nmfac = 180d*60d/Math.PI; double meterfac = 1852*180d*60d/Math.PI; Pone x= new Pone(), y=new Pone(); double h=0; Pos altPos=null; // altPos is a small cache. Problem is not performance, but accuracy: // aPos.toDatum(d1).toDatum(d2).toDatum(d1) != aPos.toDatum(d1) Pos (double xddd, double yddd, Datum dt) { x.p=xddd % 360d; if (x.p>180d) { x.p = x.p -360d; } y.p=yddd % 90d; theDatum = dt; } Pos (Pone px, Pone py, Datum dt) { x.p = px.p; y.p = py.p; theDatum = dt; } Pos (int xg,double xm,int yg,double ym) { x.p = ((double) xg + xm/60d)%360d; if (x.p>180d) { x.p = x.p -360d; } y.p = ((double) yg + ym/60d)%90d; } Pos(){} public Pos eCopy() { return(new Pos(x.p,y.p,theDatum)); } static public Datum getGtDatum(int dt){ int di=0; while (Pos.datumList[di]!=null) { if (datumList[di].gpstransNo == dt) { return(datumList[di]); } di++; } return(defaultDatum); } static public Datum getNamedDatum(String name){ int di=0; while (Pos.datumList[di] != null) { if (datumList[di].name.equals(name)) { return(datumList[di]); } di++; } return(defaultDatum); } boolean eq(Pos p1){ //return(p1.x.p == x.p && p1.y.p == y.p); return(dist(p1) < 0.5d); // < 1 meter. } double icoursedg(Pos p2) { return(180d/Math.PI*icourse(p2)); } // IF sin(lon2-lon1)<0 // tc1=acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1))) // ELSE // tc1=2*pi-acos((sin(lat2)-sin(lat1)*cos(d))/(sin(d)*cos(lat1))) // ENDIF double icourse(Pos p2) { double dst = radDist(p2); double ac = (Math.sin(p2.y.r())-Math.sin(y.r())*Math.cos(dst))/(Math.sin(dst)*Math.cos(y.r())); double c0; c0 = Math.acos(ac); //CAux.perr("ic d=" + dst + " ac="+ac+ "c0="+c0, 3); // Rounding problems if (ac < -1d) { ac=-1d; } if (ac > 1d) { ac=1d; } if (Math.sin(x.r()-p2.x.r()) < 0d) { return(c0); } else { return(2d*Math.PI - c0); } } double dist(Pos p2) { return(meterfac*radDist(p2)); } double radDist(Pos pOther) { Pos p1 = pOther.toDatum(theDatum); return (2d*Math.asin(Math.sqrt(Math.pow(Math.sin(y.r()-p1.y.r())/2d,2) + Math.cos(y.r())*Math.cos(p1.y.r())* Math.pow(Math.sin((-x.r()+p1.x.r())/2d),2))) ); } // Ed Williams // d = 2*asin(sqrt((sin((lat1-lat2)/2))^2 + cos(lat1)*cos(lat2)*(sin((lon1-lon2)/2))^2)) Pos plus(Pos p1){ Pos p1d = p1.toDatum(theDatum); return (new Pos(x.p + p1d.x.p, y.p+p1d.y.p, theDatum)); } void sane() { x.p = x.p %360d; double fx,fy; if (x.p > 180d){ x.p = - (360d - x.p); } //fx = x.p + 1d/120; //fy = y.p + 1d/120; //x.p = fx - (fx % (1d/60d)); //y.p = fy - (fy % (1d/60d)); } public Pos toDatum(Datum dat) { //Standard Molodensky Pos rPos; double from_phi, from_L, from_h; double to_phi, to_L, to_h; double from_a, from_f; double to_a, to_f; double Rn, Rm,dPhi, dL, dh, sPhi, cPhi, sL, cL,from_es; double bda; double dx,dy,dz; double da,df; if (dat==theDatum){ return(this); // NOTE: reference sematic here } if (altPos != null && altPos.theDatum ==dat){ return(altPos); // NOTE: reference sematic here } //System.out.println(" old pos= "+ this.toString(Pone.MDD)); from_phi=y.r(); from_L=x.r(); from_h=0d; to_h=0d; from_a=theDatum.e.a; from_f=1/theDatum.e.rf; to_a=dat.e.a; to_f=1/dat.e.rf; bda = 1d - from_f; dx = theDatum.dx - dat.dx; dy = theDatum.dx - dat.dy; dz = theDatum.dx - dat.dz; da = to_a - from_a; df = to_f - from_f; //System.out.println(" from_L = "+ from_L+ " from_phi = "+ from_phi); //System.out.println(" dx = "+ dx + " dy = "+ dy +" dz = "+ dz +" da = "+ da + " df = "+ df); //System.out.println(" to_a = "+ to_a + " to_f = "+ to_f +" from_a = "+ from_a +" from_f = "+ from_f); sPhi = Math.sin(from_phi); cPhi = Math.cos(from_phi); sL = Math.sin(from_L); cL = Math.cos(from_L); from_es = 2d*from_f - from_f*from_f; Rn = from_a/Math.sqrt(1d - from_es * sPhi*sPhi); Rm = (from_a * (1 -from_es))/Math.pow(1d-from_es*Math.pow(sPhi,2),1.5d); dPhi = ( ( ((-dx*sPhi*cL - dy*sPhi*sL) + dz*cPhi) + da*(Rn*from_es*sPhi*cPhi)/from_a) + df*(Rm/bda + Rn*bda)*sPhi*cPhi)/(Rm+from_h); dL = (-dx*sL + dy*cL)/((Rn+from_h)*cPhi); //System.out.println(" dPhi = "+ dPhi + " dL = "+ dL+" from_es="+from_es); dh = dx*cPhi*cL + dy*cPhi*sL + dz*sPhi - da *(from_a/Rn) + df*bda*Rn*sPhi*sPhi; // System.out.println(" dH= = "+ dh); to_phi = from_phi + dPhi; to_L = from_L + dL; to_h = from_h+dh; rPos = new Pos(to_L*180d/Math.PI, to_phi*180d/Math.PI, dat); rPos.altPos = this; altPos = rPos; //rPos.sane(); return(rPos); } public String toString() { return(toString(Pone.MALL)); } public String toString(int pf) { if (pf==Pone.pfGPSTRANS) { return ( (y.toString(pf)) + "\t" + x.toString(pf)); } else if (pf==Pone.pfGARLINK) { return ( (y.toString(pf))+((y.p>0d)?"N":"S") + " " + x.toString(pf)+((x.p>0d)?"E":"W")); } else{ return (y.toString(pf)+ ((y.p>0d)?"N":"S") + ((pf==Pone.MD)?"":" ") + x.toString(pf) +((x.p>0d)?"E":"W")); } } }