corrade-vassal – Blame information for rev 1

Subversion Repositories:
Rev:
Rev Author Line No. Line
1 vero 1 /*
2 * Copyright (c) 2006-2014, openmetaverse.org
3 * All rights reserved.
4 *
5 * - Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 *
8 * - Redistributions of source code must retain the above copyright notice, this
9 * list of conditions and the following disclaimer.
10 * - Neither the name of the openmetaverse.org nor the names
11 * of its contributors may be used to endorse or promote products derived from
12 * this software without specific prior written permission.
13 *
14 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
15 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
18 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
19 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
20 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
21 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
22 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
23 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
24 * POSSIBILITY OF SUCH DAMAGE.
25 */
26  
27 using System;
28 using System.Runtime.InteropServices;
29  
30 namespace OpenMetaverse
31 {
32 [Serializable]
33 [StructLayout(LayoutKind.Sequential)]
34 public struct Matrix4 : IEquatable<Matrix4>
35 {
36 public float M11, M12, M13, M14;
37 public float M21, M22, M23, M24;
38 public float M31, M32, M33, M34;
39 public float M41, M42, M43, M44;
40  
41 #region Properties
42  
43 public Vector3 AtAxis
44 {
45 get
46 {
47 return new Vector3(M11, M21, M31);
48 }
49 set
50 {
51 M11 = value.X;
52 M21 = value.Y;
53 M31 = value.Z;
54 }
55 }
56  
57 public Vector3 LeftAxis
58 {
59 get
60 {
61 return new Vector3(M12, M22, M32);
62 }
63 set
64 {
65 M12 = value.X;
66 M22 = value.Y;
67 M32 = value.Z;
68 }
69 }
70  
71 public Vector3 UpAxis
72 {
73 get
74 {
75 return new Vector3(M13, M23, M33);
76 }
77 set
78 {
79 M13 = value.X;
80 M23 = value.Y;
81 M33 = value.Z;
82 }
83 }
84  
85 #endregion Properties
86  
87 #region Constructors
88  
89 public Matrix4(
90 float m11, float m12, float m13, float m14,
91 float m21, float m22, float m23, float m24,
92 float m31, float m32, float m33, float m34,
93 float m41, float m42, float m43, float m44)
94 {
95 M11 = m11;
96 M12 = m12;
97 M13 = m13;
98 M14 = m14;
99  
100 M21 = m21;
101 M22 = m22;
102 M23 = m23;
103 M24 = m24;
104  
105 M31 = m31;
106 M32 = m32;
107 M33 = m33;
108 M34 = m34;
109  
110 M41 = m41;
111 M42 = m42;
112 M43 = m43;
113 M44 = m44;
114 }
115  
116 public Matrix4(float roll, float pitch, float yaw)
117 {
118 this = CreateFromEulers(roll, pitch, yaw);
119 }
120  
121 public Matrix4(Matrix4 m)
122 {
123 M11 = m.M11;
124 M12 = m.M12;
125 M13 = m.M13;
126 M14 = m.M14;
127  
128 M21 = m.M21;
129 M22 = m.M22;
130 M23 = m.M23;
131 M24 = m.M24;
132  
133 M31 = m.M31;
134 M32 = m.M32;
135 M33 = m.M33;
136 M34 = m.M34;
137  
138 M41 = m.M41;
139 M42 = m.M42;
140 M43 = m.M43;
141 M44 = m.M44;
142 }
143  
144 #endregion Constructors
145  
146 #region Public Methods
147  
148 public float Determinant()
149 {
150 return
151 M14 * M23 * M32 * M41 - M13 * M24 * M32 * M41 - M14 * M22 * M33 * M41 + M12 * M24 * M33 * M41 +
152 M13 * M22 * M34 * M41 - M12 * M23 * M34 * M41 - M14 * M23 * M31 * M42 + M13 * M24 * M31 * M42 +
153 M14 * M21 * M33 * M42 - M11 * M24 * M33 * M42 - M13 * M21 * M34 * M42 + M11 * M23 * M34 * M42 +
154 M14 * M22 * M31 * M43 - M12 * M24 * M31 * M43 - M14 * M21 * M32 * M43 + M11 * M24 * M32 * M43 +
155 M12 * M21 * M34 * M43 - M11 * M22 * M34 * M43 - M13 * M22 * M31 * M44 + M12 * M23 * M31 * M44 +
156 M13 * M21 * M32 * M44 - M11 * M23 * M32 * M44 - M12 * M21 * M33 * M44 + M11 * M22 * M33 * M44;
157 }
158  
159 public float Determinant3x3()
160 {
161 float det = 0f;
162  
163 float diag1 = M11 * M22 * M33;
164 float diag2 = M12 * M32 * M31;
165 float diag3 = M13 * M21 * M32;
166 float diag4 = M31 * M22 * M13;
167 float diag5 = M32 * M23 * M11;
168 float diag6 = M33 * M21 * M12;
169  
170 det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
171  
172 return det;
173 }
174  
175 public float Trace()
176 {
177 return M11 + M22 + M33 + M44;
178 }
179  
180 /// <summary>
181 /// Convert this matrix to euler rotations
182 /// </summary>
183 /// <param name="roll">X euler angle</param>
184 /// <param name="pitch">Y euler angle</param>
185 /// <param name="yaw">Z euler angle</param>
186 public void GetEulerAngles(out float roll, out float pitch, out float yaw)
187 {
188 double angleX, angleY, angleZ;
189 double cx, cy, cz; // cosines
190 double sx, sz; // sines
191  
192 angleY = Math.Asin(Utils.Clamp(M13, -1f, 1f));
193 cy = Math.Cos(angleY);
194  
195 if (Math.Abs(cy) > 0.005f)
196 {
197 // No gimbal lock
198 cx = M33 / cy;
199 sx = (-M23) / cy;
200  
201 angleX = (float)Math.Atan2(sx, cx);
202  
203 cz = M11 / cy;
204 sz = (-M12) / cy;
205  
206 angleZ = (float)Math.Atan2(sz, cz);
207 }
208 else
209 {
210 // Gimbal lock
211 angleX = 0;
212  
213 cz = M22;
214 sz = M21;
215  
216 angleZ = Math.Atan2(sz, cz);
217 }
218  
219 // Return only positive angles in [0,360]
220 if (angleX < 0) angleX += 360d;
221 if (angleY < 0) angleY += 360d;
222 if (angleZ < 0) angleZ += 360d;
223  
224 roll = (float)angleX;
225 pitch = (float)angleY;
226 yaw = (float)angleZ;
227 }
228  
229 /// <summary>
230 /// Convert this matrix to a quaternion rotation
231 /// </summary>
232 /// <returns>A quaternion representation of this rotation matrix</returns>
233 public Quaternion GetQuaternion()
234 {
235 Quaternion quat = new Quaternion();
236 float trace = Trace() + 1f;
237  
238 if (trace > Single.Epsilon)
239 {
240 float s = 0.5f / (float)Math.Sqrt(trace);
241  
242 quat.X = (M32 - M23) * s;
243 quat.Y = (M13 - M31) * s;
244 quat.Z = (M21 - M12) * s;
245 quat.W = 0.25f / s;
246 }
247 else
248 {
249 if (M11 > M22 && M11 > M33)
250 {
251 float s = 2.0f * (float)Math.Sqrt(1.0f + M11 - M22 - M33);
252  
253 quat.X = 0.25f * s;
254 quat.Y = (M12 + M21) / s;
255 quat.Z = (M13 + M31) / s;
256 quat.W = (M23 - M32) / s;
257 }
258 else if (M22 > M33)
259 {
260 float s = 2.0f * (float)Math.Sqrt(1.0f + M22 - M11 - M33);
261  
262 quat.X = (M12 + M21) / s;
263 quat.Y = 0.25f * s;
264 quat.Z = (M23 + M32) / s;
265 quat.W = (M13 - M31) / s;
266 }
267 else
268 {
269 float s = 2.0f * (float)Math.Sqrt(1.0f + M33 - M11 - M22);
270  
271 quat.X = (M13 + M31) / s;
272 quat.Y = (M23 + M32) / s;
273 quat.Z = 0.25f * s;
274 quat.W = (M12 - M21) / s;
275 }
276 }
277  
278 return quat;
279 }
280  
281 public bool Decompose(out Vector3 scale, out Quaternion rotation, out Vector3 translation)
282 {
283 translation.X = this.M41;
284 translation.Y = this.M42;
285 translation.Z = this.M43;
286  
287 float xs = (Math.Sign(M11 * M12 * M13 * M14) < 0) ? -1 : 1;
288 float ys = (Math.Sign(M21 * M22 * M23 * M24) < 0) ? -1 : 1;
289 float zs = (Math.Sign(M31 * M32 * M33 * M34) < 0) ? -1 : 1;
290  
291 scale.X = xs * (float)Math.Sqrt(this.M11 * this.M11 + this.M12 * this.M12 + this.M13 * this.M13);
292 scale.Y = ys * (float)Math.Sqrt(this.M21 * this.M21 + this.M22 * this.M22 + this.M23 * this.M23);
293 scale.Z = zs * (float)Math.Sqrt(this.M31 * this.M31 + this.M32 * this.M32 + this.M33 * this.M33);
294  
295 if (scale.X == 0.0 || scale.Y == 0.0 || scale.Z == 0.0)
296 {
297 rotation = Quaternion.Identity;
298 return false;
299 }
300  
301 Matrix4 m1 = new Matrix4(this.M11 / scale.X, M12 / scale.X, M13 / scale.X, 0,
302 this.M21 / scale.Y, M22 / scale.Y, M23 / scale.Y, 0,
303 this.M31 / scale.Z, M32 / scale.Z, M33 / scale.Z, 0,
304 0, 0, 0, 1);
305  
306 rotation = Quaternion.CreateFromRotationMatrix(m1);
307 return true;
308 }
309  
310 #endregion Public Methods
311  
312 #region Static Methods
313  
314 public static Matrix4 Add(Matrix4 matrix1, Matrix4 matrix2)
315 {
316 Matrix4 matrix;
317 matrix.M11 = matrix1.M11 + matrix2.M11;
318 matrix.M12 = matrix1.M12 + matrix2.M12;
319 matrix.M13 = matrix1.M13 + matrix2.M13;
320 matrix.M14 = matrix1.M14 + matrix2.M14;
321  
322 matrix.M21 = matrix1.M21 + matrix2.M21;
323 matrix.M22 = matrix1.M22 + matrix2.M22;
324 matrix.M23 = matrix1.M23 + matrix2.M23;
325 matrix.M24 = matrix1.M24 + matrix2.M24;
326  
327 matrix.M31 = matrix1.M31 + matrix2.M31;
328 matrix.M32 = matrix1.M32 + matrix2.M32;
329 matrix.M33 = matrix1.M33 + matrix2.M33;
330 matrix.M34 = matrix1.M34 + matrix2.M34;
331  
332 matrix.M41 = matrix1.M41 + matrix2.M41;
333 matrix.M42 = matrix1.M42 + matrix2.M42;
334 matrix.M43 = matrix1.M43 + matrix2.M43;
335 matrix.M44 = matrix1.M44 + matrix2.M44;
336 return matrix;
337 }
338  
339 public static Matrix4 CreateFromAxisAngle(Vector3 axis, float angle)
340 {
341 Matrix4 matrix = new Matrix4();
342  
343 float x = axis.X;
344 float y = axis.Y;
345 float z = axis.Z;
346 float sin = (float)Math.Sin(angle);
347 float cos = (float)Math.Cos(angle);
348 float xx = x * x;
349 float yy = y * y;
350 float zz = z * z;
351 float xy = x * y;
352 float xz = x * z;
353 float yz = y * z;
354  
355 matrix.M11 = xx + (cos * (1f - xx));
356 matrix.M12 = (xy - (cos * xy)) + (sin * z);
357 matrix.M13 = (xz - (cos * xz)) - (sin * y);
358 //matrix.M14 = 0f;
359  
360 matrix.M21 = (xy - (cos * xy)) - (sin * z);
361 matrix.M22 = yy + (cos * (1f - yy));
362 matrix.M23 = (yz - (cos * yz)) + (sin * x);
363 //matrix.M24 = 0f;
364  
365 matrix.M31 = (xz - (cos * xz)) + (sin * y);
366 matrix.M32 = (yz - (cos * yz)) - (sin * x);
367 matrix.M33 = zz + (cos * (1f - zz));
368 //matrix.M34 = 0f;
369  
370 //matrix.M41 = matrix.M42 = matrix.M43 = 0f;
371 matrix.M44 = 1f;
372  
373 return matrix;
374 }
375  
376 /// <summary>
377 /// Construct a matrix from euler rotation values in radians
378 /// </summary>
379 /// <param name="roll">X euler angle in radians</param>
380 /// <param name="pitch">Y euler angle in radians</param>
381 /// <param name="yaw">Z euler angle in radians</param>
382 public static Matrix4 CreateFromEulers(float roll, float pitch, float yaw)
383 {
384 Matrix4 m;
385  
386 float a, b, c, d, e, f;
387 float ad, bd;
388  
389 a = (float)Math.Cos(roll);
390 b = (float)Math.Sin(roll);
391 c = (float)Math.Cos(pitch);
392 d = (float)Math.Sin(pitch);
393 e = (float)Math.Cos(yaw);
394 f = (float)Math.Sin(yaw);
395  
396 ad = a * d;
397 bd = b * d;
398  
399 m.M11 = c * e;
400 m.M12 = -c * f;
401 m.M13 = d;
402 m.M14 = 0f;
403  
404 m.M21 = bd * e + a * f;
405 m.M22 = -bd * f + a * e;
406 m.M23 = -b * c;
407 m.M24 = 0f;
408  
409 m.M31 = -ad * e + b * f;
410 m.M32 = ad * f + b * e;
411 m.M33 = a * c;
412 m.M34 = 0f;
413  
414 m.M41 = m.M42 = m.M43 = 0f;
415 m.M44 = 1f;
416  
417 return m;
418 }
419  
420 public static Matrix4 CreateFromQuaternion(Quaternion quaternion)
421 {
422 Matrix4 matrix;
423  
424 float xx = quaternion.X * quaternion.X;
425 float yy = quaternion.Y * quaternion.Y;
426 float zz = quaternion.Z * quaternion.Z;
427 float xy = quaternion.X * quaternion.Y;
428 float zw = quaternion.Z * quaternion.W;
429 float zx = quaternion.Z * quaternion.X;
430 float yw = quaternion.Y * quaternion.W;
431 float yz = quaternion.Y * quaternion.Z;
432 float xw = quaternion.X * quaternion.W;
433  
434 matrix.M11 = 1f - (2f * (yy + zz));
435 matrix.M12 = 2f * (xy + zw);
436 matrix.M13 = 2f * (zx - yw);
437 matrix.M14 = 0f;
438  
439 matrix.M21 = 2f * (xy - zw);
440 matrix.M22 = 1f - (2f * (zz + xx));
441 matrix.M23 = 2f * (yz + xw);
442 matrix.M24 = 0f;
443  
444 matrix.M31 = 2f * (zx + yw);
445 matrix.M32 = 2f * (yz - xw);
446 matrix.M33 = 1f - (2f * (yy + xx));
447 matrix.M34 = 0f;
448  
449 matrix.M41 = matrix.M42 = matrix.M43 = 0f;
450 matrix.M44 = 1f;
451  
452 return matrix;
453 }
454  
455 public static Matrix4 CreateLookAt(Vector3 cameraPosition, Vector3 cameraTarget, Vector3 cameraUpVector)
456 {
457 Matrix4 matrix;
458  
459 Vector3 z = Vector3.Normalize(cameraPosition - cameraTarget);
460 Vector3 x = Vector3.Normalize(Vector3.Cross(cameraUpVector, z));
461 Vector3 y = Vector3.Cross(z, x);
462  
463 matrix.M11 = x.X;
464 matrix.M12 = y.X;
465 matrix.M13 = z.X;
466 matrix.M14 = 0f;
467  
468 matrix.M21 = x.Y;
469 matrix.M22 = y.Y;
470 matrix.M23 = z.Y;
471 matrix.M24 = 0f;
472  
473 matrix.M31 = x.Z;
474 matrix.M32 = y.Z;
475 matrix.M33 = z.Z;
476 matrix.M34 = 0f;
477  
478 matrix.M41 = -Vector3.Dot(x, cameraPosition);
479 matrix.M42 = -Vector3.Dot(y, cameraPosition);
480 matrix.M43 = -Vector3.Dot(z, cameraPosition);
481 matrix.M44 = 1f;
482  
483 return matrix;
484 }
485  
486 public static Matrix4 CreateRotationX(float radians)
487 {
488 Matrix4 matrix;
489  
490 float cos = (float)Math.Cos(radians);
491 float sin = (float)Math.Sin(radians);
492  
493 matrix.M11 = 1f;
494 matrix.M12 = 0f;
495 matrix.M13 = 0f;
496 matrix.M14 = 0f;
497  
498 matrix.M21 = 0f;
499 matrix.M22 = cos;
500 matrix.M23 = sin;
501 matrix.M24 = 0f;
502  
503 matrix.M31 = 0f;
504 matrix.M32 = -sin;
505 matrix.M33 = cos;
506 matrix.M34 = 0f;
507  
508 matrix.M41 = 0f;
509 matrix.M42 = 0f;
510 matrix.M43 = 0f;
511 matrix.M44 = 1f;
512  
513 return matrix;
514 }
515  
516 public static Matrix4 CreateRotationY(float radians)
517 {
518 Matrix4 matrix;
519  
520 float cos = (float)Math.Cos(radians);
521 float sin = (float)Math.Sin(radians);
522  
523 matrix.M11 = cos;
524 matrix.M12 = 0f;
525 matrix.M13 = -sin;
526 matrix.M14 = 0f;
527  
528 matrix.M21 = 0f;
529 matrix.M22 = 1f;
530 matrix.M23 = 0f;
531 matrix.M24 = 0f;
532  
533 matrix.M31 = sin;
534 matrix.M32 = 0f;
535 matrix.M33 = cos;
536 matrix.M34 = 0f;
537  
538 matrix.M41 = 0f;
539 matrix.M42 = 0f;
540 matrix.M43 = 0f;
541 matrix.M44 = 1f;
542  
543 return matrix;
544 }
545  
546 public static Matrix4 CreateRotationZ(float radians)
547 {
548 Matrix4 matrix;
549  
550 float cos = (float)Math.Cos(radians);
551 float sin = (float)Math.Sin(radians);
552  
553 matrix.M11 = cos;
554 matrix.M12 = sin;
555 matrix.M13 = 0f;
556 matrix.M14 = 0f;
557  
558 matrix.M21 = -sin;
559 matrix.M22 = cos;
560 matrix.M23 = 0f;
561 matrix.M24 = 0f;
562  
563 matrix.M31 = 0f;
564 matrix.M32 = 0f;
565 matrix.M33 = 1f;
566 matrix.M34 = 0f;
567  
568 matrix.M41 = 0f;
569 matrix.M42 = 0f;
570 matrix.M43 = 0f;
571 matrix.M44 = 1f;
572  
573 return matrix;
574 }
575  
576 public static Matrix4 CreateScale(Vector3 scale)
577 {
578 Matrix4 matrix;
579  
580 matrix.M11 = scale.X;
581 matrix.M12 = 0f;
582 matrix.M13 = 0f;
583 matrix.M14 = 0f;
584  
585 matrix.M21 = 0f;
586 matrix.M22 = scale.Y;
587 matrix.M23 = 0f;
588 matrix.M24 = 0f;
589  
590 matrix.M31 = 0f;
591 matrix.M32 = 0f;
592 matrix.M33 = scale.Z;
593 matrix.M34 = 0f;
594  
595 matrix.M41 = 0f;
596 matrix.M42 = 0f;
597 matrix.M43 = 0f;
598 matrix.M44 = 1f;
599  
600 return matrix;
601 }
602  
603 public static Matrix4 CreateTranslation(Vector3 position)
604 {
605 Matrix4 matrix;
606  
607 matrix.M11 = 1f;
608 matrix.M12 = 0f;
609 matrix.M13 = 0f;
610 matrix.M14 = 0f;
611  
612 matrix.M21 = 0f;
613 matrix.M22 = 1f;
614 matrix.M23 = 0f;
615 matrix.M24 = 0f;
616  
617 matrix.M31 = 0f;
618 matrix.M32 = 0f;
619 matrix.M33 = 1f;
620 matrix.M34 = 0f;
621  
622 matrix.M41 = position.X;
623 matrix.M42 = position.Y;
624 matrix.M43 = position.Z;
625 matrix.M44 = 1f;
626  
627 return matrix;
628 }
629  
630 public static Matrix4 CreateWorld(Vector3 position, Vector3 forward, Vector3 up)
631 {
632 Matrix4 result;
633  
634 // Normalize forward vector
635 forward.Normalize();
636  
637 // Calculate right vector
638 Vector3 right = Vector3.Cross(forward, up);
639 right.Normalize();
640  
641 // Recalculate up vector
642 up = Vector3.Cross(right, forward);
643 up.Normalize();
644  
645 result.M11 = right.X;
646 result.M12 = right.Y;
647 result.M13 = right.Z;
648 result.M14 = 0.0f;
649  
650 result.M21 = up.X;
651 result.M22 = up.Y;
652 result.M23 = up.Z;
653 result.M24 = 0.0f;
654  
655 result.M31 = -forward.X;
656 result.M32 = -forward.Y;
657 result.M33 = -forward.Z;
658 result.M34 = 0.0f;
659  
660 result.M41 = position.X;
661 result.M42 = position.Y;
662 result.M43 = position.Z;
663 result.M44 = 1.0f;
664  
665 return result;
666 }
667  
668 public static Matrix4 Divide(Matrix4 matrix1, Matrix4 matrix2)
669 {
670 Matrix4 matrix;
671  
672 matrix.M11 = matrix1.M11 / matrix2.M11;
673 matrix.M12 = matrix1.M12 / matrix2.M12;
674 matrix.M13 = matrix1.M13 / matrix2.M13;
675 matrix.M14 = matrix1.M14 / matrix2.M14;
676  
677 matrix.M21 = matrix1.M21 / matrix2.M21;
678 matrix.M22 = matrix1.M22 / matrix2.M22;
679 matrix.M23 = matrix1.M23 / matrix2.M23;
680 matrix.M24 = matrix1.M24 / matrix2.M24;
681  
682 matrix.M31 = matrix1.M31 / matrix2.M31;
683 matrix.M32 = matrix1.M32 / matrix2.M32;
684 matrix.M33 = matrix1.M33 / matrix2.M33;
685 matrix.M34 = matrix1.M34 / matrix2.M34;
686  
687 matrix.M41 = matrix1.M41 / matrix2.M41;
688 matrix.M42 = matrix1.M42 / matrix2.M42;
689 matrix.M43 = matrix1.M43 / matrix2.M43;
690 matrix.M44 = matrix1.M44 / matrix2.M44;
691  
692 return matrix;
693 }
694  
695 public static Matrix4 Divide(Matrix4 matrix1, float divider)
696 {
697 Matrix4 matrix;
698  
699 float oodivider = 1f / divider;
700 matrix.M11 = matrix1.M11 * oodivider;
701 matrix.M12 = matrix1.M12 * oodivider;
702 matrix.M13 = matrix1.M13 * oodivider;
703 matrix.M14 = matrix1.M14 * oodivider;
704  
705 matrix.M21 = matrix1.M21 * oodivider;
706 matrix.M22 = matrix1.M22 * oodivider;
707 matrix.M23 = matrix1.M23 * oodivider;
708 matrix.M24 = matrix1.M24 * oodivider;
709  
710 matrix.M31 = matrix1.M31 * oodivider;
711 matrix.M32 = matrix1.M32 * oodivider;
712 matrix.M33 = matrix1.M33 * oodivider;
713 matrix.M34 = matrix1.M34 * oodivider;
714  
715 matrix.M41 = matrix1.M41 * oodivider;
716 matrix.M42 = matrix1.M42 * oodivider;
717 matrix.M43 = matrix1.M43 * oodivider;
718 matrix.M44 = matrix1.M44 * oodivider;
719  
720 return matrix;
721 }
722  
723 public static Matrix4 Lerp(Matrix4 matrix1, Matrix4 matrix2, float amount)
724 {
725 Matrix4 matrix;
726  
727 matrix.M11 = matrix1.M11 + ((matrix2.M11 - matrix1.M11) * amount);
728 matrix.M12 = matrix1.M12 + ((matrix2.M12 - matrix1.M12) * amount);
729 matrix.M13 = matrix1.M13 + ((matrix2.M13 - matrix1.M13) * amount);
730 matrix.M14 = matrix1.M14 + ((matrix2.M14 - matrix1.M14) * amount);
731  
732 matrix.M21 = matrix1.M21 + ((matrix2.M21 - matrix1.M21) * amount);
733 matrix.M22 = matrix1.M22 + ((matrix2.M22 - matrix1.M22) * amount);
734 matrix.M23 = matrix1.M23 + ((matrix2.M23 - matrix1.M23) * amount);
735 matrix.M24 = matrix1.M24 + ((matrix2.M24 - matrix1.M24) * amount);
736  
737 matrix.M31 = matrix1.M31 + ((matrix2.M31 - matrix1.M31) * amount);
738 matrix.M32 = matrix1.M32 + ((matrix2.M32 - matrix1.M32) * amount);
739 matrix.M33 = matrix1.M33 + ((matrix2.M33 - matrix1.M33) * amount);
740 matrix.M34 = matrix1.M34 + ((matrix2.M34 - matrix1.M34) * amount);
741  
742 matrix.M41 = matrix1.M41 + ((matrix2.M41 - matrix1.M41) * amount);
743 matrix.M42 = matrix1.M42 + ((matrix2.M42 - matrix1.M42) * amount);
744 matrix.M43 = matrix1.M43 + ((matrix2.M43 - matrix1.M43) * amount);
745 matrix.M44 = matrix1.M44 + ((matrix2.M44 - matrix1.M44) * amount);
746  
747 return matrix;
748 }
749  
750 public static Matrix4 Multiply(Matrix4 matrix1, Matrix4 matrix2)
751 {
752 return new Matrix4(
753 matrix1.M11 * matrix2.M11 + matrix1.M12 * matrix2.M21 + matrix1.M13 * matrix2.M31 + matrix1.M14 * matrix2.M41,
754 matrix1.M11 * matrix2.M12 + matrix1.M12 * matrix2.M22 + matrix1.M13 * matrix2.M32 + matrix1.M14 * matrix2.M42,
755 matrix1.M11 * matrix2.M13 + matrix1.M12 * matrix2.M23 + matrix1.M13 * matrix2.M33 + matrix1.M14 * matrix2.M43,
756 matrix1.M11 * matrix2.M14 + matrix1.M12 * matrix2.M24 + matrix1.M13 * matrix2.M34 + matrix1.M14 * matrix2.M44,
757  
758 matrix1.M21 * matrix2.M11 + matrix1.M22 * matrix2.M21 + matrix1.M23 * matrix2.M31 + matrix1.M24 * matrix2.M41,
759 matrix1.M21 * matrix2.M12 + matrix1.M22 * matrix2.M22 + matrix1.M23 * matrix2.M32 + matrix1.M24 * matrix2.M42,
760 matrix1.M21 * matrix2.M13 + matrix1.M22 * matrix2.M23 + matrix1.M23 * matrix2.M33 + matrix1.M24 * matrix2.M43,
761 matrix1.M21 * matrix2.M14 + matrix1.M22 * matrix2.M24 + matrix1.M23 * matrix2.M34 + matrix1.M24 * matrix2.M44,
762  
763 matrix1.M31 * matrix2.M11 + matrix1.M32 * matrix2.M21 + matrix1.M33 * matrix2.M31 + matrix1.M34 * matrix2.M41,
764 matrix1.M31 * matrix2.M12 + matrix1.M32 * matrix2.M22 + matrix1.M33 * matrix2.M32 + matrix1.M34 * matrix2.M42,
765 matrix1.M31 * matrix2.M13 + matrix1.M32 * matrix2.M23 + matrix1.M33 * matrix2.M33 + matrix1.M34 * matrix2.M43,
766 matrix1.M31 * matrix2.M14 + matrix1.M32 * matrix2.M24 + matrix1.M33 * matrix2.M34 + matrix1.M34 * matrix2.M44,
767  
768 matrix1.M41 * matrix2.M11 + matrix1.M42 * matrix2.M21 + matrix1.M43 * matrix2.M31 + matrix1.M44 * matrix2.M41,
769 matrix1.M41 * matrix2.M12 + matrix1.M42 * matrix2.M22 + matrix1.M43 * matrix2.M32 + matrix1.M44 * matrix2.M42,
770 matrix1.M41 * matrix2.M13 + matrix1.M42 * matrix2.M23 + matrix1.M43 * matrix2.M33 + matrix1.M44 * matrix2.M43,
771 matrix1.M41 * matrix2.M14 + matrix1.M42 * matrix2.M24 + matrix1.M43 * matrix2.M34 + matrix1.M44 * matrix2.M44
772 );
773 }
774  
775 public static Matrix4 Multiply(Matrix4 matrix1, float scaleFactor)
776 {
777 Matrix4 matrix;
778 matrix.M11 = matrix1.M11 * scaleFactor;
779 matrix.M12 = matrix1.M12 * scaleFactor;
780 matrix.M13 = matrix1.M13 * scaleFactor;
781 matrix.M14 = matrix1.M14 * scaleFactor;
782  
783 matrix.M21 = matrix1.M21 * scaleFactor;
784 matrix.M22 = matrix1.M22 * scaleFactor;
785 matrix.M23 = matrix1.M23 * scaleFactor;
786 matrix.M24 = matrix1.M24 * scaleFactor;
787  
788 matrix.M31 = matrix1.M31 * scaleFactor;
789 matrix.M32 = matrix1.M32 * scaleFactor;
790 matrix.M33 = matrix1.M33 * scaleFactor;
791 matrix.M34 = matrix1.M34 * scaleFactor;
792  
793 matrix.M41 = matrix1.M41 * scaleFactor;
794 matrix.M42 = matrix1.M42 * scaleFactor;
795 matrix.M43 = matrix1.M43 * scaleFactor;
796 matrix.M44 = matrix1.M44 * scaleFactor;
797 return matrix;
798 }
799  
800 public static Matrix4 Negate(Matrix4 matrix)
801 {
802 Matrix4 result;
803 result.M11 = -matrix.M11;
804 result.M12 = -matrix.M12;
805 result.M13 = -matrix.M13;
806 result.M14 = -matrix.M14;
807  
808 result.M21 = -matrix.M21;
809 result.M22 = -matrix.M22;
810 result.M23 = -matrix.M23;
811 result.M24 = -matrix.M24;
812  
813 result.M31 = -matrix.M31;
814 result.M32 = -matrix.M32;
815 result.M33 = -matrix.M33;
816 result.M34 = -matrix.M34;
817  
818 result.M41 = -matrix.M41;
819 result.M42 = -matrix.M42;
820 result.M43 = -matrix.M43;
821 result.M44 = -matrix.M44;
822 return result;
823 }
824  
825 public static Matrix4 Subtract(Matrix4 matrix1, Matrix4 matrix2)
826 {
827 Matrix4 matrix;
828 matrix.M11 = matrix1.M11 - matrix2.M11;
829 matrix.M12 = matrix1.M12 - matrix2.M12;
830 matrix.M13 = matrix1.M13 - matrix2.M13;
831 matrix.M14 = matrix1.M14 - matrix2.M14;
832  
833 matrix.M21 = matrix1.M21 - matrix2.M21;
834 matrix.M22 = matrix1.M22 - matrix2.M22;
835 matrix.M23 = matrix1.M23 - matrix2.M23;
836 matrix.M24 = matrix1.M24 - matrix2.M24;
837  
838 matrix.M31 = matrix1.M31 - matrix2.M31;
839 matrix.M32 = matrix1.M32 - matrix2.M32;
840 matrix.M33 = matrix1.M33 - matrix2.M33;
841 matrix.M34 = matrix1.M34 - matrix2.M34;
842  
843 matrix.M41 = matrix1.M41 - matrix2.M41;
844 matrix.M42 = matrix1.M42 - matrix2.M42;
845 matrix.M43 = matrix1.M43 - matrix2.M43;
846 matrix.M44 = matrix1.M44 - matrix2.M44;
847 return matrix;
848 }
849  
850 public static Matrix4 Transform(Matrix4 value, Quaternion rotation)
851 {
852 Matrix4 matrix;
853  
854 float x2 = rotation.X + rotation.X;
855 float y2 = rotation.Y + rotation.Y;
856 float z2 = rotation.Z + rotation.Z;
857  
858 float a = (1f - rotation.Y * y2) - rotation.Z * z2;
859 float b = rotation.X * y2 - rotation.W * z2;
860 float c = rotation.X * z2 + rotation.W * y2;
861 float d = rotation.X * y2 + rotation.W * z2;
862 float e = (1f - rotation.X * x2) - rotation.Z * z2;
863 float f = rotation.Y * z2 - rotation.W * x2;
864 float g = rotation.X * z2 - rotation.W * y2;
865 float h = rotation.Y * z2 + rotation.W * x2;
866 float i = (1f - rotation.X * x2) - rotation.Y * y2;
867  
868 matrix.M11 = ((value.M11 * a) + (value.M12 * b)) + (value.M13 * c);
869 matrix.M12 = ((value.M11 * d) + (value.M12 * e)) + (value.M13 * f);
870 matrix.M13 = ((value.M11 * g) + (value.M12 * h)) + (value.M13 * i);
871 matrix.M14 = value.M14;
872  
873 matrix.M21 = ((value.M21 * a) + (value.M22 * b)) + (value.M23 * c);
874 matrix.M22 = ((value.M21 * d) + (value.M22 * e)) + (value.M23 * f);
875 matrix.M23 = ((value.M21 * g) + (value.M22 * h)) + (value.M23 * i);
876 matrix.M24 = value.M24;
877  
878 matrix.M31 = ((value.M31 * a) + (value.M32 * b)) + (value.M33 * c);
879 matrix.M32 = ((value.M31 * d) + (value.M32 * e)) + (value.M33 * f);
880 matrix.M33 = ((value.M31 * g) + (value.M32 * h)) + (value.M33 * i);
881 matrix.M34 = value.M34;
882  
883 matrix.M41 = ((value.M41 * a) + (value.M42 * b)) + (value.M43 * c);
884 matrix.M42 = ((value.M41 * d) + (value.M42 * e)) + (value.M43 * f);
885 matrix.M43 = ((value.M41 * g) + (value.M42 * h)) + (value.M43 * i);
886 matrix.M44 = value.M44;
887  
888 return matrix;
889 }
890  
891 public static Matrix4 Transpose(Matrix4 matrix)
892 {
893 Matrix4 result;
894  
895 result.M11 = matrix.M11;
896 result.M12 = matrix.M21;
897 result.M13 = matrix.M31;
898 result.M14 = matrix.M41;
899  
900 result.M21 = matrix.M12;
901 result.M22 = matrix.M22;
902 result.M23 = matrix.M32;
903 result.M24 = matrix.M42;
904  
905 result.M31 = matrix.M13;
906 result.M32 = matrix.M23;
907 result.M33 = matrix.M33;
908 result.M34 = matrix.M43;
909  
910 result.M41 = matrix.M14;
911 result.M42 = matrix.M24;
912 result.M43 = matrix.M34;
913 result.M44 = matrix.M44;
914  
915 return result;
916 }
917  
918 public static Matrix4 Inverse3x3(Matrix4 matrix)
919 {
920 if (matrix.Determinant3x3() == 0f)
921 throw new ArgumentException("Singular matrix inverse not possible");
922  
923 return (Adjoint3x3(matrix) / matrix.Determinant3x3());
924 }
925  
926 public static Matrix4 Adjoint3x3(Matrix4 matrix)
927 {
928 Matrix4 adjointMatrix = new Matrix4();
929 for (int i = 0; i < 4; i++)
930 {
931 for (int j = 0; j < 4; j++)
932 adjointMatrix[i,j] = (float)(Math.Pow(-1, i + j) * (Minor(matrix, i, j).Determinant3x3()));
933 }
934  
935 adjointMatrix = Transpose(adjointMatrix);
936 return adjointMatrix;
937 }
938  
939 public static Matrix4 Inverse(Matrix4 matrix)
940 {
941 if (matrix.Determinant() == 0f)
942 throw new ArgumentException("Singular matrix inverse not possible");
943  
944 return (Adjoint(matrix) / matrix.Determinant());
945 }
946  
947 public static Matrix4 Adjoint(Matrix4 matrix)
948 {
949 Matrix4 adjointMatrix = new Matrix4();
950 for (int i = 0; i < 4; i++)
951 {
952 for (int j = 0; j < 4; j++)
953 adjointMatrix[i,j] = (float)(Math.Pow(-1, i + j) * ((Minor(matrix, i, j)).Determinant()));
954 }
955  
956 adjointMatrix = Transpose(adjointMatrix);
957 return adjointMatrix;
958 }
959  
960 public static Matrix4 Minor(Matrix4 matrix, int row, int col)
961 {
962 Matrix4 minor = new Matrix4();
963 int m = 0, n = 0;
964  
965 for (int i = 0; i < 4; i++)
966 {
967 if (i == row)
968 continue;
969 n = 0;
970 for (int j = 0; j < 4; j++)
971 {
972 if (j == col)
973 continue;
974 minor[m,n] = matrix[i,j];
975 n++;
976 }
977 m++;
978 }
979  
980 return minor;
981 }
982  
983 #endregion Static Methods
984  
985 #region Overrides
986  
987 public override bool Equals(object obj)
988 {
989 return (obj is Matrix4) ? this == (Matrix4)obj : false;
990 }
991  
992 public bool Equals(Matrix4 other)
993 {
994 return this == other;
995 }
996  
997 public override int GetHashCode()
998 {
999 return
1000 M11.GetHashCode() ^ M12.GetHashCode() ^ M13.GetHashCode() ^ M14.GetHashCode() ^
1001 M21.GetHashCode() ^ M22.GetHashCode() ^ M23.GetHashCode() ^ M24.GetHashCode() ^
1002 M31.GetHashCode() ^ M32.GetHashCode() ^ M33.GetHashCode() ^ M34.GetHashCode() ^
1003 M41.GetHashCode() ^ M42.GetHashCode() ^ M43.GetHashCode() ^ M44.GetHashCode();
1004 }
1005  
1006 /// <summary>
1007 /// Get a formatted string representation of the vector
1008 /// </summary>
1009 /// <returns>A string representation of the vector</returns>
1010 public override string ToString()
1011 {
1012 return string.Format(Utils.EnUsCulture,
1013 "|{0}, {1}, {2}, {3}|\n|{4}, {5}, {6}, {7}|\n|{8}, {9}, {10}, {11}|\n|{12}, {13}, {14}, {15}|",
1014 M11, M12, M13, M14, M21, M22, M23, M24, M31, M32, M33, M34, M41, M42, M43, M44);
1015 }
1016  
1017 #endregion Overrides
1018  
1019 #region Operators
1020  
1021 public static bool operator ==(Matrix4 left, Matrix4 right)
1022 {
1023 return left.Equals(right);
1024 }
1025  
1026 public static bool operator !=(Matrix4 left, Matrix4 right)
1027 {
1028 return !left.Equals(right);
1029 }
1030  
1031 public static Matrix4 operator +(Matrix4 left, Matrix4 right)
1032 {
1033 return Add(left, right);
1034 }
1035  
1036 public static Matrix4 operator -(Matrix4 matrix)
1037 {
1038 return Negate(matrix);
1039 }
1040  
1041 public static Matrix4 operator -(Matrix4 left, Matrix4 right)
1042 {
1043 return Subtract(left, right);
1044 }
1045  
1046 public static Matrix4 operator *(Matrix4 left, Matrix4 right)
1047 {
1048 return Multiply(left, right);
1049 }
1050  
1051 public static Matrix4 operator *(Matrix4 left, float scalar)
1052 {
1053 return Multiply(left, scalar);
1054 }
1055  
1056 public static Matrix4 operator /(Matrix4 left, Matrix4 right)
1057 {
1058 return Divide(left, right);
1059 }
1060  
1061 public static Matrix4 operator /(Matrix4 matrix, float divider)
1062 {
1063 return Divide(matrix, divider);
1064 }
1065  
1066 public Vector4 this[int row]
1067 {
1068 get
1069 {
1070 switch (row)
1071 {
1072 case 0:
1073 return new Vector4(M11, M12, M13, M14);
1074 case 1:
1075 return new Vector4(M21, M22, M23, M24);
1076 case 2:
1077 return new Vector4(M31, M32, M33, M34);
1078 case 3:
1079 return new Vector4(M41, M42, M43, M44);
1080 default:
1081 throw new IndexOutOfRangeException("Matrix4 row index must be from 0-3");
1082 }
1083 }
1084 set
1085 {
1086 switch (row)
1087 {
1088 case 0:
1089 M11 = value.X;
1090 M12 = value.Y;
1091 M13 = value.Z;
1092 M14 = value.W;
1093 break;
1094 case 1:
1095 M21 = value.X;
1096 M22 = value.Y;
1097 M23 = value.Z;
1098 M24 = value.W;
1099 break;
1100 case 2:
1101 M31 = value.X;
1102 M32 = value.Y;
1103 M33 = value.Z;
1104 M34 = value.W;
1105 break;
1106 case 3:
1107 M41 = value.X;
1108 M42 = value.Y;
1109 M43 = value.Z;
1110 M44 = value.W;
1111 break;
1112 default:
1113 throw new IndexOutOfRangeException("Matrix4 row index must be from 0-3");
1114 }
1115 }
1116 }
1117  
1118 public float this[int row, int column]
1119 {
1120 get
1121 {
1122 switch (row)
1123 {
1124 case 0:
1125 switch (column)
1126 {
1127 case 0:
1128 return M11;
1129 case 1:
1130 return M12;
1131 case 2:
1132 return M13;
1133 case 3:
1134 return M14;
1135 default:
1136 throw new IndexOutOfRangeException("Matrix4 row and column values must be from 0-3");
1137 }
1138 case 1:
1139 switch (column)
1140 {
1141 case 0:
1142 return M21;
1143 case 1:
1144 return M22;
1145 case 2:
1146 return M23;
1147 case 3:
1148 return M24;
1149 default:
1150 throw new IndexOutOfRangeException("Matrix4 row and column values must be from 0-3");
1151 }
1152 case 2:
1153 switch (column)
1154 {
1155 case 0:
1156 return M31;
1157 case 1:
1158 return M32;
1159 case 2:
1160 return M33;
1161 case 3:
1162 return M34;
1163 default:
1164 throw new IndexOutOfRangeException("Matrix4 row and column values must be from 0-3");
1165 }
1166 case 3:
1167 switch (column)
1168 {
1169 case 0:
1170 return M41;
1171 case 1:
1172 return M42;
1173 case 2:
1174 return M43;
1175 case 3:
1176 return M44;
1177 default:
1178 throw new IndexOutOfRangeException("Matrix4 row and column values must be from 0-3");
1179 }
1180 default:
1181 throw new IndexOutOfRangeException("Matrix4 row and column values must be from 0-3");
1182 }
1183 }
1184 set
1185 {
1186 switch (row)
1187 {
1188 case 0:
1189 switch (column)
1190 {
1191 case 0:
1192 M11 = value; return;
1193 case 1:
1194 M12 = value; return;
1195 case 2:
1196 M13 = value; return;
1197 case 3:
1198 M14 = value; return;
1199 default:
1200 throw new IndexOutOfRangeException("Matrix4 row and column values must be from 0-3");
1201 }
1202 case 1:
1203 switch (column)
1204 {
1205 case 0:
1206 M21 = value; return;
1207 case 1:
1208 M22 = value; return;
1209 case 2:
1210 M23 = value; return;
1211 case 3:
1212 M24 = value; return;
1213 default:
1214 throw new IndexOutOfRangeException("Matrix4 row and column values must be from 0-3");
1215 }
1216 case 2:
1217 switch (column)
1218 {
1219 case 0:
1220 M31 = value; return;
1221 case 1:
1222 M32 = value; return;
1223 case 2:
1224 M33 = value; return;
1225 case 3:
1226 M34 = value; return;
1227 default:
1228 throw new IndexOutOfRangeException("Matrix4 row and column values must be from 0-3");
1229 }
1230 case 3:
1231 switch (column)
1232 {
1233 case 0:
1234 M41 = value; return;
1235 case 1:
1236 M42 = value; return;
1237 case 2:
1238 M43 = value; return;
1239 case 3:
1240 M44 = value; return;
1241 default:
1242 throw new IndexOutOfRangeException("Matrix4 row and column values must be from 0-3");
1243 }
1244 default:
1245 throw new IndexOutOfRangeException("Matrix4 row and column values must be from 0-3");
1246 }
1247 }
1248 }
1249  
1250 #endregion Operators
1251  
1252 /// <summary>A 4x4 matrix containing all zeroes</summary>
1253 public static readonly Matrix4 Zero = new Matrix4();
1254  
1255 /// <summary>A 4x4 identity matrix</summary>
1256 public static readonly Matrix4 Identity = new Matrix4(
1257 1f, 0f, 0f, 0f,
1258 0f, 1f, 0f, 0f,
1259 0f, 0f, 1f, 0f,
1260 0f, 0f, 0f, 1f);
1261 }
1262 }