/// <summary> /// Set matrix to rotation by angle /// </summary> /// <param name="angle">angle of rotation in radians</param> /// <returns></returns> public Matrix3d3 <T> Rotate(T angle, T unit) { this = this * Matrix3d3 <T> .Identity(unit).SetRotation(angle, unit); return(this); }
Inverse(T unit) { int i, j, k; Matrix3d3 <T> s = Matrix3d3 <T> .Identity(unit); Matrix3d3 <T> t = new Matrix3d3 <T>(this); // Forward elimination for (i = 0; i < 2; i++) { int pivot = i; Numeric <T> pivotsize = (t[i][i]); if (pivotsize < Numeric <T> .Zero()) { pivotsize = -pivotsize; } for (j = i + 1; j < 3; j++) { Numeric <T> tmp = (t[j][i]); if (tmp < Numeric <T> .Zero()) { tmp = -tmp; } if (tmp > pivotsize) { pivot = j; pivotsize = tmp; } } if (pivotsize.Equals(Numeric <T> .Zero())) { throw new Exception("Cannot invert singular matrix."); } if (pivot != i) { for (j = 0; j < 3; j++) { T tmp; tmp = t[i][j]; t[i][j] = t[pivot][j]; t[pivot][j] = tmp; tmp = s[i][j]; s[i][j] = s[pivot][j]; s[pivot][j] = tmp; } } for (j = i + 1; j < 3; j++) { T f = t[j][i] / t[i][i]; for (k = 0; k < 3; k++) { t[j][k] -= f * t[i][k]; s[j][k] -= f * s[i][k]; } } } // Backward substitution for (i = 2; i >= 0; --i) { Numeric <T> f; if ((f = t[i][i]).Equals(Numeric <T> .Zero())) { throw new Exception("Cannot invert singular matrix."); } for (j = 0; j < 3; j++) { t[i][j] /= f; s[i][j] /= f; } for (j = 0; j < i; j++) { f = t[j][i]; for (k = 0; k < 3; k++) { t[j][k] -= f * t[i][k]; s[j][k] -= f * s[i][k]; } } } return(s); }