四元数，欧拉角和旋转矩阵之间的互相转换

摘自：http://www.oschina.net/code/snippet_876234_20178

公式是从网上搜索的，代码自己编写的，适用任意支持C的平台，原本是为了6轴融合模拟9轴效果用的。 

标签： <无>

代码片段(1)[全屏查看所有代码]

1. [文件] 四元数与欧拉角以及矩阵之间的转换.c ~ 4KB     下载(93)     

?

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179

//公式都是网上搜罗的，下面这些经过简单的测试，确认可用。   //ps: x,y,z,w 分别是四元素的四个值。稍微修改下就可以用。    // 由旋转矩阵创建四元数    inline CQuaternion( const _Matrix4& m)    {     float tr, s, q[4];     int i, j, k;          int nxt[3] = {1, 2, 0 };     // 计算矩阵轨迹     tr = m._11 + m._22 + m._33;          // 检查矩阵轨迹是正还是负     if (tr>0.0f)     {      s = sqrt (tr + 1.0f);      this ->w = s / 2.0f;      s = 0.5f / s;      this ->x = (m._23 - m._32) * s;      this ->y = (m._31 - m._13) * s;      this ->z = (m._12 - m._21) * s;     }     else     {      // 轨迹是负      // 寻找m11 m22 m33中的最大分量      i = 0;      if (m.m[1][1]>m.m[0][0]) i = 1;      if (m.m[2][2]>m.m[i][i]) i = 2;      j = nxt[i];      k = nxt[j];            s = sqrt ((m.m[i][i] - (m.m[j][j] + m.m[k][k])) + 1.0f);      q[i] = s * 0.5f;      if ( s!= 0.0f) s = 0.5f / s;      q[3] = (m.m[j][k] - m.m[k][j]) * s;      q[j] = (m.m[i][j] - m.m[j][i]) * s;      q[k] = (m.m[i][k] - m.m[k][i]) * s;      this ->x = q[0];      this ->y = q[1];      this ->z = q[2];      this ->w = q[3];     }    };      // 由欧拉角创建四元数    inline CQuaternion( const _Vector3& angle)    {     float cx = cos (angle.x/2);     float sx = sin (angle.x/2);     float cy = cos (angle.y/2);     float sy = sin (angle.y/2);     float cz = cos (angle.z/2);     float sz = sin (angle.z/2);       this ->w = cx*cy*cz + sx*sy*sz;     this ->x = sx*cy*cz - cx*sy*sz;     this ->y = cx*sy*cz + sx*cy*sz;     this ->z = cx*cy*sz - sx*sy*cz;    };      // 给定角度和轴创建四元数    inline CQuaternion(_Vector3 anxi, const float & angle)    {     CVector3 t;     t.x = anxi.x;     t.y = anxi.y;     t.z = anxi.z;     t.Normalize();     float cosa = cos (angle);     float sina = sin (angle);     this ->w = cosa;     this ->x = sina * t.x;     this ->y = sina * t.y;     this ->z = sina * t.z;    };   // 由旋转四元数推导出矩阵    inline CMatrix4 GetMatrixLH()    {     CMatrix4 ret;     float xx = x*x;     float yy = y*y;     float zz = z*z;     float xy = x*y;     float wz = w*z;     float wy = w*y;     float xz = x*z;     float yz = y*z;     float wx = w*x;       ret._11 = 1.0f-2*(yy+zz);     ret._12 = 2*(xy-wz);     ret._13 = 2*(wy+xz);     ret._14 = 0.0f;       ret._21 = 2*(xy+wz);     ret._22 = 1.0f-2*(xx+zz);     ret._23 = 2*(yz-wx);     ret._24 = 0.0f;       ret._31 = 2*(xy-wy);     ret._32 = 2*(yz+wx);     ret._33 = 1.0f-2*(xx+yy);     ret._34 = 0.0f;       ret._41 = 0.0f;     ret._42 = 0.0f;     ret._43 = 0.0f;     ret._44 = 1.0f;       return ret;    };    inline CMatrix4 GetMatrixRH()    {     CMatrix4 ret;     float xx = x*x;     float yy = y*y;     float zz = z*z;     float xy = x*y;     float wz = -w*z;     float wy = -w*y;     float xz = x*z;     float yz = y*z;     float wx = -w*x;       ret._11 = 1.0f-2*(yy+zz);     ret._12 = 2*(xy-wz);     ret._13 = 2*(wy+xz);     ret._14 = 0.0f;       ret._21 = 2*(xy+wz);     ret._22 = 1.0f-2*(xx+zz);     ret._23 = 2*(yz-wx);     ret._24 = 0.0f;       ret._31 = 2*(xy-wy);     ret._32 = 2*(yz+wx);     ret._33 = 1.0f-2*(xx+yy);     ret._34 = 0.0f;       ret._41 = 0.0f;     ret._42 = 0.0f;     ret._43 = 0.0f;     ret._44 = 1.0f;       return ret;    };      // 由四元数返回欧拉角（主要是这个dx api里没有提供）    inline CVector3 GetEulerAngle()    {     CVector3 ret;       float test = y*z + x*w;     if (test > 0.4999f)     {      ret.z = 2.0f * atan2 (y, w);      ret.y = PIOver2;      ret.x = 0.0f;      return ret;     }     if (test < -0.4999f)     {      ret.z = 2.0f * atan2 (y, w);      ret.y = -PIOver2;      ret.x = 0.0f;      return ret;     }     float sqx = x * x;     float sqy = y * y;     float sqz = z * z;     ret.z = atan2 (2.0f * z * w - 2.0f * y * x, 1.0f - 2.0f * sqz - 2.0f * sqx);     ret.y = asin (2.0f * test);     ret.x = atan2 (2.0f * y * w - 2.0f * z * x, 1.0f - 2.0f * sqy - 2.0f * sqx);            return ret;    };