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