编辑代码

import kotlin.math.*

// 计算向量的叉乘
fun crossProduct(v1: DoubleArray, v2: DoubleArray): DoubleArray {
    return doubleArrayOf(
        v1[1] * v2[2] - v1[2] * v2[1],  // x component
        v1[2] * v2[0] - v1[0] * v2[2],  // y component
        v1[0] * v2[1] - v1[1] * v2[0]   // z component
    )
}

// 向量的归一化
fun normalize(v: DoubleArray): DoubleArray {
    val length = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2])
    return if (length == 0.0) doubleArrayOf(0.0, 0.0, 0.0) else doubleArrayOf(v[0] / length, v[1] / length, v[2] / length)
}

// 向量点乘
fun dotProduct(v1: DoubleArray, v2: DoubleArray): Double {
    return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]
}

// 向量的模
fun magnitude(v: DoubleArray): Double {
    return sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2])
}

fun calPitchAngle(waypoint1: DoubleArray, waypoint2: DoubleArray, point1: DoubleArray, point2: DoubleArray): Double {
    
    val direction1 = doubleArrayOf(
        waypoint2[0] - waypoint1[0],
        waypoint2[1] - waypoint1[1],
        waypoint2[2] - waypoint1[2]
    )
    val direction2 = doubleArrayOf(
        waypoint2[0] - point1[0],
        waypoint2[1] - point1[1],
        waypoint2[2] - point1[2]
    )
    val direction3 = doubleArrayOf(
        waypoint2[0] - point2[0],
        waypoint2[1] - point2[1],
        waypoint2[2] - point2[2]
    )

    var f1 = crossProduct(direction2, direction1)
    var f2 = crossProduct(direction1, direction3)
    
    f1 = normalize(f1)
    f2 = normalize(f2)

    val dot = dotProduct(f1, f2)
    val magF1 = magnitude(f1)
    val magF2 = magnitude(f2)
    var cosTheta = dot / (magF1 * magF2)

    cosTheta = cosTheta.coerceIn(-1.0, 1.0)

    val theta = acos(cosTheta)

    val angleInDegrees = theta * (180.0 / PI)

    return if (f1[2] + f2[2] > 0) angleInDegrees else -angleInDegrees
}
fun main() {
val point1 = doubleArrayOf(0.0,0.0,0.0)
val point2 = doubleArrayOf(0.0,1.0,0.0)
val point3 = doubleArrayOf(1.0,1.0,0.0)
val point4 = doubleArrayOf(0.0,1.0,1.0)
val angle = calPitchAngle(point1,point2,point3,point4)
println(angle)
}