3차원 회전행렬 구하기 및 캘리브레이션

가속도 센서를 캘리브레이션 할 일이 생겨서 만들어 보았다. 지피티는 신이다.

아래 함수들 전부 검증해보고 제대로 동작하는 것 확인하였다.

data class Vector3D(val x: Double, val y: Double, val z: Double)

fun normalize(vector: Vector3D): Vector3D {
    val length = sqrt(vector.x * vector.x + vector.y * vector.y + vector.z * vector.z)
    return Vector3D(vector.x / length, vector.y / length, vector.z / length)
}

fun dotProduct(vector1: Vector3D, vector2: Vector3D): Double {
    return vector1.x * vector2.x + vector1.y * vector2.y + vector1.z * vector2.z
}

fun crossProduct(vector1: Vector3D, vector2: Vector3D): Vector3D {
    val x = vector1.y * vector2.z - vector1.z * vector2.y
    val y = vector1.z * vector2.x - vector1.x * vector2.z
    val z = vector1.x * vector2.y - vector1.y * vector2.x
    return Vector3D(x, y, z)
}

fun calculateRotationAngle(vector1: Vector3D, vector2: Vector3D) : Double {
    val normalizedVector1 = normalize(vector1)
    val normalizedVector2 = normalize(vector2)
    val dot = dotProduct(normalizedVector1, normalizedVector2)
    val rotationAngle = acos(dot)
    return rotationAngle
}
fun calculateRotationMatrix(vector1: Vector3D, vector2: Vector3D): Array<DoubleArray> {
    val normalizedVector1 = normalize(vector1)
    val normalizedVector2 = normalize(vector2)
    val dot = dotProduct(normalizedVector1, normalizedVector2)
    val rotationAxis = crossProduct(normalizedVector1, normalizedVector2)
    val rotationAngle = acos(dot)

    val sinAngle = sin(rotationAngle)
    val cosAngle = cos(rotationAngle)
    val oneMinusCosAngle = 1 - cosAngle

    val rotationMatrix = Array(3) { DoubleArray(3) }

    rotationMatrix[0][0]= cosAngle + rotationAxis.x * rotationAxis.x * oneMinusCosAngle
    rotationMatrix[0][1]= rotationAxis.x * rotationAxis.y * oneMinusCosAngle - rotationAxis.z * sinAngle
    rotationMatrix[0][2]= rotationAxis.x * rotationAxis.z * oneMinusCosAngle + rotationAxis.y * sinAngle

    rotationMatrix[1][0]= rotationAxis.y * rotationAxis.x * oneMinusCosAngle + rotationAxis.z * sinAngle
    rotationMatrix[1][1]= cosAngle + rotationAxis.y * rotationAxis.y * oneMinusCosAngle
    rotationMatrix[1][2]= rotationAxis.y * rotationAxis.z * oneMinusCosAngle - rotationAxis.x * sinAngle

    rotationMatrix[2][0]= rotationAxis.z * rotationAxis.x * oneMinusCosAngle - rotationAxis.y * sinAngle
    rotationMatrix[2][1]= rotationAxis.z * rotationAxis.y * oneMinusCosAngle + rotationAxis.x * sinAngle
    rotationMatrix[2][2]= cosAngle + rotationAxis.z * rotationAxis.z * oneMinusCosAngle

    return rotationMatrix
}

fun rotateVector(vector: Vector3D, rotationMatrix: Array<DoubleArray>): Vector3D {
    val rotatedX = vector.x * rotationMatrix[0][0]+ vector.y * rotationMatrix[1][0]+ vector.z * rotationMatrix[2][0]
    val rotatedY = vector.x * rotationMatrix[0][1]+ vector.y * rotationMatrix[1][1]+ vector.z * rotationMatrix[2][1]
    val rotatedZ = vector.x * rotationMatrix[0][2]+ vector.y * rotationMatrix[1][2]+ vector.z * rotationMatrix[2][2]
    return Vector3D(rotatedX, rotatedY, rotatedZ)
}

 

 

 

캘리브레이션은 다음과 같은 방식으로 진행한다.

 

1. 축을 하나 잡는다

2. 몇초동안 인풋을 받고, 그 평균값이 1번에서 정한 축과 동일하다고 가정한다

3. 두 벡터간의 회전행렬을 구한다

4. 다른 축들도 회전행렬을 통해 축을 구해준다

 

새로운 축을 이용하여 이제 수학계산을 하면 된다