/* Copyright 2010 Aaron J. Radke Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. */ package cc.drx object Ned{ def apply(n:Length, e:Length, d:Length=Length(0)):Ned = new Ned(Vec(n.m, e.m, d.m)) //meters def apply(range:Length, bearing:Angle):Ned = new Ned(Vec.polar(range.m, bearing)) //meters def apply(ned:Vec):Ned = new Ned(ned) //meters def zero:Ned = Ned(Vec.zero) //meters //def apply(n:Double, e:Double, d:Double=0):Ned = new Ned(n, e, d) //meters } class Ned(val vec:Vec) extends AnyVal{ //meters def n = Length(vec.x) def e = Length(vec.y) def d = Length(vec.z) /**convenience function for -d to support ENU from NED*/ def u = Length(-vec.z) def dist:Length = Length(vec.norm) def bearing:Angle = vec.heading def unary_- :Ned = Ned(-vec) def +(that:Ned):Ned = Ned(this.vec + that.vec) def -(that:Ned):Ned = Ned(this.vec - that.vec) override def toString = s"Ned($n,$e,$d)" } object Ecef{ /**semi-major (equatorial) axis*/ val a:Length = 6378137.m /**semi-minor (polar) axis*/ val b:Length = 6356752.3142.m /**flattening*/ val f:Double = 1 - b.m/a.m /**eccentricity*/ val e:Double = 2*f - f*f /**average radius of the earth*/ val r:Length = 6371E3.m def apply(loc:Loc):Ecef = { val sinLat:Double = loc.latR.sin val cosLat:Double = loc.latR.cos val N:Length = a * math.sqrt(1d - e*e * sinLat*sinLat).inv // val N:Length = a / math.sqrt(1d - e*e * sinLat*sinLat) //TODO failure in dotty-0.10.0 is it fixed yet? val X:Length = (N + loc.alt) * cosLat * loc.lonR.cos val Y:Length = (N + loc.alt) * cosLat * loc.lonR.sin val Z:Length = (N*(1 - e*e) + loc.alt) * sinLat Ecef(X,Y,Z) //meters } def apply(X:Length, Y:Length, Z:Length):Ecef = new Ecef(Vec(X.m, Y.m, Z.m)) //meters def apply(vec:Vec):Ecef = new Ecef(vec) //meters } class Ecef(val vec:Vec) extends AnyVal{ //meters def X = Length(vec.x) def Y = Length(vec.y) def Z = Length(vec.z) //def -(that:Ecef):Ecef = Ecef(this.vec - that.vec) //def +(that:Ecef):Ecef = Ecef(this.vec + that.vec) override def toString = s"Ecef($X,$Y,$Z)" def loc:Loc = Loc(this) /** spherical interpolation (great circle) wikipedia://Slerp*/ def slerp(that:Ecef)(t:Double):Ecef = Ecef((this.vec slerp that.vec)(t)) /** linear interpolation*/ def lerp(that:Ecef)(t:Double):Ecef = Ecef((this.vec lerp that.vec)(t)) /**nudge by ned*/ def -(ned:Ned):Ecef = this + (-ned) def +(ned:Ned):Ecef = { //init enu coordinates val n = ned.vec.x; val e = ned.vec.y; val d = ned.vec.z // val Vec(n,e,d) = ned.vec val u = -d //to support enu from ned val loc = this.loc val latR = loc.latR val lonR = loc.lonR //calc val X = -lonR.sin*e - latR.sin*lonR.cos*n + latR.cos*lonR.cos*u; val Y = lonR.cos*e - latR.sin*lonR.sin*n + latR.cos*lonR.sin*u; val Z = latR.cos *n + latR.sin *u; Ecef(vec + Vec(X,Y,Z)) } } object Loc{ //val NYC = ??? //val LON = ??? def apply(lat:Double, lon:Double, alt:Length=Length(0)):Loc = new Loc(lat,lon, alt) //alt in Meters /** wikipedia://Geographic_coordinate_conversion using Bowrings irrational geodedic-lattitude equation with Newton-Raphson*/ def apply(ecef:Ecef):Loc = { //constants val e = Ecef.e val a = Ecef.a.m val k0 = 1/(1 - e*e) //init val Vec(x,y,z) = ecef.vec val p = math.sqrt(x*x + y*y) def kNext(k:Double) = { val c = ((p*p + (1-e*e)*z*z*k*k)**1.5)/(a*e*e) (c + (1-e*e)*(z*z)*(k*k*k))/(c - p*p) } val k = kNext(kNext(kNext(k0))) //TODO make automatic number of iterations instead of fixed 3 //val k = p/z*lat.tan val lat = (k*z/p).atan val lon = math.atan2(y,x) val alt = 1/(e*e)*(1/k - 1/k0)*math.sqrt(p*p + z*z*k*k) Loc(lat*rad2deg,lon*rad2deg,Length(alt)) } } class Loc(val lat:Double, val lon:Double, val alt:Length){ //alt in Meters lazy val latR = lat*deg2rad lazy val lonR = lon*deg2rad lazy val ecef = Ecef(this) lazy val utm = Utm(this) def slerp(that:Loc)(t:Double):Loc = (this.ecef slerp that.ecef)(t).loc def lerp(that:Loc)(t:Double):Loc = (this.ecef lerp that.ecef)(t).loc def +(ned:Ned):Loc = (ecef + ned).loc def -(ned:Ned):Loc = (ecef - ned).loc /** great circle bearing * http://www.movable-type.co.uk/scripts/latlong.html */ def greatBearingTo(that:Loc):Angle = { val dLon = that.lonR - this.lonR val y = dLon.sin * that.latR.cos val x = this.latR.cos*that.latR.sin - this.latR.sin*that.latR.cos*dLon.cos Angle(y atan2 x) } /** great circle dist * http://www.movable-type.co.uk/scripts/latlong.html */ def greatDist(that:Loc):Length = { val dLat = that.latR - this.latR val dLon = that.lonR - this.lonR val sinHalfdLat = (dLat/2).sin val sinHalfdLon = (dLon/2).sin val a = sinHalfdLat*sinHalfdLat + this.latR.cos*that.latR.cos*sinHalfdLon*sinHalfdLon val c = 2*(a.sqrt atan2 (1-a).sqrt) Ecef.r*c //radius of the earth } /**accurate but slower version of `~>` (using test:runMain LocSpeedTest shows about 4x slower) This uses an intermediate ECEF coordinate transformation */ def nedTo(that:Loc):Ned = { val delta = that.ecef.vec - this.ecef.vec val e = - lonR.sin*delta.x + lonR.cos*delta.y val n = -latR.sin*lonR.cos*delta.x - latR.sin*lonR.sin*delta.y + latR.cos*delta.z val u = latR.cos*lonR.cos*delta.x + latR.cos*lonR.sin*delta.y + latR.sin*delta.z val d = -u Ned(Vec(n,e,d)) } 2017-07-30("use nedTo(Loc) instead to reserve the drx ~> semantic meanings for something else","v0.1.21c") def ~>(that:Loc):Ned = nedToApproximate(that) /**approximate but faster version of `~>` (using test:runMain LocSpeedTest shows about 4x faster) * This function uses the average radius of the earth and assumes small angles */ def nedToApproximate(that:Loc):Ned = { val R = Ecef.r + (this.alt + that.alt)/2 //radius of the earth + avg altitude val n = R*(that.latR - this.latR) val e = R*(that.lonR - this.lonR)*math.cos((this.latR + that.latR)/2) val d = that.alt - this.alt Ned(n, e, d) } override def toString = s"Loc(${lat}deg, ${lon}deg, ${alt.m}m)" } /** https://en.wikipedia.org/wiki/Universal_Transverse_Mercator_coordinate_system */ object Utm{ sealed trait Hemisphere{ val N0:Double; val symbol:String } case object North extends Hemisphere{ val N0 = 0.0 ; val symbol="N"} case object South extends Hemisphere{ val N0 = 10000E3; val symbol="S" } import math._ val a = 6378137.0 val f_inv = 298.257223563 //f = 1.0/298.257223563 private val n = 1.0/(2.0*f_inv-1.0) private val n2 = n*n private val n3 = n*n*n private val A = a/(1.0+n)*(1.0 + n2/4 + n3*n/64 /*+ ...*/) private val alpha = Array(0, n/2 - n2*2/3 + n3*5/16, n2*13/48 + n3*3/5, n3*61/240) private val beta = Array(0, n/2 - n2*2/3 + n3*37/96, n2/48 + n3/15, n3*17/480) private val delta = Array(0, n*2 - n2*2/3 - n3*2, n2*7/3 - n3*8/5, n3*56/15) private val k0 = 0.9996 private val k0A = k0*A private val E0 = 500E3 private val nrootJib = n.sqrt*2/(n+1) private def trigSum(coef:Int => Double, termA:Double=>Double, paramA:Double, termB:Double=>Double, paramB:Double):Double = (1 to 3).foldLeft(0.0){case (a,j) => a + coef(j)*termA(2*j*paramA)*termB(2*j*paramB)} def toLoc(utm:Utm):Loc = {//wikipedia implementation val zeta = (utm.n.m - utm.hemisphere.N0)/k0A val mu = (utm.e.m - E0)/k0A val zetaP = zeta - trigSum(j => beta(j), sin, zeta, cosh, mu) val muP = mu - trigSum(j => beta(j), cos, zeta, sinh, mu) val chi = (zetaP.sin / muP.cosh).asin val lat = chi + trigSum(j => delta(j), sin, chi, _=>1, 1) //radians val lon = lon0(utm.zone) + ( muP.sinh / zetaP.cos ).atand //degrees Loc(lat*rad2deg,lon,utm.alt) } def zone(loc:Loc):Int = ((loc.lon + 180)/6).toInt % 60 + 1 //TODO need to add special zone mods for Svalbard def lon0(zone:Int):Double = 6.0*zone - 183.0 //degrees private val num = """([\+\-\d\.]+)\s*([A-Z]*)\s*""" //broad but brittle //a little more robust using drx.Parse[Double] private lazy val UtmPat = raw".*?(?i)$num$num$num($num)?.*".r //three fields required 4th for altitutde private def make(d:Double,u:String):Length = { val utrim = u.trim (for(z <- utrim.lastOption if z == 'n' || z == 'N' || z == 'e' || z == 'E') yield Length(d,utrim.dropRight(1)) ) getOrElse Length(d) } private def make(z:Int,zu:String, ad:Double,au:String, bd:Double,bu:String, alt:Length):Utm = { val isNorth = (zu == "" && z >= 0) || zu.toUpperCase.startsWith("N") //answers north as well val hemi = isNorth.getOrElse(North,South) if(au == "" || (au endsWith "e") || (au endsWith "E")) Utm(z.abs,hemi, make(ad,au), make(bd,bu), alt) else Utm(z.abs,hemi, make(bd,bu), make(ad,au), alt) } def apply(loc:String):Utm = loc match { case UtmPat(ad,au, bd,bu, cd,cu, _, altd,altu) => val a = Parse[Double](ad) val b = Parse[Double](bd) val c = Parse[Double](cd) val alt = Option(altd).map{s => make(Parse[Double](s),altu)}.getOrElse(0.m) //regex empty match is null if( a.abs <= 60) make(a.toInt,au, b,bu, c,cu, alt) //first slot is the zone else if(c.abs <= 60) make(c.toInt,cu, a,au, b,bu, alt) //second slot is the zone else make(b.toInt,bu, a,au, c,cu, alt) //third slot is the zone case _ => Utm(0,North,0.m,0.m) //TODO add throw of parse exception } def apply(loc:Loc):Utm = {//wikipedia simple implementation of lat,lon -> Utm coord val hemisphere = if(loc.lat >= 0) Utm.North else Utm.South val latsin = loc.lat.sind //assuming degrees val t = ( latsin.atanh - nrootJib*(nrootJib*latsin).atanh ).sinh val zn:Int = zone(loc) val lonDelta = loc.lon - lon0(zn) val zetaP = (t/lonDelta.cosd).atan val muP = (lonDelta.sind/(1+t*t).sqrt).atanh val sigma = 1.0 + trigSum(j => alpha(j)*2*j, cos, zetaP, cosh, muP) val tau = trigSum(j => alpha(j)*2*j, sin, zetaP, sinh, muP) val E:Double = E0 + k0A*(muP + trigSum(j => alpha(j), cos, zetaP, sinh, muP)) val N:Double = hemisphere.N0 + k0A*(zetaP + trigSum(j => alpha(j), sin, zetaP, cosh, muP)) Utm(zn,hemisphere,Length(E),Length(N),loc.alt) } implicit object ParsableUtm extends Parsable[Utm]{ def apply(v:String):Utm = Utm(v) } } case class Utm(val zone:Int, val hemisphere:Utm.Hemisphere, val e:Length, val n:Length, alt:Length=Length(0)){ //no altitude assert(1 <= zone && zone <= 61) //TODO remove for compiled release override def toString = s"Utm($format, alt=${alt.m.toInt})" private def altToString = if(alt == 0.m) "" else " " + alt.m.round.toInt def format = s"$zone${hemisphere.symbol} ${e.m.round.toInt} ${n.m.round.toInt}" + altToString lazy val loc = Utm.toLoc(this) def +(ned:Ned):Utm = Utm(zone, hemisphere, e+ned.e, n+ned.n, alt-ned.d) def -(ned:Ned):Utm = Utm(zone, hemisphere, e-ned.e, n-ned.n, alt+ned.d) //swap the north and east components (useful to see if someone switch the order of the units) def swap:Utm = Utm(zone, hemisphere, n,e, alt) def nedTo(that:Utm):Ned = { assert(this.zone == that.zone, s"Utm zones do not match in $this and $that") Ned(that.n-this.n, that.e-this.e, that.alt-this.alt) } }