├── README.md ├── LICENSE └── Matrix.cs /README.md: -------------------------------------------------------------------------------- 1 | # UnityMatrix 2 | A Matrix class for Unity C# that is much better than the Matrix4x4 class 3 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | This is free and unencumbered software released into the public domain. 2 | 3 | Anyone is free to copy, modify, publish, use, compile, sell, or 4 | distribute this software, either in source code form or as a compiled 5 | binary, for any purpose, commercial or non-commercial, and by any 6 | means. 7 | 8 | In jurisdictions that recognize copyright laws, the author or authors 9 | of this software dedicate any and all copyright interest in the 10 | software to the public domain. We make this dedication for the benefit 11 | of the public at large and to the detriment of our heirs and 12 | successors. We intend this dedication to be an overt act of 13 | relinquishment in perpetuity of all present and future rights to this 14 | software under copyright law. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 17 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. 19 | IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR 20 | OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, 21 | ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 22 | OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | For more information, please refer to 25 | -------------------------------------------------------------------------------- /Matrix.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using UnityEngine; 3 | 4 | // Modified from https://msdn.microsoft.com/en-us/magazine/mt736457.aspx 5 | public class Matrix 6 | { 7 | public float[][] mat; 8 | public int rows 9 | { 10 | get { return mat.Length; } 11 | } 12 | public int cols 13 | { 14 | get { return mat[0].Length; } 15 | } 16 | 17 | public Matrix(int rows, int cols) 18 | { 19 | mat = MatrixCreate(rows, cols); 20 | } 21 | 22 | public Matrix(float[][] values) 23 | { 24 | mat = new float[values.GetLength(0)][]; 25 | for (int i = 0; i < values.GetLength(0); i++) 26 | { 27 | mat[i] = new float[values[0].Length]; 28 | } 29 | for (int i = 0; i < rows; ++i) 30 | for (int j = 0; j < cols; ++j) 31 | mat[i][j] = values[i][j]; 32 | } 33 | 34 | 35 | public Matrix(Matrix4x4 m) 36 | { 37 | Matrix res = new Matrix(new float[] { 38 | m.m00, m.m01, m.m02, m.m03, 39 | m.m10, m.m11, m.m12, m.m13, 40 | m.m20, m.m21, m.m22, m.m23, 41 | m.m30, m.m31, m.m32, m.m33 42 | }, 4, 4); 43 | 44 | mat = res.mat; 45 | } 46 | public Matrix(Transform transform, bool useLocalValues) 47 | { 48 | /* 49 | Matrix4x4 m = transform.localToWorldMatrix; 50 | Matrix res = new Matrix(new float[] { 51 | m.m00, m.m01, m.m02, m.m03, 52 | m.m10, m.m11, m.m12, m.m13, 53 | m.m20, m.m21, m.m22, m.m23, 54 | m.m30, m.m31, m.m32, m.m33 55 | }, 4, 4); 56 | 57 | mat = res.mat; 58 | */ 59 | 60 | if (useLocalValues) 61 | { 62 | Matrix rot = new Matrix(Matrix4x4.TRS(transform.localPosition, transform.localRotation, transform.localScale)); 63 | mat = rot.mat; 64 | } 65 | else 66 | { 67 | Matrix rot = new Matrix(Matrix4x4.TRS(transform.position, transform.rotation, transform.lossyScale)); 68 | mat = rot.mat; 69 | } 70 | } 71 | 72 | public Matrix T 73 | { 74 | get { 75 | float[] resValues = new float[rows * cols]; 76 | 77 | int pos = 0; 78 | for (int i = 0; i < cols; ++i) 79 | for (int j = 0; j < rows; ++j) 80 | resValues[pos++] = mat[j][i]; 81 | return new Matrix(resValues, cols, rows); 82 | } 83 | } 84 | 85 | public Matrix(Vector4 vec) 86 | { 87 | Matrix res = new Matrix(new float[] { vec.x, vec.y, vec.z, vec.w }, 4, 1); 88 | mat = res.mat; 89 | } 90 | 91 | public Matrix(Vector3 vec) 92 | { 93 | Matrix res = new Matrix(new float[] { vec.x, vec.y, vec.z }, 3, 1); 94 | mat = res.mat; 95 | } 96 | 97 | public Matrix(Vector2 vec) 98 | { 99 | Matrix res = new Matrix(new float[] { vec.x, vec.y }, 2, 1); 100 | mat = res.mat; 101 | } 102 | 103 | // Fro http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/ 104 | public Matrix(Quaternion quat) 105 | { 106 | mat = MatrixCreate(3, 3); 107 | 108 | Vector4 q = new Vector4(quat.x, quat.y, quat.z, quat.w); 109 | q = q.normalized; 110 | 111 | float x = q.x; 112 | float y = q.y; 113 | float z = q.z; 114 | float w = q.w; 115 | 116 | mat[0][0] = 1 - 2 * y * y - 2 * z * z; 117 | mat[0][1] = 2 * x * y - 2 * z * w; 118 | mat[0][2] = 2 * x * z + 2 * y * w; 119 | 120 | mat[1][0] = 2 * x * y + 2 * z * w; 121 | mat[1][1] = 1 - 2 * x * x - 2 * z * z; 122 | mat[1][2] = 2 * y * z - 2 * x * w; 123 | 124 | mat[2][0] = 2 * x * z - 2 * y * w; 125 | mat[2][1] = 2 * y * z + 2 * x * w; 126 | mat[2][2] = 1 - 2 * x * x - 2 * y * y; 127 | } 128 | 129 | public Matrix(float[] values, int rows, int cols) 130 | { 131 | if (rows*cols != values.Length) 132 | { 133 | throw new Exception("rows x cols: " + rows + "x" + cols + " = " + rows * cols + " does not equal given data of size " + values.Length); 134 | } 135 | 136 | mat = MatrixCreate(rows, cols); 137 | 138 | int pos = 0; 139 | for (int i = 0; i < rows; ++i) 140 | { 141 | for (int j = 0; j < cols; ++j) 142 | { 143 | mat[i][j] = values[pos]; 144 | pos++; 145 | } 146 | } 147 | } 148 | 149 | public Matrix(double[] values, int rows, int cols) 150 | { 151 | if (rows * cols != values.Length) 152 | { 153 | throw new Exception("rows x cols: " + rows + "x" + cols + " = " + rows * cols + " does not equal given data of size " + values.Length); 154 | } 155 | 156 | mat = MatrixCreate(rows, cols); 157 | 158 | int pos = 0; 159 | for (int i = 0; i < rows; ++i) 160 | { 161 | for (int j = 0; j < cols; ++j) 162 | { 163 | mat[i][j] = (float)values[pos]; 164 | pos++; 165 | } 166 | } 167 | } 168 | 169 | // From http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ 170 | // Uses only top left 3x3 171 | public Quaternion ToQuaterion() 172 | { 173 | float[][] a = mat; 174 | Quaternion q = new Quaternion(); 175 | float trace = a[0][0] + a[1][1] + a[2][2]; // I removed + 1.0f; see discussion with Ethan 176 | if (trace > 0) 177 | {// I changed M_EPSILON to 0 178 | float s = 0.5f / Mathf.Sqrt(trace + 1.0f); 179 | q.w = 0.25f / s; 180 | q.x = (a[2][1] - a[1][2]) * s; 181 | q.y = (a[0][2] - a[2][0]) * s; 182 | q.z = (a[1][0] - a[0][1]) * s; 183 | } 184 | else 185 | { 186 | if (a[0][0] > a[1][1] && a[0][0] > a[2][2]) 187 | { 188 | float s = 2.0f * Mathf.Sqrt(1.0f + a[0][0] - a[1][1] - a[2][2]); 189 | q.w = (a[2][1] - a[1][2]) / s; 190 | q.x = 0.25f * s; 191 | q.y = (a[0][1] + a[1][0]) / s; 192 | q.z = (a[0][2] + a[2][0]) / s; 193 | } 194 | else if (a[1][1] > a[2][2]) 195 | { 196 | float s = 2.0f * Mathf.Sqrt(1.0f + a[1][1] - a[0][0] - a[2][2]); 197 | q.w = (a[0][2] - a[2][0]) / s; 198 | q.x = (a[0][1] + a[1][0]) / s; 199 | q.y = 0.25f * s; 200 | q.z = (a[1][2] + a[2][1]) / s; 201 | } 202 | else 203 | { 204 | float s = 2.0f * Mathf.Sqrt(1.0f + a[2][2] - a[0][0] - a[1][1]); 205 | q.w = (a[1][0] - a[0][1]) / s; 206 | q.x = (a[0][2] + a[2][0]) / s; 207 | q.y = (a[1][2] + a[2][1]) / s; 208 | q.z = 0.25f * s; 209 | } 210 | } 211 | return q; 212 | } 213 | 214 | public Vector4 ToVec4() 215 | { 216 | float[] arr = ToArray(); 217 | 218 | if (!((rows == 4 && cols == 1) || (rows == 1 && cols == 4))) 219 | { 220 | throw new ArgumentException("Matrix is of dimension (" + rows + ", " + cols + ") which cannot be converted to a Vector4"); 221 | } 222 | 223 | return new Vector4(arr[0], arr[1], arr[2], arr[3]); 224 | } 225 | 226 | public Vector3 ToVec3() 227 | { 228 | float[] arr = ToArray(); 229 | 230 | if (!((rows == 3 && cols == 1) || (rows == 1 && cols == 3))) 231 | { 232 | throw new ArgumentException("Matrix is of dimension (" + rows + ", " + cols + ") which cannot be converted to a Vector3"); 233 | } 234 | 235 | return new Vector3(arr[0], arr[1], arr[2]); 236 | } 237 | 238 | public Vector2 ToVec2() 239 | { 240 | float[] arr = ToArray(); 241 | 242 | if (!((rows == 2 && cols == 1) || (rows == 1 && cols == 2))) 243 | { 244 | throw new ArgumentException("Matrix is of dimension (" + rows + ", " + cols + ") which cannot be converted to a Vector2"); 245 | } 246 | 247 | return new Vector2(arr[0], arr[1]); 248 | } 249 | 250 | 251 | 252 | public override string ToString() 253 | { 254 | return MatrixAsString(mat); 255 | } 256 | 257 | public float[] ToArray() 258 | { 259 | float[] arr = new float[rows * cols]; 260 | int pos = 0; 261 | for (int i = 0; i < rows; ++i) 262 | for (int j = 0; j < cols; ++j) 263 | arr[pos++] = mat[i][j]; 264 | return arr; 265 | } 266 | 267 | public float this[int i, int j] 268 | { 269 | get { return mat[i][j]; } 270 | set { mat[i][j] = value; } 271 | } 272 | 273 | public static Matrix operator *(Matrix m, float s) 274 | { 275 | Matrix res = new Matrix(m.rows, m.cols); 276 | for (int i = 0; i < m.rows; ++i) 277 | for (int j = 0; j < m.cols; ++j) 278 | res[i, j] = m[i, j] * s; 279 | return res; 280 | } 281 | 282 | public static Matrix operator *(float s, Matrix m) 283 | { 284 | return m * s; 285 | } 286 | 287 | public static Matrix operator +(Matrix m, float s) 288 | { 289 | Matrix res = new Matrix(m.rows, m.cols); 290 | for (int i = 0; i < m.rows; ++i) 291 | for (int j = 0; j < m.cols; ++j) 292 | res[i, j] = m[i, j] + s; 293 | return res; 294 | } 295 | 296 | public static Matrix operator +(float s, Matrix m) 297 | { 298 | return m + s; 299 | } 300 | 301 | public static Matrix operator -(Matrix m, float s) 302 | { 303 | return m + (-s); 304 | } 305 | 306 | public static Matrix operator -(float s, Matrix m) 307 | { 308 | return (-m) + s; 309 | } 310 | 311 | 312 | public static Matrix operator *(Matrix c1, Matrix c2) 313 | { 314 | return new Matrix(MatrixProduct(c1.mat, c2.mat)); 315 | } 316 | 317 | public static Matrix operator *(Matrix c1, Vector3 c2) 318 | { 319 | return new Matrix(MatrixProduct(c1.mat, (new Matrix(c2)).mat)); 320 | } 321 | public static Matrix operator *(Vector3 c1, Matrix c2) 322 | { 323 | return new Matrix(MatrixProduct((new Matrix(c1)).T.mat, c2.mat)); 324 | } 325 | 326 | public static Matrix operator *(Matrix c1, Vector4 c2) 327 | { 328 | return new Matrix(MatrixProduct(c1.mat, (new Matrix(c2)).mat)); 329 | } 330 | public static Matrix operator *(Vector4 c1, Matrix c2) 331 | { 332 | return new Matrix(MatrixProduct((new Matrix(c1)).T.mat, c2.mat)); 333 | } 334 | 335 | public static Matrix operator -(Matrix m) 336 | { 337 | Matrix res = new Matrix(m.rows, m.cols); 338 | 339 | for (int i = 0; i < m.rows; ++i) 340 | for (int j = 0; j < m.cols; ++j) 341 | res[i, j] = -m[i, j]; 342 | return res; 343 | } 344 | 345 | public static Matrix operator +(Matrix c1, Matrix c2) 346 | { 347 | if (c1.rows != c2.rows || c1.cols != c2.cols) 348 | { 349 | throw new ArgumentException("Left hand side size: (" + c1.rows + ", " + c1.cols + ") != Right hand side size: (" + c2.rows + ", " + c2.cols + ")"); 350 | } 351 | Matrix res = new Matrix(c1.rows, c1.cols); 352 | for (int i = 0; i < c1.rows; ++i) 353 | for (int j = 0; j < c1.cols; ++j) 354 | res[i, j] = c1[i, j] + c2[i, j]; 355 | return res; 356 | } 357 | 358 | public static Matrix operator -(Matrix c1, Matrix c2) 359 | { 360 | return c1 + (-c2); 361 | } 362 | 363 | public Matrix Inverse() 364 | { 365 | return new Matrix(MatrixInverse(mat)); 366 | } 367 | 368 | 369 | public static float[][] MatrixInverse(float[][] matrix) 370 | { 371 | // assumes determinant is not 0 372 | // that is, the matrix does have an inverse 373 | int n = matrix.Length; 374 | float[][] result = MatrixCreate(n, n); // make a copy of matrix 375 | for (int i = 0; i < n; ++i) 376 | for (int j = 0; j < n; ++j) 377 | result[i][j] = matrix[i][j]; 378 | 379 | float[][] lum; // combined lower & upper 380 | int[] perm; 381 | int toggle; 382 | toggle = MatrixDecompose(matrix, out lum, out perm); 383 | 384 | float[] b = new float[n]; 385 | for (int i = 0; i < n; ++i) 386 | { 387 | for (int j = 0; j < n; ++j) 388 | if (i == perm[j]) 389 | b[j] = 1.0f; 390 | else 391 | b[j] = 0.0f; 392 | 393 | float[] x = Helper(lum, b); // 394 | for (int j = 0; j < n; ++j) 395 | result[j][i] = x[j]; 396 | } 397 | return result; 398 | } // MatrixInverse 399 | 400 | public static int MatrixDecompose(float[][] m, out float[][] lum, out int[] perm) 401 | { 402 | // Crout's LU decomposition for matrix determinant and inverse 403 | // stores combined lower & upper in lum[][] 404 | // stores row permuations into perm[] 405 | // returns +1 or -1 according to even or odd number of row permutations 406 | // lower gets dummy 1.0s on diagonal (0.0s above) 407 | // upper gets lum values on diagonal (0.0s below) 408 | 409 | int toggle = +1; // even (+1) or odd (-1) row permutatuions 410 | int n = m.Length; 411 | 412 | // make a copy of m[][] into result lu[][] 413 | lum = MatrixCreate(n, n); 414 | for (int i = 0; i < n; ++i) 415 | for (int j = 0; j < n; ++j) 416 | lum[i][j] = m[i][j]; 417 | 418 | 419 | // make perm[] 420 | perm = new int[n]; 421 | for (int i = 0; i < n; ++i) 422 | perm[i] = i; 423 | 424 | for (int j = 0; j < n - 1; ++j) // process by column. note n-1 425 | { 426 | float max = Math.Abs(lum[j][j]); 427 | int piv = j; 428 | 429 | for (int i = j + 1; i < n; ++i) // find pivot index 430 | { 431 | float xij = Math.Abs(lum[i][j]); 432 | if (xij > max) 433 | { 434 | max = xij; 435 | piv = i; 436 | } 437 | } // i 438 | 439 | if (piv != j) 440 | { 441 | float[] tmp = lum[piv]; // swap rows j, piv 442 | lum[piv] = lum[j]; 443 | lum[j] = tmp; 444 | 445 | int t = perm[piv]; // swap perm elements 446 | perm[piv] = perm[j]; 447 | perm[j] = t; 448 | 449 | toggle = -toggle; 450 | } 451 | 452 | float xjj = lum[j][j]; 453 | if (xjj != 0.0) 454 | { 455 | for (int i = j + 1; i < n; ++i) 456 | { 457 | float xij = lum[i][j] / xjj; 458 | lum[i][j] = xij; 459 | for (int k = j + 1; k < n; ++k) 460 | lum[i][k] -= xij * lum[j][k]; 461 | } 462 | } 463 | 464 | } // j 465 | 466 | return toggle; 467 | } // MatrixDecompose 468 | 469 | public static float[] Helper(float[][] luMatrix, float[] b) // helper 470 | { 471 | int n = luMatrix.Length; 472 | float[] x = new float[n]; 473 | b.CopyTo(x, 0); 474 | 475 | for (int i = 1; i < n; ++i) 476 | { 477 | float sum = x[i]; 478 | for (int j = 0; j < i; ++j) 479 | sum -= luMatrix[i][j] * x[j]; 480 | x[i] = sum; 481 | } 482 | 483 | x[n - 1] /= luMatrix[n - 1][n - 1]; 484 | for (int i = n - 2; i >= 0; --i) 485 | { 486 | float sum = x[i]; 487 | for (int j = i + 1; j < n; ++j) 488 | sum -= luMatrix[i][j] * x[j]; 489 | x[i] = sum / luMatrix[i][i]; 490 | } 491 | 492 | return x; 493 | } // Helper 494 | 495 | public static float MatrixDeterminant(float[][] matrix) 496 | { 497 | float[][] lum; 498 | int[] perm; 499 | int toggle = MatrixDecompose(matrix, out lum, out perm); 500 | float result = toggle; 501 | for (int i = 0; i < lum.Length; ++i) 502 | result *= lum[i][i]; 503 | return result; 504 | } 505 | 506 | // ---------------------------------------------------------------- 507 | 508 | public static float[][] MatrixCreate(int rows, int cols) 509 | { 510 | float[][] result = new float[rows][]; 511 | for (int i = 0; i < rows; ++i) 512 | result[i] = new float[cols]; 513 | return result; 514 | } 515 | 516 | public static float[][] MatrixProduct(float[][] matrixA, 517 | float[][] matrixB) 518 | { 519 | int aRows = matrixA.Length; 520 | int aCols = matrixA[0].Length; 521 | int bRows = matrixB.Length; 522 | int bCols = matrixB[0].Length; 523 | if (aCols != bRows) 524 | throw new Exception("Non-conformable matrices"); 525 | 526 | float[][] result = MatrixCreate(aRows, bCols); 527 | 528 | for (int i = 0; i < aRows; ++i) // each row of A 529 | for (int j = 0; j < bCols; ++j) // each col of B 530 | for (int k = 0; k < aCols; ++k) // could use k < bRows 531 | result[i][j] += matrixA[i][k] * matrixB[k][j]; 532 | 533 | return result; 534 | } 535 | 536 | public static string MatrixAsString(float[][] matrix) 537 | { 538 | string s = ""; 539 | for (int i = 0; i < matrix.Length; ++i) 540 | { 541 | for (int j = 0; j < matrix[i].Length; ++j) 542 | s += matrix[i][j].ToString("F3").PadLeft(8) + " "; 543 | s += Environment.NewLine; 544 | } 545 | return s; 546 | } 547 | 548 | public static float[][] ExtractLower(float[][] lum) 549 | { 550 | // lower part of an LU Doolittle decomposition (dummy 1.0s on diagonal, 0.0s above) 551 | int n = lum.Length; 552 | float[][] result = MatrixCreate(n, n); 553 | for (int i = 0; i < n; ++i) 554 | { 555 | for (int j = 0; j < n; ++j) 556 | { 557 | if (i == j) 558 | result[i][j] = 1.0f; 559 | else if (i > j) 560 | result[i][j] = lum[i][j]; 561 | } 562 | } 563 | return result; 564 | } 565 | 566 | public static float[][] ExtractUpper(float[][] lum) 567 | { 568 | // upper part of an LU (lu values on diagional and above, 0.0s below) 569 | int n = lum.Length; 570 | float[][] result = MatrixCreate(n, n); 571 | for (int i = 0; i < n; ++i) 572 | { 573 | for (int j = 0; j < n; ++j) 574 | { 575 | if (i <= j) 576 | result[i][j] = lum[i][j]; 577 | } 578 | } 579 | return result; 580 | } 581 | } 582 | --------------------------------------------------------------------------------