弯道

src/main/java/commons/gis/CurveAnalyzer.kt

+package commons.gis
+
+import kotlinx.coroutines.Dispatchers
+import kotlinx.coroutines.withContext
+import java.math.RoundingMode
+import java.util.Collections
+import kotlin.math.PI
+import kotlin.math.abs
+import kotlin.math.cos
+import kotlin.math.pow
+
+/*弯道分类标准（基于高速公路设计规范）
+直道：半径 > 2000米（曲率 < 0.0005）
+缓弯：半径 500-2000米
+中弯：半径 200-500米
+急弯：半径 < 200米*/
+
+/**
+ * 地理坐标点类
+ * @param longitude 经度
+ * @param latitude 纬度
+ */
+data class CurvePoint(
+    val longitude: Double,
+    val latitude: Double
+)
+
+/**
+ * 弯道分析结果
+ * @param curvature 曲率 (1/m)
+ * @param radius 曲率半径 (m)，如果是直线则为无穷大
+ * @param curveDirection 弯道方向：-1=左弯，0=直道，1=右弯
+ * @param curveMagnitude 弯道大小等级：0=直道，1=小弯，2=中弯，3=大弯
+ * @param angle 转向角度 (度)
+ */
+data class CurveAnalysis(
+    val curvature: Double,
+    val radius: Double,
+    var curveDirection: Int,
+    val curveMagnitude: Int,
+    val angle: Double
+)
+
+/**
+ * 三点分析结果
+ *  @param avgDistance 3点2线段的平均距离
+ * @param angle 转向角度 (度)
+ */
+data class ThreePointAnalysis(
+    val avgDistance: Double,
+    val angle: Double
+)
+
+/**
+ * 地球相关常数
+ */
+object EarthConstants {
+    // 地球半径 (米)
+    const val EARTH_RADIUS = 6371000.0
+
+    // 弧度转角度
+    const val RAD_TO_DEG = 180.0 / PI
+
+    // 弯道大小阈值 (米)
+    const val SMALL_CURVE_THRESHOLD = 2000.0
+    const val MEDIUM_CURVE_THRESHOLD = 500.0
+    const val LARGE_CURVE_THRESHOLD = 200.0
+}
+
+/**
+ * 弯道分析器
+ */
+object CurveAnalyzer {
+
+    /**
+     * 计算两点间的地理距离 (Haversine公式)
+     */
+    private fun calculateDistance(point1: CurvePoint, point2: CurvePoint): Double {
+        val dis=BasicTools.calculateDistance(point1.longitude,point1.latitude,point2.longitude,point2.latitude)
+        return dis.toDouble()
+
+/*        val lat1Rad = Math.toRadians(point1.latitude)
+        val lat2Rad = Math.toRadians(point2.latitude)
+        val deltaLatRad = Math.toRadians(point2.latitude - point1.latitude)
+        val deltaLonRad = Math.toRadians(point2.longitude - point1.longitude)
+
+        val a = sin(deltaLatRad / 2).pow(2) +
+                cos(lat1Rad) * cos(lat2Rad) *
+                sin(deltaLonRad / 2).pow(2)
+
+        val c = 2 * atan2(sqrt(a), sqrt(1 - a))
+
+        return EarthConstants.EARTH_RADIUS * c*/
+    }
+
+/*    private fun calculateDistance(point1: GeoPoint, point2: GeoPoint): Double {
+        val lat1Rad = Math.toRadians(point1.latitude)
+        val lat2Rad = Math.toRadians(point2.latitude)
+        val deltaLatRad = Math.toRadians(point2.latitude - point1.latitude)
+        val deltaLonRad = Math.toRadians(point2.longitude - point1.longitude)
+
+        val a = sin(deltaLatRad / 2).pow(2) +
+                cos(lat1Rad) * cos(lat2Rad) *
+                sin(deltaLonRad / 2).pow(2)
+
+        val c = 2 * atan2(sqrt(a), sqrt(1 - a))
+
+        return EarthConstants.EARTH_RADIUS * c
+    }*/
+
+    /**
+     * 计算两点间的方位角 (bearing)
+     */
+    private fun calculateBearing(point1: CurvePoint, point2: CurvePoint): Double {
+        val pair=BasicTools.calculateDistancePair(point1.latitude,point1.longitude,point2.latitude,point2.longitude)
+        return pair.second.toDouble()
+
+
+/*        val lat1Rad = Math.toRadians(point1.latitude)
+        val lat2Rad = Math.toRadians(point2.latitude)
+        val lon1Rad = Math.toRadians(point1.longitude)
+        val lon2Rad = Math.toRadians(point2.longitude)
+
+        val y = sin(lon2Rad - lon1Rad) * cos(lat2Rad)
+        val x = cos(lat1Rad) * sin(lat2Rad) -
+                sin(lat1Rad) * cos(lat2Rad) * cos(lon2Rad - lon1Rad)
+
+        var bearing = atan2(y, x)
+        bearing = (bearing * EarthConstants.RAD_TO_DEG + 360) % 360
+
+        return bearing*/
+    }
+
+    private var bufferSize = 0
+    private var windowSize = 3
+
+    //缓存轨迹
+    private var bufList = Collections.synchronizedList(mutableListOf<CurvePoint>())
+
+    /**
+     *更新点位
+     */
+    suspend fun upTrajectory(geoPoint: CurvePoint, bufferSize: Int = 9): CurveAnalysis {
+        return withContext(Dispatchers.Default){
+            CurveAnalyzer.bufferSize = bufferSize
+            if (bufList.count() > bufferSize) {
+                val iterator = bufList.iterator()
+                if (iterator.hasNext()) {
+                    iterator.next()  // 移动到第一个元素
+                    iterator.remove()  // 删除当前元素（第一个）
+                }
+            }
+            bufList.add(geoPoint)
+            val curveAnalysis = analyzeTrajectory(bufList)
+            return@withContext curveAnalysis
+        }
+    }
+
+
+    /**
+     * 滑动窗口分析轨迹
+     * @param points 轨迹点列表
+     * @param windowSize 滑动窗口大小，默认为3
+     * @return 每个中间点的分析结果列表
+     */
+    /*    private fun analyzeTrajectory(points: List<GeoPoint>, windowSize: Int = 3): List<CurveAnalysis> {
+            if (points.count() < windowSize) {
+                return emptyList()
+            }
+            val results = mutableListOf<CurveAnalysis>()
+
+            for (i in 0..points.size - windowSize) {
+                val windowPoints = points.subList(i, i + windowSize)
+                val analysis = calculateThreePoints(windowPoints)
+                results.add(analysis)
+            }
+
+            return results
+        }*/
+    /**
+     * 滑动窗口分析轨迹
+     * @param points 轨迹点列表
+     * @return 基于一整段点的分析结果
+     */
+    private fun analyzeTrajectory(points: List<CurvePoint>): CurveAnalysis {
+        if (points.count() < windowSize) {
+            return CurveAnalysis(
+                curvature = 1.0,
+                radius = Double.POSITIVE_INFINITY,
+                curveDirection = 0,
+                curveMagnitude = 0,
+                angle = 0.0
+            )
+        }
+        //收集三点计算结果
+        val threeResult = mutableListOf<ThreePointAnalysis>()
+        for (i in 0..points.count() - windowSize) {
+            val windowPoints = points.subList(i, i + windowSize)
+            val analysis = calculateThreePoints(windowPoints)
+            threeResult.add(analysis)
+        }
+        if (points.count() < bufferSize - windowSize) {
+            return CurveAnalysis(
+                curvature = 1.0,
+                radius = 0.0,
+                curveDirection = 0,
+                curveMagnitude = 0,
+                angle = 0.0
+            )
+        }
+        //分析三点计算结果
+        return analyzeThreePoints(threeResult)
+    }
+
+    /**收集三点计算结果*/
+    private fun calculateThreePoints(points: List<CurvePoint>): ThreePointAnalysis {
+        require(points.count() == 3) { "需要3个点进行计算" }
+
+        val p0 = points[0]
+        val p1 = points[1]
+        val p2 = points[2]
+
+        // 计算各段距离
+        val d1 = calculateDistance(p0, p1)
+        val d2 = calculateDistance(p1, p2)
+
+        // 计算各段方位角
+        val bearing1 = calculateBearing(p0, p1)
+        val bearing2 = calculateBearing(p1, p2)
+
+        // 计算转向角度（角度差）
+//        var deltaAngle = bearing2 - bearing1
+        val diffAngle = bearing2.toBigDecimal().subtract(bearing1.toBigDecimal()).setScale(
+            5,
+            RoundingMode.HALF_UP
+        ).toDouble()
+        // 规范化角度到 [-180, 180] 范围
+        val deltaAngle = when {
+            diffAngle > 180 -> diffAngle - 360
+            diffAngle < -180 -> diffAngle + 360
+            else -> diffAngle
+        }
+
+        // 计算曲率半径
+        val avgDistance = (d1 + d2) / 2
+
+        return ThreePointAnalysis(
+            avgDistance = avgDistance,
+            angle = deltaAngle
+        )
+    }
+
+    /**
+     * 使用三点法计算曲率和转向信息
+     * @param points 三点计算结果
+     * @return 弯道分析结果
+     */
+    private fun analyzeThreePoints(points: List<ThreePointAnalysis>): CurveAnalysis {
+        // 计算转向角度（累计角度差）
+        var totalDeltaAngle = points.sumOf { it.angle }
+        // 计算平均移动距离
+        val totalAvgDistance = points.map { it.avgDistance }.average()
+        // 计算曲率半径
+        val deltaAngleRad = Math.toRadians(totalDeltaAngle)
+        // 防止除以零
+        val radius = if (abs(deltaAngleRad) > 1e-10) {
+            totalAvgDistance / abs(deltaAngleRad)
+        } else {
+            Double.POSITIVE_INFINITY
+        }
+
+        // 计算曲率
+        val curvature = if (radius.isFinite()) {
+            1.0 / radius
+        } else {
+            0.0
+        }
+        // 判断弯道方向
+        val curveDirection = when {
+            totalDeltaAngle > 2 -> 1    // 右转
+            totalDeltaAngle < -2 -> -1  // 左转
+            else -> 0              // 直行
+        }
+        // 判断弯道大小
+        val curveMagnitude = when {
+            !radius.isFinite() || radius > EarthConstants.SMALL_CURVE_THRESHOLD -> 0 // 直道
+            radius > EarthConstants.MEDIUM_CURVE_THRESHOLD -> 1 // 缓弯
+            radius > EarthConstants.LARGE_CURVE_THRESHOLD -> 2 // 中弯
+            else -> 3 // 急弯
+        }
+
+        return CurveAnalysis(
+            curvature = curvature,
+            radius = radius,
+            curveDirection = curveDirection,
+            curveMagnitude = curveMagnitude,
+            angle = totalDeltaAngle
+        )
+    }
+
+
+    /**
+     * 使用三点法计算曲率和转向信息
+     * @param points 三个连续的点 [p0, p1, p2]
+     * @return 弯道分析结果
+     */
+    /*    private fun analyzeThreePoints(points: List<GeoPoint>): CurveAnalysis {
+            require(points.count() == 3) { "需要3个点进行计算" }
+
+            val p0 = points[0]
+            val p1 = points[1]
+            val p2 = points[2]
+
+            // 计算各段距离
+            val d1 = calculateDistance(p0, p1)
+            val d2 = calculateDistance(p1, p2)
+
+            // 计算各段方位角
+            val bearing1 = calculateBearing(p0, p1)
+            val bearing2 = calculateBearing(p1, p2)
+
+            // 计算转向角度（角度差）
+            var deltaAngle = bearing2 - bearing1
+
+            // 规范化角度到 [-180, 180] 范围
+            deltaAngle = when {
+                deltaAngle > 180 -> deltaAngle - 360
+                deltaAngle < -180 -> deltaAngle + 360
+                else -> deltaAngle
+            }
+
+            // 计算曲率半径
+            val avgDistance = (d1 + d2) / 2
+            val deltaAngleRad = Math.toRadians(deltaAngle)
+
+            // 防止除以零
+            val radius = if (abs(deltaAngleRad) > 1e-10) {
+                avgDistance / abs(deltaAngleRad)
+            } else {
+                Double.POSITIVE_INFINITY
+            }
+
+            // 计算曲率
+            val curvature = if (radius.isFinite()) {
+                1.0 / radius
+            } else {
+                0.0
+            }
+
+            // 判断弯道方向（高速）
+            val curveDirection = when {
+                deltaAngle > 2 -> 1    // 右转
+                deltaAngle < -2 -> -1  // 左转
+                else -> 0              // 直行
+            }
+
+            // 判断弯道大小
+            val curveMagnitude = when {
+                !radius.isFinite() || radius > EarthConstants.SMALL_CURVE_THRESHOLD -> 0 // 直道
+                radius > EarthConstants.MEDIUM_CURVE_THRESHOLD -> 1 // 小弯
+                radius > EarthConstants.LARGE_CURVE_THRESHOLD -> 2 // 中弯
+                else -> 3 // 大弯
+            }
+
+            return CurveAnalysis(
+                curvature = curvature,
+                radius = radius,
+                curveDirection = curveDirection,
+                curveMagnitude = curveMagnitude,
+                angle = deltaAngle
+            )
+        }*/
+
+
+    /**
+     * 使用最小二乘法拟合曲线计算曲率（更精确的方法）
+     * @param points 多个点（至少3个）
+     * @return 拟合的曲率
+     */
+    fun calculateCurvatureByLeastSquares(points: List<CurvePoint>): Double {
+        if (points.size < 3) return 0.0
+
+        // 将经纬度转换为平面坐标（使用UTM简化投影）
+        val xyPoints = points.map { point ->
+            // 简化的投影转换：使用经纬度的线性近似（适用于小范围）
+            // 在实际应用中，应考虑使用UTM或其他投影
+            val x =
+                point.longitude * EarthConstants.EARTH_RADIUS * cos(Math.toRadians(point.latitude))
+            val y = point.latitude * EarthConstants.EARTH_RADIUS
+            Pair(x, y)
+        }
+
+        // 计算二阶导数来估算曲率
+        val n = xyPoints.size
+        val curvatures = mutableListOf<Double>()
+
+        for (i in 1 until n - 1) {
+            val pPrev = xyPoints[i - 1]
+            val pCurr = xyPoints[i]
+            val pNext = xyPoints[i + 1]
+
+            // 计算一阶导数（中心差分）
+            val dxPrev = pCurr.first - pPrev.first
+            val dyPrev = pCurr.second - pPrev.second
+            val dxNext = pNext.first - pCurr.first
+            val dyNext = pNext.second - pCurr.second
+
+            val dx = (dxPrev + dxNext) / 2
+            val dy = (dyPrev + dyNext) / 2
+
+            // 计算二阶导数
+            val d2x = dxNext - dxPrev
+            val d2y = dyNext - dyPrev
+
+            // 计算曲率
+            val numerator = abs(dx * d2y - dy * d2x)
+            val denominator = (dx * dx + dy * dy).pow(1.5)
+
+            val curvature = if (denominator > 1e-10) {
+                numerator / denominator
+            } else {
+                0.0
+            }
+
+            curvatures.add(curvature)
+        }
+
+        // 返回平均曲率
+        return curvatures.average()
+    }
+
+    /**
+     * 格式化输出分析结果
+     */
+    fun formatAnalysis(analysis: CurveAnalysis): String {
+        val directionStr = when (analysis.curveDirection) {
+            -1 -> "左转"
+            0 -> "直行"
+            1 -> "右转"
+            else -> "未知"
+        }
+
+        val magnitudeStr = when (analysis.curveMagnitude) {
+            0 -> "直道"
+            1 -> "小弯"
+            2 -> "中弯"
+            3 -> "大弯"
+            else -> "未知"
+        }
+
+        return String.format(
+            "曲率: %.6f 1/m, 半径: %.1f m, 方向: %s, 大小: %s, 角度: %.1f°",
+            analysis.curvature,
+            if (analysis.radius.isFinite()) analysis.radius else Double.POSITIVE_INFINITY,
+            directionStr,
+            magnitudeStr,
+            analysis.angle
+        )
+    }
+}
+
+/**
+ * 使用示例
+ */
+/*fun main() {
+    val analyzer = CurveAnalyzer()
+
+    // 示例轨迹点（经纬度）
+    val trajectory = listOf(
+        GeoPoint(116.397128, 39.916527),  // 北京天安门
+        GeoPoint(116.397428, 39.916727),  // 向东向北移动
+        GeoPoint(116.397828, 39.916527),  // 向东移动，形成右转
+        GeoPoint(116.398128, 39.916327),  // 继续右转
+        GeoPoint(116.398028, 39.916127)   // 轻微左转
+    )
+
+    println("=== 弯道分析示例 ===")
+    println("轨迹点数量: ${trajectory.size}")
+    println()
+
+    // 方法1：滑动窗口分析
+    println("方法1: 滑动窗口分析 (3点法)")
+    val slidingResults = analyzer.analyzeTrajectory(trajectory)
+    slidingResults.forEachIndexed { index, analysis ->
+        println("窗口 ${index + 1}: ${analyzer.formatAnalysis(analysis)}")
+    }
+
+    println()
+
+    // 方法2：最小二乘法分析整个轨迹
+    println("方法2: 最小二乘法拟合曲率")
+    val curvature = analyzer.calculateCurvatureByLeastSquares(trajectory)
+    println("平均曲率: ${"%.6f".format(curvature)} 1/m")
+    println("平均半径: ${"%.1f".format(1.0 / curvature)} m")
+
+    println()
+
+    // 计算单个弯道
+    println("方法3: 三点法计算单个弯道")
+    val threePoints = trajectory.take(3)
+    val singleAnalysis = analyzer.analyzeThreePoints(threePoints)
+    println("三点分析: ${analyzer.formatAnalysis(singleAnalysis)}")
+}*/
+
+/**
+ * 扩展函数：计算两点间距离
+ */
+//fun GeoPoint.distanceTo(other: GeoPoint): Double {
+//    return calculateDistance(this, other)
+//}
+
+/**
+ * 扩展函数：计算到另一点的方位角
+ */
+//fun GeoPoint.bearingTo(other: GeoPoint): Double {
+//    return CurveAnalyzer().calculateBearing(this, other)
+//}
+
+/**
+ * 单位转换工具
+ */
+/*
+object UnitConverter {
+    */
+/**
+     * 弧度转角度
+     *//*
+
+    fun radiansToDegrees(radians: Double): Double {
+        return radians * EarthConstants.RAD_TO_DEG
+    }
+
+    */
+/**
+     * 角度转弧度
+     *//*
+
+    fun degreesToRadians(degrees: Double): Double {
+        return degrees / EarthConstants.RAD_TO_DEG
+    }
+
+    */
+/**
+     * 米转千米
+     *//*
+
+    fun metersToKilometers(meters: Double): Double {
+        return meters / 1000.0
+    }
+
+    */
+/**
+     * 千米转米
+     *//*
+
+    fun kilometersToMeters(kilometers: Double): Double {
+        return kilometers * 1000.0
+    }
+
+
+}*/