From e251d30aa9afe655ad71b0ebb4aed1f9654e9b7b Mon Sep 17 00:00:00 2001 From: cosmonaut Date: Wed, 20 Jan 2021 14:44:59 -0800 Subject: [PATCH] Math --- licenses/LICENSE | 23 + licenses/fna.LICENSE | 63 + licenses/monoxna.LICENSE | 22 + src/Audio/SoundInstance.cs | 2 +- src/Math/BoundingBox.cs | 614 +++++++ src/Math/BoundingFrustum.cs | 729 ++++++++ src/Math/BoundingSphere.cs | 683 ++++++++ src/Math/ContainmentType.cs | 37 + src/Math/MathHelper.cs | 415 +++++ src/Math/Matrix.cs | 2661 +++++++++++++++++++++++++++++ src/Math/Plane.cs | 329 ++++ src/Math/PlaneIntersectionType.cs | 37 + src/Math/Point.cs | 214 +++ src/Math/Quaternion.cs | 989 +++++++++++ src/Math/Ray.cs | 300 ++++ src/Math/Rectangle.cs | 447 +++++ src/Math/Vector2.cs | 1166 +++++++++++++ src/Math/Vector3.cs | 1509 ++++++++++++++++ src/Math/Vector4.cs | 1479 ++++++++++++++++ 19 files changed, 11718 insertions(+), 1 deletion(-) create mode 100644 licenses/LICENSE create mode 100644 licenses/fna.LICENSE create mode 100644 licenses/monoxna.LICENSE create mode 100644 src/Math/BoundingBox.cs create mode 100644 src/Math/BoundingFrustum.cs create mode 100644 src/Math/BoundingSphere.cs create mode 100644 src/Math/ContainmentType.cs create mode 100644 src/Math/MathHelper.cs create mode 100644 src/Math/Matrix.cs create mode 100644 src/Math/Plane.cs create mode 100644 src/Math/PlaneIntersectionType.cs create mode 100644 src/Math/Point.cs create mode 100644 src/Math/Quaternion.cs create mode 100644 src/Math/Ray.cs create mode 100644 src/Math/Rectangle.cs create mode 100644 src/Math/Vector2.cs create mode 100644 src/Math/Vector3.cs create mode 100644 src/Math/Vector4.cs diff --git a/licenses/LICENSE b/licenses/LICENSE new file mode 100644 index 0000000..b1c1d21 --- /dev/null +++ b/licenses/LICENSE @@ -0,0 +1,23 @@ +MoonWorks - Game Development Framework + +Copyright (c) 2021 Evan Hemsley + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from +the use of this software. + +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not +claim that you wrote the original software. If you use this software in a +product, an acknowledgment in the product documentation would be +appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and must not be +misrepresented as being the original software. + +3. This notice may not be removed or altered from any source distribution. + +Evan "cosmonaut" Hemsley diff --git a/licenses/fna.LICENSE b/licenses/fna.LICENSE new file mode 100644 index 0000000..7918439 --- /dev/null +++ b/licenses/fna.LICENSE @@ -0,0 +1,63 @@ +Microsoft Public License (Ms-PL) +FNA - Copyright 2009-2021 Ethan Lee and the MonoGame Team + +All rights reserved. + +This license governs use of the accompanying software. If you use the software, +you accept this license. If you do not accept the license, do not use the +software. + +1. Definitions + +The terms "reproduce," "reproduction," "derivative works," and "distribution" +have the same meaning here as under U.S. copyright law. + +A "contribution" is the original software, or any additions or changes to the +software. + +A "contributor" is any person that distributes its contribution under this +license. + +"Licensed patents" are a contributor's patent claims that read directly on its +contribution. + +2. Grant of Rights + +(A) Copyright Grant- Subject to the terms of this license, including the +license conditions and limitations in section 3, each contributor grants you a +non-exclusive, worldwide, royalty-free copyright license to reproduce its +contribution, prepare derivative works of its contribution, and distribute its +contribution or any derivative works that you create. + +(B) Patent Grant- Subject to the terms of this license, including the license +conditions and limitations in section 3, each contributor grants you a +non-exclusive, worldwide, royalty-free license under its licensed patents to +make, have made, use, sell, offer for sale, import, and/or otherwise dispose of +its contribution in the software or derivative works of the contribution in the +software. + +3. Conditions and Limitations + +(A) No Trademark License- This license does not grant you rights to use any +contributors' name, logo, or trademarks. + +(B) If you bring a patent claim against any contributor over patents that you +claim are infringed by the software, your patent license from such contributor +to the software ends automatically. + +(C) If you distribute any portion of the software, you must retain all +copyright, patent, trademark, and attribution notices that are present in the +software. + +(D) If you distribute any portion of the software in source code form, you may +do so only under this license by including a complete copy of this license with +your distribution. If you distribute any portion of the software in compiled or +object code form, you may only do so under a license that complies with this +license. + +(E) The software is licensed "as-is." You bear the risk of using it. The +contributors give no express warranties, guarantees or conditions. You may have +additional consumer rights under your local laws which this license cannot +change. To the extent permitted under your local laws, the contributors exclude +the implied warranties of merchantability, fitness for a particular purpose and +non-infringement. diff --git a/licenses/monoxna.LICENSE b/licenses/monoxna.LICENSE new file mode 100644 index 0000000..185aa38 --- /dev/null +++ b/licenses/monoxna.LICENSE @@ -0,0 +1,22 @@ +MIT License +Copyright 2006 The Mono.Xna Team + +All rights reserved. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/src/Audio/SoundInstance.cs b/src/Audio/SoundInstance.cs index 42b2794..0f00779 100644 --- a/src/Audio/SoundInstance.cs +++ b/src/Audio/SoundInstance.cs @@ -67,7 +67,7 @@ namespace MoonWorks.Audio _pitch = value; FAudio.FAudioSourceVoice_SetFrequencyRatio( Handle, - (float) Math.Pow(2.0, _pitch) * doppler, + (float) System.Math.Pow(2.0, _pitch) * doppler, 0 ); } diff --git a/src/Math/BoundingBox.cs b/src/Math/BoundingBox.cs new file mode 100644 index 0000000..c55c51d --- /dev/null +++ b/src/Math/BoundingBox.cs @@ -0,0 +1,614 @@ +#region License + +/* MoonWorks - Game Development Framework + * Copyright 2021 Evan Hemsley + */ + +/* Derived from code by Ethan Lee (Copyright 2009-2021). + * Released under the Microsoft Public License. + * See fna.LICENSE for details. + + * Derived from code by the Mono.Xna Team (Copyright 2006). + * Released under the MIT License. See monoxna.LICENSE for details. + */ + +#endregion + +#region Using Statements +using System; +using System.Collections.Generic; +using System.Diagnostics; + +#endregion + +namespace MoonWorks.Math +{ + [Serializable] + [DebuggerDisplay("{DebugDisplayString,nq}")] + public struct BoundingBox : IEquatable + { + #region Internal Properties + + internal string DebugDisplayString + { + get + { + return string.Concat( + "Min( ", Min.DebugDisplayString, " ) \r\n", + "Max( ", Max.DebugDisplayString, " )" + ); + } + } + + #endregion + + #region Public Fields + + public Vector3 Min; + + public Vector3 Max; + + public const int CornerCount = 8; + + #endregion + + #region Private Static Variables + + private static readonly Vector3 MaxVector3 = new Vector3(float.MaxValue); + private static readonly Vector3 MinVector3 = new Vector3(float.MinValue); + + #endregion + + #region Public Constructors + + public BoundingBox(Vector3 min, Vector3 max) + { + this.Min = min; + this.Max = max; + } + + #endregion + + #region Public Methods + + public void Contains(ref BoundingBox box, out ContainmentType result) + { + result = Contains(box); + } + + public void Contains(ref BoundingSphere sphere, out ContainmentType result) + { + result = this.Contains(sphere); + } + + public ContainmentType Contains(Vector3 point) + { + ContainmentType result; + this.Contains(ref point, out result); + return result; + } + + public ContainmentType Contains(BoundingBox box) + { + // Test if all corner is in the same side of a face by just checking min and max + if ( box.Max.X < Min.X || + box.Min.X > Max.X || + box.Max.Y < Min.Y || + box.Min.Y > Max.Y || + box.Max.Z < Min.Z || + box.Min.Z > Max.Z ) + { + return ContainmentType.Disjoint; + } + + + if ( box.Min.X >= Min.X && + box.Max.X <= Max.X && + box.Min.Y >= Min.Y && + box.Max.Y <= Max.Y && + box.Min.Z >= Min.Z && + box.Max.Z <= Max.Z ) + { + return ContainmentType.Contains; + } + + return ContainmentType.Intersects; + } + + public ContainmentType Contains(BoundingFrustum frustum) + { + /* TODO: bad done here need a fix. + * Because the question is not if frustum contains box but the reverse and + * this is not the same. + */ + int i; + ContainmentType contained; + Vector3[] corners = frustum.GetCorners(); + + // First we check if frustum is in box. + for (i = 0; i < corners.Length; i += 1) + { + this.Contains(ref corners[i], out contained); + if (contained == ContainmentType.Disjoint) + { + break; + } + } + + // This means we checked all the corners and they were all contain or instersect + if (i == corners.Length) + { + return ContainmentType.Contains; + } + + // If i is not equal to zero, we can fastpath and say that this box intersects + if (i != 0) + { + return ContainmentType.Intersects; + } + + + /* If we get here, it means the first (and only) point we checked was + * actually contained in the frustum. So we assume that all other points + * will also be contained. If one of the points is disjoint, we can + * exit immediately saying that the result is Intersects + */ + i += 1; + for (; i < corners.Length; i += 1) + { + this.Contains(ref corners[i], out contained); + if (contained != ContainmentType.Contains) + { + return ContainmentType.Intersects; + } + + } + + /* If we get here, then we know all the points were actually contained, + * therefore result is Contains. + */ + return ContainmentType.Contains; + } + + public ContainmentType Contains(BoundingSphere sphere) + { + if ( sphere.Center.X - Min.X >= sphere.Radius && + sphere.Center.Y - Min.Y >= sphere.Radius && + sphere.Center.Z - Min.Z >= sphere.Radius && + Max.X - sphere.Center.X >= sphere.Radius && + Max.Y - sphere.Center.Y >= sphere.Radius && + Max.Z - sphere.Center.Z >= sphere.Radius ) + { + return ContainmentType.Contains; + } + + double dmin = 0; + + double e = sphere.Center.X - Min.X; + if (e < 0) + { + if (e < -sphere.Radius) + { + return ContainmentType.Disjoint; + } + dmin += e * e; + } + else + { + e = sphere.Center.X - Max.X; + if (e > 0) + { + if (e > sphere.Radius) + { + return ContainmentType.Disjoint; + } + dmin += e * e; + } + } + + e = sphere.Center.Y - Min.Y; + if (e < 0) + { + if (e < -sphere.Radius) + { + return ContainmentType.Disjoint; + } + dmin += e * e; + } + else + { + e = sphere.Center.Y - Max.Y; + if (e > 0) + { + if (e > sphere.Radius) + { + return ContainmentType.Disjoint; + } + dmin += e * e; + } + } + + e = sphere.Center.Z - Min.Z; + if (e < 0) + { + if (e < -sphere.Radius) + { + return ContainmentType.Disjoint; + } + dmin += e * e; + } + else + { + e = sphere.Center.Z - Max.Z; + if (e > 0) + { + if (e > sphere.Radius) + { + return ContainmentType.Disjoint; + } + dmin += e * e; + } + } + + if (dmin <= sphere.Radius * sphere.Radius) + { + return ContainmentType.Intersects; + } + + return ContainmentType.Disjoint; + } + + public void Contains(ref Vector3 point, out ContainmentType result) + { + // Determine if point is outside of this box. + if ( point.X < this.Min.X || + point.X > this.Max.X || + point.Y < this.Min.Y || + point.Y > this.Max.Y || + point.Z < this.Min.Z || + point.Z > this.Max.Z ) + { + result = ContainmentType.Disjoint; + } + else + { + result = ContainmentType.Contains; + } + } + + public Vector3[] GetCorners() + { + return new Vector3[] { + new Vector3(this.Min.X, this.Max.Y, this.Max.Z), + new Vector3(this.Max.X, this.Max.Y, this.Max.Z), + new Vector3(this.Max.X, this.Min.Y, this.Max.Z), + new Vector3(this.Min.X, this.Min.Y, this.Max.Z), + new Vector3(this.Min.X, this.Max.Y, this.Min.Z), + new Vector3(this.Max.X, this.Max.Y, this.Min.Z), + new Vector3(this.Max.X, this.Min.Y, this.Min.Z), + new Vector3(this.Min.X, this.Min.Y, this.Min.Z) + }; + } + + public void GetCorners(Vector3[] corners) + { + if (corners == null) + { + throw new ArgumentNullException("corners"); + } + if (corners.Length < 8) + { + throw new ArgumentOutOfRangeException("corners", "Not Enought Corners"); + } + corners[0].X = this.Min.X; + corners[0].Y = this.Max.Y; + corners[0].Z = this.Max.Z; + corners[1].X = this.Max.X; + corners[1].Y = this.Max.Y; + corners[1].Z = this.Max.Z; + corners[2].X = this.Max.X; + corners[2].Y = this.Min.Y; + corners[2].Z = this.Max.Z; + corners[3].X = this.Min.X; + corners[3].Y = this.Min.Y; + corners[3].Z = this.Max.Z; + corners[4].X = this.Min.X; + corners[4].Y = this.Max.Y; + corners[4].Z = this.Min.Z; + corners[5].X = this.Max.X; + corners[5].Y = this.Max.Y; + corners[5].Z = this.Min.Z; + corners[6].X = this.Max.X; + corners[6].Y = this.Min.Y; + corners[6].Z = this.Min.Z; + corners[7].X = this.Min.X; + corners[7].Y = this.Min.Y; + corners[7].Z = this.Min.Z; + } + + public Nullable Intersects(Ray ray) + { + return ray.Intersects(this); + } + + public void Intersects(ref Ray ray, out Nullable result) + { + result = Intersects(ray); + } + + public bool Intersects(BoundingFrustum frustum) + { + return frustum.Intersects(this); + } + + public void Intersects(ref BoundingSphere sphere, out bool result) + { + result = Intersects(sphere); + } + + public bool Intersects(BoundingBox box) + { + bool result; + Intersects(ref box, out result); + return result; + } + + public PlaneIntersectionType Intersects(Plane plane) + { + PlaneIntersectionType result; + Intersects(ref plane, out result); + return result; + } + + public void Intersects(ref BoundingBox box, out bool result) + { + if ((this.Max.X >= box.Min.X) && (this.Min.X <= box.Max.X)) + { + if ((this.Max.Y < box.Min.Y) || (this.Min.Y > box.Max.Y)) + { + result = false; + return; + } + + result = (this.Max.Z >= box.Min.Z) && (this.Min.Z <= box.Max.Z); + return; + } + + result = false; + return; + } + + public bool Intersects(BoundingSphere sphere) + { + if ( sphere.Center.X - Min.X > sphere.Radius && + sphere.Center.Y - Min.Y > sphere.Radius && + sphere.Center.Z - Min.Z > sphere.Radius && + Max.X - sphere.Center.X > sphere.Radius && + Max.Y - sphere.Center.Y > sphere.Radius && + Max.Z - sphere.Center.Z > sphere.Radius ) + { + return true; + } + + double dmin = 0; + + if (sphere.Center.X - Min.X <= sphere.Radius) + { + dmin += (sphere.Center.X - Min.X) * (sphere.Center.X - Min.X); + } + else if (Max.X - sphere.Center.X <= sphere.Radius) + { + dmin += (sphere.Center.X - Max.X) * (sphere.Center.X - Max.X); + } + + if (sphere.Center.Y - Min.Y <= sphere.Radius) + { + dmin += (sphere.Center.Y - Min.Y) * (sphere.Center.Y - Min.Y); + } + else if (Max.Y - sphere.Center.Y <= sphere.Radius) + { + dmin += (sphere.Center.Y - Max.Y) * (sphere.Center.Y - Max.Y); + } + + if (sphere.Center.Z - Min.Z <= sphere.Radius) + { + dmin += (sphere.Center.Z - Min.Z) * (sphere.Center.Z - Min.Z); + } + else if (Max.Z - sphere.Center.Z <= sphere.Radius) + { + dmin += (sphere.Center.Z - Max.Z) * (sphere.Center.Z - Max.Z); + } + + if (dmin <= sphere.Radius * sphere.Radius) + { + return true; + } + + return false; + } + + public void Intersects(ref Plane plane, out PlaneIntersectionType result) + { + // See http://zach.in.tu-clausthal.de/teaching/cg_literatur/lighthouse3d_view_frustum_culling/index.html + + Vector3 positiveVertex; + Vector3 negativeVertex; + + if (plane.Normal.X >= 0) + { + positiveVertex.X = Max.X; + negativeVertex.X = Min.X; + } + else + { + positiveVertex.X = Min.X; + negativeVertex.X = Max.X; + } + + if (plane.Normal.Y >= 0) + { + positiveVertex.Y = Max.Y; + negativeVertex.Y = Min.Y; + } + else + { + positiveVertex.Y = Min.Y; + negativeVertex.Y = Max.Y; + } + + if (plane.Normal.Z >= 0) + { + positiveVertex.Z = Max.Z; + negativeVertex.Z = Min.Z; + } + else + { + positiveVertex.Z = Min.Z; + negativeVertex.Z = Max.Z; + } + + // Inline Vector3.Dot(plane.Normal, negativeVertex) + plane.D; + float distance = ( + plane.Normal.X * negativeVertex.X + + plane.Normal.Y * negativeVertex.Y + + plane.Normal.Z * negativeVertex.Z + + plane.D + ); + if (distance > 0) + { + result = PlaneIntersectionType.Front; + return; + } + + // Inline Vector3.Dot(plane.Normal, positiveVertex) + plane.D; + distance = ( + plane.Normal.X * positiveVertex.X + + plane.Normal.Y * positiveVertex.Y + + plane.Normal.Z * positiveVertex.Z + + plane.D + ); + if (distance < 0) + { + result = PlaneIntersectionType.Back; + return; + } + + result = PlaneIntersectionType.Intersecting; + } + + public bool Equals(BoundingBox other) + { + return (this.Min == other.Min) && (this.Max == other.Max); + } + + #endregion + + #region Public Static Methods + + /// + /// Create a bounding box from the given list of points. + /// + /// + /// The list of Vector3 instances defining the point cloud to bound. + /// + /// A bounding box that encapsulates the given point cloud. + /// + /// Thrown if the given list has no points. + /// + public static BoundingBox CreateFromPoints(IEnumerable points) + { + if (points == null) + { + throw new ArgumentNullException("points"); + } + + bool empty = true; + Vector3 minVec = MaxVector3; + Vector3 maxVec = MinVector3; + foreach (Vector3 ptVector in points) + { + minVec.X = (minVec.X < ptVector.X) ? minVec.X : ptVector.X; + minVec.Y = (minVec.Y < ptVector.Y) ? minVec.Y : ptVector.Y; + minVec.Z = (minVec.Z < ptVector.Z) ? minVec.Z : ptVector.Z; + + maxVec.X = (maxVec.X > ptVector.X) ? maxVec.X : ptVector.X; + maxVec.Y = (maxVec.Y > ptVector.Y) ? maxVec.Y : ptVector.Y; + maxVec.Z = (maxVec.Z > ptVector.Z) ? maxVec.Z : ptVector.Z; + + empty = false; + } + if (empty) + { + throw new ArgumentException("Collection is empty", "points"); + } + + return new BoundingBox(minVec, maxVec); + } + + public static BoundingBox CreateFromSphere(BoundingSphere sphere) + { + BoundingBox result; + CreateFromSphere(ref sphere, out result); + return result; + } + + public static void CreateFromSphere(ref BoundingSphere sphere, out BoundingBox result) + { + Vector3 corner = new Vector3(sphere.Radius); + result.Min = sphere.Center - corner; + result.Max = sphere.Center + corner; + } + + public static BoundingBox CreateMerged(BoundingBox original, BoundingBox additional) + { + BoundingBox result; + CreateMerged(ref original, ref additional, out result); + return result; + } + + public static void CreateMerged(ref BoundingBox original, ref BoundingBox additional, out BoundingBox result) + { + result.Min.X = System.Math.Min(original.Min.X, additional.Min.X); + result.Min.Y = System.Math.Min(original.Min.Y, additional.Min.Y); + result.Min.Z = System.Math.Min(original.Min.Z, additional.Min.Z); + result.Max.X = System.Math.Max(original.Max.X, additional.Max.X); + result.Max.Y = System.Math.Max(original.Max.Y, additional.Max.Y); + result.Max.Z = System.Math.Max(original.Max.Z, additional.Max.Z); + } + + #endregion + + #region Public Static Operators and Override Methods + + public override bool Equals(object obj) + { + return (obj is BoundingBox) && Equals((BoundingBox) obj); + } + + public override int GetHashCode() + { + return this.Min.GetHashCode() + this.Max.GetHashCode(); + } + + public static bool operator ==(BoundingBox a, BoundingBox b) + { + return a.Equals(b); + } + + public static bool operator !=(BoundingBox a, BoundingBox b) + { + return !a.Equals(b); + } + + public override string ToString() + { + return ( + "{{Min:" + Min.ToString() + + " Max:" + Max.ToString() + + "}}" + ); + } + + #endregion + } +} diff --git a/src/Math/BoundingFrustum.cs b/src/Math/BoundingFrustum.cs new file mode 100644 index 0000000..48cb172 --- /dev/null +++ b/src/Math/BoundingFrustum.cs @@ -0,0 +1,729 @@ +#region License + +/* MoonWorks - Game Development Framework + * Copyright 2021 Evan Hemsley + */ + +/* Derived from code by Ethan Lee (Copyright 2009-2021). + * Released under the Microsoft Public License. + * See fna.LICENSE for details. + + * Derived from code by the Mono.Xna Team (Copyright 2006). + * Released under the MIT License. See monoxna.LICENSE for details. + */ + +#endregion + +#region Using Statements +using System; +using System.Diagnostics; +using System.Text; +#endregion + +namespace MoonWorks.Math +{ + /// + /// Defines a viewing frustum for intersection operations. + /// + [DebuggerDisplay("{DebugDisplayString,nq}")] + public class BoundingFrustum : IEquatable + { + #region Public Properties + + /// + /// Gets or sets the of the frustum. + /// + public Matrix Matrix + { + get + { + return this.matrix; + } + set + { + /* FIXME: The odds are the planes will be used a lot more often than + * the matrix is updated, so this should help performance. I hope. ;) + */ + this.matrix = value; + this.CreatePlanes(); + this.CreateCorners(); + } + } + + /// + /// Gets the near plane of the frustum. + /// + public Plane Near + { + get + { + return this.planes[0]; + } + } + + /// + /// Gets the far plane of the frustum. + /// + public Plane Far + { + get + { + return this.planes[1]; + } + } + + /// + /// Gets the left plane of the frustum. + /// + public Plane Left + { + get + { + return this.planes[2]; + } + } + + /// + /// Gets the right plane of the frustum. + /// + public Plane Right + { + get + { + return this.planes[3]; + } + } + + /// + /// Gets the top plane of the frustum. + /// + public Plane Top + { + get + { + return this.planes[4]; + } + } + + /// + /// Gets the bottom plane of the frustum. + /// + public Plane Bottom + { + get + { + return this.planes[5]; + } + } + + #endregion + + #region Internal Properties + + internal string DebugDisplayString + { + get + { + return string.Concat( + "Near( ", planes[0].DebugDisplayString, " ) \r\n", + "Far( ", planes[1].DebugDisplayString, " ) \r\n", + "Left( ", planes[2].DebugDisplayString, " ) \r\n", + "Right( ", planes[3].DebugDisplayString, " ) \r\n", + "Top( ", planes[4].DebugDisplayString, " ) \r\n", + "Bottom( ", planes[5].DebugDisplayString, " ) " + ); + } + } + + #endregion + + #region Public Fields + + /// + /// The number of corner points in the frustum. + /// + public const int CornerCount = 8; + + #endregion + + #region Private Fields + + private Matrix matrix; + private readonly Vector3[] corners = new Vector3[CornerCount]; + private readonly Plane[] planes = new Plane[PlaneCount]; + + /// + /// The number of planes in the frustum. + /// + private const int PlaneCount = 6; + + #endregion + + #region Public Constructors + + /// + /// Constructs the frustum by extracting the view planes from a matrix. + /// + /// Combined matrix which usually is (View * Projection). + public BoundingFrustum(Matrix value) + { + this.matrix = value; + this.CreatePlanes(); + this.CreateCorners(); + } + + #endregion + + #region Public Methods + + /// + /// Containment test between this and specified . + /// + /// A for testing. + /// Result of testing for containment between this and specified . + public ContainmentType Contains(BoundingFrustum frustum) + { + if (this == frustum) + { + return ContainmentType.Contains; + } + bool intersects = false; + for (int i = 0; i < PlaneCount; i += 1) + { + PlaneIntersectionType planeIntersectionType; + frustum.Intersects(ref planes[i], out planeIntersectionType); + if (planeIntersectionType == PlaneIntersectionType.Front) + { + return ContainmentType.Disjoint; + } + else if (planeIntersectionType == PlaneIntersectionType.Intersecting) + { + intersects = true; + } + } + return intersects ? ContainmentType.Intersects : ContainmentType.Contains; + } + + /// + /// Containment test between this and specified . + /// + /// A for testing. + /// Result of testing for containment between this and specified . + public ContainmentType Contains(BoundingBox box) + { + ContainmentType result = default(ContainmentType); + this.Contains(ref box, out result); + return result; + } + + /// + /// Containment test between this and specified . + /// + /// A for testing. + /// Result of testing for containment between this and specified as an output parameter. + public void Contains(ref BoundingBox box, out ContainmentType result) + { + bool intersects = false; + for (int i = 0; i < PlaneCount; i += 1) + { + PlaneIntersectionType planeIntersectionType = default(PlaneIntersectionType); + box.Intersects(ref this.planes[i], out planeIntersectionType); + switch (planeIntersectionType) + { + case PlaneIntersectionType.Front: + result = ContainmentType.Disjoint; + return; + case PlaneIntersectionType.Intersecting: + intersects = true; + break; + } + } + result = intersects ? ContainmentType.Intersects : ContainmentType.Contains; + } + + /// + /// Containment test between this and specified . + /// + /// A for testing. + /// Result of testing for containment between this and specified . + public ContainmentType Contains(BoundingSphere sphere) + { + ContainmentType result = default(ContainmentType); + this.Contains(ref sphere, out result); + return result; + } + + /// + /// Containment test between this and specified . + /// + /// A for testing. + /// Result of testing for containment between this and specified as an output parameter. + public void Contains(ref BoundingSphere sphere, out ContainmentType result) + { + bool intersects = false; + for (int i = 0; i < PlaneCount; i += 1) + { + PlaneIntersectionType planeIntersectionType = default(PlaneIntersectionType); + + // TODO: We might want to inline this for performance reasons. + sphere.Intersects(ref this.planes[i], out planeIntersectionType); + switch (planeIntersectionType) + { + case PlaneIntersectionType.Front: + result = ContainmentType.Disjoint; + return; + case PlaneIntersectionType.Intersecting: + intersects = true; + break; + } + } + result = intersects ? ContainmentType.Intersects : ContainmentType.Contains; + } + + /// + /// Containment test between this and specified . + /// + /// A for testing. + /// Result of testing for containment between this and specified . + public ContainmentType Contains(Vector3 point) + { + ContainmentType result = default(ContainmentType); + this.Contains(ref point, out result); + return result; + } + + /// + /// Containment test between this and specified . + /// + /// A for testing. + /// Result of testing for containment between this and specified as an output parameter. + public void Contains(ref Vector3 point, out ContainmentType result) + { + bool intersects = false; + for (int i = 0; i < PlaneCount; i += 1) + { + float classifyPoint = ( + (point.X * planes[i].Normal.X) + + (point.Y * planes[i].Normal.Y) + + (point.Z * planes[i].Normal.Z) + + planes[i].D + ); + if (classifyPoint > 0) + { + result = ContainmentType.Disjoint; + return; + } + else if (classifyPoint == 0) + { + intersects = true; + break; + } + } + result = intersects ? ContainmentType.Intersects : ContainmentType.Contains; + } + + /// + /// Returns a copy of internal corners array. + /// + /// The array of corners. + public Vector3[] GetCorners() + { + return (Vector3[]) this.corners.Clone(); + } + + /// + /// Returns a copy of internal corners array. + /// + /// The array which values will be replaced to corner values of this instance. It must have size of . + public void GetCorners(Vector3[] corners) + { + if (corners == null) + { + throw new ArgumentNullException("corners"); + } + if (corners.Length < CornerCount) + { + throw new ArgumentOutOfRangeException("corners"); + } + + this.corners.CopyTo(corners, 0); + } + + /// + /// Gets whether or not a specified intersects with this . + /// + /// An other for intersection test. + /// true if other intersects with this ; false otherwise. + public bool Intersects(BoundingFrustum frustum) + { + return (Contains(frustum) != ContainmentType.Disjoint); + } + + /// + /// Gets whether or not a specified intersects with this . + /// + /// A for intersection test. + /// true if specified intersects with this ; false otherwise. + public bool Intersects(BoundingBox box) + { + bool result = false; + this.Intersects(ref box, out result); + return result; + } + + /// + /// Gets whether or not a specified intersects with this . + /// + /// A for intersection test. + /// true if specified intersects with this ; false otherwise as an output parameter. + public void Intersects(ref BoundingBox box, out bool result) + { + ContainmentType containment = default(ContainmentType); + this.Contains(ref box, out containment); + result = containment != ContainmentType.Disjoint; + } + + /// + /// Gets whether or not a specified intersects with this . + /// + /// A for intersection test. + /// true if specified intersects with this ; false otherwise. + public bool Intersects(BoundingSphere sphere) + { + bool result = default(bool); + this.Intersects(ref sphere, out result); + return result; + } + + /// + /// Gets whether or not a specified intersects with this . + /// + /// A for intersection test. + /// true if specified intersects with this ; false otherwise as an output parameter. + public void Intersects(ref BoundingSphere sphere, out bool result) + { + ContainmentType containment = default(ContainmentType); + this.Contains(ref sphere, out containment); + result = containment != ContainmentType.Disjoint; + } + + /// + /// Gets type of intersection between specified and this . + /// + /// A for intersection test. + /// A plane intersection type. + public PlaneIntersectionType Intersects(Plane plane) + { + PlaneIntersectionType result; + Intersects(ref plane, out result); + return result; + } + + /// + /// Gets type of intersection between specified and this . + /// + /// A for intersection test. + /// A plane intersection type as an output parameter. + public void Intersects(ref Plane plane, out PlaneIntersectionType result) + { + result = plane.Intersects(ref corners[0]); + for (int i = 1; i < corners.Length; i += 1) + { + if (plane.Intersects(ref corners[i]) != result) + { + result = PlaneIntersectionType.Intersecting; + } + } + } + + /// + /// Gets the distance of intersection of and this or null if no intersection happens. + /// + /// A for intersection test. + /// Distance at which ray intersects with this or null if no intersection happens. + public float? Intersects(Ray ray) + { + float? result; + Intersects(ref ray, out result); + return result; + } + + /// + /// Gets the distance of intersection of and this or null if no intersection happens. + /// + /// A for intersection test. + /// Distance at which ray intersects with this or null if no intersection happens as an output parameter. + public void Intersects(ref Ray ray, out float? result) + { + ContainmentType ctype; + Contains(ref ray.Position, out ctype); + + if (ctype == ContainmentType.Disjoint) + { + result = null; + return; + } + if (ctype == ContainmentType.Contains) + { + result = 0.0f; + return; + } + if (ctype != ContainmentType.Intersects) + { + throw new ArgumentOutOfRangeException("ctype"); + } + + throw new NotImplementedException(); + } + + #endregion + + #region Private Methods + + private void CreateCorners() + { + IntersectionPoint( + ref this.planes[0], + ref this.planes[2], + ref this.planes[4], + out this.corners[0] + ); + IntersectionPoint( + ref this.planes[0], + ref this.planes[3], + ref this.planes[4], + out this.corners[1] + ); + IntersectionPoint( + ref this.planes[0], + ref this.planes[3], + ref this.planes[5], + out this.corners[2] + ); + IntersectionPoint( + ref this.planes[0], + ref this.planes[2], + ref this.planes[5], + out this.corners[3] + ); + IntersectionPoint( + ref this.planes[1], + ref this.planes[2], + ref this.planes[4], + out this.corners[4] + ); + IntersectionPoint( + ref this.planes[1], + ref this.planes[3], + ref this.planes[4], + out this.corners[5] + ); + IntersectionPoint( + ref this.planes[1], + ref this.planes[3], + ref this.planes[5], + out this.corners[6] + ); + IntersectionPoint( + ref this.planes[1], + ref this.planes[2], + ref this.planes[5], + out this.corners[7] + ); + } + + private void CreatePlanes() + { + this.planes[0] = new Plane( + -this.matrix.M13, + -this.matrix.M23, + -this.matrix.M33, + -this.matrix.M43 + ); + this.planes[1] = new Plane( + this.matrix.M13 - this.matrix.M14, + this.matrix.M23 - this.matrix.M24, + this.matrix.M33 - this.matrix.M34, + this.matrix.M43 - this.matrix.M44 + ); + this.planes[2] = new Plane( + -this.matrix.M14 - this.matrix.M11, + -this.matrix.M24 - this.matrix.M21, + -this.matrix.M34 - this.matrix.M31, + -this.matrix.M44 - this.matrix.M41 + ); + this.planes[3] = new Plane( + this.matrix.M11 - this.matrix.M14, + this.matrix.M21 - this.matrix.M24, + this.matrix.M31 - this.matrix.M34, + this.matrix.M41 - this.matrix.M44 + ); + this.planes[4] = new Plane( + this.matrix.M12 - this.matrix.M14, + this.matrix.M22 - this.matrix.M24, + this.matrix.M32 - this.matrix.M34, + this.matrix.M42 - this.matrix.M44 + ); + this.planes[5] = new Plane( + -this.matrix.M14 - this.matrix.M12, + -this.matrix.M24 - this.matrix.M22, + -this.matrix.M34 - this.matrix.M32, + -this.matrix.M44 - this.matrix.M42 + ); + + this.NormalizePlane(ref this.planes[0]); + this.NormalizePlane(ref this.planes[1]); + this.NormalizePlane(ref this.planes[2]); + this.NormalizePlane(ref this.planes[3]); + this.NormalizePlane(ref this.planes[4]); + this.NormalizePlane(ref this.planes[5]); + } + + private void NormalizePlane(ref Plane p) + { + float factor = 1f / p.Normal.Length(); + p.Normal.X *= factor; + p.Normal.Y *= factor; + p.Normal.Z *= factor; + p.D *= factor; + } + + #endregion + + #region Private Static Methods + + private static void IntersectionPoint( + ref Plane a, + ref Plane b, + ref Plane c, + out Vector3 result + ) { + /* Formula used + * d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 ) + * P = ------------------------------------------------------------------- + * N1 . ( N2 * N3 ) + * + * Note: N refers to the normal, d refers to the displacement. '.' means dot + * product. '*' means cross product + */ + + Vector3 v1, v2, v3; + Vector3 cross; + + Vector3.Cross(ref b.Normal, ref c.Normal, out cross); + + float f; + Vector3.Dot(ref a.Normal, ref cross, out f); + f *= -1.0f; + + Vector3.Cross(ref b.Normal, ref c.Normal, out cross); + Vector3.Multiply(ref cross, a.D, out v1); + // v1 = (a.D * (Vector3.Cross(b.Normal, c.Normal))); + + + Vector3.Cross(ref c.Normal, ref a.Normal, out cross); + Vector3.Multiply(ref cross, b.D, out v2); + // v2 = (b.D * (Vector3.Cross(c.Normal, a.Normal))); + + + Vector3.Cross(ref a.Normal, ref b.Normal, out cross); + Vector3.Multiply(ref cross, c.D, out v3); + // v3 = (c.D * (Vector3.Cross(a.Normal, b.Normal))); + + result.X = (v1.X + v2.X + v3.X) / f; + result.Y = (v1.Y + v2.Y + v3.Y) / f; + result.Z = (v1.Z + v2.Z + v3.Z) / f; + } + + #endregion + + #region Public Static Operators and Override Methods + + /// + /// Compares whether two instances are equal. + /// + /// instance on the left of the equal sign. + /// instance on the right of the equal sign. + /// true if the instances are equal; false otherwise. + public static bool operator ==(BoundingFrustum a, BoundingFrustum b) + { + if (object.Equals(a, null)) + { + return (object.Equals(b, null)); + } + + if (object.Equals(b, null)) + { + return (object.Equals(a, null)); + } + + return a.matrix == (b.matrix); + } + + /// + /// Compares whether two instances are not equal. + /// + /// instance on the left of the not equal sign. + /// instance on the right of the not equal sign. + /// true if the instances are not equal; false otherwise. + public static bool operator !=(BoundingFrustum a, BoundingFrustum b) + { + return !(a == b); + } + + /// + /// Compares whether current instance is equal to specified . + /// + /// The to compare. + /// true if the instances are equal; false otherwise. + public bool Equals(BoundingFrustum other) + { + return (this == other); + } + + /// + /// Compares whether current instance is equal to specified . + /// + /// The to compare. + /// true if the instances are equal; false otherwise. + public override bool Equals(object obj) + { + return (obj is BoundingFrustum) && Equals((BoundingFrustum) obj); + } + + /// + /// Returns a representation of this in the format: + /// {Near:[nearPlane] Far:[farPlane] Left:[leftPlane] Right:[rightPlane] Top:[topPlane] Bottom:[bottomPlane]} + /// + /// representation of this . + public override string ToString() + { + StringBuilder sb = new StringBuilder(256); + sb.Append("{Near:"); + sb.Append(this.planes[0].ToString()); + sb.Append(" Far:"); + sb.Append(this.planes[1].ToString()); + sb.Append(" Left:"); + sb.Append(this.planes[2].ToString()); + sb.Append(" Right:"); + sb.Append(this.planes[3].ToString()); + sb.Append(" Top:"); + sb.Append(this.planes[4].ToString()); + sb.Append(" Bottom:"); + sb.Append(this.planes[5].ToString()); + sb.Append("}"); + return sb.ToString(); + } + + /// + /// Gets the hash code of this . + /// + /// Hash code of this . + public override int GetHashCode() + { + return this.matrix.GetHashCode(); + } + + #endregion + } +} diff --git a/src/Math/BoundingSphere.cs b/src/Math/BoundingSphere.cs new file mode 100644 index 0000000..c0c1870 --- /dev/null +++ b/src/Math/BoundingSphere.cs @@ -0,0 +1,683 @@ +#region License + +/* MoonWorks - Game Development Framework + * Copyright 2021 Evan Hemsley + */ + +/* Derived from code by Ethan Lee (Copyright 2009-2021). + * Released under the Microsoft Public License. + * See fna.LICENSE for details. + + * Derived from code by the Mono.Xna Team (Copyright 2006). + * Released under the MIT License. See monoxna.LICENSE for details. + */ + +#endregion + +#region Using Statements +using System; +using System.Collections.Generic; +using System.Diagnostics; + +#endregion + +namespace MoonWorks.Math +{ + /// + /// Describes a sphere in 3D-space for bounding operations. + /// + [Serializable] + [DebuggerDisplay("{DebugDisplayString,nq}")] + public struct BoundingSphere : IEquatable + { + #region Internal Properties + + internal string DebugDisplayString + { + get + { + return string.Concat( + "Center( ", Center.DebugDisplayString, " ) \r\n", + "Radius( ", Radius.ToString(), " ) " + ); + } + } + + #endregion + + #region Public Fields + + /// + /// The sphere center. + /// + public Vector3 Center; + + /// + /// The sphere radius. + /// + public float Radius; + + #endregion + + #region Public Constructors + + /// + /// Constructs a bounding sphere with the specified center and radius. + /// + /// The sphere center. + /// The sphere radius. + public BoundingSphere(Vector3 center, float radius) + { + this.Center = center; + this.Radius = radius; + } + + #endregion + + #region Public Methods + + /// + /// Creates a new that contains a transformation of translation and scale from this sphere by the specified . + /// + /// The transformation . + /// Transformed . + public BoundingSphere Transform(Matrix matrix) + { + BoundingSphere sphere = new BoundingSphere(); + sphere.Center = Vector3.Transform(this.Center, matrix); + sphere.Radius = this.Radius * + ( + (float) System.Math.Sqrt((double) System.Math.Max( + ((matrix.M11 * matrix.M11) + (matrix.M12 * matrix.M12)) + (matrix.M13 * matrix.M13), + System.Math.Max( + ((matrix.M21 * matrix.M21) + (matrix.M22 * matrix.M22)) + (matrix.M23 * matrix.M23), + ((matrix.M31 * matrix.M31) + (matrix.M32 * matrix.M32)) + (matrix.M33 * matrix.M33)) + ) + ) + ); + return sphere; + } + + /// + /// Creates a new that contains a transformation of translation and scale from this sphere by the specified . + /// + /// The transformation . + /// Transformed as an output parameter. + public void Transform(ref Matrix matrix, out BoundingSphere result) + { + result.Center = Vector3.Transform(this.Center, matrix); + result.Radius = this.Radius * + ( + (float) System.Math.Sqrt((double) System.Math.Max( + ((matrix.M11 * matrix.M11) + (matrix.M12 * matrix.M12)) + (matrix.M13 * matrix.M13), + System.Math.Max( + ((matrix.M21 * matrix.M21) + (matrix.M22 * matrix.M22)) + (matrix.M23 * matrix.M23), + ((matrix.M31 * matrix.M31) + (matrix.M32 * matrix.M32)) + (matrix.M33 * matrix.M33)) + ) + ) + ); + } + + /// + /// Test if a bounding box is fully inside, outside, or just intersecting the sphere. + /// + /// The box for testing. + /// The containment type as an output parameter. + public void Contains(ref BoundingBox box, out ContainmentType result) + { + result = this.Contains(box); + } + + /// + /// Test if a sphere is fully inside, outside, or just intersecting the sphere. + /// + /// The other sphere for testing. + /// The containment type as an output parameter. + public void Contains(ref BoundingSphere sphere, out ContainmentType result) + { + result = Contains(sphere); + } + + /// + /// Test if a point is fully inside, outside, or just intersecting the sphere. + /// + /// The vector in 3D-space for testing. + /// The containment type as an output parameter. + public void Contains(ref Vector3 point, out ContainmentType result) + { + result = Contains(point); + } + + /// + /// Test if a bounding box is fully inside, outside, or just intersecting the sphere. + /// + /// The box for testing. + /// The containment type. + public ContainmentType Contains(BoundingBox box) + { + // Check if all corners are in sphere. + bool inside = true; + foreach (Vector3 corner in box.GetCorners()) + { + if (this.Contains(corner) == ContainmentType.Disjoint) + { + inside = false; + break; + } + } + + if (inside) + { + return ContainmentType.Contains; + } + + // Check if the distance from sphere center to cube face is less than radius. + double dmin = 0; + + if (Center.X < box.Min.X) + { + dmin += (Center.X - box.Min.X) * (Center.X - box.Min.X); + } + else if (Center.X > box.Max.X) + { + dmin += (Center.X - box.Max.X) * (Center.X - box.Max.X); + } + + if (Center.Y < box.Min.Y) + { + dmin += (Center.Y - box.Min.Y) * (Center.Y - box.Min.Y); + } + else if (Center.Y > box.Max.Y) + { + dmin += (Center.Y - box.Max.Y) * (Center.Y - box.Max.Y); + } + + if (Center.Z < box.Min.Z) + { + dmin += (Center.Z - box.Min.Z) * (Center.Z - box.Min.Z); + } + else if (Center.Z > box.Max.Z) + { + dmin += (Center.Z - box.Max.Z) * (Center.Z - box.Max.Z); + } + + if (dmin <= Radius * Radius) + { + return ContainmentType.Intersects; + } + + // Else disjoint + return ContainmentType.Disjoint; + } + + /// + /// Test if a frustum is fully inside, outside, or just intersecting the sphere. + /// + /// The box for testing. + /// The containment type as an output parameter. + public ContainmentType Contains(BoundingFrustum frustum) + { + // Check if all corners are in sphere. + bool inside = true; + + Vector3[] corners = frustum.GetCorners(); + foreach (Vector3 corner in corners) + { + if (this.Contains(corner) == ContainmentType.Disjoint) + { + inside = false; + break; + } + } + if (inside) + { + return ContainmentType.Contains; + } + + // Check if the distance from sphere center to frustrum face is less than radius. + double dmin = 0; + // TODO : calcul dmin + + if (dmin <= Radius * Radius) + { + return ContainmentType.Intersects; + } + + // Else disjoint + return ContainmentType.Disjoint; + } + + /// + /// Test if a sphere is fully inside, outside, or just intersecting the sphere. + /// + /// The other sphere for testing. + /// The containment type. + public ContainmentType Contains(BoundingSphere sphere) + { + float sqDistance; + Vector3.DistanceSquared(ref sphere.Center, ref Center, out sqDistance); + + if (sqDistance > (sphere.Radius + Radius) * (sphere.Radius + Radius)) + { + return ContainmentType.Disjoint; + } + else if (sqDistance <= (Radius - sphere.Radius) * (Radius - sphere.Radius)) + { + return ContainmentType.Contains; + } + return ContainmentType.Intersects; + } + + /// + /// Test if a point is fully inside, outside, or just intersecting the sphere. + /// + /// The vector in 3D-space for testing. + /// The containment type. + public ContainmentType Contains(Vector3 point) + { + float sqRadius = Radius * Radius; + float sqDistance; + Vector3.DistanceSquared(ref point, ref Center, out sqDistance); + + if (sqDistance > sqRadius) + { + return ContainmentType.Disjoint; + } + else if (sqDistance < sqRadius) + { + return ContainmentType.Contains; + } + return ContainmentType.Intersects; + } + + /// + /// Compares whether current instance is equal to specified . + /// + /// The to compare. + /// true if the instances are equal; false otherwise. + public bool Equals(BoundingSphere other) + { + return ( Center == other.Center && + Radius == other.Radius ); + } + + #endregion + + #region Public Static Methods + + /// + /// Creates the smallest that can contain a specified . + /// + /// The box to create the sphere from. + /// The new . + public static BoundingSphere CreateFromBoundingBox(BoundingBox box) + { + BoundingSphere result; + CreateFromBoundingBox(ref box, out result); + return result; + } + + /// + /// Creates the smallest that can contain a specified . + /// + /// The box to create the sphere from. + /// The new as an output parameter. + public static void CreateFromBoundingBox(ref BoundingBox box, out BoundingSphere result) + { + // Find the center of the box. + Vector3 center = new Vector3( + (box.Min.X + box.Max.X) / 2.0f, + (box.Min.Y + box.Max.Y) / 2.0f, + (box.Min.Z + box.Max.Z) / 2.0f + ); + + // Find the distance between the center and one of the corners of the box. + float radius = Vector3.Distance(center, box.Max); + + result = new BoundingSphere(center, radius); + } + + /// + /// Creates the smallest that can contain a specified . + /// + /// The frustum to create the sphere from. + /// The new . + public static BoundingSphere CreateFromFrustum(BoundingFrustum frustum) + { + return CreateFromPoints(frustum.GetCorners()); + } + + /// + /// Creates the smallest that can contain a specified list of points in 3D-space. + /// + /// List of point to create the sphere from. + /// The new . + public static BoundingSphere CreateFromPoints(IEnumerable points) + { + if (points == null) + { + throw new ArgumentNullException("points"); + } + + // From "Real-Time Collision Detection" (Page 89) + + Vector3 minx = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue); + Vector3 maxx = -minx; + Vector3 miny = minx; + Vector3 maxy = -minx; + Vector3 minz = minx; + Vector3 maxz = -minx; + + // Find the most extreme points along the principle axis. + int numPoints = 0; + foreach (Vector3 pt in points) + { + numPoints += 1; + + if (pt.X < minx.X) + { + minx = pt; + } + if (pt.X > maxx.X) + { + maxx = pt; + } + if (pt.Y < miny.Y) + { + miny = pt; + } + if (pt.Y > maxy.Y) + { + maxy = pt; + } + if (pt.Z < minz.Z) + { + minz = pt; + } + if (pt.Z > maxz.Z) + { + maxz = pt; + } + } + + if (numPoints == 0) + { + throw new ArgumentException( + "You should have at least one point in points." + ); + } + + float sqDistX = Vector3.DistanceSquared(maxx, minx); + float sqDistY = Vector3.DistanceSquared(maxy, miny); + float sqDistZ = Vector3.DistanceSquared(maxz, minz); + + // Pick the pair of most distant points. + Vector3 min = minx; + Vector3 max = maxx; + if (sqDistY > sqDistX && sqDistY > sqDistZ) + { + max = maxy; + min = miny; + } + if (sqDistZ > sqDistX && sqDistZ > sqDistY) + { + max = maxz; + min = minz; + } + + Vector3 center = (min + max) * 0.5f; + float radius = Vector3.Distance(max, center); + + // Test every point and expand the sphere. + // The current bounding sphere is just a good approximation and may not enclose all points. + // From: Mathematics for 3D Game Programming and Computer Graphics, Eric Lengyel, Third Edition. + // Page 218 + float sqRadius = radius * radius; + foreach (Vector3 pt in points) + { + Vector3 diff = (pt - center); + float sqDist = diff.LengthSquared(); + if (sqDist > sqRadius) + { + float distance = (float) System.Math.Sqrt(sqDist); // equal to diff.Length(); + Vector3 direction = diff / distance; + Vector3 G = center - radius * direction; + center = (G + pt) / 2; + radius = Vector3.Distance(pt, center); + sqRadius = radius * radius; + } + } + + return new BoundingSphere(center, radius); + } + + /// + /// Creates the smallest that can contain two spheres. + /// + /// First sphere. + /// Second sphere. + /// The new . + public static BoundingSphere CreateMerged(BoundingSphere original, BoundingSphere additional) + { + BoundingSphere result; + CreateMerged(ref original, ref additional, out result); + return result; + } + + /// + /// Creates the smallest that can contain two spheres. + /// + /// First sphere. + /// Second sphere. + /// The new as an output parameter. + public static void CreateMerged( + ref BoundingSphere original, + ref BoundingSphere additional, + out BoundingSphere result + ) { + Vector3 ocenterToaCenter = Vector3.Subtract(additional.Center, original.Center); + float distance = ocenterToaCenter.Length(); + + // Intersect + if (distance <= original.Radius + additional.Radius) + { + // Original contains additional. + if (distance <= original.Radius - additional.Radius) + { + result = original; + return; + } + + // Additional contains original. + if (distance <= additional.Radius - original.Radius) + { + result = additional; + return; + } + } + + // Else find center of new sphere and radius + float leftRadius = System.Math.Max(original.Radius - distance, additional.Radius); + float Rightradius = System.Math.Max(original.Radius + distance, additional.Radius); + + // oCenterToResultCenter + ocenterToaCenter = ocenterToaCenter + + ( + ((leftRadius - Rightradius) / (2 * ocenterToaCenter.Length())) + * ocenterToaCenter + ); + + result = new BoundingSphere(); + result.Center = original.Center + ocenterToaCenter; + result.Radius = (leftRadius + Rightradius) / 2; + } + + /// + /// Gets whether or not a specified intersects with this sphere. + /// + /// The box for testing. + /// true if intersects with this sphere; false otherwise. + public bool Intersects(BoundingBox box) + { + return box.Intersects(this); + } + + /// + /// Gets whether or not a specified intersects with this sphere. + /// + /// The box for testing. + /// true if intersects with this sphere; false otherwise. As an output parameter. + public void Intersects(ref BoundingBox box, out bool result) + { + box.Intersects(ref this, out result); + } + + public bool Intersects(BoundingFrustum frustum) + { + return frustum.Intersects(this); + } + + /// + /// Gets whether or not the other intersects with this sphere. + /// + /// The other sphere for testing. + /// true if other intersects with this sphere; false otherwise. + public bool Intersects(BoundingSphere sphere) + { + bool result; + Intersects(ref sphere, out result); + return result; + } + + /// + /// Gets whether or not the other intersects with this sphere. + /// + /// The other sphere for testing. + /// true if other intersects with this sphere; false otherwise. As an output parameter. + public void Intersects(ref BoundingSphere sphere, out bool result) + { + float sqDistance; + Vector3.DistanceSquared(ref sphere.Center, ref Center, out sqDistance); + result = !(sqDistance > (sphere.Radius + Radius) * (sphere.Radius + Radius)); + } + + /// + /// Gets whether or not a specified intersects with this sphere. + /// + /// The ray for testing. + /// Distance of ray intersection or null if there is no intersection. + public float? Intersects(Ray ray) + { + return ray.Intersects(this); + } + + /// + /// Gets whether or not a specified intersects with this sphere. + /// + /// The ray for testing. + /// Distance of ray intersection or null if there is no intersection as an output parameter. + public void Intersects(ref Ray ray, out float? result) + { + ray.Intersects(ref this, out result); + } + + /// + /// Gets whether or not a specified intersects with this sphere. + /// + /// The plane for testing. + /// Type of intersection. + public PlaneIntersectionType Intersects(Plane plane) + { + PlaneIntersectionType result = default(PlaneIntersectionType); + // TODO: We might want to inline this for performance reasons. + this.Intersects(ref plane, out result); + return result; + } + + /// + /// Gets whether or not a specified intersects with this sphere. + /// + /// The plane for testing. + /// Type of intersection as an output parameter. + public void Intersects(ref Plane plane, out PlaneIntersectionType result) + { + float distance = default(float); + // TODO: We might want to inline this for performance reasons. + Vector3.Dot(ref plane.Normal, ref this.Center, out distance); + distance += plane.D; + if (distance > this.Radius) + { + result = PlaneIntersectionType.Front; + } + else if (distance < -this.Radius) + { + result = PlaneIntersectionType.Back; + } + else + { + result = PlaneIntersectionType.Intersecting; + } + } + + #endregion + + #region Public Static Operators and Override Methods + + /// + /// Compares whether current instance is equal to specified . + /// + /// The to compare. + /// true if the instances are equal; false otherwise. + public override bool Equals(object obj) + { + return (obj is BoundingSphere) && Equals((BoundingSphere) obj); + } + + /// + /// Gets the hash code of this . + /// + /// Hash code of this . + public override int GetHashCode() + { + return this.Center.GetHashCode() + this.Radius.GetHashCode(); + } + + /// + /// Returns a representation of this in the format: + /// {Center:[] Radius:[]} + /// + /// A representation of this . + public override string ToString() + { + return ( + "{Center:" + Center.ToString() + + " Radius:" + Radius.ToString() + + "}" + ); + } + + /// + /// Compares whether two instances are equal. + /// + /// instance on the left of the equal sign. + /// instance on the right of the equal sign. + /// true if the instances are equal; false otherwise. + public static bool operator ==(BoundingSphere a, BoundingSphere b) + { + return a.Equals(b); + } + + /// + /// Compares whether two instances are not equal. + /// + /// instance on the left of the not equal sign. + /// instance on the right of the not equal sign. + /// true if the instances are not equal; false otherwise. + public static bool operator !=(BoundingSphere a, BoundingSphere b) + { + return !a.Equals(b); + } + + #endregion + } +} diff --git a/src/Math/ContainmentType.cs b/src/Math/ContainmentType.cs new file mode 100644 index 0000000..081a454 --- /dev/null +++ b/src/Math/ContainmentType.cs @@ -0,0 +1,37 @@ +#region License + +/* MoonWorks - Game Development Framework + * Copyright 2021 Evan Hemsley + */ + +/* Derived from code by Ethan Lee (Copyright 2009-2021). + * Released under the Microsoft Public License. + * See fna.LICENSE for details. + + * Derived from code by the Mono.Xna Team (Copyright 2006). + * Released under the MIT License. See monoxna.LICENSE for details. + */ + +#endregion + +namespace MoonWorks.Math +{ + /// + /// Defines how the bounding volumes intersects or contain one another. + /// + public enum ContainmentType + { + /// + /// Indicates that there is no overlap between two bounding volumes. + /// + Disjoint, + /// + /// Indicates that one bounding volume completely contains another volume. + /// + Contains, + /// + /// Indicates that bounding volumes partially overlap one another. + /// + Intersects + } +} diff --git a/src/Math/MathHelper.cs b/src/Math/MathHelper.cs new file mode 100644 index 0000000..a1414d0 --- /dev/null +++ b/src/Math/MathHelper.cs @@ -0,0 +1,415 @@ +#region License + +/* MoonWorks - Game Development Framework + * Copyright 2021 Evan Hemsley + */ + +/* Derived from code by Ethan Lee (Copyright 2009-2021). + * Released under the Microsoft Public License. + * See fna.LICENSE for details. + + * Derived from code by the Mono.Xna Team (Copyright 2006). + * Released under the MIT License. See monoxna.LICENSE for details. + */ + +#endregion + +namespace MoonWorks.Math +{ + /// + /// Contains commonly used precalculated values and mathematical operations. + /// + public static class MathHelper + { + #region Public Constants + + /// + /// Represents the mathematical constant e(2.71828175). + /// + public const float E = (float) System.Math.E; + + /// + /// Represents the log base ten of e(0.4342945). + /// + public const float Log10E = 0.4342945f; + + /// + /// Represents the log base two of e(1.442695). + /// + public const float Log2E = 1.442695f; + + /// + /// Represents the value of pi(3.14159274). + /// + public const float Pi = (float) System.Math.PI; + + /// + /// Represents the value of pi divided by two(1.57079637). + /// + public const float PiOver2 = (float) (System.Math.PI / 2.0); + + /// + /// Represents the value of pi divided by four(0.7853982). + /// + public const float PiOver4 = (float) (System.Math.PI / 4.0); + + /// + /// Represents the value of pi times two(6.28318548). + /// + public const float TwoPi = (float) (System.Math.PI * 2.0); + + #endregion + + #region Internal Static Readonly Fields + + internal static readonly float MachineEpsilonFloat = GetMachineEpsilonFloat(); + + #endregion + + #region Public Static Methods + + /// + /// Returns the Cartesian coordinate for one axis of a point that is defined by a + /// given triangle and two normalized barycentric (areal) coordinates. + /// + /// + /// The coordinate on one axis of vertex 1 of the defining triangle. + /// + /// + /// The coordinate on the same axis of vertex 2 of the defining triangle. + /// + /// + /// The coordinate on the same axis of vertex 3 of the defining triangle. + /// + /// + /// The normalized barycentric (areal) coordinate b2, equal to the weighting factor + /// for vertex 2, the coordinate of which is specified in value2. + /// + /// + /// The normalized barycentric (areal) coordinate b3, equal to the weighting factor + /// for vertex 3, the coordinate of which is specified in value3. + /// + /// + /// Cartesian coordinate of the specified point with respect to the axis being used. + /// + public static float Barycentric( + float value1, + float value2, + float value3, + float amount1, + float amount2 + ) { + return value1 + (value2 - value1) * amount1 + (value3 - value1) * amount2; + } + + /// + /// Performs a Catmull-Rom interpolation using the specified positions. + /// + /// The first position in the interpolation. + /// The second position in the interpolation. + /// The third position in the interpolation. + /// The fourth position in the interpolation. + /// Weighting factor. + /// A position that is the result of the Catmull-Rom interpolation. + public static float CatmullRom( + float value1, + float value2, + float value3, + float value4, + float amount + ) { + /* Using formula from http://www.mvps.org/directx/articles/catmull/ + * Internally using doubles not to lose precision. + */ + double amountSquared = amount * amount; + double amountCubed = amountSquared * amount; + return (float) ( + 0.5 * + ( + ((2.0 * value2 + (value3 - value1) * amount) + + ((2.0 * value1 - 5.0 * value2 + 4.0 * value3 - value4) * amountSquared) + + (3.0 * value2 - value1 - 3.0 * value3 + value4) * amountCubed) + ) + ); + } + + /// + /// Restricts a value to be within a specified range. + /// + /// The value to clamp. + /// + /// The minimum value. If value is less than min, min + /// will be returned. + /// + /// + /// The maximum value. If value is greater than max, max + /// will be returned. + /// + /// The clamped value. + public static float Clamp(float value, float min, float max) + { + // First we check to see if we're greater than the max. + value = (value > max) ? max : value; + + // Then we check to see if we're less than the min. + value = (value < min) ? min : value; + + // There's no check to see if min > max. + return value; + } + + /// + /// Calculates the absolute value of the difference of two values. + /// + /// Source value. + /// Source value. + /// Distance between the two values. + public static float Distance(float value1, float value2) + { + return System.Math.Abs(value1 - value2); + } + + /// + /// Performs a Hermite spline interpolation. + /// + /// Source position. + /// Source tangent. + /// Source position. + /// Source tangent. + /// Weighting factor. + /// The result of the Hermite spline interpolation. + public static float Hermite( + float value1, + float tangent1, + float value2, + float tangent2, + float amount + ) { + /* All transformed to double not to lose precision + * Otherwise, for high numbers of param:amount the result is NaN instead + * of Infinity. + */ + double v1 = value1, v2 = value2, t1 = tangent1, t2 = tangent2, s = amount; + double result; + double sCubed = s * s * s; + double sSquared = s * s; + + if (WithinEpsilon(amount, 0f)) + { + result = value1; + } + else if (WithinEpsilon(amount, 1f)) + { + result = value2; + } + else + { + result = ( + ((2 * v1 - 2 * v2 + t2 + t1) * sCubed) + + ((3 * v2 - 3 * v1 - 2 * t1 - t2) * sSquared) + + (t1 * s) + + v1 + ); + } + + return (float) result; + } + + + /// + /// Linearly interpolates between two values. + /// + /// Source value. + /// Source value. + /// + /// Value between 0 and 1 indicating the weight of value2. + /// + /// Interpolated value. + /// + /// This method performs the linear interpolation based on the following formula. + /// value1 + (value2 - value1) * amount + /// Passing amount a value of 0 will cause value1 to be returned, a value of 1 will + /// cause value2 to be returned. + /// + public static float Lerp(float value1, float value2, float amount) + { + return value1 + (value2 - value1) * amount; + } + + /// + /// Returns the greater of two values. + /// + /// Source value. + /// Source value. + /// The greater value. + public static float Max(float value1, float value2) + { + return value1 > value2 ? value1 : value2; + } + + /// + /// Returns the lesser of two values. + /// + /// Source value. + /// Source value. + /// The lesser value. + public static float Min(float value1, float value2) + { + return value1 < value2 ? value1 : value2; + } + + /// + /// Interpolates between two values using a cubic equation. + /// + /// Source value. + /// Source value. + /// Weighting value. + /// Interpolated value. + public static float SmoothStep(float value1, float value2, float amount) + { + /* It is expected that 0 < amount < 1. + * If amount < 0, return value1. + * If amount > 1, return value2. + */ + float result = MathHelper.Clamp(amount, 0f, 1f); + result = MathHelper.Hermite(value1, 0f, value2, 0f, result); + + return result; + } + + /// + /// Converts radians to degrees. + /// + /// The angle in radians. + /// The angle in degrees. + /// + /// This method uses double precision internally, though it returns single float. + /// Factor = 180 / pi + /// + public static float ToDegrees(float radians) + { + return (float) (radians * 57.295779513082320876798154814105); + } + + /// + /// Converts degrees to radians. + /// + /// The angle in degrees. + /// The angle in radians. + /// + /// This method uses double precision internally, though it returns single float. + /// Factor = pi / 180 + /// + public static float ToRadians(float degrees) + { + return (float) (degrees * 0.017453292519943295769236907684886); + } + + /// + /// Reduces a given angle to a value between pi and -pi. + /// + /// The angle to reduce, in radians. + /// The new angle, in radians. + public static float WrapAngle(float angle) + { + if ((angle > -Pi) && (angle <= Pi)) + { + return angle; + } + angle %= TwoPi; + if (angle <= -Pi) + { + return angle + TwoPi; + } + if (angle > Pi) + { + return angle - TwoPi; + } + return angle; + } + + #endregion + + #region Internal Static Methods + + // FIXME: This could be an extension! ClampIntEXT? -flibit + /// + /// Restricts a value to be within a specified range. + /// + /// The value to clamp. + /// + /// The minimum value. If value is less than min, min + /// will be returned. + /// + /// + /// The maximum value. If value is greater than max, max + /// will be returned. + /// + /// The clamped value. + internal static int Clamp(int value, int min, int max) + { + value = (value > max) ? max : value; + value = (value < min) ? min : value; + return value; + } + + internal static bool WithinEpsilon(float floatA, float floatB) + { + return System.Math.Abs(floatA - floatB) < MachineEpsilonFloat; + } + + internal static int ClosestMSAAPower(int value) + { + /* Checking for the highest power of two _after_ than the given int: + * http://graphics.stanford.edu/~seander/bithacks.html#RoundUpPowerOf2 + * Take result, divide by 2, get the highest power of two _before_! + * -flibit + */ + if (value == 1) + { + // ... Except for 1, which is invalid for MSAA -flibit + return 0; + } + int result = value - 1; + result |= result >> 1; + result |= result >> 2; + result |= result >> 4; + result |= result >> 8; + result |= result >> 16; + result += 1; + if (result == value) + { + return result; + } + return result >> 1; + } + + #endregion + + #region Private Static Methods + + /// + /// Find the current machine's Epsilon for the float data type. + /// (That is, the largest float, e, where e == 0.0f is true.) + /// + private static float GetMachineEpsilonFloat() + { + float machineEpsilon = 1.0f; + float comparison; + + /* Keep halving the working value of machineEpsilon until we get a number that + * when added to 1.0f will still evaluate as equal to 1.0f. + */ + do + { + machineEpsilon *= 0.5f; + comparison = 1.0f + machineEpsilon; + } + while (comparison > 1.0f); + + return machineEpsilon; + } + + #endregion + } +} diff --git a/src/Math/Matrix.cs b/src/Math/Matrix.cs new file mode 100644 index 0000000..29aca4a --- /dev/null +++ b/src/Math/Matrix.cs @@ -0,0 +1,2661 @@ +#region License + +/* MoonWorks - Game Development Framework + * Copyright 2021 Evan Hemsley + */ + +/* Derived from code by Ethan Lee (Copyright 2009-2021). + * Released under the Microsoft Public License. + * See fna.LICENSE for details. + + * Derived from code by the Mono.Xna Team (Copyright 2006). + * Released under the MIT License. See monoxna.LICENSE for details. + */ + +#endregion + +#region Using Statements +using System; +using System.Diagnostics; + +#endregion + +namespace MoonWorks.Math +{ + /// + /// Represents the right-handed 4x4 floating point matrix, which can store translation, scale and rotation information. + /// + [Serializable] + [DebuggerDisplay("{DebugDisplayString,nq}")] + public struct Matrix : IEquatable + { + #region Public Properties + + /// + /// The backward vector formed from the third row M31, M32, M33 elements. + /// + public Vector3 Backward + { + get + { + return new Vector3(M31, M32, M33); + } + set + { + M31 = value.X; + M32 = value.Y; + M33 = value.Z; + } + } + + /// + /// The down vector formed from the second row -M21, -M22, -M23 elements. + /// + public Vector3 Down + { + get + { + return new Vector3(-M21, -M22, -M23); + } + set + { + M21 = -value.X; + M22 = -value.Y; + M23 = -value.Z; + } + } + + /// + /// The forward vector formed from the third row -M31, -M32, -M33 elements. + /// + public Vector3 Forward + { + get + { + return new Vector3(-M31, -M32, -M33); + } + set + { + M31 = -value.X; + M32 = -value.Y; + 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(-M11, -M12, -M13); + } + set + { + M11 = -value.X; + M12 = -value.Y; + M13 = -value.Z; + } + } + + /// + /// The right vector formed from the first row M11, M12, M13 elements. + /// + public Vector3 Right + { + get + { + return new Vector3(M11, M12, M13); + } + set + { + M11 = value.X; + M12 = value.Y; + M13 = value.Z; + } + } + + /// + /// Position stored in this matrix. + /// + public Vector3 Translation + { + get + { + return new Vector3(M41, M42, M43); + } + set + { + M41 = value.X; + M42 = value.Y; + M43 = value.Z; + } + } + + /// + /// The upper vector formed from the second row M21, M22, M23 elements. + /// + public Vector3 Up + { + get + { + return new Vector3(M21, M22, M23); + } + set + { + M21 = value.X; + M22 = value.Y; + M23 = value.Z; + } + } + + #endregion + + #region Internal Properties + + internal string DebugDisplayString + { + get + { + return string.Concat( + "( ", M11.ToString(), " ", + M12.ToString(), " ", + M13.ToString(), " ", + M14.ToString(), " ) \r\n", + "( ", M21.ToString(), " ", + M22.ToString(), " ", + M23.ToString(), " ", + M24.ToString(), " ) \r\n", + "( ", M31.ToString(), " ", + M32.ToString(), " ", + M33.ToString(), " ", + M34.ToString(), " ) \r\n", + "( ", M41.ToString(), " ", + M42.ToString(), " ", + M43.ToString(), " ", + M44.ToString(), " )" + ); + } + } + + #endregion + + #region Public Fields + + /// + /// A first row and first column value. + /// + public float M11; + + /// + /// A first row and second column value. + /// + public float M12; + + /// + /// A first row and third column value. + /// + public float M13; + + /// + /// A first row and fourth column value. + /// + public float M14; + + /// + /// A second row and first column value. + /// + public float M21; + + /// + /// A second row and second column value. + /// + public float M22; + + /// + /// A second row and third column value. + /// + public float M23; + + /// + /// A second row and fourth column value. + /// + public float M24; + + /// + /// A third row and first column value. + /// + public float M31; + + /// + /// A third row and second column value. + /// + public float M32; + + /// + /// A third row and third column value. + /// + public float M33; + + /// + /// A third row and fourth column value. + /// + public float M34; + + /// + /// A fourth row and first column value. + /// + public float M41; + + /// + /// A fourth row and second column value. + /// + public float M42; + + /// + /// A fourth row and third column value. + /// + public float M43; + + /// + /// A fourth row and fourth column value. + /// + public float M44; + + #endregion + + #region Private Static Variables + + 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 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( + float m11, float m12, float m13, float m14, + float m21, float m22, float m23, float m24, + float m31, float m32, float m33, float m34, + float m41, float m42, float m43, float m44 + ) { + 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; + } + + #endregion + + #region Public Methods + + /// + /// Decomposes this matrix to translation, rotation and scale elements. Returns true if matrix can be decomposed; false otherwise. + /// + /// Scale vector as an output parameter. + /// Rotation quaternion as an output parameter. + /// Translation vector as an output parameter. + /// true if matrix can be decomposed; false otherwise. + public bool Decompose( + out Vector3 scale, + out Quaternion rotation, + out Vector3 translation + ) { + translation.X = M41; + translation.Y = M42; + translation.Z = M43; + + float xs = (System.Math.Sign(M11 * M12 * M13 * M14) < 0) ? -1 : 1; + float ys = (System.Math.Sign(M21 * M22 * M23 * M24) < 0) ? -1 : 1; + float zs = (System.Math.Sign(M31 * M32 * M33 * M34) < 0) ? -1 : 1; + + scale.X = xs * (float) System.Math.Sqrt(M11 * M11 + M12 * M12 + M13 * M13); + scale.Y = ys * (float) System.Math.Sqrt(M21 * M21 + M22 * M22 + M23 * M23); + scale.Z = zs * (float) System.Math.Sqrt(M31 * M31 + M32 * M32 + M33 * M33); + + if ( MathHelper.WithinEpsilon(scale.X, 0.0f) || + MathHelper.WithinEpsilon(scale.Y, 0.0f) || + MathHelper.WithinEpsilon(scale.Z, 0.0f) ) + { + rotation = Quaternion.Identity; + return false; + } + + Matrix m1 = new Matrix( + M11 / scale.X, M12 / scale.X, M13 / scale.X, 0, + M21 / scale.Y, M22 / scale.Y, M23 / scale.Y, 0, + M31 / scale.Z, M32 / scale.Z, M33 / scale.Z, 0, + 0, 0, 0, 1 + ); + + rotation = Quaternion.CreateFromRotationMatrix(m1); + return true; + } + + /// + /// Returns a determinant of this . + /// + /// Determinant of this + /// See more about determinant here - http://en.wikipedia.org/wiki/Determinant. + /// + public float Determinant() + { + float num18 = (M33 * M44) - (M34 * M43); + float num17 = (M32 * M44) - (M34 * M42); + float num16 = (M32 * M43) - (M33 * M42); + float num15 = (M31 * M44) - (M34 * M41); + float num14 = (M31 * M43) - (M33 * M41); + float num13 = (M31 * M42) - (M32 * M41); + return ( + ( + ( + (M11 * (((M22 * num18) - (M23 * num17)) + (M24 * num16))) - + (M12 * (((M21 * num18) - (M23 * num15)) + (M24 * num14))) + ) + (M13 * (((M21 * num17) - (M22 * num15)) + (M24 * num13))) + ) - (M14 * (((M21 * num16) - (M22 * num14)) + (M23 * num13))) + ); + } + + /// + /// 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 ( M11 == other.M11 && + M12 == other.M12 && + M13 == other.M13 && + M14 == other.M14 && + M21 == other.M21 && + M22 == other.M22 && + M23 == other.M23 && + M24 == other.M24 && + M31 == other.M31 && + M32 == other.M32 && + M33 == other.M33 && + M34 == other.M34 && + M41 == other.M41 && + M42 == other.M42 && + M43 == other.M43 && + M44 == other.M44 ); + } + + /// + /// 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) + { + return (obj is Matrix) && Equals((Matrix) obj); + } + + /// + /// Gets the hash code of this . + /// + /// Hash code of this . + public override int GetHashCode() + { + return ( + M11.GetHashCode() + M12.GetHashCode() + M13.GetHashCode() + M14.GetHashCode() + + M21.GetHashCode() + M22.GetHashCode() + M23.GetHashCode() + M24.GetHashCode() + + M31.GetHashCode() + M32.GetHashCode() + M33.GetHashCode() + M34.GetHashCode() + + M41.GetHashCode() + M42.GetHashCode() + M43.GetHashCode() + M44.GetHashCode() + ); + } + + /// + /// 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.ToString() + + " M12:" + M12.ToString() + + " M13:" + M13.ToString() + + " M14:" + M14.ToString() + + "} {M21:" + M21.ToString() + + " M22:" + M22.ToString() + + " M23:" + M23.ToString() + + " M24:" + M24.ToString() + + "} {M31:" + M31.ToString() + + " M32:" + M32.ToString() + + " M33:" + M33.ToString() + + " M34:" + M34.ToString() + + "} {M41:" + M41.ToString() + + " M42:" + M42.ToString() + + " M43:" + M43.ToString() + + " M44:" + M44.ToString() + "}" + ); + } + + #endregion + + #region Public Static 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 for spherical billboarding that rotates around specified object position. + /// + /// Position of billboard object. It will rotate around that vector. + /// The camera position. + /// The camera up vector. + /// Optional camera forward vector. + /// The for spherical billboarding. + public static Matrix CreateBillboard( + Vector3 objectPosition, + Vector3 cameraPosition, + Vector3 cameraUpVector, + Nullable cameraForwardVector + ) { + Matrix result; + + // Delegate to the other overload of the function to do the work + CreateBillboard( + ref objectPosition, + ref cameraPosition, + ref cameraUpVector, + cameraForwardVector, + out result + ); + + return result; + } + + /// + /// Creates a new for spherical billboarding that rotates around specified object position. + /// + /// Position of billboard object. It will rotate around that vector. + /// The camera position. + /// The camera up vector. + /// Optional camera forward vector. + /// The for spherical billboarding as an output parameter. + public static void CreateBillboard( + ref Vector3 objectPosition, + ref Vector3 cameraPosition, + ref Vector3 cameraUpVector, + Vector3? cameraForwardVector, + out Matrix result + ) { + Vector3 vector; + Vector3 vector2; + Vector3 vector3; + vector.X = objectPosition.X - cameraPosition.X; + vector.Y = objectPosition.Y - cameraPosition.Y; + vector.Z = objectPosition.Z - cameraPosition.Z; + float num = vector.LengthSquared(); + if (num < 0.0001f) + { + vector = cameraForwardVector.HasValue ? + -cameraForwardVector.Value : + Vector3.Forward; + } + else + { + Vector3.Multiply( + ref vector, + (float) (1f / ((float) System.Math.Sqrt((double) num))), + out vector + ); + } + Vector3.Cross(ref cameraUpVector, ref vector, out vector3); + vector3.Normalize(); + Vector3.Cross(ref vector, ref vector3, out vector2); + result.M11 = vector3.X; + result.M12 = vector3.Y; + result.M13 = vector3.Z; + result.M14 = 0; + result.M21 = vector2.X; + result.M22 = vector2.Y; + result.M23 = vector2.Z; + result.M24 = 0; + result.M31 = vector.X; + result.M32 = vector.Y; + result.M33 = vector.Z; + result.M34 = 0; + result.M41 = objectPosition.X; + result.M42 = objectPosition.Y; + result.M43 = objectPosition.Z; + result.M44 = 1; + } + + /// + /// Creates a new for cylindrical billboarding that rotates around specified axis. + /// + /// Object position the billboard will rotate around. + /// Camera position. + /// Axis of billboard for rotation. + /// Optional camera forward vector. + /// Optional object forward vector. + /// The for cylindrical billboarding. + public static Matrix CreateConstrainedBillboard( + Vector3 objectPosition, + Vector3 cameraPosition, + Vector3 rotateAxis, + Nullable cameraForwardVector, + Nullable objectForwardVector + ) { + Matrix result; + CreateConstrainedBillboard( + ref objectPosition, + ref cameraPosition, + ref rotateAxis, + cameraForwardVector, + objectForwardVector, + out result + ); + return result; + } + + /// + /// Creates a new for cylindrical billboarding that rotates around specified axis. + /// + /// Object position the billboard will rotate around. + /// Camera position. + /// Axis of billboard for rotation. + /// Optional camera forward vector. + /// Optional object forward vector. + /// The for cylindrical billboarding as an output parameter. + public static void CreateConstrainedBillboard( + ref Vector3 objectPosition, + ref Vector3 cameraPosition, + ref Vector3 rotateAxis, + Vector3? cameraForwardVector, + Vector3? objectForwardVector, + out Matrix result + ) { + float num; + Vector3 vector; + Vector3 vector2; + Vector3 vector3; + vector2.X = objectPosition.X - cameraPosition.X; + vector2.Y = objectPosition.Y - cameraPosition.Y; + vector2.Z = objectPosition.Z - cameraPosition.Z; + float num2 = vector2.LengthSquared(); + if (num2 < 0.0001f) + { + vector2 = cameraForwardVector.HasValue ? + -cameraForwardVector.Value : + Vector3.Forward; + } + else + { + Vector3.Multiply( + ref vector2, + (float) (1f / ((float) System.Math.Sqrt((double) num2))), + out vector2 + ); + } + Vector3 vector4 = rotateAxis; + Vector3.Dot(ref rotateAxis, ref vector2, out num); + if (System.Math.Abs(num) > 0.9982547f) + { + if (objectForwardVector.HasValue) + { + vector = objectForwardVector.Value; + Vector3.Dot(ref rotateAxis, ref vector, out num); + if (System.Math.Abs(num) > 0.9982547f) + { + num = ( + (rotateAxis.X * Vector3.Forward.X) + + (rotateAxis.Y * Vector3.Forward.Y) + ) + (rotateAxis.Z * Vector3.Forward.Z); + vector = (System.Math.Abs(num) > 0.9982547f) ? + Vector3.Right : + Vector3.Forward; + } + } + else + { + num = ( + (rotateAxis.X * Vector3.Forward.X) + + (rotateAxis.Y * Vector3.Forward.Y) + ) + (rotateAxis.Z * Vector3.Forward.Z); + vector = (System.Math.Abs(num) > 0.9982547f) ? + Vector3.Right : + Vector3.Forward; + } + Vector3.Cross(ref rotateAxis, ref vector, out vector3); + vector3.Normalize(); + Vector3.Cross(ref vector3, ref rotateAxis, out vector); + vector.Normalize(); + } + else + { + Vector3.Cross(ref rotateAxis, ref vector2, out vector3); + vector3.Normalize(); + Vector3.Cross(ref vector3, ref vector4, out vector); + vector.Normalize(); + } + + result.M11 = vector3.X; + result.M12 = vector3.Y; + result.M13 = vector3.Z; + result.M14 = 0; + result.M21 = vector4.X; + result.M22 = vector4.Y; + result.M23 = vector4.Z; + result.M24 = 0; + result.M31 = vector.X; + result.M32 = vector.Y; + result.M33 = vector.Z; + result.M34 = 0; + result.M41 = objectPosition.X; + result.M42 = objectPosition.Y; + result.M43 = objectPosition.Z; + result.M44 = 1; + } + + /// + /// Creates a new which contains the rotation moment around specified axis. + /// + /// The axis of rotation. + /// The angle of rotation in radians. + /// The rotation . + public static Matrix CreateFromAxisAngle(Vector3 axis, float angle) + { + Matrix result; + CreateFromAxisAngle(ref axis, angle, out result); + return result; + } + + /// + /// Creates a new which contains the rotation moment around specified axis. + /// + /// The axis of rotation. + /// The angle of rotation in radians. + /// The rotation as an output parameter. + public static void CreateFromAxisAngle( + ref Vector3 axis, + float angle, + out Matrix result + ) { + float x = axis.X; + float y = axis.Y; + float z = axis.Z; + float num2 = (float) System.Math.Sin((double) angle); + float num = (float) System.Math.Cos((double) angle); + float num11 = x * x; + float num10 = y * y; + float num9 = z * z; + float num8 = x * y; + float num7 = x * z; + float num6 = y * z; + result.M11 = num11 + (num * (1f - num11)); + result.M12 = (num8 - (num * num8)) + (num2 * z); + result.M13 = (num7 - (num * num7)) - (num2 * y); + result.M14 = 0; + result.M21 = (num8 - (num * num8)) - (num2 * z); + result.M22 = num10 + (num * (1f - num10)); + result.M23 = (num6 - (num * num6)) + (num2 * x); + result.M24 = 0; + result.M31 = (num7 - (num * num7)) + (num2 * y); + result.M32 = (num6 - (num * num6)) - (num2 * x); + result.M33 = num9 + (num * (1f - num9)); + result.M34 = 0; + result.M41 = 0; + result.M42 = 0; + result.M43 = 0; + result.M44 = 1; + } + + /// + /// Creates a new rotation from a . + /// + /// of rotation moment. + /// The rotation . + public static Matrix CreateFromQuaternion(Quaternion quaternion) + { + Matrix result; + CreateFromQuaternion(ref quaternion, out result); + return result; + } + + /// + /// Creates a new rotation from a . + /// + /// of rotation moment. + /// The rotation as an output parameter. + public static void CreateFromQuaternion(ref Quaternion quaternion, out Matrix result) + { + float num9 = quaternion.X * quaternion.X; + float num8 = quaternion.Y * quaternion.Y; + float num7 = quaternion.Z * quaternion.Z; + float num6 = quaternion.X * quaternion.Y; + float num5 = quaternion.Z * quaternion.W; + float num4 = quaternion.Z * quaternion.X; + float num3 = quaternion.Y * quaternion.W; + float num2 = quaternion.Y * quaternion.Z; + float num = quaternion.X * quaternion.W; + result.M11 = 1f - (2f * (num8 + num7)); + result.M12 = 2f * (num6 + num5); + result.M13 = 2f * (num4 - num3); + result.M14 = 0f; + result.M21 = 2f * (num6 - num5); + result.M22 = 1f - (2f * (num7 + num9)); + result.M23 = 2f * (num2 + num); + result.M24 = 0f; + result.M31 = 2f * (num4 + num3); + result.M32 = 2f * (num2 - num); + result.M33 = 1f - (2f * (num8 + num9)); + result.M34 = 0f; + result.M41 = 0f; + result.M42 = 0f; + result.M43 = 0f; + result.M44 = 1f; + } + + /// + /// Creates a new rotation from the specified yaw, pitch and roll values. + /// + /// The yaw rotation value in radians. + /// The pitch rotation value in radians. + /// The roll rotation value in radians. + /// The rotation . + /// For more information about yaw, pitch and roll visit http://en.wikipedia.org/wiki/Euler_angles. + /// + public static Matrix CreateFromYawPitchRoll(float yaw, float pitch, float roll) + { + Matrix matrix; + CreateFromYawPitchRoll(yaw, pitch, roll, out matrix); + return matrix; + } + + /// + /// Creates a new rotation from the specified yaw, pitch and roll values. + /// + /// The yaw rotation value in radians. + /// The pitch rotation value in radians. + /// The roll rotation value in radians. + /// The rotation as an output parameter. + /// For more information about yaw, pitch and roll visit http://en.wikipedia.org/wiki/Euler_angles. + /// + public static void CreateFromYawPitchRoll( + float yaw, + float pitch, + float roll, + out Matrix result + ) { + Quaternion quaternion; + Quaternion.CreateFromYawPitchRoll(yaw, pitch, roll, out quaternion); + CreateFromQuaternion(ref quaternion, out result); + } + + /// + /// Creates a new viewing . + /// + /// Position of the camera. + /// Lookup vector of the camera. + /// The direction of the upper edge of the camera. + /// The viewing . + public static Matrix CreateLookAt( + Vector3 cameraPosition, + Vector3 cameraTarget, + Vector3 cameraUpVector + ) { + Matrix matrix; + CreateLookAt(ref cameraPosition, ref cameraTarget, ref cameraUpVector, out matrix); + return matrix; + } + + /// + /// Creates a new viewing . + /// + /// Position of the camera. + /// Lookup vector of the camera. + /// The direction of the upper edge of the camera. + /// The viewing as an output parameter. + public static void CreateLookAt( + ref Vector3 cameraPosition, + ref Vector3 cameraTarget, + ref Vector3 cameraUpVector, + out Matrix result + ) { + Vector3 vectorA = Vector3.Normalize(cameraPosition - cameraTarget); + Vector3 vectorB = Vector3.Normalize(Vector3.Cross(cameraUpVector, vectorA)); + Vector3 vectorC = Vector3.Cross(vectorA, vectorB); + result.M11 = vectorB.X; + result.M12 = vectorC.X; + result.M13 = vectorA.X; + result.M14 = 0f; + result.M21 = vectorB.Y; + result.M22 = vectorC.Y; + result.M23 = vectorA.Y; + result.M24 = 0f; + result.M31 = vectorB.Z; + result.M32 = vectorC.Z; + result.M33 = vectorA.Z; + result.M34 = 0f; + result.M41 = -Vector3.Dot(vectorB, cameraPosition); + result.M42 = -Vector3.Dot(vectorC, cameraPosition); + result.M43 = -Vector3.Dot(vectorA, cameraPosition); + result.M44 = 1f; + } + + /// + /// Creates a new projection for orthographic view. + /// + /// Width of the viewing volume. + /// Height of the viewing volume. + /// Depth of the near plane. + /// Depth of the far plane. + /// The new projection for orthographic view. + public static Matrix CreateOrthographic( + float width, + float height, + float zNearPlane, + float zFarPlane + ) { + Matrix matrix; + CreateOrthographic(width, height, zNearPlane, zFarPlane, out matrix); + return matrix; + } + + /// + /// Creates a new projection for orthographic view. + /// + /// Width of the viewing volume. + /// Height of the viewing volume. + /// Depth of the near plane. + /// Depth of the far plane. + /// The new projection for orthographic view as an output parameter. + public static void CreateOrthographic( + float width, + float height, + float zNearPlane, + float zFarPlane, + out Matrix result + ) { + result.M11 = 2f / width; + result.M12 = result.M13 = result.M14 = 0f; + result.M22 = 2f / height; + result.M21 = result.M23 = result.M24 = 0f; + result.M33 = 1f / (zNearPlane - zFarPlane); + result.M31 = result.M32 = result.M34 = 0f; + result.M41 = result.M42 = 0f; + result.M43 = zNearPlane / (zNearPlane - zFarPlane); + result.M44 = 1f; + } + + /// + /// Creates a new projection for customized orthographic view. + /// + /// Lower x-value at the near plane. + /// Upper x-value at the near plane. + /// Lower y-coordinate at the near plane. + /// Upper y-value at the near plane. + /// Depth of the near plane. + /// Depth of the far plane. + /// The new projection for customized orthographic view. + public static Matrix CreateOrthographicOffCenter( + float left, + float right, + float bottom, + float top, + float zNearPlane, + float zFarPlane + ) { + Matrix matrix; + CreateOrthographicOffCenter( + left, + right, + bottom, + top, + zNearPlane, + zFarPlane, + out matrix + ); + return matrix; + } + + /// + /// Creates a new projection for customized orthographic view. + /// + /// Lower x-value at the near plane. + /// Upper x-value at the near plane. + /// Lower y-coordinate at the near plane. + /// Upper y-value at the near plane. + /// Depth of the near plane. + /// Depth of the far plane. + /// The new projection for customized orthographic view as an output parameter. + public static void CreateOrthographicOffCenter( + float left, + float right, + float bottom, + float top, + float zNearPlane, + float zFarPlane, + out Matrix result + ) { + result.M11 = (float) (2.0 / ((double) right - (double) left)); + result.M12 = 0.0f; + result.M13 = 0.0f; + result.M14 = 0.0f; + result.M21 = 0.0f; + result.M22 = (float) (2.0 / ((double) top - (double) bottom)); + result.M23 = 0.0f; + result.M24 = 0.0f; + result.M31 = 0.0f; + result.M32 = 0.0f; + result.M33 = (float) (1.0 / ((double) zNearPlane - (double) zFarPlane)); + result.M34 = 0.0f; + result.M41 = (float) ( + ((double) left + (double) right) / + ((double) left - (double) right) + ); + result.M42 = (float) ( + ((double) top + (double) bottom) / + ((double) bottom - (double) top) + ); + result.M43 = (float) ( + (double) zNearPlane / + ((double) zNearPlane - (double) zFarPlane) + ); + result.M44 = 1.0f; + } + + /// + /// Creates a new projection for perspective view. + /// + /// Width of the viewing volume. + /// Height of the viewing volume. + /// Distance to the near plane. + /// Distance to the far plane. + /// The new projection for perspective view. + public static Matrix CreatePerspective( + float width, + float height, + float nearPlaneDistance, + float farPlaneDistance + ) { + Matrix matrix; + CreatePerspective(width, height, nearPlaneDistance, farPlaneDistance, out matrix); + return matrix; + } + + /// + /// Creates a new projection for perspective view. + /// + /// Width of the viewing volume. + /// Height of the viewing volume. + /// Distance to the near plane. + /// Distance to the far plane. + /// The new projection for perspective view as an output parameter. + public static void CreatePerspective( + float width, + float height, + float nearPlaneDistance, + float farPlaneDistance, + out Matrix result + ) { + if (nearPlaneDistance <= 0f) + { + throw new ArgumentException("nearPlaneDistance <= 0"); + } + if (farPlaneDistance <= 0f) + { + throw new ArgumentException("farPlaneDistance <= 0"); + } + if (nearPlaneDistance >= farPlaneDistance) + { + throw new ArgumentException("nearPlaneDistance >= farPlaneDistance"); + } + result.M11 = (2f * nearPlaneDistance) / width; + result.M12 = result.M13 = result.M14 = 0f; + result.M22 = (2f * nearPlaneDistance) / height; + result.M21 = result.M23 = result.M24 = 0f; + result.M33 = farPlaneDistance / (nearPlaneDistance - farPlaneDistance); + result.M31 = result.M32 = 0f; + result.M34 = -1f; + result.M41 = result.M42 = result.M44 = 0f; + result.M43 = ( + (nearPlaneDistance * farPlaneDistance) / + (nearPlaneDistance - farPlaneDistance) + ); + } + + /// + /// Creates a new projection for perspective view with field of view. + /// + /// Field of view in the y direction in radians. + /// Width divided by height of the viewing volume. + /// Distance to the near plane. + /// Distance to the far plane. + /// The new projection for perspective view with FOV. + public static Matrix CreatePerspectiveFieldOfView( + float fieldOfView, + float aspectRatio, + float nearPlaneDistance, + float farPlaneDistance + ) { + Matrix result; + CreatePerspectiveFieldOfView( + fieldOfView, + aspectRatio, + nearPlaneDistance, + farPlaneDistance, + out result + ); + return result; + } + + /// + /// Creates a new projection for perspective view with field of view. + /// + /// Field of view in the y direction in radians. + /// Width divided by height of the viewing volume. + /// Distance of the near plane. + /// Distance of the far plane. + /// The new projection for perspective view with FOV as an output parameter. + public static void CreatePerspectiveFieldOfView( + float fieldOfView, + float aspectRatio, + float nearPlaneDistance, + float farPlaneDistance, + out Matrix result + ) { + if ((fieldOfView <= 0f) || (fieldOfView >= 3.141593f)) + { + throw new ArgumentException("fieldOfView <= 0 or >= PI"); + } + if (nearPlaneDistance <= 0f) + { + throw new ArgumentException("nearPlaneDistance <= 0"); + } + if (farPlaneDistance <= 0f) + { + throw new ArgumentException("farPlaneDistance <= 0"); + } + if (nearPlaneDistance >= farPlaneDistance) + { + throw new ArgumentException("nearPlaneDistance >= farPlaneDistance"); + } + float num = 1f / ((float) System.Math.Tan((double) (fieldOfView * 0.5f))); + float num9 = num / aspectRatio; + result.M11 = num9; + result.M12 = result.M13 = result.M14 = 0; + result.M22 = num; + result.M21 = result.M23 = result.M24 = 0; + result.M31 = result.M32 = 0f; + result.M33 = farPlaneDistance / (nearPlaneDistance - farPlaneDistance); + result.M34 = -1; + result.M41 = result.M42 = result.M44 = 0; + result.M43 = ( + (nearPlaneDistance * farPlaneDistance) / + (nearPlaneDistance - farPlaneDistance) + ); + } + + /// + /// Creates a new projection for customized perspective view. + /// + /// Lower x-value at the near plane. + /// Upper x-value at the near plane. + /// Lower y-coordinate at the near plane. + /// Upper y-value at the near plane. + /// Distance to the near plane. + /// Distance to the far plane. + /// The new for customized perspective view. + public static Matrix CreatePerspectiveOffCenter( + float left, + float right, + float bottom, + float top, + float nearPlaneDistance, + float farPlaneDistance + ) { + Matrix result; + CreatePerspectiveOffCenter( + left, + right, + bottom, + top, + nearPlaneDistance, + farPlaneDistance, + out result + ); + return result; + } + + /// + /// Creates a new projection for customized perspective view. + /// + /// Lower x-value at the near plane. + /// Upper x-value at the near plane. + /// Lower y-coordinate at the near plane. + /// Upper y-value at the near plane. + /// Distance to the near plane. + /// Distance to the far plane. + /// The new for customized perspective view as an output parameter. + public static void CreatePerspectiveOffCenter( + float left, + float right, + float bottom, + float top, + float nearPlaneDistance, + float farPlaneDistance, + out Matrix result + ) { + if (nearPlaneDistance <= 0f) + { + throw new ArgumentException("nearPlaneDistance <= 0"); + } + if (farPlaneDistance <= 0f) + { + throw new ArgumentException("farPlaneDistance <= 0"); + } + if (nearPlaneDistance >= farPlaneDistance) + { + throw new ArgumentException("nearPlaneDistance >= farPlaneDistance"); + } + result.M11 = (2f * nearPlaneDistance) / (right - left); + result.M12 = result.M13 = result.M14 = 0; + result.M22 = (2f * nearPlaneDistance) / (top - bottom); + result.M21 = result.M23 = result.M24 = 0; + result.M31 = (left + right) / (right - left); + result.M32 = (top + bottom) / (top - bottom); + result.M33 = farPlaneDistance / (nearPlaneDistance - farPlaneDistance); + result.M34 = -1; + result.M43 = ( + (nearPlaneDistance * farPlaneDistance) / + (nearPlaneDistance - farPlaneDistance) + ); + result.M41 = result.M42 = result.M44 = 0; + } + + /// + /// Creates a new rotation around X axis. + /// + /// Angle in radians. + /// The rotation around X axis. + public static Matrix CreateRotationX(float 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(float radians, out Matrix result) + { + result = Matrix.Identity; + + float val1 = (float) System.Math.Cos(radians); + float val2 = (float) System.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(float 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(float radians, out Matrix result) + { + result = Matrix.Identity; + + float val1 = (float) System.Math.Cos(radians); + float val2 = (float) System.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(float 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(float radians, out Matrix result) + { + result = Matrix.Identity; + + float val1 = (float) System.Math.Cos(radians); + float val2 = (float) System.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(float 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(float 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(float xScale, float yScale, float 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( + float xScale, + float yScale, + float 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 that flattens geometry into a specified as if casting a shadow from a specified light source. + /// + /// A vector specifying the direction from which the light that will cast the shadow is coming. + /// The plane onto which the new matrix should flatten geometry so as to cast a shadow. + /// A that can be used to flatten geometry onto the specified plane from the specified direction. + public static Matrix CreateShadow(Vector3 lightDirection, Plane plane) + { + Matrix result; + CreateShadow(ref lightDirection, ref plane, out result); + return result; + } + + /// + /// Creates a new that flattens geometry into a specified as if casting a shadow from a specified light source. + /// + /// A vector specifying the direction from which the light that will cast the shadow is coming. + /// The plane onto which the new matrix should flatten geometry so as to cast a shadow. + /// A that can be used to flatten geometry onto the specified plane from the specified direction as an output parameter. + public static void CreateShadow( + ref Vector3 lightDirection, + ref Plane plane, + out Matrix result) + { + float dot = ( + (plane.Normal.X * lightDirection.X) + + (plane.Normal.Y * lightDirection.Y) + + (plane.Normal.Z * lightDirection.Z) + ); + float x = -plane.Normal.X; + float y = -plane.Normal.Y; + float z = -plane.Normal.Z; + float d = -plane.D; + + result.M11 = (x * lightDirection.X) + dot; + result.M12 = x * lightDirection.Y; + result.M13 = x * lightDirection.Z; + result.M14 = 0; + result.M21 = y * lightDirection.X; + result.M22 = (y * lightDirection.Y) + dot; + result.M23 = y * lightDirection.Z; + result.M24 = 0; + result.M31 = z * lightDirection.X; + result.M32 = z * lightDirection.Y; + result.M33 = (z * lightDirection.Z) + dot; + result.M34 = 0; + result.M41 = d * lightDirection.X; + result.M42 = d * lightDirection.Y; + result.M43 = d * lightDirection.Z; + result.M44 = dot; + } + + /// + /// Creates a new translation . + /// + /// X coordinate of translation. + /// Y coordinate of translation. + /// Z coordinate of translation. + /// The translation . + public static Matrix CreateTranslation( + float xPosition, + float yPosition, + float 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( + float xPosition, + float yPosition, + float 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; + } + + /// + /// Creates a new reflection . + /// + /// The plane that used for reflection calculation. + /// The reflection . + public static Matrix CreateReflection(Plane value) + { + Matrix result; + CreateReflection(ref value, out result); + return result; + } + + /// + /// Creates a new reflection . + /// + /// The plane that used for reflection calculation. + /// The reflection as an output parameter. + public static void CreateReflection(ref Plane value, out Matrix result) + { + Plane plane; + Plane.Normalize(ref value, out plane); + float x = plane.Normal.X; + float y = plane.Normal.Y; + float z = plane.Normal.Z; + float num3 = -2f * x; + float num2 = -2f * y; + float num = -2f * z; + result.M11 = (num3 * x) + 1f; + result.M12 = num2 * x; + result.M13 = num * x; + result.M14 = 0; + result.M21 = num3 * y; + result.M22 = (num2 * y) + 1; + result.M23 = num * y; + result.M24 = 0; + result.M31 = num3 * z; + result.M32 = num2 * z; + result.M33 = (num * z) + 1; + result.M34 = 0; + result.M41 = num3 * plane.D; + result.M42 = num2 * plane.D; + result.M43 = num * plane.D; + result.M44 = 1; + } + + /// + /// Creates a new world . + /// + /// The position vector. + /// The forward direction vector. + /// The upward direction vector. Usually . + /// The world . + public static Matrix CreateWorld(Vector3 position, Vector3 forward, Vector3 up) + { + Matrix ret; + CreateWorld(ref position, ref forward, ref up, out ret); + return ret; + } + + /// + /// Creates a new world . + /// + /// The position vector. + /// The forward direction vector. + /// The upward direction vector. Usually . + /// The world as an output parameter. + public static void CreateWorld( + ref Vector3 position, + ref Vector3 forward, + ref Vector3 up, + out Matrix result + ) { + Vector3 x, y, z; + Vector3.Normalize(ref forward, out z); + Vector3.Cross(ref forward, ref up, out x); + Vector3.Cross(ref x, ref forward, out y); + x.Normalize(); + y.Normalize(); + + result = new Matrix(); + result.Right = x; + result.Up = y; + result.Forward = z; + result.Translation = position; + result.M44 = 1f; + } + + /// + /// 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, float divider) + { + float 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, float divider, out Matrix result) + { + float 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; + } + + /// + /// Creates a new which contains inversion of the specified matrix. + /// + /// Source . + /// The inverted matrix. + public static Matrix Invert(Matrix matrix) + { + Invert(ref matrix, out matrix); + return matrix; + } + + /// + /// 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) + { + /* + * 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. + */ + + float num1 = matrix.M11; + float num2 = matrix.M12; + float num3 = matrix.M13; + float num4 = matrix.M14; + float num5 = matrix.M21; + float num6 = matrix.M22; + float num7 = matrix.M23; + float num8 = matrix.M24; + float num9 = matrix.M31; + float num10 = matrix.M32; + float num11 = matrix.M33; + float num12 = matrix.M34; + float num13 = matrix.M41; + float num14 = matrix.M42; + float num15 = matrix.M43; + float num16 = matrix.M44; + float num17 = (float) ( + (double) num11 * (double) num16 - + (double) num12 * (double) num15 + ); + float num18 = (float) ( + (double) num10 * (double) num16 - + (double) num12 * (double) num14 + ); + float num19 = (float) ( + (double) num10 * (double) num15 - + (double) num11 * (double) num14 + ); + float num20 = (float) ( + (double) num9 * (double) num16 - + (double) num12 * (double) num13 + ); + float num21 = (float) ( + (double) num9 * (double) num15 - + (double) num11 * (double) num13 + ); + float num22 = (float) ( + (double) num9 * (double) num14 - + (double) num10 * (double) num13 + ); + float num23 = (float) ( + (double) num6 * (double) num17 - + (double) num7 * (double) num18 + + (double) num8 * (double) num19 + ); + float num24 = (float) -( + (double) num5 * (double) num17 - + (double) num7 * (double) num20 + + (double) num8 * (double) num21 + ); + float num25 = (float) ( + (double) num5 * (double) num18 - + (double) num6 * (double) num20 + + (double) num8 * (double) num22 + ); + float num26 = (float) -( + (double) num5 * (double) num19 - + (double) num6 * (double) num21 + + (double) num7 * (double) num22 + ); + float num27 = (float) ( + 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 = (float) ( + -( + (double) num2 * (double) num17 - + (double) num3 * (double) num18 + + (double) num4 * (double) num19 + ) * num27 + ); + result.M22 = (float) ( + ( + (double) num1 * (double) num17 - + (double) num3 * (double) num20 + + (double) num4 * (double) num21 + ) * num27 + ); + result.M32 = (float) ( + -( + (double) num1 * (double) num18 - + (double) num2 * (double) num20 + + (double) num4 * (double) num22 + ) * num27 + ); + result.M42 = (float) ( + ( + (double) num1 * (double) num19 - + (double) num2 * (double) num21 + + (double) num3 * (double) num22 + ) * num27 + ); + float num28 = (float) ( + (double) num7 * (double) num16 - + (double) num8 * (double) num15 + ); + float num29 = (float) ( + (double) num6 * (double) num16 - + (double) num8 * (double) num14 + ); + float num30 = (float) ( + (double) num6 * (double) num15 - + (double) num7 * (double) num14 + ); + float num31 = (float) ( + (double) num5 * (double) num16 - + (double) num8 * (double) num13 + ); + float num32 = (float) ( + (double) num5 * (double) num15 - + (double) num7 * (double) num13 + ); + float num33 = (float) ( + (double) num5 * (double) num14 - + (double) num6 * (double) num13 + ); + result.M13 = (float) ( + ( + (double) num2 * (double) num28 - + (double) num3 * (double) num29 + + (double) num4 * (double) num30 + ) * num27 + ); + result.M23 = (float) ( + -( + (double) num1 * (double) num28 - + (double) num3 * (double) num31 + + (double) num4 * (double) num32 + ) * num27 + ); + result.M33 = (float) ( + ( + (double) num1 * (double) num29 - + (double) num2 * (double) num31 + + (double) num4 * (double) num33 + ) * num27 + ); + result.M43 = (float) ( + -( + (double) num1 * (double) num30 - + (double) num2 * (double) num32 + + (double) num3 * (double) num33 + ) * num27 + ); + float num34 = (float) ( + (double) num7 * (double) num12 - + (double) num8 * (double) num11 + ); + float num35 = (float) ( + (double) num6 * (double) num12 - + (double) num8 * (double) num10 + ); + float num36 = (float) ( + (double) num6 * (double) num11 - + (double) num7 * (double) num10 + ); + float num37 = (float) ( + (double) num5 * (double) num12 - + (double) num8 * (double) num9); + float num38 = (float) ( + (double) num5 * (double) num11 - + (double) num7 * (double) num9 + ); + float num39 = (float) ( + (double) num5 * (double) num10 - + (double) num6 * (double) num9 + ); + result.M14 = (float) ( + -( + (double) num2 * (double) num34 - + (double) num3 * (double) num35 + + (double) num4 * (double) num36 + ) * num27 + ); + result.M24 = (float) ( + ( + (double) num1 * (double) num34 - + (double) num3 * (double) num37 + + (double) num4 * (double) num38 + ) * num27 + ); + result.M34 = (float) ( + -( + (double) num1 * (double) num35 - + (double) num2 * (double) num37 + + (double) num4 * (double) num39 + ) * num27 + ); + result.M44 = (float) ( + ( + (double) num1 * (double) num36 - + (double) num2 * (double) num38 + + (double) num3 * (double) num39 + ) * num27 + ); + } + + /// + /// 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, float 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, + float 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 + ) { + float m11 = ( + (matrix1.M11 * matrix2.M11) + + (matrix1.M12 * matrix2.M21) + + (matrix1.M13 * matrix2.M31) + + (matrix1.M14 * matrix2.M41) + ); + float m12 = ( + (matrix1.M11 * matrix2.M12) + + (matrix1.M12 * matrix2.M22) + + (matrix1.M13 * matrix2.M32) + + (matrix1.M14 * matrix2.M42) + ); + float m13 = ( + (matrix1.M11 * matrix2.M13) + + (matrix1.M12 * matrix2.M23) + + (matrix1.M13 * matrix2.M33) + + (matrix1.M14 * matrix2.M43) + ); + float m14 = ( + (matrix1.M11 * matrix2.M14) + + (matrix1.M12 * matrix2.M24) + + (matrix1.M13 * matrix2.M34) + + (matrix1.M14 * matrix2.M44) + ); + float m21 = ( + (matrix1.M21 * matrix2.M11) + + (matrix1.M22 * matrix2.M21) + + (matrix1.M23 * matrix2.M31) + + (matrix1.M24 * matrix2.M41) + ); + float m22 = ( + (matrix1.M21 * matrix2.M12) + + (matrix1.M22 * matrix2.M22) + + (matrix1.M23 * matrix2.M32) + + (matrix1.M24 * matrix2.M42) + ); + float m23 = ( + (matrix1.M21 * matrix2.M13) + + (matrix1.M22 * matrix2.M23) + + (matrix1.M23 * matrix2.M33) + + (matrix1.M24 * matrix2.M43) + ); + float m24 = ( + (matrix1.M21 * matrix2.M14) + + (matrix1.M22 * matrix2.M24) + + (matrix1.M23 * matrix2.M34) + + (matrix1.M24 * matrix2.M44) + ); + float m31 = ( + (matrix1.M31 * matrix2.M11) + + (matrix1.M32 * matrix2.M21) + + (matrix1.M33 * matrix2.M31) + + (matrix1.M34 * matrix2.M41) + ); + float m32 = ( + (matrix1.M31 * matrix2.M12) + + (matrix1.M32 * matrix2.M22) + + (matrix1.M33 * matrix2.M32) + + (matrix1.M34 * matrix2.M42) + ); + float m33 = ( + (matrix1.M31 * matrix2.M13) + + (matrix1.M32 * matrix2.M23) + + (matrix1.M33 * matrix2.M33) + + (matrix1.M34 * matrix2.M43) + ); + float m34 = ( + (matrix1.M31 * matrix2.M14) + + (matrix1.M32 * matrix2.M24) + + (matrix1.M33 * matrix2.M34) + + (matrix1.M34 * matrix2.M44) + ); + float m41 = ( + (matrix1.M41 * matrix2.M11) + + (matrix1.M42 * matrix2.M21) + + (matrix1.M43 * matrix2.M31) + + (matrix1.M44 * matrix2.M41) + ); + float m42 = ( + (matrix1.M41 * matrix2.M12) + + (matrix1.M42 * matrix2.M22) + + (matrix1.M43 * matrix2.M32) + + (matrix1.M44 * matrix2.M42) + ); + float m43 = ( + (matrix1.M41 * matrix2.M13) + + (matrix1.M42 * matrix2.M23) + + (matrix1.M43 * matrix2.M33) + + (matrix1.M44 * matrix2.M43) + ); + float 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) + { + float m11 = ( + (matrix1.M11 * matrix2.M11) + + (matrix1.M12 * matrix2.M21) + + (matrix1.M13 * matrix2.M31) + + (matrix1.M14 * matrix2.M41) + ); + float m12 = ( + (matrix1.M11 * matrix2.M12) + + (matrix1.M12 * matrix2.M22) + + (matrix1.M13 * matrix2.M32) + + (matrix1.M14 * matrix2.M42) + ); + float m13 = ( + (matrix1.M11 * matrix2.M13) + + (matrix1.M12 * matrix2.M23) + + (matrix1.M13 * matrix2.M33) + + (matrix1.M14 * matrix2.M43) + ); + float m14 = ( + (matrix1.M11 * matrix2.M14) + + (matrix1.M12 * matrix2.M24) + + (matrix1.M13 * matrix2.M34) + + (matrix1.M14 * matrix2.M44) + ); + float m21 = ( + (matrix1.M21 * matrix2.M11) + + (matrix1.M22 * matrix2.M21) + + (matrix1.M23 * matrix2.M31) + + (matrix1.M24 * matrix2.M41) + ); + float m22 = ( + (matrix1.M21 * matrix2.M12) + + (matrix1.M22 * matrix2.M22) + + (matrix1.M23 * matrix2.M32) + + (matrix1.M24 * matrix2.M42) + ); + float m23 = ( + (matrix1.M21 * matrix2.M13) + + (matrix1.M22 * matrix2.M23) + + (matrix1.M23 * matrix2.M33) + + (matrix1.M24 * matrix2.M43) + ); + float m24 = ( + (matrix1.M21 * matrix2.M14) + + (matrix1.M22 * matrix2.M24) + + (matrix1.M23 * matrix2.M34) + + (matrix1.M24 * matrix2.M44) + ); + float m31 = ( + (matrix1.M31 * matrix2.M11) + + (matrix1.M32 * matrix2.M21) + + (matrix1.M33 * matrix2.M31) + + (matrix1.M34 * matrix2.M41) + ); + float m32 = ( + (matrix1.M31 * matrix2.M12) + + (matrix1.M32 * matrix2.M22) + + (matrix1.M33 * matrix2.M32) + + (matrix1.M34 * matrix2.M42) + ); + float m33 = ( + (matrix1.M31 * matrix2.M13) + + (matrix1.M32 * matrix2.M23) + + (matrix1.M33 * matrix2.M33) + + (matrix1.M34 * matrix2.M43) + ); + float m34 = ( + (matrix1.M31 * matrix2.M14) + + (matrix1.M32 * matrix2.M24) + + (matrix1.M33 * matrix2.M34) + + (matrix1.M34 * matrix2.M44) + ); + float m41 = ( + (matrix1.M41 * matrix2.M11) + + (matrix1.M42 * matrix2.M21) + + (matrix1.M43 * matrix2.M31) + + (matrix1.M44 * matrix2.M41) + ); + float m42 = ( + (matrix1.M41 * matrix2.M12) + + (matrix1.M42 * matrix2.M22) + + (matrix1.M43 * matrix2.M32) + + (matrix1.M44 * matrix2.M42) + ); + float m43 = ( + (matrix1.M41 * matrix2.M13) + + (matrix1.M42 * matrix2.M23) + + (matrix1.M43 * matrix2.M33) + + (matrix1.M44 * matrix2.M43) + ); + float 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, float 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, float 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; + + } + + /// + /// 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; + } + + /// + /// 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 -= 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 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; + } + + /// + /// 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; + } + + public static Matrix Transform(Matrix value, Quaternion rotation) + { + Matrix result; + Transform(ref value, ref rotation, out result); + return result; + } + + public static void Transform( + ref Matrix value, + ref Quaternion rotation, + out Matrix result + ) { + Matrix rotMatrix = CreateFromQuaternion(rotation); + Multiply(ref value, ref rotMatrix, out result); + } + + #endregion + + #region Public Static Operator Overloads + + /// + /// 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) + { + return Matrix.Add(matrix1, matrix2); + } + + /// + /// 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) + { + return Matrix.Divide(matrix1, matrix2); + } + + /// + /// 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, float divider) + { + return Matrix.Divide(matrix, divider); + } + + /// + /// 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.Equals(matrix2); + } + + /// + /// 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.Equals(matrix2); + } + + /// + /// 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) + { + return Multiply(matrix1, matrix2); + } + + /// + /// 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, float scaleFactor) + { + return Multiply(matrix, scaleFactor); + } + + /// + /// 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) + { + return Subtract(matrix1, matrix2); + } + + /// + /// Inverts values in the specified . + /// + /// Source on the right of the sub sign. + /// Result of the inversion. + public static Matrix operator -(Matrix matrix) + { + return Negate(matrix); + } + + #endregion + } +} diff --git a/src/Math/Plane.cs b/src/Math/Plane.cs new file mode 100644 index 0000000..091bbf6 --- /dev/null +++ b/src/Math/Plane.cs @@ -0,0 +1,329 @@ +#region License + +/* MoonWorks - Game Development Framework + * Copyright 2021 Evan Hemsley + */ + +/* Derived from code by Ethan Lee (Copyright 2009-2021). + * Released under the Microsoft Public License. + * See fna.LICENSE for details. + + * Derived from code by the Mono.Xna Team (Copyright 2006). + * Released under the MIT License. See monoxna.LICENSE for details. + */ + +#endregion + +#region Using Statements +using System; +using System.Diagnostics; + +#endregion + +namespace MoonWorks.Math +{ + [Serializable] + [DebuggerDisplay("{DebugDisplayString,nq}")] + public struct Plane : IEquatable + { + #region Internal Properties + + internal string DebugDisplayString + { + get + { + return string.Concat( + Normal.DebugDisplayString, " ", + D.ToString() + ); + } + } + + #endregion + + #region Public Fields + + public Vector3 Normal; + public float D; + + #endregion + + #region Public Constructors + + public Plane(Vector4 value) + : this(new Vector3(value.X, value.Y, value.Z), value.W) + { + } + + public Plane(Vector3 normal, float d) + { + Normal = normal; + D = d; + } + + public Plane(Vector3 a, Vector3 b, Vector3 c) + { + Vector3 ab = b - a; + Vector3 ac = c - a; + + Vector3 cross = Vector3.Cross(ab, ac); + Vector3.Normalize(ref cross, out Normal); + D = -(Vector3.Dot(Normal, a)); + } + + public Plane(float a, float b, float c, float d) + : this(new Vector3(a, b, c), d) + { + + } + + #endregion + + #region Public Methods + + public float Dot(Vector4 value) + { + return ( + (this.Normal.X * value.X) + + (this.Normal.Y * value.Y) + + (this.Normal.Z * value.Z) + + (this.D * value.W) + ); + } + + public void Dot(ref Vector4 value, out float result) + { + result = ( + (this.Normal.X * value.X) + + (this.Normal.Y * value.Y) + + (this.Normal.Z * value.Z) + + (this.D * value.W) + ); + } + + public float DotCoordinate(Vector3 value) + { + return ( + (this.Normal.X * value.X) + + (this.Normal.Y * value.Y) + + (this.Normal.Z * value.Z) + + this.D + ); + } + + public void DotCoordinate(ref Vector3 value, out float result) + { + result = ( + (this.Normal.X * value.X) + + (this.Normal.Y * value.Y) + + (this.Normal.Z * value.Z) + + this.D + ); + } + + public float DotNormal(Vector3 value) + { + return ( + (this.Normal.X * value.X) + + (this.Normal.Y * value.Y) + + (this.Normal.Z * value.Z) + ); + } + + public void DotNormal(ref Vector3 value, out float result) + { + result = ( + (this.Normal.X * value.X) + + (this.Normal.Y * value.Y) + + (this.Normal.Z * value.Z) + ); + } + + public void Normalize() + { + float length = Normal.Length(); + float factor = 1.0f / length; + Vector3.Multiply(ref Normal, factor, out Normal); + D = D * factor; + } + + public PlaneIntersectionType Intersects(BoundingBox box) + { + return box.Intersects(this); + } + + public void Intersects(ref BoundingBox box, out PlaneIntersectionType result) + { + box.Intersects(ref this, out result); + } + + public PlaneIntersectionType Intersects(BoundingSphere sphere) + { + return sphere.Intersects(this); + } + + public void Intersects(ref BoundingSphere sphere, out PlaneIntersectionType result) + { + sphere.Intersects(ref this, out result); + } + + public PlaneIntersectionType Intersects(BoundingFrustum frustum) + { + return frustum.Intersects(this); + } + + #endregion + + #region Internal Methods + + internal PlaneIntersectionType Intersects(ref Vector3 point) + { + float distance; + DotCoordinate(ref point, out distance); + if (distance > 0) + { + return PlaneIntersectionType.Front; + } + if (distance < 0) + { + return PlaneIntersectionType.Back; + } + return PlaneIntersectionType.Intersecting; + } + + #endregion + + #region Public Static Methods + + public static Plane Normalize(Plane value) + { + Plane ret; + Normalize(ref value, out ret); + return ret; + } + + public static void Normalize(ref Plane value, out Plane result) + { + float length = value.Normal.Length(); + float factor = 1.0f / length; + Vector3.Multiply(ref value.Normal, factor, out result.Normal); + result.D = value.D * factor; + } + + /// + /// Transforms a normalized plane by a matrix. + /// + /// The normalized plane to transform. + /// The transformation matrix. + /// The transformed plane. + public static Plane Transform(Plane plane, Matrix matrix) + { + Plane result; + Transform(ref plane, ref matrix, out result); + return result; + } + + /// + /// Transforms a normalized plane by a matrix. + /// + /// The normalized plane to transform. + /// The transformation matrix. + /// The transformed plane. + public static void Transform( + ref Plane plane, + ref Matrix matrix, + out Plane result + ) { + /* See "Transforming Normals" in + * http://www.glprogramming.com/red/appendixf.html + * for an explanation of how this works. + */ + Matrix transformedMatrix; + Matrix.Invert(ref matrix, out transformedMatrix); + Matrix.Transpose( + ref transformedMatrix, + out transformedMatrix + ); + Vector4 vector = new Vector4(plane.Normal, plane.D); + Vector4 transformedVector; + Vector4.Transform( + ref vector, + ref transformedMatrix, + out transformedVector + ); + result = new Plane(transformedVector); + } + + /// + /// Transforms a normalized plane by a quaternion rotation. + /// + /// The normalized plane to transform. + /// The quaternion rotation. + /// The transformed plane. + public static Plane Transform(Plane plane, Quaternion rotation) + { + Plane result; + Transform(ref plane, ref rotation, out result); + return result; + } + + /// + /// Transforms a normalized plane by a quaternion rotation. + /// + /// The normalized plane to transform. + /// The quaternion rotation. + /// The transformed plane. + public static void Transform( + ref Plane plane, + ref Quaternion rotation, + out Plane result + ) { + Vector3.Transform( + ref plane.Normal, + ref rotation, + out result.Normal + ); + result.D = plane.D; + } + + #endregion + + #region Public Static Operators and Override Methods + + public static bool operator !=(Plane plane1, Plane plane2) + { + return !plane1.Equals(plane2); + } + + public static bool operator ==(Plane plane1, Plane plane2) + { + return plane1.Equals(plane2); + } + + public override bool Equals(object obj) + { + return (obj is Plane) && this.Equals((Plane) obj); + } + + public bool Equals(Plane other) + { + return (Normal == other.Normal && D == other.D); + } + + public override int GetHashCode() + { + return Normal.GetHashCode() ^ D.GetHashCode(); + } + + public override string ToString() + { + return ( + "{Normal:" + Normal.ToString() + + " D:" + D.ToString() + + "}" + ); + } + + #endregion + } +} diff --git a/src/Math/PlaneIntersectionType.cs b/src/Math/PlaneIntersectionType.cs new file mode 100644 index 0000000..2d6e0e1 --- /dev/null +++ b/src/Math/PlaneIntersectionType.cs @@ -0,0 +1,37 @@ +#region License + +/* MoonWorks - Game Development Framework + * Copyright 2021 Evan Hemsley + */ + +/* Derived from code by Ethan Lee (Copyright 2009-2021). + * Released under the Microsoft Public License. + * See fna.LICENSE for details. + + * Derived from code by the Mono.Xna Team (Copyright 2006). + * Released under the MIT License. See monoxna.LICENSE for details. + */ + +#endregion + +namespace MoonWorks.Math +{ + /// + /// Defines the intersection between a and a bounding volume. + /// + public enum PlaneIntersectionType + { + /// + /// There is no intersection, the bounding volume is in the negative half space of the plane. + /// + Front, + /// + /// There is no intersection, the bounding volume is in the positive half space of the plane. + /// + Back, + /// + /// The plane is intersected. + /// + Intersecting + } +} diff --git a/src/Math/Point.cs b/src/Math/Point.cs new file mode 100644 index 0000000..77618f9 --- /dev/null +++ b/src/Math/Point.cs @@ -0,0 +1,214 @@ +#region License + +/* MoonWorks - Game Development Framework + * Copyright 2021 Evan Hemsley + */ + +/* Derived from code by Ethan Lee (Copyright 2009-2021). + * Released under the Microsoft Public License. + * See fna.LICENSE for details. + + * Derived from code by the Mono.Xna Team (Copyright 2006). + * Released under the MIT License. See monoxna.LICENSE for details. + */ + +#endregion + +#region Using Statements +using System; +using System.Diagnostics; + +#endregion + +namespace MoonWorks.Math +{ + /// + /// Describes a 2D-point. + /// + [Serializable] + [DebuggerDisplay("{DebugDisplayString,nq}")] + public struct Point : IEquatable + { + #region Public Static Properties + + /// + /// Returns a with coordinates 0, 0. + /// + public static Point Zero + { + get + { + return zeroPoint; + } + } + + #endregion + + #region Internal Properties + + internal string DebugDisplayString + { + get + { + return string.Concat( + X.ToString(), " ", + Y.ToString() + ); + } + } + + #endregion + + #region Public Fields + + /// + /// The x coordinate of this . + /// + public int X; + + /// + /// The y coordinate of this . + /// + public int Y; + + #endregion + + #region Private Static Variables + + private static readonly Point zeroPoint = new Point(); + + #endregion + + #region Public Constructors + + /// + /// Constructs a point with X and Y from two values. + /// + /// The x coordinate in 2d-space. + /// The y coordinate in 2d-space. + public Point(int x, int y) + { + this.X = x; + this.Y = y; + } + + #endregion + + #region Public Methods + + /// + /// Compares whether current instance is equal to specified . + /// + /// The to compare. + /// true if the instances are equal; false otherwise. + public bool Equals(Point other) + { + return ((X == other.X) && (Y == other.Y)); + } + + /// + /// Compares whether current instance is equal to specified . + /// + /// The to compare. + /// true if the instances are equal; false otherwise. + public override bool Equals(object obj) + { + return (obj is Point) && Equals((Point) obj); + } + + /// + /// Gets the hash code of this . + /// + /// Hash code of this . + public override int GetHashCode() + { + return X ^ Y; + } + + /// + /// Returns a representation of this in the format: + /// {X:[] Y:[]} + /// + /// representation of this . + public override string ToString() + { + return ( + "{X:" + X.ToString() + + " Y:" + Y.ToString() + + "}" + ); + } + + #endregion + + #region Public Static Operators + + /// + /// Adds two points. + /// + /// Source on the left of the add sign. + /// Source on the right of the add sign. + /// Sum of the points. + public static Point operator +(Point value1, Point value2) + { + return new Point(value1.X + value2.X, value1.Y + value2.Y); + } + + /// + /// Subtracts a from a . + /// + /// Source on the left of the sub sign. + /// Source on the right of the sub sign. + /// Result of the subtraction. + public static Point operator -(Point value1, Point value2) + { + return new Point(value1.X - value2.X, value1.Y - value2.Y); + } + + /// + /// Multiplies the components of two points by each other. + /// + /// Source on the left of the mul sign. + /// Source on the right of the mul sign. + /// Result of the multiplication. + public static Point operator *(Point value1, Point value2) + { + return new Point(value1.X * value2.X, value1.Y * value2.Y); + } + + /// + /// Divides the components of a by the components of another . + /// + /// Source on the left of the div sign. + /// Divisor on the right of the div sign. + /// The result of dividing the points. + public static Point operator /(Point value1, Point value2) + { + return new Point(value1.X / value2.X, value1.Y / value2.Y); + } + + /// + /// Compares whether two instances are equal. + /// + /// instance on the left of the equal sign. + /// instance on the right of the equal sign. + /// true if the instances are equal; false otherwise. + public static bool operator ==(Point a, Point b) + { + return a.Equals(b); + } + + /// + /// Compares whether two instances are not equal. + /// + /// instance on the left of the not equal sign. + /// instance on the right of the not equal sign. + /// true if the instances are not equal; false otherwise. + public static bool operator !=(Point a, Point b) + { + return !a.Equals(b); + } + + #endregion + } +} diff --git a/src/Math/Quaternion.cs b/src/Math/Quaternion.cs new file mode 100644 index 0000000..620b032 --- /dev/null +++ b/src/Math/Quaternion.cs @@ -0,0 +1,989 @@ +#region License + +/* MoonWorks - Game Development Framework + * Copyright 2021 Evan Hemsley + */ + +/* Derived from code by Ethan Lee (Copyright 2009-2021). + * Released under the Microsoft Public License. + * See fna.LICENSE for details. + + * Derived from code by the Mono.Xna Team (Copyright 2006). + * Released under the MIT License. See monoxna.LICENSE for details. + */ + +#endregion + +#region Using Statements +using System; +using System.Diagnostics; + +#endregion + +namespace MoonWorks.Math +{ + /// + /// An efficient mathematical representation for three dimensional rotations. + /// + [Serializable] + [DebuggerDisplay("{DebugDisplayString,nq}")] + public struct Quaternion : IEquatable + { + #region Public Static Properties + + /// + /// Returns a quaternion representing no rotation. + /// + public static Quaternion Identity + { + get + { + return identity; + } + } + + #endregion + + #region Internal Properties + + internal string DebugDisplayString + { + get + { + if (this == Quaternion.Identity) + { + return "Identity"; + } + + return string.Concat( + X.ToString(), " ", + Y.ToString(), " ", + Z.ToString(), " ", + W.ToString() + ); + } + } + + #endregion + + #region Public Fields + + /// + /// The x coordinate of this . + /// + public float X; + + /// + /// The y coordinate of this . + /// + public float Y; + + /// + /// The z coordinate of this . + /// + public float Z; + + /// + /// The rotation component of this . + /// + public float W; + + #endregion + + #region Private Static Variables + + private static readonly Quaternion identity = new Quaternion(0, 0, 0, 1); + + #endregion + + #region Public Constructors + + /// + /// Constructs a quaternion with X, Y, Z and W from four values. + /// + /// The x coordinate in 3d-space. + /// The y coordinate in 3d-space. + /// The z coordinate in 3d-space. + /// The rotation component. + public Quaternion(float x, float y, float z, float w) + { + this.X = x; + this.Y = y; + this.Z = z; + this.W = w; + } + + /// + /// Constructs a quaternion with X, Y, Z from and rotation component from a scalar. + /// + /// The x, y, z coordinates in 3d-space. + /// The rotation component. + public Quaternion(Vector3 vectorPart, float scalarPart) + { + this.X = vectorPart.X; + this.Y = vectorPart.Y; + this.Z = vectorPart.Z; + this.W = scalarPart; + } + + #endregion + + #region Public Methods + + /// + /// Transforms this quaternion into its conjugated version. + /// + public void Conjugate() + { + X = -X; + Y = -Y; + Z = -Z; + } + + /// + /// Compares whether current instance is equal to specified . + /// + /// The to compare. + /// true if the instances are equal; false otherwise. + public override bool Equals(object obj) + { + return (obj is Quaternion) && Equals((Quaternion) obj); + } + + /// + /// Compares whether current instance is equal to specified . + /// + /// The to compare. + /// true if the instances are equal; false otherwise. + public bool Equals(Quaternion other) + { + return ( X == other.X && + Y == other.Y && + Z == other.Z && + W == other.W ); + } + + /// + /// Gets the hash code of this . + /// + /// Hash code of this . + public override int GetHashCode() + { + return ( + this.X.GetHashCode() + + this.Y.GetHashCode() + + this.Z.GetHashCode() + + this.W.GetHashCode() + ); + } + + /// + /// Returns the magnitude of the quaternion components. + /// + /// The magnitude of the quaternion components. + public float Length() + { + float num = ( + (this.X * this.X) + + (this.Y * this.Y) + + (this.Z * this.Z) + + (this.W * this.W) + ); + return (float) System.Math.Sqrt((double) num); + } + + /// + /// Returns the squared magnitude of the quaternion components. + /// + /// The squared magnitude of the quaternion components. + public float LengthSquared() + { + return ( + (this.X * this.X) + + (this.Y * this.Y) + + (this.Z * this.Z) + + (this.W * this.W) + ); + } + + /// + /// Scales the quaternion magnitude to unit length. + /// + public void Normalize() + { + float num = 1.0f / ((float) System.Math.Sqrt( + (X * X) + + (Y * Y) + + (Z * Z) + + (W * W) + )); + this.X *= num; + this.Y *= num; + this.Z *= num; + this.W *= num; + } + + /// + /// Returns a representation of this in the format: + /// {X:[] Y:[] Z:[] W:[]} + /// + /// A representation of this . + public override string ToString() + { + return ( + "{X:" + X.ToString() + + " Y:" + Y.ToString() + + " Z:" + Z.ToString() + + " W:" + W.ToString() + + "}" + ); + } + + #endregion + + #region Public Static Methods + + /// + /// Creates a new that contains the sum of two quaternions. + /// + /// Source . + /// Source . + /// The result of the quaternion addition. + public static Quaternion Add(Quaternion quaternion1, Quaternion quaternion2) + { + Quaternion quaternion; + Add(ref quaternion1, ref quaternion2, out quaternion); + return quaternion; + } + + /// + /// Creates a new that contains the sum of two quaternions. + /// + /// Source . + /// Source . + /// The result of the quaternion addition as an output parameter. + public static void Add( + ref Quaternion quaternion1, + ref Quaternion quaternion2, + out Quaternion result + ) { + result.X = quaternion1.X + quaternion2.X; + result.Y = quaternion1.Y + quaternion2.Y; + result.Z = quaternion1.Z + quaternion2.Z; + result.W = quaternion1.W + quaternion2.W; + } + + /// + /// Creates a new that contains concatenation between two quaternion. + /// + /// The first to concatenate. + /// The second to concatenate. + /// The result of rotation of followed by rotation. + public static Quaternion Concatenate(Quaternion value1, Quaternion value2) + { + Quaternion quaternion; + Concatenate(ref value1, ref value2, out quaternion); + return quaternion; + } + + /// + /// Creates a new that contains concatenation between two quaternion. + /// + /// The first to concatenate. + /// The second to concatenate. + /// The result of rotation of followed by rotation as an output parameter. + public static void Concatenate( + ref Quaternion value1, + ref Quaternion value2, + out Quaternion result + ) { + float x1 = value1.X; + float y1 = value1.Y; + float z1 = value1.Z; + float w1 = value1.W; + + float x2 = value2.X; + float y2 = value2.Y; + float z2 = value2.Z; + float w2 = value2.W; + + result.X = ((x2 * w1) + (x1 * w2)) + ((y2 * z1) - (z2 * y1)); + result.Y = ((y2 * w1) + (y1 * w2)) + ((z2 * x1) - (x2 * z1)); + result.Z = ((z2 * w1) + (z1 * w2)) + ((x2 * y1) - (y2 * x1)); + result.W = (w2 * w1) - (((x2 * x1) + (y2 * y1)) + (z2 * z1)); + } + + /// + /// Creates a new that contains conjugated version of the specified quaternion. + /// + /// The quaternion which values will be used to create the conjugated version. + /// The conjugate version of the specified quaternion. + public static Quaternion Conjugate(Quaternion value) + { + return new Quaternion(-value.X, -value.Y, -value.Z, value.W); + } + + /// + /// Creates a new that contains conjugated version of the specified quaternion. + /// + /// The quaternion which values will be used to create the conjugated version. + /// The conjugated version of the specified quaternion as an output parameter. + public static void Conjugate(ref Quaternion value, out Quaternion result) + { + result.X = -value.X; + result.Y = -value.Y; + result.Z = -value.Z; + result.W = value.W; + } + + /// + /// Creates a new from the specified axis and angle. + /// + /// The axis of rotation. + /// The angle in radians. + /// The new quaternion builded from axis and angle. + public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle) + { + Quaternion quaternion; + CreateFromAxisAngle(ref axis, angle, out quaternion); + return quaternion; + } + + /// + /// Creates a new from the specified axis and angle. + /// + /// The axis of rotation. + /// The angle in radians. + /// The new quaternion builded from axis and angle as an output parameter. + public static void CreateFromAxisAngle( + ref Vector3 axis, + float angle, + out Quaternion result + ) { + float half = angle * 0.5f; + float sin = (float) System.Math.Sin((double) half); + float cos = (float) System.Math.Cos((double) half); + result.X = axis.X * sin; + result.Y = axis.Y * sin; + result.Z = axis.Z * sin; + result.W = cos; + } + + /// + /// Creates a new from the specified . + /// + /// The rotation matrix. + /// A quaternion composed from the rotation part of the matrix. + public static Quaternion CreateFromRotationMatrix(Matrix matrix) + { + Quaternion quaternion; + CreateFromRotationMatrix(ref matrix, out quaternion); + return quaternion; + } + + /// + /// Creates a new from the specified . + /// + /// The rotation matrix. + /// A quaternion composed from the rotation part of the matrix as an output parameter. + public static void CreateFromRotationMatrix(ref Matrix matrix, out Quaternion result) + { + float sqrt; + float half; + float scale = matrix.M11 + matrix.M22 + matrix.M33; + + if (scale > 0.0f) + { + sqrt = (float) System.Math.Sqrt(scale + 1.0f); + result.W = sqrt * 0.5f; + sqrt = 0.5f / sqrt; + + result.X = (matrix.M23 - matrix.M32) * sqrt; + result.Y = (matrix.M31 - matrix.M13) * sqrt; + result.Z = (matrix.M12 - matrix.M21) * sqrt; + } + else if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33)) + { + sqrt = (float) System.Math.Sqrt(1.0f + matrix.M11 - matrix.M22 - matrix.M33); + half = 0.5f / sqrt; + + result.X = 0.5f * sqrt; + result.Y = (matrix.M12 + matrix.M21) * half; + result.Z = (matrix.M13 + matrix.M31) * half; + result.W = (matrix.M23 - matrix.M32) * half; + } + else if (matrix.M22 > matrix.M33) + { + sqrt = (float) System.Math.Sqrt(1.0f + matrix.M22 - matrix.M11 - matrix.M33); + half = 0.5f/sqrt; + + result.X = (matrix.M21 + matrix.M12)*half; + result.Y = 0.5f*sqrt; + result.Z = (matrix.M32 + matrix.M23)*half; + result.W = (matrix.M31 - matrix.M13)*half; + } + else + { + sqrt = (float) System.Math.Sqrt(1.0f + matrix.M33 - matrix.M11 - matrix.M22); + half = 0.5f / sqrt; + + result.X = (matrix.M31 + matrix.M13) * half; + result.Y = (matrix.M32 + matrix.M23) * half; + result.Z = 0.5f * sqrt; + result.W = (matrix.M12 - matrix.M21) * half; + } + } + + /// + /// Creates a new from the specified yaw, pitch and roll angles. + /// + /// Yaw around the y axis in radians. + /// Pitch around the x axis in radians. + /// Roll around the z axis in radians. + /// A new quaternion from the concatenated yaw, pitch, and roll angles. + public static Quaternion CreateFromYawPitchRoll(float yaw, float pitch, float roll) + { + Quaternion quaternion; + CreateFromYawPitchRoll(yaw, pitch, roll, out quaternion); + return quaternion; + } + + /// + /// Creates a new from the specified yaw, pitch and roll angles. + /// + /// Yaw around the y axis in radians. + /// Pitch around the x axis in radians. + /// Roll around the z axis in radians. + /// A new quaternion from the concatenated yaw, pitch, and roll angles as an output parameter. + public static void CreateFromYawPitchRoll( + float yaw, + float pitch, + float roll, + out Quaternion result) + { + float halfRoll = roll * 0.5f; + float sinRoll = (float) System.Math.Sin(halfRoll); + float cosRoll = (float) System.Math.Cos(halfRoll); + float halfPitch = pitch * 0.5f; + float sinPitch = (float) System.Math.Sin(halfPitch); + float cosPitch = (float) System.Math.Cos(halfPitch); + float halfYaw = yaw * 0.5f; + float sinYaw = (float) System.Math.Sin(halfYaw); + float cosYaw = (float) System.Math.Cos(halfYaw); + result.X = ((cosYaw * sinPitch) * cosRoll) + ((sinYaw * cosPitch) * sinRoll); + result.Y = ((sinYaw * cosPitch) * cosRoll) - ((cosYaw * sinPitch) * sinRoll); + result.Z = ((cosYaw * cosPitch) * sinRoll) - ((sinYaw * sinPitch) * cosRoll); + result.W = ((cosYaw * cosPitch) * cosRoll) + ((sinYaw * sinPitch) * sinRoll); + } + + /// + /// Divides a by the other . + /// + /// Source . + /// Divisor . + /// The result of dividing the quaternions. + public static Quaternion Divide(Quaternion quaternion1, Quaternion quaternion2) + { + Quaternion quaternion; + Divide(ref quaternion1, ref quaternion2, out quaternion); + return quaternion; + } + + /// + /// Divides a by the other . + /// + /// Source . + /// Divisor . + /// The result of dividing the quaternions as an output parameter. + public static void Divide( + ref Quaternion quaternion1, + ref Quaternion quaternion2, + out Quaternion result + ) { + float x = quaternion1.X; + float y = quaternion1.Y; + float z = quaternion1.Z; + float w = quaternion1.W; + float num14 = ( + (quaternion2.X * quaternion2.X) + + (quaternion2.Y * quaternion2.Y) + + (quaternion2.Z * quaternion2.Z) + + (quaternion2.W * quaternion2.W) + ); + float num5 = 1f / num14; + float num4 = -quaternion2.X * num5; + float num3 = -quaternion2.Y * num5; + float num2 = -quaternion2.Z * num5; + float num = quaternion2.W * num5; + float num13 = (y * num2) - (z * num3); + float num12 = (z * num4) - (x * num2); + float num11 = (x * num3) - (y * num4); + float num10 = ((x * num4) + (y * num3)) + (z * num2); + result.X = ((x * num) + (num4 * w)) + num13; + result.Y = ((y * num) + (num3 * w)) + num12; + result.Z = ((z * num) + (num2 * w)) + num11; + result.W = (w * num) - num10; + } + + /// + /// Returns a dot product of two quaternions. + /// + /// The first quaternion. + /// The second quaternion. + /// The dot product of two quaternions. + public static float Dot(Quaternion quaternion1, Quaternion quaternion2) + { + return ( + (quaternion1.X * quaternion2.X) + + (quaternion1.Y * quaternion2.Y) + + (quaternion1.Z * quaternion2.Z) + + (quaternion1.W * quaternion2.W) + ); + } + + /// + /// Returns a dot product of two quaternions. + /// + /// The first quaternion. + /// The second quaternion. + /// The dot product of two quaternions as an output parameter. + public static void Dot( + ref Quaternion quaternion1, + ref Quaternion quaternion2, + out float result + ) { + result = ( + (quaternion1.X * quaternion2.X) + + (quaternion1.Y * quaternion2.Y) + + (quaternion1.Z * quaternion2.Z) + + (quaternion1.W * quaternion2.W) + ); + } + + /// + /// Returns the inverse quaternion which represents the opposite rotation. + /// + /// Source . + /// The inverse quaternion. + public static Quaternion Inverse(Quaternion quaternion) + { + Quaternion inverse; + Inverse(ref quaternion, out inverse); + return inverse; + } + + /// + /// Returns the inverse quaternion which represents the opposite rotation. + /// + /// Source . + /// The inverse quaternion as an output parameter. + public static void Inverse(ref Quaternion quaternion, out Quaternion result) + { + float num2 = ( + (quaternion.X * quaternion.X) + + (quaternion.Y * quaternion.Y) + + (quaternion.Z * quaternion.Z) + + (quaternion.W * quaternion.W) + ); + float num = 1f / num2; + result.X = -quaternion.X * num; + result.Y = -quaternion.Y * num; + result.Z = -quaternion.Z * num; + result.W = quaternion.W * num; + } + + /// + /// Performs a linear blend between two quaternions. + /// + /// Source . + /// Source . + /// The blend amount where 0 returns and 1 . + /// The result of linear blending between two quaternions. + public static Quaternion Lerp( + Quaternion quaternion1, + Quaternion quaternion2, + float amount + ) { + Quaternion quaternion; + Lerp(ref quaternion1, ref quaternion2, amount, out quaternion); + return quaternion; + } + + /// + /// Performs a linear blend between two quaternions. + /// + /// Source . + /// Source . + /// The blend amount where 0 returns and 1 . + /// The result of linear blending between two quaternions as an output parameter. + public static void Lerp( + ref Quaternion quaternion1, + ref Quaternion quaternion2, + float amount, + out Quaternion result + ) { + float num = amount; + float num2 = 1f - num; + float num5 = ( + (quaternion1.X * quaternion2.X) + + (quaternion1.Y * quaternion2.Y) + + (quaternion1.Z * quaternion2.Z) + + (quaternion1.W * quaternion2.W) + ); + if (num5 >= 0f) + { + result.X = (num2 * quaternion1.X) + (num * quaternion2.X); + result.Y = (num2 * quaternion1.Y) + (num * quaternion2.Y); + result.Z = (num2 * quaternion1.Z) + (num * quaternion2.Z); + result.W = (num2 * quaternion1.W) + (num * quaternion2.W); + } + else + { + result.X = (num2 * quaternion1.X) - (num * quaternion2.X); + result.Y = (num2 * quaternion1.Y) - (num * quaternion2.Y); + result.Z = (num2 * quaternion1.Z) - (num * quaternion2.Z); + result.W = (num2 * quaternion1.W) - (num * quaternion2.W); + } + float num4 = ( + (result.X * result.X) + + (result.Y * result.Y) + + (result.Z * result.Z) + + (result.W * result.W) + ); + float num3 = 1f / ((float) System.Math.Sqrt((double) num4)); + result.X *= num3; + result.Y *= num3; + result.Z *= num3; + result.W *= num3; + } + + /// + /// Performs a spherical linear blend between two quaternions. + /// + /// Source . + /// Source . + /// The blend amount where 0 returns and 1 . + /// The result of spherical linear blending between two quaternions. + public static Quaternion Slerp( + Quaternion quaternion1, + Quaternion quaternion2, + float amount + ) { + Quaternion quaternion; + Slerp(ref quaternion1, ref quaternion2, amount, out quaternion); + return quaternion; + } + + /// + /// Performs a spherical linear blend between two quaternions. + /// + /// Source . + /// Source . + /// The blend amount where 0 returns and 1 . + /// The result of spherical linear blending between two quaternions as an output parameter. + public static void Slerp( + ref Quaternion quaternion1, + ref Quaternion quaternion2, + float amount, + out Quaternion result + ) { + float num2; + float num3; + float num = amount; + float num4 = ( + (quaternion1.X * quaternion2.X) + + (quaternion1.Y * quaternion2.Y) + + (quaternion1.Z * quaternion2.Z) + + (quaternion1.W * quaternion2.W) + ); + float flag = 1.0f; + if (num4 < 0f) + { + flag = -1.0f; + num4 = -num4; + } + if (num4 > 0.999999f) + { + num3 = 1f - num; + num2 = num * flag; + } + else + { + float num5 = (float) System.Math.Acos((double) num4); + float num6 = (float) (1.0 / System.Math.Sin((double) num5)); + num3 = ((float) System.Math.Sin((double) ((1f - num) * num5))) * num6; + num2 = flag * (((float) System.Math.Sin((double) (num * num5))) * num6); + } + result.X = (num3 * quaternion1.X) + (num2 * quaternion2.X); + result.Y = (num3 * quaternion1.Y) + (num2 * quaternion2.Y); + result.Z = (num3 * quaternion1.Z) + (num2 * quaternion2.Z); + result.W = (num3 * quaternion1.W) + (num2 * quaternion2.W); + } + + /// + /// Creates a new that contains subtraction of one from another. + /// + /// Source . + /// Source . + /// The result of the quaternion subtraction. + public static Quaternion Subtract(Quaternion quaternion1, Quaternion quaternion2) + { + Quaternion quaternion; + Subtract(ref quaternion1, ref quaternion2, out quaternion); + return quaternion; + } + + /// + /// Creates a new that contains subtraction of one from another. + /// + /// Source . + /// Source . + /// The result of the quaternion subtraction as an output parameter. + public static void Subtract( + ref Quaternion quaternion1, + ref Quaternion quaternion2, + out Quaternion result + ) { + result.X = quaternion1.X - quaternion2.X; + result.Y = quaternion1.Y - quaternion2.Y; + result.Z = quaternion1.Z - quaternion2.Z; + result.W = quaternion1.W - quaternion2.W; + } + + /// + /// Creates a new that contains a multiplication of two quaternions. + /// + /// Source . + /// Source . + /// The result of the quaternion multiplication. + public static Quaternion Multiply(Quaternion quaternion1, Quaternion quaternion2) + { + Quaternion quaternion; + Multiply(ref quaternion1, ref quaternion2, out quaternion); + return quaternion; + } + + /// + /// Creates a new that contains a multiplication of and a scalar. + /// + /// Source . + /// Scalar value. + /// The result of the quaternion multiplication with a scalar. + public static Quaternion Multiply(Quaternion quaternion1, float scaleFactor) + { + Quaternion quaternion; + Multiply(ref quaternion1, scaleFactor, out quaternion); + return quaternion; + } + + /// + /// Creates a new that contains a multiplication of two quaternions. + /// + /// Source . + /// Source . + /// The result of the quaternion multiplication as an output parameter. + public static void Multiply( + ref Quaternion quaternion1, + ref Quaternion quaternion2, + out Quaternion result + ) { + float x = quaternion1.X; + float y = quaternion1.Y; + float z = quaternion1.Z; + float w = quaternion1.W; + float num4 = quaternion2.X; + float num3 = quaternion2.Y; + float num2 = quaternion2.Z; + float num = quaternion2.W; + float num12 = (y * num2) - (z * num3); + float num11 = (z * num4) - (x * num2); + float num10 = (x * num3) - (y * num4); + float num9 = ((x * num4) + (y * num3)) + (z * num2); + result.X = ((x * num) + (num4 * w)) + num12; + result.Y = ((y * num) + (num3 * w)) + num11; + result.Z = ((z * num) + (num2 * w)) + num10; + result.W = (w * num) - num9; + } + + /// + /// Creates a new that contains a multiplication of and a scalar. + /// + /// Source . + /// Scalar value. + /// The result of the quaternion multiplication with a scalar as an output parameter. + public static void Multiply( + ref Quaternion quaternion1, + float scaleFactor, + out Quaternion result + ) { + result.X = quaternion1.X * scaleFactor; + result.Y = quaternion1.Y * scaleFactor; + result.Z = quaternion1.Z * scaleFactor; + result.W = quaternion1.W * scaleFactor; + } + + /// + /// Flips the sign of the all the quaternion components. + /// + /// Source . + /// The result of the quaternion negation. + public static Quaternion Negate(Quaternion quaternion) + { + return new Quaternion( + -quaternion.X, + -quaternion.Y, + -quaternion.Z, + -quaternion.W + ); + } + + /// + /// Flips the sign of the all the quaternion components. + /// + /// Source . + /// The result of the quaternion negation as an output parameter. + public static void Negate(ref Quaternion quaternion, out Quaternion result) + { + result.X = -quaternion.X; + result.Y = -quaternion.Y; + result.Z = -quaternion.Z; + result.W = -quaternion.W; + } + + /// + /// Scales the quaternion magnitude to unit length. + /// + /// Source . + /// The unit length quaternion. + public static Quaternion Normalize(Quaternion quaternion) + { + Quaternion quaternion2; + Normalize(ref quaternion, out quaternion2); + return quaternion2; + } + + /// + /// Scales the quaternion magnitude to unit length. + /// + /// Source . + /// The unit length quaternion an output parameter. + public static void Normalize(ref Quaternion quaternion, out Quaternion result) + { + float num = 1.0f / ((float) System.Math.Sqrt( + (quaternion.X * quaternion.X) + + (quaternion.Y * quaternion.Y) + + (quaternion.Z * quaternion.Z) + + (quaternion.W * quaternion.W) + )); + result.X = quaternion.X * num; + result.Y = quaternion.Y * num; + result.Z = quaternion.Z * num; + result.W = quaternion.W * num; + } + + #endregion + + #region Public Static Operator Overloads + + /// + /// Adds two quaternions. + /// + /// Source on the left of the add sign. + /// Source on the right of the add sign. + /// Sum of the vectors. + public static Quaternion operator +(Quaternion quaternion1, Quaternion quaternion2) + { + Quaternion quaternion; + Add(ref quaternion1, ref quaternion2, out quaternion); + return quaternion; + } + + /// + /// Divides a by the other . + /// + /// Source on the left of the div sign. + /// Divisor on the right of the div sign. + /// The result of dividing the quaternions. + public static Quaternion operator /(Quaternion quaternion1, Quaternion quaternion2) + { + Quaternion quaternion; + Divide(ref quaternion1, ref quaternion2, out quaternion); + return quaternion; + } + + /// + /// Compares whether two instances are equal. + /// + /// instance on the left of the equal sign. + /// instance on the right of the equal sign. + /// true if the instances are equal; false otherwise. + public static bool operator ==(Quaternion quaternion1, Quaternion quaternion2) + { + return quaternion1.Equals(quaternion2); + } + + /// + /// Compares whether two instances are not equal. + /// + /// instance on the left of the not equal sign. + /// instance on the right of the not equal sign. + /// true if the instances are not equal; false otherwise. + public static bool operator !=(Quaternion quaternion1, Quaternion quaternion2) + { + return !quaternion1.Equals(quaternion2); + } + + /// + /// Multiplies two quaternions. + /// + /// Source on the left of the mul sign. + /// Source on the right of the mul sign. + /// Result of the quaternions multiplication. + public static Quaternion operator *(Quaternion quaternion1, Quaternion quaternion2) + { + Quaternion quaternion; + Multiply(ref quaternion1, ref quaternion2, out quaternion); + return quaternion; + } + + /// + /// Multiplies the components of quaternion by a scalar. + /// + /// Source on the left of the mul sign. + /// Scalar value on the right of the mul sign. + /// Result of the quaternion multiplication with a scalar. + public static Quaternion operator *(Quaternion quaternion1, float scaleFactor) + { + Quaternion quaternion; + Multiply(ref quaternion1, scaleFactor, out quaternion); + return quaternion; + } + + /// + /// Subtracts a from a . + /// + /// Source on the left of the sub sign. + /// Source on the right of the sub sign. + /// Result of the quaternion subtraction. + public static Quaternion operator -(Quaternion quaternion1, Quaternion quaternion2) + { + Quaternion quaternion; + Subtract(ref quaternion1, ref quaternion2, out quaternion); + return quaternion; + } + + /// + /// Flips the sign of the all the quaternion components. + /// + /// Source on the right of the sub sign. + /// The result of the quaternion negation. + public static Quaternion operator -(Quaternion quaternion) + { + Quaternion quaternion2; + Negate(ref quaternion, out quaternion2); + return quaternion2; + } + + #endregion + } +} diff --git a/src/Math/Ray.cs b/src/Math/Ray.cs new file mode 100644 index 0000000..9e61314 --- /dev/null +++ b/src/Math/Ray.cs @@ -0,0 +1,300 @@ +#region License + +/* MoonWorks - Game Development Framework + * Copyright 2021 Evan Hemsley + */ + +/* Derived from code by Ethan Lee (Copyright 2009-2021). + * Released under the Microsoft Public License. + * See fna.LICENSE for details. + + * Derived from code by the Mono.Xna Team (Copyright 2006). + * Released under the MIT License. See monoxna.LICENSE for details. + */ + +#endregion + +#region Using Statements +using System; +using System.Diagnostics; + +#endregion + +namespace MoonWorks.Math +{ + [Serializable] + [DebuggerDisplay("{DebugDisplayString,nq}")] + public struct Ray : IEquatable + { + #region Internal Properties + + internal string DebugDisplayString + { + get + { + return string.Concat( + "Pos( ", Position.DebugDisplayString, " ) \r\n", + "Dir( ", Direction.DebugDisplayString, " )" + ); + } + } + + #endregion + + #region Public Fields + + public Vector3 Position; + public Vector3 Direction; + + #endregion + + + #region Public Constructors + + public Ray(Vector3 position, Vector3 direction) + { + Position = position; + Direction = direction; + } + + #endregion + + + #region Public Methods + + public override bool Equals(object obj) + { + return (obj is Ray) && Equals((Ray) obj); + } + + + public bool Equals(Ray other) + { + return ( this.Position.Equals(other.Position) && + this.Direction.Equals(other.Direction) ); + } + + + public override int GetHashCode() + { + return Position.GetHashCode() ^ Direction.GetHashCode(); + } + + // Adapted from http://www.scratchapixel.com/lessons/3d-basic-lessons/lesson-7-intersecting-simple-shapes/ray-box-intersection/ + public float? Intersects(BoundingBox box) + { + float? tMin = null, tMax = null; + + if (MathHelper.WithinEpsilon(Direction.X, 0.0f)) + { + if (Position.X < box.Min.X || Position.X > box.Max.X) + { + return null; + } + } + else + { + tMin = (box.Min.X - Position.X) / Direction.X; + tMax = (box.Max.X - Position.X) / Direction.X; + + if (tMin > tMax) + { + float? temp = tMin; + tMin = tMax; + tMax = temp; + } + } + + if (MathHelper.WithinEpsilon(Direction.Y, 0.0f)) + { + if (Position.Y < box.Min.Y || Position.Y > box.Max.Y) + { + return null; + } + } + else + { + float tMinY = (box.Min.Y - Position.Y) / Direction.Y; + float tMaxY = (box.Max.Y - Position.Y) / Direction.Y; + + if (tMinY > tMaxY) + { + float temp = tMinY; + tMinY = tMaxY; + tMaxY = temp; + } + + if ( (tMin.HasValue && tMin > tMaxY) || + (tMax.HasValue && tMinY > tMax) ) + { + return null; + } + + if (!tMin.HasValue || tMinY > tMin) tMin = tMinY; + if (!tMax.HasValue || tMaxY < tMax) tMax = tMaxY; + } + + if (MathHelper.WithinEpsilon(Direction.Z, 0.0f)) + { + if (Position.Z < box.Min.Z || Position.Z > box.Max.Z) + { + return null; + } + } + else + { + float tMinZ = (box.Min.Z - Position.Z) / Direction.Z; + float tMaxZ = (box.Max.Z - Position.Z) / Direction.Z; + + if (tMinZ > tMaxZ) + { + float temp = tMinZ; + tMinZ = tMaxZ; + tMaxZ = temp; + } + + if ( (tMin.HasValue && tMin > tMaxZ) || + (tMax.HasValue && tMinZ > tMax) ) + { + return null; + } + + if (!tMin.HasValue || tMinZ > tMin) tMin = tMinZ; + if (!tMax.HasValue || tMaxZ < tMax) tMax = tMaxZ; + } + + /* Having a positive tMin and a negative tMax means the ray is inside the + * box we expect the intesection distance to be 0 in that case. + */ + if ((tMin.HasValue && tMin < 0) && tMax > 0) return 0; + + /* A negative tMin means that the intersection point is behind the ray's + * origin. We discard these as not hitting the AABB. + */ + if (tMin < 0) return null; + + return tMin; + } + + + public void Intersects(ref BoundingBox box, out float? result) + { + result = Intersects(box); + } + + public float? Intersects(BoundingSphere sphere) + { + float? result; + Intersects(ref sphere, out result); + return result; + } + + public float? Intersects(Plane plane) + { + float? result; + Intersects(ref plane, out result); + return result; + } + + public float? Intersects(BoundingFrustum frustum) + { + float? result; + frustum.Intersects(ref this, out result); + return result; + } + + public void Intersects(ref Plane plane, out float? result) + { + float den = Vector3.Dot(Direction, plane.Normal); + if (System.Math.Abs(den) < 0.00001f) + { + result = null; + return; + } + + result = (-plane.D - Vector3.Dot(plane.Normal, Position)) / den; + + if (result < 0.0f) + { + if (result < -0.00001f) + { + result = null; + return; + } + + result = 0.0f; + } + } + + public void Intersects(ref BoundingSphere sphere, out float? result) + { + // Find the vector between where the ray starts the the sphere's center. + Vector3 difference = sphere.Center - this.Position; + + float differenceLengthSquared = difference.LengthSquared(); + float sphereRadiusSquared = sphere.Radius * sphere.Radius; + + float distanceAlongRay; + + /* If the distance between the ray start and the sphere's center is less than + * the radius of the sphere, it means we've intersected. Checking the + * LengthSquared is faster. + */ + if (differenceLengthSquared < sphereRadiusSquared) + { + result = 0.0f; + return; + } + + Vector3.Dot(ref this.Direction, ref difference, out distanceAlongRay); + // If the ray is pointing away from the sphere then we don't ever intersect. + if (distanceAlongRay < 0) + { + result = null; + return; + } + + /* Next we kinda use Pythagoras to check if we are within the bounds of the + * sphere. + * if x = radius of sphere + * if y = distance between ray position and sphere centre + * if z = the distance we've travelled along the ray + * if x^2 + z^2 - y^2 < 0, we do not intersect + */ + float dist = ( + sphereRadiusSquared + + (distanceAlongRay * distanceAlongRay) - + differenceLengthSquared + ); + + result = (dist < 0) ? null : distanceAlongRay - (float?) System.Math.Sqrt(dist); + } + + #endregion + + #region Public Static Methods + + public static bool operator !=(Ray a, Ray b) + { + return !a.Equals(b); + } + + + public static bool operator ==(Ray a, Ray b) + { + return a.Equals(b); + } + + + public override string ToString() + { + return ( + "{{Position:" + Position.ToString() + + " Direction:" + Direction.ToString() + + "}}" + ); + } + + #endregion + } +} diff --git a/src/Math/Rectangle.cs b/src/Math/Rectangle.cs new file mode 100644 index 0000000..42f113a --- /dev/null +++ b/src/Math/Rectangle.cs @@ -0,0 +1,447 @@ +#region License + +/* MoonWorks - Game Development Framework + * Copyright 2021 Evan Hemsley + */ + +/* Derived from code by Ethan Lee (Copyright 2009-2021). + * Released under the Microsoft Public License. + * See fna.LICENSE for details. + + * Derived from code by the Mono.Xna Team (Copyright 2006). + * Released under the MIT License. See monoxna.LICENSE for details. + */ + +#endregion + +#region Using Statements +using System; +using System.Diagnostics; + +#endregion + +namespace MoonWorks.Math +{ + /// + /// Describes a 2D-rectangle. + /// + [Serializable] + [DebuggerDisplay("{DebugDisplayString,nq}")] + public struct Rectangle : IEquatable + { + #region Public Properties + + /// + /// Returns the x coordinate of the left edge of this . + /// + public int Left + { + get + { + return X; + } + } + + /// + /// Returns the x coordinate of the right edge of this . + /// + public int Right + { + get + { + return (X + Width); + } + } + + /// + /// Returns the y coordinate of the top edge of this . + /// + public int Top + { + get + { + return Y; + } + } + + /// + /// Returns the y coordinate of the bottom edge of this . + /// + public int Bottom + { + get + { + return (Y + Height); + } + } + + /// + /// The top-left coordinates of this . + /// + public Point Location + { + get + { + return new Point(X, Y); + } + set + { + X = value.X; + Y = value.Y; + } + } + + /// + /// A located in the center of this 's bounds. + /// + /// + /// If or is an odd number, + /// the center point will be rounded down. + /// + public Point Center + { + get + { + return new Point( + X + (Width / 2), + Y + (Height / 2) + ); + } + } + + /// + /// Whether or not this has a width and + /// height of 0, and a position of (0, 0). + /// + public bool IsEmpty + { + get + { + return ( (Width == 0) && + (Height == 0) && + (X == 0) && + (Y == 0) ); + } + } + + #endregion + + #region Public Static Properties + + /// + /// Returns a with X=0, Y=0, Width=0, and Height=0. + /// + public static Rectangle Empty + { + get + { + return emptyRectangle; + } + } + + #endregion + + #region Internal Properties + + internal string DebugDisplayString + { + get + { + return string.Concat( + X.ToString(), " ", + Y.ToString(), " ", + Width.ToString(), " ", + Height.ToString() + ); + } + } + + #endregion + + #region Public Fields + + /// + /// The x coordinate of the top-left corner of this . + /// + public int X; + + /// + /// The y coordinate of the top-left corner of this . + /// + public int Y; + + /// + /// The width of this . + /// + public int Width; + + /// + /// The height of this . + /// + public int Height; + + #endregion + + #region Private Static Fields + + private static Rectangle emptyRectangle = new Rectangle(); + + #endregion + + #region Public Constructors + + /// + /// Creates a with the specified + /// position, width, and height. + /// + /// The x coordinate of the top-left corner of the created . + /// The y coordinate of the top-left corner of the created . + /// The width of the created . + /// The height of the created . + public Rectangle(int x, int y, int width, int height) + { + X = x; + Y = y; + Width = width; + Height = height; + } + + #endregion + + #region Public Methods + + /// + /// Gets whether or not the provided coordinates lie within the bounds of this . + /// + /// The x coordinate of the point to check for containment. + /// The y coordinate of the point to check for containment. + /// true if the provided coordinates lie inside this . false otherwise. + public bool Contains(int x, int y) + { + return ( (this.X <= x) && + (x < (this.X + this.Width)) && + (this.Y <= y) && + (y < (this.Y + this.Height)) ); + } + + /// + /// Gets whether or not the provided lies within the bounds of this . + /// + /// The coordinates to check for inclusion in this . + /// true if the provided lies inside this . false otherwise. + public bool Contains(Point value) + { + return ( (this.X <= value.X) && + (value.X < (this.X + this.Width)) && + (this.Y <= value.Y) && + (value.Y < (this.Y + this.Height)) ); + } + + /// + /// Gets whether or not the provided lies within the bounds of this . + /// + /// The to check for inclusion in this . + /// true if the provided 's bounds lie entirely inside this . false otherwise. + public bool Contains(Rectangle value) + { + return ( (this.X <= value.X) && + ((value.X + value.Width) <= (this.X + this.Width)) && + (this.Y <= value.Y) && + ((value.Y + value.Height) <= (this.Y + this.Height)) ); + } + + public void Contains(ref Point value, out bool result) + { + result = ( (this.X <= value.X) && + (value.X < (this.X + this.Width)) && + (this.Y <= value.Y) && + (value.Y < (this.Y + this.Height)) ); + } + + public void Contains(ref Rectangle value, out bool result) + { + result = ( (this.X <= value.X) && + ((value.X + value.Width) <= (this.X + this.Width)) && + (this.Y <= value.Y) && + ((value.Y + value.Height) <= (this.Y + this.Height)) ); + } + + /// + /// Increments this 's by the + /// x and y components of the provided . + /// + /// The x and y components to add to this 's . + public void Offset(Point offset) + { + X += offset.X; + Y += offset.Y; + } + + /// + /// Increments this 's by the + /// provided x and y coordinates. + /// + /// The x coordinate to add to this 's . + /// The y coordinate to add to this 's . + public void Offset(int offsetX, int offsetY) + { + X += offsetX; + Y += offsetY; + } + + public void Inflate(int horizontalValue, int verticalValue) + { + X -= horizontalValue; + Y -= verticalValue; + Width += horizontalValue * 2; + Height += verticalValue * 2; + } + + /// + /// Checks whether or not this is equivalent + /// to a provided . + /// + /// The to test for equality. + /// + /// true if this 's x coordinate, y coordinate, width, and height + /// match the values for the provided . false otherwise. + /// + public bool Equals(Rectangle other) + { + return this == other; + } + + /// + /// Checks whether or not this is equivalent + /// to a provided object. + /// + /// The to test for equality. + /// + /// true if the provided object is a , and this + /// 's x coordinate, y coordinate, width, and height + /// match the values for the provided . false otherwise. + /// + public override bool Equals(object obj) + { + return (obj is Rectangle) && this == ((Rectangle) obj); + } + + public override string ToString() + { + return ( + "{X:" + X.ToString() + + " Y:" + Y.ToString() + + " Width:" + Width.ToString() + + " Height:" + Height.ToString() + + "}" + ); + } + + public override int GetHashCode() + { + return (this.X ^ this.Y ^ this.Width ^ this.Height); + } + + /// + /// Gets whether or not the other intersects with this rectangle. + /// + /// The other rectangle for testing. + /// true if other intersects with this rectangle; false otherwise. + public bool Intersects(Rectangle value) + { + return ( value.Left < Right && + Left < value.Right && + value.Top < Bottom && + Top < value.Bottom ); + } + + /// + /// Gets whether or not the other intersects with this rectangle. + /// + /// The other rectangle for testing. + /// true if other intersects with this rectangle; false otherwise. As an output parameter. + public void Intersects(ref Rectangle value, out bool result) + { + result = ( value.Left < Right && + Left < value.Right && + value.Top < Bottom && + Top < value.Bottom ); + } + + #endregion + + #region Public Static Methods + + public static bool operator ==(Rectangle a, Rectangle b) + { + return ( (a.X == b.X) && + (a.Y == b.Y) && + (a.Width == b.Width) && + (a.Height == b.Height) ); + } + + public static bool operator !=(Rectangle a, Rectangle b) + { + return !(a == b); + } + + public static Rectangle Intersect(Rectangle value1, Rectangle value2) + { + Rectangle rectangle; + Intersect(ref value1, ref value2, out rectangle); + return rectangle; + } + + public static void Intersect( + ref Rectangle value1, + ref Rectangle value2, + out Rectangle result + ) { + if (value1.Intersects(value2)) + { + int right_side = System.Math.Min( + value1.X + value1.Width, + value2.X + value2.Width + ); + int left_side = System.Math.Max(value1.X, value2.X); + int top_side = System.Math.Max(value1.Y, value2.Y); + int bottom_side = System.Math.Min( + value1.Y + value1.Height, + value2.Y + value2.Height + ); + result = new Rectangle( + left_side, + top_side, + right_side - left_side, + bottom_side - top_side + ); + } + else + { + result = new Rectangle(0, 0, 0, 0); + } + } + + public static Rectangle Union(Rectangle value1, Rectangle value2) + { + int x = System.Math.Min(value1.X, value2.X); + int y = System.Math.Min(value1.Y, value2.Y); + return new Rectangle( + x, + y, + System.Math.Max(value1.Right, value2.Right) - x, + System.Math.Max(value1.Bottom, value2.Bottom) - y + ); + } + + public static void Union(ref Rectangle value1, ref Rectangle value2, out Rectangle result) + { + result.X = System.Math.Min(value1.X, value2.X); + result.Y = System.Math.Min(value1.Y, value2.Y); + result.Width = System.Math.Max(value1.Right, value2.Right) - result.X; + result.Height = System.Math.Max(value1.Bottom, value2.Bottom) - result.Y; + } + + #endregion + } +} diff --git a/src/Math/Vector2.cs b/src/Math/Vector2.cs new file mode 100644 index 0000000..7e33e54 --- /dev/null +++ b/src/Math/Vector2.cs @@ -0,0 +1,1166 @@ +#region License + +/* MoonWorks - Game Development Framework + * Copyright 2021 Evan Hemsley + */ + +/* Derived from code by Ethan Lee (Copyright 2009-2021). + * Released under the Microsoft Public License. + * See fna.LICENSE for details. + + * Derived from code by the Mono.Xna Team (Copyright 2006). + * Released under the MIT License. See monoxna.LICENSE for details. + */ + +#endregion + +#region Using Statements +using System; +using System.Diagnostics; + +#endregion + +namespace MoonWorks.Math +{ + /// + /// Describes a 2D-vector. + /// + [Serializable] + [DebuggerDisplay("{DebugDisplayString,nq}")] + public struct Vector2 : IEquatable + { + #region Public Static Properties + + /// + /// Returns a with components 0, 0. + /// + public static Vector2 Zero + { + get + { + return zeroVector; + } + } + + /// + /// Returns a with components 1, 1. + /// + public static Vector2 One + { + get + { + return unitVector; + } + } + + /// + /// Returns a with components 1, 0. + /// + public static Vector2 UnitX + { + get + { + return unitXVector; + } + } + + /// + /// Returns a with components 0, 1. + /// + public static Vector2 UnitY + { + get + { + return unitYVector; + } + } + + #endregion + + #region Internal Properties + + internal string DebugDisplayString + { + get + { + return string.Concat( + X.ToString(), " ", + Y.ToString() + ); + } + } + + #endregion + + #region Public Fields + + /// + /// The x coordinate of this . + /// + public float X; + + /// + /// The y coordinate of this . + /// + public float Y; + + #endregion + + #region Private Static Fields + + private static readonly Vector2 zeroVector = new Vector2(0f, 0f); + private static readonly Vector2 unitVector = new Vector2(1f, 1f); + private static readonly Vector2 unitXVector = new Vector2(1f, 0f); + private static readonly Vector2 unitYVector = new Vector2(0f, 1f); + + #endregion + + #region Public Constructors + + /// + /// Constructs a 2d vector with X and Y from two values. + /// + /// The x coordinate in 2d-space. + /// The y coordinate in 2d-space. + public Vector2(float x, float y) + { + this.X = x; + this.Y = y; + } + + /// + /// Constructs a 2d vector with X and Y set to the same value. + /// + /// The x and y coordinates in 2d-space. + public Vector2(float value) + { + this.X = value; + this.Y = value; + } + + #endregion + + #region Public Methods + + /// + /// Compares whether current instance is equal to specified . + /// + /// The to compare. + /// true if the instances are equal; false otherwise. + public override bool Equals(object obj) + { + return (obj is Vector2) && Equals((Vector2) obj); + } + + /// + /// Compares whether current instance is equal to specified . + /// + /// The to compare. + /// true if the instances are equal; false otherwise. + public bool Equals(Vector2 other) + { + return ( X == other.X && + Y == other.Y ); + } + + /// + /// Gets the hash code of this . + /// + /// Hash code of this . + public override int GetHashCode() + { + return X.GetHashCode() + Y.GetHashCode(); + } + + /// + /// Returns the length of this . + /// + /// The length of this . + public float Length() + { + return (float) System.Math.Sqrt((X * X) + (Y * Y)); + } + + /// + /// Returns the squared length of this . + /// + /// The squared length of this . + public float LengthSquared() + { + return (X * X) + (Y * Y); + } + + /// + /// Turns this to a unit vector with the same direction. + /// + public void Normalize() + { + float val = 1.0f / (float) System.Math.Sqrt((X * X) + (Y * Y)); + X *= val; + Y *= val; + } + + /// + /// Returns a representation of this in the format: + /// {X:[] Y:[]} + /// + /// A representation of this . + public override string ToString() + { + return ( + "{X:" + X.ToString() + + " Y:" + Y.ToString() + + "}" + ); + } + + #endregion + + #region Public Static Methods + + /// + /// Performs vector addition on and . + /// + /// The first vector to add. + /// The second vector to add. + /// The result of the vector addition. + public static Vector2 Add(Vector2 value1, Vector2 value2) + { + value1.X += value2.X; + value1.Y += value2.Y; + return value1; + } + + /// + /// Performs vector addition on and + /// , storing the result of the + /// addition in . + /// + /// The first vector to add. + /// The second vector to add. + /// The result of the vector addition. + public static void Add(ref Vector2 value1, ref Vector2 value2, out Vector2 result) + { + result.X = value1.X + value2.X; + result.Y = value1.Y + value2.Y; + } + + /// + /// Creates a new that contains the cartesian coordinates of a vector specified in barycentric coordinates and relative to 2d-triangle. + /// + /// The first vector of 2d-triangle. + /// The second vector of 2d-triangle. + /// The third vector of 2d-triangle. + /// Barycentric scalar b2 which represents a weighting factor towards second vector of 2d-triangle. + /// Barycentric scalar b3 which represents a weighting factor towards third vector of 2d-triangle. + /// The cartesian translation of barycentric coordinates. + public static Vector2 Barycentric( + Vector2 value1, + Vector2 value2, + Vector2 value3, + float amount1, + float amount2 + ) { + return new Vector2( + MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2), + MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2) + ); + } + + /// + /// Creates a new that contains the cartesian coordinates of a vector specified in barycentric coordinates and relative to 2d-triangle. + /// + /// The first vector of 2d-triangle. + /// The second vector of 2d-triangle. + /// The third vector of 2d-triangle. + /// Barycentric scalar b2 which represents a weighting factor towards second vector of 2d-triangle. + /// Barycentric scalar b3 which represents a weighting factor towards third vector of 2d-triangle. + /// The cartesian translation of barycentric coordinates as an output parameter. + public static void Barycentric( + ref Vector2 value1, + ref Vector2 value2, + ref Vector2 value3, + float amount1, + float amount2, + out Vector2 result + ) { + result.X = MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2); + result.Y = MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2); + } + + /// + /// Creates a new that contains CatmullRom interpolation of the specified vectors. + /// + /// The first vector in interpolation. + /// The second vector in interpolation. + /// The third vector in interpolation. + /// The fourth vector in interpolation. + /// Weighting factor. + /// The result of CatmullRom interpolation. + public static Vector2 CatmullRom( + Vector2 value1, + Vector2 value2, + Vector2 value3, + Vector2 value4, + float amount + ) { + return new Vector2( + MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), + MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount) + ); + } + + /// + /// Creates a new that contains CatmullRom interpolation of the specified vectors. + /// + /// The first vector in interpolation. + /// The second vector in interpolation. + /// The third vector in interpolation. + /// The fourth vector in interpolation. + /// Weighting factor. + /// The result of CatmullRom interpolation as an output parameter. + public static void CatmullRom( + ref Vector2 value1, + ref Vector2 value2, + ref Vector2 value3, + ref Vector2 value4, + float amount, + out Vector2 result + ) { + result.X = MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount); + result.Y = MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount); + } + + /// + /// Clamps the specified value within a range. + /// + /// The value to clamp. + /// The min value. + /// The max value. + /// The clamped value. + public static Vector2 Clamp(Vector2 value1, Vector2 min, Vector2 max) + { + return new Vector2( + MathHelper.Clamp(value1.X, min.X, max.X), + MathHelper.Clamp(value1.Y, min.Y, max.Y) + ); + } + + /// + /// Clamps the specified value within a range. + /// + /// The value to clamp. + /// The min value. + /// The max value. + /// The clamped value as an output parameter. + public static void Clamp( + ref Vector2 value1, + ref Vector2 min, + ref Vector2 max, + out Vector2 result + ) { + result.X = MathHelper.Clamp(value1.X, min.X, max.X); + result.Y = MathHelper.Clamp(value1.Y, min.Y, max.Y); + } + + /// + /// Returns the distance between two vectors. + /// + /// The first vector. + /// The second vector. + /// The distance between two vectors. + public static float Distance(Vector2 value1, Vector2 value2) + { + float v1 = value1.X - value2.X, v2 = value1.Y - value2.Y; + return (float) System.Math.Sqrt((v1 * v1) + (v2 * v2)); + } + + /// + /// Returns the distance between two vectors. + /// + /// The first vector. + /// The second vector. + /// The distance between two vectors as an output parameter. + public static void Distance(ref Vector2 value1, ref Vector2 value2, out float result) + { + float v1 = value1.X - value2.X, v2 = value1.Y - value2.Y; + result = (float) System.Math.Sqrt((v1 * v1) + (v2 * v2)); + } + + /// + /// Returns the squared distance between two vectors. + /// + /// The first vector. + /// The second vector. + /// The squared distance between two vectors. + public static float DistanceSquared(Vector2 value1, Vector2 value2) + { + float v1 = value1.X - value2.X, v2 = value1.Y - value2.Y; + return (v1 * v1) + (v2 * v2); + } + + /// + /// Returns the squared distance between two vectors. + /// + /// The first vector. + /// The second vector. + /// The squared distance between two vectors as an output parameter. + public static void DistanceSquared( + ref Vector2 value1, + ref Vector2 value2, + out float result + ) { + float v1 = value1.X - value2.X, v2 = value1.Y - value2.Y; + result = (v1 * v1) + (v2 * v2); + } + + /// + /// Divides the components of a by the components of another . + /// + /// Source . + /// Divisor . + /// The result of dividing the vectors. + public static Vector2 Divide(Vector2 value1, Vector2 value2) + { + value1.X /= value2.X; + value1.Y /= value2.Y; + return value1; + } + + /// + /// Divides the components of a by the components of another . + /// + /// Source . + /// Divisor . + /// The result of dividing the vectors as an output parameter. + public static void Divide(ref Vector2 value1, ref Vector2 value2, out Vector2 result) + { + result.X = value1.X / value2.X; + result.Y = value1.Y / value2.Y; + } + + /// + /// Divides the components of a by a scalar. + /// + /// Source . + /// Divisor scalar. + /// The result of dividing a vector by a scalar. + public static Vector2 Divide(Vector2 value1, float divider) + { + float factor = 1 / divider; + value1.X *= factor; + value1.Y *= factor; + return value1; + } + + /// + /// Divides the components of a by a scalar. + /// + /// Source . + /// Divisor scalar. + /// The result of dividing a vector by a scalar as an output parameter. + public static void Divide(ref Vector2 value1, float divider, out Vector2 result) + { + float factor = 1 / divider; + result.X = value1.X * factor; + result.Y = value1.Y * factor; + } + + /// + /// Returns a dot product of two vectors. + /// + /// The first vector. + /// The second vector. + /// The dot product of two vectors. + public static float Dot(Vector2 value1, Vector2 value2) + { + return (value1.X * value2.X) + (value1.Y * value2.Y); + } + + /// + /// Returns a dot product of two vectors. + /// + /// The first vector. + /// The second vector. + /// The dot product of two vectors as an output parameter. + public static void Dot(ref Vector2 value1, ref Vector2 value2, out float result) + { + result = (value1.X * value2.X) + (value1.Y * value2.Y); + } + + /// + /// Creates a new that contains hermite spline interpolation. + /// + /// The first position vector. + /// The first tangent vector. + /// The second position vector. + /// The second tangent vector. + /// Weighting factor. + /// The hermite spline interpolation vector. + public static Vector2 Hermite( + Vector2 value1, + Vector2 tangent1, + Vector2 value2, + Vector2 tangent2, + float amount + ) { + Vector2 result = new Vector2(); + Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result); + return result; + } + + /// + /// Creates a new that contains hermite spline interpolation. + /// + /// The first position vector. + /// The first tangent vector. + /// The second position vector. + /// The second tangent vector. + /// Weighting factor. + /// The hermite spline interpolation vector as an output parameter. + public static void Hermite( + ref Vector2 value1, + ref Vector2 tangent1, + ref Vector2 value2, + ref Vector2 tangent2, + float amount, + out Vector2 result + ) { + result.X = MathHelper.Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount); + result.Y = MathHelper.Hermite(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount); + } + + /// + /// Creates a new that contains linear interpolation of the specified vectors. + /// + /// The first vector. + /// The second vector. + /// Weighting value(between 0.0 and 1.0). + /// The result of linear interpolation of the specified vectors. + public static Vector2 Lerp(Vector2 value1, Vector2 value2, float amount) + { + return new Vector2( + MathHelper.Lerp(value1.X, value2.X, amount), + MathHelper.Lerp(value1.Y, value2.Y, amount) + ); + } + + /// + /// Creates a new that contains linear interpolation of the specified vectors. + /// + /// The first vector. + /// The second vector. + /// Weighting value(between 0.0 and 1.0). + /// The result of linear interpolation of the specified vectors as an output parameter. + public static void Lerp( + ref Vector2 value1, + ref Vector2 value2, + float amount, + out Vector2 result + ) { + result.X = MathHelper.Lerp(value1.X, value2.X, amount); + result.Y = MathHelper.Lerp(value1.Y, value2.Y, amount); + } + + /// + /// Creates a new that contains a maximal values from the two vectors. + /// + /// The first vector. + /// The second vector. + /// The with maximal values from the two vectors. + public static Vector2 Max(Vector2 value1, Vector2 value2) + { + return new Vector2( + value1.X > value2.X ? value1.X : value2.X, + value1.Y > value2.Y ? value1.Y : value2.Y + ); + } + + /// + /// Creates a new that contains a maximal values from the two vectors. + /// + /// The first vector. + /// The second vector. + /// The with maximal values from the two vectors as an output parameter. + public static void Max(ref Vector2 value1, ref Vector2 value2, out Vector2 result) + { + result.X = value1.X > value2.X ? value1.X : value2.X; + result.Y = value1.Y > value2.Y ? value1.Y : value2.Y; + } + + /// + /// Creates a new that contains a minimal values from the two vectors. + /// + /// The first vector. + /// The second vector. + /// The with minimal values from the two vectors. + public static Vector2 Min(Vector2 value1, Vector2 value2) + { + return new Vector2( + value1.X < value2.X ? value1.X : value2.X, + value1.Y < value2.Y ? value1.Y : value2.Y + ); + } + + /// + /// Creates a new that contains a minimal values from the two vectors. + /// + /// The first vector. + /// The second vector. + /// The with minimal values from the two vectors as an output parameter. + public static void Min(ref Vector2 value1, ref Vector2 value2, out Vector2 result) + { + result.X = value1.X < value2.X ? value1.X : value2.X; + result.Y = value1.Y < value2.Y ? value1.Y : value2.Y; + } + + /// + /// Creates a new that contains a multiplication of two vectors. + /// + /// Source . + /// Source . + /// The result of the vector multiplication. + public static Vector2 Multiply(Vector2 value1, Vector2 value2) + { + value1.X *= value2.X; + value1.Y *= value2.Y; + return value1; + } + + /// + /// Creates a new that contains a multiplication of and a scalar. + /// + /// Source . + /// Scalar value. + /// The result of the vector multiplication with a scalar. + public static Vector2 Multiply(Vector2 value1, float scaleFactor) + { + value1.X *= scaleFactor; + value1.Y *= scaleFactor; + return value1; + } + + /// + /// Creates a new that contains a multiplication of and a scalar. + /// + /// Source . + /// Scalar value. + /// The result of the multiplication with a scalar as an output parameter. + public static void Multiply(ref Vector2 value1, float scaleFactor, out Vector2 result) + { + result.X = value1.X * scaleFactor; + result.Y = value1.Y * scaleFactor; + } + + /// + /// Creates a new that contains a multiplication of two vectors. + /// + /// Source . + /// Source . + /// The result of the vector multiplication as an output parameter. + public static void Multiply(ref Vector2 value1, ref Vector2 value2, out Vector2 result) + { + result.X = value1.X * value2.X; + result.Y = value1.Y * value2.Y; + } + + /// + /// Creates a new that contains the specified vector inversion. + /// direction of . + /// + /// Source . + /// The result of the vector inversion. + public static Vector2 Negate(Vector2 value) + { + value.X = -value.X; + value.Y = -value.Y; + return value; + } + + /// + /// Creates a new that contains the specified vector inversion. + /// direction of in . + /// + /// Source . + /// The result of the vector inversion as an output parameter. + public static void Negate(ref Vector2 value, out Vector2 result) + { + result.X = -value.X; + result.Y = -value.Y; + } + + /// + /// Creates a new that contains a normalized values from another vector. + /// + /// Source . + /// Unit vector. + public static Vector2 Normalize(Vector2 value) + { + float val = 1.0f / (float) System.Math.Sqrt((value.X * value.X) + (value.Y * value.Y)); + value.X *= val; + value.Y *= val; + return value; + } + + /// + /// Creates a new that contains a normalized values from another vector. + /// + /// Source . + /// Unit vector as an output parameter. + public static void Normalize(ref Vector2 value, out Vector2 result) + { + float val = 1.0f / (float) System.Math.Sqrt((value.X * value.X) + (value.Y * value.Y)); + result.X = value.X * val; + result.Y = value.Y * val; + } + + /// + /// Creates a new that contains reflect vector of the given vector and normal. + /// + /// Source . + /// Reflection normal. + /// Reflected vector. + public static Vector2 Reflect(Vector2 vector, Vector2 normal) + { + Vector2 result; + float val = 2.0f * ((vector.X * normal.X) + (vector.Y * normal.Y)); + result.X = vector.X - (normal.X * val); + result.Y = vector.Y - (normal.Y * val); + return result; + } + + /// + /// Creates a new that contains reflect vector of the given vector and normal. + /// + /// Source . + /// Reflection normal. + /// Reflected vector as an output parameter. + public static void Reflect(ref Vector2 vector, ref Vector2 normal, out Vector2 result) + { + float val = 2.0f * ((vector.X * normal.X) + (vector.Y * normal.Y)); + result.X = vector.X - (normal.X * val); + result.Y = vector.Y - (normal.Y * val); + } + + /// + /// Creates a new that contains cubic interpolation of the specified vectors. + /// + /// Source . + /// Source . + /// Weighting value. + /// Cubic interpolation of the specified vectors. + public static Vector2 SmoothStep(Vector2 value1, Vector2 value2, float amount) + { + return new Vector2( + MathHelper.SmoothStep(value1.X, value2.X, amount), + MathHelper.SmoothStep(value1.Y, value2.Y, amount) + ); + } + + /// + /// Creates a new that contains cubic interpolation of the specified vectors. + /// + /// Source . + /// Source . + /// Weighting value. + /// Cubic interpolation of the specified vectors as an output parameter. + public static void SmoothStep( + ref Vector2 value1, + ref Vector2 value2, + float amount, + out Vector2 result + ) { + result.X = MathHelper.SmoothStep(value1.X, value2.X, amount); + result.Y = MathHelper.SmoothStep(value1.Y, value2.Y, amount); + } + + /// + /// Creates a new that contains subtraction of on from a another. + /// + /// Source . + /// Source . + /// The result of the vector subtraction. + public static Vector2 Subtract(Vector2 value1, Vector2 value2) + { + value1.X -= value2.X; + value1.Y -= value2.Y; + return value1; + } + + /// + /// Creates a new that contains subtraction of on from a another. + /// + /// Source . + /// Source . + /// The result of the vector subtraction as an output parameter. + public static void Subtract(ref Vector2 value1, ref Vector2 value2, out Vector2 result) + { + result.X = value1.X - value2.X; + result.Y = value1.Y - value2.Y; + } + + /// + /// Creates a new that contains a transformation of 2d-vector by the specified . + /// + /// Source . + /// The transformation . + /// Transformed . + public static Vector2 Transform(Vector2 position, Matrix matrix) + { + return new Vector2( + (position.X * matrix.M11) + (position.Y * matrix.M21) + matrix.M41, + (position.X * matrix.M12) + (position.Y * matrix.M22) + matrix.M42 + ); + } + + /// + /// Creates a new that contains a transformation of 2d-vector by the specified . + /// + /// Source . + /// The transformation . + /// Transformed as an output parameter. + public static void Transform( + ref Vector2 position, + ref Matrix matrix, + out Vector2 result + ) { + float x = (position.X * matrix.M11) + (position.Y * matrix.M21) + matrix.M41; + float y = (position.X * matrix.M12) + (position.Y * matrix.M22) + matrix.M42; + result.X = x; + result.Y = y; + } + + /// + /// Creates a new that contains a transformation of 2d-vector by the specified , representing the rotation. + /// + /// Source . + /// The which contains rotation transformation. + /// Transformed . + public static Vector2 Transform(Vector2 value, Quaternion rotation) + { + Transform(ref value, ref rotation, out value); + return value; + } + + /// + /// Creates a new that contains a transformation of 2d-vector by the specified , representing the rotation. + /// + /// Source . + /// The which contains rotation transformation. + /// Transformed as an output parameter. + public static void Transform( + ref Vector2 value, + ref Quaternion rotation, + out Vector2 result + ) { + float x = 2 * -(rotation.Z * value.Y); + float y = 2 * (rotation.Z * value.X); + float z = 2 * (rotation.X * value.Y - rotation.Y * value.X); + + result.X = value.X + x * rotation.W + (rotation.Y * z - rotation.Z * y); + result.Y = value.Y + y * rotation.W + (rotation.Z * x - rotation.X * z); + } + + /// + /// Apply transformation on all vectors within array of by the specified and places the results in an another array. + /// + /// Source array. + /// The transformation . + /// Destination array. + public static void Transform( + Vector2[] sourceArray, + ref Matrix matrix, + Vector2[] destinationArray + ) { + Transform(sourceArray, 0, ref matrix, destinationArray, 0, sourceArray.Length); + } + + /// + /// Apply transformation on vectors within array of by the specified and places the results in an another array. + /// + /// Source array. + /// The starting index of transformation in the source array. + /// The transformation . + /// Destination array. + /// The starting index in the destination array, where the first should be written. + /// The number of vectors to be transformed. + public static void Transform( + Vector2[] sourceArray, + int sourceIndex, + ref Matrix matrix, + Vector2[] destinationArray, + int destinationIndex, + int length + ) { + for (int x = 0; x < length; x += 1) + { + Vector2 position = sourceArray[sourceIndex + x]; + Vector2 destination = destinationArray[destinationIndex + x]; + destination.X = (position.X * matrix.M11) + (position.Y * matrix.M21) + + matrix.M41; + destination.Y = (position.X * matrix.M12) + (position.Y * matrix.M22) + + matrix.M42; + destinationArray[destinationIndex + x] = destination; + } + } + + /// + /// Apply transformation on all vectors within array of by the specified and places the results in an another array. + /// + /// Source array. + /// The which contains rotation transformation. + /// Destination array. + public static void Transform( + Vector2[] sourceArray, + ref Quaternion rotation, + Vector2[] destinationArray + ) { + Transform( + sourceArray, + 0, + ref rotation, + destinationArray, + 0, + sourceArray.Length + ); + } + + /// + /// Apply transformation on vectors within array of by the specified and places the results in an another array. + /// + /// Source array. + /// The starting index of transformation in the source array. + /// The which contains rotation transformation. + /// Destination array. + /// The starting index in the destination array, where the first should be written. + /// The number of vectors to be transformed. + public static void Transform( + Vector2[] sourceArray, + int sourceIndex, + ref Quaternion rotation, + Vector2[] destinationArray, + int destinationIndex, + int length + ) { + for (int i = 0; i < length; i += 1) + { + Vector2 position = sourceArray[sourceIndex + i]; + Vector2 v; + Transform(ref position, ref rotation, out v); + destinationArray[destinationIndex + i] = v; + } + } + + /// + /// Creates a new that contains a transformation of the specified normal by the specified . + /// + /// Source which represents a normal vector. + /// The transformation . + /// Transformed normal. + public static Vector2 TransformNormal(Vector2 normal, Matrix matrix) + { + return new Vector2( + (normal.X * matrix.M11) + (normal.Y * matrix.M21), + (normal.X * matrix.M12) + (normal.Y * matrix.M22) + ); + } + + /// + /// Creates a new that contains a transformation of the specified normal by the specified . + /// + /// Source which represents a normal vector. + /// The transformation . + /// Transformed normal as an output parameter. + public static void TransformNormal( + ref Vector2 normal, + ref Matrix matrix, + out Vector2 result + ) { + float x = (normal.X * matrix.M11) + (normal.Y * matrix.M21); + float y = (normal.X * matrix.M12) + (normal.Y * matrix.M22); + result.X = x; + result.Y = y; + } + + /// + /// Apply transformation on all normals within array of by the specified and places the results in an another array. + /// + /// Source array. + /// The transformation . + /// Destination array. + public static void TransformNormal( + Vector2[] sourceArray, + ref Matrix matrix, + Vector2[] destinationArray + ) { + TransformNormal( + sourceArray, + 0, + ref matrix, + destinationArray, + 0, + sourceArray.Length + ); + } + + /// + /// Apply transformation on normals within array of by the specified and places the results in an another array. + /// + /// Source array. + /// The starting index of transformation in the source array. + /// The transformation . + /// Destination array. + /// The starting index in the destination array, where the first should be written. + /// The number of normals to be transformed. + public static void TransformNormal( + Vector2[] sourceArray, + int sourceIndex, + ref Matrix matrix, + Vector2[] destinationArray, + int destinationIndex, + int length + ) { + for (int i = 0; i < length; i += 1) + { + Vector2 position = sourceArray[sourceIndex + i]; + Vector2 result; + result.X = (position.X * matrix.M11) + (position.Y * matrix.M21); + result.Y = (position.X * matrix.M12) + (position.Y * matrix.M22); + destinationArray[destinationIndex + i] = result; + } + } + + #endregion + + #region Public Static Operators + + /// + /// Inverts values in the specified . + /// + /// Source on the right of the sub sign. + /// Result of the inversion. + public static Vector2 operator -(Vector2 value) + { + value.X = -value.X; + value.Y = -value.Y; + return value; + } + + /// + /// Compares whether two instances are equal. + /// + /// instance on the left of the equal sign. + /// instance on the right of the equal sign. + /// true if the instances are equal; false otherwise. + public static bool operator ==(Vector2 value1, Vector2 value2) + { + return ( value1.X == value2.X && + value1.Y == value2.Y ); + } + + /// + /// Compares whether two instances are equal. + /// + /// instance on the left of the equal sign. + /// instance on the right of the equal sign. + /// true if the instances are equal; false otherwise. + public static bool operator !=(Vector2 value1, Vector2 value2) + { + return !(value1 == value2); + } + + /// + /// Adds two vectors. + /// + /// Source on the left of the add sign. + /// Source on the right of the add sign. + /// Sum of the vectors. + public static Vector2 operator +(Vector2 value1, Vector2 value2) + { + value1.X += value2.X; + value1.Y += value2.Y; + return value1; + } + + /// + /// Subtracts a from a . + /// + /// Source on the left of the sub sign. + /// Source on the right of the sub sign. + /// Result of the vector subtraction. + public static Vector2 operator -(Vector2 value1, Vector2 value2) + { + value1.X -= value2.X; + value1.Y -= value2.Y; + return value1; + } + + /// + /// Multiplies the components of two vectors by each other. + /// + /// Source on the left of the mul sign. + /// Source on the right of the mul sign. + /// Result of the vector multiplication. + public static Vector2 operator *(Vector2 value1, Vector2 value2) + { + value1.X *= value2.X; + value1.Y *= value2.Y; + return value1; + } + + /// + /// Multiplies the components of vector by a scalar. + /// + /// Source on the left of the mul sign. + /// Scalar value on the right of the mul sign. + /// Result of the vector multiplication with a scalar. + public static Vector2 operator *(Vector2 value, float scaleFactor) + { + value.X *= scaleFactor; + value.Y *= scaleFactor; + return value; + } + + /// + /// Multiplies the components of vector by a scalar. + /// + /// Scalar value on the left of the mul sign. + /// Source on the right of the mul sign. + /// Result of the vector multiplication with a scalar. + public static Vector2 operator *(float scaleFactor, Vector2 value) + { + value.X *= scaleFactor; + value.Y *= scaleFactor; + return value; + } + + /// + /// Divides the components of a by the components of another . + /// + /// Source on the left of the div sign. + /// Divisor on the right of the div sign. + /// The result of dividing the vectors. + public static Vector2 operator /(Vector2 value1, Vector2 value2) + { + value1.X /= value2.X; + value1.Y /= value2.Y; + return value1; + } + + /// + /// Divides the components 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 vector by a scalar. + public static Vector2 operator /(Vector2 value1, float divider) + { + float factor = 1 / divider; + value1.X *= factor; + value1.Y *= factor; + return value1; + } + + #endregion + } +} diff --git a/src/Math/Vector3.cs b/src/Math/Vector3.cs new file mode 100644 index 0000000..2c372c4 --- /dev/null +++ b/src/Math/Vector3.cs @@ -0,0 +1,1509 @@ +#region License + +/* MoonWorks - Game Development Framework + * Copyright 2021 Evan Hemsley + */ + +/* Derived from code by Ethan Lee (Copyright 2009-2021). + * Released under the Microsoft Public License. + * See fna.LICENSE for details. + + * Derived from code by the Mono.Xna Team (Copyright 2006). + * Released under the MIT License. See monoxna.LICENSE for details. + */ + +#endregion + +#region Using Statements +using System; +using System.Diagnostics; +using System.Text; + +#endregion + +namespace MoonWorks.Math +{ + /// + /// Describes a 3D-vector. + /// + [Serializable] + [DebuggerDisplay("{DebugDisplayString,nq}")] + public struct Vector3 : IEquatable + { + #region Public Static Properties + + /// + /// Returns a with components 0, 0, 0. + /// + public static Vector3 Zero + { + get + { + return zero; + } + } + + /// + /// Returns a with components 1, 1, 1. + /// + public static Vector3 One + { + get + { + return one; + } + } + + /// + /// Returns a with components 1, 0, 0. + /// + public static Vector3 UnitX + { + get + { + return unitX; + } + } + + /// + /// Returns a with components 0, 1, 0. + /// + public static Vector3 UnitY + { + get + { + return unitY; + } + } + + /// + /// Returns a with components 0, 0, 1. + /// + public static Vector3 UnitZ + { + get + { + return unitZ; + } + } + + /// + /// Returns a with components 0, 1, 0. + /// + public static Vector3 Up + { + get + { + return up; + } + } + + /// + /// Returns a with components 0, -1, 0. + /// + public static Vector3 Down + { + get + { + return down; + } + } + + /// + /// Returns a with components 1, 0, 0. + /// + public static Vector3 Right + { + get + { + return right; + } + } + + /// + /// Returns a with components -1, 0, 0. + /// + public static Vector3 Left + { + get + { + return left; + } + } + + /// + /// Returns a with components 0, 0, -1. + /// + public static Vector3 Forward + { + get + { + return forward; + } + } + + /// + /// Returns a with components 0, 0, 1. + /// + public static Vector3 Backward + { + get + { + return backward; + } + } + + #endregion + + #region Internal Properties + + internal string DebugDisplayString + { + get + { + return string.Concat( + X.ToString(), " ", + Y.ToString(), " ", + Z.ToString() + ); + } + } + + #endregion + + #region Private Static Fields + + private static Vector3 zero = new Vector3(0f, 0f, 0f); // Not readonly for performance -flibit + private static readonly Vector3 one = new Vector3(1f, 1f, 1f); + private static readonly Vector3 unitX = new Vector3(1f, 0f, 0f); + private static readonly Vector3 unitY = new Vector3(0f, 1f, 0f); + private static readonly Vector3 unitZ = new Vector3(0f, 0f, 1f); + private static readonly Vector3 up = new Vector3(0f, 1f, 0f); + private static readonly Vector3 down = new Vector3(0f, -1f, 0f); + private static readonly Vector3 right = new Vector3(1f, 0f, 0f); + private static readonly Vector3 left = new Vector3(-1f, 0f, 0f); + private static readonly Vector3 forward = new Vector3(0f, 0f, -1f); + private static readonly Vector3 backward = new Vector3(0f, 0f, 1f); + + #endregion + + #region Public Fields + + /// + /// The x coordinate of this . + /// + public float X; + + /// + /// The y coordinate of this . + /// + public float Y; + + /// + /// The z coordinate of this . + /// + public float Z; + + #endregion + + #region Public Constructors + + /// + /// Constructs a 3d vector with X, Y and Z from three values. + /// + /// The x coordinate in 3d-space. + /// The y coordinate in 3d-space. + /// The z coordinate in 3d-space. + public Vector3(float x, float y, float z) + { + this.X = x; + this.Y = y; + this.Z = z; + } + + /// + /// Constructs a 3d vector with X, Y and Z set to the same value. + /// + /// The x, y and z coordinates in 3d-space. + public Vector3(float value) + { + this.X = value; + this.Y = value; + this.Z = value; + } + + /// + /// Constructs a 3d vector with X, Y from and Z from a scalar. + /// + /// The x and y coordinates in 3d-space. + /// The z coordinate in 3d-space. + public Vector3(Vector2 value, float z) + { + this.X = value.X; + this.Y = value.Y; + this.Z = z; + } + + #endregion + + #region Public Methods + + /// + /// Compares whether current instance is equal to specified . + /// + /// The to compare. + /// true if the instances are equal; false otherwise. + public override bool Equals(object obj) + { + return (obj is Vector3) && Equals((Vector3) obj); + } + + /// + /// Compares whether current instance is equal to specified . + /// + /// The to compare. + /// true if the instances are equal; false otherwise. + public bool Equals(Vector3 other) + { + return ( X == other.X && + Y == other.Y && + Z == other.Z ); + } + + /// + /// Gets the hash code of this . + /// + /// Hash code of this . + public override int GetHashCode() + { + return X.GetHashCode() + Y.GetHashCode() + Z.GetHashCode(); + } + + /// + /// Returns the length of this . + /// + /// The length of this . + public float Length() + { + return (float) System.Math.Sqrt((X * X) + (Y * Y) + (Z * Z)); + } + + /// + /// Returns the squared length of this . + /// + /// The squared length of this . + public float LengthSquared() + { + return (X * X) + (Y * Y) + (Z * Z); + } + + /// + /// Turns this to a unit vector with the same direction. + /// + public void Normalize() + { + float factor = 1.0f / (float) System.Math.Sqrt( + (X * X) + + (Y * Y) + + (Z * Z) + ); + X *= factor; + Y *= factor; + Z *= factor; + } + + /// + /// Returns a representation of this in the format: + /// {X:[] Y:[] Z:[]} + /// + /// A representation of this . + public override string ToString() + { + StringBuilder sb = new StringBuilder(32); + sb.Append("{X:"); + sb.Append(this.X); + sb.Append(" Y:"); + sb.Append(this.Y); + sb.Append(" Z:"); + sb.Append(this.Z); + sb.Append("}"); + return sb.ToString(); + } + + #endregion + + #region Public Static Methods + + /// + /// Performs vector addition on and . + /// + /// The first vector to add. + /// The second vector to add. + /// The result of the vector addition. + public static Vector3 Add(Vector3 value1, Vector3 value2) + { + value1.X += value2.X; + value1.Y += value2.Y; + value1.Z += value2.Z; + return value1; + } + + /// + /// Performs vector addition on and + /// , storing the result of the + /// addition in . + /// + /// The first vector to add. + /// The second vector to add. + /// The result of the vector addition. + public static void Add(ref Vector3 value1, ref Vector3 value2, out Vector3 result) + { + result.X = value1.X + value2.X; + result.Y = value1.Y + value2.Y; + result.Z = value1.Z + value2.Z; + } + + /// + /// Creates a new that contains the cartesian coordinates of a vector specified in barycentric coordinates and relative to 3d-triangle. + /// + /// The first vector of 3d-triangle. + /// The second vector of 3d-triangle. + /// The third vector of 3d-triangle. + /// Barycentric scalar b2 which represents a weighting factor towards second vector of 3d-triangle. + /// Barycentric scalar b3 which represents a weighting factor towards third vector of 3d-triangle. + /// The cartesian translation of barycentric coordinates. + public static Vector3 Barycentric( + Vector3 value1, + Vector3 value2, + Vector3 value3, + float amount1, + float amount2 + ) { + return new Vector3( + MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2), + MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2), + MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2) + ); + } + + /// + /// Creates a new that contains the cartesian coordinates of a vector specified in barycentric coordinates and relative to 3d-triangle. + /// + /// The first vector of 3d-triangle. + /// The second vector of 3d-triangle. + /// The third vector of 3d-triangle. + /// Barycentric scalar b2 which represents a weighting factor towards second vector of 3d-triangle. + /// Barycentric scalar b3 which represents a weighting factor towards third vector of 3d-triangle. + /// The cartesian translation of barycentric coordinates as an output parameter. + public static void Barycentric( + ref Vector3 value1, + ref Vector3 value2, + ref Vector3 value3, + float amount1, + float amount2, + out Vector3 result + ) { + result.X = MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2); + result.Y = MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2); + result.Z = MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2); + } + + /// + /// Creates a new that contains CatmullRom interpolation of the specified vectors. + /// + /// The first vector in interpolation. + /// The second vector in interpolation. + /// The third vector in interpolation. + /// The fourth vector in interpolation. + /// Weighting factor. + /// The result of CatmullRom interpolation. + public static Vector3 CatmullRom( + Vector3 value1, + Vector3 value2, + Vector3 value3, + Vector3 value4, + float amount + ) { + return new Vector3( + MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), + MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount), + MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount) + ); + } + + /// + /// Creates a new that contains CatmullRom interpolation of the specified vectors. + /// + /// The first vector in interpolation. + /// The second vector in interpolation. + /// The third vector in interpolation. + /// The fourth vector in interpolation. + /// Weighting factor. + /// The result of CatmullRom interpolation as an output parameter. + public static void CatmullRom( + ref Vector3 value1, + ref Vector3 value2, + ref Vector3 value3, + ref Vector3 value4, + float amount, + out Vector3 result + ) { + result.X = MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount); + result.Y = MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount); + result.Z = MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount); + } + + /// + /// Clamps the specified value within a range. + /// + /// The value to clamp. + /// The min value. + /// The max value. + /// The clamped value. + public static Vector3 Clamp(Vector3 value1, Vector3 min, Vector3 max) + { + return new Vector3( + MathHelper.Clamp(value1.X, min.X, max.X), + MathHelper.Clamp(value1.Y, min.Y, max.Y), + MathHelper.Clamp(value1.Z, min.Z, max.Z) + ); + } + + /// + /// Clamps the specified value within a range. + /// + /// The value to clamp. + /// The min value. + /// The max value. + /// The clamped value as an output parameter. + public static void Clamp( + ref Vector3 value1, + ref Vector3 min, + ref Vector3 max, + out Vector3 result + ) { + result.X = MathHelper.Clamp(value1.X, min.X, max.X); + result.Y = MathHelper.Clamp(value1.Y, min.Y, max.Y); + result.Z = MathHelper.Clamp(value1.Z, min.Z, max.Z); + } + + /// + /// Computes the cross product of two vectors. + /// + /// The first vector. + /// The second vector. + /// The cross product of two vectors. + public static Vector3 Cross(Vector3 vector1, Vector3 vector2) + { + Cross(ref vector1, ref vector2, out vector1); + return vector1; + } + + /// + /// Computes the cross product of two vectors. + /// + /// The first vector. + /// The second vector. + /// The cross product of two vectors as an output parameter. + public static void Cross(ref Vector3 vector1, ref Vector3 vector2, out Vector3 result) + { + float x = vector1.Y * vector2.Z - vector2.Y * vector1.Z; + float y = -(vector1.X * vector2.Z - vector2.X * vector1.Z); + float z = vector1.X * vector2.Y - vector2.X * vector1.Y; + result.X = x; + result.Y = y; + result.Z = z; + } + + /// + /// Returns the distance between two vectors. + /// + /// The first vector. + /// The second vector. + /// The distance between two vectors. + public static float Distance(Vector3 vector1, Vector3 vector2) + { + float result; + DistanceSquared(ref vector1, ref vector2, out result); + return (float) System.Math.Sqrt(result); + } + + /// + /// Returns the distance between two vectors. + /// + /// The first vector. + /// The second vector. + /// The distance between two vectors as an output parameter. + public static void Distance(ref Vector3 value1, ref Vector3 value2, out float result) + { + DistanceSquared(ref value1, ref value2, out result); + result = (float) System.Math.Sqrt(result); + } + + /// + /// Returns the squared distance between two vectors. + /// + /// The first vector. + /// The second vector. + /// The squared distance between two vectors. + public static float DistanceSquared(Vector3 value1, Vector3 value2) + { + return ( + (value1.X - value2.X) * (value1.X - value2.X) + + (value1.Y - value2.Y) * (value1.Y - value2.Y) + + (value1.Z - value2.Z) * (value1.Z - value2.Z) + ); + } + + /// + /// Returns the squared distance between two vectors. + /// + /// The first vector. + /// The second vector. + /// The squared distance between two vectors as an output parameter. + public static void DistanceSquared( + ref Vector3 value1, + ref Vector3 value2, + out float result + ) { + result = ( + (value1.X - value2.X) * (value1.X - value2.X) + + (value1.Y - value2.Y) * (value1.Y - value2.Y) + + (value1.Z - value2.Z) * (value1.Z - value2.Z) + ); + } + + /// + /// Divides the components of a by the components of another . + /// + /// Source . + /// Divisor . + /// The result of dividing the vectors. + public static Vector3 Divide(Vector3 value1, Vector3 value2) + { + value1.X /= value2.X; + value1.Y /= value2.Y; + value1.Z /= value2.Z; + return value1; + } + + /// + /// Divides the components of a by the components of another . + /// + /// Source . + /// Divisor . + /// The result of dividing the vectors as an output parameter. + public static void Divide(ref Vector3 value1, ref Vector3 value2, out Vector3 result) + { + result.X = value1.X / value2.X; + result.Y = value1.Y / value2.Y; + result.Z = value1.Z / value2.Z; + } + + /// + /// Divides the components of a by a scalar. + /// + /// Source . + /// Divisor scalar. + /// The result of dividing a vector by a scalar. + public static Vector3 Divide(Vector3 value1, float value2) + { + float factor = 1 / value2; + value1.X *= factor; + value1.Y *= factor; + value1.Z *= factor; + return value1; + } + + /// + /// Divides the components of a by a scalar. + /// + /// Source . + /// Divisor scalar. + /// The result of dividing a vector by a scalar as an output parameter. + public static void Divide(ref Vector3 value1, float value2, out Vector3 result) + { + float factor = 1 / value2; + result.X = value1.X * factor; + result.Y = value1.Y * factor; + result.Z = value1.Z * factor; + } + + /// + /// Returns a dot product of two vectors. + /// + /// The first vector. + /// The second vector. + /// The dot product of two vectors. + public static float Dot(Vector3 vector1, Vector3 vector2) + { + return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z; + } + + /// + /// Returns a dot product of two vectors. + /// + /// The first vector. + /// The second vector. + /// The dot product of two vectors as an output parameter. + public static void Dot(ref Vector3 vector1, ref Vector3 vector2, out float result) + { + result = ( + (vector1.X * vector2.X) + + (vector1.Y * vector2.Y) + + (vector1.Z * vector2.Z) + ); + } + + /// + /// Creates a new that contains hermite spline interpolation. + /// + /// The first position vector. + /// The first tangent vector. + /// The second position vector. + /// The second tangent vector. + /// Weighting factor. + /// The hermite spline interpolation vector. + public static Vector3 Hermite( + Vector3 value1, + Vector3 tangent1, + Vector3 value2, + Vector3 tangent2, + float amount + ) { + Vector3 result = new Vector3(); + Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result); + return result; + } + + /// + /// Creates a new that contains hermite spline interpolation. + /// + /// The first position vector. + /// The first tangent vector. + /// The second position vector. + /// The second tangent vector. + /// Weighting factor. + /// The hermite spline interpolation vector as an output parameter. + public static void Hermite( + ref Vector3 value1, + ref Vector3 tangent1, + ref Vector3 value2, + ref Vector3 tangent2, + float amount, + out Vector3 result + ) { + result.X = MathHelper.Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount); + result.Y = MathHelper.Hermite(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount); + result.Z = MathHelper.Hermite(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount); + } + + /// + /// Creates a new that contains linear interpolation of the specified vectors. + /// + /// The first vector. + /// The second vector. + /// Weighting value(between 0.0 and 1.0). + /// The result of linear interpolation of the specified vectors. + public static Vector3 Lerp(Vector3 value1, Vector3 value2, float amount) + { + return new Vector3( + MathHelper.Lerp(value1.X, value2.X, amount), + MathHelper.Lerp(value1.Y, value2.Y, amount), + MathHelper.Lerp(value1.Z, value2.Z, amount) + ); + } + + /// + /// Creates a new that contains linear interpolation of the specified vectors. + /// + /// The first vector. + /// The second vector. + /// Weighting value(between 0.0 and 1.0). + /// The result of linear interpolation of the specified vectors as an output parameter. + public static void Lerp( + ref Vector3 value1, + ref Vector3 value2, + float amount, + out Vector3 result + ) { + result.X = MathHelper.Lerp(value1.X, value2.X, amount); + result.Y = MathHelper.Lerp(value1.Y, value2.Y, amount); + result.Z = MathHelper.Lerp(value1.Z, value2.Z, amount); + } + + /// + /// Creates a new that contains a maximal values from the two vectors. + /// + /// The first vector. + /// The second vector. + /// The with maximal values from the two vectors. + public static Vector3 Max(Vector3 value1, Vector3 value2) + { + return new Vector3( + MathHelper.Max(value1.X, value2.X), + MathHelper.Max(value1.Y, value2.Y), + MathHelper.Max(value1.Z, value2.Z) + ); + } + + /// + /// Creates a new that contains a maximal values from the two vectors. + /// + /// The first vector. + /// The second vector. + /// The with maximal values from the two vectors as an output parameter. + public static void Max(ref Vector3 value1, ref Vector3 value2, out Vector3 result) + { + result.X = MathHelper.Max(value1.X, value2.X); + result.Y = MathHelper.Max(value1.Y, value2.Y); + result.Z = MathHelper.Max(value1.Z, value2.Z); + } + + /// + /// Creates a new that contains a minimal values from the two vectors. + /// + /// The first vector. + /// The second vector. + /// The with minimal values from the two vectors. + public static Vector3 Min(Vector3 value1, Vector3 value2) + { + return new Vector3( + MathHelper.Min(value1.X, value2.X), + MathHelper.Min(value1.Y, value2.Y), + MathHelper.Min(value1.Z, value2.Z) + ); + } + + /// + /// Creates a new that contains a minimal values from the two vectors. + /// + /// The first vector. + /// The second vector. + /// The with minimal values from the two vectors as an output parameter. + public static void Min(ref Vector3 value1, ref Vector3 value2, out Vector3 result) + { + result.X = MathHelper.Min(value1.X, value2.X); + result.Y = MathHelper.Min(value1.Y, value2.Y); + result.Z = MathHelper.Min(value1.Z, value2.Z); + } + + /// + /// Creates a new that contains a multiplication of two vectors. + /// + /// Source . + /// Source . + /// The result of the vector multiplication. + public static Vector3 Multiply(Vector3 value1, Vector3 value2) + { + value1.X *= value2.X; + value1.Y *= value2.Y; + value1.Z *= value2.Z; + return value1; + } + + /// + /// Creates a new that contains a multiplication of and a scalar. + /// + /// Source . + /// Scalar value. + /// The result of the vector multiplication with a scalar. + public static Vector3 Multiply(Vector3 value1, float scaleFactor) + { + value1.X *= scaleFactor; + value1.Y *= scaleFactor; + value1.Z *= scaleFactor; + return value1; + } + + /// + /// Creates a new that contains a multiplication of and a scalar. + /// + /// Source . + /// Scalar value. + /// The result of the multiplication with a scalar as an output parameter. + public static void Multiply(ref Vector3 value1, float scaleFactor, out Vector3 result) + { + result.X = value1.X * scaleFactor; + result.Y = value1.Y * scaleFactor; + result.Z = value1.Z * scaleFactor; + } + + /// + /// Creates a new that contains a multiplication of two vectors. + /// + /// Source . + /// Source . + /// The result of the vector multiplication as an output parameter. + public static void Multiply(ref Vector3 value1, ref Vector3 value2, out Vector3 result) + { + result.X = value1.X * value2.X; + result.Y = value1.Y * value2.Y; + result.Z = value1.Z * value2.Z; + } + + /// + /// Creates a new that contains the specified vector inversion. + /// + /// Source . + /// The result of the vector inversion. + public static Vector3 Negate(Vector3 value) + { + value = new Vector3(-value.X, -value.Y, -value.Z); + return value; + } + + /// + /// Creates a new that contains the specified vector inversion. + /// + /// Source . + /// The result of the vector inversion as an output parameter. + public static void Negate(ref Vector3 value, out Vector3 result) + { + result.X = -value.X; + result.Y = -value.Y; + result.Z = -value.Z; + } + + /// + /// Creates a new that contains a normalized values from another vector. + /// + /// Source . + /// Unit vector. + public static Vector3 Normalize(Vector3 value) + { + float factor = 1.0f / (float) System.Math.Sqrt( + (value.X * value.X) + + (value.Y * value.Y) + + (value.Z * value.Z) + ); + return new Vector3( + value.X * factor, + value.Y * factor, + value.Z * factor + ); + } + + /// + /// Creates a new that contains a normalized values from another vector. + /// + /// Source . + /// Unit vector as an output parameter. + public static void Normalize(ref Vector3 value, out Vector3 result) + { + float factor = 1.0f / (float) System.Math.Sqrt( + (value.X * value.X) + + (value.Y * value.Y) + + (value.Z * value.Z) + ); + result.X = value.X * factor; + result.Y = value.Y * factor; + result.Z = value.Z * factor; + } + + /// + /// Creates a new that contains reflect vector of the given vector and normal. + /// + /// Source . + /// Reflection normal. + /// Reflected vector. + public static Vector3 Reflect(Vector3 vector, Vector3 normal) + { + /* I is the original array. + * N is the normal of the incident plane. + * R = I - (2 * N * ( DotProduct[ I,N] )) + */ + Vector3 reflectedVector; + // Inline the dotProduct here instead of calling method + float dotProduct = ((vector.X * normal.X) + (vector.Y * normal.Y)) + + (vector.Z * normal.Z); + reflectedVector.X = vector.X - (2.0f * normal.X) * dotProduct; + reflectedVector.Y = vector.Y - (2.0f * normal.Y) * dotProduct; + reflectedVector.Z = vector.Z - (2.0f * normal.Z) * dotProduct; + + return reflectedVector; + } + + /// + /// Creates a new that contains reflect vector of the given vector and normal. + /// + /// Source . + /// Reflection normal. + /// Reflected vector as an output parameter. + public static void Reflect(ref Vector3 vector, ref Vector3 normal, out Vector3 result) + { + /* I is the original array. + * N is the normal of the incident plane. + * R = I - (2 * N * ( DotProduct[ I,N] )) + */ + + // Inline the dotProduct here instead of calling method. + float dotProduct = ((vector.X * normal.X) + (vector.Y * normal.Y)) + + (vector.Z * normal.Z); + result.X = vector.X - (2.0f * normal.X) * dotProduct; + result.Y = vector.Y - (2.0f * normal.Y) * dotProduct; + result.Z = vector.Z - (2.0f * normal.Z) * dotProduct; + + } + + /// + /// Creates a new that contains cubic interpolation of the specified vectors. + /// + /// Source . + /// Source . + /// Weighting value. + /// Cubic interpolation of the specified vectors. + public static Vector3 SmoothStep(Vector3 value1, Vector3 value2, float amount) + { + return new Vector3( + MathHelper.SmoothStep(value1.X, value2.X, amount), + MathHelper.SmoothStep(value1.Y, value2.Y, amount), + MathHelper.SmoothStep(value1.Z, value2.Z, amount) + ); + } + + /// + /// Creates a new that contains cubic interpolation of the specified vectors. + /// + /// Source . + /// Source . + /// Weighting value. + /// Cubic interpolation of the specified vectors as an output parameter. + public static void SmoothStep( + ref Vector3 value1, + ref Vector3 value2, + float amount, + out Vector3 result + ) { + result.X = MathHelper.SmoothStep(value1.X, value2.X, amount); + result.Y = MathHelper.SmoothStep(value1.Y, value2.Y, amount); + result.Z = MathHelper.SmoothStep(value1.Z, value2.Z, amount); + } + + /// + /// Creates a new that contains subtraction of on from a another. + /// + /// Source . + /// Source . + /// The result of the vector subtraction. + public static Vector3 Subtract(Vector3 value1, Vector3 value2) + { + value1.X -= value2.X; + value1.Y -= value2.Y; + value1.Z -= value2.Z; + return value1; + } + + /// + /// Creates a new that contains subtraction of on from a another. + /// + /// Source . + /// Source . + /// The result of the vector subtraction as an output parameter. + public static void Subtract(ref Vector3 value1, ref Vector3 value2, out Vector3 result) + { + result.X = value1.X - value2.X; + result.Y = value1.Y - value2.Y; + result.Z = value1.Z - value2.Z; + } + + /// + /// Creates a new that contains a transformation of 3d-vector by the specified . + /// + /// Source . + /// The transformation . + /// Transformed . + public static Vector3 Transform(Vector3 position, Matrix matrix) + { + Transform(ref position, ref matrix, out position); + return position; + } + + /// + /// Creates a new that contains a transformation of 3d-vector by the specified . + /// + /// Source . + /// The transformation . + /// Transformed as an output parameter. + public static void Transform( + ref Vector3 position, + ref Matrix matrix, + out Vector3 result + ) { + float x = ( + (position.X * matrix.M11) + + (position.Y * matrix.M21) + + (position.Z * matrix.M31) + + matrix.M41 + ); + float y = ( + (position.X * matrix.M12) + + (position.Y * matrix.M22) + + (position.Z * matrix.M32) + + matrix.M42 + ); + float z = ( + (position.X * matrix.M13) + + (position.Y * matrix.M23) + + (position.Z * matrix.M33) + + matrix.M43 + ); + result.X = x; + result.Y = y; + result.Z = z; + } + + /// + /// Apply transformation on all vectors within array of by the specified and places the results in an another array. + /// + /// Source array. + /// The transformation . + /// Destination array. + public static void Transform( + Vector3[] sourceArray, + ref Matrix matrix, + Vector3[] destinationArray + ) { + Debug.Assert( + destinationArray.Length >= sourceArray.Length, + "The destination array is smaller than the source array." + ); + + /* TODO: Are there options on some platforms to implement + * a vectorized version of this? + */ + + for (int i = 0; i < sourceArray.Length; i += 1) + { + Vector3 position = sourceArray[i]; + destinationArray[i] = new Vector3( + (position.X * matrix.M11) + (position.Y * matrix.M21) + + (position.Z * matrix.M31) + matrix.M41, + (position.X * matrix.M12) + (position.Y * matrix.M22) + + (position.Z * matrix.M32) + matrix.M42, + (position.X * matrix.M13) + (position.Y * matrix.M23) + + (position.Z * matrix.M33) + matrix.M43 + ); + } + } + + /// + /// Apply transformation on vectors within array of by the specified and places the results in an another array. + /// + /// Source array. + /// The starting index of transformation in the source array. + /// The transformation . + /// Destination array. + /// The starting index in the destination array, where the first should be written. + /// The number of vectors to be transformed. + public static void Transform( + Vector3[] sourceArray, + int sourceIndex, + ref Matrix matrix, + Vector3[] destinationArray, + int destinationIndex, + int length + ) { + Debug.Assert( + sourceArray.Length - sourceIndex >= length, + "The source array is too small for the given sourceIndex and length." + ); + Debug.Assert( + destinationArray.Length - destinationIndex >= length, + "The destination array is too small for " + + "the given destinationIndex and length." + ); + + /* TODO: Are there options on some platforms to implement a + * vectorized version of this? + */ + + for (int i = 0; i < length; i += 1) + { + Vector3 position = sourceArray[sourceIndex + i]; + destinationArray[destinationIndex + i] = new Vector3( + (position.X * matrix.M11) + (position.Y * matrix.M21) + + (position.Z * matrix.M31) + matrix.M41, + (position.X * matrix.M12) + (position.Y * matrix.M22) + + (position.Z * matrix.M32) + matrix.M42, + (position.X * matrix.M13) + (position.Y * matrix.M23) + + (position.Z * matrix.M33) + matrix.M43 + ); + } + } + + /// + /// Creates a new that contains a transformation of 3d-vector by the specified , representing the rotation. + /// + /// Source . + /// The which contains rotation transformation. + /// Transformed . + public static Vector3 Transform(Vector3 value, Quaternion rotation) + { + Vector3 result; + Transform(ref value, ref rotation, out result); + return result; + } + + /// + /// Creates a new that contains a transformation of 3d-vector by the specified , representing the rotation. + /// + /// Source . + /// The which contains rotation transformation. + /// Transformed as an output parameter. + public static void Transform( + ref Vector3 value, + ref Quaternion rotation, + out Vector3 result + ) { + float x = 2 * (rotation.Y * value.Z - rotation.Z * value.Y); + float y = 2 * (rotation.Z * value.X - rotation.X * value.Z); + float z = 2 * (rotation.X * value.Y - rotation.Y * value.X); + + result.X = value.X + x * rotation.W + (rotation.Y * z - rotation.Z * y); + result.Y = value.Y + y * rotation.W + (rotation.Z * x - rotation.X * z); + result.Z = value.Z + z * rotation.W + (rotation.X * y - rotation.Y * x); + } + + /// + /// Apply transformation on all vectors within array of by the specified and places the results in an another array. + /// + /// Source array. + /// The which contains rotation transformation. + /// Destination array. + public static void Transform( + Vector3[] sourceArray, + ref Quaternion rotation, + Vector3[] destinationArray + ) { + Debug.Assert( + destinationArray.Length >= sourceArray.Length, + "The destination array is smaller than the source array." + ); + + /* TODO: Are there options on some platforms to implement + * a vectorized version of this? + */ + + for (int i = 0; i < sourceArray.Length; i += 1) + { + Vector3 position = sourceArray[i]; + + float x = 2 * (rotation.Y * position.Z - rotation.Z * position.Y); + float y = 2 * (rotation.Z * position.X - rotation.X * position.Z); + float z = 2 * (rotation.X * position.Y - rotation.Y * position.X); + + destinationArray[i] = new Vector3( + position.X + x * rotation.W + (rotation.Y * z - rotation.Z * y), + position.Y + y * rotation.W + (rotation.Z * x - rotation.X * z), + position.Z + z * rotation.W + (rotation.X * y - rotation.Y * x) + ); + } + } + + /// + + /// Apply transformation on vectors within array of by the specified and places the results in an another array. + /// + /// Source array. + /// The starting index of transformation in the source array. + /// The which contains rotation transformation. + /// Destination array. + /// The starting index in the destination array, where the first should be written. + /// The number of vectors to be transformed. + public static void Transform( + Vector3[] sourceArray, + int sourceIndex, + ref Quaternion rotation, + Vector3[] destinationArray, + int destinationIndex, + int length + ) { + Debug.Assert( + sourceArray.Length - sourceIndex >= length, + "The source array is too small for the given sourceIndex and length." + ); + Debug.Assert( + destinationArray.Length - destinationIndex >= length, + "The destination array is too small for the " + + "given destinationIndex and length." + ); + + /* TODO: Are there options on some platforms to implement + * a vectorized version of this? + */ + + for (int i = 0; i < length; i += 1) + { + Vector3 position = sourceArray[sourceIndex + i]; + + float x = 2 * (rotation.Y * position.Z - rotation.Z * position.Y); + float y = 2 * (rotation.Z * position.X - rotation.X * position.Z); + float z = 2 * (rotation.X * position.Y - rotation.Y * position.X); + + destinationArray[destinationIndex + i] = new Vector3( + position.X + x * rotation.W + (rotation.Y * z - rotation.Z * y), + position.Y + y * rotation.W + (rotation.Z * x - rotation.X * z), + position.Z + z * rotation.W + (rotation.X * y - rotation.Y * x) + ); + } + } + + /// + /// Creates a new that contains a transformation of the specified normal by the specified . + /// + /// Source which represents a normal vector. + /// The transformation . + /// Transformed normal. + public static Vector3 TransformNormal(Vector3 normal, Matrix matrix) + { + TransformNormal(ref normal, ref matrix, out normal); + return normal; + } + + /// + /// Creates a new that contains a transformation of the specified normal by the specified . + /// + /// Source which represents a normal vector. + /// The transformation . + /// Transformed normal as an output parameter. + public static void TransformNormal( + ref Vector3 normal, + ref Matrix matrix, + out Vector3 result + ) { + float x = (normal.X * matrix.M11) + (normal.Y * matrix.M21) + (normal.Z * matrix.M31); + float y = (normal.X * matrix.M12) + (normal.Y * matrix.M22) + (normal.Z * matrix.M32); + float z = (normal.X * matrix.M13) + (normal.Y * matrix.M23) + (normal.Z * matrix.M33); + result.X = x; + result.Y = y; + result.Z = z; + } + + /// + /// Apply transformation on all normals within array of by the specified and places the results in an another array. + /// + /// Source array. + /// The transformation . + /// Destination array. + public static void TransformNormal( + Vector3[] sourceArray, + ref Matrix matrix, + Vector3[] destinationArray + ) { + Debug.Assert( + destinationArray.Length >= sourceArray.Length, + "The destination array is smaller than the source array." + ); + + for (int i = 0; i < sourceArray.Length; i += 1) + { + Vector3 normal = sourceArray[i]; + destinationArray[i].X = (normal.X * matrix.M11) + (normal.Y * matrix.M21) + (normal.Z * matrix.M31); + destinationArray[i].Y = (normal.X * matrix.M12) + (normal.Y * matrix.M22) + (normal.Z * matrix.M32); + destinationArray[i].Z = (normal.X * matrix.M13) + (normal.Y * matrix.M23) + (normal.Z * matrix.M33); + } + } + + /// + /// Apply transformation on normals within array of by the specified and places the results in an another array. + /// + /// Source array. + /// The starting index of transformation in the source array. + /// The transformation . + /// Destination array. + /// The starting index in the destination array, where the first should be written. + /// The number of normals to be transformed. + public static void TransformNormal( + Vector3[] sourceArray, + int sourceIndex, + ref Matrix matrix, + Vector3[] destinationArray, + int destinationIndex, + int length + ) { + if (sourceArray == null) + { + throw new ArgumentNullException("sourceArray"); + } + if (destinationArray == null) + { + throw new ArgumentNullException("destinationArray"); + } + if ((sourceIndex + length) > sourceArray.Length) + { + throw new ArgumentException( + "the combination of sourceIndex and " + + "length was greater than sourceArray.Length" + ); + } + if ((destinationIndex + length) > destinationArray.Length) + { + throw new ArgumentException( + "destinationArray is too small to " + + "contain the result" + ); + } + + for (int i = 0; i < length; i += 1) + { + Vector3 normal = sourceArray[i + sourceIndex]; + destinationArray[i + destinationIndex].X = ( + (normal.X * matrix.M11) + + (normal.Y * matrix.M21) + + (normal.Z * matrix.M31) + ); + destinationArray[i + destinationIndex].Y = ( + (normal.X * matrix.M12) + + (normal.Y * matrix.M22) + + (normal.Z * matrix.M32) + ); + destinationArray[i + destinationIndex].Z = ( + (normal.X * matrix.M13) + + (normal.Y * matrix.M23) + + (normal.Z * matrix.M33) + ); + } + } + + #endregion + + #region Public Static Operators + + /// + /// Compares whether two instances are equal. + /// + /// instance on the left of the equal sign. + /// instance on the right of the equal sign. + /// true if the instances are equal; false otherwise. + public static bool operator ==(Vector3 value1, Vector3 value2) + { + return ( value1.X == value2.X && + value1.Y == value2.Y && + value1.Z == value2.Z ); + } + + /// + /// Compares whether two instances are not equal. + /// + /// instance on the left of the not equal sign. + /// instance on the right of the not equal sign. + /// true if the instances are not equal; false otherwise. + public static bool operator !=(Vector3 value1, Vector3 value2) + { + return !(value1 == value2); + } + + /// + /// Adds two vectors. + /// + /// Source on the left of the add sign. + /// Source on the right of the add sign. + /// Sum of the vectors. + public static Vector3 operator +(Vector3 value1, Vector3 value2) + { + value1.X += value2.X; + value1.Y += value2.Y; + value1.Z += value2.Z; + return value1; + } + + /// + /// Inverts values in the specified . + /// + /// Source on the right of the sub sign. + /// Result of the inversion. + public static Vector3 operator -(Vector3 value) + { + value = new Vector3(-value.X, -value.Y, -value.Z); + return value; + } + + /// + /// Subtracts a from a . + /// + /// Source on the left of the sub sign. + /// Source on the right of the sub sign. + /// Result of the vector subtraction. + public static Vector3 operator -(Vector3 value1, Vector3 value2) + { + value1.X -= value2.X; + value1.Y -= value2.Y; + value1.Z -= value2.Z; + return value1; + } + + /// + /// Multiplies the components of two vectors by each other. + /// + /// Source on the left of the mul sign. + /// Source on the right of the mul sign. + /// Result of the vector multiplication. + public static Vector3 operator *(Vector3 value1, Vector3 value2) + { + value1.X *= value2.X; + value1.Y *= value2.Y; + value1.Z *= value2.Z; + return value1; + } + + /// + /// Multiplies the components of vector by a scalar. + /// + /// Source on the left of the mul sign. + /// Scalar value on the right of the mul sign. + /// Result of the vector multiplication with a scalar. + public static Vector3 operator *(Vector3 value, float scaleFactor) + { + value.X *= scaleFactor; + value.Y *= scaleFactor; + value.Z *= scaleFactor; + return value; + } + + /// + /// Multiplies the components of vector by a scalar. + /// + /// Scalar value on the left of the mul sign. + /// Source on the right of the mul sign. + /// Result of the vector multiplication with a scalar. + public static Vector3 operator *(float scaleFactor, Vector3 value) + { + value.X *= scaleFactor; + value.Y *= scaleFactor; + value.Z *= scaleFactor; + return value; + } + + /// + /// Divides the components of a by the components of another . + /// + /// Source on the left of the div sign. + /// Divisor on the right of the div sign. + /// The result of dividing the vectors. + public static Vector3 operator /(Vector3 value1, Vector3 value2) + { + value1.X /= value2.X; + value1.Y /= value2.Y; + value1.Z /= value2.Z; + return value1; + } + + /// + /// Divides the components 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 vector by a scalar. + public static Vector3 operator /(Vector3 value, float divider) + { + float factor = 1 / divider; + value.X *= factor; + value.Y *= factor; + value.Z *= factor; + return value; + } + + #endregion + } +} diff --git a/src/Math/Vector4.cs b/src/Math/Vector4.cs new file mode 100644 index 0000000..ce869f8 --- /dev/null +++ b/src/Math/Vector4.cs @@ -0,0 +1,1479 @@ +#region License + +/* MoonWorks - Game Development Framework + * Copyright 2021 Evan Hemsley + */ + +/* Derived from code by Ethan Lee (Copyright 2009-2021). + * Released under the Microsoft Public License. + * See fna.LICENSE for details. + + * Derived from code by the Mono.Xna Team (Copyright 2006). + * Released under the MIT License. See monoxna.LICENSE for details. + */ + +#endregion + +#region Using Statements +using System; +using System.Diagnostics; + +#endregion + +namespace MoonWorks.Math +{ + /// + /// Describes a 4D-vector. + /// + [Serializable] + [DebuggerDisplay("{DebugDisplayString,nq}")] + public struct Vector4 : IEquatable + { + #region Public Static Properties + + /// + /// Returns a with components 0, 0, 0, 0. + /// + public static Vector4 Zero + { + get + { + return zero; + } + } + + /// + /// Returns a with components 1, 1, 1, 1. + /// + public static Vector4 One + { + get + { + return unit; + } + } + + /// + /// Returns a with components 1, 0, 0, 0. + /// + public static Vector4 UnitX + { + get + { + return unitX; + } + } + + /// + /// Returns a with components 0, 1, 0, 0. + /// + public static Vector4 UnitY + { + get + { + return unitY; + } + } + + /// + /// Returns a with components 0, 0, 1, 0. + /// + public static Vector4 UnitZ + { + get + { + return unitZ; + } + } + + /// + /// Returns a with components 0, 0, 0, 1. + /// + public static Vector4 UnitW + { + get + { + return unitW; + } + } + + #endregion + + #region Internal Properties + + internal string DebugDisplayString + { + get + { + return string.Concat( + X.ToString(), " ", + Y.ToString(), " ", + Z.ToString(), " ", + W.ToString() + ); + } + } + + #endregion + + #region Public Fields + + /// + /// The x coordinate of this . + /// + public float X; + + /// + /// The y coordinate of this . + /// + public float Y; + + /// + /// The z coordinate of this . + /// + public float Z; + + /// + /// The w coordinate of this . + /// + public float W; + + #endregion + + #region Private Static Fields + + private static Vector4 zero = new Vector4(); // Not readonly for performance -flibit + private static readonly Vector4 unit = new Vector4(1f, 1f, 1f, 1f); + private static readonly Vector4 unitX = new Vector4(1f, 0f, 0f, 0f); + private static readonly Vector4 unitY = new Vector4(0f, 1f, 0f, 0f); + private static readonly Vector4 unitZ = new Vector4(0f, 0f, 1f, 0f); + private static readonly Vector4 unitW = new Vector4(0f, 0f, 0f, 1f); + + #endregion + + #region Public Constructors + + /// + /// Constructs a 3d vector with X, Y, Z and W from four values. + /// + /// The x coordinate in 4d-space. + /// The y coordinate in 4d-space. + /// The z coordinate in 4d-space. + /// The w coordinate in 4d-space. + public Vector4(float x, float y, float z, float w) + { + this.X = x; + this.Y = y; + this.Z = z; + this.W = w; + } + + /// + /// Constructs a 3d vector with X and Z from and Z and W from the scalars. + /// + /// The x and y coordinates in 4d-space. + /// The z coordinate in 4d-space. + /// The w coordinate in 4d-space. + public Vector4(Vector2 value, float z, float w) + { + this.X = value.X; + this.Y = value.Y; + this.Z = z; + this.W = w; + } + + /// + /// Constructs a 3d vector with X, Y, Z from and W from a scalar. + /// + /// The x, y and z coordinates in 4d-space. + /// The w coordinate in 4d-space. + public Vector4(Vector3 value, float w) + { + this.X = value.X; + this.Y = value.Y; + this.Z = value.Z; + this.W = w; + } + + /// + /// Constructs a 4d vector with X, Y, Z and W set to the same value. + /// + /// The x, y, z and w coordinates in 4d-space. + public Vector4(float value) + { + this.X = value; + this.Y = value; + this.Z = value; + this.W = value; + } + + #endregion + + #region Public Methods + + /// + /// Compares whether current instance is equal to specified . + /// + /// The to compare. + /// true if the instances are equal; false otherwise. + public override bool Equals(object obj) + { + return (obj is Vector4) && Equals((Vector4) obj); + } + + /// + /// Compares whether current instance is equal to specified . + /// + /// The to compare. + /// true if the instances are equal; false otherwise. + public bool Equals(Vector4 other) + { + return ( X == other.X && + Y == other.Y && + Z == other.Z && + W == other.W ); + } + + /// + /// Gets the hash code of this . + /// + /// Hash code of this . + public override int GetHashCode() + { + return W.GetHashCode() + X.GetHashCode() + Y.GetHashCode() + Z.GetHashCode(); + } + + /// + /// Returns the length of this . + /// + /// The length of this . + public float Length() + { + return (float) System.Math.Sqrt((X * X) + (Y * Y) + (Z * Z) + (W * W)); + } + + /// + /// Returns the squared length of this . + /// + /// The squared length of this . + public float LengthSquared() + { + return (X * X) + (Y * Y) + (Z * Z) + (W * W); + } + + /// + /// Turns this to a unit vector with the same direction. + /// + public void Normalize() + { + float factor = 1.0f / (float) System.Math.Sqrt( + (X * X) + + (Y * Y) + + (Z * Z) + + (W * W) + ); + X *= factor; + Y *= factor; + Z *= factor; + W *= factor; + } + + public override string ToString() + { + return ( + "{X:" + X.ToString() + + " Y:" + Y.ToString() + + " Z:" + Z.ToString() + + " W:" + W.ToString() + "}" + ); + } + + #endregion + + #region Public Static Methods + + /// + /// Performs vector addition on and . + /// + /// The first vector to add. + /// The second vector to add. + /// The result of the vector addition. + public static Vector4 Add(Vector4 value1, Vector4 value2) + { + value1.W += value2.W; + value1.X += value2.X; + value1.Y += value2.Y; + value1.Z += value2.Z; + return value1; + } + + /// + /// Performs vector addition on and + /// , storing the result of the + /// addition in . + /// + /// The first vector to add. + /// The second vector to add. + /// The result of the vector addition. + public static void Add(ref Vector4 value1, ref Vector4 value2, out Vector4 result) + { + result.W = value1.W + value2.W; + result.X = value1.X + value2.X; + result.Y = value1.Y + value2.Y; + result.Z = value1.Z + value2.Z; + } + + /// + /// Creates a new that contains the cartesian coordinates of a vector specified in barycentric coordinates and relative to 4d-triangle. + /// + /// The first vector of 4d-triangle. + /// The second vector of 4d-triangle. + /// The third vector of 4d-triangle. + /// Barycentric scalar b2 which represents a weighting factor towards second vector of 4d-triangle. + /// Barycentric scalar b3 which represents a weighting factor towards third vector of 4d-triangle. + /// The cartesian translation of barycentric coordinates. + public static Vector4 Barycentric( + Vector4 value1, + Vector4 value2, + Vector4 value3, + float amount1, + float amount2 + ) { + return new Vector4( + MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2), + MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2), + MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2), + MathHelper.Barycentric(value1.W, value2.W, value3.W, amount1, amount2) + ); + } + + /// + /// Creates a new that contains the cartesian coordinates of a vector specified in barycentric coordinates and relative to 4d-triangle. + /// + /// The first vector of 4d-triangle. + /// The second vector of 4d-triangle. + /// The third vector of 4d-triangle. + /// Barycentric scalar b2 which represents a weighting factor towards second vector of 4d-triangle. + /// Barycentric scalar b3 which represents a weighting factor towards third vector of 4d-triangle. + /// The cartesian translation of barycentric coordinates as an output parameter. + public static void Barycentric( + ref Vector4 value1, + ref Vector4 value2, + ref Vector4 value3, + float amount1, + float amount2, + out Vector4 result + ) { + result.X = MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2); + result.Y = MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2); + result.Z = MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2); + result.W = MathHelper.Barycentric(value1.W, value2.W, value3.W, amount1, amount2); + } + + /// + /// Creates a new that contains CatmullRom interpolation of the specified vectors. + /// + /// The first vector in interpolation. + /// The second vector in interpolation. + /// The third vector in interpolation. + /// The fourth vector in interpolation. + /// Weighting factor. + /// The result of CatmullRom interpolation. + public static Vector4 CatmullRom( + Vector4 value1, + Vector4 value2, + Vector4 value3, + Vector4 value4, + float amount + ) { + return new Vector4( + MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount), + MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount), + MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount), + MathHelper.CatmullRom(value1.W, value2.W, value3.W, value4.W, amount) + ); + } + + /// + /// Creates a new that contains CatmullRom interpolation of the specified vectors. + /// + /// The first vector in interpolation. + /// The second vector in interpolation. + /// The third vector in interpolation. + /// The fourth vector in interpolation. + /// Weighting factor. + /// The result of CatmullRom interpolation as an output parameter. + public static void CatmullRom( + ref Vector4 value1, + ref Vector4 value2, + ref Vector4 value3, + ref Vector4 value4, + float amount, + out Vector4 result + ) { + result.X = MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount); + result.Y = MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount); + result.Z = MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount); + result.W = MathHelper.CatmullRom(value1.W, value2.W, value3.W, value4.W, amount); + } + + /// + /// Clamps the specified value within a range. + /// + /// The value to clamp. + /// The min value. + /// The max value. + /// The clamped value. + public static Vector4 Clamp(Vector4 value1, Vector4 min, Vector4 max) + { + return new Vector4( + MathHelper.Clamp(value1.X, min.X, max.X), + MathHelper.Clamp(value1.Y, min.Y, max.Y), + MathHelper.Clamp(value1.Z, min.Z, max.Z), + MathHelper.Clamp(value1.W, min.W, max.W) + ); + } + + /// + /// Clamps the specified value within a range. + /// + /// The value to clamp. + /// The min value. + /// The max value. + /// The clamped value as an output parameter. + public static void Clamp( + ref Vector4 value1, + ref Vector4 min, + ref Vector4 max, + out Vector4 result + ) { + result.X = MathHelper.Clamp(value1.X, min.X, max.X); + result.Y = MathHelper.Clamp(value1.Y, min.Y, max.Y); + result.Z = MathHelper.Clamp(value1.Z, min.Z, max.Z); + result.W = MathHelper.Clamp(value1.W, min.W, max.W); + } + + /// + /// Returns the distance between two vectors. + /// + /// The first vector. + /// The second vector. + /// The distance between two vectors. + public static float Distance(Vector4 value1, Vector4 value2) + { + return (float) System.Math.Sqrt(DistanceSquared(value1, value2)); + } + + /// + /// Returns the distance between two vectors. + /// + /// The first vector. + /// The second vector. + /// The distance between two vectors as an output parameter. + public static void Distance(ref Vector4 value1, ref Vector4 value2, out float result) + { + result = (float) System.Math.Sqrt(DistanceSquared(value1, value2)); + } + + /// + /// Returns the squared distance between two vectors. + /// + /// The first vector. + /// The second vector. + /// The squared distance between two vectors. + public static float DistanceSquared(Vector4 value1, Vector4 value2) + { + return ( + (value1.W - value2.W) * (value1.W - value2.W) + + (value1.X - value2.X) * (value1.X - value2.X) + + (value1.Y - value2.Y) * (value1.Y - value2.Y) + + (value1.Z - value2.Z) * (value1.Z - value2.Z) + ); + } + + /// + /// Returns the squared distance between two vectors. + /// + /// The first vector. + /// The second vector. + /// The squared distance between two vectors as an output parameter. + public static void DistanceSquared( + ref Vector4 value1, + ref Vector4 value2, + out float result + ) { + result = ( + (value1.W - value2.W) * (value1.W - value2.W) + + (value1.X - value2.X) * (value1.X - value2.X) + + (value1.Y - value2.Y) * (value1.Y - value2.Y) + + (value1.Z - value2.Z) * (value1.Z - value2.Z) + ); + } + + /// + /// Divides the components of a by the components of another . + /// + /// Source . + /// Divisor . + /// The result of dividing the vectors. + public static Vector4 Divide(Vector4 value1, Vector4 value2) + { + value1.W /= value2.W; + value1.X /= value2.X; + value1.Y /= value2.Y; + value1.Z /= value2.Z; + return value1; + } + + /// + /// Divides the components of a by a scalar. + /// + /// Source . + /// Divisor scalar. + /// The result of dividing a vector by a scalar. + public static Vector4 Divide(Vector4 value1, float divider) + { + float factor = 1f / divider; + value1.W *= factor; + value1.X *= factor; + value1.Y *= factor; + value1.Z *= factor; + return value1; + } + + /// + /// Divides the components of a by a scalar. + /// + /// Source . + /// Divisor scalar. + /// The result of dividing a vector by a scalar as an output parameter. + public static void Divide(ref Vector4 value1, float divider, out Vector4 result) + { + float factor = 1f / divider; + result.W = value1.W * factor; + result.X = value1.X * factor; + result.Y = value1.Y * factor; + result.Z = value1.Z * factor; + } + + /// + /// Divides the components of a by the components of another . + /// + /// Source . + /// Divisor . + /// The result of dividing the vectors as an output parameter. + public static void Divide( + ref Vector4 value1, + ref Vector4 value2, + out Vector4 result + ) { + result.W = value1.W / value2.W; + result.X = value1.X / value2.X; + result.Y = value1.Y / value2.Y; + result.Z = value1.Z / value2.Z; + } + + /// + /// Returns a dot product of two vectors. + /// + /// The first vector. + /// The second vector. + /// The dot product of two vectors. + public static float Dot(Vector4 vector1, Vector4 vector2) + { + return ( + vector1.X * vector2.X + + vector1.Y * vector2.Y + + vector1.Z * vector2.Z + + vector1.W * vector2.W + ); + } + + /// + /// Returns a dot product of two vectors. + /// + /// The first vector. + /// The second vector. + /// The dot product of two vectors as an output parameter. + public static void Dot(ref Vector4 vector1, ref Vector4 vector2, out float result) + { + result = ( + (vector1.X * vector2.X) + + (vector1.Y * vector2.Y) + + (vector1.Z * vector2.Z) + + (vector1.W * vector2.W) + ); + } + + /// + /// Creates a new that contains hermite spline interpolation. + /// + /// The first position vector. + /// The first tangent vector. + /// The second position vector. + /// The second tangent vector. + /// Weighting factor. + /// The hermite spline interpolation vector. + public static Vector4 Hermite( + Vector4 value1, + Vector4 tangent1, + Vector4 value2, + Vector4 tangent2, + float amount + ) { + return new Vector4( + MathHelper.Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount), + MathHelper.Hermite(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount), + MathHelper.Hermite(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount), + MathHelper.Hermite(value1.W, tangent1.W, value2.W, tangent2.W, amount) + ); + } + + /// + /// Creates a new that contains hermite spline interpolation. + /// + /// The first position vector. + /// The first tangent vector. + /// The second position vector. + /// The second tangent vector. + /// Weighting factor. + /// The hermite spline interpolation vector as an output parameter. + public static void Hermite( + ref Vector4 value1, + ref Vector4 tangent1, + ref Vector4 value2, + ref Vector4 tangent2, + float amount, + out Vector4 result + ) { + result.W = MathHelper.Hermite(value1.W, tangent1.W, value2.W, tangent2.W, amount); + result.X = MathHelper.Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount); + result.Y = MathHelper.Hermite(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount); + result.Z = MathHelper.Hermite(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount); + } + + /// + /// Creates a new that contains linear interpolation of the specified vectors. + /// + /// The first vector. + /// The second vector. + /// Weighting value(between 0.0 and 1.0). + /// The result of linear interpolation of the specified vectors. + public static Vector4 Lerp(Vector4 value1, Vector4 value2, float amount) + { + return new Vector4( + MathHelper.Lerp(value1.X, value2.X, amount), + MathHelper.Lerp(value1.Y, value2.Y, amount), + MathHelper.Lerp(value1.Z, value2.Z, amount), + MathHelper.Lerp(value1.W, value2.W, amount) + ); + } + + /// + /// Creates a new that contains linear interpolation of the specified vectors. + /// + /// The first vector. + /// The second vector. + /// Weighting value(between 0.0 and 1.0). + /// The result of linear interpolation of the specified vectors as an output parameter. + public static void Lerp( + ref Vector4 value1, + ref Vector4 value2, + float amount, + out Vector4 result + ) { + result.X = MathHelper.Lerp(value1.X, value2.X, amount); + result.Y = MathHelper.Lerp(value1.Y, value2.Y, amount); + result.Z = MathHelper.Lerp(value1.Z, value2.Z, amount); + result.W = MathHelper.Lerp(value1.W, value2.W, amount); + } + + /// + /// Creates a new that contains a maximal values from the two vectors. + /// + /// The first vector. + /// The second vector. + /// The with maximal values from the two vectors. + public static Vector4 Max(Vector4 value1, Vector4 value2) + { + return new Vector4( + MathHelper.Max(value1.X, value2.X), + MathHelper.Max(value1.Y, value2.Y), + MathHelper.Max(value1.Z, value2.Z), + MathHelper.Max(value1.W, value2.W) + ); + } + + /// + /// Creates a new that contains a maximal values from the two vectors. + /// + /// The first vector. + /// The second vector. + /// The with maximal values from the two vectors as an output parameter. + public static void Max(ref Vector4 value1, ref Vector4 value2, out Vector4 result) + { + result.X = MathHelper.Max(value1.X, value2.X); + result.Y = MathHelper.Max(value1.Y, value2.Y); + result.Z = MathHelper.Max(value1.Z, value2.Z); + result.W = MathHelper.Max(value1.W, value2.W); + } + + /// + /// Creates a new that contains a minimal values from the two vectors. + /// + /// The first vector. + /// The second vector. + /// The with minimal values from the two vectors. + public static Vector4 Min(Vector4 value1, Vector4 value2) + { + return new Vector4( + MathHelper.Min(value1.X, value2.X), + MathHelper.Min(value1.Y, value2.Y), + MathHelper.Min(value1.Z, value2.Z), + MathHelper.Min(value1.W, value2.W) + ); + } + + /// + /// Creates a new that contains a minimal values from the two vectors. + /// + /// The first vector. + /// The second vector. + /// The with minimal values from the two vectors as an output parameter. + public static void Min(ref Vector4 value1, ref Vector4 value2, out Vector4 result) + { + result.X = MathHelper.Min(value1.X, value2.X); + result.Y = MathHelper.Min(value1.Y, value2.Y); + result.Z = MathHelper.Min(value1.Z, value2.Z); + result.W = MathHelper.Min(value1.W, value2.W); + } + + /// + /// Creates a new that contains a multiplication of two vectors. + /// + /// Source . + /// Source . + /// The result of the vector multiplication. + public static Vector4 Multiply(Vector4 value1, Vector4 value2) + { + value1.W *= value2.W; + value1.X *= value2.X; + value1.Y *= value2.Y; + value1.Z *= value2.Z; + return value1; + } + + /// + /// Creates a new that contains a multiplication of and a scalar. + /// + /// Source . + /// Scalar value. + /// The result of the vector multiplication with a scalar. + public static Vector4 Multiply(Vector4 value1, float scaleFactor) + { + value1.W *= scaleFactor; + value1.X *= scaleFactor; + value1.Y *= scaleFactor; + value1.Z *= scaleFactor; + return value1; + } + + /// + /// Creates a new that contains a multiplication of and a scalar. + /// + /// Source . + /// Scalar value. + /// The result of the multiplication with a scalar as an output parameter. + public static void Multiply(ref Vector4 value1, float scaleFactor, out Vector4 result) + { + result.W = value1.W * scaleFactor; + result.X = value1.X * scaleFactor; + result.Y = value1.Y * scaleFactor; + result.Z = value1.Z * scaleFactor; + } + + /// + /// Creates a new that contains a multiplication of two vectors. + /// + /// Source . + /// Source . + /// The result of the vector multiplication as an output parameter. + public static void Multiply(ref Vector4 value1, ref Vector4 value2, out Vector4 result) + { + result.W = value1.W * value2.W; + result.X = value1.X * value2.X; + result.Y = value1.Y * value2.Y; + result.Z = value1.Z * value2.Z; + } + + /// + /// Creates a new that contains the specified vector inversion. + /// + /// Source . + /// The result of the vector inversion. + public static Vector4 Negate(Vector4 value) + { + value = new Vector4(-value.X, -value.Y, -value.Z, -value.W); + return value; + } + + /// + /// Creates a new that contains the specified vector inversion. + /// + /// Source . + /// The result of the vector inversion as an output parameter. + public static void Negate(ref Vector4 value, out Vector4 result) + { + result.X = -value.X; + result.Y = -value.Y; + result.Z = -value.Z; + result.W = -value.W; + } + + /// + /// Creates a new that contains a normalized values from another vector. + /// + /// Source . + /// Unit vector. + public static Vector4 Normalize(Vector4 vector) + { + float factor = 1.0f / (float) System.Math.Sqrt( + (vector.X * vector.X) + + (vector.Y * vector.Y) + + (vector.Z * vector.Z) + + (vector.W * vector.W) + ); + return new Vector4( + vector.X * factor, + vector.Y * factor, + vector.Z * factor, + vector.W * factor + ); + } + + /// + /// Creates a new that contains a normalized values from another vector. + /// + /// Source . + /// Unit vector as an output parameter. + public static void Normalize(ref Vector4 vector, out Vector4 result) + { + float factor = 1.0f / (float) System.Math.Sqrt( + (vector.X * vector.X) + + (vector.Y * vector.Y) + + (vector.Z * vector.Z) + + (vector.W * vector.W) + ); + result.X = vector.X * factor; + result.Y = vector.Y * factor; + result.Z = vector.Z * factor; + result.W = vector.W * factor; + } + + /// + /// Creates a new that contains cubic interpolation of the specified vectors. + /// + /// Source . + /// Source . + /// Weighting value. + /// Cubic interpolation of the specified vectors. + public static Vector4 SmoothStep(Vector4 value1, Vector4 value2, float amount) + { + return new Vector4( + MathHelper.SmoothStep(value1.X, value2.X, amount), + MathHelper.SmoothStep(value1.Y, value2.Y, amount), + MathHelper.SmoothStep(value1.Z, value2.Z, amount), + MathHelper.SmoothStep(value1.W, value2.W, amount) + ); + } + + /// + /// Creates a new that contains cubic interpolation of the specified vectors. + /// + /// Source . + /// Source . + /// Weighting value. + /// Cubic interpolation of the specified vectors as an output parameter. + public static void SmoothStep( + ref Vector4 value1, + ref Vector4 value2, + float amount, + out Vector4 result + ) { + result.X = MathHelper.SmoothStep(value1.X, value2.X, amount); + result.Y = MathHelper.SmoothStep(value1.Y, value2.Y, amount); + result.Z = MathHelper.SmoothStep(value1.Z, value2.Z, amount); + result.W = MathHelper.SmoothStep(value1.W, value2.W, amount); + } + + /// + /// Creates a new that contains subtraction of on from a another. + /// + /// Source . + /// Source . + /// The result of the vector subtraction. + public static Vector4 Subtract(Vector4 value1, Vector4 value2) + { + value1.W -= value2.W; + value1.X -= value2.X; + value1.Y -= value2.Y; + value1.Z -= value2.Z; + return value1; + } + + /// + /// Creates a new that contains subtraction of on from a another. + /// + /// Source . + /// Source . + /// The result of the vector subtraction as an output parameter. + public static void Subtract(ref Vector4 value1, ref Vector4 value2, out Vector4 result) + { + result.W = value1.W - value2.W; + result.X = value1.X - value2.X; + result.Y = value1.Y - value2.Y; + result.Z = value1.Z - value2.Z; + } + + /// + /// Creates a new that contains a transformation of 2d-vector by the specified . + /// + /// Source . + /// The transformation . + /// Transformed . + public static Vector4 Transform(Vector2 position, Matrix matrix) + { + Vector4 result; + Transform(ref position, ref matrix, out result); + return result; + } + + /// + /// Creates a new that contains a transformation of 3d-vector by the specified . + /// + /// Source . + /// The transformation . + /// Transformed . + public static Vector4 Transform(Vector3 position, Matrix matrix) + { + Vector4 result; + Transform(ref position, ref matrix, out result); + return result; + } + + /// + /// Creates a new that contains a transformation of 4d-vector by the specified . + /// + /// Source . + /// The transformation . + /// Transformed . + public static Vector4 Transform(Vector4 vector, Matrix matrix) + { + Transform(ref vector, ref matrix, out vector); + return vector; + } + + /// + /// Creates a new that contains a transformation of 2d-vector by the specified . + /// + /// Source . + /// The transformation . + /// Transformed as an output parameter. + public static void Transform(ref Vector2 position, ref Matrix matrix, out Vector4 result) + { + result = new Vector4( + (position.X * matrix.M11) + (position.Y * matrix.M21) + matrix.M41, + (position.X * matrix.M12) + (position.Y * matrix.M22) + matrix.M42, + (position.X * matrix.M13) + (position.Y * matrix.M23) + matrix.M43, + (position.X * matrix.M14) + (position.Y * matrix.M24) + matrix.M44 + ); + } + + /// + /// Creates a new that contains a transformation of 3d-vector by the specified . + /// + /// Source . + /// The transformation . + /// Transformed as an output parameter. + public static void Transform(ref Vector3 position, ref Matrix matrix, out Vector4 result) + { + float x = ( + (position.X * matrix.M11) + + (position.Y * matrix.M21) + + (position.Z * matrix.M31) + + matrix.M41 + ); + float y = ( + (position.X * matrix.M12) + + (position.Y * matrix.M22) + + (position.Z * matrix.M32) + + matrix.M42 + ); + float z = ( + (position.X * matrix.M13) + + (position.Y * matrix.M23) + + (position.Z * matrix.M33) + + matrix.M43 + ); + float w = ( + (position.X * matrix.M14) + + (position.Y * matrix.M24) + + (position.Z * matrix.M34) + + matrix.M44 + ); + result.X = x; + result.Y = y; + result.Z = z; + result.W = w; + } + + /// + /// Creates a new that contains a transformation of 4d-vector by the specified . + /// + /// Source . + /// The transformation . + /// Transformed as an output parameter. + public static void Transform(ref Vector4 vector, ref Matrix matrix, out Vector4 result) + { + float x = ( + (vector.X * matrix.M11) + + (vector.Y * matrix.M21) + + (vector.Z * matrix.M31) + + (vector.W * matrix.M41) + ); + float y = ( + (vector.X * matrix.M12) + + (vector.Y * matrix.M22) + + (vector.Z * matrix.M32) + + (vector.W * matrix.M42) + ); + float z = ( + (vector.X * matrix.M13) + + (vector.Y * matrix.M23) + + (vector.Z * matrix.M33) + + (vector.W * matrix.M43) + ); + float w = ( + (vector.X * matrix.M14) + + (vector.Y * matrix.M24) + + (vector.Z * matrix.M34) + + (vector.W * matrix.M44) + ); + result.X = x; + result.Y = y; + result.Z = z; + result.W = w; + } + + /// + /// Apply transformation on all vectors within array of by the specified and places the results in an another array. + /// + /// Source array. + /// The transformation . + /// Destination array. + public static void Transform( + Vector4[] sourceArray, + ref Matrix matrix, + Vector4[] destinationArray + ) { + if (sourceArray == null) + { + throw new ArgumentNullException("sourceArray"); + } + if (destinationArray == null) + { + throw new ArgumentNullException("destinationArray"); + } + if (destinationArray.Length < sourceArray.Length) + { + throw new ArgumentException( + "destinationArray is too small to contain the result." + ); + } + for (int i = 0; i < sourceArray.Length; i += 1) + { + Transform( + ref sourceArray[i], + ref matrix, + out destinationArray[i] + ); + } + } + + /// + /// Apply transformation on vectors within array of by the specified and places the results in an another array. + /// + /// Source array. + /// The starting index of transformation in the source array. + /// The transformation . + /// Destination array. + /// The starting index in the destination array, where the first should be written. + /// The number of vectors to be transformed. + public static void Transform( + Vector4[] sourceArray, + int sourceIndex, + ref Matrix matrix, + Vector4[] destinationArray, + int destinationIndex, + int length + ) { + if (sourceArray == null) + { + throw new ArgumentNullException("sourceArray"); + } + if (destinationArray == null) + { + throw new ArgumentNullException("destinationArray"); + } + if (destinationIndex + length > destinationArray.Length) + { + throw new ArgumentException( + "destinationArray is too small to contain the result." + ); + } + if (sourceIndex + length > sourceArray.Length) + { + throw new ArgumentException( + "The combination of sourceIndex and length was greater than sourceArray.Length." + ); + } + for (int i = 0; i < length; i += 1) + { + Transform( + ref sourceArray[i + sourceIndex], + ref matrix, + out destinationArray[i + destinationIndex] + ); + } + } + + /// + /// Creates a new that contains a transformation of 2d-vector by the specified . + /// + /// Source . + /// The which contains rotation transformation. + /// Transformed . + public static Vector4 Transform(Vector2 value, Quaternion rotation) + { + Vector4 result; + Transform(ref value, ref rotation, out result); + return result; + } + + /// + /// Creates a new that contains a transformation of 3d-vector by the specified . + /// + /// Source . + /// The which contains rotation transformation. + /// Transformed . + public static Vector4 Transform(Vector3 value, Quaternion rotation) + { + Vector4 result; + Transform(ref value, ref rotation, out result); + return result; + } + + /// + /// Creates a new that contains a transformation of 4d-vector by the specified . + /// + /// Source . + /// The which contains rotation transformation. + /// Transformed . + public static Vector4 Transform(Vector4 value, Quaternion rotation) + { + Vector4 result; + Transform(ref value, ref rotation, out result); + return result; + } + + /// + /// Creates a new that contains a transformation of 2d-vector by the specified . + /// + /// Source . + /// The which contains rotation transformation. + /// Transformed as an output parameter. + public static void Transform( + ref Vector2 value, + ref Quaternion rotation, + out Vector4 result + ) { + double xx = rotation.X + rotation.X; + double yy = rotation.Y + rotation.Y; + double zz = rotation.Z + rotation.Z; + double wxx = rotation.W * xx; + double wyy = rotation.W * yy; + double wzz = rotation.W * zz; + double xxx = rotation.X * xx; + double xyy = rotation.X * yy; + double xzz = rotation.X * zz; + double yyy = rotation.Y * yy; + double yzz = rotation.Y * zz; + double zzz = rotation.Z * zz; + result.X = (float) ( + (double) value.X * (1.0 - yyy - zzz) + + (double) value.Y * (xyy - wzz) + ); + result.Y = (float) ( + (double) value.X * (xyy + wzz) + + (double) value.Y * (1.0 - xxx - zzz) + ); + result.Z = (float) ( + (double) value.X * (xzz - wyy) + + (double) value.Y * (yzz + wxx) + ); + result.W = 1.0f; + } + + /// + /// Creates a new that contains a transformation of 3d-vector by the specified . + /// + /// Source . + /// The which contains rotation transformation. + /// Transformed as an output parameter. + public static void Transform( + ref Vector3 value, + ref Quaternion rotation, + out Vector4 result + ) { + double xx = rotation.X + rotation.X; + double yy = rotation.Y + rotation.Y; + double zz = rotation.Z + rotation.Z; + double wxx = rotation.W * xx; + double wyy = rotation.W * yy; + double wzz = rotation.W * zz; + double xxx = rotation.X * xx; + double xyy = rotation.X * yy; + double xzz = rotation.X * zz; + double yyy = rotation.Y * yy; + double yzz = rotation.Y * zz; + double zzz = rotation.Z * zz; + result.X = (float) ( + (double) value.X * (1.0 - yyy - zzz) + + (double) value.Y * (xyy - wzz) + + (double) value.Z * (xzz + wyy) + ); + result.Y = (float) ( + (double) value.X * (xyy + wzz) + + (double) value.Y * (1.0 - xxx - zzz) + + (double) value.Z * (yzz - wxx) + ); + result.Z = (float) ( + (double) value.X * (xzz - wyy) + + (double) value.Y * (yzz + wxx) + + (double) value.Z * (1.0 - xxx - yyy) + ); + result.W = 1.0f; + } + + /// + /// Creates a new that contains a transformation of 4d-vector by the specified . + /// + /// Source . + /// The which contains rotation transformation. + /// Transformed as an output parameter. + public static void Transform( + ref Vector4 value, + ref Quaternion rotation, + out Vector4 result + ) { + double xx = rotation.X + rotation.X; + double yy = rotation.Y + rotation.Y; + double zz = rotation.Z + rotation.Z; + double wxx = rotation.W * xx; + double wyy = rotation.W * yy; + double wzz = rotation.W * zz; + double xxx = rotation.X * xx; + double xyy = rotation.X * yy; + double xzz = rotation.X * zz; + double yyy = rotation.Y * yy; + double yzz = rotation.Y * zz; + double zzz = rotation.Z * zz; + result.X = (float) ( + (double) value.X * (1.0 - yyy - zzz) + + (double) value.Y * (xyy - wzz) + + (double) value.Z * (xzz + wyy) + ); + result.Y = (float) ( + (double) value.X * (xyy + wzz) + + (double) value.Y * (1.0 - xxx - zzz) + + (double) value.Z * (yzz - wxx) + ); + result.Z = (float) ( + (double) value.X * (xzz - wyy) + + (double) value.Y * (yzz + wxx) + + (double) value.Z * (1.0 - xxx - yyy) + ); + result.W = value.W; + } + + /// + /// Apply transformation on all vectors within array of by the specified and places the results in an another array. + /// + /// Source array. + /// The which contains rotation transformation. + /// Destination array. + public static void Transform( + Vector4[] sourceArray, + ref Quaternion rotation, + Vector4[] destinationArray + ) { + if (sourceArray == null) + { + throw new ArgumentException("sourceArray"); + } + if (destinationArray == null) + { + throw new ArgumentException("destinationArray"); + } + if (destinationArray.Length < sourceArray.Length) + { + throw new ArgumentException( + "destinationArray is too small to contain the result." + ); + } + for (int i = 0; i < sourceArray.Length; i += 1) + { + Transform( + ref sourceArray[i], + ref rotation, + out destinationArray[i] + ); + } + } + + /// + /// Apply transformation on vectors within array of by the specified and places the results in an another array. + /// + /// Source array. + /// The starting index of transformation in the source array. + /// The which contains rotation transformation. + /// Destination array. + /// The starting index in the destination array, where the first should be written. + /// The number of vectors to be transformed. + public static void Transform( + Vector4[] sourceArray, + int sourceIndex, + ref Quaternion rotation, + Vector4[] destinationArray, + int destinationIndex, + int length + ) { + if (sourceArray == null) + { + throw new ArgumentException("sourceArray"); + } + if (destinationArray == null) + { + throw new ArgumentException("destinationArray"); + } + if (destinationIndex + length > destinationArray.Length) + { + throw new ArgumentException( + "destinationArray is too small to contain the result." + ); + } + if (sourceIndex + length > sourceArray.Length) + { + throw new ArgumentException( + "The combination of sourceIndex and length was greater than sourceArray.Length." + ); + } + for (int i = 0; i < length; i += 1) + { + Transform( + ref sourceArray[i + sourceIndex], + ref rotation, + out destinationArray[i + destinationIndex] + ); + } + } + + #endregion + + #region Public Static Operators + + public static Vector4 operator -(Vector4 value) + { + return new Vector4(-value.X, -value.Y, -value.Z, -value.W); + } + + public static bool operator ==(Vector4 value1, Vector4 value2) + { + return ( value1.X == value2.X && + value1.Y == value2.Y && + value1.Z == value2.Z && + value1.W == value2.W ); + } + + public static bool operator !=(Vector4 value1, Vector4 value2) + { + return !(value1 == value2); + } + + public static Vector4 operator +(Vector4 value1, Vector4 value2) + { + value1.W += value2.W; + value1.X += value2.X; + value1.Y += value2.Y; + value1.Z += value2.Z; + return value1; + } + + public static Vector4 operator -(Vector4 value1, Vector4 value2) + { + value1.W -= value2.W; + value1.X -= value2.X; + value1.Y -= value2.Y; + value1.Z -= value2.Z; + return value1; + } + + public static Vector4 operator *(Vector4 value1, Vector4 value2) + { + value1.W *= value2.W; + value1.X *= value2.X; + value1.Y *= value2.Y; + value1.Z *= value2.Z; + return value1; + } + + public static Vector4 operator *(Vector4 value1, float scaleFactor) + { + value1.W *= scaleFactor; + value1.X *= scaleFactor; + value1.Y *= scaleFactor; + value1.Z *= scaleFactor; + return value1; + } + + public static Vector4 operator *(float scaleFactor, Vector4 value1) + { + value1.W *= scaleFactor; + value1.X *= scaleFactor; + value1.Y *= scaleFactor; + value1.Z *= scaleFactor; + return value1; + } + + public static Vector4 operator /(Vector4 value1, Vector4 value2) + { + value1.W /= value2.W; + value1.X /= value2.X; + value1.Y /= value2.Y; + value1.Z /= value2.Z; + return value1; + } + + public static Vector4 operator /(Vector4 value1, float divider) + { + float factor = 1f / divider; + value1.W *= factor; + value1.X *= factor; + value1.Y *= factor; + value1.Z *= factor; + return value1; + } + + #endregion + } +}