Adding coordtrans.py.

src/pyutil/coordtrans.py

+from pyproj import Proj
+
+class CoordTrans():
+    def __init__(self, lat=57.7605573519, lon=16.6827607783, alt=29.8, location=None):
+        self.locations = {}
+        self.locations["granso"] = (57.7605573519, 16.6827607783, 29.8, 27.9)
+        self.locations["motala"] = (58.4948444444, 15.1022444444, 132.9, 29.95)
+        self.locations["petterstedt"] = (58.323751, 15.406307, 128.0, 29.8)
+        self.locations["terra"] = (58.3954055556, 15.5723222222, 104.4, 29.4)
+
+        if location and location in self.locations:
+            print("INITIALIZING FROM LOCATION:", location)
+            self.lat0 = self.locations[location][0]
+            self.lon0 = self.locations[location][1]
+            self.alt0 = self.locations[location][2]
+        else:
+            self.lat0 = lat
+            self.lon0 = lon
+            self.alt0 = alt
+
+        print("ORIGIN:", lon, lat, alt)
+        self.zone = self.utm_zone(self.lon0, self.lat0)
+        print("UTM ZONE:", self.zone)
+        self.proj = Proj(proj='utm', zone=self.zone, ellps='WGS84', preserve_units=False)        
+        (x, y) = self.proj(self.lon0, self.lat0)
+        print("UTM ORIGIN:", x, y)
+        self.utm_x = x
+        self.utm_y = y
+
+    def utm_zone(self, lon, lat):
+        res = (int)((lon + 180.0) / 6.0) + 1;
+        if lat >= 56.0 and lat < 64.0 and lon >= 3.0 and lon < 12.0:
+            res = 32;
+        # Special zones for Svalbard
+        if lat >= 72.0 and lat < 84.0:
+            if lon >= 0.0  and lon <  9.0:
+                res = 31;
+            elif lon >= 9.0  and lon < 21.0:
+                res = 33;
+            elif lon >= 21.0 and lon < 33.0:
+                res = 35;
+            elif lon >= 33.0 and lon < 42.0:
+                res = 37;
+        return res
+
+    def world_to_wgs84(self, x, y, z=0.0):
+        try:
+            ux = self.utm_x + x
+            uy = self.utm_y + y
+            (lon, lat) = self.proj(ux, uy, inverse=True)
+            alt = self.alt0 + z
+            return (lon, lat, alt)
+        except Exception as exc:
+            print("EXCEPTION world_to_wgs84", type(exc))
+            print(exc)
+
+    def wgs84_to_world(self, lon, lat, alt=None):
+        try:
+            if alt == None:
+                alt = self.alt0
+            #print("wgs84_to_world:", lon, lat, alt)
+            (x, y) = self.proj(lon, lat)
+            return (x - self.utm_x, y - self.utm_y, alt - self.alt0)
+        except Exception as exc:
+            print("EXCEPTION wgs84_to_world", type(exc))
+            print(exc)
+    
+
+if __name__ == '__main__':
+
+    ct = CoordTrans()
+
+    (lon, lat, alt) = ct.world_to_wgs84(10.0, 20.0, 30.0)
+    print(lon, lat, alt)
+
+    world = ct.wgs84_to_world(lon, lat)
+    print(world)
+
+    world = ct.wgs84_to_world(lon, lat, alt)
+    print(world)
+              
+    cmotala = CoordTrans(location="motala")
+    (lon, lat, alt) = cmotala.world_to_wgs84(10.0, 20.0, 30.0)
+    print(lon, lat, alt)
+    world = cmotala.wgs84_to_world(lon, lat, alt)
+    print(world)
+