converting rotation quaternion into matrix, and vice versa

3차원 공간에서 오브젝트의 아핀변환을 위해서는 4x4 메트릭스를 사용한다. 그런데 회전변환의 경우 오일러회전에 특정한 상황에서 짐벌락이라는 문제가 발생할 수 있어 사원수를 사용하여 회전을 정의한다고 설명했었다. 오브젝트 애니메이션 등 회전요소의 보간이 필요한 상황에선 사원수의 사용은 특히나 중요했었다.

일반적인 경우에 오브젝트의 회전을 사원수로 표현하여 관리하면 대체로 편리하다. 하지만 다른 변환 메트릭스와 결합하여 최종 변환 메트릭스를 계산하여야 하므로 쉬운 선택은 아니었다. 다행히 회전사원수와 회전변환 메트릭스 사이에는 서로 변환 가능하다. 이 특징이 사원수의 활용을 더욱 빛나게 한다.

사원수를 [math]\mathbf{q} = \left( q_x + q_y + q_z + q_w \right)[/math]와 같이 나타낼 때 사원수는 다음 행렬로 변환된다.

[math!]\mathbf{q} \rightarrow \begin{bmatrix} 1 - 2(q_y^2 + q_z^2) & 2(q_xq_y - q_zq_w) & 2(q_xq_z + q_yq_w)\\ 2(q_xq_y + q_zq_w) & 1-2(q_z^2 + q_x^2) & 2(q_yq_z - q_xq_w) \\ 2(q_xq_z - q_yq_w) & 2(q_yq_z + q_xq_w) & 1-2(q_x^2 + q_y^2) \end{bmatrix}[/math!]

이는 [math]\mathbf{q}\mathbf{p}\mathbf{q}^*[/math]를 계산하여 확인할 수 있다. (수식으로 표현하기엔 용기가 부족했다..)

혹은 아래와 같이 두 4x4 메트릭스의 곱으로 표현하기도 한다.

[math!] \begin{bmatrix} q_w & q_z & -q_y & q_x \\ -q_z & q_w & q_x & q_y  \\ q_y & -q_x & q_w & q_z \\ -q_x & -q_y & -q_z & q_w \end{bmatrix}\begin{bmatrix} q_w & q_z & -q_y & -q_x \\ -q_z & q_w & q_x & -q_y  \\ q_y & -q_x & q_w & -q_z \\ q_x & q_y & q_z & q_w \end{bmatrix} [/math!]

암튼 이런형태 생각해내는 사람들 대단하다. 바로 위 형태는 구현이 깔끔하지만 성능이 그리 좋지는 못해서 주로 첫번째 3x3의 형태로 표현한다.

 

// [math]q_s = q_w[/math]이다.

const Matrix3 Quaternion::ToMatrix3(bool isNormalized){

real xx, xy, xz, xs;
real yy, yz, ys;
real zz, zs;

if (isNormalized){

xx = x*x;    xy = x*y;    xz = x*z;    xs = x*s;

yy = y*y;    yz = y*z;    ys = y*s;

zz = z*z;    zs = z*s;

}

else{

Quaternion normq = this->Normal();
xx = normq.x*normq.x;    xy = normq.x*normq.y;    xz = normq.x*normq.z;    xs = normq.x*normq.s;

yy = normq.y*normq.y;    yz = normq.y*normq.z;    ys = normq.y*normq.s;

zz = normq.z*normq.z;    zs = normq.z*normq.s;

}

return Matrix3(

1.0f - 2.0f*(yy + zz), 2.0f*(xy - zs), 2.0f*(xz + ys),
2.0f*(xy + zs), 1.0f - 2.0f*(xx + zz), 2.0f*(yz - xs),
2.0f*(xz - ys), 2.0f*(yz + xs), 1.0f - 2.0f*(xx + yy) );

}

 

 

반대로 3x3의 회전행렬로 부터 사원수로의 변환은 회전행렬이 특수직교행렬(special orthogonal matrix)이기 때문에 다음의 식이 성립한다.

[math!]\begin{aligned}q_w &= \sqrt{ (1 + m_{00} + m_{11} + m_{22} ) } /2 \\ q_x&= (m_{21} - m_{12})/( 4 q_w) \\ q_y &= (m_{02} - m_{20})/( 4 q_w) \\ q_z &= (m_{10} - m_{01})/( 4 q_w) \\ \end{aligned}[/math!]

하지만 [math]1 + m_{00} + m_{11} + m_{22}[/math]가 음의 값을 가지면 안되기 때문에 구현시 이를 보완해야한다. 참고한 페이지에서는 너무 작은 값에 대한 연산도 문제가 될 수 있기 때문에 이 또한 고려하여야 한다고 설명하고 있다. (아래 구현은 참고2 페이지의 코드)

 

// convert from matrix 3D (only for rotation matrix)

void Quaternion::FromMatrix3(const Matrix3& mat){

real tr = mat.m[0][0] + mat.m[1][1] + mat.m[2][2];

if (tr > 0) {

real S = math::sqrt(tr + 1.f) * 2.f; // S=4*qw 
s = 0.25f * S;
x = (mat.m[2][1] - mat.m[1][2]) / S;
y = (mat.m[0][2] - mat.m[2][0]) / S;
z = (mat.m[1][0] - mat.m[0][1]) / S;

}
else if ((mat.m[0][0] > mat.m[1][1]) && (mat.m[0][0] > mat.m[2][2])) {

real S = math::sqrt(1.f + mat.m[0][0] - mat.m[1][1] - mat.m[2][2]) * 2.f; // S=4*qx 
s = (mat.m[2][1] - mat.m[1][2]) / S;
x = 0.25f * S;
y = (mat.m[0][1] + mat.m[1][0]) / S;
z = (mat.m[0][2] + mat.m[2][0]) / S;

}
else if (mat.m[1][1] > mat.m[2][2]) {

real S = math::sqrt(1.f + mat.m[1][1] - mat.m[0][0] - mat.m[2][2]) * 2.f; // S=4*qy
s = (mat.m[0][2] - mat.m[2][0]) / S;
x = (mat.m[0][1] + mat.m[1][0]) / S;
y = 0.25f * S;
z = (mat.m[1][2] + mat.m[2][1]) / S;

}
else {

real S = math::sqrt(1.f + mat.m[2][2] - mat.m[0][0] - mat.m[1][1]) * 2.f; // S=4*qz
s = (mat.m[1][0] - mat.m[0][1]) / S;
x = (mat.m[0][2] + mat.m[2][0]) / S;
y = (mat.m[1][2] + mat.m[2][1]) / S;
z = 0.25f * S;

}

}

 

float을 기본 자료형으로 사원수 -> 회전행렬 -> 사원수로의 재 변환을 테스트해봤는데, 소수점 6번째 자리까지의 정확성을 보였다. 아무래도 sqrt()함수와 소수 나누기연산으로 인해 그리 정확한 값을 표현할 수는 없나보다.

 

참고1: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/

참고2: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/