/// <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);
}
