/[gr2lib]/trunk/gr2lib/core/coretypes/implementation/Quaterion.cs
ViewVC logotype

Contents of /trunk/gr2lib/core/coretypes/implementation/Quaterion.cs

Parent Directory Parent Directory | Revision Log Revision Log


Revision 187 - (show annotations) (download)
Sat Jul 24 04:16:43 2010 UTC (9 years, 3 months ago) by william
File size: 18773 byte(s)
continue to add xml documentation

1 using System;
2 using System.Collections.Generic;
3 using System.Linq;
4 using System.Text;
5 using System.Runtime.InteropServices;
6
7 namespace gr2lib.core.coretypes.implementation
8 {
9 /// <summary>
10 /// IQuaternion interface
11 /// </summary>
12 public interface IQuaternion
13 {
14 /// <summary>
15 /// v
16 /// </summary>
17 Vector3 v { get; set; }
18 /// <summary>
19 /// n
20 /// </summary>
21 float n { get; set; }
22 }
23
24 /// <summary>
25 /// Quaternion class
26 /// </summary>
27 [StructLayout(LayoutKind.Sequential)]
28 public struct Quaternion : IQuaternion
29 {
30 /// <summary>
31 /// Identity
32 /// </summary>
33 public static readonly Quaternion Identity;
34 private Vector3 _v;
35 private float _n;
36 /// <summary>
37 /// v
38 /// </summary>
39 public Vector3 v { get { return _v; } set { _v = value; } }
40 /// <summary>
41 /// n
42 /// </summary>
43 public float n { get { return _n; } set { _n = value; } }
44
45 /// <summary>
46 /// Default constructor from x,y,z,n
47 /// </summary>
48 /// <param name="_x"></param>
49 /// <param name="_y"></param>
50 /// <param name="_z"></param>
51 /// <param name="_n"></param>
52 public Quaternion(float _x, float _y, float _z, float _n)
53 {
54 this._n = _n;
55 this._v = new Vector3(_x, _y, _z);
56 }
57 internal Quaternion(native.Quaternion q)
58 {
59 this._n = q.W;
60 this._v = new Vector3(q.X, q.Y, q.Z);
61 }
62
63 /// <summary>
64 /// Magnitude
65 /// </summary>
66 public float Magnitude
67 {
68 get
69 {
70 return (float)Math.Sqrt((double)((((this.n * this.n) + (this.v.x * this.v.x)) + (this.v.y * this.v.y)) + (this.v.z * this.v.z)));
71 }
72 }
73 /// <summary>
74 /// Vector
75 /// </summary>
76 public Vector3 Vector
77 {
78 get
79 {
80 return this.v;
81 }
82 }
83 /// <summary>
84 /// Scaler
85 /// </summary>
86 public float Scaler
87 {
88 get
89 {
90 return this.n;
91 }
92 }
93 /// <summary>
94 /// ToMatrix33()
95 /// </summary>
96 /// <returns></returns>
97 public Matrix33 ToMatrix33()
98 {
99 Matrix33 matrix = new Matrix33();
100 matrix.SetRotation(this);
101 return matrix;
102 }
103 /// <summary>
104 /// ToMatrix33EA()
105 /// </summary>
106 /// <returns></returns>
107 public Matrix33 ToMatrix33EA()
108 {
109 Matrix33 matrix = Matrix33.CopyMatrix(Matrix33.Identity);
110 matrix.right.x = (((this.n * this.n) + (this.v.x * this.v.x)) - (this.v.y * this.v.y)) - (this.v.z * this.v.z);
111 matrix.right.y = ((2f * this.v.x) * this.v.y) - ((2f * this.v.z) * this.n);
112 matrix.right.z = ((2f * this.v.x) * this.v.z) + ((2f * this.v.y) * this.n);
113 matrix.up.x = ((2f * this.v.x) * this.v.y) + ((2f * this.v.z) * this.n);
114 matrix.up.y = (((this.n * this.n) - (this.v.x * this.v.x)) + (this.v.y * this.v.y)) - (this.v.z * this.v.z);
115 matrix.up.z = ((2f * this.v.y) * this.v.z) - ((2f * this.v.x) * this.n);
116 matrix.at.x = ((2f * this.v.z) * this.v.x) - ((2f * this.v.y) * this.n);
117 matrix.at.y = ((2f * this.v.z) * this.v.y) + ((2f * this.v.x) * this.n);
118 matrix.at.z = (((this.n * this.n) - (this.v.x * this.v.x)) - (this.v.y * this.v.y)) + (this.v.z * this.v.z);
119 return matrix;
120 }
121
122 /// <summary>
123 /// ToMatrix44()
124 /// </summary>
125 /// <returns></returns>
126 public Matrix44 ToMatrix44()
127 {
128 Matrix44 matrix = Matrix44.CopyMatrix(Matrix44.Identity);
129 matrix.SetRotation(this);
130 return matrix;
131 }
132 /// <summary>
133 /// ToMatrix44EA()
134 /// </summary>
135 /// <returns></returns>
136 public Matrix44 ToMatrix44EA()
137 {
138 Matrix44 matrix = Matrix44.CopyMatrix(Matrix44.Identity);
139 matrix.right.x = (((this.n * this.n) + (this.v.x * this.v.x)) - (this.v.y * this.v.y)) - (this.v.z * this.v.z);
140 matrix.right.y = ((2f * this.v.x) * this.v.y) - ((2f * this.v.z) * this.n);
141 matrix.right.z = ((2f * this.v.x) * this.v.z) + ((2f * this.v.y) * this.n);
142 matrix.up.x = ((2f * this.v.x) * this.v.y) + ((2f * this.v.z) * this.n);
143 matrix.up.y = (((this.n * this.n) - (this.v.x * this.v.x)) + (this.v.y * this.v.y)) - (this.v.z * this.v.z);
144 matrix.up.z = ((2f * this.v.y) * this.v.z) - ((2f * this.v.x) * this.n);
145 matrix.at.x = ((2f * this.v.z) * this.v.x) - ((2f * this.v.y) * this.n);
146 matrix.at.y = ((2f * this.v.z) * this.v.y) + ((2f * this.v.x) * this.n);
147 matrix.at.z = (((this.n * this.n) - (this.v.x * this.v.x)) - (this.v.y * this.v.y)) + (this.v.z * this.v.z);
148 return matrix;
149 }
150 /// <summary>
151 /// Norm()
152 /// </summary>
153 /// <returns></returns>
154 public float Norm()
155 {
156 return this.v.x * this.v.x + this.v.y * this.v.y + this.v.z * this.v.z + this.n * this.n;
157 }
158 /// <summary>
159 /// Normalize()
160 /// </summary>
161 /// <returns></returns>
162 public Quaternion Normalize()
163 {
164 float len = this.Magnitude;
165 if (Math.Abs(len) < 1E-05f)
166 {
167 this._v.x = this._v.y = this._v.z = this._n = 0f;
168 return this;
169 }
170 this._v.x /= len;
171 this._v.y /= len;
172 this._v.z /= len;
173 this._n /= len;
174 return this;
175 }
176 /// <summary>
177 /// Inverse()
178 /// </summary>
179 /// <returns></returns>
180 public Quaternion Inverse()
181 {
182 float l = this.Norm();
183 return new Quaternion(-this.v.x / l, -this.v.y / l, -this.v.z / l, this.n / l);
184 }
185 /// <summary>
186 /// operator ~(Quaternion a)
187 /// </summary>
188 /// <param name="a"></param>
189 /// <returns></returns>
190 public static Quaternion operator ~(Quaternion a)
191 {
192 return new Quaternion(-a.v.x, -a.v.y, -a.v.z, a.n);
193 }
194 /// <summary>
195 /// operator +(Quaternion a, Quaternion b)
196 /// </summary>
197 /// <param name="a"></param>
198 /// <param name="b"></param>
199 /// <returns></returns>
200 public static Quaternion operator +(Quaternion a, Quaternion b)
201 {
202 return new Quaternion(a.v.x + b.v.x, a.v.y + b.v.y, a.v.z + b.v.z, a.n + b.n);
203 }
204 /// <summary>
205 /// operator -(Quaternion a, Quaternion b)
206 /// </summary>
207 /// <param name="a"></param>
208 /// <param name="b"></param>
209 /// <returns></returns>
210 public static Quaternion operator -(Quaternion a, Quaternion b)
211 {
212 return new Quaternion(a.v.x - b.v.x, a.v.y - b.v.y, a.v.z - b.v.z, a.n - b.n);
213 }
214 /// <summary>
215 /// operator *(Quaternion a, Quaternion b)
216 /// </summary>
217 /// <param name="a"></param>
218 /// <param name="b"></param>
219 /// <returns></returns>
220 public static Quaternion operator *(Quaternion a, Quaternion b)
221 {
222 return new Quaternion((((a.n * b.v.x) + (a.v.x * b.n)) + (a.v.y * b.v.z)) - (a.v.z * b.v.y), (((a.n * b.v.y) + (a.v.y * b.n)) + (a.v.z * b.v.x)) - (a.v.x * b.v.z), (((a.n * b.v.z) + (a.v.z * b.n)) + (a.v.x * b.v.y)) - (a.v.y * b.v.x), (((a.n * b.n) - (a.v.x * b.v.x)) - (a.v.y * b.v.y)) - (a.v.z * b.v.z));
223 }
224 /// <summary>
225 /// operator *(Quaternion a, float s)
226 /// </summary>
227 /// <param name="a"></param>
228 /// <param name="s"></param>
229 /// <returns></returns>
230 public static Quaternion operator *(Quaternion a, float s)
231 {
232 return new Quaternion(a.v.x * s, a.v.y * s, a.v.z * s, a.n * s);
233 }
234 /// <summary>
235 /// operator *(float s, Quaternion a)
236 /// </summary>
237 /// <param name="s"></param>
238 /// <param name="a"></param>
239 /// <returns></returns>
240 public static Quaternion operator *(float s, Quaternion a)
241 {
242 return new Quaternion(a.v.x * s, a.v.y * s, a.v.z * s, a.n * s);
243 }
244 /// <summary>
245 /// operator *(Quaternion q, Vector3 v)
246 /// </summary>
247 /// <param name="q"></param>
248 /// <param name="v"></param>
249 /// <returns></returns>
250 public static Quaternion operator *(Quaternion q, Vector3 v)
251 {
252 return new Quaternion(((q.n * v.x) + (q.v.y * v.z)) - (q.v.z * v.y), ((q.n * v.y) + (q.v.z * v.x)) - (q.v.x * v.z), ((q.n * v.z) + (q.v.x * v.y)) - (q.v.y * v.x), -(((q.v.x * v.x) + (q.v.y * v.y)) + (q.v.z * v.z)));
253 }
254 /// <summary>
255 /// operator *(Vector3 v, Quaternion q)
256 /// </summary>
257 /// <param name="v"></param>
258 /// <param name="q"></param>
259 /// <returns></returns>
260 public static Quaternion operator *(Vector3 v, Quaternion q)
261 {
262 return new Quaternion(((q.n * v.x) + (q.v.z * v.y)) - (q.v.y * v.z), ((q.n * v.y) + (q.v.x * v.z)) - (q.v.z * v.x), ((q.n * v.z) + (q.v.y * v.x)) - (q.v.x * v.y), -(((q.v.x * v.x) + (q.v.y * v.y)) + (q.v.z * v.z)));
263 }
264 /// <summary>
265 /// operator /(Quaternion q, float s)
266 /// </summary>
267 /// <param name="q"></param>
268 /// <param name="s"></param>
269 /// <returns></returns>
270 public static Quaternion operator /(Quaternion q, float s)
271 {
272 return new Quaternion(q.v.x / s, q.v.y / s, q.v.z / s, q.n / s);
273 }
274 /// <summary>
275 /// operator /(float s, Quaternion q)
276 /// </summary>
277 /// <param name="s"></param>
278 /// <param name="q"></param>
279 /// <returns></returns>
280 public static Quaternion operator /(float s, Quaternion q)
281 {
282 return new Quaternion(q.v.x / s, q.v.y / s, q.v.z / s, q.n / s);
283 }
284 /// <summary>
285 /// GetAngle(Quaternion q)
286 /// </summary>
287 /// <param name="q"></param>
288 /// <returns></returns>
289 public static float GetAngle(Quaternion q)
290 {
291 return (float)(2.0 * Math.Acos((double)q.n));
292 }
293 /// <summary>
294 /// GetAxis(Quaternion q)
295 /// </summary>
296 /// <param name="q"></param>
297 /// <returns></returns>
298 public static Vector3 GetAxis(Quaternion q)
299 {
300 Vector3 v = q.v;
301 float num = v.Length();
302 if (num <= 0.0001f)
303 {
304 return new Vector3(0f, 0f, 0f);
305 }
306 return (Vector3)(v / num);
307 }
308 /// <summary>
309 /// Rotate(Quaternion q1, Quaternion q2)
310 /// </summary>
311 /// <param name="q1"></param>
312 /// <param name="q2"></param>
313 /// <returns></returns>
314 public static Quaternion Rotate(Quaternion q1, Quaternion q2)
315 {
316 return ((q1 * q2) * ~q1);
317 }
318 /// <summary>
319 /// VRotate(Quaternion q, Vector3 v)
320 /// </summary>
321 /// <param name="q"></param>
322 /// <param name="v"></param>
323 /// <returns></returns>
324 public static Vector3 VRotate(Quaternion q, Vector3 v)
325 {
326 Quaternion quaternion = (q * v) * ~q;
327 return quaternion.v;
328 }
329 /// <summary>
330 /// MakeFromEulerAngles(float x, float y, float z)
331 /// </summary>
332 /// <param name="x"></param>
333 /// <param name="y"></param>
334 /// <param name="z"></param>
335 /// <returns></returns>
336 public static Quaternion MakeFromEulerAngles(float x, float y, float z)
337 {
338 double dX = x;//bank | tilt | roll | applied 3rd
339 double dY = y;//heading | azimuth | yaw | applied 1st
340 double dZ = z;//attitude | elevation | pitch | applied 2nd
341 double c2 = Math.Cos(0.5 * dZ);
342 double c1 = Math.Cos(0.5 * dY);
343 double c3 = Math.Cos(0.5 * dX);
344 double s2 = Math.Sin(0.5 * dZ);
345 double s1 = Math.Sin(0.5 * dY);
346 double s3 = Math.Sin(0.5 * dX);
347 double c1c2 = c2 * c1;
348 double s1s2 = s2 * s1;
349 double s1c2 = c2 * s1;
350 double c1s2 = s2 * c1;
351 return new Quaternion((float)((c1c2 * s3) + (s1s2 * c3)), (float)((s1c2 * c3) + (c1s2 * s3)), (float)((c1s2 * c3) - (s1c2 * s3)), (float)((c1c2 * c3) - (s1s2 * s3)));
352 }
353 /// <summary>
354 /// MakeFromEulerAngles(Vector3 rv)
355 /// </summary>
356 /// <param name="rv"></param>
357 /// <returns></returns>
358 public static Quaternion MakeFromEulerAngles(Vector3 rv)
359 {
360 return MakeFromEulerAngles(rv.x, rv.y, rv.z);
361 }
362 /// <summary>
363 /// ToEulerAngles()
364 /// </summary>
365 /// <returns></returns>
366 public Vector3 ToEulerAngles()
367 {
368 Vector3 euler = new Vector3();
369 double pole = this.v.x * this.v.y + this.v.z * this.n;
370 if (pole == 0.5 || pole == -0.5)
371 {
372 euler.x = 0f;
373 euler.y = (float)(4 * pole * Math.Atan2(this.v.x, this.n));
374 euler.z = (float)(pole * Math.PI);
375 }
376 else
377 {
378 euler.x = (float)Math.Atan2(2 * (this.v.x * this.n - this.v.y * this.v.z), 1 - 2 * (this.v.x * this.v.x + this.v.z * this.v.z));
379 euler.y = (float)Math.Atan2(2 * (this.v.y * this.n - this.v.x * this.v.z), 1 - 2 * (this.v.y * this.v.y + this.v.z * this.v.z));
380 euler.z = (float)Math.Asin(2 * pole);
381 }
382 return euler;
383 }
384 /// <summary>
385 /// MakeFromRotationAxis(Vector3 axis, float angle)
386 /// </summary>
387 /// <param name="axis"></param>
388 /// <param name="angle"></param>
389 /// <returns></returns>
390 public static Quaternion MakeFromRotationAxis(Vector3 axis, float angle)
391 {
392 return new Quaternion(axis.x, axis.y, axis.z, (float)(0.5 * Math.Cos(angle)));
393 }
394 /// <summary>
395 /// MakeFromRotationAxis(Vector4 raxis)
396 /// </summary>
397 /// <param name="raxis"></param>
398 /// <returns></returns>
399 public static Quaternion MakeFromRotationAxis(Vector4 raxis)
400 {
401 return new Quaternion(raxis.x, raxis.y, raxis.z, (float)(0.5 * Math.Cos(raxis.w)));
402 }
403 /// <summary>
404 /// MakeFromForwardVector(Vector3 forward)
405 /// </summary>
406 /// <param name="forward"></param>
407 /// <returns></returns>
408 public static Quaternion MakeFromForwardVector(Vector3 forward)
409 {
410 Matrix44 matrix = new Matrix44();
411 forward = forward.Normalize();
412 Vector3 b = Vector3.CrossProduct(Vector3.UnitY, forward);
413 Vector3 v = Vector3.CrossProduct(forward, b);
414 matrix.right = new Vector4(b);
415 matrix.up = new Vector4(v);
416 matrix.at = new Vector4(forward);
417 matrix.pos = new Vector4(0f, 0f, 0f, 1f);
418 return MakeFromMatrix44(matrix);
419 }
420 /// <summary>
421 /// MakeFromMatrix44(Matrix44 xf)
422 /// </summary>
423 /// <param name="xf"></param>
424 /// <returns></returns>
425 public static Quaternion MakeFromMatrix44(Matrix44 xf)
426 {
427 float num6;
428 float qx = 0f;
429 float qy = 0f;
430 float qz = 0f;
431 float qw = 0f;
432 float trace = (xf.right.x + xf.up.y) + xf.at.z;
433 if (trace >= 0f)
434 {
435 num6 = (float)Math.Sqrt((double)(trace + 1f));
436 qw = 0.5f * num6;
437 num6 = 0.5f / num6;
438 qx = (xf.up.z - xf.at.y) * num6;
439 qy = (xf.at.x - xf.right.z) * num6;
440 qz = (xf.right.y - xf.up.x) * num6;
441 }
442 else
443 {
444 int maxDiagIndex = 0;
445 if (xf.up.y > xf.right.x)
446 {
447 maxDiagIndex = 1;
448 if (xf.at.z > xf.up.y)
449 {
450 maxDiagIndex = 2;
451 }
452 }
453 else if (xf.at.z > xf.right.x)
454 {
455 maxDiagIndex = 2;
456 }
457 switch (maxDiagIndex)
458 {
459 case 0:
460 num6 = (float)Math.Sqrt((double)((xf.right.x - (xf.up.y + xf.at.z)) + 1f));
461 qx = 0.5f * num6;
462 num6 = 0.5f / num6;
463 qy = (xf.up.x + xf.right.y) * num6;
464 qz = (xf.right.z + xf.at.x) * num6;
465 qw = (xf.up.z - xf.at.y) * num6;
466 break;
467
468 case 1:
469 num6 = (float)Math.Sqrt((double)((xf.up.y - (xf.at.z + xf.right.x)) + 1f));
470 qy = 0.5f * num6;
471 num6 = 0.5f / num6;
472 qz = (xf.at.y + xf.up.z) * num6;
473 qx = (xf.up.x + xf.right.y) * num6;
474 qw = (xf.at.x - xf.right.z) * num6;
475 break;
476
477 case 2:
478 num6 = (float)Math.Sqrt((double)((xf.at.z - (xf.right.x + xf.up.y)) + 1f));
479 qz = 0.5f * num6;
480 num6 = 0.5f / num6;
481 qx = (xf.right.z + xf.at.x) * num6;
482 qy = (xf.at.y + xf.up.z) * num6;
483 qw = (xf.right.y - xf.up.x) * num6;
484 break;
485 }
486 }
487 return new Quaternion(qx, qy, qz, qw);
488 }
489
490 static Quaternion()
491 {
492 Identity = new Quaternion(0f, 0f, 0f, 1f);
493 }
494 }
495 }

  ViewVC Help
Powered by ViewVC 1.1.22