package com.flightfeather.uav.common.location
|
|
import com.flightfeather.uav.domain.entity.BaseRealTimeData
|
import java.math.BigDecimal
|
import kotlin.math.abs
|
import kotlin.math.atan
|
import kotlin.math.cos
|
import kotlin.math.sin
|
|
/**
|
* 走航轨迹分割分类
|
* @date 2024/7/3
|
* @author feiyu02
|
*/
|
object TrackSegment {
|
|
// 坐标点间最小距离，单位米
|
private const val MIN_DISTANCE = 6
|
|
// 两条直线夹角为90度时，认为垂直。实际情况中，角度允许有一定偏差，允许偏差角度
|
private const val VERTICAL_OFFSET_DEG = 45
|
|
/**
|
* 按照道路对走航轨迹进行分割
|
* 此处简化认为当连续的前后两段轨迹趋向于一条直线（平行）时，即为在同一条道路上；当其更趋向于垂直时，即认为车辆进行了转向，进入了新的道路
|
* 具体的计算方法为：
|
* 1. 求出每个坐标点相对于前一个坐标点在经纬度坐标系下偏转的角度（当前后两个坐标点的距离过近时，直接认为其同属于同一路段）；
|
* 2. 依次比较前后两个偏转角度的差值，当差值角度在[45°, 135°]或[225°, 315°]范围内时，认为车辆进行了转向，进入新的道路，否则依旧处于同一道路上
|
*/
|
fun segmentWithRoad(data: List<BaseRealTimeData>): List<List<BaseRealTimeData>> {
|
val records = mutableListOf<MutableList<BaseRealTimeData>>()
|
if (data.isEmpty()) return records
|
// 记录上一组接近平行的坐标点的偏转角度
|
val lastDegList = mutableListOf<Double>()
|
// 记录上一组距离接近的坐标点
|
val closeList = mutableListOf<BaseRealTimeData>()
|
records.add(mutableListOf())
|
data.forEachIndexed { i, d ->
|
var isSame = false
|
if (i > 0) {
|
// 前一个有效监测点
|
var lastData = data[i - 1]
|
// 确保两点坐标合法
|
if ((lastData.longitude != null && lastData.longitude != BigDecimal.ZERO)
|
&& (lastData.latitude != null && lastData.latitude != BigDecimal.ZERO)
|
&& (d.longitude != null && d.longitude != BigDecimal.ZERO)
|
&& (d.latitude != null && d.latitude != BigDecimal.ZERO)
|
) {
|
// 计算两点距离，过近时直接认为是同一个点，同一路段
|
var distance = CoordinateUtil.calculateDistance(
|
lastData.longitude!!.toDouble(), lastData.latitude!!.toDouble(),
|
d.longitude!!.toDouble(), d.latitude!!.toDouble(),
|
)
|
// 距离过近时, 将距离替换为当前点和近点集合中的第一个点的距离
|
if (distance < MIN_DISTANCE && closeList.isNotEmpty()) {
|
// 如果已经有距离过近的点集合，则还需要和第一个点进行距离判断，
|
// 解决当车辆行驶速度过低时，连续点的距离都过近导致都判定为同一点的问题
|
val firstCloseData = closeList[0]
|
// lastData = closeList.toList().avg()
|
distance = CoordinateUtil.calculateDistance(
|
firstCloseData.longitude!!.toDouble(), firstCloseData.latitude!!.toDouble(),
|
d.longitude!!.toDouble(), d.latitude!!.toDouble())
|
}
|
|
if (distance >= MIN_DISTANCE) {
|
// 两点距离较大时，计算夹角
|
val deg = CoordinateUtil.getAngle(
|
lastData.longitude!!.toDouble(), lastData.latitude!!.toDouble(),
|
d.longitude!!.toDouble(), d.latitude!!.toDouble(),
|
)
|
// 计算
|
isSame = if (lastDegList.isNotEmpty()) {
|
var bool = true
|
|
// 将当前坐标点的角度和之前所有属于同一道路的坐标点的角度的均值做比较，
|
// 解决当车辆行驶轨迹为弧线时（例如立交桥的弧线转弯等），一直被判定为转向幅度不大属于同一路段的问题
|
val avgDeg = avgDegree(lastDegList)
|
val diffDeg = abs(deg - avgDeg)
|
if (diffDeg in (90.0 - VERTICAL_OFFSET_DEG)..(90.0 + VERTICAL_OFFSET_DEG)
|
|| diffDeg in (270.0 - VERTICAL_OFFSET_DEG)..(270.0 + VERTICAL_OFFSET_DEG)
|
) {
|
bool = false
|
}
|
|
// 当出现转弯点时，清空历史角度，并且舍弃转弯点相对于前一个点的角度（解决一种极端情况，当连续出现转弯点时，当前坐标点会被单独分割为一段）
|
if (!bool) {
|
lastDegList.clear()
|
} else {
|
lastDegList.add(deg)
|
}
|
bool
|
} else {
|
// 当没有历史角度时，直接保存，并认为是同一路段
|
lastDegList.add(deg)
|
true
|
}
|
// 清空上一组距离接近的坐标点
|
closeList.clear()
|
} else {
|
// 两点距离接近时，添加记录
|
closeList.add(d)
|
isSame = true
|
}
|
}
|
// 两点坐标不合法，则认为同一路段
|
else {
|
isSame = true
|
}
|
} else {
|
isSame = true
|
}
|
|
if (!isSame)
|
records.add(mutableListOf())
|
records.last().add(d)
|
}
|
|
return records
|
}
|
|
/**
|
* 求转向角度的均值
|
*/
|
private fun avgDegree(degList: List<Double>): Double {
|
if (degList.isEmpty()) return .0
|
//采用单位矢量法求取均值
|
var u = .0//东西方位分量总和
|
var v = .0//南北方位分量总和
|
var c = 0//数据计数
|
|
degList.forEach {
|
val r = Math.toRadians(it)
|
u += sin(r)
|
v += cos(r)
|
c++
|
}
|
|
val avgU = u / c
|
val avgV = v / c
|
var a = atan(avgU / avgV)
|
a = Math.toDegrees(a)
|
/**
|
* avgU>0;avgV>0: 真实角度处于第一象限，修正值为+0°
|
* avgU>0;avgV<0: 真实角度处于第二象限，修正值为+180°
|
* avgU<0;avgV<0: 真实角度处于第三象限，修正值为+180°
|
* avgU<0;avgV>0: 真实角度处于第四象限，修正值为+360°
|
*/
|
a += if (avgV > 0) {
|
if (avgU > 0) 0 else 360
|
} else {
|
180
|
}
|
|
return a
|
}
|
}
|
