| | | 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 | | [JsonInclude] |
| | | 43 | | [MemoryPackOrder(0)] |
| | | 44 | | public Fixed64 m00; |
| | | 45 | | [JsonInclude] |
| | | 46 | | [MemoryPackOrder(1)] |
| | | 47 | | public Fixed64 m01; |
| | | 48 | | [JsonInclude] |
| | | 49 | | [MemoryPackOrder(2)] |
| | | 50 | | public Fixed64 m02; |
| | | 51 | | |
| | | 52 | | [JsonInclude] |
| | | 53 | | [MemoryPackOrder(3)] |
| | | 54 | | public Fixed64 m10; |
| | | 55 | | [JsonInclude] |
| | | 56 | | [MemoryPackOrder(4)] |
| | | 57 | | public Fixed64 m11; |
| | | 58 | | [JsonInclude] |
| | | 59 | | [MemoryPackOrder(5)] |
| | | 60 | | public Fixed64 m12; |
| | | 61 | | |
| | | 62 | | [JsonInclude] |
| | | 63 | | [MemoryPackOrder(6)] |
| | | 64 | | public Fixed64 m20; |
| | | 65 | | [JsonInclude] |
| | | 66 | | [MemoryPackOrder(7)] |
| | | 67 | | public Fixed64 m21; |
| | | 68 | | [JsonInclude] |
| | | 69 | | [MemoryPackOrder(8)] |
| | | 70 | | public Fixed64 m22; |
| | | 71 | | |
| | | 72 | | #endregion |
| | | 73 | | |
| | | 74 | | #region Constructors |
| | | 75 | | |
| | | 76 | | /// <summary> |
| | | 77 | | /// Initializes a new FixedMatrix3x3 with the specified elements. |
| | | 78 | | /// </summary> |
| | | 79 | | public Fixed3x3( |
| | | 80 | | Fixed64 m00, Fixed64 m01, Fixed64 m02, |
| | | 81 | | Fixed64 m10, Fixed64 m11, Fixed64 m12, |
| | | 82 | | Fixed64 m20, Fixed64 m21, Fixed64 m22 |
| | | 83 | | ) |
| | 99 | 84 | | { |
| | 297 | 85 | | this.m00 = m00; this.m01 = m01; this.m02 = m02; |
| | 297 | 86 | | this.m10 = m10; this.m11 = m11; this.m12 = m12; |
| | 297 | 87 | | this.m20 = m20; this.m21 = m21; this.m22 = m22; |
| | 99 | 88 | | } |
| | | 89 | | |
| | | 90 | | /// <summary> |
| | | 91 | | /// Initializes a new FixedMatrix3x3 using three Vector3d values representing the rows. |
| | | 92 | | /// </summary> |
| | | 93 | | public Fixed3x3( |
| | | 94 | | Vector3d m00_m01_m02, |
| | | 95 | | Vector3d m10_m11_m12, |
| | | 96 | | Vector3d m20_m21_m22 |
| | 12 | 97 | | ) : 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 |
| | | 98 | | |
| | | 99 | | #endregion |
| | | 100 | | |
| | | 101 | | #region Properties |
| | | 102 | | |
| | | 103 | | [JsonIgnore] |
| | | 104 | | [MemoryPackIgnore] |
| | | 105 | | public Fixed64 this[int index] |
| | | 106 | | { |
| | | 107 | | get |
| | 13 | 108 | | { |
| | 13 | 109 | | return index switch |
| | 13 | 110 | | { |
| | 1 | 111 | | 0 => m00, |
| | 1 | 112 | | 1 => m10, |
| | 1 | 113 | | 2 => m20, |
| | 1 | 114 | | 4 => m01, |
| | 1 | 115 | | 5 => m11, |
| | 1 | 116 | | 6 => m21, |
| | 1 | 117 | | 8 => m02, |
| | 1 | 118 | | 9 => m12, |
| | 1 | 119 | | 10 => m22, |
| | 4 | 120 | | _ => throw new IndexOutOfRangeException("Invalid matrix index!"), |
| | 13 | 121 | | }; |
| | 9 | 122 | | } |
| | | 123 | | set |
| | 13 | 124 | | { |
| | 13 | 125 | | switch (index) |
| | | 126 | | { |
| | | 127 | | case 0: |
| | 1 | 128 | | m00 = value; |
| | 1 | 129 | | break; |
| | | 130 | | case 1: |
| | 1 | 131 | | m10 = value; |
| | 1 | 132 | | break; |
| | | 133 | | case 2: |
| | 1 | 134 | | m20 = value; |
| | 1 | 135 | | break; |
| | | 136 | | case 4: |
| | 1 | 137 | | m01 = value; |
| | 1 | 138 | | break; |
| | | 139 | | case 5: |
| | 1 | 140 | | m11 = value; |
| | 1 | 141 | | break; |
| | | 142 | | case 6: |
| | 1 | 143 | | m21 = value; |
| | 1 | 144 | | break; |
| | | 145 | | case 8: |
| | 1 | 146 | | m02 = value; |
| | 1 | 147 | | break; |
| | | 148 | | case 9: |
| | 1 | 149 | | m12 = value; |
| | 1 | 150 | | break; |
| | | 151 | | case 10: |
| | 1 | 152 | | m22 = value; |
| | 1 | 153 | | break; |
| | | 154 | | default: |
| | 4 | 155 | | throw new IndexOutOfRangeException("Invalid matrix index!"); |
| | | 156 | | } |
| | 9 | 157 | | } |
| | | 158 | | } |
| | | 159 | | |
| | | 160 | | #endregion |
| | | 161 | | |
| | | 162 | | #region Methods (Instance) |
| | | 163 | | |
| | | 164 | | /// <inheritdoc cref="Normalize(Fixed3x3)" /> |
| | | 165 | | public Fixed3x3 Normalize() |
| | 3 | 166 | | { |
| | 3 | 167 | | return this = Normalize(this); |
| | 3 | 168 | | } |
| | | 169 | | |
| | | 170 | | /// <inheritdoc cref="ResetScaleToIdentity(Fixed3x3)" /> |
| | | 171 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 172 | | public Fixed3x3 ResetScaleToIdentity() |
| | 2 | 173 | | { |
| | 2 | 174 | | return this = ResetScaleToIdentity(this); |
| | 2 | 175 | | } |
| | | 176 | | |
| | | 177 | | /// <summary> |
| | | 178 | | /// Calculates the determinant of a 3x3 matrix. |
| | | 179 | | /// </summary> |
| | | 180 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 181 | | public Fixed64 GetDeterminant() |
| | 5 | 182 | | { |
| | 5 | 183 | | return m00 * (m11 * m22 - m12 * m21) - |
| | 5 | 184 | | m01 * (m10 * m22 - m12 * m20) + |
| | 5 | 185 | | m02 * (m10 * m21 - m11 * m20); |
| | 5 | 186 | | } |
| | | 187 | | |
| | | 188 | | /// <summary> |
| | | 189 | | /// Inverts the diagonal elements of the matrix. |
| | | 190 | | /// </summary> |
| | | 191 | | /// <remarks> |
| | | 192 | | /// protects against the case where you would have an infinite value on the diagonal, which would cause problems in |
| | | 193 | | /// If m00 or m22 are zero, handle that as a special case and manually set the inverse to zero, |
| | | 194 | | /// since for a theoretical object with no inertia along those axes, it would be impossible to impart a rotation in |
| | | 195 | | /// |
| | | 196 | | /// bear in mind that having a zero on the inertia tensor's diagonal isn't generally valid for real, |
| | | 197 | | /// 3-dimensional objects (unless they are "infinitely thin" along one axis), |
| | | 198 | | /// so if you end up with such a tensor, it's a sign that something else might be wrong in your setup. |
| | | 199 | | /// </remarks> |
| | | 200 | | public Fixed3x3 InvertDiagonal() |
| | 2 | 201 | | { |
| | 2 | 202 | | if (m11 == Fixed64.Zero) |
| | 1 | 203 | | { |
| | 1 | 204 | | Console.WriteLine("Cannot invert a diagonal matrix with zero elements on the diagonal."); |
| | 1 | 205 | | return this; |
| | | 206 | | } |
| | | 207 | | |
| | 1 | 208 | | return new Fixed3x3( |
| | 1 | 209 | | m00 != Fixed64.Zero ? Fixed64.One / m00 : Fixed64.Zero, Fixed64.Zero, Fixed64.Zero, |
| | 1 | 210 | | Fixed64.Zero, Fixed64.One / m11, Fixed64.Zero, |
| | 1 | 211 | | Fixed64.Zero, Fixed64.Zero, m22 != Fixed64.Zero ? Fixed64.One / m22 : Fixed64.Zero |
| | 1 | 212 | | ); |
| | 2 | 213 | | } |
| | | 214 | | |
| | | 215 | | #endregion |
| | | 216 | | |
| | | 217 | | #region Static Matrix Generators and Transformations |
| | | 218 | | |
| | | 219 | | /// <summary> |
| | | 220 | | /// Creates a 3x3 matrix representing a rotation around the X-axis. |
| | | 221 | | /// </summary> |
| | | 222 | | /// <param name="angle">The angle of rotation in radians.</param> |
| | | 223 | | /// <returns>A 3x3 rotation matrix.</returns> |
| | | 224 | | public static Fixed3x3 CreateRotationX(Fixed64 angle) |
| | 4 | 225 | | { |
| | 4 | 226 | | Fixed64 cos = FixedMath.Cos(angle); |
| | 4 | 227 | | Fixed64 sin = FixedMath.Sin(angle); |
| | | 228 | | |
| | 4 | 229 | | return new Fixed3x3( |
| | 4 | 230 | | Fixed64.One, Fixed64.Zero, Fixed64.Zero, |
| | 4 | 231 | | Fixed64.Zero, cos, -sin, |
| | 4 | 232 | | Fixed64.Zero, sin, cos |
| | 4 | 233 | | ); |
| | 4 | 234 | | } |
| | | 235 | | |
| | | 236 | | /// <summary> |
| | | 237 | | /// Creates a 3x3 matrix representing a rotation around the Y-axis. |
| | | 238 | | /// </summary> |
| | | 239 | | /// <param name="angle">The angle of rotation in radians.</param> |
| | | 240 | | /// <returns>A 3x3 rotation matrix.</returns> |
| | | 241 | | public static Fixed3x3 CreateRotationY(Fixed64 angle) |
| | 5 | 242 | | { |
| | 5 | 243 | | Fixed64 cos = FixedMath.Cos(angle); |
| | 5 | 244 | | Fixed64 sin = FixedMath.Sin(angle); |
| | | 245 | | |
| | 5 | 246 | | return new Fixed3x3( |
| | 5 | 247 | | cos, Fixed64.Zero, sin, |
| | 5 | 248 | | Fixed64.Zero, Fixed64.One, Fixed64.Zero, |
| | 5 | 249 | | -sin, Fixed64.Zero, cos |
| | 5 | 250 | | ); |
| | 5 | 251 | | } |
| | | 252 | | |
| | | 253 | | /// <summary> |
| | | 254 | | /// Creates a 3x3 matrix representing a rotation around the Z-axis. |
| | | 255 | | /// </summary> |
| | | 256 | | /// <param name="angle">The angle of rotation in radians.</param> |
| | | 257 | | /// <returns>A 3x3 rotation matrix.</returns> |
| | | 258 | | public static Fixed3x3 CreateRotationZ(Fixed64 angle) |
| | 2 | 259 | | { |
| | 2 | 260 | | Fixed64 cos = FixedMath.Cos(angle); |
| | 2 | 261 | | Fixed64 sin = FixedMath.Sin(angle); |
| | | 262 | | |
| | 2 | 263 | | return new Fixed3x3( |
| | 2 | 264 | | cos, -sin, Fixed64.Zero, |
| | 2 | 265 | | sin, cos, Fixed64.Zero, |
| | 2 | 266 | | Fixed64.Zero, Fixed64.Zero, Fixed64.One |
| | 2 | 267 | | ); |
| | 2 | 268 | | } |
| | | 269 | | |
| | | 270 | | /// <summary> |
| | | 271 | | /// Creates a 3x3 shear matrix. |
| | | 272 | | /// </summary> |
| | | 273 | | /// <param name="shX">Shear factor along the X-axis.</param> |
| | | 274 | | /// <param name="shY">Shear factor along the Y-axis.</param> |
| | | 275 | | /// <param name="shZ">Shear factor along the Z-axis.</param> |
| | | 276 | | /// <returns>A 3x3 shear matrix.</returns> |
| | | 277 | | public static Fixed3x3 CreateShear(Fixed64 shX, Fixed64 shY, Fixed64 shZ) |
| | 1 | 278 | | { |
| | 1 | 279 | | return new Fixed3x3( |
| | 1 | 280 | | Fixed64.One, shX, shY, |
| | 1 | 281 | | shX, Fixed64.One, shZ, |
| | 1 | 282 | | shY, shZ, Fixed64.One |
| | 1 | 283 | | ); |
| | 1 | 284 | | } |
| | | 285 | | |
| | | 286 | | /// <summary> |
| | | 287 | | /// Creates a scaling matrix that applies a uniform or non-uniform scale transformation. |
| | | 288 | | /// </summary> |
| | | 289 | | /// <param name="scale">The scale factors along the X, Y, and Z axes.</param> |
| | | 290 | | /// <returns>A 3x3 scaling matrix.</returns> |
| | | 291 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 292 | | public static Fixed3x3 CreateScale(Vector3d scale) |
| | 7 | 293 | | { |
| | 7 | 294 | | return new Fixed3x3( |
| | 7 | 295 | | scale.x, Fixed64.Zero, Fixed64.Zero, |
| | 7 | 296 | | Fixed64.Zero, scale.y, Fixed64.Zero, |
| | 7 | 297 | | Fixed64.Zero, Fixed64.Zero, scale.z |
| | 7 | 298 | | ); |
| | 7 | 299 | | } |
| | | 300 | | |
| | | 301 | | /// <summary> |
| | | 302 | | /// Creates a uniform scaling matrix with the same scale factor on all axes. |
| | | 303 | | /// </summary> |
| | | 304 | | /// <param name="scaleFactor">The uniform scale factor.</param> |
| | | 305 | | /// <returns>A 3x3 scaling matrix with uniform scaling.</returns> |
| | | 306 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 307 | | public static Fixed3x3 CreateScale(Fixed64 scaleFactor) |
| | 0 | 308 | | { |
| | 0 | 309 | | return CreateScale(new Vector3d(scaleFactor, scaleFactor, scaleFactor)); |
| | 0 | 310 | | } |
| | | 311 | | |
| | | 312 | | /// <summary> |
| | | 313 | | /// Normalizes the basis vectors of a 3x3 matrix to ensure they are orthogonal and unit length. |
| | | 314 | | /// </summary> |
| | | 315 | | /// <remarks> |
| | | 316 | | /// This method recalculates and normalizes the X, Y, and Z basis vectors of the matrix to avoid numerical drift |
| | | 317 | | /// that can occur after multiple transformations. It also ensures that the Z-axis is recomputed to maintain |
| | | 318 | | /// orthogonality by taking the cross-product of the normalized X and Y axes. |
| | | 319 | | /// |
| | | 320 | | /// Use Cases: |
| | | 321 | | /// - Ensuring stability and correctness after repeated transformations involving rotation and scaling. |
| | | 322 | | /// - Useful in physics calculations where orthogonal matrices are required (e.g., inertia tensors or rotations). |
| | | 323 | | /// </remarks> |
| | | 324 | | public static Fixed3x3 Normalize(Fixed3x3 matrix) |
| | 3 | 325 | | { |
| | 3 | 326 | | var x = new Vector3d(matrix.m00, matrix.m01, matrix.m02).Normalize(); |
| | 3 | 327 | | var y = new Vector3d(matrix.m10, matrix.m11, matrix.m12).Normalize(); |
| | 3 | 328 | | var z = Vector3d.Cross(x, y).Normalize(); |
| | | 329 | | |
| | 9 | 330 | | matrix.m00 = x.x; matrix.m01 = x.y; matrix.m02 = x.z; |
| | 9 | 331 | | matrix.m10 = y.x; matrix.m11 = y.y; matrix.m12 = y.z; |
| | 9 | 332 | | matrix.m20 = z.x; matrix.m21 = z.y; matrix.m22 = z.z; |
| | | 333 | | |
| | 3 | 334 | | return matrix; |
| | 3 | 335 | | } |
| | | 336 | | |
| | | 337 | | /// <summary> |
| | | 338 | | /// Resets the scaling part of the matrix to identity (1,1,1). |
| | | 339 | | /// </summary> |
| | | 340 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 341 | | public static Fixed3x3 ResetScaleToIdentity(Fixed3x3 matrix) |
| | 2 | 342 | | { |
| | 2 | 343 | | matrix.m00 = Fixed64.One; // Reset scale on X-axis |
| | 2 | 344 | | matrix.m11 = Fixed64.One; // Reset scale on Y-axis |
| | 2 | 345 | | matrix.m22 = Fixed64.One; // Reset scale on Z-axis |
| | 2 | 346 | | return matrix; |
| | 2 | 347 | | } |
| | | 348 | | |
| | | 349 | | /// <inheritdoc cref="SetLossyScale(Fixed64, Fixed64, Fixed64)" /> |
| | | 350 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 351 | | public static Fixed3x3 SetLossyScale(Vector3d scale) |
| | 1 | 352 | | { |
| | 1 | 353 | | return SetLossyScale(scale.x, scale.y, scale.z); |
| | 1 | 354 | | } |
| | | 355 | | |
| | | 356 | | /// <summary> |
| | | 357 | | /// Creates a scaling matrix (puts the 'scale' vector down the diagonal) |
| | | 358 | | /// </summary> |
| | | 359 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 360 | | public static Fixed3x3 SetLossyScale(Fixed64 x, Fixed64 y, Fixed64 z) |
| | 2 | 361 | | { |
| | 2 | 362 | | return new Fixed3x3( |
| | 2 | 363 | | x, Fixed64.Zero, Fixed64.Zero, |
| | 2 | 364 | | Fixed64.Zero, y, Fixed64.Zero, |
| | 2 | 365 | | Fixed64.Zero, Fixed64.Zero, z |
| | 2 | 366 | | ); |
| | 2 | 367 | | } |
| | | 368 | | |
| | | 369 | | /// <summary> |
| | | 370 | | /// Applies the provided local scale to the matrix by modifying the diagonal elements. |
| | | 371 | | /// </summary> |
| | | 372 | | /// <param name="matrix">The matrix to set the scale against.</param> |
| | | 373 | | /// <param name="localScale">A Vector3d representing the local scale to apply.</param> |
| | | 374 | | public static Fixed3x3 SetScale(Fixed3x3 matrix, Vector3d localScale) |
| | 4 | 375 | | { |
| | 4 | 376 | | matrix.m00 = localScale.x; // Apply scale on X-axis |
| | 4 | 377 | | matrix.m11 = localScale.y; // Apply scale on Y-axis |
| | 4 | 378 | | matrix.m22 = localScale.z; // Apply scale on Z-axis |
| | | 379 | | |
| | 4 | 380 | | return matrix; |
| | 4 | 381 | | } |
| | | 382 | | |
| | | 383 | | /// <summary> |
| | | 384 | | /// Sets the global scale of an object using FixedMatrix3x3. |
| | | 385 | | /// Similar to SetGlobalScale for FixedMatrix4x4, but for a 3x3 matrix. |
| | | 386 | | /// </summary> |
| | | 387 | | /// <param name="matrix">The transformation matrix (3x3) representing the object's global state.</param> |
| | | 388 | | /// <param name="globalScale">The desired global scale represented as a Vector3d.</param> |
| | | 389 | | /// <remarks> |
| | | 390 | | /// The method extracts the current global scale from the matrix and computes the new local scale |
| | | 391 | | /// by dividing the desired global scale by the current global scale. |
| | | 392 | | /// The new local scale is then applied to the matrix. |
| | | 393 | | /// </remarks> |
| | | 394 | | public static Fixed3x3 SetGlobalScale(Fixed3x3 matrix, Vector3d globalScale) |
| | 2 | 395 | | { |
| | | 396 | | // normalize the matrix to avoid drift in the rotation component |
| | 2 | 397 | | matrix.Normalize(); |
| | | 398 | | |
| | | 399 | | // Reset the local scaling portion of the matrix |
| | 2 | 400 | | matrix.ResetScaleToIdentity(); |
| | | 401 | | |
| | | 402 | | // Compute the new local scale by dividing the desired global scale by the current global scale |
| | 2 | 403 | | Vector3d newLocalScale = new Vector3d( |
| | 2 | 404 | | globalScale.x / Fixed64.One, |
| | 2 | 405 | | globalScale.y / Fixed64.One, |
| | 2 | 406 | | globalScale.z / Fixed64.One |
| | 2 | 407 | | ); |
| | | 408 | | |
| | | 409 | | // Apply the new local scale to the matrix |
| | 2 | 410 | | return matrix.SetScale(newLocalScale); |
| | 2 | 411 | | } |
| | | 412 | | |
| | | 413 | | /// <summary> |
| | | 414 | | /// Extracts the scaling factors from the matrix by returning the diagonal elements. |
| | | 415 | | /// </summary> |
| | | 416 | | /// <returns>A Vector3d representing the scale along X, Y, and Z axes.</returns> |
| | | 417 | | public static Vector3d ExtractScale(Fixed3x3 matrix) |
| | 5 | 418 | | { |
| | 5 | 419 | | return new Vector3d( |
| | 5 | 420 | | new Vector3d(matrix.m00, matrix.m01, matrix.m02).Magnitude, |
| | 5 | 421 | | new Vector3d(matrix.m10, matrix.m11, matrix.m12).Magnitude, |
| | 5 | 422 | | new Vector3d(matrix.m20, matrix.m21, matrix.m22).Magnitude |
| | 5 | 423 | | ); |
| | 5 | 424 | | } |
| | | 425 | | |
| | | 426 | | /// <summary> |
| | | 427 | | /// Extracts the scaling factors from the matrix by returning the diagonal elements (lossy). |
| | | 428 | | /// </summary> |
| | | 429 | | /// <returns>A Vector3d representing the scale along X, Y, and Z axes (lossy).</returns> |
| | | 430 | | public static Vector3d ExtractLossyScale(Fixed3x3 matrix) |
| | 1 | 431 | | { |
| | 1 | 432 | | return new Vector3d(matrix.m00, matrix.m11, matrix.m22); |
| | 1 | 433 | | } |
| | | 434 | | |
| | | 435 | | #endregion |
| | | 436 | | |
| | | 437 | | #region Static Matrix Operations |
| | | 438 | | |
| | | 439 | | /// <summary> |
| | | 440 | | /// Linearly interpolates between two matrices. |
| | | 441 | | /// </summary> |
| | | 442 | | public static Fixed3x3 Lerp(Fixed3x3 a, Fixed3x3 b, Fixed64 t) |
| | 1 | 443 | | { |
| | | 444 | | // Perform a linear interpolation between two matrices |
| | 1 | 445 | | return new Fixed3x3( |
| | 1 | 446 | | FixedMath.LinearInterpolate(a.m00, b.m00, t), FixedMath.LinearInterpolate(a.m01, b.m01, t), FixedMath.Linear |
| | 1 | 447 | | FixedMath.LinearInterpolate(a.m10, b.m10, t), FixedMath.LinearInterpolate(a.m11, b.m11, t), FixedMath.Linear |
| | 1 | 448 | | FixedMath.LinearInterpolate(a.m20, b.m20, t), FixedMath.LinearInterpolate(a.m21, b.m21, t), FixedMath.Linear |
| | 1 | 449 | | ); |
| | 1 | 450 | | } |
| | | 451 | | |
| | | 452 | | /// <summary> |
| | | 453 | | /// Transposes the matrix (swaps rows and columns). |
| | | 454 | | /// </summary> |
| | | 455 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 456 | | public static Fixed3x3 Transpose(Fixed3x3 matrix) |
| | 1 | 457 | | { |
| | 1 | 458 | | return new Fixed3x3( |
| | 1 | 459 | | matrix.m00, matrix.m10, matrix.m20, |
| | 1 | 460 | | matrix.m01, matrix.m11, matrix.m21, |
| | 1 | 461 | | matrix.m02, matrix.m12, matrix.m22 |
| | 1 | 462 | | ); |
| | 1 | 463 | | } |
| | | 464 | | |
| | | 465 | | /// <summary> |
| | | 466 | | /// Attempts to invert the matrix. If the determinant is zero, returns false and sets result to null. |
| | | 467 | | /// </summary> |
| | | 468 | | public static bool Invert(Fixed3x3 matrix, out Fixed3x3? result) |
| | 5 | 469 | | { |
| | | 470 | | // Calculate the determinant |
| | 5 | 471 | | Fixed64 det = matrix.GetDeterminant(); |
| | | 472 | | |
| | 5 | 473 | | if (det == Fixed64.Zero) |
| | 2 | 474 | | { |
| | 2 | 475 | | result = null; |
| | 2 | 476 | | return false; |
| | | 477 | | } |
| | | 478 | | |
| | | 479 | | // Calculate the inverse |
| | 3 | 480 | | Fixed64 invDet = Fixed64.One / det; |
| | | 481 | | |
| | | 482 | | // Compute the inverse matrix |
| | 3 | 483 | | result = new Fixed3x3( |
| | 3 | 484 | | invDet * (matrix.m11 * matrix.m22 - matrix.m21 * matrix.m12), |
| | 3 | 485 | | invDet * (matrix.m02 * matrix.m21 - matrix.m01 * matrix.m22), |
| | 3 | 486 | | invDet * (matrix.m01 * matrix.m12 - matrix.m02 * matrix.m11), |
| | 3 | 487 | | |
| | 3 | 488 | | invDet * (matrix.m12 * matrix.m20 - matrix.m10 * matrix.m22), |
| | 3 | 489 | | invDet * (matrix.m00 * matrix.m22 - matrix.m02 * matrix.m20), |
| | 3 | 490 | | invDet * (matrix.m02 * matrix.m10 - matrix.m00 * matrix.m12), |
| | 3 | 491 | | |
| | 3 | 492 | | invDet * (matrix.m10 * matrix.m21 - matrix.m11 * matrix.m20), |
| | 3 | 493 | | invDet * (matrix.m01 * matrix.m20 - matrix.m00 * matrix.m21), |
| | 3 | 494 | | invDet * (matrix.m00 * matrix.m11 - matrix.m01 * matrix.m10) |
| | 3 | 495 | | ); |
| | | 496 | | |
| | 3 | 497 | | return true; |
| | 5 | 498 | | } |
| | | 499 | | |
| | | 500 | | /// <summary> |
| | | 501 | | /// Transforms a direction vector from local space to world space using this transformation matrix. |
| | | 502 | | /// Ignores translation. |
| | | 503 | | /// </summary> |
| | | 504 | | /// <param name="matrix">The transformation matrix.</param> |
| | | 505 | | /// <param name="direction">The local-space direction vector.</param> |
| | | 506 | | /// <returns>The transformed direction in world space.</returns> |
| | | 507 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 508 | | public static Vector3d TransformDirection(Fixed3x3 matrix, Vector3d direction) |
| | 10 | 509 | | { |
| | 10 | 510 | | return new Vector3d( |
| | 10 | 511 | | matrix.m00 * direction.x + matrix.m01 * direction.y + matrix.m02 * direction.z, |
| | 10 | 512 | | matrix.m10 * direction.x + matrix.m11 * direction.y + matrix.m12 * direction.z, |
| | 10 | 513 | | matrix.m20 * direction.x + matrix.m21 * direction.y + matrix.m22 * direction.z |
| | 10 | 514 | | ); |
| | 10 | 515 | | } |
| | | 516 | | |
| | | 517 | | /// <summary> |
| | | 518 | | /// Transforms a direction from world space into the local space of the matrix. |
| | | 519 | | /// Ignores translation. |
| | | 520 | | /// </summary> |
| | | 521 | | /// <param name="matrix">The transformation matrix.</param> |
| | | 522 | | /// <param name="direction">The world-space direction.</param> |
| | | 523 | | /// <returns>The transformed local-space direction.</returns> |
| | | 524 | | public static Vector3d InverseTransformDirection(Fixed3x3 matrix, Vector3d direction) |
| | 3 | 525 | | { |
| | 3 | 526 | | if (!Invert(matrix, out Fixed3x3? inverseMatrix) || !inverseMatrix.HasValue) |
| | 1 | 527 | | throw new InvalidOperationException("Matrix is not invertible."); |
| | | 528 | | |
| | 2 | 529 | | return new Vector3d( |
| | 2 | 530 | | inverseMatrix.Value.m00 * direction.x + inverseMatrix.Value.m01 * direction.y + inverseMatrix.Value.m02 * di |
| | 2 | 531 | | inverseMatrix.Value.m10 * direction.x + inverseMatrix.Value.m11 * direction.y + inverseMatrix.Value.m12 * di |
| | 2 | 532 | | inverseMatrix.Value.m20 * direction.x + inverseMatrix.Value.m21 * direction.y + inverseMatrix.Value.m22 * di |
| | 2 | 533 | | ); |
| | 2 | 534 | | } |
| | | 535 | | |
| | | 536 | | #endregion |
| | | 537 | | |
| | | 538 | | #region Operators |
| | | 539 | | |
| | | 540 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 541 | | public static Fixed3x3 operator -(Fixed3x3 a, Fixed3x3 b) |
| | 1 | 542 | | { |
| | | 543 | | // Subtract each element |
| | 1 | 544 | | return new Fixed3x3( |
| | 1 | 545 | | a.m00 - b.m00, a.m01 - b.m01, a.m02 - b.m02, |
| | 1 | 546 | | a.m10 - b.m10, a.m11 - b.m11, a.m12 - b.m12, |
| | 1 | 547 | | a.m20 - b.m20, a.m21 - b.m21, a.m22 - b.m22 |
| | 1 | 548 | | ); |
| | 1 | 549 | | } |
| | | 550 | | |
| | | 551 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 552 | | public static Fixed3x3 operator +(Fixed3x3 a, Fixed3x3 b) |
| | 1 | 553 | | { |
| | | 554 | | // Add each element |
| | 1 | 555 | | return new Fixed3x3( |
| | 1 | 556 | | a.m00 + b.m00, a.m01 + b.m01, a.m02 + b.m02, |
| | 1 | 557 | | a.m10 + b.m10, a.m11 + b.m11, a.m12 + b.m12, |
| | 1 | 558 | | a.m20 + b.m20, a.m21 + b.m21, a.m22 + b.m22 |
| | 1 | 559 | | ); |
| | 1 | 560 | | } |
| | | 561 | | /// <summary> |
| | | 562 | | /// Negates all elements of the matrix. |
| | | 563 | | /// </summary> |
| | | 564 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 565 | | public static Fixed3x3 operator -(Fixed3x3 a) |
| | 1 | 566 | | { |
| | | 567 | | // Negate each element |
| | 1 | 568 | | return new Fixed3x3( |
| | 1 | 569 | | -a.m00, -a.m01, -a.m02, |
| | 1 | 570 | | -a.m10, -a.m11, -a.m12, |
| | 1 | 571 | | -a.m20, -a.m21, -a.m22 |
| | 1 | 572 | | ); |
| | 1 | 573 | | } |
| | | 574 | | public static Fixed3x3 operator *(Fixed3x3 a, Fixed3x3 b) |
| | 1 | 575 | | { |
| | | 576 | | // Perform matrix multiplication |
| | 1 | 577 | | return new Fixed3x3( |
| | 1 | 578 | | a.m00 * b.m00 + a.m01 * b.m10 + a.m02 * b.m20, |
| | 1 | 579 | | a.m00 * b.m01 + a.m01 * b.m11 + a.m02 * b.m21, |
| | 1 | 580 | | a.m00 * b.m02 + a.m01 * b.m12 + a.m02 * b.m22, |
| | 1 | 581 | | |
| | 1 | 582 | | a.m10 * b.m00 + a.m11 * b.m10 + a.m12 * b.m20, |
| | 1 | 583 | | a.m10 * b.m01 + a.m11 * b.m11 + a.m12 * b.m21, |
| | 1 | 584 | | a.m10 * b.m02 + a.m11 * b.m12 + a.m12 * b.m22, |
| | 1 | 585 | | |
| | 1 | 586 | | a.m20 * b.m00 + a.m21 * b.m10 + a.m22 * b.m20, |
| | 1 | 587 | | a.m20 * b.m01 + a.m21 * b.m11 + a.m22 * b.m21, |
| | 1 | 588 | | a.m20 * b.m02 + a.m21 * b.m12 + a.m22 * b.m22 |
| | 1 | 589 | | ); |
| | 1 | 590 | | } |
| | | 591 | | |
| | | 592 | | public static Fixed3x3 operator *(Fixed3x3 a, Fixed64 scalar) |
| | 2 | 593 | | { |
| | | 594 | | // Perform matrix multiplication by scalar |
| | 2 | 595 | | return new Fixed3x3( |
| | 2 | 596 | | a.m00 * scalar, a.m01 * scalar, a.m02 * scalar, |
| | 2 | 597 | | a.m10 * scalar, a.m11 * scalar, a.m12 * scalar, |
| | 2 | 598 | | a.m20 * scalar, a.m21 * scalar, a.m22 * scalar |
| | 2 | 599 | | ); |
| | 2 | 600 | | } |
| | | 601 | | |
| | | 602 | | public static Fixed3x3 operator *(Fixed64 scalar, Fixed3x3 a) |
| | 1 | 603 | | { |
| | | 604 | | // Perform matrix multiplication by scalar |
| | 1 | 605 | | return a * scalar; |
| | 1 | 606 | | } |
| | | 607 | | |
| | | 608 | | public static Fixed3x3 operator /(Fixed3x3 a, int divisor) |
| | 2 | 609 | | { |
| | | 610 | | // Perform matrix multiplication by scalar |
| | 2 | 611 | | return new Fixed3x3( |
| | 2 | 612 | | a.m00 / divisor, a.m01 / divisor, a.m02 / divisor, |
| | 2 | 613 | | a.m10 / divisor, a.m11 / divisor, a.m12 / divisor, |
| | 2 | 614 | | a.m20 / divisor, a.m21 / divisor, a.m22 / divisor |
| | 2 | 615 | | ); |
| | 2 | 616 | | } |
| | | 617 | | |
| | | 618 | | public static Fixed3x3 operator /(int divisor, Fixed3x3 a) |
| | 1 | 619 | | { |
| | 1 | 620 | | return a / divisor; |
| | 1 | 621 | | } |
| | | 622 | | |
| | | 623 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 624 | | public static bool operator ==(Fixed3x3 left, Fixed3x3 right) |
| | 1 | 625 | | { |
| | 1 | 626 | | return left.Equals(right); |
| | 1 | 627 | | } |
| | | 628 | | |
| | | 629 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 630 | | public static bool operator !=(Fixed3x3 left, Fixed3x3 right) |
| | 0 | 631 | | { |
| | 0 | 632 | | return !left.Equals(right); |
| | 0 | 633 | | } |
| | | 634 | | |
| | | 635 | | #endregion |
| | | 636 | | |
| | | 637 | | #region Equality and HashCode Overrides |
| | | 638 | | |
| | | 639 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 640 | | public bool Equals(Fixed3x3 other) |
| | 25 | 641 | | { |
| | | 642 | | // Compare each element for equality |
| | 25 | 643 | | return |
| | 25 | 644 | | m00 == other.m00 && m01 == other.m01 && m02 == other.m02 && |
| | 25 | 645 | | m10 == other.m10 && m11 == other.m11 && m12 == other.m12 && |
| | 25 | 646 | | m20 == other.m20 && m21 == other.m21 && m22 == other.m22; |
| | 25 | 647 | | } |
| | | 648 | | |
| | | 649 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 650 | | public override bool Equals(object? obj) |
| | 4 | 651 | | { |
| | 4 | 652 | | return obj is Fixed3x3 other && Equals(other); |
| | 4 | 653 | | } |
| | | 654 | | |
| | | 655 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 656 | | public override int GetHashCode() |
| | 3 | 657 | | { |
| | | 658 | | unchecked |
| | 3 | 659 | | { |
| | 3 | 660 | | int hash = 17; |
| | 3 | 661 | | hash = hash * 23 + m00.GetHashCode(); |
| | 3 | 662 | | hash = hash * 23 + m01.GetHashCode(); |
| | 3 | 663 | | hash = hash * 23 + m02.GetHashCode(); |
| | 3 | 664 | | hash = hash * 23 + m10.GetHashCode(); |
| | 3 | 665 | | hash = hash * 23 + m11.GetHashCode(); |
| | 3 | 666 | | hash = hash * 23 + m12.GetHashCode(); |
| | 3 | 667 | | hash = hash * 23 + m20.GetHashCode(); |
| | 3 | 668 | | hash = hash * 23 + m21.GetHashCode(); |
| | 3 | 669 | | hash = hash * 23 + m22.GetHashCode(); |
| | 3 | 670 | | return hash; |
| | | 671 | | } |
| | 3 | 672 | | } |
| | | 673 | | |
| | | 674 | | #endregion |
| | | 675 | | |
| | | 676 | | #region Conversion |
| | | 677 | | |
| | | 678 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 679 | | public override string ToString() |
| | 17 | 680 | | { |
| | 17 | 681 | | return $"[{m00}, {m01}, {m02}; {m10}, {m11}, {m12}; {m20}, {m21}, {m22}]"; |
| | 17 | 682 | | } |
| | | 683 | | |
| | | 684 | | #endregion |
| | | 685 | | } |