/[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 83 - (show annotations) (download)
Fri Jul 16 21:26:08 2010 UTC (10 years, 1 month ago) by william
File size: 12925 byte(s)
bulk commit --
** Enable Support for Skeletons & Bones

** A bug has found its way into the code
 -get an unhandled EHAccessViolation
 - I think the problem is in the Textures Class (or one of the sub-classes)

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 [StructLayout(LayoutKind.Sequential)]
10 public struct Quaternion
11 {
12 public static readonly Quaternion Identity;
13 public Vector3 v;
14 public float n;
15 public Quaternion(float _x, float _y, float _z, float _n)
16 {
17 this.n = _n;
18 this.v = new Vector3(_x, _y, _z);
19 }
20 internal Quaternion(native.Quaternion q)
21 {
22 this.n = q.W;
23 this.v = new Vector3(q.X, q.Y, q.Z);
24 }
25
26 public float Magnitude
27 {
28 get
29 {
30 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)));
31 }
32 }
33 public Vector3 Vector
34 {
35 get
36 {
37 return this.v;
38 }
39 }
40 public float Scaler
41 {
42 get
43 {
44 return this.n;
45 }
46 }
47
48 public Matrix33 ToMatrix33()
49 {
50 Matrix33 matrix = new Matrix33();
51 matrix.SetRotation(this);
52 return matrix;
53 }
54
55 public Matrix33 ToMatrix33EA()
56 {
57 Matrix33 matrix = Matrix33.CopyMatrix(Matrix33.Identity);
58 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);
59 matrix.right.y = ((2f * this.v.x) * this.v.y) - ((2f * this.v.z) * this.n);
60 matrix.right.z = ((2f * this.v.x) * this.v.z) + ((2f * this.v.y) * this.n);
61 matrix.up.x = ((2f * this.v.x) * this.v.y) + ((2f * this.v.z) * this.n);
62 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);
63 matrix.up.z = ((2f * this.v.y) * this.v.z) - ((2f * this.v.x) * this.n);
64 matrix.at.x = ((2f * this.v.z) * this.v.x) - ((2f * this.v.y) * this.n);
65 matrix.at.y = ((2f * this.v.z) * this.v.y) + ((2f * this.v.x) * this.n);
66 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);
67 return matrix;
68 }
69
70 public Matrix44 ToMatrix44()
71 {
72 Matrix44 matrix = Matrix44.CopyMatrix(Matrix44.Identity);
73 matrix.SetRotation(this);
74 return matrix;
75 }
76
77 public Matrix44 ToMatrix44EA()
78 {
79 Matrix44 matrix = Matrix44.CopyMatrix(Matrix44.Identity);
80 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);
81 matrix.right.y = ((2f * this.v.x) * this.v.y) - ((2f * this.v.z) * this.n);
82 matrix.right.z = ((2f * this.v.x) * this.v.z) + ((2f * this.v.y) * this.n);
83 matrix.up.x = ((2f * this.v.x) * this.v.y) + ((2f * this.v.z) * this.n);
84 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);
85 matrix.up.z = ((2f * this.v.y) * this.v.z) - ((2f * this.v.x) * this.n);
86 matrix.at.x = ((2f * this.v.z) * this.v.x) - ((2f * this.v.y) * this.n);
87 matrix.at.y = ((2f * this.v.z) * this.v.y) + ((2f * this.v.x) * this.n);
88 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);
89 return matrix;
90 }
91
92 public float Norm()
93 {
94 return this.v.x * this.v.x + this.v.y * this.v.y + this.v.z * this.v.z + this.n * this.n;
95 }
96
97 public Quaternion Normalize()
98 {
99 float len = this.Magnitude;
100 if (Math.Abs(len) < 1E-05f)
101 {
102 this.v.x = this.v.y = this.v.z = this.n = 0f;
103 return this;
104 }
105 this.v.x /= len;
106 this.v.y /= len;
107 this.v.z /= len;
108 this.n /= len;
109 return this;
110 }
111
112 public Quaternion Inverse()
113 {
114 float l = this.Norm();
115 return new Quaternion(-this.v.x / l, -this.v.y / l, -this.v.z / l, this.n / l);
116 }
117
118 public static Quaternion operator ~(Quaternion a)
119 {
120 return new Quaternion(-a.v.x, -a.v.y, -a.v.z, a.n);
121 }
122
123 public static Quaternion operator +(Quaternion a, Quaternion b)
124 {
125 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);
126 }
127
128 public static Quaternion operator -(Quaternion a, Quaternion b)
129 {
130 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);
131 }
132
133 public static Quaternion operator *(Quaternion a, Quaternion b)
134 {
135 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));
136 }
137
138 public static Quaternion operator *(Quaternion a, float s)
139 {
140 return new Quaternion(a.v.x * s, a.v.y * s, a.v.z * s, a.n * s);
141 }
142
143 public static Quaternion operator *(float s, Quaternion a)
144 {
145 return new Quaternion(a.v.x * s, a.v.y * s, a.v.z * s, a.n * s);
146 }
147
148 public static Quaternion operator *(Quaternion q, Vector3 v)
149 {
150 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)));
151 }
152
153 public static Quaternion operator *(Vector3 v, Quaternion q)
154 {
155 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)));
156 }
157
158 public static Quaternion operator /(Quaternion q, float s)
159 {
160 return new Quaternion(q.v.x / s, q.v.y / s, q.v.z / s, q.n / s);
161 }
162
163 public static Quaternion operator /(float s, Quaternion q)
164 {
165 return new Quaternion(q.v.x / s, q.v.y / s, q.v.z / s, q.n / s);
166 }
167
168 public static float GetAngle(Quaternion q)
169 {
170 return (float)(2.0 * Math.Acos((double)q.n));
171 }
172
173 public static Vector3 GetAxis(Quaternion q)
174 {
175 Vector3 v = q.v;
176 float num = v.Length();
177 if (num <= 0.0001f)
178 {
179 return new Vector3(0f, 0f, 0f);
180 }
181 return (Vector3)(v / num);
182 }
183
184 public static Quaternion Rotate(Quaternion q1, Quaternion q2)
185 {
186 return ((q1 * q2) * ~q1);
187 }
188
189 public static Vector3 VRotate(Quaternion q, Vector3 v)
190 {
191 Quaternion quaternion = (q * v) * ~q;
192 return quaternion.v;
193 }
194
195 public static Quaternion MakeFromEulerAngles(float x, float y, float z)
196 {
197 double dX = x;//bank | tilt | roll | applied 3rd
198 double dY = y;//heading | azimuth | yaw | applied 1st
199 double dZ = z;//attitude | elevation | pitch | applied 2nd
200 double c2 = Math.Cos(0.5 * dZ);
201 double c1 = Math.Cos(0.5 * dY);
202 double c3 = Math.Cos(0.5 * dX);
203 double s2 = Math.Sin(0.5 * dZ);
204 double s1 = Math.Sin(0.5 * dY);
205 double s3 = Math.Sin(0.5 * dX);
206 double c1c2 = c2 * c1;
207 double s1s2 = s2 * s1;
208 double s1c2 = c2 * s1;
209 double c1s2 = s2 * c1;
210 return new Quaternion((float)((c1c2 * s3) + (s1s2 * c3)), (float)((s1c2 * c3) + (c1s2 * s3)), (float)((c1s2 * c3) - (s1c2 * s3)), (float)((c1c2 * c3) - (s1s2 * s3)));
211 }
212
213 public static Quaternion MakeFromEulerAngles(Vector3 rv)
214 {
215 return MakeFromEulerAngles(rv.x, rv.y, rv.z);
216 }
217
218 public Vector3 ToEulerAngles()
219 {
220 Vector3 euler = new Vector3();
221 double pole = this.v.x * this.v.y + this.v.z * this.n;
222 if (pole == 0.5 || pole == -0.5)
223 {
224 euler.x = 0f;
225 euler.y = (float)(4 * pole * Math.Atan2(this.v.x, this.n));
226 euler.z = (float)(pole * Math.PI);
227 }
228 else
229 {
230 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));
231 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));
232 euler.z = (float)Math.Asin(2 * pole);
233 }
234 return euler;
235 }
236
237 public static Quaternion MakeFromRotationAxis(Vector3 axis, float angle)
238 {
239 return new Quaternion(axis.x, axis.y, axis.z, (float)(0.5 * Math.Cos(angle)));
240 }
241
242 public static Quaternion MakeFromRotationAxis(Vector4 raxis)
243 {
244 return new Quaternion(raxis.x, raxis.y, raxis.z, (float)(0.5 * Math.Cos(raxis.w)));
245 }
246
247 public static Quaternion MakeFromForwardVector(Vector3 forward)
248 {
249 Matrix44 matrix = new Matrix44();
250 forward = forward.Normalize();
251 Vector3 b = Vector3.CrossProduct(Vector3.UnitY, forward);
252 Vector3 v = Vector3.CrossProduct(forward, b);
253 matrix.right = new Vector4(b);
254 matrix.up = new Vector4(v);
255 matrix.at = new Vector4(forward);
256 matrix.pos = new Vector4(0f, 0f, 0f, 1f);
257 return MakeFromMatrix44(matrix);
258 }
259
260 public static Quaternion MakeFromMatrix44(Matrix44 xf)
261 {
262 float num6;
263 float qx = 0f;
264 float qy = 0f;
265 float qz = 0f;
266 float qw = 0f;
267 float trace = (xf.right.x + xf.up.y) + xf.at.z;
268 if (trace >= 0f)
269 {
270 num6 = (float)Math.Sqrt((double)(trace + 1f));
271 qw = 0.5f * num6;
272 num6 = 0.5f / num6;
273 qx = (xf.up.z - xf.at.y) * num6;
274 qy = (xf.at.x - xf.right.z) * num6;
275 qz = (xf.right.y - xf.up.x) * num6;
276 }
277 else
278 {
279 int maxDiagIndex = 0;
280 if (xf.up.y > xf.right.x)
281 {
282 maxDiagIndex = 1;
283 if (xf.at.z > xf.up.y)
284 {
285 maxDiagIndex = 2;
286 }
287 }
288 else if (xf.at.z > xf.right.x)
289 {
290 maxDiagIndex = 2;
291 }
292 switch (maxDiagIndex)
293 {
294 case 0:
295 num6 = (float)Math.Sqrt((double)((xf.right.x - (xf.up.y + xf.at.z)) + 1f));
296 qx = 0.5f * num6;
297 num6 = 0.5f / num6;
298 qy = (xf.up.x + xf.right.y) * num6;
299 qz = (xf.right.z + xf.at.x) * num6;
300 qw = (xf.up.z - xf.at.y) * num6;
301 break;
302
303 case 1:
304 num6 = (float)Math.Sqrt((double)((xf.up.y - (xf.at.z + xf.right.x)) + 1f));
305 qy = 0.5f * num6;
306 num6 = 0.5f / num6;
307 qz = (xf.at.y + xf.up.z) * num6;
308 qx = (xf.up.x + xf.right.y) * num6;
309 qw = (xf.at.x - xf.right.z) * num6;
310 break;
311
312 case 2:
313 num6 = (float)Math.Sqrt((double)((xf.at.z - (xf.right.x + xf.up.y)) + 1f));
314 qz = 0.5f * num6;
315 num6 = 0.5f / num6;
316 qx = (xf.right.z + xf.at.x) * num6;
317 qy = (xf.at.y + xf.up.z) * num6;
318 qw = (xf.right.y - xf.up.x) * num6;
319 break;
320 }
321 }
322 return new Quaternion(qx, qy, qz, qw);
323 }
324
325 static Quaternion()
326 {
327 Identity = new Quaternion(0f, 0f, 0f, 1f);
328 }
329 }
330 }

  ViewVC Help
Powered by ViewVC 1.1.22