글로벌 좌표를 로컬 좌표로 변환 feat codex

구차니 2026. 10. 30. 18:10

먼가 이렇게 작성해주는데 솔찍히 설명해달라고 해도 먼 소리인지 모르겠다.

/* Convert a world-space direction to a frame aligned with vehicle heading.
* Telemetry axes: X=right, Y=up, Z=forward; yaw rotates about world Y.
*/
static void world_to_heading_local(float yaw, float wx, float wy, float wz,
                                   float *lx, float *ly, float *lz)
{
    const float c = cosf(yaw);
    const float s = sinf(yaw);
    *lx = c * wx - s * wz;
    *ly = wy;
    *lz = s * wx + c * wz;
}

헷갈릴 만해요. 핵심은 벡터를 돌리는 것과 좌표축을 돌려서 같은 벡터를 표현하는 것이 반대 방향이라는 점이에요. 자동차 방향 기준으로 바꾸는 건 두 번째라서, yaw를 빼는 회전을 적용합니다. 회전행렬의 역변환은 전치행렬과 같다는 설명은 Illinois 대학 로보틱스 자료 (https://motion.cs.illinois.edu/RoboticSystems/CoordinateTransformations.html)에 있고, 회전축 변환 그림 (https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/multiscaleroboticlab-dam/documents/trm/HS2019/TRM19.pdf)도 도움이 됩니다.

저장소에서 사용한 축은 X=오른쪽, Y=위, Z=앞입니다. 차가 세계 좌표에서 yaw만큼 돌아 있다고 생각해볼게요.

세계 좌표:   X 오른쪽, Z 앞
자동차 좌표: x 오른쪽, z 앞

세계의 방향을 차 기준으로 보려면 자동차가 향한 만큼 좌표축을 반대로 돌려서 바라봅니다. 그 결과가 다음 식이에요.

x =  cos(yaw) × X - sin(yaw) × Z
z =  sin(yaw) × X + cos(yaw) × Z
y =  Y

예를 들어 차가 오른쪽으로 90° 돌아서 세계의 +X 방향을 향한다고 해보죠. 이때 세계의 +X 방향 벡터 (1, 0, 0)을 차 기준으로 바꾸면:

x = cos(90°)×1 - sin(90°)×0 = 0
z = sin(90°)×1 + cos(90°)×0 = 1

즉 (1, 0, 0)이 자동차 좌표에서는 (0, 0, 1)이 됩니다. 세계에서 오른쪽이던 방향이, 그 방향을 바라보는 자동차 입장에서는 앞쪽이라는 뜻이에요.

행렬은 이 두 계산을 한 번에 적은 표기입니다.

[ x ]   [ cos(yaw)  0  -sin(yaw) ] [ X ]
[ y ] = [    0      1      0     ] [ Y ]
[ z ]   [ sin(yaw)  0   cos(yaw) ] [ Z ]

참고로 앞서 추가한 화면 예시처럼 세계의 “앞” 벡터 (0, 0, 1) 자체를 yaw로 되돌리면, 자동차 기준 앞 (0, 0, 1)이 됩니다. 실제로 변환을 확인하려면 (1, 0, 0) 같은 세계 방향도 함께 넣어보면 차이를 더 쉽게 볼 수 있어요.

이 식은 방향 벡터를 변환합니다. yaw, pitch, roll 세 값을 통째로 벡터처럼 넣는 식은 아니에요. Euler 각은 3차원 회전의 표현이라 회전 순서를 알아야 서로 다른 좌표계 사이에서 정확히 변환할 수 있습니다.

예전에 주워들었던 쿼터니안 적용

typedef struct {
    float w, x, y, z;
} Quaternion;

static Quaternion quaternion_multiply(Quaternion a, Quaternion b)
{
    Quaternion q = {
        a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z,
        a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y,
        a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x,
        a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w
    };
    return q;
}

static Quaternion quaternion_axis_angle(float ax, float ay, float az, float angle)
{
    const float half = angle * 0.5f;
    const float s = sinf(half);
    Quaternion q = {cosf(half), ax*s, ay*s, az*s};
    return q;
}

/*
* Convert a world vector to body coordinates using the full vehicle attitude.
* Convention/assumption: vehicle-to-world attitude is
* q = q_yaw(Y) * q_pitch(X) * q_roll(Z), applied roll, then pitch, then yaw.
* With Hamilton quaternions, world->vehicle is q^-1 * v * q.
*/
static void world_to_vehicle_local_quaternion(float yaw, float pitch, float roll,
                                               float wx, float wy, float wz,
                                               float *lx, float *ly, float *lz)
{
    const Quaternion q_yaw = quaternion_axis_angle(0.0f, 1.0f, 0.0f, yaw);
    const Quaternion q_pitch = quaternion_axis_angle(1.0f, 0.0f, 0.0f, pitch);
    const Quaternion q_roll = quaternion_axis_angle(0.0f, 0.0f, 1.0f, roll);
    const Quaternion q = quaternion_multiply(
        quaternion_multiply(q_yaw, q_pitch), q_roll);
    const Quaternion q_inverse = {q.w, -q.x, -q.y, -q.z};
    const Quaternion v_world = {0.0f, wx, wy, wz};
    const Quaternion v_local = quaternion_multiply(
        quaternion_multiply(q_inverse, v_world), q);
    *lx = v_local.x;
    *ly = v_local.y;
    *lz = v_local.z;
}

아 증말.. 먼소리냐고 ㅠㅠ

쿼터니안은 3차원 회전을 숫자 4개 (w, x, y, z)로 표현합니다. 각 축 회전은 각도의 절반으로 만듭니다.

q = (cos(각도/2), 축x·sin(각도/2), 축y·sin(각도/2), 축z·sin(각도/2))

코드는 차량 자세를 q = q_yaw × q_pitch × q_roll로 구성한다고 가정합니다. 그리고 world 벡터 (X, Y, Z)를 스칼라부가 0인 쿼터니안 v = (0, X, Y, Z)로 놓고 다음처럼 변환합니다.

v_local = conjugate(q) × v_world × q

쉽게 말하면, 차량을 world에서 돌린 회전을 벡터에 역순으로 적용해 차량 좌표로 되돌리는 것입니다. 회전 순서는 결과에 영향을 줍니다. 쿼터니안으로 벡터를 회전하는 q × v × q⁻¹ 방식은 University of Maryland 강의 자료 (https://www.cs.umd.edu/class/fall2022/cmsc828X/LEC/quaternion.pdf), 좌표계 변환에 역회전을 적용하는 원리는 Illinois 대학 로보틱스 자료 (https://motion.cs.illinois.edu/RoboticSystems/CoordinateTransformations.html)에서 설명합니다.

HUD에는 world의 위쪽 (0, 1, 0)을 차량 기준으로 변환한 값도 표시하도록 했습니다. 차체가 수평이면 대략 (0, 1, 0)이고, 차가 기울면 X/Z 성분이 생깁니다.

주의할 점은 저장소가 yaw·pitch·roll의 정확한 회전 조합 순서와 부호 규칙을 정의하지 않는다는 것입니다. 그래서 yaw(Y) × pitch(X) × roll(Z)를 가정으로 명시했습니다. 컴파일은 성공했지만, 실제 게임에서 평지·pitch만 있는 자세·roll만 있는 자세를 비교해 부호와 순서를 확인해야 합니다.

저작자표시 (새창열림)