四元数

  1 using System;
  2 
  3 namespace WorldWind
  4 {
  5     /// <summary>
  6     /// 四元数
  7     /// </summary>
  8     public struct Quaternion4d
  9     {
 10         public double X;
 11         public double Y;
 12         public double Z;
 13         public double W;
 14 
 15         public Quaternion4d(double x, double y, double z, double w)
 16         {
 17             X = x;
 18             Y = y;
 19             Z = z;
 20             W = w;
 21         }
 22 
 23         public override int GetHashCode() 
 24         {
 25             return (int)(X / Y / Z / W);
 26         }
 27 
 28         public override bool Equals(object obj)
 29         {
 30             if(obj is Quaternion4d)
 31             {
 32                 Quaternion4d q = (Quaternion4d)obj;
 33                 return q == this;
 34             }
 35             else
 36                 return false;
 37         }
 38         /// <summary>
 39         /// 欧拉角转四元数
 40         /// </summary>
 41         /// <param name="yaw"></param>
 42         /// <param name="pitch"></param>
 43         /// <param name="roll"></param>
 44         /// <returns></returns>
 45         public static Quaternion4d EulerToQuaternion(double yaw, double pitch, double roll)
 46         {
 47             double cy = Math.Cos(yaw * 0.5);
 48             double cp = Math.Cos(pitch * 0.5);
 49             double cr = Math.Cos(roll * 0.5);
 50             double sy = Math.Sin(yaw * 0.5);
 51             double sp = Math.Sin(pitch * 0.5);
 52             double sr = Math.Sin(roll * 0.5);
 53 
 54             double qw = cy*cp*cr + sy*sp*sr;
 55             double qx = sy*cp*cr - cy*sp*sr;
 56             double qy = cy*sp*cr + sy*cp*sr;
 57             double qz = cy*cp*sr - sy*sp*cr;
 58 
 59             return new Quaternion4d(qx, qy, qz, qw);
 60         }
 61 
 62         /// <summary>
 63         /// Transforms a rotation in quaternion form to a set of Euler angles 
 64         /// 欧拉角
 65         /// </summary>
 66         /// <returns>The rotation transformed to Euler angles, X=Yaw, Y=Pitch, Z=Roll (radians)</returns>
 67         public static Point3d QuaternionToEuler(Quaternion4d q)
 68         {
 69             double q0 = q.W;
 70             double q1 = q.X;
 71             double q2 = q.Y;
 72             double q3 = q.Z;
 73 
 74             double x = Math.Atan2( 2 * (q2*q3 + q0*q1), (q0*q0 - q1*q1 - q2*q2 + q3*q3));
 75             double y = Math.Asin( -2 * (q1*q3 - q0*q2));
 76             double z = Math.Atan2( 2 * (q1*q2 + q0*q3), (q0*q0 + q1*q1 - q2*q2 - q3*q3));
 77 
 78             return new Point3d(x, y, z);
 79         }
 80 
 81         public static Quaternion4d operator+(Quaternion4d a, Quaternion4d b)
 82         {
 83             return new Quaternion4d(a.X + b.X, a.Y + b.Y, a.Z + b.Z, a.W + b.W);
 84         }
 85 
 86         public static Quaternion4d operator-(Quaternion4d a, Quaternion4d b)
 87         {
 88             return new Quaternion4d( a.X - b.X, a.Y - b.Y, a.Z - b.Z, a.W - b.W);
 89         }
 90 
 91         public static Quaternion4d operator*(Quaternion4d a, Quaternion4d b)
 92         {
 93             return new Quaternion4d(
 94                     a.W * b.X + a.X * b.W + a.Y * b.Z - a.Z * b.Y,
 95                     a.W * b.Y + a.Y * b.W + a.Z * b.X - a.X * b.Z,
 96                     a.W * b.Z + a.Z * b.W + a.X * b.Y - a.Y * b.X,
 97                     a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z);
 98         }
 99 
100         public static Quaternion4d operator*(double s, Quaternion4d q)
101         {
102             return new Quaternion4d(s * q.X, s * q.Y, s * q.Z, s * q.W);
103         }
104 
105         public static Quaternion4d operator*(Quaternion4d q, double s)
106         {
107             return new Quaternion4d(s * q.X, s * q.Y, s * q.Z, s * q.W);
108         }
109 
110         // equivalent to multiplying by the quaternion (0, v)
111         public static Quaternion4d operator*(Point3d v, Quaternion4d q)
112         {
113             return new Quaternion4d(
114                      v.X * q.W + v.Y * q.Z - v.Z * q.Y,
115                      v.Y * q.W + v.Z * q.X - v.X * q.Z,
116                      v.Z * q.W + v.X * q.Y - v.Y * q.X,
117                     -v.X * q.X - v.Y * q.Y - v.Z * q.Z);
118         }
119 
120         public static Quaternion4d operator/(Quaternion4d q, double s)
121         {
122             return q * (1 / s);
123         }
124 
125         // conjugate operator
126         public Quaternion4d Conjugate()
127         {
128             return new Quaternion4d( -X, -Y, -Z, W);
129         }
130 
131         public static double Norm2(Quaternion4d q)
132         {
133             return q.X * q.X + q.Y * q.Y + q.Z * q.Z + q.W * q.W;
134         }
135 
136         public static double Abs(Quaternion4d q)
137         {
138             return Math.Sqrt(Norm2(q));
139         }
140 
141         public static Quaternion4d operator/(Quaternion4d a, Quaternion4d b)
142         {
143             return a * (b.Conjugate() / Abs(b));
144         }
145 
146         public static bool operator==(Quaternion4d a, Quaternion4d b)
147         {
148             return a.X == b.X && a.Y == b.Y && a.Z == b.Z && a.W == b.W;
149         }
150 
151         public static bool operator!=(Quaternion4d a, Quaternion4d b)
152         {
153             return a.X != b.X || a.Y != b.Y || a.Z != b.Z || a.W != b.W;
154         }
155 
156         public static double Dot(Quaternion4d a, Quaternion4d b)
157         {
158             return a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W;
159         }
160 
161         public void Normalize()
162         {
163             double L = Length();
164 
165             X = X / L;
166             Y = Y / L;
167             Z = Z / L;
168             W = W / L;
169         }
170 
171         public double Length()
172         {
173             return Math.Sqrt(X * X + Y * Y +
174                 Z * Z + W * W);
175         }
176         
177         public static Quaternion4d Slerp(Quaternion4d q0, Quaternion4d q1, double t)
178         {
179             double cosom = q0.X * q1.X + q0.Y * q1.Y + q0.Z * q1.Z + q0.W * q1.W;
180             double tmp0, tmp1, tmp2, tmp3;
181             if (cosom < 0.0)
182             {
183                 cosom = -cosom;
184                 tmp0 = -q1.X;
185                 tmp1 = -q1.Y;
186                 tmp2 = -q1.Z;
187                 tmp3 = -q1.W;
188             }
189             else
190             {
191                 tmp0 = q1.X;
192                 tmp1 = q1.Y;
193                 tmp2 = q1.Z;
194                 tmp3 = q1.W;
195             }
196 
197             /* calc coeffs */
198             double scale0, scale1;
199 
200             if ((1.0 - cosom) > double.Epsilon)
201             {
202                 // standard case (slerp)
203                 double omega =  Math.Acos (cosom);
204                 double sinom = Math.Sin (omega);
205                 scale0 =  Math.Sin ((1.0 - t) * omega) / sinom;
206                 scale1 =  Math.Sin (t * omega) / sinom;
207             }
208             else
209             {
210                 /* just lerp */
211                 scale0 = 1.0 - t;
212                 scale1 = t;
213             }
214 
215             Quaternion4d q = new Quaternion4d();
216 
217             q.X = scale0 * q0.X + scale1 * tmp0;
218             q.Y = scale0 * q0.Y + scale1 * tmp1;
219             q.Z = scale0 * q0.Z + scale1 * tmp2;
220             q.W = scale0 * q0.W + scale1 * tmp3;
221 
222             return q;
223         }
224 
225         public Quaternion4d Ln() 
226         {
227             return Ln(this);
228         }
229 
230         public static Quaternion4d Ln(Quaternion4d q)
231         {
232             double t = 0;
233  
234             double s = Math.Sqrt(q.X * q.X + q.Y * q.Y + q.Z * q.Z); 
235             double om = Math.Atan2(s, q.W); 
236             
237             if (Math.Abs(s) < double.Epsilon) 
238                 t = 0.0f; 
239             else 
240                 t = om/s; 
241             
242             q.X = q.X * t;
243             q.Y = q.Y * t;
244             q.Z = q.Z * t;
245             q.W = 0.0f;
246 
247             return q;
248         }
249 
250         //the below functions have not been certified to work properly
251         public static Quaternion4d Exp(Quaternion4d q)
252         {
253             double sinom;
254             double om = Math.Sqrt(q.X * q.X + q.Y * q.Y + q.Z * q.Z);
255             
256             if (Math.Abs(om) < double.Epsilon) 
257                 sinom = 1.0; 
258             else 
259                 sinom = Math.Sin(om)/om; 
260             
261             q.X = q.X * sinom;
262             q.Y = q.Y * sinom;
263             q.Z = q.Z * sinom;
264             q.W = Math.Cos(om);
265             
266             return q;
267         }
268         
269         public Quaternion4d Exp()
270         {
271             return Ln(this);
272         }
273         
274         public static Quaternion4d Squad(
275             Quaternion4d q1,
276             Quaternion4d a,
277             Quaternion4d b,
278             Quaternion4d c,
279             double t)
280         {
281             return Slerp(
282                 Slerp(q1, c, t), Slerp(a, b, t), 2 * t * (1.0 - t));
283         }
284 
285         public static void SquadSetup(
286             ref Quaternion4d outA,
287             ref Quaternion4d outB,
288             ref Quaternion4d outC,
289             Quaternion4d q0,
290             Quaternion4d q1,
291             Quaternion4d q2,
292             Quaternion4d q3)
293         {
294             q0 = q0 + q1;
295             q0.Normalize();
296 
297             q2 = q2 + q1;
298             q2.Normalize();
299 
300             q3 = q3 + q1;
301             q3.Normalize();
302             
303             q1.Normalize();
304 
305             outA = q1 * Exp(-0.25 * (Ln(Exp(q1) * q2) + Ln(Exp(q1) * q0)));
306             outB = q2 * Exp(-0.25 * (Ln(Exp(q2) * q3) + Ln(Exp(q2) * q1)));
307             outC = q2;
308 
309         }
310     }
311 }