diff --git a/Math3D/Extensions/FloatExtensions.cs b/Math3D/Extensions/FloatExtensions.cs new file mode 100644 index 0000000..c367dc1 --- /dev/null +++ b/Math3D/Extensions/FloatExtensions.cs @@ -0,0 +1,20 @@ +// © XIV-Tools. +// Licensed under the MIT license. + +namespace XivToolsWpf.Math3D.Extensions; + +using System; +using System.Runtime.CompilerServices; + +public static class FloatExtensions +{ + /// + /// Determines whether two floating-point numbers are approximately equal within a specified error margin. + /// + /// The first floating-point number. + /// The second floating-point number. + /// The acceptable error margin for the comparison. + /// True if the absolute difference between the two numbers is less than the error margin; otherwise, false. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static bool IsApproximately(this float a, float b, float errorMargin) => MathF.Abs(a - b) < errorMargin; +} diff --git a/Math3D/Extensions/Matrix4x4Extensions.cs b/Math3D/Extensions/Matrix4x4Extensions.cs new file mode 100644 index 0000000..6bf306a --- /dev/null +++ b/Math3D/Extensions/Matrix4x4Extensions.cs @@ -0,0 +1,88 @@ +// © XIV-Tools. +// Licensed under the MIT license. + +namespace XivToolsWpf.Math3D.Extensions; + +using System.Numerics; +using System.Runtime.CompilerServices; +using System.Windows.Media.Media3D; + +public static class Matrix4x4Extensions +{ + /// + /// Determines whether two matrices are approximately equal within a specified error margin. + /// + /// + /// This method is useful for comparing matrices when minor floating-point inaccuracies are + /// expected. It performs an element-wise comparison using the specified error margin. + /// + /// The first matrix to compare. + /// The second matrix to compare. + /// + /// The maximum allowed difference between corresponding elements for the matrices to be + /// considered approximately equal. Must be greater than or equal to 0. The default is 0.0001. + /// + /// + /// true if all corresponding elements of the matrices differ by no more than the + /// specified error margin; otherwise, false. + /// + public static bool IsApproximately(this Matrix4x4 lhs, Matrix4x4 rhs, float errorMargin = 0.0001f) + { + return lhs.M11.IsApproximately(rhs.M11, errorMargin) + && lhs.M12.IsApproximately(rhs.M12, errorMargin) + && lhs.M13.IsApproximately(rhs.M13, errorMargin) + && lhs.M14.IsApproximately(rhs.M14, errorMargin) + && lhs.M21.IsApproximately(rhs.M21, errorMargin) + && lhs.M22.IsApproximately(rhs.M22, errorMargin) + && lhs.M23.IsApproximately(rhs.M23, errorMargin) + && lhs.M24.IsApproximately(rhs.M24, errorMargin) + && lhs.M31.IsApproximately(rhs.M31, errorMargin) + && lhs.M32.IsApproximately(rhs.M32, errorMargin) + && lhs.M33.IsApproximately(rhs.M33, errorMargin) + && lhs.M34.IsApproximately(rhs.M34, errorMargin) + && lhs.M41.IsApproximately(rhs.M41, errorMargin) + && lhs.M42.IsApproximately(rhs.M42, errorMargin) + && lhs.M43.IsApproximately(rhs.M43, errorMargin) + && lhs.M44.IsApproximately(rhs.M44, errorMargin); + } + + /// + /// Converts a Matrix3D to a Matrix4x4. + /// + /// + /// The matrix to convert. + /// + /// + /// The converted Matrix4x4. + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix4x4 ToMatrix4x4(this Matrix3D m) + { + return new Matrix4x4( + (float)m.M11, (float)m.M12, (float)m.M13, (float)m.M14, + (float)m.M21, (float)m.M22, (float)m.M23, (float)m.M24, + (float)m.M31, (float)m.M32, (float)m.M33, (float)m.M34, + (float)m.OffsetX, (float)m.OffsetY, (float)m.OffsetZ, (float)m.M44); + } + + /// + /// Converts a Matrix4x4 to a Matrix3D. + /// + /// + /// The matrix to convert. + /// + /// + /// The converted Matrix3D. + /// + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3D ToMatrix3D(this Matrix4x4 m) + { + return new Matrix3D( + m.M11, m.M12, m.M13, m.M14, + m.M21, m.M22, m.M23, m.M24, + m.M31, m.M32, m.M33, m.M34, + m.M41, m.M42, m.M43, m.M44); + } +} + diff --git a/Math3D/Extensions/VectorExtensions.cs b/Math3D/Extensions/VectorExtensions.cs index b63d213..e6104db 100644 --- a/Math3D/Extensions/VectorExtensions.cs +++ b/Math3D/Extensions/VectorExtensions.cs @@ -22,9 +22,9 @@ public static class VectorExtensions /// public static bool IsApproximately(this Vector3 lhs, Vector3 rhs, float errorMargin = 0.001f) { - return IsApproximately(lhs.X, rhs.X, errorMargin) - && IsApproximately(lhs.Y, rhs.Y, errorMargin) - && IsApproximately(lhs.Z, rhs.Z, errorMargin); + return lhs.X.IsApproximately(rhs.X, errorMargin) + && lhs.Y.IsApproximately(rhs.Y, errorMargin) + && lhs.Z.IsApproximately(rhs.Z, errorMargin); } /// @@ -120,7 +120,7 @@ public static System.Windows.Media.Media3D.Vector3D ToMedia3DVector(this Vector3 /// /// The Media3D Vector3D to be converted. /// A new System.Numerics Vector3 with the same components as the Media3D Vector3D. - public static Vector3 FromMedia3DQuaternion(this System.Windows.Media.Media3D.Vector3D self) + public static Vector3 FromMedia3DVector(this System.Windows.Media.Media3D.Vector3D self) { return new Vector3((float)self.X, (float)self.Y, (float)self.Z); } @@ -218,16 +218,6 @@ private static float NormalizeAngle(float angle) return angle; } - /// - /// Determines whether two floating-point numbers are approximately equal within a specified error margin. - /// - /// The first floating-point number. - /// The second floating-point number. - /// The acceptable error margin for the comparison. - /// True if the absolute difference between the two numbers is less than the error margin; otherwise, false. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static bool IsApproximately(float a, float b, float errorMargin) => MathF.Abs(a - b) < errorMargin; - /// /// Determines whether a nullable float is valid (i.e., not null, not infinity, and not NaN). /// diff --git a/Math3D/Line.cs b/Math3D/Line.cs index df351f2..672a1a4 100644 --- a/Math3D/Line.cs +++ b/Math3D/Line.cs @@ -4,14 +4,19 @@ namespace XivToolsWpf.Math3D; using System; +using System.Buffers; +using System.Numerics; using System.Runtime.CompilerServices; using System.Windows; using System.Windows.Media; using System.Windows.Media.Media3D; +using XivToolsWpf.Math3D.Extensions; /// Represents a Media3D line. public class Line : ModelVisual3D, IDisposable { + private const float APPROX_EQUALITY_EPSILON = 1e-6f; + /// Identifies the dependency property. public static readonly DependencyProperty ColorProperty = DependencyProperty.Register(nameof(Color), typeof(Color), typeof(Line), new PropertyMetadata(Colors.White, OnColorChanged)); @@ -24,8 +29,10 @@ public class Line : ModelVisual3D, IDisposable private readonly GeometryModel3D model; private readonly MeshGeometry3D mesh; - private Matrix3D visualToScreen; - private Matrix3D screenToVisual; + private Matrix4x4 visualToScreen; + private Matrix4x4 screenToVisual; + private Viewport3DVisual? cachedViewport; + private Visual3D? cachedRoot3D; /// /// Initializes a new instance of the class. @@ -39,9 +46,12 @@ public Line() this.Content = this.model; this.Points = []; - CompositionTarget.Rendering += this.OnRender; + LineManager.Instance.Value.Register(this); } + /// Gets a value indicating whether the element is currently visible. + public bool IsVisible { get; private set; } = true; + /// Gets or sets the color of the line. public Color Color { @@ -66,11 +76,13 @@ public Point3DCollection Points /// Releases all resources used by the class. public void Dispose() { - CompositionTarget.Rendering -= this.OnRender; + LineManager.Instance.Value.Unregister(this); this.Points.Clear(); this.Children.Clear(); this.Content = null; + this.cachedViewport = null; + this.cachedRoot3D = null; GC.SuppressFinalize(this); } @@ -101,23 +113,37 @@ public void MakeWireframe(Model3D model) /// The nearest point on the line, or null if no point is found. public Point3D? NearestPoint2D(Point3D cameraPoint) { - double closest = double.MaxValue; - Point3D? closestPoint = null; + if (this.Points.Count == 0 && this.mesh.Positions.Count == 0) + return null; + + var viewport = this.GetOrFindViewport(); + if (viewport == null) + return null; - if (!MathUtils.ToViewportTransform(this, out Matrix3D matrix)) + Matrix4x4? modelToWorld = this.TryGetModelToWorldMatrix(); + if (modelToWorld == null) return null; - var transform = new MatrixTransform3D(matrix); + if (!MathUtils.TryTransformVisualToViewport(viewport, (Matrix4x4)modelToWorld, out Matrix4x4 matrix)) + return null; + + float closest = float.MaxValue; + Point3D? closestPoint = null; foreach (Point3D point in this.Points) { - Point3D cameraSpacePoint = transform.Transform(point); - cameraSpacePoint.Z *= 100; + Vector4 transformed4 = Vector4.Transform(point.FromMedia3DPoint(), matrix); + var transformed = new Vector3( + transformed4.X / transformed4.W, + transformed4.Y / transformed4.W, + transformed4.Z / transformed4.W); + + transformed.Z *= 100f; - Vector3D dir = cameraPoint - cameraSpacePoint; - if (dir.Length < closest) + float dirLength = Vector3.Distance(cameraPoint.FromMedia3DPoint(), transformed); + if (dirLength < closest) { - closest = dir.Length; + closest = dirLength; closestPoint = point; } } @@ -125,6 +151,64 @@ public void MakeWireframe(Model3D model) return closestPoint; } + /// + /// Gets or finds the viewport that contains this line. + /// + /// + /// The viewport that contains this line, or null if none is found. + /// + /// + /// If available, this method returns a cached viewport reference. + /// + public Viewport3DVisual? GetOrFindViewport() + { + if (this.cachedViewport != null) + return this.cachedViewport; + + this.cachedViewport = MathUtils.FindViewport(this, out this.cachedRoot3D); + return this.cachedViewport; + } + + /// + /// Updates the transforms for the line. + /// + /// + /// The view-projection-screen matrix. + /// + /// + /// This method is intended to be called by the during rendering. + /// + public void UpdateGeometry(in Matrix4x4 viewProjScreen) + { + if (this.Points.Count == 0 && this.mesh.Positions.Count == 0 || this.cachedRoot3D == null) + return; + + Matrix4x4? modelToWorld = this.TryGetModelToWorldMatrix(); + if (modelToWorld == null) + return; + + Matrix4x4 newV2S = (Matrix4x4)modelToWorld * viewProjScreen; + if (newV2S.IsApproximately(this.visualToScreen, APPROX_EQUALITY_EPSILON)) + return; + + if (!Matrix4x4.Invert(newV2S, out Matrix4x4 newS2V)) + return; + + this.visualToScreen = newV2S; + this.screenToVisual = newS2V; + + this.RebuildGeometry(); + } + + /// + protected override void OnVisualParentChanged(DependencyObject? oldParent) + { + base.OnVisualParentChanged(oldParent); + this.GeometryDirty(); + + this.IsVisible = VisualTreeHelper.GetParent(this) != null; + } + /// /// Handles changes to the property. /// @@ -155,6 +239,33 @@ private static void OnPointsChanged(DependencyObject sender, DependencyPropertyC ((Line)sender).GeometryDirty(); } + /// + /// Widens a point in 4D space by a given delta in screen space. + /// + /// + /// The input point in 4D space. + /// + /// + /// The delta to apply in screen space. + /// + /// + /// The screen-to-visual transformation matrix. + /// + /// + /// The widened point in 3D space. + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static Point3D Widen(Vector4 pIn4, Vector2 delta, in Matrix4x4 s2v) + { + // Apply delta scaled by W + pIn4.X += delta.X * pIn4.W; + pIn4.Y += delta.Y * pIn4.W; + + // Un-project back to visual space + Vector4 pOut4 = Vector4.Transform(pIn4, s2v); + return new Point3D(pOut4.X / pOut4.W, pOut4.Y / pOut4.W, pOut4.Z / pOut4.W); + } + /// /// Sets the color of the line. /// @@ -170,144 +281,98 @@ private void SetColor(Color color) this.model.BackMaterial = unlitMaterial; } - /// - /// Handles the rendering event to update the line geometry. - /// - /// The object that raised the event. - /// The event data. - private void OnRender(object? sender, EventArgs e) - { - if (this.Points.Count == 0 && this.mesh.Positions.Count == 0) - return; - - if (this.UpdateTransforms()) - { - this.RebuildGeometry(); - } - } - /// /// Marks the geometry as dirty, forcing a rebuild on the next render. /// private void GeometryDirty() { // Force next call to UpdateTransforms() to return true. - this.visualToScreen = MathUtils.ZeroMatrix; + this.cachedViewport = null; + this.cachedRoot3D = null; + this.visualToScreen = MathUtils.ZeroMatrix4x4; } /// Rebuilds the geometry of the line. [MethodImpl(MethodImplOptions.AggressiveInlining)] private void RebuildGeometry() { - double halfThickness = this.Thickness / 2.0; var points = this.Points; - int numLines = points.Count / 2; - - var positions = new Point3DCollection(numLines * 4); - var indices = new Int32Collection(points.Count * 3); - - for (int i = 0; i < numLines; i++) - { - int startIndex = i * 2; - - Point3D startPoint = points[startIndex]; - Point3D endPoint = points[startIndex + 1]; - - this.AddSegment(positions, startPoint, endPoint, halfThickness); - - int baseIndex = i * 4; - indices.Add(baseIndex + 2); - indices.Add(baseIndex + 1); - indices.Add(baseIndex + 0); - - indices.Add(baseIndex + 2); - indices.Add(baseIndex + 3); - indices.Add(baseIndex + 1); - } - - positions.Freeze(); - this.mesh.Positions = positions; - - indices.Freeze(); - this.mesh.TriangleIndices = indices; - } - - /// Adds a segment to the line geometry. - /// The collection of positions to add to. - /// The start point of the segment. - /// The end point of the segment. - /// Half the thickness of the line. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private void AddSegment(Point3DCollection positions, Point3D startPoint, Point3D endPoint, double halfThickness) - { - // NOTE: We want the vector below to be perpendicular post projection so - // we need to compute the line direction in post-projective space. - Vector3D lineDirection = (endPoint * this.visualToScreen) - (startPoint * this.visualToScreen); - lineDirection.Z = 0; - lineDirection.Normalize(); + int pointCount = points.Count; + if (pointCount < 2) + return; - // NOTE: Implicit Rot(90) during construction to get a perpendicular vector. - var delta = new Vector(-lineDirection.Y, lineDirection.X); - delta *= halfThickness; + int numLines = pointCount / 2; + int numVertices = numLines * 4; + int numIndices = numLines * 6; - this.Widen(startPoint, delta, out Point3D pOut1, out Point3D pOut2); + Point3D[] vertexBuffer = ArrayPool.Shared.Rent(numVertices); + var indices = new Int32Collection(numIndices); - positions.Add(pOut1); - positions.Add(pOut2); + float halfThickness = (float)this.Thickness / 2.0f; + Matrix4x4 v2s = this.visualToScreen; + Matrix4x4 s2v = this.screenToVisual; - this.Widen(endPoint, delta, out pOut1, out pOut2); + try + { + for (int i = 0; i < numLines; i++) + { + int ptIdx = i * 2; + int vBase = i * 4; - positions.Add(pOut1); - positions.Add(pOut2); - } + Vector3 startP = points[ptIdx].FromMedia3DPoint(); + Vector3 endP = points[ptIdx + 1].FromMedia3DPoint(); - /// Widens a point by a specified delta. - /// The input point. - /// The delta to widen by. - /// The first widened point. - /// The second widened point. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private void Widen(Point3D pIn, Vector delta, out Point3D pOut1, out Point3D pOut2) - { - Point4D pIn4 = (Point4D)pIn; - Point4D pOut41 = pIn4 * this.visualToScreen; - Point4D pOut42 = pOut41; + // Transform to homogeneous clip space + Vector4 startClip = Vector4.Transform(startP, v2s); + Vector4 endClip = Vector4.Transform(endP, v2s); - pOut41.X += delta.X * pOut41.W; - pOut41.Y += delta.Y * pOut41.W; + if (startClip.W <= 0f || endClip.W <= 0f) + continue; - pOut42.X -= delta.X * pOut42.W; - pOut42.Y -= delta.Y * pOut42.W; + // Compute screen-space direction + Vector2 sStart = new(startClip.X / startClip.W, startClip.Y / startClip.W); + Vector2 sEnd = new(endClip.X / endClip.W, endClip.Y / endClip.W); - pOut41 *= this.screenToVisual; - pOut42 *= this.screenToVisual; + Vector2 lineDir = sEnd - sStart; + float len = lineDir.Length(); - // NOTE: Z is not modified above, so we use the original Z below. - pOut1 = new Point3D(pOut41.X / pOut41.W, pOut41.Y / pOut41.W, pOut41.Z / pOut41.W); - pOut2 = new Point3D(pOut42.X / pOut42.W, pOut42.Y / pOut42.W, pOut42.Z / pOut42.W); - } + Vector2 delta; + if (len < APPROX_EQUALITY_EPSILON) + { + delta = new Vector2(halfThickness, 0); + } + else + { + // Perpendicular vector in screen space + delta = new Vector2(-lineDir.Y, lineDir.X) * (halfThickness / len); + } - /// Updates the transforms for the line. - /// true if the transforms were updated; otherwise, false. - private bool UpdateTransforms() - { - Matrix3D visualToScreen = MathUtils.TryTransformTo2DAncestor(this, out Viewport3DVisual? viewport, out bool success); + // Widen and invert + // We scale the delta by W to keep thickness constant in screen space + vertexBuffer[vBase + 0] = Widen(startClip, delta, s2v); + vertexBuffer[vBase + 1] = Widen(startClip, -delta, s2v); + vertexBuffer[vBase + 2] = Widen(endClip, delta, s2v); + vertexBuffer[vBase + 3] = Widen(endClip, -delta, s2v); + + // Indexing + indices.Add(vBase + 2); + indices.Add(vBase + 1); + indices.Add(vBase + 0); + + indices.Add(vBase + 2); + indices.Add(vBase + 3); + indices.Add(vBase + 1); + } - if (!success || !visualToScreen.HasInverse) - { - this.mesh.Positions = null; - return false; + this.mesh.Positions = [.. vertexBuffer.AsSpan(0, numVertices).ToArray()]; + this.mesh.Positions.Freeze(); + this.mesh.TriangleIndices = indices; + this.mesh.TriangleIndices.Freeze(); } - - if (visualToScreen == this.visualToScreen) + finally { - return false; + ArrayPool.Shared.Return(vertexBuffer); } - - this.visualToScreen = this.screenToVisual = visualToScreen; - this.screenToVisual.Invert(); - - return true; } /// Helper method to create a wireframe representation of a 3D model. @@ -394,7 +459,7 @@ private void WireframeHelper(GeometryModel3D model, Matrix3DStack matrixStack) break; } - this.AddTriangle(positions, i0, i1, i2); + this.AddTriangle(ref positions, i0, i1, i2); } } else @@ -405,7 +470,7 @@ private void WireframeHelper(GeometryModel3D model, Matrix3DStack matrixStack) int i1 = i - 1; int i2 = i; - this.AddTriangle(positions, i0, i1, i2); + this.AddTriangle(ref positions, i0, i1, i2); } } } @@ -419,7 +484,7 @@ private void WireframeHelper(GeometryModel3D model, Matrix3DStack matrixStack) /// The second index of the triangle. /// The third index of the triangle. [MethodImpl(MethodImplOptions.AggressiveInlining)] - private void AddTriangle(Point3D[] positions, int i0, int i1, int i2) + private void AddTriangle(ref Point3D[] positions, int i0, int i1, int i2) { this.Points.Add(positions[i0]); this.Points.Add(positions[i1]); @@ -428,4 +493,37 @@ private void AddTriangle(Point3D[] positions, int i0, int i1, int i2) this.Points.Add(positions[i2]); this.Points.Add(positions[i0]); } + + private Matrix4x4? TryGetModelToWorldMatrix() + { + if (this.cachedRoot3D == null) + return null; + + try + { + GeneralTransform3D transform = this.TransformToAncestor(this.cachedRoot3D); + + Matrix4x4 modelToWorld; + if (transform is Transform3D t3d) + { + modelToWorld = t3d.Value.ToMatrix4x4(); + } + else + { + modelToWorld = Matrix4x4.Identity; + } + + if (this.cachedRoot3D.Transform != null && !this.cachedRoot3D.Transform.Value.IsIdentity) + { + modelToWorld *= this.cachedRoot3D.Transform.Value.ToMatrix4x4(); + } + + return modelToWorld; + } + catch (InvalidOperationException) + { + this.cachedViewport = null; + return null; + } + } } diff --git a/Math3D/LineManager.cs b/Math3D/LineManager.cs new file mode 100644 index 0000000..e2a80a5 --- /dev/null +++ b/Math3D/LineManager.cs @@ -0,0 +1,95 @@ +// © XIV-Tools. +// Licensed under the MIT license. + +namespace XivToolsWpf.Math3D; + +using System; +using System.Collections.Generic; +using System.Numerics; +using System.Threading; +using System.Windows.Media; +using System.Windows.Media.Media3D; + +/// +/// A manager that streamlines the rendering of objects in the viewport. +/// Without this manager, each object would need to calculate its +/// view-projection matrix, which adds significant overhead when many lines are present. +/// +/// +/// The use of the manager is recommended over individual object rendering for performance reasons. +/// +public sealed class LineManager +{ + /// + /// Gets the singleton instance of the . + /// + public static readonly Lazy Instance = new(() => new LineManager()); + + private readonly Lock objLock = new(); + private readonly HashSet lines = []; + private readonly Dictionary viewportMatrixCache = []; + + private LineManager() + { + CompositionTarget.Rendering += this.OnRendering; + } + + /// + /// Registers a new object to the manager. + /// + /// + /// The object to register. + /// + public void Register(Line line) + { + lock (this.objLock) + { + this.lines.Add(line); + } + } + + /// + /// Unregisters an existing object from the manager. + /// + /// + /// The object to unregister. + /// + public void Unregister(Line line) + { + lock (this.objLock) + { + this.lines.Remove(line); + } + } + + private void OnRendering(object? sender, EventArgs e) + { + this.viewportMatrixCache.Clear(); + + lock (this.objLock) + { + if (this.lines.Count == 0) + return; + + foreach (Line line in this.lines) + { + if (!line.IsVisible) + continue; + + Viewport3DVisual? viewport = line.GetOrFindViewport(); + if (viewport == null) + continue; + + if (!this.viewportMatrixCache.TryGetValue(viewport, out Matrix4x4 viewProjScreen)) + { + if (MathUtils.TryGetViewProjectionViewportMatrix(viewport, out viewProjScreen)) + this.viewportMatrixCache[viewport] = viewProjScreen; + else + continue; // Invalid camera or viewport + } + + line.UpdateGeometry(viewProjScreen); + } + } + } +} diff --git a/Math3D/MathUtils.cs b/Math3D/MathUtils.cs index 0aacd57..0403cda 100644 --- a/Math3D/MathUtils.cs +++ b/Math3D/MathUtils.cs @@ -5,18 +5,33 @@ namespace XivToolsWpf.Math3D; using System; using System.Diagnostics; +using System.Numerics; using System.Runtime.CompilerServices; using System.Windows; using System.Windows.Media; using System.Windows.Media.Media3D; +using XivToolsWpf.Math3D.Extensions; public static class MathUtils { + public static readonly Matrix3D IdentityMatrix = Matrix3D.Identity; public static readonly Matrix3D ZeroMatrix = new(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + public static readonly Matrix4x4 ZeroMatrix4x4 = new(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); public static readonly Vector3D XAxis = new(1, 0, 0); public static readonly Vector3D YAxis = new(0, 1, 0); public static readonly Vector3D ZAxis = new(0, 0, 1); + private const double DEG_TO_RAD = Math.PI / 180.0; + private const float DEG_TO_RADF = (float)(Math.PI / 180.0); + private const double RAD_TO_DEG = 180.0 / Math.PI; + private const float RAD_TO_DEGF = (float)(180.0 / Math.PI); + + private const float LOOK_DIR_EPSILON = 1e-10f; + private const float PARALLEL_VEC_DOT_THRESHOLD = 0.9999f; + private const float MIN_FOV = 0.001f; + private const float MAX_FOV = 179.999f; + private const float MIN_ORTHO_SIZE = 1e-5f; + /// Gets the aspect ratio of the specified size. /// The size to calculate the aspect ratio for. /// The aspect ratio of the size. @@ -27,13 +42,25 @@ public static class MathUtils /// The angle in degrees. /// The angle in radians. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static double DegreesToRadians(double degrees) => degrees * (Math.PI / 180.0); + public static double DegreesToRadians(double degrees) => degrees * DEG_TO_RAD; + + /// Converts degrees to radians. + /// The angle in degrees. + /// The angle in radians. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float DegreesToRadians(float degrees) => degrees * DEG_TO_RADF; /// Converts radians to degrees. /// The angle in radians. /// The angle in degrees. [MethodImpl(MethodImplOptions.AggressiveInlining)] - public static double RadiansToDegrees(double radians) => radians * (180 / Math.PI); + public static double RadiansToDegrees(double radians) => radians * RAD_TO_DEG; + + /// Converts radians to degrees. + /// The angle in radians. + /// The angle in degrees. + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float RadiansToDegrees(float radians) => radians * RAD_TO_DEGF; /// /// Computes the effective view matrix for the given camera. @@ -42,13 +69,23 @@ public static class MathUtils /// The view matrix for the camera. /// Thrown when the camera is null. /// Thrown when the camera type is unsupported. - public static Matrix3D GetViewMatrix(Camera camera) => camera switch + public static Matrix3D GetViewMatrix(Camera camera) { - null => throw new ArgumentNullException(nameof(camera)), - ProjectionCamera projectionCamera => GetViewMatrix(projectionCamera), - MatrixCamera matrixCamera => matrixCamera.ViewMatrix, - _ => throw new ArgumentException($"Unsupported camera type '{camera.GetType().FullName}'.", nameof(camera)) - }; + ArgumentNullException.ThrowIfNull(camera); + + if (camera is MatrixCamera matrixCamera) + return matrixCamera.ViewMatrix; + + if (camera is ProjectionCamera projectionCamera) + { + if (TryGetViewMatrixInternal(projectionCamera, applyTransform: false, out Matrix4x4 view)) + return view.ToMatrix3D(); + + return IdentityMatrix; // Fallback + } + + throw new ArgumentException($"Unsupported camera type '{camera.GetType().FullName}'.", nameof(camera)); + } /// /// Computes the effective projection matrix for the given camera. @@ -58,148 +95,269 @@ public static class MathUtils /// The projection matrix for the camera. /// Thrown when the camera is null. /// Thrown when the camera type is unsupported. - public static Matrix3D GetProjectionMatrix(Camera camera, double aspectRatio) => camera switch + public static Matrix3D GetProjectionMatrix(Camera camera, double aspectRatio) { - null => throw new ArgumentNullException(nameof(camera)), - PerspectiveCamera perspectiveCamera => GetProjectionMatrix(perspectiveCamera, aspectRatio), - OrthographicCamera orthographicCamera => GetProjectionMatrix(orthographicCamera, aspectRatio), - MatrixCamera matrixCamera => matrixCamera.ProjectionMatrix, - _ => throw new ArgumentException($"Unsupported camera type '{camera.GetType().FullName}'.", nameof(camera)) - }; + ArgumentNullException.ThrowIfNull(camera); - /// - /// Computes the transformation matrix from the specified visual to the viewport. - /// - /// The visual to compute the transformation for. - /// The resulting transformation matrix. - /// True if the transformation was successful; otherwise, false. - public static bool ToViewportTransform(DependencyObject visual, out Matrix3D matrix) - { - matrix = Matrix3D.Identity; - Matrix3D toWorld = GetWorldTransformationMatrix(visual, out var viewportVisual); - Matrix3D toViewport = TryWorldToViewportTransform(viewportVisual, out var success); + if (camera is MatrixCamera matrixCamera) + return matrixCamera.ProjectionMatrix; - if (!success) - return false; + if (TryGetProjectionMatrixInternal(camera, (float)aspectRatio, out Matrix4x4 proj)) + return proj.ToMatrix3D(); - toWorld.Append(toViewport); - matrix = toWorld; - return true; + throw new ArgumentException($"Unsupported camera type '{camera.GetType().FullName}'.", nameof(camera)); } /// - /// Computes the transform from world space to the Viewport3DVisual's inner 2D space. - /// This method can fail if Camera.Transform is non-invertable in which case the camera - /// clip planes will be coincident and nothing will render. In this case success will be false. + /// Finds the nearest ancestor of the given visual and + /// calculates the transform from the visual to the viewport's coordinate space. /// - /// The Viewport3DVisual to compute the transformation for. - /// True if the transformation was successful; otherwise, false. - /// The transformation matrix from world space to the viewport's inner 2D space. - public static Matrix3D TryWorldToViewportTransform(Viewport3DVisual? visual, out bool success) + /// + /// The viewport's coordinate space is considered to be the world space. + /// + /// + /// The visual to start searching from. + /// + /// + /// The output relative visual to viewport transform. + /// Returns the accumulated transform from the visual to the viewport if a viewport is found; + /// Otherwise, the method returns the identity matrix. + /// + /// + /// The nearest ancestor, or null if none is found. + /// + public static Viewport3DVisual? FindViewport(DependencyObject visual, out Matrix4x4 modelToWorld) { - success = false; - Matrix3D result = TryWorldToCameraTransform(visual, out success); + Matrix4x4 accumulatedTransform = Matrix4x4.Identity; + DependencyObject current = visual; - if (visual != null && success) + while (current != null) { - result.Append(GetProjectionMatrix(visual.Camera, GetAspectRatio(visual.Viewport.Size))); - result.Append(GetHomogeneousToViewportTransform(visual.Viewport)); - success = true; + if (current is ModelVisual3D modelVisual) + { + var transform = modelVisual.Transform; + if (transform != null) + { + var matrix = transform.Value.ToMatrix4x4(); + if (!matrix.IsIdentity) + { + accumulatedTransform *= matrix; + } + } + } + else if (current is Viewport3DVisual viewport) + { + modelToWorld = accumulatedTransform; + return viewport; + } + + current = VisualTreeHelper.GetParent(current); } - return result; + modelToWorld = Matrix4x4.Identity; + return null; } /// - /// Computes the transform from world space to camera space. - /// This method can fail if Camera.Transform is non-invertable in which case the camera - /// clip planes will be coincident and nothing will render. In this case success will be false. + /// Finds the nearest ancestor of the given visual and + /// calculates the transform from the visual to the viewport's coordinate space. /// - /// The Viewport3DVisual to compute the transformation for. - /// True if the transformation was successful; otherwise, false. - /// The transformation matrix from world space to camera space. - public static Matrix3D TryWorldToCameraTransform(Viewport3DVisual? visual, out bool success) + /// + /// This variant of the method returns a direct child of the found viewport, which allows + /// the caller to cache the viewport and calculate the model-to-world transform later. + /// If you don't need to cache, use instead. + /// + /// + /// The visual to start searching from. + /// + /// + /// The direct child of the found , or null if none is found. + /// + /// + /// The nearest ancestor, or null if none is found. + /// + public static Viewport3DVisual? FindViewport(Visual3D visual, out Visual3D? root3D) { - success = false; - - if (visual == null) - return ZeroMatrix; - - Matrix3D result = Matrix3D.Identity; - Camera camera = visual.Camera; + root3D = null; + DependencyObject current = visual; + Visual3D? lastV3D = visual; - if (camera == null || visual.Viewport == Rect.Empty) - return ZeroMatrix; - - Transform3D cameraTransform = camera.Transform; - - if (cameraTransform != null) + while (current != null) { - Matrix3D m = cameraTransform.Value; - - if (!m.HasInverse) + if (current is Viewport3DVisual viewport) { - return ZeroMatrix; + root3D = lastV3D; // This is the child of the Viewport + return viewport; } - m.Invert(); - result.Append(m); + if (current is Visual3D v3d) + lastV3D = v3d; + + current = VisualTreeHelper.GetParent(current); } + return null; + } - result.Append(GetViewMatrix(camera)); + /// + /// Attempts to calculate the full transformation matrix + /// from a 3D Visual object's local space to the 2D Screen/Viewport space. + /// + /// + /// This is a convenience method that encapsulates: + /// Model-to-World → World-to-Camera (View) → Camera-to-NDC (Projection) → NDC-to-Screen. + /// + /// The source 3D visual. + /// The resulting transformation matrix. + /// + /// True if the transformation matrix was successfully calculated; otherwise, false. + /// + public static bool TryTransformVisualToViewport(DependencyObject visual, out Matrix3D screenMatrix) + { + var success = TryTransformVisualToViewport(visual, out Matrix4x4 result); + screenMatrix = success ? result.ToMatrix3D() : ZeroMatrix; + return success; + } - success = true; - return result; + /// + /// Attempts to calculate the full transformation matrix + /// from a 3D Visual object's local space to the 2D Screen/Viewport space. + /// + /// + /// This is a convenience method that encapsulates: + /// Model-to-World → World-to-Camera (View) → Camera-to-NDC (Projection) → NDC-to-Screen. + /// + /// The source 3D visual. + /// The resulting transformation matrix. + /// + /// The ancestor viewport found during the search, if any. + /// + /// + /// True if the transformation matrix was successfully calculated; otherwise, false. + /// + public static bool TryTransformVisualToViewport(DependencyObject visual, out Matrix3D screenMatrix, out Viewport3DVisual? viewport) + { + var success = TryTransformVisualToViewport(visual, out Matrix4x4 result, out viewport); + screenMatrix = success ? result.ToMatrix3D() : ZeroMatrix; + return success; } /// - /// Computes the transform from the inner space of the given Visual3D to the 2D space of the Viewport3DVisual which - /// contains it. The result will contain the transform of the given visual. This method can fail if Camera.Transform - /// is non-invertable in which case the camera clip planes will be coincident and nothing will render. - /// In this case success will be false. + /// Attempts to calculate the full transformation matrix + /// from a 3D Visual object's local space to the 2D Screen/Viewport space. /// - /// The visual to compute the transformation for. - /// The Viewport3DVisual that contains the visual. - /// True if the transformation was successful; otherwise, false. - /// The transformation matrix from the visual's inner space to the viewport's 2D space. - public static Matrix3D TryTransformTo2DAncestor(DependencyObject visual, out Viewport3DVisual? viewport, out bool success) + /// + /// This is a convenience method that encapsulates: + /// Model-to-World → World-to-Camera (View) → Camera-to-NDC (Projection) → NDC-to-Screen. + /// + /// The source 3D visual. + /// The resulting transformation matrix. + /// + /// True if the transformation matrix was successfully calculated; otherwise, false. + /// + public static bool TryTransformVisualToViewport(DependencyObject visual, out Matrix4x4 screenMatrix) { - Matrix3D to2D = GetWorldTransformationMatrix(visual, out viewport); + var viewport = FindViewport(visual, out Matrix4x4 modelToWorld); + if (viewport == null) + { + screenMatrix = default; + return false; + } + + return TryTransformVisualToViewport(viewport, modelToWorld, out screenMatrix); + } + /// + /// Attempts to calculate the full transformation matrix + /// from a 3D Visual object's local space to the 2D Screen/Viewport space. + /// + /// + /// This is a convenience method that encapsulates: + /// Model-to-World → World-to-Camera (View) → Camera-to-NDC (Projection) → NDC-to-Screen. + /// + /// The source 3D visual. + /// The resulting transformation matrix. + /// + /// The ancestor viewport found during the search, if any. + /// + /// + /// True if the transformation matrix was successfully calculated; otherwise, false. + /// + public static bool TryTransformVisualToViewport(DependencyObject visual, out Matrix4x4 screenMatrix, out Viewport3DVisual? viewport) + { + viewport = FindViewport(visual, out Matrix4x4 modelToWorld); if (viewport == null) { - success = false; - return ZeroMatrix; + screenMatrix = default; + return false; } - Matrix3D toViewport = TryWorldToViewportTransform(viewport, out success); + return TryTransformVisualToViewport(viewport, modelToWorld, out screenMatrix); + } - if (!success) - return ZeroMatrix; + /// + /// Calculates the full transformation matrix from the given model-to-world matrix + /// to the 2D Screen/Viewport space of the specified viewport. + /// + /// + /// The viewport to calculate the matrix for. + /// + /// + /// The model-to-world transformation matrix. + /// + /// + /// The output model-to-screen transformation matrix, including view, projection, and viewport transforms. + /// + /// + /// True if the transformation matrix was successfully calculated; otherwise, false. + /// + public static bool TryTransformVisualToViewport(Viewport3DVisual viewport, Matrix4x4 modelToWorld, out Matrix4x4 modelToScreen) + { + if (!TryGetViewProjectionViewportMatrix(viewport, out Matrix4x4 viewProj)) + { + modelToScreen = default; + return false; + } - to2D.Append(toViewport); - return to2D; + modelToScreen = modelToWorld * viewProj; + return true; } /// - /// Computes the transform from the inner space of the given Visual3D to the camera coordinate space. - /// The result will contain the transform of the given visual. This method can fail if Camera.Transform - /// is non-invertable in which case the camera clip planes will be coincident and nothing will render. - /// In this case success will be false. + /// Calculates the combined View-Projection-Screen matrix for the given viewport. /// - /// The visual to compute the transformation for. - /// The Viewport3DVisual that contains the visual. - /// True if the transformation was successful; otherwise, false. - /// The transformation matrix from the visual's inner space to the camera coordinate space. - public static Matrix3D TryTransformToCameraSpace(DependencyObject visual, out Viewport3DVisual? viewport, out bool success) + /// + /// The viewport to calculate the matrix for. + /// + /// + /// The resulting combined matrix. + /// + /// + /// True if the transformation matrix was successfully calculated; otherwise, false. + /// + public static bool TryGetViewProjectionViewportMatrix(Viewport3DVisual viewport, out Matrix4x4 result) { - Matrix3D toViewSpace = GetWorldTransformationMatrix(visual, out viewport); - toViewSpace.Append(TryWorldToCameraTransform(viewport, out success)); + if (viewport == null) + { + result = default; + return false; + } + + var camera = viewport.Camera; + Rect rect = viewport.Viewport; - if (!success) - return ZeroMatrix; + // Combine view and camera projection matrices + if (!TryGetViewProjectionMatrix(camera, rect.Size, out Matrix4x4 viewProj)) + { + result = Matrix4x4.Identity; + return false; + } - return toViewSpace; + // Map normalized device coordinates (-1 to 1) to screen space (0 to Width/Height) + Matrix4x4 viewportMat = ConvertNDCToScreenMatrix(rect); + + // Combine transforms: World -> View projection -> Screen + result = viewProj * viewportMat; + return true; } /// @@ -253,6 +411,7 @@ public static Rect3D TransformBounds(Rect3D bounds, Matrix3D transform) /// /// The vector to normalize. /// 'true' if v was normalized. + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static bool TryNormalize(ref Vector3D v) { double length = v.Length; @@ -292,141 +451,162 @@ public static Point3D NearestPointOnRay(Point3D rayOrigin, Vector3D rayDirection } /// - /// Computes the view matrix for a projection camera. + /// Converts normalized device coordinates (-1 to 1) to screen space (0 to Width/Height). /// - /// The projection camera. - /// The view matrix for the camera. - private static Matrix3D GetViewMatrix(ProjectionCamera camera) + /// + /// The viewport rectangle. + /// + /// + /// A transformation matrix of the screen space. + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix4x4 ConvertNDCToScreenMatrix(Rect viewport) { - Debug.Assert(camera != null, "Caller needs to ensure camera is non-null."); - - // This math is identical to what you find documented for - // D3DXMatrixLookAtRH with the exception that WPF uses a - // LookDirection vector rather than a LookAt point. - Vector3D zAxis = -camera.LookDirection; - zAxis.Normalize(); - - Vector3D xAxis = Vector3D.CrossProduct(camera.UpDirection, zAxis); - xAxis.Normalize(); - - Vector3D yAxis = Vector3D.CrossProduct(zAxis, xAxis); - - Vector3D position = (Vector3D)camera.Position; - double offsetX = -Vector3D.DotProduct(xAxis, position); - double offsetY = -Vector3D.DotProduct(yAxis, position); - double offsetZ = -Vector3D.DotProduct(zAxis, position); - - return new Matrix3D(xAxis.X, yAxis.X, zAxis.X, 0, xAxis.Y, yAxis.Y, zAxis.Y, 0, xAxis.Z, yAxis.Z, zAxis.Z, 0, offsetX, offsetY, offsetZ, 1); + float scaleX = (float)(viewport.Width / 2); + float scaleY = (float)(viewport.Height / 2); + float offsetX = (float)(viewport.X + scaleX); + float offsetY = (float)(viewport.Y + scaleY); + return new Matrix4x4(scaleX, 0, 0, 0, 0, -scaleY, 0, 0, 0, 0, 1, 0, offsetX, offsetY, 0, 1); } /// - /// Computes the projection matrix for an orthographic camera. + /// Converts normalized device coordinates (-1 to 1) to screen space (0 to Width/Height). /// - /// The orthographic camera. - /// The aspect ratio of the viewport. - /// The projection matrix for the camera. - private static Matrix3D GetProjectionMatrix(OrthographicCamera camera, double aspectRatio) + /// + /// The viewport rectangle. + /// + /// + /// A transformation matrix of the screen space. + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static Matrix3D ConvertNDCToScreenMatrix3D(Rect viewport) { - Debug.Assert(camera != null, "Caller needs to ensure camera is non-null."); - - // This math is identical to what you find documented for - // D3DXMatrixOrthoRH with the exception that in WPF only - // the camera's width is specified. Height is calculated - // from width and the aspect ratio. - double w = camera.Width; - double h = w / aspectRatio; - double zn = camera.NearPlaneDistance; - double zf = camera.FarPlaneDistance; - - double m33 = 1 / (zn - zf); - double m43 = zn * m33; - - return new Matrix3D(2 / w, 0, 0, 0, 0, 2 / h, 0, 0, 0, 0, m33, 0, 0, 0, m43, 1); + double scaleX = viewport.Width / 2; + double scaleY = viewport.Height / 2; + double offsetX = viewport.X + scaleX; + double offsetY = viewport.Y + scaleY; + return new Matrix3D(scaleX, 0, 0, 0, 0, -scaleY, 0, 0, 0, 0, 1, 0, offsetX, offsetY, 0, 1); } - /// - /// Computes the projection matrix for a perspective camera. - /// - /// The perspective camera. - /// The aspect ratio of the viewport. - /// The projection matrix for the camera. - private static Matrix3D GetProjectionMatrix(PerspectiveCamera camera, double aspectRatio) + private static bool TryGetViewProjectionMatrix(Camera camera, Size viewportSize, out Matrix4x4 result) { - Debug.Assert(camera != null, "Caller needs to ensure camera is non-null."); + if (viewportSize.Width <= 0 || viewportSize.Height <= 0) + { + result = Matrix4x4.Identity; + return false; + } - // This math is identical to what you find documented for - // D3DXMatrixPerspectiveFovRH with the exception that in - // WPF the camera's horizontal rather the vertical - // field-of-view is specified. - double hFoV = DegreesToRadians(camera.FieldOfView); - double zn = camera.NearPlaneDistance; - double zf = camera.FarPlaneDistance; + // Apply camera transform to compute view matrix + if (camera is not ProjectionCamera projCam || + !TryGetViewMatrixInternal(projCam, applyTransform: true, out Matrix4x4 view)) + { + result = Matrix4x4.Identity; + return false; + } - double xScale = 1 / Math.Tan(hFoV / 2); - double yScale = aspectRatio * xScale; - double m33 = (zf == double.PositiveInfinity) ? -1 : (zf / (zn - zf)); - double m43 = zn * m33; + // Calculate projection matrix + float aspectRatio = (float)(viewportSize.Width / viewportSize.Height); + if (!TryGetProjectionMatrixInternal(camera, aspectRatio, out Matrix4x4 proj)) + { + result = default; + return false; + } - return new Matrix3D(xScale, 0, 0, 0, 0, yScale, 0, 0, 0, 0, m33, -1, 0, 0, m43, 0); + result = view * proj; + return true; } - /// - /// Computes the transformation matrix from homogeneous coordinates to viewport coordinates. - /// - /// The viewport rectangle. - /// The transformation matrix from homogeneous coordinates to viewport coordinates. - private static Matrix3D GetHomogeneousToViewportTransform(Rect viewport) + private static bool TryGetViewMatrixInternal(ProjectionCamera camera, bool applyTransform, out Matrix4x4 viewMatrix) { - double scaleX = viewport.Width / 2; - double scaleY = viewport.Height / 2; - double offsetX = viewport.X + scaleX; - double offsetY = viewport.Y + scaleY; + Vector3 pos = camera.Position.FromMedia3DPoint(); + Vector3 lookDir = camera.LookDirection.FromMedia3DVector(); + Vector3 upDir = camera.UpDirection.FromMedia3DVector(); - return new Matrix3D(scaleX, 0, 0, 0, 0, -scaleY, 0, 0, 0, 0, 1, 0, offsetX, offsetY, 0, 1); - } - - /// - /// Gets the object space to world space transformation for the given DependencyObject. - /// - /// The visual whose world space transform should be found. - /// The Viewport3DVisual the Visual is contained within. - /// The world space transformation. - [MethodImpl(MethodImplOptions.AggressiveInlining)] - private static Matrix3D GetWorldTransformationMatrix(DependencyObject visual, out Viewport3DVisual? viewport) - { - Matrix3D worldTransform = Matrix3D.Identity; - viewport = null; + if (applyTransform && camera.Transform is Transform3D transform && !transform.Value.IsIdentity) + { + Matrix4x4 camM = transform.Value.ToMatrix4x4(); + pos = Vector3.Transform(pos, camM); + lookDir = Vector3.TransformNormal(lookDir, camM); + upDir = Vector3.TransformNormal(upDir, camM); + } - if (visual is not Visual3D) + float lookLenSq = lookDir.LengthSquared(); + if (lookLenSq < LOOK_DIR_EPSILON) { - throw new ArgumentException("Must be of type Visual3D.", nameof(visual)); + viewMatrix = default; + return false; } - while (visual is ModelVisual3D modelVisual) + Vector3 nLook = lookDir / MathF.Sqrt(lookLenSq); + Vector3 nUp = Vector3.Normalize(upDir); + float dot = MathF.Abs(Vector3.Dot(nLook, nUp)); + + // Parallel vector correction + if (dot > PARALLEL_VEC_DOT_THRESHOLD) { - Transform3D? transform = modelVisual.Transform; - if (transform != null) - { - worldTransform.Append(transform.Value); - } + float absX = MathF.Abs(nLook.X); + float absY = MathF.Abs(nLook.Y); + float absZ = MathF.Abs(nLook.Z); - visual = VisualTreeHelper.GetParent(visual); + if (absX <= absY && absX <= absZ) nUp = Vector3.UnitX; + else if (absY <= absX && absY <= absZ) nUp = Vector3.UnitY; + else nUp = Vector3.UnitZ; } - viewport = visual as Viewport3DVisual; + // D3DXMatrixLookAtRH equivalent (WPF uses RH) + // WPF LookDirection is Vector, CreateLookAt expects Target Point (Pos + Look) + viewMatrix = Matrix4x4.CreateLookAt(pos, pos + nLook, nUp); + return true; + } - if (viewport == null) + private static bool TryGetProjectionMatrixInternal(Camera camera, float aspectRatio, out Matrix4x4 projMatrix) + { + projMatrix = camera switch { - if (visual != null) - { - // In WPF 3D v1 the only possible configuration is a chain of - // ModelVisual3Ds leading up to a Viewport3DVisual. - throw new ApplicationException($"Unsupported type: '{visual.GetType().FullName}'. Expected tree of ModelVisual3Ds leading up to a Viewport3DVisual."); - } + PerspectiveCamera persCam => GetProjectionMatrixInternal(persCam, aspectRatio), + OrthographicCamera ortho => GetProjectionMatrixInternal(ortho, aspectRatio), + _ => default + }; - return ZeroMatrix; - } + return camera is PerspectiveCamera or OrthographicCamera; + } + + private static Matrix4x4 GetProjectionMatrixInternal(OrthographicCamera camera, double aspectRatio) + { + Debug.Assert(camera != null, "Caller needs to ensure camera is non-null."); + + // This math is identical to what you find documented for + // D3DXMatrixOrthoRH with the exception that in WPF only + // the camera's width is specified. Height is calculated + // from width and the aspect ratio. + float w = MathF.Max((float)camera.Width, MIN_ORTHO_SIZE); + float h = (float)(w / aspectRatio); + float zn = (float)camera.NearPlaneDistance; + float zf = (float)camera.FarPlaneDistance; + float m33 = 1.0f / (zn - zf); + float m43 = zn * m33; + + return new Matrix4x4(2.0f / w, 0, 0, 0, 0, 2.0f / h, 0, 0, 0, 0, m33, 0, 0, 0, m43, 1); + } + + private static Matrix4x4 GetProjectionMatrixInternal(PerspectiveCamera camera, double aspectRatio) + { + Debug.Assert(camera != null, "Caller needs to ensure camera is non-null."); + + // This math is identical to what you find documented for + // D3DXMatrixPerspectiveFovRH with the exception that in + // WPF the camera's horizontal rather the vertical + // field-of-view is specified. + float hFoV = Math.Clamp((float)camera.FieldOfView, MIN_FOV, MAX_FOV); + float hFovRad = DegreesToRadians(hFoV); + float zn = (float)camera.NearPlaneDistance; + float zf = (float)camera.FarPlaneDistance; + + float xScale = 1.0f / MathF.Tan(hFovRad * 0.5f); + float yScale = (float)aspectRatio * xScale; + float m33 = (zf == float.PositiveInfinity) ? -1.0f : (zf / (zn - zf)); + float m43 = zn * m33; - return worldTransform; + return new Matrix4x4(xScale, 0, 0, 0, 0, yScale, 0, 0, 0, 0, m33, -1, 0, 0, m43, 0); } -} +} \ No newline at end of file