鸣鸟不飞21 -24网盘:四元数和旋转矩阵互相转换

来源：百度文库编辑：九乡新闻网时间：2024/04/29 16:02:04

四元数的用途和好处我就不多说了。
之前在网上找旋转矩阵和四元数相互转换的代码，找了几个都不大对劲，正反算算不过来，最后还是从osg源码里贴出来的这个，应该没什么问题。
这里给一个链接，Matrix and Quaternion FAQ http://www.flipcode.com/documents/matrfaq.html

以下是源文件：

#include
#include
using namespace std;

typedef double ValType;

struct Quat;
struct Matrix;

struct Quat {
ValType _v[4];//x, y, z, w

/// Length of the quaternion = sqrt( vec . vec )
ValType length() const
{
    return sqrt( _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] + _v[3]*_v[3]);
}

/// Length of the quaternion = vec . vec
ValType length2() const
{
    return _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] + _v[3]*_v[3];
}
};

struct Matrix {
ValType _mat[3][3];
};

#define QX q._v[0]
#define QY q._v[1]
#define QZ q._v[2]
#define QW q._v[3]

void Quat2Matrix(const Quat& q, Matrix& m)
{
    double length2 = q.length2();
    if (fabs(length2) <= std::numeric_limits::min())
    {
        m._mat[0][0] = 0.0; m._mat[1][0] = 0.0; m._mat[2][0] = 0.0;
        m._mat[0][1] = 0.0; m._mat[1][1] = 0.0; m._mat[2][1] = 0.0;
        m._mat[0][2] = 0.0; m._mat[1][2] = 0.0; m._mat[2][2] = 0.0;
    }
    else
    {
        double rlength2;
        // normalize quat if required.
        // We can avoid the expensive sqrt in this case since all 'coefficients' below are products of two q components.
        // That is a square of a square root, so it is possible to avoid that
        if (length2 != 1.0)
        {
            rlength2 = 2.0/length2;
        }
        else
        {
            rlength2 = 2.0;
        }

        // Source: Gamasutra, Rotating Objects Using Quaternions
        //
        //http://www.gamasutra.com/features/19980703/quaternions_01.htm

        double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

        // calculate coefficients
        x2 = rlength2*QX;
        y2 = rlength2*QY;
        z2 = rlength2*QZ;

        xx = QX * x2;
        xy = QX * y2;
        xz = QX * z2;

        yy = QY * y2;
        yz = QY * z2;
        zz = QZ * z2;

        wx = QW * x2;
        wy = QW * y2;
        wz = QW * z2;

        // Note. Gamasutra gets the matrix assignments inverted, resulting
        // in left-handed rotations, which is contrary to OpenGL and OSG's
        // methodology. The matrix assignment has been altered in the next
        // few lines of code to do the right thing.
        // Don Burns - Oct 13, 2001
        m._mat[0][0] = 1.0 - (yy + zz);
        m._mat[1][0] = xy - wz;
        m._mat[2][0] = xz + wy;


        m._mat[0][1] = xy + wz;
        m._mat[1][1] = 1.0 - (xx + zz);
        m._mat[2][1] = yz - wx;

        m._mat[0][2] = xz - wy;
        m._mat[1][2] = yz + wx;
        m._mat[2][2] = 1.0 - (xx + yy);
    }
}

