diff options
Diffstat (limited to '')
-rw-r--r-- | libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs | 431 |
1 files changed, 0 insertions, 431 deletions
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs deleted file mode 100644 index b6d9d34..0000000 --- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs +++ /dev/null | |||
@@ -1,431 +0,0 @@ | |||
1 | #region License | ||
2 | /* | ||
3 | MIT License | ||
4 | Copyright © 2006 The Mono.Xna Team | ||
5 | |||
6 | All rights reserved. | ||
7 | |||
8 | Permission is hereby granted, free of charge, to any person obtaining a copy | ||
9 | of this software and associated documentation files (the "Software"), to deal | ||
10 | in the Software without restriction, including without limitation the rights | ||
11 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
12 | copies of the Software, and to permit persons to whom the Software is | ||
13 | furnished to do so, subject to the following conditions: | ||
14 | |||
15 | The above copyright notice and this permission notice shall be included in all | ||
16 | copies or substantial portions of the Software. | ||
17 | |||
18 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
19 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
20 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
21 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
22 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
23 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE | ||
24 | SOFTWARE. | ||
25 | */ | ||
26 | #endregion License | ||
27 | |||
28 | using System; | ||
29 | using System.ComponentModel; | ||
30 | |||
31 | namespace MonoXnaCompactMaths | ||
32 | { | ||
33 | [Serializable] | ||
34 | //[TypeConverter(typeof(QuaternionConverter))] | ||
35 | public struct Quaternion : IEquatable<Quaternion> | ||
36 | { | ||
37 | public float X; | ||
38 | public float Y; | ||
39 | public float Z; | ||
40 | public float W; | ||
41 | static Quaternion identity = new Quaternion(0, 0, 0, 1); | ||
42 | |||
43 | |||
44 | public Quaternion(float x, float y, float z, float w) | ||
45 | { | ||
46 | this.X = x; | ||
47 | this.Y = y; | ||
48 | this.Z = z; | ||
49 | this.W = w; | ||
50 | } | ||
51 | |||
52 | |||
53 | public Quaternion(Vector3 vectorPart, float scalarPart) | ||
54 | { | ||
55 | this.X = vectorPart.X; | ||
56 | this.Y = vectorPart.Y; | ||
57 | this.Z = vectorPart.Z; | ||
58 | this.W = scalarPart; | ||
59 | } | ||
60 | |||
61 | public static Quaternion Identity | ||
62 | { | ||
63 | get{ return identity; } | ||
64 | } | ||
65 | |||
66 | |||
67 | public static Quaternion Add(Quaternion quaternion1, Quaternion quaternion2) | ||
68 | { | ||
69 | throw new NotImplementedException(); | ||
70 | } | ||
71 | |||
72 | |||
73 | public static void Add(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result) | ||
74 | { | ||
75 | throw new NotImplementedException(); | ||
76 | } | ||
77 | |||
78 | |||
79 | public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle) | ||
80 | { | ||
81 | throw new NotImplementedException(); | ||
82 | } | ||
83 | |||
84 | |||
85 | public static void CreateFromAxisAngle(ref Vector3 axis, float angle, out Quaternion result) | ||
86 | { | ||
87 | throw new NotImplementedException(); | ||
88 | } | ||
89 | |||
90 | |||
91 | public static Quaternion CreateFromRotationMatrix(Matrix matrix) | ||
92 | { | ||
93 | float Omega2 = matrix.M44; | ||
94 | if (!isAprox(Omega2, 1f)) | ||
95 | { | ||
96 | //"Normalize" the Rotation matrix. Norma = M44 = Omega2 | ||
97 | matrix = matrix / Omega2; | ||
98 | } | ||
99 | //Deducted from: public static Matrix CreateFromQuaternion(Quaternion quaternion) | ||
100 | float lambda1pos, lambda2pos, lambda3pos, lambda1neg, lambda2neg, lambda3neg; | ||
101 | lambda1pos = (1f - matrix.M11 + matrix.M23 + matrix.M32) / 2f; | ||
102 | lambda2pos = (1f - matrix.M22 + matrix.M13 + matrix.M31) / 2f; | ||
103 | lambda3pos = (1f - matrix.M33 + matrix.M12 + matrix.M21) / 2f; | ||
104 | lambda1neg = (1f - matrix.M11 - matrix.M23 - matrix.M32) / 2f; | ||
105 | lambda2neg = (1f - matrix.M22 - matrix.M13 - matrix.M31) / 2f; | ||
106 | lambda3neg = (1f - matrix.M33 - matrix.M12 - matrix.M21) / 2f; | ||
107 | |||
108 | //lambadIS = (qJ + s*qK)^2 | ||
109 | //q0 = w | q1 = x | q2 = y, q3 = z | ||
110 | //Every value of qI (I=1,2,3) has 4 possible values cause the sqrt | ||
111 | float[] x = new float[4]; float[] y = new float[4]; float[] z = new float[4]; | ||
112 | float[] sig1 = {1f, 1f, -1f, -1f}; | ||
113 | float[] sig2 = {1f, -1f, 1f, -1f}; | ||
114 | for (int i = 0; i < 4; i++) | ||
115 | { | ||
116 | x[i] = (sig1[i] * (float)Math.Sqrt(lambda1pos) + sig2[i] * (float)Math.Sqrt(lambda1neg)) / 2f; | ||
117 | y[i] = (sig1[i] * (float)Math.Sqrt(lambda2pos) + sig2[i] * (float)Math.Sqrt(lambda2neg)) / 2f; | ||
118 | z[i] = (sig1[i] * (float)Math.Sqrt(lambda3pos) + sig2[i] * (float)Math.Sqrt(lambda3neg)) / 2f; | ||
119 | } | ||
120 | |||
121 | //Only a set of x, y, z are the corrects values. So it requires testing | ||
122 | int li_i=0, li_j=0, li_k=0; | ||
123 | bool lb_testL1P, lb_testL2P, lb_testL3P, lb_testL1N, lb_testL2N, lb_testL3N; | ||
124 | bool lb_superLambda = false; | ||
125 | while((li_i<4)&&(!lb_superLambda)) | ||
126 | { | ||
127 | while ((li_j < 4) && (!lb_superLambda)) | ||
128 | { | ||
129 | while ((li_k < 4) && (!lb_superLambda)) | ||
130 | { | ||
131 | lb_testL1P = isAprox((float)( | ||
132 | Math.Pow((double)(y[li_j] + z[li_k]), 2.0)), lambda1pos); | ||
133 | lb_testL2P = isAprox((float)( | ||
134 | Math.Pow((double)(x[li_i] + z[li_k]), 2.0)), lambda2pos); | ||
135 | lb_testL3P = isAprox((float)( | ||
136 | Math.Pow((double)(x[li_i] + y[li_j]), 2.0)), lambda3pos); | ||
137 | lb_testL1N = isAprox((float)( | ||
138 | Math.Pow((double)(y[li_j] - z[li_k]), 2.0)), lambda1neg); | ||
139 | lb_testL2N = isAprox((float)( | ||
140 | Math.Pow((double)(x[li_i] - z[li_k]), 2.0)), lambda2neg); | ||
141 | lb_testL3N = isAprox((float)( | ||
142 | Math.Pow((double)(x[li_i] - y[li_j]), 2.0)), lambda3neg); | ||
143 | |||
144 | lb_superLambda = (lb_testL1P && lb_testL2P && lb_testL3P | ||
145 | && lb_testL1N && lb_testL2N && lb_testL3N); | ||
146 | |||
147 | if (!lb_superLambda) li_k++; | ||
148 | } | ||
149 | if (!lb_superLambda) li_j++; | ||
150 | } | ||
151 | if (!lb_superLambda) li_i++; | ||
152 | } | ||
153 | |||
154 | Quaternion q = new Quaternion(); | ||
155 | |||
156 | if (lb_superLambda) | ||
157 | { | ||
158 | q.X = x[li_i]; q.Y = y[li_j]; q.Z = z[li_k]; | ||
159 | q.W = (matrix.M12 - 2f * q.X * q.Y) / (2f * q.Z); | ||
160 | |||
161 | if (!isAprox(Omega2, 1f)) | ||
162 | { | ||
163 | if (Omega2 < 0) throw new Exception("Quaternion.CreateFromRotationMatrix: Omega2 is negative!"); | ||
164 | q = q * (float)Math.Sqrt(Omega2);//2 possibles values (+/-). For now only 1. | ||
165 | } | ||
166 | } | ||
167 | else | ||
168 | { | ||
169 | q = Quaternion.identity; | ||
170 | } | ||
171 | |||
172 | return q; | ||
173 | } | ||
174 | private static float floatError = 0.000001f; | ||
175 | private static bool isAprox(float test, float realValue) | ||
176 | { | ||
177 | return (((realValue * (1f - floatError)) <= test) && (test <= (realValue * (1f + floatError)))); | ||
178 | } | ||
179 | |||
180 | |||
181 | public static void CreateFromRotationMatrix(ref Matrix matrix, out Quaternion result) | ||
182 | { | ||
183 | throw new NotImplementedException(); | ||
184 | } | ||
185 | |||
186 | |||
187 | public static Quaternion Divide(Quaternion quaternion1, Quaternion quaternion2) | ||
188 | { | ||
189 | throw new NotImplementedException(); | ||
190 | } | ||
191 | |||
192 | |||
193 | public static void Divide(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result) | ||
194 | { | ||
195 | throw new NotImplementedException(); | ||
196 | } | ||
197 | |||
198 | |||
199 | public static float Dot(Quaternion quaternion1, Quaternion quaternion2) | ||
200 | { | ||
201 | throw new NotImplementedException(); | ||
202 | } | ||
203 | |||
204 | |||
205 | public static void Dot(ref Quaternion quaternion1, ref Quaternion quaternion2, out float result) | ||
206 | { | ||
207 | throw new NotImplementedException(); | ||
208 | } | ||
209 | |||
210 | |||
211 | public override bool Equals(object obj) | ||
212 | { | ||
213 | throw new NotImplementedException(); | ||
214 | } | ||
215 | |||
216 | |||
217 | public bool Equals(Quaternion other) | ||
218 | { | ||
219 | throw new NotImplementedException(); | ||
220 | } | ||
221 | |||
222 | |||
223 | public override int GetHashCode() | ||
224 | { | ||
225 | throw new NotImplementedException(); | ||
226 | } | ||
227 | |||
228 | |||
229 | public static Quaternion Inverse(Quaternion quaternion) | ||
230 | { | ||
231 | throw new NotImplementedException(); | ||
232 | } | ||
233 | |||
234 | |||
235 | public static void Inverse(ref Quaternion quaternion, out Quaternion result) | ||
236 | { | ||
237 | throw new NotImplementedException(); | ||
238 | } | ||
239 | |||
240 | |||
241 | public float Length() | ||
242 | { | ||
243 | //--- | ||
244 | return (float)Math.Sqrt(Math.Pow(this.W, 2.0) + Math.Pow(this.X, 2.0) + Math.Pow(this.Y, 2.0) + Math.Pow(this.Z, 2.0)); | ||
245 | //--- | ||
246 | //throw new NotImplementedException(); | ||
247 | } | ||
248 | |||
249 | |||
250 | public float LengthSquared() | ||
251 | { | ||
252 | //--- | ||
253 | return (float)(Math.Pow(this.W, 2.0) + Math.Pow(this.X, 2.0) + Math.Pow(this.Y, 2.0) + Math.Pow(this.Z, 2.0)); | ||
254 | //--- | ||
255 | //throw new NotImplementedException(); | ||
256 | } | ||
257 | |||
258 | |||
259 | public static Quaternion Lerp(Quaternion quaternion1, Quaternion quaternion2, float amount) | ||
260 | { | ||
261 | throw new NotImplementedException(); | ||
262 | } | ||
263 | |||
264 | |||
265 | public static void Lerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result) | ||
266 | { | ||
267 | throw new NotImplementedException(); | ||
268 | } | ||
269 | |||
270 | |||
271 | public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, float amount) | ||
272 | { | ||
273 | throw new NotImplementedException(); | ||
274 | } | ||
275 | |||
276 | |||
277 | public static void Slerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result) | ||
278 | { | ||
279 | throw new NotImplementedException(); | ||
280 | } | ||
281 | |||
282 | |||
283 | public static Quaternion Subtract(Quaternion quaternion1, Quaternion quaternion2) | ||
284 | { | ||
285 | throw new NotImplementedException(); | ||
286 | } | ||
287 | |||
288 | |||
289 | public static void Subtract(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result) | ||
290 | { | ||
291 | throw new NotImplementedException(); | ||
292 | } | ||
293 | |||
294 | |||
295 | public static Quaternion Multiply(Quaternion quaternion1, Quaternion quaternion2) | ||
296 | { | ||
297 | throw new NotImplementedException(); | ||
298 | } | ||
299 | |||
300 | |||
301 | public static Quaternion Multiply(Quaternion quaternion1, float scaleFactor) | ||
302 | { | ||
303 | throw new NotImplementedException(); | ||
304 | } | ||
305 | |||
306 | |||
307 | public static void Multiply(ref Quaternion quaternion1, float scaleFactor, out Quaternion result) | ||
308 | { | ||
309 | throw new NotImplementedException(); | ||
310 | } | ||
311 | |||
312 | |||
313 | public static void Multiply(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result) | ||
314 | { | ||
315 | throw new NotImplementedException(); | ||
316 | } | ||
317 | |||
318 | |||
319 | public static Quaternion Negate(Quaternion quaternion) | ||
320 | { | ||
321 | throw new NotImplementedException(); | ||
322 | } | ||
323 | |||
324 | |||
325 | public static void Negate(ref Quaternion quaternion, out Quaternion result) | ||
326 | { | ||
327 | throw new NotImplementedException(); | ||
328 | } | ||
329 | |||
330 | |||
331 | public void Normalize() | ||
332 | { | ||
333 | //--- | ||
334 | this = Normalize(this); | ||
335 | //--- | ||
336 | //throw new NotImplementedException(); | ||
337 | } | ||
338 | |||
339 | |||
340 | public static Quaternion Normalize(Quaternion quaternion) | ||
341 | { | ||
342 | //--- | ||
343 | return quaternion / quaternion.Length(); | ||
344 | //--- | ||
345 | //throw new NotImplementedException(); | ||
346 | } | ||
347 | |||
348 | |||
349 | public static void Normalize(ref Quaternion quaternion, out Quaternion result) | ||
350 | { | ||
351 | throw new NotImplementedException(); | ||
352 | } | ||
353 | |||
354 | |||
355 | public static Quaternion operator +(Quaternion quaternion1, Quaternion quaternion2) | ||
356 | { | ||
357 | throw new NotImplementedException(); | ||
358 | } | ||
359 | |||
360 | |||
361 | public static Quaternion operator /(Quaternion quaternion1, Quaternion quaternion2) | ||
362 | { | ||
363 | throw new NotImplementedException(); | ||
364 | } | ||
365 | public static Quaternion operator /(Quaternion quaternion, float factor) | ||
366 | { | ||
367 | quaternion.W /= factor; | ||
368 | quaternion.X /= factor; | ||
369 | quaternion.Y /= factor; | ||
370 | quaternion.Z /= factor; | ||
371 | return quaternion; | ||
372 | } | ||
373 | |||
374 | public static bool operator ==(Quaternion quaternion1, Quaternion quaternion2) | ||
375 | { | ||
376 | throw new NotImplementedException(); | ||
377 | } | ||
378 | |||
379 | |||
380 | public static bool operator !=(Quaternion quaternion1, Quaternion quaternion2) | ||
381 | { | ||
382 | throw new NotImplementedException(); | ||
383 | } | ||
384 | |||
385 | |||
386 | public static Quaternion operator *(Quaternion quaternion1, Quaternion quaternion2) | ||
387 | { | ||
388 | //--- | ||
389 | //Grassmann product | ||
390 | Quaternion quaternionProduct = new Quaternion(); | ||
391 | |||
392 | quaternionProduct.W = quaternion1.W * quaternion2.W - quaternion1.X * quaternion2.X - quaternion1.Y * quaternion2.Y - quaternion1.Z * quaternion2.Z; | ||
393 | quaternionProduct.X = quaternion1.W * quaternion2.X + quaternion1.X * quaternion2.W + quaternion1.Y * quaternion2.Z - quaternion1.Z * quaternion2.Y; | ||
394 | quaternionProduct.Y = quaternion1.W * quaternion2.Y - quaternion1.X * quaternion2.Z + quaternion1.Y * quaternion2.W + quaternion1.Z * quaternion2.X; | ||
395 | quaternionProduct.Z = quaternion1.W * quaternion2.Z + quaternion1.X * quaternion2.Y - quaternion1.Y * quaternion2.X + quaternion1.Z * quaternion2.W; | ||
396 | return quaternionProduct; | ||
397 | //--- | ||
398 | //throw new NotImplementedException(); | ||
399 | } | ||
400 | |||
401 | |||
402 | public static Quaternion operator *(Quaternion quaternion1, float scaleFactor) | ||
403 | { | ||
404 | return new Quaternion(quaternion1.X / scaleFactor, quaternion1.Y / scaleFactor, | ||
405 | quaternion1.Z / scaleFactor, quaternion1.W / scaleFactor); | ||
406 | } | ||
407 | |||
408 | |||
409 | public static Quaternion operator -(Quaternion quaternion1, Quaternion quaternion2) | ||
410 | { | ||
411 | throw new NotImplementedException(); | ||
412 | } | ||
413 | |||
414 | |||
415 | public static Quaternion operator -(Quaternion quaternion) | ||
416 | { | ||
417 | throw new NotImplementedException(); | ||
418 | } | ||
419 | |||
420 | |||
421 | public override string ToString() | ||
422 | { | ||
423 | return "(" + this.X + ", " + this.Y + ", " + this.Z + ", " + this.W + ")"; | ||
424 | } | ||
425 | |||
426 | private static void Conjugate(ref Quaternion quaternion, out Quaternion result) | ||
427 | { | ||
428 | throw new NotImplementedException(); | ||
429 | } | ||
430 | } | ||
431 | } | ||