http://www.azure.com.cn
四元数与同构矩阵之间可以互相转换.
一般四元数的计算矩阵运算如下:
[ w2+x2-y2-z2 , 2xy-2wz , 2xz+2wy ]
[ 2xy+2wz , w2-x2-y2-z2 , 2yz-2wx ]
[ 2xz-2wy , 2yz+2wx , w2-x2-y2-z2 ]
但是对于范化的四元数, 可以简化上述的矩阵变换:
[ 1-2y2-2z2 , 2xy-2wz , 2xz+2wy ]
[ 2xy+2wz , 1-2x2-2z2 , 2yz-2wx ]
[ 2xz-2wy , 2yz+2wx , 1-2x2-2y2 ]
矩阵转换四元数代码如下:
void MatToQuat( float m[4][4], Quat* quat )
{
float tr, s, q[4];
int i, j, k;
int nxt[3] = {1, 2, 0 };
//计算矩阵轨迹
tr = m[0][0] + m[1][1] + m[2][2];
//检查矩阵轨迹是正还是负
if(tr>0.0)
{
s = sqrt(tr + 1.0);
quat->w = s / 2.0;
s = 0.5 / s;
quat->x = (m[1][2] - m[2][1]) * s;
quat->y = (m[2][0] - m[0][2]) * s;
quat->z = (m[0][1] - m[1][0]) * s;
}
else
{
//轨迹是负
i = 0;
if(m[1][1]>m[0][0]) i = 1;
if(m[2][2]>m[i][i]) i = 2;
j = nxt[i];
k = nxt[j];
s = sqrt((m[i][i] - (m[j][j] + m[k][k])) + 1.0);
q[i] = s * 0.5;
if( s!= 0.0) s = 0.5 / s;
q[3] = (m[j][k] - m[k][j]) * s;
q[j] = (m[i][j] - m[j][i]) * s;
q[k] = (m[i][k] - m[k][i]) * s;
quat->x = q[0];
quat->y = q[1];
quat->z = q[2];
quat->w = q[3];
}
}
{
float tr, s, q[4];
int i, j, k;
int nxt[3] = {1, 2, 0 };
//计算矩阵轨迹
tr = m[0][0] + m[1][1] + m[2][2];
//检查矩阵轨迹是正还是负
if(tr>0.0)
{
s = sqrt(tr + 1.0);
quat->w = s / 2.0;
s = 0.5 / s;
quat->x = (m[1][2] - m[2][1]) * s;
quat->y = (m[2][0] - m[0][2]) * s;
quat->z = (m[0][1] - m[1][0]) * s;
}
else
{
//轨迹是负
i = 0;
if(m[1][1]>m[0][0]) i = 1;
if(m[2][2]>m[i][i]) i = 2;
j = nxt[i];
k = nxt[j];
s = sqrt((m[i][i] - (m[j][j] + m[k][k])) + 1.0);
q[i] = s * 0.5;
if( s!= 0.0) s = 0.5 / s;
q[3] = (m[j][k] - m[k][j]) * s;
q[j] = (m[i][j] - m[j][i]) * s;
q[k] = (m[i][k] - m[k][i]) * s;
quat->x = q[0];
quat->y = q[1];
quat->z = q[2];
quat->w = q[3];
}
}