转自:http://www.oschina.net/code/snippet_876234_20178
001 | //公式都是网上搜罗的,下面这些经过简单的测试,确认可用。 |
002 | |
003 | //ps: x,y,z,w 分别是四元素的四个值。稍微修改下就可以用。 |
004 | // 由旋转矩阵创建四元数 |
005 | inline CQuaternion( const _Matrix4& m) |
006 | { |
007 | float tr, s, q[4]; |
008 | int i, j, k; |
009 | |
010 | int nxt[3] = {1, 2, 0 }; |
011 | // 计算矩阵轨迹 |
012 | tr = m._11 + m._22 + m._33; |
013 | |
014 | // 检查矩阵轨迹是正还是负 |
015 | if (tr>0.0f) |
016 | { |
017 | s = sqrt (tr + 1.0f); |
018 | this ->w = s / 2.0f; |
019 | s = 0.5f / s; |
020 | this ->x = (m._23 - m._32) * s; |
021 | this ->y = (m._31 - m._13) * s; |
022 | this ->z = (m._12 - m._21) * s; |
023 | } |
024 | else |
025 | { |
026 | // 轨迹是负 |
027 | // 寻找m11 m22 m33中的最大分量 |
028 | i = 0; |
029 | if (m.m[1][1]>m.m[0][0]) i = 1; |
030 | if (m.m[2][2]>m.m[i][i]) i = 2; |
031 | j = nxt[i]; |
032 | k = nxt[j]; |
033 | |
034 | s = sqrt ((m.m[i][i] - (m.m[j][j] + m.m[k][k])) + 1.0f); |
035 | q[i] = s * 0.5f; |
036 | if ( s!= 0.0f) s = 0.5f / s; |
037 | q[3] = (m.m[j][k] - m.m[k][j]) * s; |
038 | q[j] = (m.m[i][j] - m.m[j][i]) * s; |
039 | q[k] = (m.m[i][k] - m.m[k][i]) * s; |
040 | this ->x = q[0]; |
041 | this ->y = q[1]; |
042 | this ->z = q[2]; |
043 | this ->w = q[3]; |
044 | } |
045 | }; |
046 | |
047 | // 由欧拉角创建四元数 |
048 | inline CQuaternion( const _Vector3& angle) |
049 | { |
050 | float cx = cos (angle.x/2); |
051 | float sx = sin (angle.x/2); |
052 | float cy = cos (angle.y/2); |
053 | float sy = sin (angle.y/2); |
054 | float cz = cos (angle.z/2); |
055 | float sz = sin (angle.z/2); |
056 | |
057 | this ->w = cx*cy*cz + sx*sy*sz; |
058 | this ->x = sx*cy*cz - cx*sy*sz; |
059 | this ->y = cx*sy*cz + sx*cy*sz; |
060 | this ->z = cx*cy*sz - sx*sy*cz; |
061 | }; |
062 | |
063 | // 给定角度和轴创建四元数 |
064 | inline CQuaternion(_Vector3 anxi, const float & angle) |
065 | { |
066 | CVector3 t; |
067 | t.x = anxi.x; |
068 | t.y = anxi.y; |
069 | t.z = anxi.z; |
070 | t.Normalize(); |
071 | float cosa = cos (angle); |
072 | float sina = sin (angle); |
073 | this ->w = cosa; |
074 | this ->x = sina * t.x; |
075 | this ->y = sina * t.y; |
076 | this ->z = sina * t.z; |
077 | }; |
078 | |
079 | // 由旋转四元数推导出矩阵 |
080 | inline CMatrix4 GetMatrixLH() |
081 | { |
082 | CMatrix4 ret; |
083 | float xx = x*x; |
084 | float yy = y*y; |
085 | float zz = z*z; |
086 | float xy = x*y; |
087 | float wz = w*z; |
088 | float wy = w*y; |
089 | float xz = x*z; |
090 | float yz = y*z; |
091 | float wx = w*x; |
092 | |
093 | ret._11 = 1.0f-2*(yy+zz); |
094 | ret._12 = 2*(xy-wz); |
095 | ret._13 = 2*(wy+xz); |
096 | ret._14 = 0.0f; |
097 | |
098 | ret._21 = 2*(xy+wz); |
099 | ret._22 = 1.0f-2*(xx+zz); |
100 | ret._23 = 2*(yz-wx); |
101 | ret._24 = 0.0f; |
102 | |
103 | ret._31 = 2*(xy-wy); |
104 | ret._32 = 2*(yz+wx); |
105 | ret._33 = 1.0f-2*(xx+yy); |
106 | ret._34 = 0.0f; |
107 | |
108 | ret._41 = 0.0f; |
109 | ret._42 = 0.0f; |
110 | ret._43 = 0.0f; |
111 | ret._44 = 1.0f; |
112 | |
113 | return ret; |
114 | }; |
115 | inline CMatrix4 GetMatrixRH() |
116 | { |
117 | CMatrix4 ret; |
118 | float xx = x*x; |
119 | float yy = y*y; |
120 | float zz = z*z; |
121 | float xy = x*y; |
122 | float wz = -w*z; |
123 | float wy = -w*y; |
124 | float xz = x*z; |
125 | float yz = y*z; |
126 | float wx = -w*x; |
127 | |
128 | ret._11 = 1.0f-2*(yy+zz); |
129 | ret._12 = 2*(xy-wz); |
130 | ret._13 = 2*(wy+xz); |
131 | ret._14 = 0.0f; |
132 | |
133 | ret._21 = 2*(xy+wz); |
134 | ret._22 = 1.0f-2*(xx+zz); |
135 | ret._23 = 2*(yz-wx); |
136 | ret._24 = 0.0f; |
137 | |
138 | ret._31 = 2*(xy-wy); |
139 | ret._32 = 2*(yz+wx); |
140 | ret._33 = 1.0f-2*(xx+yy); |
141 | ret._34 = 0.0f; |
142 | |
143 | ret._41 = 0.0f; |
144 | ret._42 = 0.0f; |
145 | ret._43 = 0.0f; |
146 | ret._44 = 1.0f; |
147 | |
148 | return ret; |
149 | }; |
150 | |
151 | // 由四元数返回欧拉角(主要是这个dx api里没有提供) |
152 | inline CVector3 GetEulerAngle() |
153 | { |
154 | CVector3 ret; |
155 | |
156 | float test = y*z + x*w; |
157 | if (test > 0.4999f) |
158 | { |
159 | ret.z = 2.0f * atan2 (y, w); |
160 | ret.y = PIOver2; |
161 | ret.x = 0.0f; |
162 | return ret; |
163 | } |
164 | if (test < -0.4999f) |
165 | { |
166 | ret.z = 2.0f * atan2 (y, w); |
167 | ret.y = -PIOver2; |
168 | ret.x = 0.0f; |
169 | return ret; |
170 | } |
171 | float sqx = x * x; |
172 | float sqy = y * y; |
173 | float sqz = z * z; |
174 | ret.z = atan2 (2.0f * z * w - 2.0f * y * x, 1.0f - 2.0f * sqz - 2.0f * sqx); |
175 | ret.y = asin (2.0f * test); |
176 | ret.x = atan2 (2.0f * y * w - 2.0f * z * x, 1.0f - 2.0f * sqy - 2.0f * sqx); |
177 | |
178 | return ret; |
179 | }; |