從零開始游戲開發——2.3 歐拉角與四元數-有解無憂

　　在2.2矩陣章節講到可以將坐標空間的基向量使用矩陣來表示，從而可以用3x3的矩陣來表達物體的方位，并且使用矩陣表示方位也是圖形API使用的形式，但3x3的旋轉矩陣需要存盤9個資料，相比較歐拉角和四元數，記憶體占用會比較大，并且表達并不直觀，

　　歐拉角可以使用3個數來表達方位，歐拉角使用Roll-Pitch-Yaw 或 Heading-Pitch-Bank的命名約定可以用來表示物體繞各個坐標軸的旋轉，在我們實作中，并不打算寫一個歐拉角的類，而是直接使用Vector3來存盤歐拉角繞x、y、z的旋轉，歐拉角在開發中對于程式除錯非方便，但歐拉角本身也存在一定的缺點，首先歐拉角有萬向鎖的問題，即我們先繞y軸進行水平方向的旋轉，再繞x軸進行垂直方向的旋轉，而這時旋轉了90度，則當繞z軸旋轉時，旋轉的方位仍然是在水平方向，在這種情況丟失了一個旋轉維度，另外一點就是歐拉角表示方位的方式不唯一，即繞x軸旋轉theta度和這個角度加上n * 360度表示的是同一個方位，這可能會對我們方向的插值造成困擾，

　　四元數使用4個數來表示繞軸v旋轉theta度的方位，在使用四元數表示方位時，類似于向量表示方向的單位向量，我們使用的是單位四元數，即四元數||q|| = 1，因此四元數中并不是直接存盤了旋轉角度theta和旋轉軸，而是以

q = [cos(θ/2) sin(θ/2)n_x sin(θ/2)n_y sin(θ/2)n_z]

這種形式來存盤資料，使用單位四元數時，他的四元數的逆等于四元數的共軛：

q^-1= q^*/||q|| = q^* = [w -v]

這樣就可以使用共軛來表示相反的方位角，四元數主要包含了下面的操作：

 1 template<typename T>
 2 class Quaternion
 3 {
 4 public: 
 5     Quaternion();
 6     Quaternion(T w, T x, T y, T z);
 7 
 8     void Identity();
 9     void Normalize();
10 
11     Vector3<T> ToEulerAngle();
12     Quaternion &FromEulerAngle(T x, T y, T z);
13 
14     Quaternion &MakeRotateByX(T theta);
15     Quaternion &MakeRotateByY(T theta);
16     Quaternion &MakeRotateByZ(T theta);
17     Quaternion &MakeRotateByAxis(const Vector3<T> &axis, T theta);
18     T GetRotationAngle() const;
19     Vector3<T> GetRotationAxis() const;
20 
21     Quaternion operator*(const Quaternion &q) const;
22     Vector3<T> operator*(const Vector3<T> &v) const;
23 
24     static Quaternion Slerp(const Quaternion &q0, const Quaternion &q1, float t);
25 
26     template <typename T1>
27     friend ostream &operator<<(ostream &out, const Quaternion<T1> &q);
28 private:
29     T w, x, y, z;
30 };

第11行~12行宣告了四元數與歐拉角之間的轉換，第14~17行則是以四元數的形式宣告繞各軸旋轉，第21~22行宣告了四元數和乘法操作，四元數之前的乘法與矩陣乘法類似，可以將多個旋轉操作連接為一個四元數，四元數與向量相乘則是對用來旋轉向量的，

