add android and desktop modules
This commit is contained in:
@@ -0,0 +1,24 @@
|
||||
package com.icegps.common
|
||||
|
||||
import androidx.test.platform.app.InstrumentationRegistry
|
||||
import androidx.test.ext.junit.runners.AndroidJUnit4
|
||||
|
||||
import org.junit.Test
|
||||
import org.junit.runner.RunWith
|
||||
|
||||
import org.junit.Assert.*
|
||||
|
||||
/**
|
||||
* Instrumented test, which will execute on an Android device.
|
||||
*
|
||||
* See [testing documentation](http://d.android.com/tools/testing).
|
||||
*/
|
||||
@RunWith(AndroidJUnit4::class)
|
||||
class ExampleInstrumentedTest {
|
||||
@Test
|
||||
fun useAppContext() {
|
||||
// Context of the app under test.
|
||||
val appContext = InstrumentationRegistry.getInstrumentation().targetContext
|
||||
assertEquals("com.icegps.common.test", appContext.packageName)
|
||||
}
|
||||
}
|
||||
4
icegps-common/src/main/AndroidManifest.xml
Normal file
4
icegps-common/src/main/AndroidManifest.xml
Normal file
@@ -0,0 +1,4 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<manifest xmlns:android="http://schemas.android.com/apk/res/android">
|
||||
|
||||
</manifest>
|
||||
274
icegps-common/src/main/java/com/icegps/common/helper/BlhToEnu.kt
Normal file
274
icegps-common/src/main/java/com/icegps/common/helper/BlhToEnu.kt
Normal file
@@ -0,0 +1,274 @@
|
||||
package com.icegps.common.helper
|
||||
|
||||
import kotlin.math.atan2
|
||||
import kotlin.math.cos
|
||||
import kotlin.math.pow
|
||||
import kotlin.math.sin
|
||||
import kotlin.math.sqrt
|
||||
|
||||
/**
|
||||
* BLH -> ENU
|
||||
*
|
||||
* @author lm
|
||||
* @date 2025/3/12
|
||||
* @link https://gist.github.com/komasaru/6ce0634475923ddac597f868288c54e9
|
||||
*/
|
||||
class BlhToEnu {
|
||||
|
||||
companion object {
|
||||
private const val PI_180 = Math.PI / 180.0
|
||||
|
||||
// WGS84 坐标参数
|
||||
private const val A = 6378137.0 // a（地球椭球体长半径（赤道面平均半径））
|
||||
private const val ONE_F = 298.257223563 // 1 / f（地球椭球体扁平率 = (a - b) / a）
|
||||
private val B = A * (1.0 - 1.0 / ONE_F) // b（地球椭球体短半径）
|
||||
private val E2 = (1.0 / ONE_F) * (2 - (1.0 / ONE_F))
|
||||
|
||||
// e^2 = 2 * f - f * f = (a^2 - b^2) / a^2
|
||||
private val ED2 = E2 * A * A / (B * B) // e'^2 = (a^2 - b^2) / b^2
|
||||
}
|
||||
|
||||
private var originLat: Double = 0.0
|
||||
private var originLon: Double = 0.0
|
||||
private var originHeight: Double = 0.0
|
||||
private var isOriginSet: Boolean = false
|
||||
|
||||
fun getOriginLat(): Double = originLat
|
||||
|
||||
fun getOriginLon(): Double = originLon
|
||||
|
||||
fun getOriginHeight(): Double = originHeight
|
||||
|
||||
fun resetEnuBenchmarkPoint() {
|
||||
isOriginSet = false
|
||||
}
|
||||
|
||||
fun wgs84ToEnu(lat: Double, lon: Double, height: Double = 0.0): DoubleArray {
|
||||
if (!isOriginSet) {
|
||||
originLat = lat
|
||||
originLon = lon
|
||||
originHeight = height
|
||||
isOriginSet = true
|
||||
return doubleArrayOf(0.0, 0.0, 0.0)
|
||||
}
|
||||
|
||||
val enu = blh2enu(originLat, originLon, originHeight, lat, lon, height)
|
||||
// var az = atan2(enu[0], enu[1]) * 180.0 / Math.PI
|
||||
// if (az < 0.0) {
|
||||
// az += 360.0
|
||||
// }
|
||||
// val el = atan2(
|
||||
// enu[2],
|
||||
// sqrt(enu[0] * enu[0] + enu[1] * enu[1])
|
||||
// ) * 180.0 / Math.PI
|
||||
// val dst = sqrt(enu.sumOf { it * it })
|
||||
|
||||
// println("--->")
|
||||
// println(
|
||||
// """
|
||||
// ENU: E = ${enu[0].format(3)}m
|
||||
// N = ${enu[1].format(3)}m
|
||||
// U = ${enu[2].format(3)}m
|
||||
// 方位角 = ${az.format(3)}°
|
||||
// 仰角 = ${el.format(3)}°
|
||||
// 距离 = ${dst.format(3)}m
|
||||
// """.trimIndent()
|
||||
// )
|
||||
|
||||
return enu
|
||||
}
|
||||
|
||||
fun enuToWgs84(e: Double, n: Double, u: Double): DoubleArray {
|
||||
if (!isOriginSet) {
|
||||
return doubleArrayOf(0.0, 0.0, 0.0)
|
||||
}
|
||||
|
||||
val blh = enu2blh(originLat, originLon, originHeight, e, n, u)
|
||||
|
||||
// println("--->")
|
||||
// println(
|
||||
// """
|
||||
// BLH: Beta = ${blh[0].format(8)}°
|
||||
// Lambda = ${blh[1].format(8)}°
|
||||
// Height = ${blh[2].format(3)}m
|
||||
// """.trimIndent()
|
||||
// )
|
||||
|
||||
return blh
|
||||
}
|
||||
|
||||
private fun Double.format(digits: Int) = "%.${digits}f".format(this)
|
||||
|
||||
/**
|
||||
* BLH -> ENU 转换(East, North, Up)
|
||||
*
|
||||
* @param bO 原点 Beta(纬度)
|
||||
* @param lO 原点 Lambda(经度)
|
||||
* @param hO 原点 Height(高度)
|
||||
* @param b 目标点 Beta(纬度)
|
||||
* @param l 目标点 Lambda(经度)
|
||||
* @param h 目标点 Height(高度)
|
||||
* @return ENU 坐标 [e, n, u]
|
||||
*/
|
||||
private fun blh2enu(bO: Double, lO: Double, hO: Double, b: Double, l: Double, h: Double): DoubleArray {
|
||||
val (xO, yO, zO) = blh2ecef(bO, lO, hO)
|
||||
val (x, y, z) = blh2ecef(b, l, h)
|
||||
val mat0 = matZ(90.0)
|
||||
val mat1 = matY(90.0 - bO)
|
||||
val mat2 = matZ(lO)
|
||||
val mat = mulMat(mulMat(mat0, mat1), mat2)
|
||||
return rotate(mat, doubleArrayOf(x - xO, y - yO, z - zO))
|
||||
}
|
||||
|
||||
/**
|
||||
* BLH -> ECEF 转换
|
||||
*
|
||||
* @param lat 纬度
|
||||
* @param lon 经度
|
||||
* @param height 高度
|
||||
* @return ECEF 坐标 [x, y, z]
|
||||
*/
|
||||
private fun blh2ecef(lat: Double, lon: Double, height: Double): DoubleArray {
|
||||
val n = { x: Double -> A / sqrt(1.0 - E2 * sin(x * PI_180).pow(2)) }
|
||||
val x = (n(lat) + height) * cos(lat * PI_180) * cos(lon * PI_180)
|
||||
val y = (n(lat) + height) * cos(lat * PI_180) * sin(lon * PI_180)
|
||||
val z = (n(lat) * (1.0 - E2) + height) * sin(lat * PI_180)
|
||||
return doubleArrayOf(x, y, z)
|
||||
}
|
||||
|
||||
/**
|
||||
* ENU -> BLH 转换
|
||||
*
|
||||
* @param e East 坐标
|
||||
* @param n North 坐标
|
||||
* @param u Up 坐标
|
||||
* @return WGS84 坐标 [纬度, 经度, 高度]
|
||||
*/
|
||||
private fun enu2blh(bO: Double, lO: Double, hO: Double, e: Double, n: Double, u: Double): DoubleArray {
|
||||
val mat0 = matZ(-lO)
|
||||
val mat1 = matY(-(90.0 - bO))
|
||||
val mat2 = matZ(-90.0)
|
||||
val mat = mulMat(mulMat(mat0, mat1), mat2)
|
||||
|
||||
val enu = doubleArrayOf(e, n, u)
|
||||
val xyz = rotate(mat, enu)
|
||||
|
||||
val (xO, yO, zO) = blh2ecef(bO, lO, hO)
|
||||
val x = xyz[0] + xO
|
||||
val y = xyz[1] + yO
|
||||
val z = xyz[2] + zO
|
||||
|
||||
return ecef2blh(x, y, z)
|
||||
}
|
||||
|
||||
/**
|
||||
* ECEF -> BLH 转换
|
||||
*
|
||||
* @param x ECEF X 坐标
|
||||
* @param y ECEF Y 坐标
|
||||
* @param z ECEF Z 坐标
|
||||
* @return WGS84 坐标 [纬度, 经度, 高度]
|
||||
*/
|
||||
private fun ecef2blh(x: Double, y: Double, z: Double): DoubleArray {
|
||||
val p = sqrt(x * x + y * y)
|
||||
val theta = atan2(z * A, p * B)
|
||||
val sinTheta = sin(theta)
|
||||
val cosTheta = cos(theta)
|
||||
|
||||
val lat = atan2(
|
||||
z + ED2 * B * sinTheta.pow(3),
|
||||
p - E2 * A * cosTheta.pow(3)
|
||||
)
|
||||
val lon = atan2(y, x)
|
||||
|
||||
val sinLat = sin(lat)
|
||||
val n = A / sqrt(1.0 - E2 * sinLat * sinLat)
|
||||
val h = p / cos(lat) - n
|
||||
|
||||
return doubleArrayOf(
|
||||
lat * 180.0 / Math.PI,
|
||||
lon * 180.0 / Math.PI,
|
||||
h
|
||||
)
|
||||
}
|
||||
|
||||
/**
|
||||
* 以 x 轴为轴的旋转矩阵
|
||||
*
|
||||
* @param ang 旋转角度（°）
|
||||
* @return 旋转矩阵（3x3）
|
||||
*/
|
||||
private fun matX(ang: Double): Array<DoubleArray> {
|
||||
val a = ang * PI_180
|
||||
val c = cos(a)
|
||||
val s = sin(a)
|
||||
return arrayOf(
|
||||
doubleArrayOf(1.0, 0.0, 0.0),
|
||||
doubleArrayOf(0.0, c, s),
|
||||
doubleArrayOf(0.0, -s, c)
|
||||
)
|
||||
}
|
||||
|
||||
/**
|
||||
* 以 y 轴为轴的旋转矩阵
|
||||
*
|
||||
* @param ang 旋转角度（°）
|
||||
* @return 旋转矩阵（3x3）
|
||||
*/
|
||||
private fun matY(ang: Double): Array<DoubleArray> {
|
||||
val a = ang * PI_180
|
||||
val c = cos(a)
|
||||
val s = sin(a)
|
||||
return arrayOf(
|
||||
doubleArrayOf(c, 0.0, -s),
|
||||
doubleArrayOf(0.0, 1.0, 0.0),
|
||||
doubleArrayOf(s, 0.0, c)
|
||||
)
|
||||
}
|
||||
|
||||
/**
|
||||
* 以 z 轴为轴的旋转矩阵
|
||||
*
|
||||
* @param ang 旋转角度（°）
|
||||
* @return 旋转矩阵（3x3）
|
||||
*/
|
||||
private fun matZ(ang: Double): Array<DoubleArray> {
|
||||
val a = ang * PI_180
|
||||
val c = cos(a)
|
||||
val s = sin(a)
|
||||
return arrayOf(
|
||||
doubleArrayOf(c, s, 0.0),
|
||||
doubleArrayOf(-s, c, 0.0),
|
||||
doubleArrayOf(0.0, 0.0, 1.0)
|
||||
)
|
||||
}
|
||||
|
||||
/**
|
||||
* 两个矩阵(3x3)的乘积
|
||||
*
|
||||
* @param matA 3x3 矩阵
|
||||
* @param matB 3x3 矩阵
|
||||
* @return 3x3 矩阵
|
||||
*/
|
||||
private fun mulMat(matA: Array<DoubleArray>, matB: Array<DoubleArray>): Array<DoubleArray> {
|
||||
return Array(3) { k ->
|
||||
DoubleArray(3) { j ->
|
||||
(0..2).sumOf { i -> matA[k][i] * matB[i][j] }
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 点的旋转
|
||||
*
|
||||
* @param mat 3x3 旋转矩阵
|
||||
* @param pt 旋转前坐标 [x, y, z]
|
||||
* @return 旋转后坐标 [x, y, z]
|
||||
*/
|
||||
private fun rotate(mat: Array<DoubleArray>, pt: DoubleArray): DoubleArray {
|
||||
return DoubleArray(3) { j ->
|
||||
(0..2).sumOf { i -> mat[j][i] * pt[i] }
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,419 @@
|
||||
package com.icegps.common.helper
|
||||
|
||||
import android.os.Parcel
|
||||
import android.os.Parcelable
|
||||
import kotlin.math.atan
|
||||
import kotlin.math.atan2
|
||||
import kotlin.math.cbrt
|
||||
import kotlin.math.cos
|
||||
import kotlin.math.exp
|
||||
import kotlin.math.ln
|
||||
import kotlin.math.sin
|
||||
import kotlin.math.sqrt
|
||||
import kotlin.math.tan
|
||||
|
||||
/**
|
||||
* WGS84、EPSG3857、ENU 的坐标转换工具类
|
||||
*
|
||||
* @author lm
|
||||
* @date 2024/8/2
|
||||
*/
|
||||
class GeoHelper private constructor() {
|
||||
|
||||
companion object {
|
||||
private var sharedInstance: GeoHelper? = null
|
||||
fun getSharedInstance(): GeoHelper = sharedInstance ?: GeoHelper().also { sharedInstance = it }
|
||||
fun createInstance(): GeoHelper = GeoHelper()
|
||||
}
|
||||
|
||||
// WGS-84 ellipsoid parameters
|
||||
private val RADIUS = 6378137.0 // Major radius
|
||||
private val RADIUS_B = 6356752.314245 // Minor radius
|
||||
private val E = (RADIUS * RADIUS - RADIUS_B * RADIUS_B) / (RADIUS * RADIUS) // Eccentricity
|
||||
private val HALF_SIZE = Math.PI * RADIUS // Half circumference of Earth
|
||||
private val DEG2RAD = Math.PI / 180 // Degrees to radians conversion factor
|
||||
private val RAD2DEG = 180 / Math.PI // Radians to degrees conversion factor
|
||||
private val RE_WGS84 = 6378137.0 // Earth's equatorial radius in WGS84
|
||||
private val FE_WGS84 = 1.0 / 298.257223563 // Flattening of the WGS84 ellipsoid
|
||||
|
||||
private var isFirstPoint = true
|
||||
private var firstPoint = DoubleArray(3)
|
||||
private val bPos = DoubleArray(3)
|
||||
private var bECEF = DoubleArray(3)
|
||||
private val rPos = DoubleArray(3)
|
||||
private var rECEF = DoubleArray(3)
|
||||
private val vECEF = DoubleArray(3)
|
||||
|
||||
private var useBlhToEnu = true
|
||||
private var blhToEnu = BlhToEnu()
|
||||
|
||||
/**
|
||||
* 将 WGS84 坐标转换为 ENU (East-North-Up) 坐标
|
||||
* 如果是第一个点，它将被设置为 ENU 坐标系的基准点
|
||||
*
|
||||
* @param lon 经度（度）
|
||||
* @param lat 纬度（度）
|
||||
* @param hgt 高度（米）
|
||||
* @return 包含 ENU 坐标的 Enu 对象
|
||||
*/
|
||||
fun wgs84ToENU(lon: Double, lat: Double, hgt: Double): ENU {
|
||||
if (useBlhToEnu) {
|
||||
val enu = blhToEnu.wgs84ToEnu(lon = lon, lat = lat, height = hgt)
|
||||
return ENU(enu[0], enu[1], enu[2])
|
||||
}
|
||||
|
||||
if (isFirstPoint) setEnuBenchmark(lon, lat, hgt)
|
||||
rPos[0] = lat * DEG2RAD
|
||||
rPos[1] = lon * DEG2RAD
|
||||
rPos[2] = hgt
|
||||
|
||||
rECEF = pos2ecef(rPos)
|
||||
vECEF[0] = rECEF[0] - bECEF[0]
|
||||
vECEF[1] = rECEF[1] - bECEF[1]
|
||||
vECEF[2] = rECEF[2] - bECEF[2]
|
||||
val enuDoubleArray = ecef2enu(bPos, vECEF)
|
||||
return ENU(enuDoubleArray[0], enuDoubleArray[1], enuDoubleArray[2])
|
||||
}
|
||||
|
||||
/**
|
||||
* 将 WGS84 坐标转换为 ENU (East-North-Up) 坐标
|
||||
* 如果是第一个点，它将被设置为 ENU 坐标系的基准点
|
||||
*
|
||||
* @param wgs84 WGS84 坐标对象
|
||||
* @return 包含 ENU 坐标的 Enu 对象
|
||||
*/
|
||||
fun wgs84ObjectToENU(wgs84: WGS84): ENU = wgs84ToENU(wgs84.lon, wgs84.lat, wgs84.hgt)
|
||||
|
||||
/**
|
||||
* 是否已设置 ENU 坐标系的基准点
|
||||
*/
|
||||
fun isEnuBenchmarkSet(): Boolean = !isFirstPoint
|
||||
|
||||
/**
|
||||
* 设置 ENU 坐标系的基准点
|
||||
*
|
||||
* @param lon 基准点经度（度）
|
||||
* @param lat 基准点纬度（度）
|
||||
* @param hgt 基准点高度（米）
|
||||
*/
|
||||
private fun setEnuBenchmark(lon: Double, lat: Double, hgt: Double) {
|
||||
firstPoint = doubleArrayOf(lon, lat, hgt)
|
||||
bPos[0] = lat * DEG2RAD
|
||||
bPos[1] = lon * DEG2RAD
|
||||
bPos[2] = hgt
|
||||
bECEF = pos2ecef(bPos)
|
||||
isFirstPoint = false
|
||||
}
|
||||
|
||||
/**
|
||||
* 获取 ENU 坐标系的基准点
|
||||
*
|
||||
* @return 包含 WGS84 坐标 {经度, 纬度, 高度} 的 DoubleArray
|
||||
*/
|
||||
fun getEnuBenchmarkPoint(): DoubleArray {
|
||||
if (useBlhToEnu) {
|
||||
return doubleArrayOf(blhToEnu.getOriginLon(), blhToEnu.getOriginLat(), blhToEnu.getOriginHeight())
|
||||
}
|
||||
return firstPoint
|
||||
}
|
||||
|
||||
/**
|
||||
* 获取 ENU 坐标系的基准点
|
||||
*
|
||||
* @return 包含 WGS84 坐标的 WGS84 对象
|
||||
*/
|
||||
fun getEnuBenchmarkPointAsWGS84(): WGS84 {
|
||||
if (useBlhToEnu) {
|
||||
return WGS84(blhToEnu.getOriginLon(), blhToEnu.getOriginLat(), blhToEnu.getOriginHeight())
|
||||
}
|
||||
return WGS84(firstPoint[0], firstPoint[1], firstPoint[2])
|
||||
}
|
||||
|
||||
/**
|
||||
* 重置 ENU 基准点
|
||||
* 调用此方法后，下一次 wgs84ToENU 调用将设置新的基准点
|
||||
*/
|
||||
fun resetEnuBenchmarkPoint() {
|
||||
if (useBlhToEnu) {
|
||||
blhToEnu.resetEnuBenchmarkPoint()
|
||||
return
|
||||
}
|
||||
isFirstPoint = true
|
||||
}
|
||||
|
||||
/**
|
||||
* 将 ENU (East-North-Up) 坐标转换为 WGS84 坐标
|
||||
*
|
||||
* @param enu 包含 ENU 坐标的 Enu 对象
|
||||
* @return 包含 WGS84 坐标 {经度, 纬度, 高度} 的 DoubleArray
|
||||
*/
|
||||
fun enuToWGS84(enu: ENU): DoubleArray {
|
||||
if (useBlhToEnu) {
|
||||
val wgs84 = blhToEnu.enuToWgs84(e = enu.x, n = enu.y, u = enu.z)
|
||||
return doubleArrayOf(wgs84[1], wgs84[0], wgs84[2])
|
||||
}
|
||||
|
||||
val enuArray = doubleArrayOf(enu.x, enu.y, enu.z)
|
||||
val enuToEcefMatrix = xyz2enu(bPos)
|
||||
val ecefArray = matmul(charArrayOf('T', 'N'), 3, 1, 3, 1.0, enuToEcefMatrix, enuArray, 0.0)
|
||||
vECEF[0] = bECEF[0] + ecefArray[0]
|
||||
vECEF[1] = bECEF[1] + ecefArray[1]
|
||||
vECEF[2] = bECEF[2] + ecefArray[2]
|
||||
return ecef2pos(vECEF)
|
||||
}
|
||||
|
||||
/**
|
||||
* 将 ENU (East-North-Up) 坐标转换为 WGS84 坐标
|
||||
*
|
||||
* @param enu 包含 ENU 坐标的 Enu 对象
|
||||
* @return 包含 WGS84 坐标的 WGS84 对象
|
||||
*/
|
||||
fun enuToWGS84Object(enu: ENU): WGS84 {
|
||||
val wgs84Array = enuToWGS84(enu)
|
||||
return WGS84(wgs84Array[0], wgs84Array[1], wgs84Array[2])
|
||||
}
|
||||
|
||||
/**
|
||||
* 将 WGS84 坐标转换为 EPSG3857 坐标
|
||||
*
|
||||
* @param lon 经度（度）
|
||||
* @param lat 纬度（度）
|
||||
* @return 包含 EPSG3857 坐标的 EPSG3857 对象
|
||||
*/
|
||||
fun wgs84ToEPSG3857(lon: Double, lat: Double): EPSG3857 {
|
||||
val x = lon * HALF_SIZE / 180
|
||||
var y = RADIUS * ln(tan(Math.PI * (lat + 90) / 360))
|
||||
y = y.coerceIn(-HALF_SIZE, HALF_SIZE)
|
||||
return EPSG3857(x, y)
|
||||
}
|
||||
|
||||
/**
|
||||
* 将 WGS84 坐标转换为 EPSG3857 坐标
|
||||
*
|
||||
* @param wgs84 WGS84 坐标对象
|
||||
* @return 包含 EPSG3857 坐标的 EPSG3857 对象
|
||||
*/
|
||||
fun wgs84ObjectToEPSG3857(wgs84: WGS84): EPSG3857 = wgs84ToEPSG3857(wgs84.lon, wgs84.lat)
|
||||
|
||||
/**
|
||||
* 将 EPSG3857 坐标转换为 WGS84 坐标
|
||||
*
|
||||
* @param epsg3857 包含 EPSG3857 坐标的 EPSG3857 对象
|
||||
* @return 包含 WGS84 坐标 {经度, 纬度} 的 DoubleArray
|
||||
*/
|
||||
fun epsg3857ToWGS84(epsg3857: EPSG3857): DoubleArray {
|
||||
val lon = (epsg3857.x / HALF_SIZE) * 180.0
|
||||
val lat = (2 * atan(exp(epsg3857.y / RADIUS)) - Math.PI / 2) * RAD2DEG
|
||||
return doubleArrayOf(lon, lat)
|
||||
}
|
||||
|
||||
/**
|
||||
* 将 EPSG3857 坐标转换为 WGS84 坐标
|
||||
*
|
||||
* @param epsg3857 包含 EPSG3857 坐标的 EPSG3857 对象
|
||||
* @return 包含 WGS84 坐标的 WGS84 对象
|
||||
*/
|
||||
fun epsg3857ToWGS84Object(epsg3857: EPSG3857): WGS84 {
|
||||
val wgs84Array = epsg3857ToWGS84(epsg3857)
|
||||
return WGS84(wgs84Array[0], wgs84Array[1], 0.0)
|
||||
}
|
||||
|
||||
fun pos2ecef(pos: DoubleArray): DoubleArray {
|
||||
val (lat, lon, hgt) = pos
|
||||
val sinp = sin(lat)
|
||||
val cosp = cos(lat)
|
||||
val sin_l = sin(lon)
|
||||
val cos_l = cos(lon)
|
||||
val e2 = FE_WGS84 * (2.0 - FE_WGS84)
|
||||
val v = RE_WGS84 / sqrt(1.0 - e2 * sinp * sinp)
|
||||
|
||||
return doubleArrayOf(
|
||||
(v + hgt) * cosp * cos_l,
|
||||
(v + hgt) * cosp * sin_l,
|
||||
(v * (1.0 - e2) + hgt) * sinp
|
||||
)
|
||||
}
|
||||
|
||||
fun ecef2enu(pos: DoubleArray, r: DoubleArray): DoubleArray {
|
||||
val E = xyz2enu(pos)
|
||||
return matmul(charArrayOf('N', 'N'), 3, 1, 3, 1.0, E, r, 0.0)
|
||||
}
|
||||
|
||||
fun matmul(
|
||||
tr: CharArray,
|
||||
n: Int,
|
||||
k: Int,
|
||||
m: Int,
|
||||
alpha: Double,
|
||||
A: DoubleArray,
|
||||
B: DoubleArray,
|
||||
beta: Double
|
||||
): DoubleArray {
|
||||
val f = when {
|
||||
tr[0] == 'N' && tr[1] == 'N' -> 1
|
||||
tr[0] == 'N' && tr[1] == 'T' -> 2
|
||||
tr[0] == 'T' && tr[1] == 'N' -> 3
|
||||
else -> 4
|
||||
}
|
||||
val C = DoubleArray(n * k)
|
||||
for (i in 0 until n) {
|
||||
for (j in 0 until k) {
|
||||
var d = 0.0
|
||||
when (f) {
|
||||
1 -> for (x in 0 until m) d += A[i + x * n] * B[x + j * m]
|
||||
2 -> for (x in 0 until m) d += A[i + x * n] * B[j + x * k]
|
||||
3 -> for (x in 0 until m) d += A[x + i * m] * B[x + j * m]
|
||||
4 -> for (x in 0 until m) d += A[x + i * m] * B[j + x * k]
|
||||
}
|
||||
C[i + j * n] = alpha * d + beta * C[i + j * n]
|
||||
}
|
||||
}
|
||||
return C
|
||||
}
|
||||
|
||||
fun xyz2enu(pos: DoubleArray): DoubleArray {
|
||||
val (lat, lon) = pos
|
||||
val sinp = sin(lat)
|
||||
val cosp = cos(lat)
|
||||
val sin_l = sin(lon)
|
||||
val cos_l = cos(lon)
|
||||
|
||||
return doubleArrayOf(
|
||||
-sin_l, cos_l, 0.0,
|
||||
-sinp * cos_l, -sinp * sin_l, cosp,
|
||||
cosp * cos_l, cosp * sin_l, sinp
|
||||
)
|
||||
}
|
||||
|
||||
fun ecef2pos(ecef: DoubleArray): DoubleArray {
|
||||
val (x, y, z) = ecef
|
||||
val a = RE_WGS84
|
||||
val b = a * (1 - FE_WGS84)
|
||||
val e2 = (a * a - b * b) / (a * a)
|
||||
val e2p = (a * a - b * b) / (b * b)
|
||||
val r2 = x * x + y * y
|
||||
val r = sqrt(r2)
|
||||
val E2 = a * a - b * b
|
||||
val F = 54 * b * b * z * z
|
||||
val G = r2 + (1 - e2) * z * z - e2 * E2
|
||||
val c = (e2 * e2 * F * r2) / (G * G * G)
|
||||
val s = cbrt(1 + c + sqrt(c * c + 2 * c))
|
||||
val P = F / (3 * (s + 1 / s + 1) * (s + 1 / s + 1) * G * G)
|
||||
val Q = sqrt(1 + 2 * e2 * e2 * P)
|
||||
val r0 = -(P * e2 * r) / (1 + Q) + sqrt(0.5 * a * a * (1 + 1.0 / Q) - P * (1 - e2) * z * z / (Q * (1 + Q)) - 0.5 * P * r2)
|
||||
val U = sqrt((r - e2 * r0) * (r - e2 * r0) + z * z)
|
||||
val V = sqrt((r - e2 * r0) * (r - e2 * r0) + (1 - e2) * z * z)
|
||||
val Z0 = b * b * z / (a * V)
|
||||
|
||||
val lon = atan2(y, x) * RAD2DEG
|
||||
val lat = atan((z + e2p * Z0) / r) * RAD2DEG
|
||||
val hgt = U * (1 - b * b / (a * V))
|
||||
return doubleArrayOf(lon, lat, hgt)
|
||||
}
|
||||
|
||||
data class WGS84(var lon: Double = 0.0, var lat: Double = 0.0, var hgt: Double = 0.0) : Parcelable {
|
||||
constructor(parcel: Parcel) : this(
|
||||
parcel.readDouble(),
|
||||
parcel.readDouble(),
|
||||
parcel.readDouble()
|
||||
)
|
||||
|
||||
constructor(wgs84: DoubleArray) : this(
|
||||
lon = wgs84.getOrElse(0) { 0.0 },
|
||||
lat = wgs84.getOrElse(1) { 0.0 },
|
||||
hgt = wgs84.getOrElse(2) { 0.0 }
|
||||
)
|
||||
|
||||
override fun writeToParcel(parcel: Parcel, flags: Int) {
|
||||
parcel.writeDouble(lon)
|
||||
parcel.writeDouble(lat)
|
||||
parcel.writeDouble(hgt)
|
||||
}
|
||||
|
||||
override fun describeContents(): Int = 0
|
||||
|
||||
companion object CREATOR : Parcelable.Creator<WGS84> {
|
||||
override fun createFromParcel(parcel: Parcel): WGS84 {
|
||||
return WGS84(parcel)
|
||||
}
|
||||
|
||||
override fun newArray(size: Int): Array<WGS84?> {
|
||||
return arrayOfNulls(size)
|
||||
}
|
||||
}
|
||||
|
||||
override fun toString(): String {
|
||||
return "WGS84(lon=$lon, lat=$lat, hgt=$hgt)"
|
||||
}
|
||||
}
|
||||
|
||||
data class EPSG3857(var x: Double = 0.0, var y: Double = 0.0) : Parcelable {
|
||||
constructor(parcel: Parcel) : this(
|
||||
parcel.readDouble(),
|
||||
parcel.readDouble()
|
||||
)
|
||||
|
||||
constructor(epsG3857: DoubleArray) : this(
|
||||
x = epsG3857.getOrElse(0) { 0.0 },
|
||||
y = epsG3857.getOrElse(1) { 0.0 }
|
||||
)
|
||||
|
||||
override fun writeToParcel(parcel: Parcel, flags: Int) {
|
||||
parcel.writeDouble(x)
|
||||
parcel.writeDouble(y)
|
||||
}
|
||||
|
||||
override fun describeContents(): Int = 0
|
||||
|
||||
companion object CREATOR : Parcelable.Creator<EPSG3857> {
|
||||
override fun createFromParcel(parcel: Parcel): EPSG3857 {
|
||||
return EPSG3857(parcel)
|
||||
}
|
||||
|
||||
override fun newArray(size: Int): Array<EPSG3857?> {
|
||||
return arrayOfNulls(size)
|
||||
}
|
||||
}
|
||||
|
||||
override fun toString(): String {
|
||||
return "EPSG3857(x=$x, y=$y)"
|
||||
}
|
||||
}
|
||||
|
||||
data class ENU(var x: Double = 0.0, var y: Double = 0.0, var z: Double = 0.0) : Parcelable {
|
||||
constructor(parcel: Parcel) : this(
|
||||
parcel.readDouble(),
|
||||
parcel.readDouble(),
|
||||
parcel.readDouble()
|
||||
)
|
||||
|
||||
constructor(enu: DoubleArray) : this(
|
||||
x = enu.getOrElse(0) { 0.0 },
|
||||
y = enu.getOrElse(1) { 0.0 },
|
||||
z = enu.getOrElse(2) { 0.0 }
|
||||
)
|
||||
|
||||
override fun writeToParcel(parcel: Parcel, flags: Int) {
|
||||
parcel.writeDouble(x)
|
||||
parcel.writeDouble(y)
|
||||
parcel.writeDouble(z)
|
||||
}
|
||||
|
||||
override fun describeContents(): Int = 0
|
||||
|
||||
companion object CREATOR : Parcelable.Creator<ENU> {
|
||||
override fun createFromParcel(parcel: Parcel): ENU {
|
||||
return ENU(parcel)
|
||||
}
|
||||
|
||||
override fun newArray(size: Int): Array<ENU?> {
|
||||
return arrayOfNulls(size)
|
||||
}
|
||||
}
|
||||
|
||||
override fun toString(): String {
|
||||
return "ENU(x=$x, y=$y, z=$z)"
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,17 @@
|
||||
package com.icegps.common
|
||||
|
||||
import org.junit.Test
|
||||
|
||||
import org.junit.Assert.*
|
||||
|
||||
/**
|
||||
* Example local unit test, which will execute on the development machine (host).
|
||||
*
|
||||
* See [testing documentation](http://d.android.com/tools/testing).
|
||||
*/
|
||||
class ExampleUnitTest {
|
||||
@Test
|
||||
fun addition_isCorrect() {
|
||||
assertEquals(4, 2 + 2)
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user