void Matrix2Quat(const Matrix& m, Quat& q)
{
    ValType s;
    ValType tq[4];
    int    i, j;

    // Use tq to store the largest trace
    tq[0] = 1 + m._mat[0][0]+m._mat[1][1]+m._mat[2][2];
    tq[1] = 1 + m._mat[0][0]-m._mat[1][1]-m._mat[2][2];
    tq[2] = 1 - m._mat[0][0]+m._mat[1][1]-m._mat[2][2];
    tq[3] = 1 - m._mat[0][0]-m._mat[1][1]+m._mat[2][2];

    // Find the maximum (could also use stacked if's later)
    j = 0;
    for(i=1;i<4;i++) j = (tq[i]>tq[j])? i : j;

    // check the diagonal
    if (j==0)
    {
        /* perform instant calculation */
        QW = tq[0];
        QX = m._mat[1][2]-m._mat[2][1];
        QY = m._mat[2][0]-m._mat[0][2];
        QZ = m._mat[0][1]-m._mat[1][0];
    }
    else if (j==1)
    {
        QW = m._mat[1][2]-m._mat[2][1];
        QX = tq[1];
        QY = m._mat[0][1]+m._mat[1][0];
        QZ = m._mat[2][0]+m._mat[0][2];
    }
    else if (j==2)
    {
        QW = m._mat[2][0]-m._mat[0][2];
        QX = m._mat[0][1]+m._mat[1][0];
        QY = tq[2];
        QZ = m._mat[1][2]+m._mat[2][1];
    }
    else /* if (j==3) */
    {
        QW = m._mat[0][1]-m._mat[1][0];
        QX = m._mat[2][0]+m._mat[0][2];
        QY = m._mat[1][2]+m._mat[2][1];
        QZ = tq[3];
    }

    s = sqrt(0.25/tq[j]);
    QW *= s;
    QX *= s;
    QY *= s;
    QZ *= s;
}

void printMatrix(const Matrix& r, string name)
{
cout<<"RotMat "<cout<<"\t"<cout<<"\t"<cout<<"\t"<cout<}

void printQuat(const Quat& q, string name)
{
cout<<"Quat "<cout<<"\t"<cout<}

int main()
{
ValType phi, omiga, kappa;

phi = 1.32148229302237 ; omiga = 0.626224465189316 ; kappa = -1.4092143985971;

    ValType a1,a2,a3,b1,b2,b3,c1,c2,c3;

    a1 = cos(phi)*cos(kappa) - sin(phi)*sin(omiga)*sin(kappa);
    a2 = -cos(phi)*sin(kappa) - sin(phi)*sin(omiga)*cos(kappa);
    a3 = -sin(phi)*cos(omiga);

    b1 = cos(omiga)*sin(kappa);
    b2 = cos(omiga)*cos(kappa);
    b3 = -sin(omiga);

    c1 = sin(phi)*cos(kappa) + cos(phi)*sin(omiga)*sin(kappa);
    c2 = -sin(phi)*sin(kappa) + cos(phi)*sin(omiga)*cos(kappa);
    c3 = cos(phi)*cos(omiga);

    Matrix r;
    r._mat[0][0] = a1;
r._mat[0][1] = a2;
r._mat[0][2] = a3;

r._mat[1][0] = b1;
r._mat[1][1] = b2;
r._mat[1][2] = b3;

r._mat[2][0] = c1;
r._mat[2][1] = c2;
r._mat[2][2] = c3;

printMatrix(r, "r");

     //////////////////////////////////////////////////////////

Quat q;
Matrix2Quat(r, q);

printQuat(q, "q");

Matrix _r;
Quat2Matrix(q, _r);

printMatrix(_r, "_r");

system("pause");
return 0;
}

四元数和旋转矩阵互相转换旋转矩阵、欧拉角、四元数比较旋转矩阵和彩票组号2--中华网彩票频道矩阵旋转入门巧用胆拖式旋转矩阵法旋转矩阵与3D Myeclipse的web工程和Eclipse互相转换1 公历、农历互相转换函数双色球：旋转矩阵10＋4缩水玩法双色球---技巧：旋转矩阵10＋4缩水玩法0 双色球---技巧：旋转矩阵10＋4缩水玩法 Word,PDF, PPT,TXT之间互相转换 Excel表格word文档完美互相转换 Excel表格word文档完美互相转换双色球---旋转矩阵10+4的复式投注法完全攻略 3D万能4码\5码\6码\7码组合(利用旋转矩阵原理) Excel表格word文档完美互相转换-20110806 李敬泽：大陆和台湾：互相阅读爱情需要互相理解和尊重传球三要素是旋转、高度和落点第二单元:对称、平移和旋转很简单实用的繁体与简体互相转换的代码关联词和句式转换练习希尔伯特矩阵