第24行的Slerp操作是對四元數的球面插值，上面代碼的具體實作代碼如下：

  1 template <typename T>
  2 Quaternion<T>::Quaternion()
  3     :w(1), x(0), y(0), z(0)
  4 {
  5 }
  6 
  7 template <typename T>
  8 Quaternion<T>::Quaternion(T w, T x, T y, T z)
  9     :w(w), x(x), y(y), z(z)
 10 {
 11 }
 12 
 13 template <typename T>
 14 void Quaternion<T>::Identity()
 15 {
 16     w = static_cast<T>(1);
 17     x = y = z = static_cast<T>(0);
 18 }
 19 
 20 template <typename T>
 21 void Quaternion<T>::Normalize()
 22 {
 23     T mag = (T)sqrt(w * w + x * x + y * y + z * z);
 24 
 25     if (mag > 0)
 26     {
 27         T oneOverMag = (T)1 / mag;
 28         w *= oneOverMag;
 29         x *= oneOverMag;
 30         y *= oneOverMag;
 31         z *= oneOverMag;
 32     }
 33     else
 34     {
 35         assert(false);
 36         Identity();
 37     }
 38 }
 39 
 40 template <typename T>
 41 Vector3<T> Quaternion<T>::ToEulerAngle()
 42 {
 43     Vector3<T> euler;
 44     Quaternion &q = *this;
 45     T sp = (T)-2 * (q.y * q.z - q.w * q.x);
 46     if (fabs(sp) > (T)0.9999)
 47     {
 48         euler.y = PI_DIV_2 * sp;
 49         euler.x = atan2(-q.x + q.z + q.w * q.y, (T)0.5 - q.y * q.y - q.z * q.z);
 50         euler.z = 0;
 51     }
 52     else
 53     {
 54         euler.y = asin(sp);
 55         euler.x = atan2(q.x * q.z + q.w * q.y, (T)0.5 - q.x * q.x - q.y * q.y);
 56         euler.z = atan2(q.x * q.y + q.w * q.z, (T)0.5 - q.x * q.x - q.z * q.z);
 57     }
 58 
 59     return euler;
 60 }
 61 
 62 template <typename T>
 63 Quaternion<T> &Quaternion<T>::FromEulerAngle(T x, T y, T z)
 64 {
 65     T hOver2 = (T)0.5 * y;
 66     T pOver2 = (T)0.5 * x;
 67     T bOver2 = (T)0.5 * z;
 68 
 69     T sh, sp, sb;
 70     T ch, cp, cb;
 71 
 72     sh = sin(hOver2);
 73     ch = cos(hOver2);
 74     sp = sin(pOver2);
 75     cp = cos(pOver2);
 76     sb = sin(bOver2);
 77     cb = cos(bOver2);
 78 
 79     this->w = ch * cp * cb + sh * sp * sb;
 80     this->x = ch * sp * cb + sh * cp * sb;
 81     this->y = -ch * sp * sb + sh * cp * cb;
 82     this->z = -sh * sp * cb + ch * cp * sb;
 83 
 84     return *this;
 85 }
 86 
 87 template <typename T>
 88 Quaternion<T> Quaternion<T>::operator*(const Quaternion<T> &q) const
 89 {
 90     //右乘
 91     return Quaternion(w * q.w - x * q.x - y * q.y - z * q.z, 
 92         w * q.x + x * q.w + z * q.y - y * q.z, 
 93         w * q.y + y * q.w + x * q.z - z * q.x,
 94         w * q.z + z * q.w + y * q.x - x * q.y);
 95 }
 96 
 97 template <typename T>
 98 Vector3<T> Quaternion<T>::operator*(const Vector3<T> &v) const
 99 {
100     Quaternion vq(0, v.x, v.y, v.z);
101     Quaternion invQ(w, -x, -y, -z);
102     Quaternion result = invQ * vq * *this;
103 
104     return Vector3<T>(result.x, result.y, result.z);
105 }
106 
107 template <typename T>
108 Quaternion<T> &Quaternion<T>::MakeRotateByX(T theta)
109 {
110     T thetaOver2 = theta * (T)0.5;
111 
112     w = cos(thetaOver2);
113     x = sin(thetaOver2);
114     y = 0;
115     z = 0;
116     return *this;
117 }
118 
119 template <typename T>
120 Quaternion<T> &Quaternion<T>::MakeRotateByY(T theta)
121 {
122     T thetaOver2 = theta * (T)0.5;
123 
124     w = cos(thetaOver2);
125     x = 0;
126     y = sin(thetaOver2);
127     z = 0;
128     return *this;
129 }
130 template <typename T>
131 Quaternion<T> &Quaternion<T>::MakeRotateByZ(T theta)
132 {
133     T thetaOver2 = theta * (T)0.5;
134 
135     w = cos(thetaOver2);
136     x = 0;
137     y = 0;
138     z = sin(thetaOver2);
139     return *this;
140 }
141 template <typename T>
142 Quaternion<T> &Quaternion<T>::MakeRotateByAxis(const Vector3<T> &axis, T theta)
143 {
144     Vector3<T> axisNormallize = axis;
145     axisNormallize.Normalize();
146 
147     T thetaOver2 = theta * (T)0.5;
148     T sinThetaOver2 = sin(thetaOver2);
149 
150     w = cos(thetaOver2);
151     x = axisNormallize.x * sinThetaOver2;
152     y = axisNormallize.y * sinThetaOver2;
153     z = axisNormallize.z * sinThetaOver2;
154     return *this;
155 }
156 
157 template <typename T>
158 T Quaternion<T>::GetRotationAngle() const
159 {
160     T thetaOver2;
161     if (w <= (T)-1)
162         thetaOver2 = PI;
163     else if (w >= (T)1)
164         thetaOver2 = 0;
165     else
166         thetaOver2 = acos(w);
167     return thetaOver2 * (T)2;
168 }
169 
170 template <typename T>
171 Vector3<T> Quaternion<T>::GetRotationAxis() const
172 {
173     T sinThetaOver2Sq = (T)1 - w * w;
174     if (sinThetaOver2Sq <= 0)
175     {
176         return Vector3<T>(1, 0, 0);
177     }
178 
179     T oneOverSinThetaOver2 = (T)1 / sqrt(sinThetaOver2Sq);
180     return Vector3<T>(x * oneOverSinThetaOver2, y * oneOverSinThetaOver2, z * oneOverSinThetaOver2);
181 }
182 
183 template <typename T>
184 Quaternion<T> Quaternion<T>::Slerp(const Quaternion &q0, const Quaternion &q1, float t)
185 {
186     if (t <= 0) return q0;
187     if (t >= (T)1) return q1;
188 
189     T cosOmega = q0.w * q1.w + q0.x * q1.x + q0.y * q1.y + q0.z * q1.z;
190     T q1w = q1.w;
191     T q1x = q1.x;
192     T q1y = q1.y;
193     T q1z = q1.z;
194     if (cosOmega < 0)
195     {
196         q1w = -q1w;
197         q1x = -q1x;
198         q1y = -q1y;
199         q1z = -q1z;
200         cosOmega = -cosOmega;
201     }
202 
203     assert(cosOmega < 1);
204 
205     T k0, k1;
206     if (cosOmega > (T)0.9999)
207     {
208         k0 = (T)1 - t;
209         k1 = t;
210     }
211     else
212     {
213         T sinOmega = sqrt((T)1 - cosOmega * cosOmega);
214         T omega = atan2(sinOmega, cosOmega);
215         T oneOverSinOmega = (T)1 / sinOmega;
216         k0 = sin(((T)(1) - t) * omega) * oneOverSinOmega;
217         k1 = sin(t * omega) * oneOverSinOmega;
218     }
219 
220     return Quaternion(
221         k0 * q0.w + k1 * q1w,
222         k0 * q0.x + k1 * q1x, 
223         k0 * q0.y + k1 * q1y,
224         k0 * q0.z + k1 * q1z);
225 }
226 
227 template <typename T>
228 ostream &operator<<(ostream &out, const Quaternion<T> &q)
229 {
230     out << q.w << "," << q.x << "," << q.y << "," << q.z;
231     return out;
232 }

具體實作都比較簡單，這里重點講一下Slerp操作，先以向量為類，如下圖所示：

向量v_t= k₀v₀ + k₁v_1，這里只需要求出k₀和k₁即可，由以k₁v₁為斜邊的直角三角形可知：

sin(theta) = sin(t * theta) / k₁，

則：

k₁ = sin(t * theta) / sin(theta),

同理：

k₀ = sin((1-t) * theta) / sin(theta),

將向量的Slerp操作擴展到四元數即得到了四元數的球面插值操作：

Slerp(q₀, q₁, t) = k₁q₀ + k₁q₁

參考文獻：

3D數學基礎：圖形與游戲開發

本文來自博客園，作者：毅安，轉載請注明原文鏈接：https://www.cnblogs.com/primarycode/p/16438709.html

轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/499526.html

標籤：其他

上一篇：UE4 內功如何學習UE C++并查看藍圖節點的原始碼

下一篇：【前端面試】（九）JavaScript選擇排序