| | | 1 | | //======================================================================= |
| | | 2 | | // Fixed3x3.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2024–present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using System; |
| | | 9 | | using System.Globalization; |
| | | 10 | | using System.Runtime.CompilerServices; |
| | | 11 | | using System.Text.Json.Serialization; |
| | | 12 | | using FixedMathSharp.Geometry; |
| | | 13 | | using MemoryPack; |
| | | 14 | | |
| | | 15 | | namespace FixedMathSharp; |
| | | 16 | | |
| | | 17 | | /// <summary> |
| | | 18 | | /// Represents a 3x3 matrix used for linear transformations in 2D and 3D space, such as rotation, scaling, and shearing. |
| | | 19 | | /// </summary> |
| | | 20 | | /// <remarks> |
| | | 21 | | /// A 3x3 matrix handles only linear transformations and is typically used when translation is not needed. |
| | | 22 | | /// It operates on directions, orientations, and vectors within a given space without affecting position. |
| | | 23 | | /// This matrix is more lightweight compared to a 4x4 matrix, making it ideal when translation and perspective are unnec |
| | | 24 | | /// |
| | | 25 | | /// Use Cases: |
| | | 26 | | /// - Rotating or scaling objects around the origin in 2D and 3D space. |
| | | 27 | | /// - Transforming vectors and normals (e.g., in lighting calculations). |
| | | 28 | | /// - Used in physics engines for inertia tensors or to represent local orientations. |
| | | 29 | | /// - Useful when optimizing transformations, as it omits the overhead of translation and perspective. |
| | | 30 | | /// </remarks> |
| | | 31 | | [Serializable] |
| | | 32 | | [MemoryPackable] |
| | | 33 | | public partial struct Fixed3x3 : IEquatable<Fixed3x3>, IFormattable |
| | | 34 | | #if NET8_0_OR_GREATER |
| | | 35 | | , ISpanFormattable |
| | | 36 | | #endif |
| | | 37 | | { |
| | | 38 | | #region Static Readonly |
| | | 39 | | |
| | | 40 | | /// <summary> |
| | | 41 | | /// Returns the identity matrix (no scaling, rotation, or translation). |
| | | 42 | | /// </summary> |
| | 1 | 43 | | public static readonly Fixed3x3 Identity = new(Vector3d.FromDouble(1f, 0f, 0f), Vector3d.FromDouble(0f, 1f, 0f), Vec |
| | | 44 | | |
| | | 45 | | /// <summary> |
| | | 46 | | /// Returns a matrix with all elements set to zero. |
| | | 47 | | /// </summary> |
| | 1 | 48 | | public static readonly Fixed3x3 Zero = new(Vector3d.FromDouble(0f, 0f, 0f), Vector3d.FromDouble(0f, 0f, 0f), Vector3 |
| | | 49 | | |
| | | 50 | | #endregion |
| | | 51 | | |
| | | 52 | | #region Fields |
| | | 53 | | |
| | | 54 | | // First row |
| | | 55 | | |
| | | 56 | | /// <summary> |
| | | 57 | | /// Represents the element in the first row and first column of the matrix. |
| | | 58 | | /// </summary> |
| | | 59 | | [JsonInclude] |
| | | 60 | | [MemoryPackOrder(0)] |
| | | 61 | | public Fixed64 M11; |
| | | 62 | | /// <summary> |
| | | 63 | | /// Represents the element in the first row and second column of the matrix. |
| | | 64 | | /// </summary> |
| | | 65 | | [JsonInclude] |
| | | 66 | | [MemoryPackOrder(1)] |
| | | 67 | | public Fixed64 M12; |
| | | 68 | | /// <summary> |
| | | 69 | | /// Represents the element in the first row and third column of the matrix. |
| | | 70 | | /// </summary> |
| | | 71 | | [JsonInclude] |
| | | 72 | | [MemoryPackOrder(2)] |
| | | 73 | | public Fixed64 M13; |
| | | 74 | | |
| | | 75 | | // Second Row |
| | | 76 | | |
| | | 77 | | /// <summary> |
| | | 78 | | /// Represents the element in the second row and first column of the matrix. |
| | | 79 | | /// </summary> |
| | | 80 | | [JsonInclude] |
| | | 81 | | [MemoryPackOrder(3)] |
| | | 82 | | public Fixed64 M21; |
| | | 83 | | /// <summary> |
| | | 84 | | /// Represents the element in the second row and second column of the matrix. |
| | | 85 | | /// </summary> |
| | | 86 | | [JsonInclude] |
| | | 87 | | [MemoryPackOrder(4)] |
| | | 88 | | public Fixed64 M22; |
| | | 89 | | /// <summary> |
| | | 90 | | /// Represents the element in the second row and third column of the matrix. |
| | | 91 | | /// </summary> |
| | | 92 | | [JsonInclude] |
| | | 93 | | [MemoryPackOrder(5)] |
| | | 94 | | public Fixed64 M23; |
| | | 95 | | |
| | | 96 | | // Third Row |
| | | 97 | | |
| | | 98 | | /// <summary> |
| | | 99 | | /// Represents the element in the third row and first column of the matrix. |
| | | 100 | | /// </summary> |
| | | 101 | | [JsonInclude] |
| | | 102 | | [MemoryPackOrder(6)] |
| | | 103 | | public Fixed64 M31; |
| | | 104 | | /// <summary> |
| | | 105 | | /// Represents the element in the third row and second column of the matrix. |
| | | 106 | | /// </summary> |
| | | 107 | | [JsonInclude] |
| | | 108 | | [MemoryPackOrder(7)] |
| | | 109 | | public Fixed64 M32; |
| | | 110 | | /// <summary> |
| | | 111 | | /// Represents the element in the third row and third column of the matrix. |
| | | 112 | | /// </summary> |
| | | 113 | | [JsonInclude] |
| | | 114 | | [MemoryPackOrder(8)] |
| | | 115 | | public Fixed64 M33; |
| | | 116 | | |
| | | 117 | | #endregion |
| | | 118 | | |
| | | 119 | | #region Constructors |
| | | 120 | | |
| | | 121 | | /// <summary> |
| | | 122 | | /// Initializes a new FixedMatrix3x3 with the specified elements. |
| | | 123 | | /// </summary> |
| | | 124 | | public Fixed3x3( |
| | | 125 | | Fixed64 m11, Fixed64 m12, Fixed64 m13, |
| | | 126 | | Fixed64 m21, Fixed64 m22, Fixed64 m23, |
| | | 127 | | Fixed64 m31, Fixed64 m32, Fixed64 m33 |
| | | 128 | | ) |
| | | 129 | | { |
| | 13935 | 130 | | M11 = m11; M12 = m12; M13 = m13; |
| | 13935 | 131 | | M21 = m21; M22 = m22; M23 = m23; |
| | 13935 | 132 | | M31 = m31; M32 = m32; M33 = m33; |
| | 4645 | 133 | | } |
| | | 134 | | |
| | | 135 | | /// <summary> |
| | | 136 | | /// Initializes a new FixedMatrix3x3 using three Vector3d values representing the rows. |
| | | 137 | | /// </summary> |
| | | 138 | | public Fixed3x3( |
| | | 139 | | Vector3d m11_m12_m13, |
| | | 140 | | Vector3d m21_m22_m23, |
| | | 141 | | Vector3d m31_m32_m33 |
| | 34 | 142 | | ) : this( |
| | 34 | 143 | | m11_m12_m13.X, |
| | 34 | 144 | | m11_m12_m13.Y, |
| | 34 | 145 | | m11_m12_m13.Z, |
| | 34 | 146 | | m21_m22_m23.X, |
| | 34 | 147 | | m21_m22_m23.Y, |
| | 34 | 148 | | m21_m22_m23.Z, |
| | 34 | 149 | | m31_m32_m33.X, |
| | 34 | 150 | | m31_m32_m33.Y, |
| | 34 | 151 | | m31_m32_m33.Z) |
| | 34 | 152 | | { } |
| | | 153 | | |
| | | 154 | | #endregion |
| | | 155 | | |
| | | 156 | | #region Properties |
| | | 157 | | |
| | | 158 | | /// <summary> |
| | | 159 | | /// Gets or sets the matrix element at the specified index. |
| | | 160 | | /// </summary> |
| | | 161 | | /// <remarks> |
| | | 162 | | /// The mapping between indices and matrix elements is non-sequential. |
| | | 163 | | /// Ensure that the index corresponds to a valid matrix element. |
| | | 164 | | /// </remarks> |
| | | 165 | | /// <param name="index">The zero-based index of the matrix element to get or set. Valid values are 0, 1, 2, 4, 5, 6, |
| | | 166 | | /// <returns>The matrix element at the specified index.</returns> |
| | | 167 | | /// <exception cref="IndexOutOfRangeException">Thrown when the specified index is not one of the valid matrix elemen |
| | | 168 | | [JsonIgnore] |
| | | 169 | | [MemoryPackIgnore] |
| | | 170 | | public Fixed64 this[int index] |
| | | 171 | | { |
| | | 172 | | get |
| | | 173 | | { |
| | 160573 | 174 | | return index switch |
| | 160573 | 175 | | { |
| | 17841 | 176 | | 0 => M11, |
| | 17841 | 177 | | 1 => M21, |
| | 17841 | 178 | | 2 => M31, |
| | 17841 | 179 | | 4 => M12, |
| | 17841 | 180 | | 5 => M22, |
| | 17841 | 181 | | 6 => M32, |
| | 17841 | 182 | | 8 => M13, |
| | 17841 | 183 | | 9 => M23, |
| | 17841 | 184 | | 10 => M33, |
| | 4 | 185 | | _ => throw new IndexOutOfRangeException("Invalid matrix index!"), |
| | 160573 | 186 | | }; |
| | | 187 | | } |
| | | 188 | | set |
| | | 189 | | { |
| | | 190 | | switch (index) |
| | | 191 | | { |
| | | 192 | | case 0: |
| | 1 | 193 | | M11 = value; |
| | 1 | 194 | | break; |
| | | 195 | | case 1: |
| | 1 | 196 | | M21 = value; |
| | 1 | 197 | | break; |
| | | 198 | | case 2: |
| | 1 | 199 | | M31 = value; |
| | 1 | 200 | | break; |
| | | 201 | | case 4: |
| | 1 | 202 | | M12 = value; |
| | 1 | 203 | | break; |
| | | 204 | | case 5: |
| | 1 | 205 | | M22 = value; |
| | 1 | 206 | | break; |
| | | 207 | | case 6: |
| | 1 | 208 | | M32 = value; |
| | 1 | 209 | | break; |
| | | 210 | | case 8: |
| | 1 | 211 | | M13 = value; |
| | 1 | 212 | | break; |
| | | 213 | | case 9: |
| | 1 | 214 | | M23 = value; |
| | 1 | 215 | | break; |
| | | 216 | | case 10: |
| | 1 | 217 | | M33 = value; |
| | 1 | 218 | | break; |
| | | 219 | | default: |
| | 4 | 220 | | throw new IndexOutOfRangeException("Invalid matrix index!"); |
| | | 221 | | } |
| | | 222 | | } |
| | | 223 | | } |
| | | 224 | | |
| | | 225 | | #endregion |
| | | 226 | | |
| | | 227 | | #region Methods (Instance) |
| | | 228 | | |
| | | 229 | | /// <inheritdoc cref="GetNormalized(Fixed3x3)" /> |
| | 1 | 230 | | public Fixed3x3 NormalizeInPlace() => this = GetNormalized(this); |
| | | 231 | | |
| | | 232 | | /// <summary> |
| | | 233 | | /// Calculates the determinant of a 3x3 matrix. |
| | | 234 | | /// </summary> |
| | | 235 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 236 | | public Fixed64 GetDeterminant() => |
| | 7 | 237 | | M11 * (M22 * M33 - M23 * M32) - |
| | 7 | 238 | | M12 * (M21 * M33 - M23 * M31) + |
| | 7 | 239 | | M13 * (M21 * M32 - M22 * M31); |
| | | 240 | | |
| | | 241 | | /// <summary> |
| | | 242 | | /// Inverts the diagonal elements of the matrix. |
| | | 243 | | /// </summary> |
| | | 244 | | /// <remarks> |
| | | 245 | | /// protects against the case where you would have an infinite value on the diagonal, which would cause problems in |
| | | 246 | | /// If m00 or m22 are zero, handle that as a special case and manually set the inverse to zero, |
| | | 247 | | /// since for a theoretical object with no inertia along those axes, it would be impossible to impart a rotation in |
| | | 248 | | /// |
| | | 249 | | /// bear in mind that having a zero on the inertia tensor's diagonal isn't generally valid for real, |
| | | 250 | | /// 3-dimensional objects (unless they are "infinitely thin" along one axis), |
| | | 251 | | /// so if you end up with such a tensor, it's a sign that something else might be wrong in your setup. |
| | | 252 | | /// </remarks> |
| | | 253 | | public Fixed3x3 InvertDiagonal() |
| | | 254 | | { |
| | | 255 | | try |
| | | 256 | | { |
| | 3 | 257 | | if (M22 == Fixed64.Zero) |
| | 1 | 258 | | throw new ArgumentException("Cannot invert a diagonal matrix with zero elements on the diagonal."); |
| | 2 | 259 | | } |
| | 1 | 260 | | catch (ArgumentException) |
| | | 261 | | { |
| | 1 | 262 | | return this; |
| | | 263 | | } |
| | | 264 | | |
| | 2 | 265 | | return new Fixed3x3( |
| | 2 | 266 | | M11 != Fixed64.Zero ? Fixed64.One / M11 : Fixed64.Zero, Fixed64.Zero, Fixed64.Zero, |
| | 2 | 267 | | Fixed64.Zero, Fixed64.One / M22, Fixed64.Zero, |
| | 2 | 268 | | Fixed64.Zero, Fixed64.Zero, M33 != Fixed64.Zero ? Fixed64.One / M33 : Fixed64.Zero |
| | 2 | 269 | | ); |
| | 1 | 270 | | } |
| | | 271 | | |
| | | 272 | | #endregion |
| | | 273 | | |
| | | 274 | | #region Static Matrix Generators and Transformations |
| | | 275 | | |
| | | 276 | | /// <summary> |
| | | 277 | | /// Creates a 3x3 matrix representing a rotation around the X-axis. |
| | | 278 | | /// </summary> |
| | | 279 | | /// <param name="angle">The angle of rotation in radians.</param> |
| | | 280 | | /// <returns>A 3x3 rotation matrix.</returns> |
| | | 281 | | public static Fixed3x3 CreateRotationX(Fixed64 angle) |
| | | 282 | | { |
| | 10 | 283 | | Fixed64 cos = FixedMath.Cos(angle); |
| | 10 | 284 | | Fixed64 sin = FixedMath.Sin(angle); |
| | | 285 | | |
| | 10 | 286 | | return new Fixed3x3( |
| | 10 | 287 | | Fixed64.One, Fixed64.Zero, Fixed64.Zero, |
| | 10 | 288 | | Fixed64.Zero, cos, sin, |
| | 10 | 289 | | Fixed64.Zero, -sin, cos |
| | 10 | 290 | | ); |
| | | 291 | | } |
| | | 292 | | |
| | | 293 | | /// <summary> |
| | | 294 | | /// Creates a 3x3 matrix representing a rotation around the Y-axis. |
| | | 295 | | /// </summary> |
| | | 296 | | /// <param name="angle">The angle of rotation in radians.</param> |
| | | 297 | | /// <returns>A 3x3 rotation matrix.</returns> |
| | | 298 | | public static Fixed3x3 CreateRotationY(Fixed64 angle) |
| | | 299 | | { |
| | 12 | 300 | | Fixed64 cos = FixedMath.Cos(angle); |
| | 12 | 301 | | Fixed64 sin = FixedMath.Sin(angle); |
| | | 302 | | |
| | 12 | 303 | | return new Fixed3x3( |
| | 12 | 304 | | cos, Fixed64.Zero, -sin, |
| | 12 | 305 | | Fixed64.Zero, Fixed64.One, Fixed64.Zero, |
| | 12 | 306 | | sin, Fixed64.Zero, cos |
| | 12 | 307 | | ); |
| | | 308 | | } |
| | | 309 | | |
| | | 310 | | /// <summary> |
| | | 311 | | /// Creates a 3x3 matrix representing a rotation around the Z-axis. |
| | | 312 | | /// </summary> |
| | | 313 | | /// <param name="angle">The angle of rotation in radians.</param> |
| | | 314 | | /// <returns>A 3x3 rotation matrix.</returns> |
| | | 315 | | public static Fixed3x3 CreateRotationZ(Fixed64 angle) |
| | | 316 | | { |
| | 7 | 317 | | Fixed64 cos = FixedMath.Cos(angle); |
| | 7 | 318 | | Fixed64 sin = FixedMath.Sin(angle); |
| | | 319 | | |
| | 7 | 320 | | return new Fixed3x3( |
| | 7 | 321 | | cos, sin, Fixed64.Zero, |
| | 7 | 322 | | -sin, cos, Fixed64.Zero, |
| | 7 | 323 | | Fixed64.Zero, Fixed64.Zero, Fixed64.One |
| | 7 | 324 | | ); |
| | | 325 | | } |
| | | 326 | | |
| | | 327 | | /// <summary> |
| | | 328 | | /// Creates a 3x3 shear matrix. |
| | | 329 | | /// </summary> |
| | | 330 | | /// <param name="shX">Shear factor along the X-axis.</param> |
| | | 331 | | /// <param name="shY">Shear factor along the Y-axis.</param> |
| | | 332 | | /// <param name="shZ">Shear factor along the Z-axis.</param> |
| | | 333 | | /// <returns>A 3x3 shear matrix.</returns> |
| | | 334 | | public static Fixed3x3 CreateShear(Fixed64 shX, Fixed64 shY, Fixed64 shZ) => |
| | 1 | 335 | | new(Fixed64.One, shX, shY, |
| | 1 | 336 | | shX, Fixed64.One, shZ, |
| | 1 | 337 | | shY, shZ, Fixed64.One); |
| | | 338 | | |
| | | 339 | | /// <summary> |
| | | 340 | | /// Creates a scaling matrix that applies a uniform or non-uniform scale transformation. |
| | | 341 | | /// </summary> |
| | | 342 | | /// <param name="scale">The scale factors along the X, Y, and Z axes.</param> |
| | | 343 | | /// <returns>A 3x3 scaling matrix.</returns> |
| | | 344 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 345 | | public static Fixed3x3 CreateScale(Vector3d scale) => |
| | 12 | 346 | | new(scale.X, Fixed64.Zero, Fixed64.Zero, |
| | 12 | 347 | | Fixed64.Zero, scale.Y, Fixed64.Zero, |
| | 12 | 348 | | Fixed64.Zero, Fixed64.Zero, scale.Z); |
| | | 349 | | |
| | | 350 | | /// <summary> |
| | | 351 | | /// Creates a uniform scaling matrix with the same scale factor on all axes. |
| | | 352 | | /// </summary> |
| | | 353 | | /// <param name="scaleFactor">The uniform scale factor.</param> |
| | | 354 | | /// <returns>A 3x3 scaling matrix with uniform scaling.</returns> |
| | | 355 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 356 | | public static Fixed3x3 CreateScale(Fixed64 scaleFactor) => |
| | 1 | 357 | | CreateScale(new Vector3d(scaleFactor, scaleFactor, scaleFactor)); |
| | | 358 | | |
| | | 359 | | /// <summary> |
| | | 360 | | /// Creates a symmetric matrix containing second-order barycentric product sums for three vectors. |
| | | 361 | | /// </summary> |
| | | 362 | | /// <remarks> |
| | | 363 | | /// The diagonal contains squared component sums, and the off-diagonal elements contain cross-component sums. |
| | | 364 | | /// </remarks> |
| | | 365 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 366 | | public static Fixed3x3 CreateBarycentricProductSums(Vector3d a, Vector3d b, Vector3d c) |
| | | 367 | | { |
| | 1 | 368 | | Fixed64 xx = FixedMath.SumSquaredBarycentricProducts(a.X, b.X, c.X); |
| | 1 | 369 | | Fixed64 yy = FixedMath.SumSquaredBarycentricProducts(a.Y, b.Y, c.Y); |
| | 1 | 370 | | Fixed64 zz = FixedMath.SumSquaredBarycentricProducts(a.Z, b.Z, c.Z); |
| | 1 | 371 | | Fixed64 xy = FixedMath.SumBarycentricProducts(a.X, b.X, c.X, a.Y, b.Y, c.Y); |
| | 1 | 372 | | Fixed64 xz = FixedMath.SumBarycentricProducts(a.X, b.X, c.X, a.Z, b.Z, c.Z); |
| | 1 | 373 | | Fixed64 yz = FixedMath.SumBarycentricProducts(a.Y, b.Y, c.Y, a.Z, b.Z, c.Z); |
| | | 374 | | |
| | 1 | 375 | | return new Fixed3x3( |
| | 1 | 376 | | xx, xy, xz, |
| | 1 | 377 | | xy, yy, yz, |
| | 1 | 378 | | xz, yz, zz); |
| | | 379 | | } |
| | | 380 | | |
| | | 381 | | /// <summary> |
| | | 382 | | /// Normalizes the basis vectors of a 3x3 matrix to ensure they are orthogonal and unit length. |
| | | 383 | | /// </summary> |
| | | 384 | | /// <remarks> |
| | | 385 | | /// This method recalculates and normalizes the X, Y, and Z basis vectors of the matrix to avoid numerical drift |
| | | 386 | | /// that can occur after multiple transformations. It also ensures that the Z-axis is recomputed to maintain |
| | | 387 | | /// orthogonality by taking the cross-product of the normalized X and Y axes. |
| | | 388 | | /// |
| | | 389 | | /// Use Cases: |
| | | 390 | | /// - Ensuring stability and correctness after repeated transformations involving rotation and scaling. |
| | | 391 | | /// - Useful in physics calculations where orthogonal matrices are required (e.g., inertia tensors or rotations). |
| | | 392 | | /// </remarks> |
| | | 393 | | public static Fixed3x3 GetNormalized(Fixed3x3 matrix) |
| | | 394 | | { |
| | 1 | 395 | | var x = new Vector3d(matrix.M11, matrix.M12, matrix.M13).NormalizeInPlace(); |
| | 1 | 396 | | var y = new Vector3d(matrix.M21, matrix.M22, matrix.M23).NormalizeInPlace(); |
| | 1 | 397 | | var z = Vector3d.Cross(x, y).NormalizeInPlace(); |
| | | 398 | | |
| | 3 | 399 | | matrix.M11 = x.X; matrix.M12 = x.Y; matrix.M13 = x.Z; |
| | 3 | 400 | | matrix.M21 = y.X; matrix.M22 = y.Y; matrix.M23 = y.Z; |
| | 3 | 401 | | matrix.M31 = z.X; matrix.M32 = z.Y; matrix.M33 = z.Z; |
| | | 402 | | |
| | 1 | 403 | | return matrix; |
| | | 404 | | } |
| | | 405 | | |
| | | 406 | | /// <summary> |
| | | 407 | | /// Extracts the unsigned magnitudes of the matrix basis rows. |
| | | 408 | | /// </summary> |
| | | 409 | | /// <returns>The nonnegative basis magnitudes along X, Y, and Z.</returns> |
| | | 410 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 411 | | public static Vector3d ExtractScaleMagnitudes(Fixed3x3 matrix) => |
| | 37 | 412 | | new(new Vector3d(matrix.M11, matrix.M12, matrix.M13).Magnitude, |
| | 37 | 413 | | new Vector3d(matrix.M21, matrix.M22, matrix.M23).Magnitude, |
| | 37 | 414 | | new Vector3d(matrix.M31, matrix.M32, matrix.M33).Magnitude); |
| | | 415 | | |
| | | 416 | | /// <summary> |
| | | 417 | | /// Extracts canonical signed lossy scale from the matrix basis rows. |
| | | 418 | | /// </summary> |
| | | 419 | | /// <remarks>A reflected basis assigns its one recoverable negative sign to X.</remarks> |
| | | 420 | | public static Vector3d ExtractLossyScale(Fixed3x3 matrix) |
| | | 421 | | { |
| | 17 | 422 | | Vector3d scale = ExtractScaleMagnitudes(matrix); |
| | 17 | 423 | | if (WideGeometry.GetTripleProductSign( |
| | 17 | 424 | | matrix.M11, matrix.M12, matrix.M13, |
| | 17 | 425 | | matrix.M21, matrix.M22, matrix.M23, |
| | 17 | 426 | | matrix.M31, matrix.M32, matrix.M33) < 0) |
| | | 427 | | { |
| | 5 | 428 | | scale.X = -scale.X; |
| | | 429 | | } |
| | | 430 | | |
| | 17 | 431 | | return scale; |
| | | 432 | | } |
| | | 433 | | |
| | | 434 | | #endregion |
| | | 435 | | |
| | | 436 | | #region Static Matrix Operations |
| | | 437 | | |
| | | 438 | | /// <summary> |
| | | 439 | | /// Linearly interpolates between two matrices. |
| | | 440 | | /// </summary> |
| | | 441 | | public static Fixed3x3 Lerp(Fixed3x3 a, Fixed3x3 b, Fixed64 t) => |
| | 3 | 442 | | new(FixedMath.Lerp(a.M11, b.M11, t), FixedMath.Lerp(a.M12, b.M12, t), FixedMath.Lerp(a.M13, b.M13, t), |
| | 3 | 443 | | FixedMath.Lerp(a.M21, b.M21, t), FixedMath.Lerp(a.M22, b.M22, t), FixedMath.Lerp(a.M23, b.M23, t), |
| | 3 | 444 | | FixedMath.Lerp(a.M31, b.M31, t), FixedMath.Lerp(a.M32, b.M32, t), FixedMath.Lerp(a.M33, b.M33, t)); |
| | | 445 | | |
| | | 446 | | /// <summary> |
| | | 447 | | /// Transposes the matrix (swaps rows and columns). |
| | | 448 | | /// </summary> |
| | | 449 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 450 | | public static Fixed3x3 Transpose(Fixed3x3 matrix) => |
| | 3 | 451 | | new(matrix.M11, matrix.M21, matrix.M31, |
| | 3 | 452 | | matrix.M12, matrix.M22, matrix.M32, |
| | 3 | 453 | | matrix.M13, matrix.M23, matrix.M33); |
| | | 454 | | |
| | | 455 | | /// <summary> |
| | | 456 | | /// Attempts to invert the matrix. If the determinant is zero, returns false and sets result to null. |
| | | 457 | | /// </summary> |
| | | 458 | | public static bool Invert(Fixed3x3 matrix, out Fixed3x3? result) |
| | | 459 | | { |
| | | 460 | | // Calculate the determinant |
| | 7 | 461 | | Fixed64 det = matrix.GetDeterminant(); |
| | | 462 | | |
| | 7 | 463 | | if (det == Fixed64.Zero) |
| | | 464 | | { |
| | 2 | 465 | | result = null; |
| | 2 | 466 | | return false; |
| | | 467 | | } |
| | | 468 | | |
| | | 469 | | // Calculate the inverse |
| | 5 | 470 | | Fixed64 invDet = Fixed64.One / det; |
| | | 471 | | |
| | | 472 | | // Compute the inverse matrix |
| | 5 | 473 | | result = new Fixed3x3( |
| | 5 | 474 | | invDet * (matrix.M22 * matrix.M33 - matrix.M32 * matrix.M23), |
| | 5 | 475 | | invDet * (matrix.M13 * matrix.M32 - matrix.M12 * matrix.M33), |
| | 5 | 476 | | invDet * (matrix.M12 * matrix.M23 - matrix.M13 * matrix.M22), |
| | 5 | 477 | | |
| | 5 | 478 | | invDet * (matrix.M23 * matrix.M31 - matrix.M21 * matrix.M33), |
| | 5 | 479 | | invDet * (matrix.M11 * matrix.M33 - matrix.M13 * matrix.M31), |
| | 5 | 480 | | invDet * (matrix.M13 * matrix.M21 - matrix.M11 * matrix.M23), |
| | 5 | 481 | | |
| | 5 | 482 | | invDet * (matrix.M21 * matrix.M32 - matrix.M22 * matrix.M31), |
| | 5 | 483 | | invDet * (matrix.M12 * matrix.M31 - matrix.M11 * matrix.M32), |
| | 5 | 484 | | invDet * (matrix.M11 * matrix.M22 - matrix.M12 * matrix.M21) |
| | 5 | 485 | | ); |
| | | 486 | | |
| | 5 | 487 | | return true; |
| | | 488 | | } |
| | | 489 | | |
| | | 490 | | /// <summary> |
| | | 491 | | /// Transforms a direction vector from local space to world space using this transformation matrix. |
| | | 492 | | /// Ignores translation. |
| | | 493 | | /// </summary> |
| | | 494 | | /// <remarks> |
| | | 495 | | /// FixedMathSharp applies matrices using a row-vector convention: <c>direction * matrix</c>. |
| | | 496 | | /// </remarks> |
| | | 497 | | /// <param name="matrix">The transformation matrix.</param> |
| | | 498 | | /// <param name="direction">The local-space direction vector.</param> |
| | | 499 | | /// <returns>The transformed direction in world space.</returns> |
| | | 500 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 501 | | public static Vector3d TransformDirection(Fixed3x3 matrix, Vector3d direction) => |
| | 13 | 502 | | new(direction.X * matrix.M11 + direction.Y * matrix.M21 + direction.Z * matrix.M31, |
| | 13 | 503 | | direction.X * matrix.M12 + direction.Y * matrix.M22 + direction.Z * matrix.M32, |
| | 13 | 504 | | direction.X * matrix.M13 + direction.Y * matrix.M23 + direction.Z * matrix.M33); |
| | | 505 | | |
| | | 506 | | /// <summary> |
| | | 507 | | /// Attempts to transform a direction with one final round-half-to-even |
| | | 508 | | /// conversion per component and no intermediate saturation. |
| | | 509 | | /// </summary> |
| | | 510 | | public static bool TryTransformDirection( |
| | | 511 | | Fixed3x3 matrix, |
| | | 512 | | Vector3d direction, |
| | | 513 | | out Vector3d result) |
| | | 514 | | { |
| | 67 | 515 | | bool representable = Fixed64.TryAddProducts( |
| | 67 | 516 | | direction.X, |
| | 67 | 517 | | matrix.M11, |
| | 67 | 518 | | direction.Y, |
| | 67 | 519 | | matrix.M21, |
| | 67 | 520 | | direction.Z, |
| | 67 | 521 | | matrix.M31, |
| | 67 | 522 | | out Fixed64 x) |
| | 67 | 523 | | & Fixed64.TryAddProducts( |
| | 67 | 524 | | direction.X, |
| | 67 | 525 | | matrix.M12, |
| | 67 | 526 | | direction.Y, |
| | 67 | 527 | | matrix.M22, |
| | 67 | 528 | | direction.Z, |
| | 67 | 529 | | matrix.M32, |
| | 67 | 530 | | out Fixed64 y) |
| | 67 | 531 | | & Fixed64.TryAddProducts( |
| | 67 | 532 | | direction.X, |
| | 67 | 533 | | matrix.M13, |
| | 67 | 534 | | direction.Y, |
| | 67 | 535 | | matrix.M23, |
| | 67 | 536 | | direction.Z, |
| | 67 | 537 | | matrix.M33, |
| | 67 | 538 | | out Fixed64 z); |
| | 67 | 539 | | if (!representable) |
| | | 540 | | { |
| | 1 | 541 | | result = default; |
| | 1 | 542 | | return false; |
| | | 543 | | } |
| | | 544 | | |
| | 66 | 545 | | result = new Vector3d(x, y, z); |
| | 66 | 546 | | return true; |
| | | 547 | | } |
| | | 548 | | |
| | | 549 | | /// <summary> |
| | | 550 | | /// Transforms a direction from world space into the local space of the matrix. |
| | | 551 | | /// Ignores translation. |
| | | 552 | | /// </summary> |
| | | 553 | | /// <param name="matrix">The transformation matrix.</param> |
| | | 554 | | /// <param name="direction">The world-space direction.</param> |
| | | 555 | | /// <returns>The transformed local-space direction.</returns> |
| | | 556 | | public static Vector3d InverseTransformDirection(Fixed3x3 matrix, Vector3d direction) |
| | | 557 | | { |
| | 5 | 558 | | bool canInvert = !Invert(matrix, out Fixed3x3? inverseMatrix) || !inverseMatrix.HasValue; |
| | 5 | 559 | | if (canInvert) |
| | 1 | 560 | | throw new InvalidOperationException("Matrix is not invertible."); |
| | | 561 | | |
| | 4 | 562 | | return TransformDirection(inverseMatrix!.Value, direction); |
| | | 563 | | } |
| | | 564 | | |
| | | 565 | | #endregion |
| | | 566 | | |
| | | 567 | | #region Operators |
| | | 568 | | |
| | | 569 | | /// <summary> |
| | | 570 | | /// Subtracts each corresponding element of one Fixed3x3 matrix from another. |
| | | 571 | | /// </summary> |
| | | 572 | | /// <param name="a">The first Fixed3x3 matrix (the minuend).</param> |
| | | 573 | | /// <param name="b">The second Fixed3x3 matrix (the subtrahend).</param> |
| | | 574 | | /// <returns> |
| | | 575 | | /// A Fixed3x3 matrix whose elements are the result of subtracting each element of parameter b from the correspondin |
| | | 576 | | /// </returns> |
| | | 577 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 578 | | public static Fixed3x3 operator -(Fixed3x3 a, Fixed3x3 b) => |
| | 1 | 579 | | new(a.M11 - b.M11, a.M12 - b.M12, a.M13 - b.M13, |
| | 1 | 580 | | a.M21 - b.M21, a.M22 - b.M22, a.M23 - b.M23, |
| | 1 | 581 | | a.M31 - b.M31, a.M32 - b.M32, a.M33 - b.M33); |
| | | 582 | | |
| | | 583 | | /// <summary> |
| | | 584 | | /// Adds two Fixed3x3 matrices element-wise. |
| | | 585 | | /// </summary> |
| | | 586 | | /// <param name="a">The first matrix to add.</param> |
| | | 587 | | /// <param name="b">The second matrix to add.</param> |
| | | 588 | | /// <returns>A Fixed3x3 matrix whose elements are the sums of the corresponding elements of the input matrices.</ret |
| | | 589 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 590 | | public static Fixed3x3 operator +(Fixed3x3 a, Fixed3x3 b) => |
| | 1 | 591 | | new(a.M11 + b.M11, a.M12 + b.M12, a.M13 + b.M13, |
| | 1 | 592 | | a.M21 + b.M21, a.M22 + b.M22, a.M23 + b.M23, |
| | 1 | 593 | | a.M31 + b.M31, a.M32 + b.M32, a.M33 + b.M33); |
| | | 594 | | |
| | | 595 | | /// <summary> |
| | | 596 | | /// Negates all elements of the matrix. |
| | | 597 | | /// </summary> |
| | | 598 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 599 | | public static Fixed3x3 operator -(Fixed3x3 a) => |
| | 1 | 600 | | new(-a.M11, -a.M12, -a.M13, |
| | 1 | 601 | | -a.M21, -a.M22, -a.M23, |
| | 1 | 602 | | -a.M31, -a.M32, -a.M33); |
| | | 603 | | |
| | | 604 | | /// <summary> |
| | | 605 | | /// Performs matrix multiplication on two 3x3 matrices. |
| | | 606 | | /// </summary> |
| | | 607 | | /// <remarks>Matrix multiplication is not commutative; the order of operands affects the result.</remarks> |
| | | 608 | | /// <param name="a">The first matrix to multiply.</param> |
| | | 609 | | /// <param name="b">The second matrix to multiply.</param> |
| | | 610 | | /// <returns>A new Fixed3x3 instance that is the product of the two input matrices.</returns> |
| | | 611 | | public static Fixed3x3 operator *(Fixed3x3 a, Fixed3x3 b) => |
| | 3 | 612 | | new(a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31, |
| | 3 | 613 | | a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32, |
| | 3 | 614 | | a.M11 * b.M13 + a.M12 * b.M23 + a.M13 * b.M33, |
| | 3 | 615 | | |
| | 3 | 616 | | a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31, |
| | 3 | 617 | | a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32, |
| | 3 | 618 | | a.M21 * b.M13 + a.M22 * b.M23 + a.M23 * b.M33, |
| | 3 | 619 | | |
| | 3 | 620 | | a.M31 * b.M11 + a.M32 * b.M21 + a.M33 * b.M31, |
| | 3 | 621 | | a.M31 * b.M12 + a.M32 * b.M22 + a.M33 * b.M32, |
| | 3 | 622 | | a.M31 * b.M13 + a.M32 * b.M23 + a.M33 * b.M33); |
| | | 623 | | |
| | | 624 | | /// <summary> |
| | | 625 | | /// Multiplies each element of the specified matrix by the given scalar value. |
| | | 626 | | /// </summary> |
| | | 627 | | /// <param name="a">The matrix whose elements are to be multiplied.</param> |
| | | 628 | | /// <param name="scalar">The scalar value by which to multiply each element of the matrix.</param> |
| | | 629 | | /// <returns> |
| | | 630 | | /// A new Fixed3x3 matrix whose elements are the result of multiplying each element of the input matrix by the scala |
| | | 631 | | /// </returns> |
| | | 632 | | public static Fixed3x3 operator *(Fixed3x3 a, Fixed64 scalar) => |
| | 2 | 633 | | new(a.M11 * scalar, a.M12 * scalar, a.M13 * scalar, |
| | 2 | 634 | | a.M21 * scalar, a.M22 * scalar, a.M23 * scalar, |
| | 2 | 635 | | a.M31 * scalar, a.M32 * scalar, a.M33 * scalar); |
| | | 636 | | |
| | | 637 | | /// <inheritdoc cref="operator *(Fixed3x3, Fixed64)"/> |
| | 1 | 638 | | public static Fixed3x3 operator *(Fixed64 scalar, Fixed3x3 a) => a * scalar; |
| | | 639 | | |
| | | 640 | | /// <summary> |
| | | 641 | | /// Divides each element of the specified matrix by the given scalar value. |
| | | 642 | | /// </summary> |
| | | 643 | | /// <remarks> |
| | | 644 | | /// Division is performed element-wise. |
| | | 645 | | /// The result may lose precision if the divisor does not evenly divide the matrix elements. |
| | | 646 | | /// </remarks> |
| | | 647 | | /// <param name="a">The matrix whose elements are to be divided.</param> |
| | | 648 | | /// <param name="divisor">The scalar value by which to divide each element of the matrix.</param> |
| | | 649 | | /// <returns> |
| | | 650 | | /// A new Fixed3x3 matrix whose elements are the result of dividing the corresponding elements of the input matrix b |
| | | 651 | | /// </returns> |
| | | 652 | | public static Fixed3x3 operator /(Fixed3x3 a, int divisor) => |
| | 1 | 653 | | new(a.M11 / divisor, a.M12 / divisor, a.M13 / divisor, |
| | 1 | 654 | | a.M21 / divisor, a.M22 / divisor, a.M23 / divisor, |
| | 1 | 655 | | a.M31 / divisor, a.M32 / divisor, a.M33 / divisor); |
| | | 656 | | |
| | | 657 | | /// <summary> |
| | | 658 | | /// Determines whether two Fixed3x3 instances are equal. |
| | | 659 | | /// </summary> |
| | | 660 | | /// <param name="left">The first Fixed3x3 instance to compare.</param> |
| | | 661 | | /// <param name="right">The second Fixed3x3 instance to compare.</param> |
| | | 662 | | /// <returns>true if the specified Fixed3x3 instances are equal; otherwise, false.</returns> |
| | | 663 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 664 | | public static bool operator ==(Fixed3x3 left, Fixed3x3 right) => left.Equals(right); |
| | | 665 | | |
| | | 666 | | /// <summary> |
| | | 667 | | /// Determines whether two Fixed3x3 instances are not equal. |
| | | 668 | | /// </summary> |
| | | 669 | | /// <param name="left">The first Fixed3x3 instance to compare.</param> |
| | | 670 | | /// <param name="right">The second Fixed3x3 instance to compare.</param> |
| | | 671 | | /// <returns>true if the specified Fixed3x3 instances are not equal; otherwise, false.</returns> |
| | | 672 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 1 | 673 | | public static bool operator !=(Fixed3x3 left, Fixed3x3 right) => !left.Equals(right); |
| | | 674 | | |
| | | 675 | | #endregion |
| | | 676 | | |
| | | 677 | | #region Equality and HashCode Overrides |
| | | 678 | | |
| | | 679 | | /// <inheritdoc/> |
| | | 680 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 681 | | public bool Equals(Fixed3x3 other) => |
| | 30 | 682 | | M11 == other.M11 && M12 == other.M12 && M13 == other.M13 && |
| | 30 | 683 | | M21 == other.M21 && M22 == other.M22 && M23 == other.M23 && |
| | 30 | 684 | | M31 == other.M31 && M32 == other.M32 && M33 == other.M33; |
| | | 685 | | |
| | | 686 | | |
| | | 687 | | /// <inheritdoc/> |
| | | 688 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 5 | 689 | | public override bool Equals(object? obj) => obj is Fixed3x3 other && Equals(other); |
| | | 690 | | |
| | | 691 | | /// <inheritdoc/> |
| | | 692 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 693 | | public override int GetHashCode() |
| | | 694 | | { |
| | | 695 | | unchecked |
| | | 696 | | { |
| | 3 | 697 | | int hash = 17; |
| | 3 | 698 | | hash = hash * 23 + M11.GetHashCode(); |
| | 3 | 699 | | hash = hash * 23 + M12.GetHashCode(); |
| | 3 | 700 | | hash = hash * 23 + M13.GetHashCode(); |
| | 3 | 701 | | hash = hash * 23 + M21.GetHashCode(); |
| | 3 | 702 | | hash = hash * 23 + M22.GetHashCode(); |
| | 3 | 703 | | hash = hash * 23 + M23.GetHashCode(); |
| | 3 | 704 | | hash = hash * 23 + M31.GetHashCode(); |
| | 3 | 705 | | hash = hash * 23 + M32.GetHashCode(); |
| | 3 | 706 | | hash = hash * 23 + M33.GetHashCode(); |
| | 3 | 707 | | return hash; |
| | | 708 | | } |
| | | 709 | | } |
| | | 710 | | |
| | | 711 | | #endregion |
| | | 712 | | |
| | | 713 | | #region Conversion |
| | | 714 | | |
| | | 715 | | /// <summary> |
| | | 716 | | /// Returns a string that represents the current matrix in a readable format. |
| | | 717 | | /// </summary> |
| | | 718 | | /// <remarks> |
| | | 719 | | /// This method is useful for debugging or logging the contents of the matrix. |
| | | 720 | | /// The returned string lists the matrix elements in row-major order. |
| | | 721 | | /// </remarks> |
| | | 722 | | /// <returns>A string containing the matrix elements formatted as "[m00, m01, m02; m10, m11, m12; m20, m21, m22]".</ |
| | | 723 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 54 | 724 | | public override string ToString() => ToString(null, CultureInfo.InvariantCulture); |
| | | 725 | | |
| | | 726 | | /// <summary> |
| | | 727 | | /// Returns a string that represents the current matrix in a readable format. |
| | | 728 | | /// </summary> |
| | | 729 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 730 | | public string ToString(string? format, IFormatProvider? formatProvider) |
| | | 731 | | { |
| | 55 | 732 | | Fixed3x3 value = this; |
| | 55 | 733 | | return FixedDiagnosticsFormatter.ToString((Span<char> destination, out int charsWritten) => |
| | 55 | 734 | | value.TryFormat(destination, out charsWritten, format.AsSpan(), formatProvider)); |
| | | 735 | | } |
| | | 736 | | |
| | | 737 | | /// <summary> |
| | | 738 | | /// Formats this matrix into the provided destination buffer. |
| | | 739 | | /// </summary> |
| | | 740 | | public bool TryFormat( |
| | | 741 | | Span<char> destination, |
| | | 742 | | out int charsWritten, |
| | | 743 | | ReadOnlySpan<char> format, |
| | | 744 | | IFormatProvider? provider) |
| | | 745 | | { |
| | 120 | 746 | | int written = 0; |
| | 120 | 747 | | if (!FixedDiagnosticsFormatter.Append('[', destination, ref written) || |
| | 120 | 748 | | !AppendRow(M11, M12, M13, destination, ref written, format, provider) || |
| | 120 | 749 | | !FixedDiagnosticsFormatter.Append("; ", destination, ref written) || |
| | 120 | 750 | | !AppendRow(M21, M22, M23, destination, ref written, format, provider) || |
| | 120 | 751 | | !FixedDiagnosticsFormatter.Append("; ", destination, ref written) || |
| | 120 | 752 | | !AppendRow(M31, M32, M33, destination, ref written, format, provider) || |
| | 120 | 753 | | !FixedDiagnosticsFormatter.Append(']', destination, ref written)) |
| | | 754 | | { |
| | 59 | 755 | | charsWritten = 0; |
| | 59 | 756 | | return false; |
| | | 757 | | } |
| | | 758 | | |
| | 61 | 759 | | charsWritten = written; |
| | 61 | 760 | | return true; |
| | | 761 | | } |
| | | 762 | | |
| | | 763 | | private static bool AppendRow( |
| | | 764 | | Fixed64 x, |
| | | 765 | | Fixed64 y, |
| | | 766 | | Fixed64 z, |
| | | 767 | | Span<char> destination, |
| | | 768 | | ref int charsWritten, |
| | | 769 | | ReadOnlySpan<char> format, |
| | | 770 | | IFormatProvider? provider) |
| | | 771 | | { |
| | 298 | 772 | | return FixedDiagnosticsFormatter.Append(x, destination, ref charsWritten, format, provider) && |
| | 298 | 773 | | FixedDiagnosticsFormatter.Append(", ", destination, ref charsWritten) && |
| | 298 | 774 | | FixedDiagnosticsFormatter.Append(y, destination, ref charsWritten, format, provider) && |
| | 298 | 775 | | FixedDiagnosticsFormatter.Append(", ", destination, ref charsWritten) && |
| | 298 | 776 | | FixedDiagnosticsFormatter.Append(z, destination, ref charsWritten, format, provider); |
| | | 777 | | } |
| | | 778 | | |
| | | 779 | | #endregion |
| | | 780 | | } |