.10.dp.source-code.geo.scala Maven / Gradle / Ivy
Go to download
Show more of this group Show more artifacts with this name
Show all versions of core_2.10 Show documentation
Show all versions of core_2.10 Show documentation
Core drx utilities that have no other dependencies other than the core scala lib
The newest version!
/*
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 = loc.latR.sin
val cosLat = loc.latR.cos
val N = a/(1 - e*e * sinLat*sinLat).sqrt
val X = (N + loc.alt) * cosLat * loc.lonR.cos
val Y = (N + loc.alt) * cosLat * loc.lonR.sin
val Z = (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 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))
}
@deprecated("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)
}
}