From 4dd67d30fa0dd54930c4a921a8d6bc2f2e42ed9a Mon Sep 17 00:00:00 2001 From: Drew DeVault Date: Wed, 6 Apr 2016 21:37:11 -0400 Subject: [PATCH] Reuse client mechanism for movement on entities Also brings in some MonoGame math classes to fulfill this purpose. --- TrueCraft.API/Matrix.cs | 1663 +++++++++++++++++ TrueCraft.API/TrueCraft.API.csproj | 1 + TrueCraft.API/Vector3.cs | 8 + TrueCraft.Client/Modules/HighlightModule.cs | 1 + .../Modules/PlayerControlModule.cs | 1 + TrueCraft.Core/AI/WanderState.cs | 2 +- TrueCraft.Core/Entities/MobEntity.cs | 6 +- TrueCraft.Core/MathHelper.cs | 5 + 8 files changed, 1683 insertions(+), 4 deletions(-) create mode 100644 TrueCraft.API/Matrix.cs diff --git a/TrueCraft.API/Matrix.cs b/TrueCraft.API/Matrix.cs new file mode 100644 index 0000000..a98a1dc --- /dev/null +++ b/TrueCraft.API/Matrix.cs @@ -0,0 +1,1663 @@ +// MIT License - Copyright (C) The Mono.Xna Team +// This file is subject to the terms and conditions defined in +// file 'LICENSE.txt', which is part of this source code package. + +using System; + +namespace TrueCraft.API +{ + /// + /// Represents the right-handed 4x4 doubleing point matrix, which can store translation, scale and rotation information. + /// + public struct Matrix : IEquatable + { + #region Public Constructors + + /// + /// Constructs a matrix. + /// + /// A first row and first column value. + /// A first row and second column value. + /// A first row and third column value. + /// A first row and fourth column value. + /// A second row and first column value. + /// A second row and second column value. + /// A second row and third column value. + /// A second row and fourth column value. + /// A third row and first column value. + /// A third row and second column value. + /// A third row and third column value. + /// A third row and fourth column value. + /// A fourth row and first column value. + /// A fourth row and second column value. + /// A fourth row and third column value. + /// A fourth row and fourth column value. + public Matrix(double m11, double m12, double m13, double m14, double m21, double m22, double m23, double m24, double m31, + double m32, double m33, double m34, double m41, double m42, double m43, double m44) + { + this.M11 = m11; + this.M12 = m12; + this.M13 = m13; + this.M14 = m14; + this.M21 = m21; + this.M22 = m22; + this.M23 = m23; + this.M24 = m24; + this.M31 = m31; + this.M32 = m32; + this.M33 = m33; + this.M34 = m34; + this.M41 = m41; + this.M42 = m42; + this.M43 = m43; + this.M44 = m44; + } + + #endregion + + #region Public Fields + + /// + /// A first row and first column value. + /// + public double M11; + + /// + /// A first row and second column value. + /// + public double M12; + + /// + /// A first row and third column value. + /// + public double M13; + + /// + /// A first row and fourth column value. + /// + public double M14; + + /// + /// A second row and first column value. + /// + public double M21; + + /// + /// A second row and second column value. + /// + public double M22; + + /// + /// A second row and third column value. + /// + public double M23; + + /// + /// A second row and fourth column value. + /// + public double M24; + + /// + /// A third row and first column value. + /// + public double M31; + + /// + /// A third row and second column value. + /// + public double M32; + + /// + /// A third row and third column value. + /// + public double M33; + + /// + /// A third row and fourth column value. + /// + public double M34; + + /// + /// A fourth row and first column value. + /// + public double M41; + + /// + /// A fourth row and second column value. + /// + public double M42; + + /// + /// A fourth row and third column value. + /// + public double M43; + + /// + /// A fourth row and fourth column value. + /// + public double M44; + + #endregion + + #region Indexers + + public double this[int index] + { + get + { + switch (index) + { + case 0: return M11; + case 1: return M12; + case 2: return M13; + case 3: return M14; + case 4: return M21; + case 5: return M22; + case 6: return M23; + case 7: return M24; + case 8: return M31; + case 9: return M32; + case 10: return M33; + case 11: return M34; + case 12: return M41; + case 13: return M42; + case 14: return M43; + case 15: return M44; + } + throw new ArgumentOutOfRangeException(); + } + + set + { + switch (index) + { + case 0: M11 = value; break; + case 1: M12 = value; break; + case 2: M13 = value; break; + case 3: M14 = value; break; + case 4: M21 = value; break; + case 5: M22 = value; break; + case 6: M23 = value; break; + case 7: M24 = value; break; + case 8: M31 = value; break; + case 9: M32 = value; break; + case 10: M33 = value; break; + case 11: M34 = value; break; + case 12: M41 = value; break; + case 13: M42 = value; break; + case 14: M43 = value; break; + case 15: M44 = value; break; + default: throw new ArgumentOutOfRangeException(); + } + } + } + + public double this[int row, int column] + { + get + { + return this[(row * 4) + column]; + } + + set + { + this[(row * 4) + column] = value; + } + } + + #endregion + + #region Private Members + private static Matrix identity = new Matrix(1f, 0f, 0f, 0f, + 0f, 1f, 0f, 0f, + 0f, 0f, 1f, 0f, + 0f, 0f, 0f, 1f); + #endregion + + #region Public Properties + + /// + /// The backward vector formed from the third row M31, M32, M33 elements. + /// + public Vector3 Backward + { + get + { + return new Vector3(this.M31, this.M32, this.M33); + } + set + { + this.M31 = value.X; + this.M32 = value.Y; + this.M33 = value.Z; + } + } + + /// + /// The down vector formed from the second row -M21, -M22, -M23 elements. + /// + public Vector3 Down + { + get + { + return new Vector3(-this.M21, -this.M22, -this.M23); + } + set + { + this.M21 = -value.X; + this.M22 = -value.Y; + this.M23 = -value.Z; + } + } + + /// + /// The forward vector formed from the third row -M31, -M32, -M33 elements. + /// + public Vector3 Forward + { + get + { + return new Vector3(-this.M31, -this.M32, -this.M33); + } + set + { + this.M31 = -value.X; + this.M32 = -value.Y; + this.M33 = -value.Z; + } + } + + /// + /// Returns the identity matrix. + /// + public static Matrix Identity + { + get { return identity; } + } + + /// + /// The left vector formed from the first row -M11, -M12, -M13 elements. + /// + public Vector3 Left + { + get + { + return new Vector3(-this.M11, -this.M12, -this.M13); + } + set + { + this.M11 = -value.X; + this.M12 = -value.Y; + this.M13 = -value.Z; + } + } + + /// + /// The right vector formed from the first row M11, M12, M13 elements. + /// + public Vector3 Right + { + get + { + return new Vector3(this.M11, this.M12, this.M13); + } + set + { + this.M11 = value.X; + this.M12 = value.Y; + this.M13 = value.Z; + } + } + + /// + /// Position stored in this matrix. + /// + public Vector3 Translation + { + get + { + return new Vector3(this.M41, this.M42, this.M43); + } + set + { + this.M41 = value.X; + this.M42 = value.Y; + this.M43 = value.Z; + } + } + + /// + /// Scale stored in this matrix. + /// + public Vector3 Scale + { + get + { + return new Vector3(this.M11, this.M22, this.M33); + } + set + { + this.M11 = value.X; + this.M22 = value.Y; + this.M33 = value.Z; + } + } + + /// + /// The upper vector formed from the second row M21, M22, M23 elements. + /// + public Vector3 Up + { + get + { + return new Vector3(this.M21, this.M22, this.M23); + } + set + { + this.M21 = value.X; + this.M22 = value.Y; + this.M23 = value.Z; + } + } + #endregion + + #region Public Methods + + /// + /// Creates a new which contains sum of two matrixes. + /// + /// The first matrix to add. + /// The second matrix to add. + /// The result of the matrix addition. + public static Matrix Add(Matrix matrix1, Matrix matrix2) + { + matrix1.M11 += matrix2.M11; + matrix1.M12 += matrix2.M12; + matrix1.M13 += matrix2.M13; + matrix1.M14 += matrix2.M14; + matrix1.M21 += matrix2.M21; + matrix1.M22 += matrix2.M22; + matrix1.M23 += matrix2.M23; + matrix1.M24 += matrix2.M24; + matrix1.M31 += matrix2.M31; + matrix1.M32 += matrix2.M32; + matrix1.M33 += matrix2.M33; + matrix1.M34 += matrix2.M34; + matrix1.M41 += matrix2.M41; + matrix1.M42 += matrix2.M42; + matrix1.M43 += matrix2.M43; + matrix1.M44 += matrix2.M44; + return matrix1; + } + + /// + /// Creates a new which contains sum of two matrixes. + /// + /// The first matrix to add. + /// The second matrix to add. + /// The result of the matrix addition as an output parameter. + public static void Add(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) + { + result.M11 = matrix1.M11 + matrix2.M11; + result.M12 = matrix1.M12 + matrix2.M12; + result.M13 = matrix1.M13 + matrix2.M13; + result.M14 = matrix1.M14 + matrix2.M14; + result.M21 = matrix1.M21 + matrix2.M21; + result.M22 = matrix1.M22 + matrix2.M22; + result.M23 = matrix1.M23 + matrix2.M23; + result.M24 = matrix1.M24 + matrix2.M24; + result.M31 = matrix1.M31 + matrix2.M31; + result.M32 = matrix1.M32 + matrix2.M32; + result.M33 = matrix1.M33 + matrix2.M33; + result.M34 = matrix1.M34 + matrix2.M34; + result.M41 = matrix1.M41 + matrix2.M41; + result.M42 = matrix1.M42 + matrix2.M42; + result.M43 = matrix1.M43 + matrix2.M43; + result.M44 = matrix1.M44 + matrix2.M44; + + } + + /// + /// Creates a new rotation around X axis. + /// + /// Angle in radians. + /// The rotation around X axis. + public static Matrix CreateRotationX(double radians) + { + Matrix result; + CreateRotationX(radians, out result); + return result; + } + + /// + /// Creates a new rotation around X axis. + /// + /// Angle in radians. + /// The rotation around X axis as an output parameter. + public static void CreateRotationX(double radians, out Matrix result) + { + result = Matrix.Identity; + + var val1 = (double)Math.Cos(radians); + var val2 = (double)Math.Sin(radians); + + result.M22 = val1; + result.M23 = val2; + result.M32 = -val2; + result.M33 = val1; + } + + /// + /// Creates a new rotation around Y axis. + /// + /// Angle in radians. + /// The rotation around Y axis. + public static Matrix CreateRotationY(double radians) + { + Matrix result; + CreateRotationY(radians, out result); + return result; + } + + /// + /// Creates a new rotation around Y axis. + /// + /// Angle in radians. + /// The rotation around Y axis as an output parameter. + public static void CreateRotationY(double radians, out Matrix result) + { + result = Matrix.Identity; + + var val1 = (double)Math.Cos(radians); + var val2 = (double)Math.Sin(radians); + + result.M11 = val1; + result.M13 = -val2; + result.M31 = val2; + result.M33 = val1; + } + + /// + /// Creates a new rotation around Z axis. + /// + /// Angle in radians. + /// The rotation around Z axis. + public static Matrix CreateRotationZ(double radians) + { + Matrix result; + CreateRotationZ(radians, out result); + return result; + } + + /// + /// Creates a new rotation around Z axis. + /// + /// Angle in radians. + /// The rotation around Z axis as an output parameter. + public static void CreateRotationZ(double radians, out Matrix result) + { + result = Matrix.Identity; + + var val1 = (double)Math.Cos(radians); + var val2 = (double)Math.Sin(radians); + + result.M11 = val1; + result.M12 = val2; + result.M21 = -val2; + result.M22 = val1; + } + + /// + /// Creates a new scaling . + /// + /// Scale value for all three axises. + /// The scaling . + public static Matrix CreateScale(double scale) + { + Matrix result; + CreateScale(scale, scale, scale, out result); + return result; + } + + /// + /// Creates a new scaling . + /// + /// Scale value for all three axises. + /// The scaling as an output parameter. + public static void CreateScale(double scale, out Matrix result) + { + CreateScale(scale, scale, scale, out result); + } + + /// + /// Creates a new scaling . + /// + /// Scale value for X axis. + /// Scale value for Y axis. + /// Scale value for Z axis. + /// The scaling . + public static Matrix CreateScale(double xScale, double yScale, double zScale) + { + Matrix result; + CreateScale(xScale, yScale, zScale, out result); + return result; + } + + /// + /// Creates a new scaling . + /// + /// Scale value for X axis. + /// Scale value for Y axis. + /// Scale value for Z axis. + /// The scaling as an output parameter. + public static void CreateScale(double xScale, double yScale, double zScale, out Matrix result) + { + result.M11 = xScale; + result.M12 = 0; + result.M13 = 0; + result.M14 = 0; + result.M21 = 0; + result.M22 = yScale; + result.M23 = 0; + result.M24 = 0; + result.M31 = 0; + result.M32 = 0; + result.M33 = zScale; + result.M34 = 0; + result.M41 = 0; + result.M42 = 0; + result.M43 = 0; + result.M44 = 1; + } + + /// + /// Creates a new scaling . + /// + /// representing x,y and z scale values. + /// The scaling . + public static Matrix CreateScale(Vector3 scales) + { + Matrix result; + CreateScale(ref scales, out result); + return result; + } + + /// + /// Creates a new scaling . + /// + /// representing x,y and z scale values. + /// The scaling as an output parameter. + public static void CreateScale(ref Vector3 scales, out Matrix result) + { + result.M11 = scales.X; + result.M12 = 0; + result.M13 = 0; + result.M14 = 0; + result.M21 = 0; + result.M22 = scales.Y; + result.M23 = 0; + result.M24 = 0; + result.M31 = 0; + result.M32 = 0; + result.M33 = scales.Z; + result.M34 = 0; + result.M41 = 0; + result.M42 = 0; + result.M43 = 0; + result.M44 = 1; + } + + + /// + /// Creates a new translation . + /// + /// X coordinate of translation. + /// Y coordinate of translation. + /// Z coordinate of translation. + /// The translation . + public static Matrix CreateTranslation(double xPosition, double yPosition, double zPosition) + { + Matrix result; + CreateTranslation(xPosition, yPosition, zPosition, out result); + return result; + } + + /// + /// Creates a new translation . + /// + /// X,Y and Z coordinates of translation. + /// The translation as an output parameter. + public static void CreateTranslation(ref Vector3 position, out Matrix result) + { + result.M11 = 1; + result.M12 = 0; + result.M13 = 0; + result.M14 = 0; + result.M21 = 0; + result.M22 = 1; + result.M23 = 0; + result.M24 = 0; + result.M31 = 0; + result.M32 = 0; + result.M33 = 1; + result.M34 = 0; + result.M41 = position.X; + result.M42 = position.Y; + result.M43 = position.Z; + result.M44 = 1; + } + + /// + /// Creates a new translation . + /// + /// X,Y and Z coordinates of translation. + /// The translation . + public static Matrix CreateTranslation(Vector3 position) + { + Matrix result; + CreateTranslation(ref position, out result); + return result; + } + + /// + /// Creates a new translation . + /// + /// X coordinate of translation. + /// Y coordinate of translation. + /// Z coordinate of translation. + /// The translation as an output parameter. + public static void CreateTranslation(double xPosition, double yPosition, double zPosition, out Matrix result) + { + result.M11 = 1; + result.M12 = 0; + result.M13 = 0; + result.M14 = 0; + result.M21 = 0; + result.M22 = 1; + result.M23 = 0; + result.M24 = 0; + result.M31 = 0; + result.M32 = 0; + result.M33 = 1; + result.M34 = 0; + result.M41 = xPosition; + result.M42 = yPosition; + result.M43 = zPosition; + result.M44 = 1; + } + + /// + /// Returns a determinant of this . + /// + /// Determinant of this + /// See more about determinant here - http://en.wikipedia.org/wiki/Determinant. + /// + public double Determinant() + { + double num22 = this.M11; + double num21 = this.M12; + double num20 = this.M13; + double num19 = this.M14; + double num12 = this.M21; + double num11 = this.M22; + double num10 = this.M23; + double num9 = this.M24; + double num8 = this.M31; + double num7 = this.M32; + double num6 = this.M33; + double num5 = this.M34; + double num4 = this.M41; + double num3 = this.M42; + double num2 = this.M43; + double num = this.M44; + double num18 = (num6 * num) - (num5 * num2); + double num17 = (num7 * num) - (num5 * num3); + double num16 = (num7 * num2) - (num6 * num3); + double num15 = (num8 * num) - (num5 * num4); + double num14 = (num8 * num2) - (num6 * num4); + double num13 = (num8 * num3) - (num7 * num4); + return ((((num22 * (((num11 * num18) - (num10 * num17)) + (num9 * num16))) - (num21 * (((num12 * num18) - (num10 * num15)) + (num9 * num14)))) + (num20 * (((num12 * num17) - (num11 * num15)) + (num9 * num13)))) - (num19 * (((num12 * num16) - (num11 * num14)) + (num10 * num13)))); + } + + /// + /// Divides the elements of a by the elements of another matrix. + /// + /// Source . + /// Divisor . + /// The result of dividing the matrix. + public static Matrix Divide(Matrix matrix1, Matrix matrix2) + { + matrix1.M11 = matrix1.M11 / matrix2.M11; + matrix1.M12 = matrix1.M12 / matrix2.M12; + matrix1.M13 = matrix1.M13 / matrix2.M13; + matrix1.M14 = matrix1.M14 / matrix2.M14; + matrix1.M21 = matrix1.M21 / matrix2.M21; + matrix1.M22 = matrix1.M22 / matrix2.M22; + matrix1.M23 = matrix1.M23 / matrix2.M23; + matrix1.M24 = matrix1.M24 / matrix2.M24; + matrix1.M31 = matrix1.M31 / matrix2.M31; + matrix1.M32 = matrix1.M32 / matrix2.M32; + matrix1.M33 = matrix1.M33 / matrix2.M33; + matrix1.M34 = matrix1.M34 / matrix2.M34; + matrix1.M41 = matrix1.M41 / matrix2.M41; + matrix1.M42 = matrix1.M42 / matrix2.M42; + matrix1.M43 = matrix1.M43 / matrix2.M43; + matrix1.M44 = matrix1.M44 / matrix2.M44; + return matrix1; + } + + /// + /// Divides the elements of a by the elements of another matrix. + /// + /// Source . + /// Divisor . + /// The result of dividing the matrix as an output parameter. + public static void Divide(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) + { + result.M11 = matrix1.M11 / matrix2.M11; + result.M12 = matrix1.M12 / matrix2.M12; + result.M13 = matrix1.M13 / matrix2.M13; + result.M14 = matrix1.M14 / matrix2.M14; + result.M21 = matrix1.M21 / matrix2.M21; + result.M22 = matrix1.M22 / matrix2.M22; + result.M23 = matrix1.M23 / matrix2.M23; + result.M24 = matrix1.M24 / matrix2.M24; + result.M31 = matrix1.M31 / matrix2.M31; + result.M32 = matrix1.M32 / matrix2.M32; + result.M33 = matrix1.M33 / matrix2.M33; + result.M34 = matrix1.M34 / matrix2.M34; + result.M41 = matrix1.M41 / matrix2.M41; + result.M42 = matrix1.M42 / matrix2.M42; + result.M43 = matrix1.M43 / matrix2.M43; + result.M44 = matrix1.M44 / matrix2.M44; + } + + /// + /// Divides the elements of a by a scalar. + /// + /// Source . + /// Divisor scalar. + /// The result of dividing a matrix by a scalar. + public static Matrix Divide(Matrix matrix1, double divider) + { + double num = 1f / divider; + matrix1.M11 = matrix1.M11 * num; + matrix1.M12 = matrix1.M12 * num; + matrix1.M13 = matrix1.M13 * num; + matrix1.M14 = matrix1.M14 * num; + matrix1.M21 = matrix1.M21 * num; + matrix1.M22 = matrix1.M22 * num; + matrix1.M23 = matrix1.M23 * num; + matrix1.M24 = matrix1.M24 * num; + matrix1.M31 = matrix1.M31 * num; + matrix1.M32 = matrix1.M32 * num; + matrix1.M33 = matrix1.M33 * num; + matrix1.M34 = matrix1.M34 * num; + matrix1.M41 = matrix1.M41 * num; + matrix1.M42 = matrix1.M42 * num; + matrix1.M43 = matrix1.M43 * num; + matrix1.M44 = matrix1.M44 * num; + return matrix1; + } + + /// + /// Divides the elements of a by a scalar. + /// + /// Source . + /// Divisor scalar. + /// The result of dividing a matrix by a scalar as an output parameter. + public static void Divide(ref Matrix matrix1, double divider, out Matrix result) + { + double num = 1f / divider; + result.M11 = matrix1.M11 * num; + result.M12 = matrix1.M12 * num; + result.M13 = matrix1.M13 * num; + result.M14 = matrix1.M14 * num; + result.M21 = matrix1.M21 * num; + result.M22 = matrix1.M22 * num; + result.M23 = matrix1.M23 * num; + result.M24 = matrix1.M24 * num; + result.M31 = matrix1.M31 * num; + result.M32 = matrix1.M32 * num; + result.M33 = matrix1.M33 * num; + result.M34 = matrix1.M34 * num; + result.M41 = matrix1.M41 * num; + result.M42 = matrix1.M42 * num; + result.M43 = matrix1.M43 * num; + result.M44 = matrix1.M44 * num; + } + + /// + /// Compares whether current instance is equal to specified without any tolerance. + /// + /// The to compare. + /// true if the instances are equal; false otherwise. + public bool Equals(Matrix other) + { + return ((((((this.M11 == other.M11) && (this.M22 == other.M22)) && ((this.M33 == other.M33) && (this.M44 == other.M44))) && (((this.M12 == other.M12) && (this.M13 == other.M13)) && ((this.M14 == other.M14) && (this.M21 == other.M21)))) && ((((this.M23 == other.M23) && (this.M24 == other.M24)) && ((this.M31 == other.M31) && (this.M32 == other.M32))) && (((this.M34 == other.M34) && (this.M41 == other.M41)) && (this.M42 == other.M42)))) && (this.M43 == other.M43)); + } + + /// + /// Compares whether current instance is equal to specified without any tolerance. + /// + /// The to compare. + /// true if the instances are equal; false otherwise. + public override bool Equals(object obj) + { + bool flag = false; + if (obj is Matrix) + { + flag = this.Equals((Matrix) obj); + } + return flag; + } + + /// + /// Gets the hash code of this . + /// + /// Hash code of this . + public override int GetHashCode() + { + return (((((((((((((((this.M11.GetHashCode() + this.M12.GetHashCode()) + this.M13.GetHashCode()) + this.M14.GetHashCode()) + this.M21.GetHashCode()) + this.M22.GetHashCode()) + this.M23.GetHashCode()) + this.M24.GetHashCode()) + this.M31.GetHashCode()) + this.M32.GetHashCode()) + this.M33.GetHashCode()) + this.M34.GetHashCode()) + this.M41.GetHashCode()) + this.M42.GetHashCode()) + this.M43.GetHashCode()) + this.M44.GetHashCode()); + } + + /// + /// Creates a new which contains inversion of the specified matrix. + /// + /// Source . + /// The inverted matrix. + public static Matrix Invert(Matrix matrix) + { + Matrix result; + Invert(ref matrix, out result); + return result; + } + + /// + /// Creates a new which contains inversion of the specified matrix. + /// + /// Source . + /// The inverted matrix as output parameter. + public static void Invert(ref Matrix matrix, out Matrix result) + { + double num1 = matrix.M11; + double num2 = matrix.M12; + double num3 = matrix.M13; + double num4 = matrix.M14; + double num5 = matrix.M21; + double num6 = matrix.M22; + double num7 = matrix.M23; + double num8 = matrix.M24; + double num9 = matrix.M31; + double num10 = matrix.M32; + double num11 = matrix.M33; + double num12 = matrix.M34; + double num13 = matrix.M41; + double num14 = matrix.M42; + double num15 = matrix.M43; + double num16 = matrix.M44; + double num17 = (double) ((double) num11 * (double) num16 - (double) num12 * (double) num15); + double num18 = (double) ((double) num10 * (double) num16 - (double) num12 * (double) num14); + double num19 = (double) ((double) num10 * (double) num15 - (double) num11 * (double) num14); + double num20 = (double) ((double) num9 * (double) num16 - (double) num12 * (double) num13); + double num21 = (double) ((double) num9 * (double) num15 - (double) num11 * (double) num13); + double num22 = (double) ((double) num9 * (double) num14 - (double) num10 * (double) num13); + double num23 = (double) ((double) num6 * (double) num17 - (double) num7 * (double) num18 + (double) num8 * (double) num19); + double num24 = (double) -((double) num5 * (double) num17 - (double) num7 * (double) num20 + (double) num8 * (double) num21); + double num25 = (double) ((double) num5 * (double) num18 - (double) num6 * (double) num20 + (double) num8 * (double) num22); + double num26 = (double) -((double) num5 * (double) num19 - (double) num6 * (double) num21 + (double) num7 * (double) num22); + double num27 = (double) (1.0 / ((double) num1 * (double) num23 + (double) num2 * (double) num24 + (double) num3 * (double) num25 + (double) num4 * (double) num26)); + + result.M11 = num23 * num27; + result.M21 = num24 * num27; + result.M31 = num25 * num27; + result.M41 = num26 * num27; + result.M12 = (double) -((double) num2 * (double) num17 - (double) num3 * (double) num18 + (double) num4 * (double) num19) * num27; + result.M22 = (double) ((double) num1 * (double) num17 - (double) num3 * (double) num20 + (double) num4 * (double) num21) * num27; + result.M32 = (double) -((double) num1 * (double) num18 - (double) num2 * (double) num20 + (double) num4 * (double) num22) * num27; + result.M42 = (double) ((double) num1 * (double) num19 - (double) num2 * (double) num21 + (double) num3 * (double) num22) * num27; + double num28 = (double) ((double) num7 * (double) num16 - (double) num8 * (double) num15); + double num29 = (double) ((double) num6 * (double) num16 - (double) num8 * (double) num14); + double num30 = (double) ((double) num6 * (double) num15 - (double) num7 * (double) num14); + double num31 = (double) ((double) num5 * (double) num16 - (double) num8 * (double) num13); + double num32 = (double) ((double) num5 * (double) num15 - (double) num7 * (double) num13); + double num33 = (double) ((double) num5 * (double) num14 - (double) num6 * (double) num13); + result.M13 = (double) ((double) num2 * (double) num28 - (double) num3 * (double) num29 + (double) num4 * (double) num30) * num27; + result.M23 = (double) -((double) num1 * (double) num28 - (double) num3 * (double) num31 + (double) num4 * (double) num32) * num27; + result.M33 = (double) ((double) num1 * (double) num29 - (double) num2 * (double) num31 + (double) num4 * (double) num33) * num27; + result.M43 = (double) -((double) num1 * (double) num30 - (double) num2 * (double) num32 + (double) num3 * (double) num33) * num27; + double num34 = (double) ((double) num7 * (double) num12 - (double) num8 * (double) num11); + double num35 = (double) ((double) num6 * (double) num12 - (double) num8 * (double) num10); + double num36 = (double) ((double) num6 * (double) num11 - (double) num7 * (double) num10); + double num37 = (double) ((double) num5 * (double) num12 - (double) num8 * (double) num9); + double num38 = (double) ((double) num5 * (double) num11 - (double) num7 * (double) num9); + double num39 = (double) ((double) num5 * (double) num10 - (double) num6 * (double) num9); + result.M14 = (double) -((double) num2 * (double) num34 - (double) num3 * (double) num35 + (double) num4 * (double) num36) * num27; + result.M24 = (double) ((double) num1 * (double) num34 - (double) num3 * (double) num37 + (double) num4 * (double) num38) * num27; + result.M34 = (double) -((double) num1 * (double) num35 - (double) num2 * (double) num37 + (double) num4 * (double) num39) * num27; + result.M44 = (double) ((double) num1 * (double) num36 - (double) num2 * (double) num38 + (double) num3 * (double) num39) * num27; + + + /* + + + /// + // Use Laplace expansion theorem to calculate the inverse of a 4x4 matrix + // + // 1. Calculate the 2x2 determinants needed the 4x4 determinant based on the 2x2 determinants + // 3. Create the adjugate matrix, which satisfies: A * adj(A) = det(A) * I + // 4. Divide adjugate matrix with the determinant to find the inverse + + double det1, det2, det3, det4, det5, det6, det7, det8, det9, det10, det11, det12; + double detMatrix; + FindDeterminants(ref matrix, out detMatrix, out det1, out det2, out det3, out det4, out det5, out det6, + out det7, out det8, out det9, out det10, out det11, out det12); + + double invDetMatrix = 1f / detMatrix; + + Matrix ret; // Allow for matrix and result to point to the same structure + + ret.M11 = (matrix.M22*det12 - matrix.M23*det11 + matrix.M24*det10) * invDetMatrix; + ret.M12 = (-matrix.M12*det12 + matrix.M13*det11 - matrix.M14*det10) * invDetMatrix; + ret.M13 = (matrix.M42*det6 - matrix.M43*det5 + matrix.M44*det4) * invDetMatrix; + ret.M14 = (-matrix.M32*det6 + matrix.M33*det5 - matrix.M34*det4) * invDetMatrix; + ret.M21 = (-matrix.M21*det12 + matrix.M23*det9 - matrix.M24*det8) * invDetMatrix; + ret.M22 = (matrix.M11*det12 - matrix.M13*det9 + matrix.M14*det8) * invDetMatrix; + ret.M23 = (-matrix.M41*det6 + matrix.M43*det3 - matrix.M44*det2) * invDetMatrix; + ret.M24 = (matrix.M31*det6 - matrix.M33*det3 + matrix.M34*det2) * invDetMatrix; + ret.M31 = (matrix.M21*det11 - matrix.M22*det9 + matrix.M24*det7) * invDetMatrix; + ret.M32 = (-matrix.M11*det11 + matrix.M12*det9 - matrix.M14*det7) * invDetMatrix; + ret.M33 = (matrix.M41*det5 - matrix.M42*det3 + matrix.M44*det1) * invDetMatrix; + ret.M34 = (-matrix.M31*det5 + matrix.M32*det3 - matrix.M34*det1) * invDetMatrix; + ret.M41 = (-matrix.M21*det10 + matrix.M22*det8 - matrix.M23*det7) * invDetMatrix; + ret.M42 = (matrix.M11*det10 - matrix.M12*det8 + matrix.M13*det7) * invDetMatrix; + ret.M43 = (-matrix.M41*det4 + matrix.M42*det2 - matrix.M43*det1) * invDetMatrix; + ret.M44 = (matrix.M31*det4 - matrix.M32*det2 + matrix.M33*det1) * invDetMatrix; + + result = ret; + */ + } + + /// + /// Creates a new that contains linear interpolation of the values in specified matrixes. + /// + /// The first . + /// The second . + /// Weighting value(between 0.0 and 1.0). + /// >The result of linear interpolation of the specified matrixes. + public static Matrix Lerp(Matrix matrix1, Matrix matrix2, double amount) + { + matrix1.M11 = matrix1.M11 + ((matrix2.M11 - matrix1.M11) * amount); + matrix1.M12 = matrix1.M12 + ((matrix2.M12 - matrix1.M12) * amount); + matrix1.M13 = matrix1.M13 + ((matrix2.M13 - matrix1.M13) * amount); + matrix1.M14 = matrix1.M14 + ((matrix2.M14 - matrix1.M14) * amount); + matrix1.M21 = matrix1.M21 + ((matrix2.M21 - matrix1.M21) * amount); + matrix1.M22 = matrix1.M22 + ((matrix2.M22 - matrix1.M22) * amount); + matrix1.M23 = matrix1.M23 + ((matrix2.M23 - matrix1.M23) * amount); + matrix1.M24 = matrix1.M24 + ((matrix2.M24 - matrix1.M24) * amount); + matrix1.M31 = matrix1.M31 + ((matrix2.M31 - matrix1.M31) * amount); + matrix1.M32 = matrix1.M32 + ((matrix2.M32 - matrix1.M32) * amount); + matrix1.M33 = matrix1.M33 + ((matrix2.M33 - matrix1.M33) * amount); + matrix1.M34 = matrix1.M34 + ((matrix2.M34 - matrix1.M34) * amount); + matrix1.M41 = matrix1.M41 + ((matrix2.M41 - matrix1.M41) * amount); + matrix1.M42 = matrix1.M42 + ((matrix2.M42 - matrix1.M42) * amount); + matrix1.M43 = matrix1.M43 + ((matrix2.M43 - matrix1.M43) * amount); + matrix1.M44 = matrix1.M44 + ((matrix2.M44 - matrix1.M44) * amount); + return matrix1; + } + + /// + /// Creates a new that contains linear interpolation of the values in specified matrixes. + /// + /// The first . + /// The second . + /// Weighting value(between 0.0 and 1.0). + /// The result of linear interpolation of the specified matrixes as an output parameter. + public static void Lerp(ref Matrix matrix1, ref Matrix matrix2, double amount, out Matrix result) + { + result.M11 = matrix1.M11 + ((matrix2.M11 - matrix1.M11) * amount); + result.M12 = matrix1.M12 + ((matrix2.M12 - matrix1.M12) * amount); + result.M13 = matrix1.M13 + ((matrix2.M13 - matrix1.M13) * amount); + result.M14 = matrix1.M14 + ((matrix2.M14 - matrix1.M14) * amount); + result.M21 = matrix1.M21 + ((matrix2.M21 - matrix1.M21) * amount); + result.M22 = matrix1.M22 + ((matrix2.M22 - matrix1.M22) * amount); + result.M23 = matrix1.M23 + ((matrix2.M23 - matrix1.M23) * amount); + result.M24 = matrix1.M24 + ((matrix2.M24 - matrix1.M24) * amount); + result.M31 = matrix1.M31 + ((matrix2.M31 - matrix1.M31) * amount); + result.M32 = matrix1.M32 + ((matrix2.M32 - matrix1.M32) * amount); + result.M33 = matrix1.M33 + ((matrix2.M33 - matrix1.M33) * amount); + result.M34 = matrix1.M34 + ((matrix2.M34 - matrix1.M34) * amount); + result.M41 = matrix1.M41 + ((matrix2.M41 - matrix1.M41) * amount); + result.M42 = matrix1.M42 + ((matrix2.M42 - matrix1.M42) * amount); + result.M43 = matrix1.M43 + ((matrix2.M43 - matrix1.M43) * amount); + result.M44 = matrix1.M44 + ((matrix2.M44 - matrix1.M44) * amount); + } + + /// + /// Creates a new that contains a multiplication of two matrix. + /// + /// Source . + /// Source . + /// Result of the matrix multiplication. + public static Matrix Multiply(Matrix matrix1, Matrix matrix2) + { + var m11 = (((matrix1.M11 * matrix2.M11) + (matrix1.M12 * matrix2.M21)) + (matrix1.M13 * matrix2.M31)) + (matrix1.M14 * matrix2.M41); + var m12 = (((matrix1.M11 * matrix2.M12) + (matrix1.M12 * matrix2.M22)) + (matrix1.M13 * matrix2.M32)) + (matrix1.M14 * matrix2.M42); + var m13 = (((matrix1.M11 * matrix2.M13) + (matrix1.M12 * matrix2.M23)) + (matrix1.M13 * matrix2.M33)) + (matrix1.M14 * matrix2.M43); + var m14 = (((matrix1.M11 * matrix2.M14) + (matrix1.M12 * matrix2.M24)) + (matrix1.M13 * matrix2.M34)) + (matrix1.M14 * matrix2.M44); + var m21 = (((matrix1.M21 * matrix2.M11) + (matrix1.M22 * matrix2.M21)) + (matrix1.M23 * matrix2.M31)) + (matrix1.M24 * matrix2.M41); + var m22 = (((matrix1.M21 * matrix2.M12) + (matrix1.M22 * matrix2.M22)) + (matrix1.M23 * matrix2.M32)) + (matrix1.M24 * matrix2.M42); + var m23 = (((matrix1.M21 * matrix2.M13) + (matrix1.M22 * matrix2.M23)) + (matrix1.M23 * matrix2.M33)) + (matrix1.M24 * matrix2.M43); + var m24 = (((matrix1.M21 * matrix2.M14) + (matrix1.M22 * matrix2.M24)) + (matrix1.M23 * matrix2.M34)) + (matrix1.M24 * matrix2.M44); + var m31 = (((matrix1.M31 * matrix2.M11) + (matrix1.M32 * matrix2.M21)) + (matrix1.M33 * matrix2.M31)) + (matrix1.M34 * matrix2.M41); + var m32 = (((matrix1.M31 * matrix2.M12) + (matrix1.M32 * matrix2.M22)) + (matrix1.M33 * matrix2.M32)) + (matrix1.M34 * matrix2.M42); + var m33 = (((matrix1.M31 * matrix2.M13) + (matrix1.M32 * matrix2.M23)) + (matrix1.M33 * matrix2.M33)) + (matrix1.M34 * matrix2.M43); + var m34 = (((matrix1.M31 * matrix2.M14) + (matrix1.M32 * matrix2.M24)) + (matrix1.M33 * matrix2.M34)) + (matrix1.M34 * matrix2.M44); + var m41 = (((matrix1.M41 * matrix2.M11) + (matrix1.M42 * matrix2.M21)) + (matrix1.M43 * matrix2.M31)) + (matrix1.M44 * matrix2.M41); + var m42 = (((matrix1.M41 * matrix2.M12) + (matrix1.M42 * matrix2.M22)) + (matrix1.M43 * matrix2.M32)) + (matrix1.M44 * matrix2.M42); + var m43 = (((matrix1.M41 * matrix2.M13) + (matrix1.M42 * matrix2.M23)) + (matrix1.M43 * matrix2.M33)) + (matrix1.M44 * matrix2.M43); + var m44 = (((matrix1.M41 * matrix2.M14) + (matrix1.M42 * matrix2.M24)) + (matrix1.M43 * matrix2.M34)) + (matrix1.M44 * matrix2.M44); + matrix1.M11 = m11; + matrix1.M12 = m12; + matrix1.M13 = m13; + matrix1.M14 = m14; + matrix1.M21 = m21; + matrix1.M22 = m22; + matrix1.M23 = m23; + matrix1.M24 = m24; + matrix1.M31 = m31; + matrix1.M32 = m32; + matrix1.M33 = m33; + matrix1.M34 = m34; + matrix1.M41 = m41; + matrix1.M42 = m42; + matrix1.M43 = m43; + matrix1.M44 = m44; + return matrix1; + } + + /// + /// Creates a new that contains a multiplication of two matrix. + /// + /// Source . + /// Source . + /// Result of the matrix multiplication as an output parameter. + public static void Multiply(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) + { + var m11 = (((matrix1.M11 * matrix2.M11) + (matrix1.M12 * matrix2.M21)) + (matrix1.M13 * matrix2.M31)) + (matrix1.M14 * matrix2.M41); + var m12 = (((matrix1.M11 * matrix2.M12) + (matrix1.M12 * matrix2.M22)) + (matrix1.M13 * matrix2.M32)) + (matrix1.M14 * matrix2.M42); + var m13 = (((matrix1.M11 * matrix2.M13) + (matrix1.M12 * matrix2.M23)) + (matrix1.M13 * matrix2.M33)) + (matrix1.M14 * matrix2.M43); + var m14 = (((matrix1.M11 * matrix2.M14) + (matrix1.M12 * matrix2.M24)) + (matrix1.M13 * matrix2.M34)) + (matrix1.M14 * matrix2.M44); + var m21 = (((matrix1.M21 * matrix2.M11) + (matrix1.M22 * matrix2.M21)) + (matrix1.M23 * matrix2.M31)) + (matrix1.M24 * matrix2.M41); + var m22 = (((matrix1.M21 * matrix2.M12) + (matrix1.M22 * matrix2.M22)) + (matrix1.M23 * matrix2.M32)) + (matrix1.M24 * matrix2.M42); + var m23 = (((matrix1.M21 * matrix2.M13) + (matrix1.M22 * matrix2.M23)) + (matrix1.M23 * matrix2.M33)) + (matrix1.M24 * matrix2.M43); + var m24 = (((matrix1.M21 * matrix2.M14) + (matrix1.M22 * matrix2.M24)) + (matrix1.M23 * matrix2.M34)) + (matrix1.M24 * matrix2.M44); + var m31 = (((matrix1.M31 * matrix2.M11) + (matrix1.M32 * matrix2.M21)) + (matrix1.M33 * matrix2.M31)) + (matrix1.M34 * matrix2.M41); + var m32 = (((matrix1.M31 * matrix2.M12) + (matrix1.M32 * matrix2.M22)) + (matrix1.M33 * matrix2.M32)) + (matrix1.M34 * matrix2.M42); + var m33 = (((matrix1.M31 * matrix2.M13) + (matrix1.M32 * matrix2.M23)) + (matrix1.M33 * matrix2.M33)) + (matrix1.M34 * matrix2.M43); + var m34 = (((matrix1.M31 * matrix2.M14) + (matrix1.M32 * matrix2.M24)) + (matrix1.M33 * matrix2.M34)) + (matrix1.M34 * matrix2.M44); + var m41 = (((matrix1.M41 * matrix2.M11) + (matrix1.M42 * matrix2.M21)) + (matrix1.M43 * matrix2.M31)) + (matrix1.M44 * matrix2.M41); + var m42 = (((matrix1.M41 * matrix2.M12) + (matrix1.M42 * matrix2.M22)) + (matrix1.M43 * matrix2.M32)) + (matrix1.M44 * matrix2.M42); + var m43 = (((matrix1.M41 * matrix2.M13) + (matrix1.M42 * matrix2.M23)) + (matrix1.M43 * matrix2.M33)) + (matrix1.M44 * matrix2.M43); + var m44 = (((matrix1.M41 * matrix2.M14) + (matrix1.M42 * matrix2.M24)) + (matrix1.M43 * matrix2.M34)) + (matrix1.M44 * matrix2.M44); + result.M11 = m11; + result.M12 = m12; + result.M13 = m13; + result.M14 = m14; + result.M21 = m21; + result.M22 = m22; + result.M23 = m23; + result.M24 = m24; + result.M31 = m31; + result.M32 = m32; + result.M33 = m33; + result.M34 = m34; + result.M41 = m41; + result.M42 = m42; + result.M43 = m43; + result.M44 = m44; + } + + /// + /// Creates a new that contains a multiplication of and a scalar. + /// + /// Source . + /// Scalar value. + /// Result of the matrix multiplication with a scalar. + public static Matrix Multiply(Matrix matrix1, double scaleFactor) + { + matrix1.M11 *= scaleFactor; + matrix1.M12 *= scaleFactor; + matrix1.M13 *= scaleFactor; + matrix1.M14 *= scaleFactor; + matrix1.M21 *= scaleFactor; + matrix1.M22 *= scaleFactor; + matrix1.M23 *= scaleFactor; + matrix1.M24 *= scaleFactor; + matrix1.M31 *= scaleFactor; + matrix1.M32 *= scaleFactor; + matrix1.M33 *= scaleFactor; + matrix1.M34 *= scaleFactor; + matrix1.M41 *= scaleFactor; + matrix1.M42 *= scaleFactor; + matrix1.M43 *= scaleFactor; + matrix1.M44 *= scaleFactor; + return matrix1; + } + + /// + /// Creates a new that contains a multiplication of and a scalar. + /// + /// Source . + /// Scalar value. + /// Result of the matrix multiplication with a scalar as an output parameter. + public static void Multiply(ref Matrix matrix1, double scaleFactor, out Matrix result) + { + result.M11 = matrix1.M11 * scaleFactor; + result.M12 = matrix1.M12 * scaleFactor; + result.M13 = matrix1.M13 * scaleFactor; + result.M14 = matrix1.M14 * scaleFactor; + result.M21 = matrix1.M21 * scaleFactor; + result.M22 = matrix1.M22 * scaleFactor; + result.M23 = matrix1.M23 * scaleFactor; + result.M24 = matrix1.M24 * scaleFactor; + result.M31 = matrix1.M31 * scaleFactor; + result.M32 = matrix1.M32 * scaleFactor; + result.M33 = matrix1.M33 * scaleFactor; + result.M34 = matrix1.M34 * scaleFactor; + result.M41 = matrix1.M41 * scaleFactor; + result.M42 = matrix1.M42 * scaleFactor; + result.M43 = matrix1.M43 * scaleFactor; + result.M44 = matrix1.M44 * scaleFactor; + + } + + /// + /// Copy the values of specified to the double array. + /// + /// The source . + /// The array which matrix values will be stored. + /// + /// Required for OpenGL 2.0 projection matrix stuff. + /// + public static double[] TodoubleArray(Matrix matrix) + { + double[] matarray = { + matrix.M11, matrix.M12, matrix.M13, matrix.M14, + matrix.M21, matrix.M22, matrix.M23, matrix.M24, + matrix.M31, matrix.M32, matrix.M33, matrix.M34, + matrix.M41, matrix.M42, matrix.M43, matrix.M44 + }; + return matarray; + } + + /// + /// Returns a matrix with the all values negated. + /// + /// Source . + /// Result of the matrix negation. + public static Matrix Negate(Matrix matrix) + { + matrix.M11 = -matrix.M11; + matrix.M12 = -matrix.M12; + matrix.M13 = -matrix.M13; + matrix.M14 = -matrix.M14; + matrix.M21 = -matrix.M21; + matrix.M22 = -matrix.M22; + matrix.M23 = -matrix.M23; + matrix.M24 = -matrix.M24; + matrix.M31 = -matrix.M31; + matrix.M32 = -matrix.M32; + matrix.M33 = -matrix.M33; + matrix.M34 = -matrix.M34; + matrix.M41 = -matrix.M41; + matrix.M42 = -matrix.M42; + matrix.M43 = -matrix.M43; + matrix.M44 = -matrix.M44; + return matrix; + } + + /// + /// Returns a matrix with the all values negated. + /// + /// Source . + /// Result of the matrix negation as an output parameter. + public static void Negate(ref Matrix matrix, out Matrix result) + { + result.M11 = -matrix.M11; + result.M12 = -matrix.M12; + result.M13 = -matrix.M13; + result.M14 = -matrix.M14; + result.M21 = -matrix.M21; + result.M22 = -matrix.M22; + result.M23 = -matrix.M23; + result.M24 = -matrix.M24; + result.M31 = -matrix.M31; + result.M32 = -matrix.M32; + result.M33 = -matrix.M33; + result.M34 = -matrix.M34; + result.M41 = -matrix.M41; + result.M42 = -matrix.M42; + result.M43 = -matrix.M43; + result.M44 = -matrix.M44; + } + + /// + /// Adds two matrixes. + /// + /// Source on the left of the add sign. + /// Source on the right of the add sign. + /// Sum of the matrixes. + public static Matrix operator +(Matrix matrix1, Matrix matrix2) + { + matrix1.M11 = matrix1.M11 + matrix2.M11; + matrix1.M12 = matrix1.M12 + matrix2.M12; + matrix1.M13 = matrix1.M13 + matrix2.M13; + matrix1.M14 = matrix1.M14 + matrix2.M14; + matrix1.M21 = matrix1.M21 + matrix2.M21; + matrix1.M22 = matrix1.M22 + matrix2.M22; + matrix1.M23 = matrix1.M23 + matrix2.M23; + matrix1.M24 = matrix1.M24 + matrix2.M24; + matrix1.M31 = matrix1.M31 + matrix2.M31; + matrix1.M32 = matrix1.M32 + matrix2.M32; + matrix1.M33 = matrix1.M33 + matrix2.M33; + matrix1.M34 = matrix1.M34 + matrix2.M34; + matrix1.M41 = matrix1.M41 + matrix2.M41; + matrix1.M42 = matrix1.M42 + matrix2.M42; + matrix1.M43 = matrix1.M43 + matrix2.M43; + matrix1.M44 = matrix1.M44 + matrix2.M44; + return matrix1; + } + + /// + /// Divides the elements of a by the elements of another . + /// + /// Source on the left of the div sign. + /// Divisor on the right of the div sign. + /// The result of dividing the matrixes. + public static Matrix operator /(Matrix matrix1, Matrix matrix2) + { + matrix1.M11 = matrix1.M11 / matrix2.M11; + matrix1.M12 = matrix1.M12 / matrix2.M12; + matrix1.M13 = matrix1.M13 / matrix2.M13; + matrix1.M14 = matrix1.M14 / matrix2.M14; + matrix1.M21 = matrix1.M21 / matrix2.M21; + matrix1.M22 = matrix1.M22 / matrix2.M22; + matrix1.M23 = matrix1.M23 / matrix2.M23; + matrix1.M24 = matrix1.M24 / matrix2.M24; + matrix1.M31 = matrix1.M31 / matrix2.M31; + matrix1.M32 = matrix1.M32 / matrix2.M32; + matrix1.M33 = matrix1.M33 / matrix2.M33; + matrix1.M34 = matrix1.M34 / matrix2.M34; + matrix1.M41 = matrix1.M41 / matrix2.M41; + matrix1.M42 = matrix1.M42 / matrix2.M42; + matrix1.M43 = matrix1.M43 / matrix2.M43; + matrix1.M44 = matrix1.M44 / matrix2.M44; + return matrix1; + } + + /// + /// Divides the elements of a by a scalar. + /// + /// Source on the left of the div sign. + /// Divisor scalar on the right of the div sign. + /// The result of dividing a matrix by a scalar. + public static Matrix operator /(Matrix matrix, double divider) + { + double num = 1f / divider; + matrix.M11 = matrix.M11 * num; + matrix.M12 = matrix.M12 * num; + matrix.M13 = matrix.M13 * num; + matrix.M14 = matrix.M14 * num; + matrix.M21 = matrix.M21 * num; + matrix.M22 = matrix.M22 * num; + matrix.M23 = matrix.M23 * num; + matrix.M24 = matrix.M24 * num; + matrix.M31 = matrix.M31 * num; + matrix.M32 = matrix.M32 * num; + matrix.M33 = matrix.M33 * num; + matrix.M34 = matrix.M34 * num; + matrix.M41 = matrix.M41 * num; + matrix.M42 = matrix.M42 * num; + matrix.M43 = matrix.M43 * num; + matrix.M44 = matrix.M44 * num; + return matrix; + } + + /// + /// Compares whether two instances are equal without any tolerance. + /// + /// Source on the left of the equal sign. + /// Source on the right of the equal sign. + /// true if the instances are equal; false otherwise. + public static bool operator ==(Matrix matrix1, Matrix matrix2) + { + return ( + matrix1.M11 == matrix2.M11 && + matrix1.M12 == matrix2.M12 && + matrix1.M13 == matrix2.M13 && + matrix1.M14 == matrix2.M14 && + matrix1.M21 == matrix2.M21 && + matrix1.M22 == matrix2.M22 && + matrix1.M23 == matrix2.M23 && + matrix1.M24 == matrix2.M24 && + matrix1.M31 == matrix2.M31 && + matrix1.M32 == matrix2.M32 && + matrix1.M33 == matrix2.M33 && + matrix1.M34 == matrix2.M34 && + matrix1.M41 == matrix2.M41 && + matrix1.M42 == matrix2.M42 && + matrix1.M43 == matrix2.M43 && + matrix1.M44 == matrix2.M44 + ); + } + + /// + /// Compares whether two instances are not equal without any tolerance. + /// + /// Source on the left of the not equal sign. + /// Source on the right of the not equal sign. + /// true if the instances are not equal; false otherwise. + public static bool operator !=(Matrix matrix1, Matrix matrix2) + { + return ( + matrix1.M11 != matrix2.M11 || + matrix1.M12 != matrix2.M12 || + matrix1.M13 != matrix2.M13 || + matrix1.M14 != matrix2.M14 || + matrix1.M21 != matrix2.M21 || + matrix1.M22 != matrix2.M22 || + matrix1.M23 != matrix2.M23 || + matrix1.M24 != matrix2.M24 || + matrix1.M31 != matrix2.M31 || + matrix1.M32 != matrix2.M32 || + matrix1.M33 != matrix2.M33 || + matrix1.M34 != matrix2.M34 || + matrix1.M41 != matrix2.M41 || + matrix1.M42 != matrix2.M42 || + matrix1.M43 != matrix2.M43 || + matrix1.M44 != matrix2.M44 + ); + } + + /// + /// Multiplies two matrixes. + /// + /// Source on the left of the mul sign. + /// Source on the right of the mul sign. + /// Result of the matrix multiplication. + /// + /// Using matrix multiplication algorithm - see http://en.wikipedia.org/wiki/Matrix_multiplication. + /// + public static Matrix operator *(Matrix matrix1, Matrix matrix2) + { + var m11 = (((matrix1.M11 * matrix2.M11) + (matrix1.M12 * matrix2.M21)) + (matrix1.M13 * matrix2.M31)) + (matrix1.M14 * matrix2.M41); + var m12 = (((matrix1.M11 * matrix2.M12) + (matrix1.M12 * matrix2.M22)) + (matrix1.M13 * matrix2.M32)) + (matrix1.M14 * matrix2.M42); + var m13 = (((matrix1.M11 * matrix2.M13) + (matrix1.M12 * matrix2.M23)) + (matrix1.M13 * matrix2.M33)) + (matrix1.M14 * matrix2.M43); + var m14 = (((matrix1.M11 * matrix2.M14) + (matrix1.M12 * matrix2.M24)) + (matrix1.M13 * matrix2.M34)) + (matrix1.M14 * matrix2.M44); + var m21 = (((matrix1.M21 * matrix2.M11) + (matrix1.M22 * matrix2.M21)) + (matrix1.M23 * matrix2.M31)) + (matrix1.M24 * matrix2.M41); + var m22 = (((matrix1.M21 * matrix2.M12) + (matrix1.M22 * matrix2.M22)) + (matrix1.M23 * matrix2.M32)) + (matrix1.M24 * matrix2.M42); + var m23 = (((matrix1.M21 * matrix2.M13) + (matrix1.M22 * matrix2.M23)) + (matrix1.M23 * matrix2.M33)) + (matrix1.M24 * matrix2.M43); + var m24 = (((matrix1.M21 * matrix2.M14) + (matrix1.M22 * matrix2.M24)) + (matrix1.M23 * matrix2.M34)) + (matrix1.M24 * matrix2.M44); + var m31 = (((matrix1.M31 * matrix2.M11) + (matrix1.M32 * matrix2.M21)) + (matrix1.M33 * matrix2.M31)) + (matrix1.M34 * matrix2.M41); + var m32 = (((matrix1.M31 * matrix2.M12) + (matrix1.M32 * matrix2.M22)) + (matrix1.M33 * matrix2.M32)) + (matrix1.M34 * matrix2.M42); + var m33 = (((matrix1.M31 * matrix2.M13) + (matrix1.M32 * matrix2.M23)) + (matrix1.M33 * matrix2.M33)) + (matrix1.M34 * matrix2.M43); + var m34 = (((matrix1.M31 * matrix2.M14) + (matrix1.M32 * matrix2.M24)) + (matrix1.M33 * matrix2.M34)) + (matrix1.M34 * matrix2.M44); + var m41 = (((matrix1.M41 * matrix2.M11) + (matrix1.M42 * matrix2.M21)) + (matrix1.M43 * matrix2.M31)) + (matrix1.M44 * matrix2.M41); + var m42 = (((matrix1.M41 * matrix2.M12) + (matrix1.M42 * matrix2.M22)) + (matrix1.M43 * matrix2.M32)) + (matrix1.M44 * matrix2.M42); + var m43 = (((matrix1.M41 * matrix2.M13) + (matrix1.M42 * matrix2.M23)) + (matrix1.M43 * matrix2.M33)) + (matrix1.M44 * matrix2.M43); + var m44 = (((matrix1.M41 * matrix2.M14) + (matrix1.M42 * matrix2.M24)) + (matrix1.M43 * matrix2.M34)) + (matrix1.M44 * matrix2.M44); + matrix1.M11 = m11; + matrix1.M12 = m12; + matrix1.M13 = m13; + matrix1.M14 = m14; + matrix1.M21 = m21; + matrix1.M22 = m22; + matrix1.M23 = m23; + matrix1.M24 = m24; + matrix1.M31 = m31; + matrix1.M32 = m32; + matrix1.M33 = m33; + matrix1.M34 = m34; + matrix1.M41 = m41; + matrix1.M42 = m42; + matrix1.M43 = m43; + matrix1.M44 = m44; + return matrix1; + } + + /// + /// Multiplies the elements of matrix by a scalar. + /// + /// Source on the left of the mul sign. + /// Scalar value on the right of the mul sign. + /// Result of the matrix multiplication with a scalar. + public static Matrix operator *(Matrix matrix, double scaleFactor) + { + matrix.M11 = matrix.M11 * scaleFactor; + matrix.M12 = matrix.M12 * scaleFactor; + matrix.M13 = matrix.M13 * scaleFactor; + matrix.M14 = matrix.M14 * scaleFactor; + matrix.M21 = matrix.M21 * scaleFactor; + matrix.M22 = matrix.M22 * scaleFactor; + matrix.M23 = matrix.M23 * scaleFactor; + matrix.M24 = matrix.M24 * scaleFactor; + matrix.M31 = matrix.M31 * scaleFactor; + matrix.M32 = matrix.M32 * scaleFactor; + matrix.M33 = matrix.M33 * scaleFactor; + matrix.M34 = matrix.M34 * scaleFactor; + matrix.M41 = matrix.M41 * scaleFactor; + matrix.M42 = matrix.M42 * scaleFactor; + matrix.M43 = matrix.M43 * scaleFactor; + matrix.M44 = matrix.M44 * scaleFactor; + return matrix; + } + + /// + /// Subtracts the values of one from another . + /// + /// Source on the left of the sub sign. + /// Source on the right of the sub sign. + /// Result of the matrix subtraction. + public static Matrix operator -(Matrix matrix1, Matrix matrix2) + { + matrix1.M11 = matrix1.M11 - matrix2.M11; + matrix1.M12 = matrix1.M12 - matrix2.M12; + matrix1.M13 = matrix1.M13 - matrix2.M13; + matrix1.M14 = matrix1.M14 - matrix2.M14; + matrix1.M21 = matrix1.M21 - matrix2.M21; + matrix1.M22 = matrix1.M22 - matrix2.M22; + matrix1.M23 = matrix1.M23 - matrix2.M23; + matrix1.M24 = matrix1.M24 - matrix2.M24; + matrix1.M31 = matrix1.M31 - matrix2.M31; + matrix1.M32 = matrix1.M32 - matrix2.M32; + matrix1.M33 = matrix1.M33 - matrix2.M33; + matrix1.M34 = matrix1.M34 - matrix2.M34; + matrix1.M41 = matrix1.M41 - matrix2.M41; + matrix1.M42 = matrix1.M42 - matrix2.M42; + matrix1.M43 = matrix1.M43 - matrix2.M43; + matrix1.M44 = matrix1.M44 - matrix2.M44; + return matrix1; + } + + /// + /// Inverts values in the specified . + /// + /// Source on the right of the sub sign. + /// Result of the inversion. + public static Matrix operator -(Matrix matrix) + { + matrix.M11 = -matrix.M11; + matrix.M12 = -matrix.M12; + matrix.M13 = -matrix.M13; + matrix.M14 = -matrix.M14; + matrix.M21 = -matrix.M21; + matrix.M22 = -matrix.M22; + matrix.M23 = -matrix.M23; + matrix.M24 = -matrix.M24; + matrix.M31 = -matrix.M31; + matrix.M32 = -matrix.M32; + matrix.M33 = -matrix.M33; + matrix.M34 = -matrix.M34; + matrix.M41 = -matrix.M41; + matrix.M42 = -matrix.M42; + matrix.M43 = -matrix.M43; + matrix.M44 = -matrix.M44; + return matrix; + } + + /// + /// Creates a new that contains subtraction of one matrix from another. + /// + /// The first . + /// The second . + /// The result of the matrix subtraction. + public static Matrix Subtract(Matrix matrix1, Matrix matrix2) + { + matrix1.M11 = matrix1.M11 - matrix2.M11; + matrix1.M12 = matrix1.M12 - matrix2.M12; + matrix1.M13 = matrix1.M13 - matrix2.M13; + matrix1.M14 = matrix1.M14 - matrix2.M14; + matrix1.M21 = matrix1.M21 - matrix2.M21; + matrix1.M22 = matrix1.M22 - matrix2.M22; + matrix1.M23 = matrix1.M23 - matrix2.M23; + matrix1.M24 = matrix1.M24 - matrix2.M24; + matrix1.M31 = matrix1.M31 - matrix2.M31; + matrix1.M32 = matrix1.M32 - matrix2.M32; + matrix1.M33 = matrix1.M33 - matrix2.M33; + matrix1.M34 = matrix1.M34 - matrix2.M34; + matrix1.M41 = matrix1.M41 - matrix2.M41; + matrix1.M42 = matrix1.M42 - matrix2.M42; + matrix1.M43 = matrix1.M43 - matrix2.M43; + matrix1.M44 = matrix1.M44 - matrix2.M44; + return matrix1; + } + + /// + /// Creates a new that contains subtraction of one matrix from another. + /// + /// The first . + /// The second . + /// The result of the matrix subtraction as an output parameter. + public static void Subtract(ref Matrix matrix1, ref Matrix matrix2, out Matrix result) + { + result.M11 = matrix1.M11 - matrix2.M11; + result.M12 = matrix1.M12 - matrix2.M12; + result.M13 = matrix1.M13 - matrix2.M13; + result.M14 = matrix1.M14 - matrix2.M14; + result.M21 = matrix1.M21 - matrix2.M21; + result.M22 = matrix1.M22 - matrix2.M22; + result.M23 = matrix1.M23 - matrix2.M23; + result.M24 = matrix1.M24 - matrix2.M24; + result.M31 = matrix1.M31 - matrix2.M31; + result.M32 = matrix1.M32 - matrix2.M32; + result.M33 = matrix1.M33 - matrix2.M33; + result.M34 = matrix1.M34 - matrix2.M34; + result.M41 = matrix1.M41 - matrix2.M41; + result.M42 = matrix1.M42 - matrix2.M42; + result.M43 = matrix1.M43 - matrix2.M43; + result.M44 = matrix1.M44 - matrix2.M44; + } + + /// + /// Returns a representation of this in the format: + /// {M11:[] M12:[] M13:[] M14:[]} + /// {M21:[] M12:[] M13:[] M14:[]} + /// {M31:[] M32:[] M33:[] M34:[]} + /// {M41:[] M42:[] M43:[] M44:[]} + /// + /// A representation of this . + public override string ToString() + { + return "{M11:" + M11 + " M12:" + M12 + " M13:" + M13 + " M14:" + M14 + "}" + + " {M21:" + M21 + " M22:" + M22 + " M23:" + M23 + " M24:" + M24 + "}" + + " {M31:" + M31 + " M32:" + M32 + " M33:" + M33 + " M34:" + M34 + "}" + + " {M41:" + M41 + " M42:" + M42 + " M43:" + M43 + " M44:" + M44 + "}"; + } + + /// + /// Swap the matrix rows and columns. + /// + /// The matrix for transposing operation. + /// The new which contains the transposing result. + public static Matrix Transpose(Matrix matrix) + { + Matrix ret; + Transpose(ref matrix, out ret); + return ret; + } + + /// + /// Swap the matrix rows and columns. + /// + /// The matrix for transposing operation. + /// The new which contains the transposing result as an output parameter. + public static void Transpose(ref Matrix matrix, out Matrix result) + { + Matrix ret; + + ret.M11 = matrix.M11; + ret.M12 = matrix.M21; + ret.M13 = matrix.M31; + ret.M14 = matrix.M41; + + ret.M21 = matrix.M12; + ret.M22 = matrix.M22; + ret.M23 = matrix.M32; + ret.M24 = matrix.M42; + + ret.M31 = matrix.M13; + ret.M32 = matrix.M23; + ret.M33 = matrix.M33; + ret.M34 = matrix.M43; + + ret.M41 = matrix.M14; + ret.M42 = matrix.M24; + ret.M43 = matrix.M34; + ret.M44 = matrix.M44; + + result = ret; + } + #endregion + + #region Private Static Methods + + /// + /// Helper method for using the Laplace expansion theorem using two rows expansions to calculate major and + /// minor determinants of a 4x4 matrix. This method is used for inverting a matrix. + /// + private static void FindDeterminants(ref Matrix matrix, out double major, + out double minor1, out double minor2, out double minor3, out double minor4, out double minor5, out double minor6, + out double minor7, out double minor8, out double minor9, out double minor10, out double minor11, out double minor12) + { + double det1 = (double)matrix.M11 * (double)matrix.M22 - (double)matrix.M12 * (double)matrix.M21; + double det2 = (double)matrix.M11 * (double)matrix.M23 - (double)matrix.M13 * (double)matrix.M21; + double det3 = (double)matrix.M11 * (double)matrix.M24 - (double)matrix.M14 * (double)matrix.M21; + double det4 = (double)matrix.M12 * (double)matrix.M23 - (double)matrix.M13 * (double)matrix.M22; + double det5 = (double)matrix.M12 * (double)matrix.M24 - (double)matrix.M14 * (double)matrix.M22; + double det6 = (double)matrix.M13 * (double)matrix.M24 - (double)matrix.M14 * (double)matrix.M23; + double det7 = (double)matrix.M31 * (double)matrix.M42 - (double)matrix.M32 * (double)matrix.M41; + double det8 = (double)matrix.M31 * (double)matrix.M43 - (double)matrix.M33 * (double)matrix.M41; + double det9 = (double)matrix.M31 * (double)matrix.M44 - (double)matrix.M34 * (double)matrix.M41; + double det10 = (double)matrix.M32 * (double)matrix.M43 - (double)matrix.M33 * (double)matrix.M42; + double det11 = (double)matrix.M32 * (double)matrix.M44 - (double)matrix.M34 * (double)matrix.M42; + double det12 = (double)matrix.M33 * (double)matrix.M44 - (double)matrix.M34 * (double)matrix.M43; + + major = (double)(det1*det12 - det2*det11 + det3*det10 + det4*det9 - det5*det8 + det6*det7); + minor1 = (double)det1; + minor2 = (double)det2; + minor3 = (double)det3; + minor4 = (double)det4; + minor5 = (double)det5; + minor6 = (double)det6; + minor7 = (double)det7; + minor8 = (double)det8; + minor9 = (double)det9; + minor10 = (double)det10; + minor11 = (double)det11; + minor12 = (double)det12; + } + + #endregion + } +} \ No newline at end of file diff --git a/TrueCraft.API/TrueCraft.API.csproj b/TrueCraft.API/TrueCraft.API.csproj index 3650e7c..1e1f137 100644 --- a/TrueCraft.API/TrueCraft.API.csproj +++ b/TrueCraft.API/TrueCraft.API.csproj @@ -126,6 +126,7 @@ + diff --git a/TrueCraft.API/Vector3.cs b/TrueCraft.API/Vector3.cs index 8b878a3..94fe7c8 100644 --- a/TrueCraft.API/Vector3.cs +++ b/TrueCraft.API/Vector3.cs @@ -111,6 +111,14 @@ namespace TrueCraft.API Square(other.Z - Z)); } + public Vector3 Transform(Matrix matrix) + { + var x = (X * matrix.M11) + (Y * matrix.M21) + (Z * matrix.M31) + matrix.M41; + var y = (X * matrix.M12) + (Y * matrix.M22) + (Z * matrix.M32) + matrix.M42; + var z = (X * matrix.M13) + (Y * matrix.M23) + (Z * matrix.M33) + matrix.M43; + return new Vector3(x, y, z); + } + /// /// Calculates the square of a num. /// diff --git a/TrueCraft.Client/Modules/HighlightModule.cs b/TrueCraft.Client/Modules/HighlightModule.cs index 3062a4b..879809b 100644 --- a/TrueCraft.Client/Modules/HighlightModule.cs +++ b/TrueCraft.Client/Modules/HighlightModule.cs @@ -3,6 +3,7 @@ using Microsoft.Xna.Framework.Graphics; using TrueCraft.API; using TrueCraft.Client.Rendering; using Microsoft.Xna.Framework; +using Matrix = Microsoft.Xna.Framework.Matrix; using XVector3 = Microsoft.Xna.Framework.Vector3; using TVector3 = TrueCraft.API.Vector3; using TRay = TrueCraft.API.Ray; diff --git a/TrueCraft.Client/Modules/PlayerControlModule.cs b/TrueCraft.Client/Modules/PlayerControlModule.cs index 366d2e6..e9f38f9 100644 --- a/TrueCraft.Client/Modules/PlayerControlModule.cs +++ b/TrueCraft.Client/Modules/PlayerControlModule.cs @@ -3,6 +3,7 @@ using System.Diagnostics; using Microsoft.Xna.Framework; using Microsoft.Xna.Framework.Input; using TrueCraft.Client.Input; +using Matrix = Microsoft.Xna.Framework.Matrix; using TVector3 = TrueCraft.API.Vector3; using XVector3 = Microsoft.Xna.Framework.Vector3; using TrueCraft.API; diff --git a/TrueCraft.Core/AI/WanderState.cs b/TrueCraft.Core/AI/WanderState.cs index f792a8b..5ba7630 100644 --- a/TrueCraft.Core/AI/WanderState.cs +++ b/TrueCraft.Core/AI/WanderState.cs @@ -24,7 +24,7 @@ namespace TrueCraft.Core.AI public WanderState() { - IdleChance = 20; + IdleChance = 10; Distance = 25; PathFinder = new AStarPathFinder(); } diff --git a/TrueCraft.Core/Entities/MobEntity.cs b/TrueCraft.Core/Entities/MobEntity.cs index 367ef62..63bc122 100644 --- a/TrueCraft.Core/Entities/MobEntity.cs +++ b/TrueCraft.Core/Entities/MobEntity.cs @@ -116,11 +116,11 @@ namespace TrueCraft.Core.Entities // Advance along path var target = (Vector3)CurrentPath.Waypoints[CurrentPath.Index]; target.Y = Position.Y; // TODO: Find better way of doing this - var diff = target - Position; - diff *= modifier; if (faceRoute) Face(target); - Position += diff; + var lookAt = Vector3.Forwards.Transform(Matrix.CreateRotationY(MathHelper.ToRadians(-(Yaw - 180) + 180))); + lookAt *= modifier; + Velocity = new Vector3(lookAt.X, Velocity.Y, lookAt.Z); if (Position.DistanceTo(target) < 0.1) { CurrentPath.Index++; diff --git a/TrueCraft.Core/MathHelper.cs b/TrueCraft.Core/MathHelper.cs index 6322c9c..4d35b5e 100644 --- a/TrueCraft.Core/MathHelper.cs +++ b/TrueCraft.Core/MathHelper.cs @@ -33,6 +33,11 @@ namespace TrueCraft.Core return (int)(value * 32); } + public static double ToRadians(double degrees) + { + return degrees * 0.017453292519943295769236907684886; + } + public static Coordinates3D BlockFaceToCoordinates(BlockFace face) { switch (face)