source: OniSplit/Math/Matrix.cs@ 1157

Last change on this file since 1157 was 1114, checked in by iritscen, 5 years ago

Adding OniSplit source code (v0.9.99.0). Many thanks to Neo for all his work over the years.
File size: 20.7 KB
RevLine 
[1114]1using System;
2
3namespace Oni
4{
5 internal struct Matrix : IEquatable<Matrix>
6 {
7 public float M11, M12, M13, M14;
8 public float M21, M22, M23, M24;
9 public float M31, M32, M33, M34;
10 public float M41, M42, M43, M44;
11
12 public Matrix(float m11, float m12, float m13, float m14,
13 float m21, float m22, float m23, float m24,
14 float m31, float m32, float m33, float m34,
15 float m41, float m42, float m43, float m44)
16 {
17 M11 = m11; M12 = m12; M13 = m13; M14 = m14;
18 M21 = m21; M22 = m22; M23 = m23; M24 = m24;
19 M31 = m31; M32 = m32; M33 = m33; M34 = m34;
20 M41 = m41; M42 = m42; M43 = m43; M44 = m44;
21 }
22
23 public Matrix(float[] values)
24 {
25 M11 = values[0]; M12 = values[4]; M13 = values[8]; M14 = values[12];
26 M21 = values[1]; M22 = values[5]; M23 = values[9]; M24 = values[13];
27 M31 = values[2]; M32 = values[6]; M33 = values[10]; M34 = values[14];
28 M41 = values[3]; M42 = values[7]; M43 = values[11]; M44 = values[15];
29 }
30
31 public void CopyTo(float[] values)
32 {
33 values[0] = M11;
34 values[1] = M21;
35 values[2] = M31;
36 values[3] = M41;
37
38 values[4] = M12;
39 values[5] = M22;
40 values[6] = M32;
41 values[7] = M42;
42
43 values[8] = M13;
44 values[9] = M23;
45 values[10] = M33;
46 values[11] = M43;
47
48 values[12] = M14;
49 values[13] = M24;
50 values[14] = M34;
51 values[15] = M44;
52 }
53
54 public static Matrix CreateTranslation(float x, float y, float z)
55 {
56 Matrix r = Identity;
57
58 r.M41 = x;
59 r.M42 = y;
60 r.M43 = z;
61
62 return r;
63 }
64
65 public static Matrix CreateTranslation(Vector3 v) => CreateTranslation(v.X, v.Y, v.Z);
66
67 public static Matrix CreateScale(float sx, float sy, float sz)
68 {
69 Matrix r = Identity;
70
71 r.M11 = sx;
72 r.M22 = sy;
73 r.M33 = sz;
74
75 return r;
76 }
77
78 public static Matrix CreateScale(float s) => CreateScale(s, s, s);
79 public static Matrix CreateScale(Vector3 s) => CreateScale(s.X, s.Y, s.Z);
80
81 public static Matrix CreateRotationX(float angle)
82 {
83 float cos = FMath.Cos(angle);
84 float sin = FMath.Sin(angle);
85
86 Matrix r = Identity;
87 r.M22 = cos;
88 r.M23 = sin;
89 r.M32 = -sin;
90 r.M33 = cos;
91 return r;
92 }
93
94 public static Matrix CreateRotationY(float angle)
95 {
96 float cos = FMath.Cos(angle);
97 float sin = FMath.Sin(angle);
98
99 Matrix r = Identity;
100 r.M11 = cos;
101 r.M13 = -sin;
102 r.M31 = sin;
103 r.M33 = cos;
104 return r;
105 }
106
107 public static Matrix CreateRotationZ(float angle)
108 {
109 float cos = FMath.Cos(angle);
110 float sin = FMath.Sin(angle);
111
112 Matrix r = Identity;
113 r.M11 = cos;
114 r.M12 = sin;
115 r.M21 = -sin;
116 r.M22 = cos;
117 return r;
118 }
119
120 public static Matrix CreateFromAxisAngle(Vector3 axis, float angle)
121 {
122 float sin = FMath.Sin(angle);
123 float cos = FMath.Cos(angle);
124
125 float x = axis.X;
126 float y = axis.Y;
127 float z = axis.Z;
128 float xx = x * x;
129 float yy = y * y;
130 float zz = z * z;
131 float xy = x * y;
132 float xz = x * z;
133 float yz = y * z;
134
135 Matrix r = Identity;
136 r.M11 = xx + (cos * (1.0f - xx));
137 r.M12 = (xy - (cos * xy)) + (sin * z);
138 r.M13 = (xz - (cos * xz)) - (sin * y);
139 r.M21 = (xy - (cos * xy)) - (sin * z);
140 r.M22 = yy + (cos * (1.0f - yy));
141 r.M23 = (yz - (cos * yz)) + (sin * x);
142 r.M31 = (xz - (cos * xz)) + (sin * y);
143 r.M32 = (yz - (cos * yz)) - (sin * x);
144 r.M33 = zz + (cos * (1.0f - zz));
145 return r;
146 }
147
148 public static Matrix CreateFromQuaternion(Quaternion q)
149 {
150 float xx = q.X * q.X;
151 float yy = q.Y * q.Y;
152 float zz = q.Z * q.Z;
153 float xy = q.X * q.Y;
154 float zw = q.Z * q.W;
155 float zx = q.Z * q.X;
156 float yw = q.Y * q.W;
157 float yz = q.Y * q.Z;
158 float xw = q.X * q.W;
159
160 Matrix r = Identity;
161
162 r.M11 = 1.0f - 2.0f * (yy + zz);
163 r.M12 = 2.0f * (xy + zw);
164 r.M13 = 2.0f * (zx - yw);
165
166 r.M21 = 2.0f * (xy - zw);
167 r.M22 = 1.0f - 2.0f * (zz + xx);
168 r.M23 = 2.0f * (yz + xw);
169
170 r.M31 = 2.0f * (zx + yw);
171 r.M32 = 2.0f * (yz - xw);
172 r.M33 = 1.0f - 2.0f * (yy + xx);
173
174 return r;
175 }
176
177 public static Matrix operator +(Matrix m1, Matrix m2)
178 {
179 m1.M11 += m2.M11;
180 m1.M12 += m2.M12;
181 m1.M13 += m2.M13;
182 m1.M14 += m2.M14;
183 m1.M21 += m2.M21;
184 m1.M22 += m2.M22;
185 m1.M23 += m2.M23;
186 m1.M24 += m2.M24;
187 m1.M31 += m2.M31;
188 m1.M32 += m2.M32;
189 m1.M33 += m2.M33;
190 m1.M34 += m2.M34;
191 m1.M41 += m2.M41;
192 m1.M42 += m2.M42;
193 m1.M43 += m2.M43;
194 m1.M44 += m2.M44;
195
196 return m1;
197 }
198
199 public static Matrix operator -(Matrix m1, Matrix m2)
200 {
201 m1.M11 -= m2.M11;
202 m1.M12 -= m2.M12;
203 m1.M13 -= m2.M13;
204 m1.M14 -= m2.M14;
205 m1.M21 -= m2.M21;
206 m1.M22 -= m2.M22;
207 m1.M23 -= m2.M23;
208 m1.M24 -= m2.M24;
209 m1.M31 -= m2.M31;
210 m1.M32 -= m2.M32;
211 m1.M33 -= m2.M33;
212 m1.M34 -= m2.M34;
213 m1.M41 -= m2.M41;
214 m1.M42 -= m2.M42;
215 m1.M43 -= m2.M43;
216 m1.M44 -= m2.M44;
217
218 return m1;
219 }
220
221 public static Matrix operator *(Matrix m, float s)
222 {
223 m.M11 *= s;
224 m.M12 *= s;
225 m.M13 *= s;
226 m.M14 *= s;
227 m.M21 *= s;
228 m.M22 *= s;
229 m.M23 *= s;
230 m.M24 *= s;
231 m.M31 *= s;
232 m.M32 *= s;
233 m.M33 *= s;
234 m.M34 *= s;
235 m.M41 *= s;
236 m.M42 *= s;
237 m.M43 *= s;
238 m.M44 *= s;
239
240 return m;
241 }
242
243 public static Matrix operator *(float s, Matrix m) => m * s;
244
245 public static Matrix operator /(Matrix m, float s) => m * (1.0f / s);
246
247 public static Matrix operator *(Matrix m1, Matrix m2)
248 {
249 Matrix r;
250
251 r.M11 = m1.M11 * m2.M11 + m1.M12 * m2.M21 + m1.M13 * m2.M31 + m1.M14 * m2.M41;
252 r.M12 = m1.M11 * m2.M12 + m1.M12 * m2.M22 + m1.M13 * m2.M32 + m1.M14 * m2.M42;
253 r.M13 = m1.M11 * m2.M13 + m1.M12 * m2.M23 + m1.M13 * m2.M33 + m1.M14 * m2.M43;
254 r.M14 = m1.M11 * m2.M14 + m1.M12 * m2.M24 + m1.M13 * m2.M34 + m1.M14 * m2.M44;
255 r.M21 = m1.M21 * m2.M11 + m1.M22 * m2.M21 + m1.M23 * m2.M31 + m1.M24 * m2.M41;
256 r.M22 = m1.M21 * m2.M12 + m1.M22 * m2.M22 + m1.M23 * m2.M32 + m1.M24 * m2.M42;
257 r.M23 = m1.M21 * m2.M13 + m1.M22 * m2.M23 + m1.M23 * m2.M33 + m1.M24 * m2.M43;
258 r.M24 = m1.M21 * m2.M14 + m1.M22 * m2.M24 + m1.M23 * m2.M34 + m1.M24 * m2.M44;
259 r.M31 = m1.M31 * m2.M11 + m1.M32 * m2.M21 + m1.M33 * m2.M31 + m1.M34 * m2.M41;
260 r.M32 = m1.M31 * m2.M12 + m1.M32 * m2.M22 + m1.M33 * m2.M32 + m1.M34 * m2.M42;
261 r.M33 = m1.M31 * m2.M13 + m1.M32 * m2.M23 + m1.M33 * m2.M33 + m1.M34 * m2.M43;
262 r.M34 = m1.M31 * m2.M14 + m1.M32 * m2.M24 + m1.M33 * m2.M34 + m1.M34 * m2.M44;
263 r.M41 = m1.M41 * m2.M11 + m1.M42 * m2.M21 + m1.M43 * m2.M31 + m1.M44 * m2.M41;
264 r.M42 = m1.M41 * m2.M12 + m1.M42 * m2.M22 + m1.M43 * m2.M32 + m1.M44 * m2.M42;
265 r.M43 = m1.M41 * m2.M13 + m1.M42 * m2.M23 + m1.M43 * m2.M33 + m1.M44 * m2.M43;
266 r.M44 = m1.M41 * m2.M14 + m1.M42 * m2.M24 + m1.M43 * m2.M34 + m1.M44 * m2.M44;
267
268 return r;
269 }
270
271 public Matrix Transpose()
272 {
273 Matrix t;
274
275 t.M11 = M11;
276 t.M12 = M21;
277 t.M13 = M31;
278 t.M14 = M41;
279 t.M21 = M12;
280 t.M22 = M22;
281 t.M23 = M32;
282 t.M24 = M42;
283 t.M31 = M13;
284 t.M32 = M23;
285 t.M33 = M33;
286 t.M34 = M43;
287 t.M41 = M14;
288 t.M42 = M24;
289 t.M43 = M34;
290 t.M44 = M44;
291
292 return t;
293 }
294
295 public static bool operator ==(Matrix m1, Matrix m2) => m1.Equals(m2);
296 public static bool operator !=(Matrix m1, Matrix m2) => !m1.Equals(m2);
297
298 public Vector3 XAxis
299 {
300 get
301 {
302 return new Vector3(M11, M12, M13);
303 }
304 set
305 {
306 M11 = value.X;
307 M12 = value.Y;
308 M13 = value.Z;
309 }
310 }
311
312 public Vector3 YAxis
313 {
314 get
315 {
316 return new Vector3(M21, M22, M23);
317 }
318 set
319 {
320 M21 = value.X;
321 M22 = value.Y;
322 M23 = value.Z;
323 }
324 }
325
326 public Vector3 ZAxis
327 {
328 get
329 {
330 return new Vector3(M31, M32, M33);
331 }
332 set
333 {
334 M31 = value.X;
335 M32 = value.Y;
336 M33 = value.Z;
337 }
338 }
339
340 public Vector3 Scale
341 {
342 get
343 {
344 return new Vector3(M11, M22, M33);
345 }
346 set
347 {
348 M11 = value.X;
349 M22 = value.Y;
350 M33 = value.Z;
351 }
352 }
353
354 public Vector3 Translation
355 {
356 get
357 {
358 return new Vector3(M41, M42, M43);
359 }
360 set
361 {
362 M41 = value.X;
363 M42 = value.Y;
364 M43 = value.Z;
365 }
366 }
367
368 public bool Equals(Matrix other)
369 {
370 return (M11 == other.M11 && M12 == other.M12 && M13 == other.M13 && M14 == other.M14
371 && M21 == other.M21 && M22 == other.M22 && M23 == other.M23 && M24 == other.M24
372 && M31 == other.M31 && M32 == other.M32 && M33 == other.M33 && M34 == other.M34
373 && M41 == other.M41 && M42 == other.M42 && M43 == other.M43 && M44 == other.M44);
374 }
375
376 public override bool Equals(object obj) => obj is Matrix && Equals((Matrix)obj);
377
378 public override int GetHashCode()
379 {
380 return M11.GetHashCode() ^ M12.GetHashCode() ^ M13.GetHashCode() ^ M14.GetHashCode()
381 ^ M11.GetHashCode() ^ M12.GetHashCode() ^ M13.GetHashCode() ^ M14.GetHashCode()
382 ^ M11.GetHashCode() ^ M12.GetHashCode() ^ M13.GetHashCode() ^ M14.GetHashCode()
383 ^ M11.GetHashCode() ^ M12.GetHashCode() ^ M13.GetHashCode() ^ M14.GetHashCode();
384 }
385
386 public override string ToString()
387 {
388 return string.Format("{{M11:{0} M12:{1} M13:{2} M14:{3}}}\n{{M21:{4} M22:{5} M23:{6} M24:{7}}}\n{{M31:{8} M32:{9} M33:{10} M34:{11}}}\n{{M41:{12} M42:{13} M43:{14} M44:{15}}}",
389 M11, M12, M13, M14,
390 M21, M22, M23, M24,
391 M31, M32, M33, M34,
392 M41, M42, M43, M44);
393 }
394
395 private static readonly Matrix identity = new Matrix(
396 1.0f, 0.0f, 0.0f, 0.0f,
397 0.0f, 1.0f, 0.0f, 0.0f,
398 0.0f, 0.0f, 1.0f, 0.0f,
399 0.0f, 0.0f, 0.0f, 1.0f);
400
401 public static Matrix Identity => identity;
402
403 public Vector3 ToEuler()
404 {
405 float a = M11;
406 float b = M21;
407 float c, s, r;
408
409 if (b == 0.0f)
410 {
411 c = FMath.Sign(a);
412 s = 0.0f;
413 r = Math.Abs(a);
414 }
415 else if (a == 0.0f)
416 {
417 c = 0.0f;
418 s = FMath.Sign(b);
419 r = Math.Abs(b);
420 }
421 else if (Math.Abs(b) > Math.Abs(a))
422 {
423 float t = a / b;
424 float u = FMath.Sign(b) * FMath.Sqrt(1.0f + t * t);
425 s = 1.0f / u;
426 c = s * t;
427 r = b * u;
428 }
429 else
430 {
431 float t = b / a;
432 float u = FMath.Sign(a) * FMath.Sqrt(1.0f + t * t);
433 c = 1.0f / u;
434 s = c * t;
435 r = a * u;
436 }
437
438 Vector3 e;
439 e.Z = MathHelper.ToDegrees(-FMath.Atan2(s, c));
440 e.Y = MathHelper.ToDegrees(FMath.Atan2(M31, r));
441 e.X = MathHelper.ToDegrees(-FMath.Atan2(M32, M33));
442 return e;
443 }
444
445 public float Determinant()
446 {
447 var m11 = M11;
448 var m12 = M12;
449 var m13 = M13;
450 var m14 = M14;
451 var m21 = M21;
452 var m22 = M22;
453 var m23 = M23;
454 var m24 = M24;
455 var m31 = M31;
456 var m32 = M32;
457 var m33 = M33;
458 var m34 = M34;
459 var m41 = M41;
460 var m42 = M42;
461 var m43 = M43;
462 var m44 = M44;
463
464 var d3434 = m33 * m44 - m34 * m43;
465 var d3424 = m32 * m44 - m34 * m42;
466 var d3423 = m32 * m43 - m33 * m42;
467 var d3414 = m31 * m44 - m34 * m41;
468 var d3413 = m31 * m43 - m33 * m41;
469 var d3412 = m31 * m42 - m32 * m41;
470
471 return m11 * (m22 * d3434 - m23 * d3424 + m24 * d3423)
472 - m12 * (m21 * d3434 - m23 * d3414 + m24 * d3413)
473 + m13 * (m21 * d3424 - m22 * d3414 + m24 * d3412)
474 - m14 * (m21 * d3423 - m22 * d3413 + m23 * d3412);
475 }
476
477 //Matrix m = Matrix.Identity;
478 //m *= Matrix.CreateScale(3.3f, 1.3f, 7.6f);
479 //m *= Matrix.CreateTranslation(2.3f, 4.5f, 6.7f);
480 //m *= Matrix.CreateRotationY(1.2f);
481 //m *= Matrix.CreateTranslation(2.3f, 4.5f, 6.7f);
482 //m *= Matrix.CreateRotationY(1.2f);
483 //m *= Matrix.CreateTranslation(2.3f, 4.5f, 6.7f);
484 //m *= Matrix.CreateRotationY(1.2f);
485
486 //Vector3 s, t;
487 //Quaternion r;
488 //m.Decompose(out s, out r, out t);
489 //Matrix m2 = Matrix.CreateScale(s) * Matrix.CreateFromQuaternion(r) * Matrix.CreateTranslation(t);
490
491 //Console.WriteLine(m2 - m);
492 //return 0;
493
494 //[StructLayout(LayoutKind.Sequential)]
495 //private unsafe struct VectorBasis
496 //{
497 // public Vector3* axis0;
498 // public Vector3* axis1;
499 // public Vector3* axis2;
500 //}
501
502 //[StructLayout(LayoutKind.Sequential)]
503 //private struct CanonicalBasis
504 //{
505 // public Vector3 axis0;
506 // public Vector3 axis1;
507 // public Vector3 axis2;
508 //}
509
510 //public unsafe bool Decompose(out Vector3 outScale, out Quaternion outRotation, out Vector3 outTranslation)
511 //{
512 // outTranslation.X = M41;
513 // outTranslation.Y = M42;
514 // outTranslation.Z = M43;
515
516 // var rotation = new Matrix(
517 // M11, M12, M13, 0.0f,
518 // M21, M22, M23, 0.0f,
519 // M31, M32, M33, 0.0f,
520 // 0.0f, 0.0f, 0.0f, 1.0f);
521
522 // var vectorBasis = new VectorBasis {
523 // axis0 = (Vector3*)&rotation.M11,
524 // axis1 = (Vector3*)&rotation.M21,
525 // axis2 = (Vector3*)&rotation.M31
526 // };
527
528 // var canonicalBasis = new CanonicalBasis {
529 // axis0 = Vector3.UnitX,
530 // axis1 = Vector3.UnitY,
531 // axis2 = Vector3.UnitZ
532 // };
533
534 // var scale = new Vector3(
535 // vectorBasis.axis0->Length(),
536 // vectorBasis.axis1->Length(),
537 // vectorBasis.axis2->Length()
538 // );
539
540 // int xi, yi, zi;
541
542 // if (scale.X < scale.Y)
543 // {
544 // if (scale.Y < scale.Z)
545 // {
546 // xi = 2;
547 // yi = 1;
548 // zi = 0;
549 // }
550 // else
551 // {
552 // xi = 1;
553
554 // if (scale.X < scale.Z)
555 // {
556 // yi = 2;
557 // zi = 0;
558 // }
559 // else
560 // {
561 // yi = 0;
562 // zi = 2;
563 // }
564 // }
565 // }
566 // else
567 // {
568 // if (scale.X < scale.Z)
569 // {
570 // xi = 2;
571 // yi = 0;
572 // zi = 1;
573 // }
574 // else
575 // {
576 // xi = 0;
577
578 // if (scale.Y < scale.Z)
579 // {
580 // yi = 2;
581 // zi = 1;
582 // }
583 // else
584 // {
585 // yi = 1;
586 // zi = 2;
587 // }
588 // }
589 // }
590
591 // var pScale = &scale.X;
592
593 // var pvBasis = &vectorBasis.axis0;
594 // var pcBasis = &canonicalBasis.axis0;
595
596 // if (pScale[xi] < 0.0001f)
597 // {
598 // //
599 // // If the smallest scale is < 0.0001 then use the coresponding cannonical basis instead
600 // //
601
602 // pvBasis[xi] = &pcBasis[xi];
603 // }
604 // else
605 // {
606 // pvBasis[xi]->Normalize();
607 // }
608
609 // if (pScale[yi] < 0.0001f)
610 // {
611 // //
612 // // The second smallest scale is < 0.0001 too, build a perpendicular vector
613 // //
614
615 // float fx = Math.Abs(pvBasis[xi]->X);
616 // float fy = Math.Abs(pvBasis[xi]->Y);
617 // float fz = Math.Abs(pvBasis[xi]->Z);
618
619 // int yij;
620
621 // if (fx < fy)
622 // {
623 // if (fy < fz)
624 // {
625 // yij = 0;
626 // }
627 // else
628 // {
629 // if (fx < fz)
630 // yij = 0;
631 // else
632 // yij = 2;
633 // }
634 // }
635 // else
636 // {
637 // if (fx < fz)
638 // {
639 // yij = 1;
640 // }
641 // else
642 // {
643 // if (fy < fz)
644 // yij = 1;
645 // else
646 // yij = 2;
647 // }
648 // }
649
650 // pcBasis[yij] = Vector3.Cross(*pvBasis[yi], *pvBasis[xi]);
651 // }
652
653 // pvBasis[yi]->Normalize();
654
655 // if (pScale[zi] < 0.0001f)
656 // *(pvBasis[zi]) = Vector3.Cross(*pvBasis[yi], *pvBasis[xi]);
657 // else
658 // pvBasis[zi]->Normalize();
659
660 // float rotDet = rotation.Determinant();
661
662 // if (rotDet < 0.0f)
663 // {
664 // pScale[xi] = -pScale[xi];
665 // *(pvBasis[xi]) = -(*(pvBasis[xi]));
666 // rotDet = -rotDet;
667 // }
668
669 // outScale = scale;
670
671 // if (Math.Abs(rotDet - 1.0f) > 0.01f)
672 // {
673 // outRotation = Quaternion.Identity;
674 // return false;
675 // }
676 // else
677 // {
678 // outRotation = Quaternion.CreateFromRotationMatrix(rotation);
679 // return true;
680 // }
681 //}
682 }
683}
Note: See TracBrowser for help on using the repository browser.