1 /+ 2 The MIT License (MIT) 3 4 Copyright (c) <2013> <Oleg Butko (deviator), Anton Akzhigitov (Akzwar)> 5 6 Permission is hereby granted, free of charge, to any person obtaining a copy 7 of this software and associated documentation files (the "Software"), to deal 8 in the Software without restriction, including without limitation the rights 9 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 copies of the Software, and to permit persons to whom the Software is 11 furnished to do so, subject to the following conditions: 12 13 The above copyright notice and this permission notice shall be included in 14 all copies or substantial portions of the Software. 15 16 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 22 THE SOFTWARE. 23 +/ 24 25 module des.math.linear.matrix; 26 27 import std.math; 28 import std.traits; 29 import std.algorithm; 30 import std.range; 31 import std.array; 32 import std.exception; 33 34 import des.util.testsuite; 35 36 import des.math.util; 37 import des.math.linear.vector; 38 import des.math.basic.traits; 39 40 private pure void isMatrixImpl(size_t H,size_t W,T)( Matrix!(H,W,T) ){} 41 pure bool isMatrix(E)() { return is( typeof( isMatrixImpl(E.init) ) ); } 42 43 pure bool isStaticMatrix(E)() 44 { 45 static if( !isMatrix!E ) return false; 46 else return E.isStatic; 47 } 48 49 pure bool isDynamicMatrix(E)() 50 { 51 static if( !isMatrix!E ) return false; 52 else return E.isDynamic; 53 } 54 55 unittest 56 { 57 static assert( !isStaticMatrix!float ); 58 static assert( !isDynamicMatrix!float ); 59 } 60 61 private @property 62 { 63 import std.string; 64 65 string identityMatrixDataString(size_t S)() 66 { return diagMatrixDataString!(S,S)(1); } 67 68 string zerosMatrixDataString(size_t H, size_t W)() 69 { return diagMatrixDataString!(H,W)(0); } 70 71 string diagMatrixDataString(size_t H, size_t W)( size_t val ) 72 { 73 string[] ret; 74 foreach( i; 0 .. H ) 75 { 76 string[] buf; 77 foreach( j; 0 .. W ) 78 buf ~= format( "%d", i == j ? val : 0 ); 79 ret ~= "[" ~ buf.join(",") ~ "]"; 80 } 81 return "[" ~ ret.join(",") ~ "]"; 82 } 83 84 unittest 85 { 86 assert( identityMatrixDataString!3 == "[[1,0,0],[0,1,0],[0,0,1]]" ); 87 assert( zerosMatrixDataString!(3,3) == "[[0,0,0],[0,0,0],[0,0,0]]" ); 88 } 89 90 string castArrayString(string type, size_t H, size_t W)() 91 { 92 return format( "cast(%s[%d][%d])", type, W, H ); 93 } 94 95 } 96 97 struct Matrix(size_t H, size_t W, E) 98 { 99 alias Matrix!(H,W,E) selftype; 100 alias E datatype; 101 102 enum isDynamic = H == 0 || W == 0; 103 enum isStatic = H != 0 && W != 0; 104 105 enum isDynamicHeight = H == 0; 106 enum isStaticHeight = H != 0; 107 108 enum isDynamicWidth = W == 0; 109 enum isStaticWidth = W != 0; 110 111 enum isStaticHeightOnly = isStaticHeight && isDynamicWidth; 112 enum isStaticWidthOnly = isStaticWidth && isDynamicHeight; 113 114 enum isDynamicAll = isDynamicHeight && isDynamicWidth; 115 enum isDynamicOne = isStaticWidthOnly || isStaticHeightOnly; 116 117 static if( isStatic ) 118 { 119 static if( isNumeric!E ) 120 { 121 static if( H == W ) 122 E[W][H] data = mixin( castArrayString!("E",H,H) ~ identityMatrixDataString!H ); 123 else 124 E[W][H] data = mixin( castArrayString!("E",H,W) ~ zerosMatrixDataString!(H,W) ); 125 } 126 else E[W][H] data; 127 } 128 else static if( isStaticWidthOnly ) 129 E[W][] data; 130 else static if( isStaticHeightOnly ) 131 E[][H] data; 132 else E[][] data; 133 134 alias data this; 135 136 pure: 137 138 private static bool allowSomeOp(size_t A, size_t B ) 139 { return A==B || A==0 || B==0; } 140 141 static if( isStatic ) 142 { 143 this(X...)( in X vals ) { _fillData( flatData!E(vals) ); } 144 } 145 else static if( isStaticWidthOnly ) 146 { 147 this(X...)( in X vals ) 148 { 149 auto buf = flatData!E(vals); 150 enforce( !(buf.length%W), "wrong args length" ); 151 resize( buf.length/W, W ); 152 _fillData( buf ); 153 } 154 } 155 else static if( isStaticHeightOnly ) 156 { 157 this(X...)( in X vals ) 158 { 159 auto buf = flatData!E(vals); 160 enforce( !(buf.length%H), "wrong args length" ); 161 resize( H, buf.length/H ); 162 _fillData( buf ); 163 } 164 } 165 else 166 { 167 this(X...)( size_t iH, size_t iW, in X vals ) 168 { 169 auto buf = flatData!E(vals); 170 enforce( buf.length == iH * iW, "wrong args length" ); 171 resize( iH, iW ); 172 _fillData( buf ); 173 } 174 175 this( size_t iH, size_t iW ) { resize( iH, iW ); } 176 } 177 178 this(size_t oH, size_t oW, X)( in Matrix!(oH,oW,X) mtr ) 179 if( is(typeof(flatData!E(mtr.data))) ) 180 { 181 static if( isDynamic ) 182 resize( mtr.height, mtr.width ); 183 _fillData( flatData!E(mtr.data) ); 184 } 185 186 static if( isDynamic ) 187 { 188 this(this) { _fillData(flatData!E(data)); } 189 190 auto opAssign(size_t bH, size_t bW, X)( in Matrix!(bH,bW,X) b ) 191 if( allowSomeOp(H,bH) && allowSomeOp(W,bW) && is(typeof(E(X.init))) ) 192 { 193 static if( isStaticHeight && b.isDynamicHeight ) enforce( height == b.height, "wrong height" ); 194 static if( isStaticWidth && b.isDynamicWidth ) enforce( width == b.width, "wrong width" ); 195 static if( isDynamic ) resize(b.height,b.width); 196 _fillData(flatData!E(b.data)); 197 return this; 198 } 199 } 200 201 private void _fillData( in E[] vals... ) 202 { 203 enforce( vals.length > 0, "no vals to fill" ); 204 if( vals.length > 1 ) 205 { 206 enforce( vals.length == height * width, "wrong vals length" ); 207 size_t k = 0; 208 foreach( ref row; data ) 209 foreach( ref v; row ) 210 v = vals[k++]; 211 } 212 else foreach( ref row; data ) row[] = vals[0]; 213 } 214 215 auto fill( in E[] vals... ) 216 { 217 _fillData( vals ); 218 return this; 219 } 220 221 static if( (W==H && isStatic) || isDynamicOne ) 222 static auto diag(X...)( in X vals ) 223 if( X.length > 0 && is(typeof(E(0))) && is(typeof(flatData!E(vals))) ) 224 { 225 selftype ret; 226 static if( isStaticHeight ) auto L = H; 227 else auto L = W; 228 229 static if( ret.isDynamic ) 230 ret.resize(L,L); 231 232 ret._fillData( E(0) ); 233 ret.fillDiag( flatData!E(vals) ); 234 235 return ret; 236 } 237 238 auto fillDiag( in E[] vals... ) 239 { 240 enforce( vals.length > 0, "no vals to fill" ); 241 enforce( height == width, "not squared" ); 242 size_t k = 0; 243 foreach( i; 0 .. height ) 244 data[i][i] = vals[k++%$]; 245 return this; 246 } 247 248 static if( isStaticHeight ) enum height = H; 249 else 250 { 251 @property size_t height() const { return data.length; } 252 @property size_t height( size_t nh ) 253 { 254 resize( nh, width ); 255 return nh; 256 } 257 } 258 259 static if( isStaticWidth ) enum width = W; 260 else 261 { 262 @property size_t width() const { return _width; } 263 @property size_t width( size_t nw ) 264 { 265 resize( height, nw ); 266 return nw; 267 } 268 } 269 270 static if( isDynamic ) 271 { 272 static if( isDynamicWidth ) private size_t _width = W; 273 274 void resize( size_t nh, size_t nw ) 275 { 276 static if( isStaticHeight ) enforce( nh == H, "height is static" ); 277 static if( isStaticWidth ) enforce( nw == W, "width is static" ); 278 279 static if( isDynamicHeight ) data.length = nh; 280 static if( isDynamicWidth ) 281 { 282 _width = nw; 283 foreach( i; 0 .. height ) 284 data[i].length = nw; 285 } 286 } 287 } 288 289 auto expandHeight(size_t bH, size_t bW, X)( in Matrix!(bH,bW,X) mtr ) const 290 if( (bW==W||W==0||bW==0) && is(typeof(E(X.init))) ) 291 { 292 static if( isDynamicWidth || mtr.isDynamicWidth ) 293 enforce( mtr.width == width, "wrong width" ); 294 295 auto ret = Matrix!(0,W,E)(this); 296 auto last_height = height; 297 ret.resize( ret.height + mtr.height, width ); 298 foreach( i; 0 .. mtr.height ) 299 foreach( j; 0 .. width ) 300 ret.data[last_height+i][j] = E(mtr.data[i][j]); 301 return ret; 302 } 303 304 auto expandHeight(X...)( in X vals ) const 305 if( is(typeof(Matrix!(1,W,E)(vals))) ) 306 { return expandHeight( Matrix!(1,W,E)(vals) ); } 307 308 auto expandWidth(size_t bH, size_t bW, X)( in Matrix!(bH,bW,X) mtr ) const 309 if( (bH==H||H==0||bH==0) && is(typeof(E(X.init))) ) 310 { 311 static if( isDynamicHeight || mtr.isDynamicHeight ) 312 enforce( mtr.height == height, "wrong height" ); 313 auto ret = Matrix!(H,0,E)(this); 314 auto last_width = width; 315 ret.resize( height, ret.width + mtr.width ); 316 foreach( i; 0 .. height ) 317 foreach( j; 0 .. mtr.width ) 318 ret.data[i][last_width+j] = E(mtr.data[i][j]); 319 return ret; 320 } 321 322 auto expandWidth(X...)( in X vals ) const 323 if( is(typeof(Matrix!(H,1,E)(vals))) ) 324 { return expandWidth( Matrix!(H,1,E)(vals) ); } 325 326 @property auto asArray() const 327 { 328 auto ret = new E[](width*height); 329 foreach( i; 0 .. height ) 330 foreach( j; 0 .. width ) 331 ret[i*width+j] = data[i][j]; 332 return ret; 333 } 334 335 auto sliceHeight( size_t start, size_t count=0 ) const 336 { 337 enforce( start < height ); 338 count = count ? count : height - start; 339 auto ret = Matrix!(0,W,E)(this); 340 ret.resize( count, width ); 341 foreach( i; 0 .. count ) 342 ret.data[i][] = data[start+i][]; 343 return ret; 344 } 345 346 auto sliceWidth( size_t start, size_t count=0 ) const 347 { 348 enforce( start < width ); 349 count = count ? count : width - start; 350 auto ret = Matrix!(H,0,E)(this); 351 ret.resize( height, count ); 352 foreach( i; 0 .. height ) 353 foreach( j; 0 .. count ) 354 ret.data[i][j] = data[i][start+j]; 355 return ret; 356 } 357 358 auto opUnary(string op)() const 359 if( op == "-" && is( typeof( E.init * (-1) ) ) ) 360 { 361 auto ret = selftype(this); 362 foreach( ref row; ret.data ) 363 foreach( ref v; row ) 364 v = v * -1; 365 return ret; 366 } 367 368 private void checkCompatible(size_t bH, size_t bW, X)( in Matrix!(bH,bW,X) mtr ) const 369 { 370 static if( isDynamicHeight || mtr.isDynamicHeight ) 371 enforce( height == mtr.height, "wrong height" ); 372 static if( isDynamicWidth || mtr.isDynamicWidth ) 373 enforce( width == mtr.width, "wrong width" ); 374 } 375 376 auto opBinary(string op, size_t bH, size_t bW, X)( in Matrix!(bH,bW,X) mtr ) const 377 if( (op=="+"||op=="-") && allowSomeOp(H,bH) && allowSomeOp(W,bW) ) 378 { 379 checkCompatible( mtr ); 380 381 auto ret = selftype(this); 382 383 foreach( i; 0 .. height ) 384 foreach( j; 0 .. width ) 385 mixin( `ret[i][j] = E(data[i][j] ` ~ op ~ ` mtr[i][j]);` ); 386 return ret; 387 } 388 389 auto opBinary(string op,X)( in X b ) const 390 if( (op!="+" && op!="-") && isValidOp!(op,E,X) ) 391 { 392 auto ret = selftype(this); 393 foreach( i; 0 .. height ) 394 foreach( j; 0 .. width ) 395 mixin( `ret[i][j] = E(data[i][j] ` ~ op ~ ` b);` ); 396 return ret; 397 } 398 399 auto opBinary(string op,size_t bH, size_t bW, X)( in Matrix!(bH,bW,X) mtr ) const 400 if( op=="*" && allowSomeOp(W,bH) && isValidOp!("*",E,X) ) 401 { 402 static if( isDynamic || mtr.isDynamic ) 403 enforce( width == mtr.height, "incompatible sizes for mul" ); 404 Matrix!(H,bW,E) ret; 405 static if( ret.isDynamic ) ret.resize(height,width); 406 407 foreach( i; 0 .. height ) 408 foreach( j; 0 .. mtr.width ) 409 { 410 ret[i][j] = E( data[i][0] * mtr.data[0][j] ); 411 foreach( k; 1 .. width ) 412 ret[i][j] = ret[i][j] + E( data[i][k] * mtr.data[k][j] ); 413 } 414 415 return ret; 416 } 417 418 auto opOpAssign(string op, E)( in E b ) 419 if( mixin( `is( typeof( selftype.init ` ~ op ~ ` E.init ) == selftype )` ) ) 420 { mixin( `return this = this ` ~ op ~ ` b;` ); } 421 422 bool opCast(E)() const if( is( E == bool ) ) 423 { 424 foreach( v; asArray ) if( !isFinite(v) ) return false; 425 return true; 426 } 427 428 @property auto T() const 429 { 430 Matrix!(W,H,E) ret; 431 static if( isDynamic ) ret.resize(width,height); 432 foreach( i; 0 .. height) 433 foreach( j; 0 .. width ) 434 ret[j][i] = data[i][j]; 435 return ret; 436 } 437 438 auto setCol(X...)( size_t no, in X vals ) 439 if( is(typeof(flatData!E(vals))) ) 440 { 441 enforce( no < width, "bad col index" ); 442 auto buf = flatData!E(vals); 443 enforce( buf.length == height, "bad data length" ); 444 foreach( i; 0 .. height ) 445 data[i][no] = buf[i]; 446 return this; 447 } 448 449 auto setRow(X...)( size_t no, in X vals ) 450 if( is(typeof(flatData!E(vals))) ) 451 { 452 enforce( no < height, "bad row index" ); 453 auto buf = flatData!E(vals); 454 enforce( buf.length == width, "bad data length" ); 455 data[no][] = buf[]; 456 return this; 457 } 458 459 auto setRect(size_t bH, size_t bW, X)( size_t pos_row, size_t pos_col, in Matrix!(bH,bW,X) mtr ) 460 if( is(typeof(E(X.init))) ) 461 { 462 enforce( pos_row < height, "bad row index" ); 463 enforce( pos_col < width, "bad col index" ); 464 enforce( pos_row + mtr.height <= height, "bad height size" ); 465 enforce( pos_col + mtr.width <= width, "bad width size" ); 466 467 foreach( i; 0 .. mtr.height ) 468 foreach( j; 0 .. mtr.width ) 469 data[i+pos_row][j+pos_col] = E(mtr[i][j]); 470 471 return this; 472 } 473 474 auto col( size_t no ) const 475 { 476 enforce( no < width, "bad col index" ); 477 Matrix!(H,1,E) ret; 478 static if( ret.isDynamic ) 479 ret.resize(height,1); 480 foreach( i; 0 .. height ) 481 ret[i][0] = data[i][no]; 482 return ret; 483 } 484 485 auto row( size_t no ) const 486 { 487 enforce( no < height, "bad row index" ); 488 Matrix!(1,W,E) ret; 489 static if( ret.isDynamic ) 490 ret.resize(1,width); 491 ret[0][] = data[no][]; 492 return ret; 493 } 494 495 auto opBinary(string op,size_t K,X,alias string AS)( in Vector!(K,X,AS) v ) const 496 if( op=="*" && allowSomeOp(W,K) && isValidOp!("*",E,X) && isValidOp!("+",E,E) ) 497 { 498 static if( isDynamic || v.isDynamic ) 499 enforce( width == v.length, "wrong vector length" ); 500 501 static if( isStatic && W == H ) 502 Vector!(H,E,AS) ret; 503 else 504 Vector!(H,E) ret; 505 506 static if( ret.isDynamic ) 507 ret.length = height; 508 509 foreach( i; 0 .. height ) 510 { 511 ret[i] = data[i][0] * v[0]; 512 513 foreach( j; 1 .. width ) 514 ret[i] = ret[i] + E(data[i][j] * v[j]); 515 } 516 517 return ret; 518 } 519 520 auto opBinaryRight(string op,size_t K,X,alias string AS)( in Vector!(K,X,AS) v ) const 521 if( op=="*" && isVector!(typeof(selftype.init.T * typeof(v).init)) ) 522 { return this.T * v; } 523 524 static private size_t[] getIndexesWithout(size_t max, in size_t[] arr) 525 { 526 size_t[] ret; 527 foreach( i; 0 .. max ) if( !canFind(arr,i) ) ret ~= i; 528 return ret; 529 } 530 531 auto subWithout( size_t[] without_rows=[], size_t[] without_cols=[] ) const 532 { 533 auto with_rows = getIndexesWithout(height,without_rows); 534 auto with_cols = getIndexesWithout(width,without_cols); 535 536 return sub( with_rows,with_cols ); 537 } 538 539 auto sub( size_t[] with_rows, size_t[] with_cols ) const 540 { 541 auto wrows = array( uniq(with_rows) ); 542 auto wcols = array( uniq(with_cols) ); 543 544 enforce( all!(a=>a<height)( wrows ), "bad row index" ); 545 enforce( all!(a=>a<width)( wcols ), "bad col index" ); 546 547 Matrix!(0,0,E) ret; 548 ret.resize( wrows.length, wcols.length ); 549 550 foreach( i, orig_i; wrows ) 551 foreach( j, orig_j; wcols ) 552 ret[i][j] = data[orig_i][orig_j]; 553 554 return ret; 555 } 556 557 static if( isFloatingPoint!E && ( isDynamicAll || 558 ( isStaticHeightOnly && H > 1 ) || 559 ( isStaticWidthOnly && W > 1 ) || 560 ( isStatic && W == H ) ) ) 561 { 562 E cofactor( size_t i, size_t j ) const 563 { return subWithout([i],[j]).det * coef(i,j); } 564 565 private static nothrow @trusted auto coef( size_t i, size_t j ) 566 { return ((i+j)%2?-1:1); } 567 568 @property auto det() const 569 { 570 static if( isDynamic ) 571 enforce( width == height, "not square matrix" ); 572 573 static if( isDynamic ) 574 { 575 if( width == 1 ) 576 return data[0][0]; 577 else if( width == 2 ) 578 return data[0][0] * data[1][1] - 579 data[0][1] * data[1][0]; 580 else return classicDet; 581 } 582 else 583 { 584 static if( W == 1 ) 585 return data[0][0]; 586 else static if( W == 2 ) 587 return data[0][0] * data[1][1] - 588 data[0][1] * data[1][0]; 589 else return classicDet; 590 } 591 } 592 593 private @property classicDet() const 594 { 595 auto i = 0UL; // TODO: find max zeros line 596 auto ret = data[i][0] * cofactor(i,0); 597 598 foreach( j; 1 .. width ) 599 ret = ret + data[i][j] * cofactor(i,j); 600 601 return ret; 602 } 603 604 @property auto inv() const 605 { 606 static if( isDynamic ) 607 enforce( width == height, "not square matrix" ); 608 else static assert( W==H, "not square matrix" ); 609 610 selftype buf; 611 612 static if( isDynamic ) 613 buf.resize(height,width); 614 615 foreach( i; 0 .. height ) 616 foreach( j; 0 .. width ) 617 buf[i][j] = cofactor(i,j); 618 619 auto i = 0UL; // TODO: find max zeros line 620 auto d = data[i][0] * buf[i][0]; 621 622 foreach( j; 1 .. width ) 623 d = d + data[i][j] * buf[i][j]; 624 625 return buf.T / d; 626 } 627 628 /++ only for transform matrix +/ 629 static if( (isStaticHeightOnly && H==4) || 630 (isStaticWidthOnly && W==4) || 631 (isStatic && H==W && H==4) || 632 isDynamicAll ) 633 { 634 @property auto speedTransformInv() const 635 { 636 static if( isDynamic ) 637 { 638 enforce( width == height, "not square matrix" ); 639 enforce( width == 4, "matrix must be 4x4" ); 640 } 641 642 selftype ret; 643 static if( isDynamic ) 644 ret.resize( height, width ); 645 646 foreach( i; 0 .. 3 ) 647 foreach( j; 0 .. 3 ) 648 ret[i][j] = this[j][i]; 649 650 auto a22k = 1.0 / this[3][3]; 651 652 ret[0][3] = -( ret[0][0] * this[0][3] + ret[0][1] * this[1][3] + ret[0][2] * this[2][3] ) * a22k; 653 ret[1][3] = -( ret[1][0] * this[0][3] + ret[1][1] * this[1][3] + ret[1][2] * this[2][3] ) * a22k; 654 ret[2][3] = -( ret[2][0] * this[0][3] + ret[2][1] * this[1][3] + ret[2][2] * this[2][3] ) * a22k; 655 656 ret[3][0] = -( this[3][0] * ret[0][0] + this[3][1] * ret[1][0] + this[3][2] * ret[2][0] ) * a22k; 657 ret[3][1] = -( this[3][0] * ret[0][1] + this[3][1] * ret[1][1] + this[3][2] * ret[2][1] ) * a22k; 658 ret[3][2] = -( this[3][0] * ret[0][2] + this[3][1] * ret[1][2] + this[3][2] * ret[2][2] ) * a22k; 659 660 ret[3][3] = a22k * ( 1.0 - ( this[3][0] * ret[0][3] + this[3][1] * ret[1][3] + this[3][2] * ret[2][3] ) ); 661 662 return ret; 663 } 664 } 665 666 @property auto rowReduceInv() const 667 { 668 static if( isDynamic ) 669 enforce( width == height, "not square matrix" ); 670 671 auto orig = selftype(this); 672 selftype invt; 673 static if( isDynamic ) 674 invt.resize(height,height); 675 invt.fill( E(0) ); 676 invt.fillDiag( E(1) ); 677 678 static if( isStaticWidth ) 679 enum ln = W; 680 else static if( isStaticHeight ) 681 enum ln = H; 682 else 683 auto ln = height; 684 685 foreach( r; 0 .. ln-1 ) 686 { 687 size_t n = r+1; 688 foreach( rr; n .. ln ) 689 { 690 E k = orig[rr][r] / orig[r][r]; 691 foreach( c; 0 .. ln ) 692 { 693 orig[rr][c] -= k * orig[r][c]; 694 invt[rr][c] -= k * invt[r][c]; 695 } 696 } 697 } 698 699 foreach_reverse( r; 0 .. ln-1 ) 700 { 701 size_t n = r+1; 702 foreach( rr; 0 .. ln ) 703 { 704 E k = orig[rr][n] / orig[n][n]; 705 foreach( c; 0 .. ln ) 706 { 707 orig[rr][c] -= k * orig[n][c]; 708 invt[rr][c] -= k * invt[n][c]; 709 } 710 } 711 } 712 713 foreach( r; 0 .. ln ) 714 { 715 E ident = orig[r][r]; 716 foreach( c; 0 .. ln ) 717 { 718 orig[r][c] /= ident; 719 invt[r][c] /= ident; 720 } 721 } 722 723 return invt; 724 } 725 } 726 } 727 728 auto quatToMatrix(E)( Vector!(4,E,"i j k a") iq ) 729 { 730 auto q = iq / iq.len2; 731 732 E wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2; 733 734 x2 = q.i + q.i; 735 y2 = q.j + q.j; 736 z2 = q.k + q.k; 737 xx = q.i * x2; xy = q.i * y2; xz = q.i * z2; 738 yy = q.j * y2; yz = q.j * z2; zz = q.k * z2; 739 wx = q.a * x2; wy = q.a * y2; wz = q.a * z2; 740 741 return Matrix!(3,3,E)( 1.0-(yy+zz), xy-wz, xz+wy, 742 xy+wz, 1.0-(xx+zz), yz-wx, 743 xz-wy, yz+wx, 1.0-(xx+yy) ); 744 } 745 746 auto quatAndPosToMatrix(A,B,string AS)( in Vector!(4,A,"i j k a") iq, in Vector!(3,B,AS) pos ) 747 { 748 auto q = iq / iq.len2; 749 750 A wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2; 751 752 x2 = q.i + q.i; 753 y2 = q.j + q.j; 754 z2 = q.k + q.k; 755 xx = q.i * x2; xy = q.i * y2; xz = q.i * z2; 756 yy = q.j * y2; yz = q.j * z2; zz = q.k * z2; 757 wx = q.a * x2; wy = q.a * y2; wz = q.a * z2; 758 759 return Matrix!(4,4,A)( 1.0-(yy+zz), xy-wz, xz+wy, pos.x, 760 xy+wz, 1.0-(xx+zz), yz-wx, pos.y, 761 xz-wy, yz+wx, 1.0-(xx+yy), pos.z, 762 0, 0, 0, 1 ); 763 764 } 765 766 alias Matrix!(2,2,float) mat2; 767 alias Matrix!(2,3,float) mat2x3; 768 alias Matrix!(2,4,float) mat2x4; 769 alias Matrix!(2,0,float) mat2xD; 770 alias Matrix!(3,2,float) mat3x2; 771 alias Matrix!(3,3,float) mat3; 772 alias Matrix!(3,4,float) mat3x4; 773 alias Matrix!(3,0,float) mat3xD; 774 alias Matrix!(4,2,float) mat4x2; 775 alias Matrix!(4,3,float) mat4x3; 776 alias Matrix!(4,4,float) mat4; 777 alias Matrix!(4,0,float) mat4xD; 778 alias Matrix!(0,2,float) matDx2; 779 alias Matrix!(0,3,float) matDx3; 780 alias Matrix!(0,4,float) matDx4; 781 alias Matrix!(0,0,float) matD; 782 783 alias Matrix!(2,2,double) dmat2; 784 alias Matrix!(2,3,double) dmat2x3; 785 alias Matrix!(2,4,double) dmat2x4; 786 alias Matrix!(2,0,double) dmat2xD; 787 alias Matrix!(3,2,double) dmat3x2; 788 alias Matrix!(3,3,double) dmat3; 789 alias Matrix!(3,4,double) dmat3x4; 790 alias Matrix!(3,0,double) dmat3xD; 791 alias Matrix!(4,2,double) dmat4x2; 792 alias Matrix!(4,3,double) dmat4x3; 793 alias Matrix!(4,4,double) dmat4; 794 alias Matrix!(4,0,double) dmat4xD; 795 alias Matrix!(0,2,double) dmatDx2; 796 alias Matrix!(0,3,double) dmatDx3; 797 alias Matrix!(0,4,double) dmatDx4; 798 alias Matrix!(0,0,double) dmatD; 799 800 alias Matrix!(2,2,real) rmat2; 801 alias Matrix!(2,3,real) rmat2x3; 802 alias Matrix!(2,4,real) rmat2x4; 803 alias Matrix!(2,0,real) rmat2xD; 804 alias Matrix!(3,2,real) rmat3x2; 805 alias Matrix!(3,3,real) rmat3; 806 alias Matrix!(3,4,real) rmat3x4; 807 alias Matrix!(3,0,real) rmat3xD; 808 alias Matrix!(4,2,real) rmat4x2; 809 alias Matrix!(4,3,real) rmat4x3; 810 alias Matrix!(4,4,real) rmat4; 811 alias Matrix!(4,0,real) rmat4xD; 812 alias Matrix!(0,2,real) rmatDx2; 813 alias Matrix!(0,3,real) rmatDx3; 814 alias Matrix!(0,4,real) rmatDx4; 815 alias Matrix!(0,0,real) rmatD; 816 817 unittest 818 { 819 static assert( Matrix!(3,3,float).sizeof == float.sizeof * 9 ); 820 static assert( Matrix!(3,3,float).isStatic ); 821 static assert( Matrix!(0,3,float).isDynamic ); 822 static assert( Matrix!(0,3,float).isDynamicHeight ); 823 static assert( Matrix!(0,3,float).isStaticWidth ); 824 static assert( Matrix!(3,0,float).isDynamic ); 825 static assert( Matrix!(3,0,float).isDynamicWidth ); 826 static assert( Matrix!(3,0,float).isStaticHeight ); 827 static assert( Matrix!(0,0,float).isDynamic ); 828 static assert( Matrix!(0,0,float).isDynamicHeight ); 829 static assert( Matrix!(0,0,float).isDynamicWidth ); 830 } 831 832 unittest 833 { 834 assert( eq( mat3.init, [[1,0,0],[0,1,0],[0,0,1]] ) ); 835 assert( eq( mat2.init, [[1,0],[0,1]] ) ); 836 assert( eq( mat2x3.init, [[0,0,0],[0,0,0]] ) ); 837 } 838 839 unittest 840 { 841 auto a = mat3( 1,2,3,4,5,6,7,8,9 ); 842 assert( a.height == 3 ); 843 assert( a.width == 3 ); 844 assert( eq( a, [[1,2,3],[4,5,6],[7,8,9]] ) ); 845 static assert( !__traits(compiles,a.resize(1,1)) ); 846 a.fill(1); 847 assert( eq( a, [[1,1,1], [1,1,1], [1,1,1]] ) ); 848 a[0][0] *= 3; 849 assert( eq( a, [[3,1,1], [1,1,1], [1,1,1]] ) ); 850 a[1][0] += 3; 851 assert( eq( a, [[3,1,1], [4,1,1], [1,1,1]] ) ); 852 } 853 854 unittest 855 { 856 static struct Test 857 { 858 union 859 { 860 mat3 um; 861 float[9] uf; 862 } 863 } 864 865 Test tt; 866 867 foreach( i, ref v; tt.uf ) v = i+1; 868 assert( eq( tt.um, [[1,2,3],[4,5,6],[7,8,9]] ) ); 869 } 870 871 unittest 872 { 873 auto a = matDx3( 1,2,3,4,5,6,7,8,9 ); 874 assert( mustExcept( {matDx3(1,2,3,4);} ) ); 875 assert( a.height == 3 ); 876 assert( a.width == 3 ); 877 assert( eq( a, [[1,2,3],[4,5,6],[7,8,9]] ) ); 878 assert( mustExcept({ a.resize(2,2); }) ); 879 a.resize(2,3); 880 assert( eq( a, [[1,2,3],[4,5,6]] ) ); 881 assert( a.height == 2 ); 882 a.fill(1); 883 assert( eq( a, [[1,1,1],[1,1,1]] ) ); 884 a.resize(0,3); 885 assert( a.width == 3 ); 886 a.resize(2,3); 887 a.fill(1); 888 889 auto b = a; 890 assert( eq( b, [[1,1,1],[1,1,1]] ) ); 891 } 892 893 unittest 894 { 895 auto a = mat3xD( 1,2,3,4,5,6,7,8,9 ); 896 assert( mustExcept( {mat3xD(1,2,3,4);} ) ); 897 assert( a.height == 3 ); 898 assert( a.width == 3 ); 899 assert( eq( a, [[1,2,3],[4,5,6],[7,8,9]] ) ); 900 assert( mustExcept({ a.resize(2,2); }) ); 901 a.resize(3,2); 902 assert( eq( a, [[1,2],[4,5],[7,8]] ) ); 903 assert( a.width == 2 ); 904 a.fill(1); 905 assert( eq( a, [[1,1],[1,1],[1,1]] ) ); 906 907 auto b = a; 908 assert( eq( b, [[1,1],[1,1],[1,1]] ) ); 909 } 910 911 unittest 912 { 913 auto a = matD( 3,3, 1,2,3,4,5,6,7,8,9 ); 914 assert( mustExcept( {matD(1,2,3,4,5);} ) ); 915 assert( a.height == 3 ); 916 assert( a.width == 3 ); 917 assert( eq( a, [[1,2,3],[4,5,6],[7,8,9]] ) ); 918 a.resize(2,2); 919 assert( eq( a, [[1,2],[4,5]] ) ); 920 auto b = matD(2,2); 921 b.fill(1); 922 assert( eq( b, [[1,1],[1,1]] ) ); 923 b = a; 924 assert( eq( a, b ) ); 925 auto c = matD( Matrix!(2,4,float)(1,2,3,4,5,6,7,8) ); 926 assert( eq( c, [[1,2,3,4],[5,6,7,8]] ) ); 927 assert( c.height == 2 ); 928 assert( c.width == 4 ); 929 assert( b.height == 2 ); 930 assert( b.width == 2 ); 931 b = c; 932 assert( b.height == 2 ); 933 assert( b.width == 4 ); 934 assert( eq( b, c ) ); 935 b[0][0] = 666; 936 assert( !eq( b, c ) ); 937 } 938 939 unittest 940 { 941 auto a = mat3xD( 1,2,3,4,5,6,7,8,9 ); 942 matD b; 943 assert( b.height == 0 ); 944 assert( b.width == 0 ); 945 b = a; 946 assert( b.height == 3 ); 947 assert( b.width == 3 ); 948 assert( eq( a, b ) ); 949 a[0][0] = 22; 950 assert( !eq( a, b ) ); 951 a = b; 952 assert( eq( a, b ) ); 953 b.height = 4; 954 assert( mustExcept({ a = b; }) ); 955 assert( !mustExcept({ b = a; }) ); 956 } 957 958 unittest 959 { 960 auto a = mat3( 1,2,3,4,5,6,7,8,9 ); 961 auto b = matD( 3,3, 1,2,3,4,5,6,7,8,9 ); 962 auto c = mat3xD( 1,2,3,4,5,6,7,8,9 ); 963 auto d = matDx3( 1,2,3,4,5,6,7,8,9 ); 964 965 auto eha = a.expandHeight( 8,8,8 ); 966 auto ehb = b.expandHeight( 8,8,8 ); 967 auto ehc = c.expandHeight( 8,8,8 ); 968 auto ehd = d.expandHeight( 8,8,8 ); 969 970 assert( eq( eha, [[1,2,3],[4,5,6],[7,8,9],[8,8,8]] )); 971 assert( eha.height == 4 ); 972 assert( ehb.height == 4 ); 973 assert( ehc.height == 4 ); 974 assert( ehd.height == 4 ); 975 assert( eq( eha, ehb ) ); 976 assert( eq( eha, ehc ) ); 977 assert( eq( eha, ehd ) ); 978 979 static assert( is(typeof(eha) == Matrix!(0,3,float)) ); 980 static assert( is(typeof(ehd) == Matrix!(0,3,float)) ); 981 982 static assert( is(typeof(ehb) == Matrix!(0,0,float)) ); 983 static assert( is(typeof(ehc) == Matrix!(0,0,float)) ); 984 985 auto ewa = a.expandWidth( 8,8,8 ); 986 auto ewb = b.expandWidth( 8,8,8 ); 987 auto ewc = c.expandWidth( 8,8,8 ); 988 auto ewd = d.expandWidth( 8,8,8 ); 989 990 assert( eq( ewa, [[1,2,3,8],[4,5,6,8],[7,8,9,8]] )); 991 assert( ewa.width == 4 ); 992 assert( ewb.width == 4 ); 993 assert( ewc.width == 4 ); 994 assert( ewd.width == 4 ); 995 assert( eq( ewa, ewb ) ); 996 assert( eq( ewa, ewc ) ); 997 assert( eq( ewa, ewd ) ); 998 999 static assert( is(typeof(ewa) == Matrix!(3,0,float)) ); 1000 static assert( is(typeof(ewc) == Matrix!(3,0,float)) ); 1001 1002 static assert( is(typeof(ewb) == Matrix!(0,0,float)) ); 1003 static assert( is(typeof(ewd) == Matrix!(0,0,float)) ); 1004 1005 auto aa = a.expandHeight(a); 1006 assert( eq( aa, [[1,2,3],[4,5,6],[7,8,9],[1,2,3],[4,5,6],[7,8,9]] )); 1007 assert( aa.height == 6 ); 1008 static assert( is(typeof(aa) == Matrix!(0,3,float)) ); 1009 } 1010 1011 unittest 1012 { 1013 auto a = mat3( 1,2,3,4,5,6,7,8,9 ); 1014 assert( a.asArray == [1.0f,2,3,4,5,6,7,8,9] ); 1015 } 1016 1017 unittest 1018 { 1019 auto a = mat3.diag(1); 1020 assert( eq(a,[[1,0,0],[0,1,0],[0,0,1]]) ); 1021 auto b = mat3xD.diag(1,2); 1022 assert( eq(b,[[1,0,0],[0,2,0],[0,0,1]]) ); 1023 auto c = mat3xD.diag(1,2,3); 1024 assert( eq(c,[[1,0,0],[0,2,0],[0,0,3]]) ); 1025 static assert( !__traits(compiles,matD.diag(1)) ); 1026 auto d = matD(3,3).fillDiag(1); 1027 assert( eq(d,[[1,0,0],[0,1,0],[0,0,1]]) ); 1028 } 1029 1030 unittest 1031 { 1032 auto a = mat3( 1,2,3,4,5,6,7,8,9 ); 1033 auto sha = a.sliceHeight(1); 1034 assert( eq(sha,[[4,5,6],[7,8,9]]) ); 1035 auto swsha = sha.sliceWidth(0,1); 1036 assert( eq(swsha,[[4],[7]]) ); 1037 } 1038 1039 unittest 1040 { 1041 auto a = mat3.diag(1); 1042 assert( eq( -a,[[-1,0,0],[0,-1,0],[0,0,-1]]) ); 1043 } 1044 1045 unittest 1046 { 1047 auto a = mat3.diag(1); 1048 auto b = a*3-a; 1049 assert( eq( b,a*2 ) ); 1050 b /= 2; 1051 assert( eq( b,a ) ); 1052 } 1053 1054 unittest 1055 { 1056 auto a = rmat3( 3,2,2, 1,3,1, 5,3,4 ); 1057 auto ainv = rmat3xD( 9,-2,-4, 1,2,-1, -12,1,7 ) / 5; 1058 1059 auto b = a * ainv; 1060 static assert( is( typeof(b) == Matrix!(3,0,real) ) ); 1061 assert( eq( b, mat3.diag(1) ) ); 1062 1063 static assert( !__traits(compiles,a *= ainv ) ); 1064 a *= rmat3(ainv); 1065 assert( eq( a, mat3.diag(1) ) ); 1066 } 1067 1068 unittest 1069 { 1070 alias Matrix!(4,5,float) mat4x5; 1071 1072 auto a = mat2x4.init * mat4xD.init; 1073 assert( mustExcept({ mat4xD.init * mat2x4.init; }) ); 1074 auto b = mat2x4.init * mat4x5.init; 1075 1076 static assert( is( typeof(a) == Matrix!(2,0,float) ) ); 1077 static assert( is( typeof(b) == Matrix!(2,5,float) ) ); 1078 static assert( is( typeof(mat4xD.init * mat2x4.init) == Matrix!(4,4,float) ) ); 1079 } 1080 1081 unittest 1082 { 1083 1084 auto a = mat2x3( 1,2,3, 4,5,6 ); 1085 auto b = mat4xD( 1,2,3,4 ); 1086 1087 auto aT = a.T; 1088 auto bT = b.T; 1089 1090 static assert( is( typeof(aT) == Matrix!(3,2,float) ) ); 1091 static assert( is( typeof(bT) == Matrix!(0,4,float) ) ); 1092 1093 assert( bT.height == b.width ); 1094 } 1095 1096 unittest 1097 { 1098 auto a = mat3.diag(1); 1099 1100 a.setRow(2,4,5,6); 1101 assert( eq( a, [[1,0,0],[0,1,0],[4,5,6]] ) ); 1102 a.setCol(1,8,4,2); 1103 assert( eq( a, [[1,8,0],[0,4,0],[4,2,6]] ) ); 1104 1105 assert( eq( a.row(0), [[1,8,0]] ) ); 1106 assert( eq( a.col(0), [[1],[0],[4]] ) ); 1107 } 1108 1109 unittest 1110 { 1111 auto b = mat3.diag(2) * vec3(2,3,4); 1112 assert( is( typeof(b) == vec3 ) ); 1113 assert( eq( b, [4,6,8] ) ); 1114 1115 auto c = vec3(1,1,1) * mat3.diag(1,2,3); 1116 assert( is( typeof(c) == vec3 ) ); 1117 assert( eq( c, [1,2,3] ) ); 1118 } 1119 1120 unittest 1121 { 1122 auto mtr = matD(2,3).fill( 1,2,3, 1123 3,4,7 ); 1124 1125 auto a = mtr * vec3(1,1,1); 1126 1127 assert( is( typeof(a) == Vector!(0,float) ) ); 1128 assert( a.length == 2 ); 1129 assert( eq( a, [ 6, 14 ] ) ); 1130 1131 auto b = vec3(1,1,1) * mtr.T; 1132 assert( eq( a, b ) ); 1133 } 1134 1135 unittest 1136 { 1137 void check(E)( E mtr ) if( isMatrix!E ) 1138 { 1139 mtr.fill( 1,2,3,4, 1140 5,6,7,8, 1141 9,10,11,12, 1142 13,14,15,16 ); 1143 auto sm = mtr.subWithout( [0,3,3,3], [1,2] ); 1144 assert( is( typeof(sm) == matD ) ); 1145 assert( sm.width == 2 ); 1146 assert( sm.height == 2 ); 1147 assert( eq( sm, [ [5,8], [9,12] ] ) ); 1148 assert( mustExcept({ mtr.sub( [0,4], [1,2] ); }) ); 1149 auto sm2 = mtr.subWithout( [], [1,2] ); 1150 assert( sm2.width == 2 ); 1151 assert( sm2.height == 4 ); 1152 assert( eq( sm2, [ [1,4],[5,8],[9,12],[13,16] ] ) ); 1153 } 1154 1155 check( matD(4,4) ); 1156 check( mat4() ); 1157 } 1158 1159 unittest 1160 { 1161 assert( eq( matD(4,4).fillDiag(1,2,3,4).det, 24 ) ); 1162 } 1163 1164 unittest 1165 { 1166 auto mtr = rmatD(4,4).fill( 1,2,5,2, 1167 5,6,1,4, 1168 9,1,3,0, 1169 9,2,4,2 ); 1170 auto xx = mtr * mtr.inv; 1171 assert( eq( xx, matD(4,4).fillDiag(1) ) ); 1172 } 1173 1174 unittest 1175 { 1176 auto mtr = matD(4,4).fill( 0,1,0,2, 1177 1,0,0,4, 1178 0,0,1,1, 1179 0,0,0,1 ); 1180 auto vv = vec4(4,2,1,1); 1181 auto rv = mtr.speedTransformInv * (mtr*vv); 1182 assert( eq( rv, vv ) ); 1183 } 1184 1185 unittest 1186 { 1187 auto mtr = matD(4,4).fillDiag(1); 1188 auto vv = vec4(4,2,1,1); 1189 auto rv = vv * mtr; 1190 auto vr = mtr.T * vv * mtr; 1191 } 1192 1193 unittest 1194 { 1195 auto mtr = rmat4.diag(1); 1196 auto vv = vec4(4,2,1,1); 1197 auto rv = vv * mtr; 1198 auto vr = mtr.T * vv * mtr; 1199 1200 auto xx = mtr * mtr.inv; 1201 assert( eq( xx, mat4.diag(1) ) ); 1202 } 1203 1204 unittest 1205 { 1206 auto mtr = rmat4( 1,2,5,2, 1207 5,6,1,4, 1208 9,1,3,0, 1209 9,2,4,2 ); 1210 1211 auto xx = mtr * mtr.rowReduceInv; 1212 1213 assert( eq( xx, mat4.diag(1) ) ); 1214 } 1215 1216 unittest 1217 { 1218 auto mtr = mat4().setRect(0,0,mat3.diag(1)).setCol(3,1,2,3,4); 1219 assert( eq( mtr, [[1,0,0,1], 1220 [0,1,0,2], 1221 [0,0,1,3], 1222 [0,0,0,4]] ) ); 1223 } 1224 1225 unittest 1226 { 1227 auto stm = mat4(); 1228 assert( stm ); 1229 auto dnm = matD(); 1230 assert( dnm ); 1231 } 1232 1233 unittest 1234 { 1235 auto q = quat.fromAngle( PI_2, vec3(0,0,1) ); 1236 1237 auto m = quatToMatrix(q); 1238 assert( eq( m, [[0,-1,0], 1239 [1, 0,0], 1240 [0, 0,1]] )); 1241 } 1242 1243 unittest 1244 { 1245 auto q = quat.fromAngle( PI_2, vec3(0,0,1) ); 1246 1247 auto m = quatAndPosToMatrix(q, vec3(1,2,3) ); 1248 assert( eq( m, [[0,-1,0,1], 1249 [1, 0,0,2], 1250 [0, 0,1,3], 1251 [0, 0,0,1]] )); 1252 1253 }