public Matrix4d4(Matrix3d3 <T> r, Vector3d <T> t, T unit)
{
x = new Numeric <T> [4][];
for (int i = 0; i < 4; i++)
{
x[i] = new Numeric <T> [4];
}
x[0][0] = r[0][0];
x[0][1] = r[0][1];
x[0][2] = r[0][2];
x[0][3] = Numeric <T> .Zero();
x[1][0] = r[1][0];
x[1][1] = r[1][1];
x[1][2] = r[1][2];
x[1][3] = Numeric <T> .Zero();
x[2][0] = r[2][0];
x[2][1] = r[2][1];
x[2][2] = r[2][2];
x[2][3] = Numeric <T> .Zero();
x[3][0] = t[0];
x[3][1] = t[1];
x[3][2] = t[2];
x[3][3] = unit;
}
public static Matrix3d3 <T> Identity(T unit)
{
Matrix3d3 <T> m = new Matrix3d3 <T>(Numeric <T> .Zero());
m.MakeIdentity(unit);
return(m);
}
public override bool Equals(object obj)
{
if (obj != null && obj is Matrix3d3 <T> )
{
Matrix3d3 <T> other = (Matrix3d3 <T>)obj;
return(Equals(ref this, ref other));
}
return(base.Equals(obj));
}
private static bool Equals(ref Matrix3d3 <T> v1, ref Matrix3d3 <T> v2)
{
return(EqualityComparer <T> .Default.Equals(v1.x[0][0], v2.x[0][0]) &&
EqualityComparer <T> .Default.Equals(v1.x[0][1], v2.x[0][1]) &&
EqualityComparer <T> .Default.Equals(v1.x[0][2], v2.x[0][2]) &&
EqualityComparer <T> .Default.Equals(v1.x[1][0], v2.x[1][0]) &&
EqualityComparer <T> .Default.Equals(v1.x[1][1], v2.x[1][1]) &&
EqualityComparer <T> .Default.Equals(v1.x[1][2], v2.x[1][2]) &&
EqualityComparer <T> .Default.Equals(v1.x[2][0], v2.x[2][0]) &&
EqualityComparer <T> .Default.Equals(v1.x[2][1], v2.x[2][1]) &&
EqualityComparer <T> .Default.Equals(v1.x[2][2], v2.x[2][2]));
}
public Matrix3d3 <T> SetTheMatrix(Matrix3d3 <T> v)
{
x[0][0] = v.x[0][0];
x[0][1] = v.x[0][1];
x[0][2] = v.x[0][2];
x[1][0] = v.x[1][0];
x[1][1] = v.x[1][1];
x[1][2] = v.x[1][2];
x[2][0] = v.x[2][0];
x[2][1] = v.x[2][1];
x[2][2] = v.x[2][2];
return(this);
}
public Matrix3d3 <T> Shear(Vector2 <T> h)
{
Matrix3d3 <T> P = new Matrix3d3 <T>(this);
x[0][0] = P[0][0] + h[1] * P[1][0];
x[0][1] = P[0][1] + h[1] * P[1][1];
x[0][2] = P[0][2] + h[1] * P[1][2];
x[1][0] = P[1][0] + h[0] * P[0][0];
x[1][1] = P[1][1] + h[0] * P[0][1];
x[1][2] = P[1][2] + h[0] * P[0][2];
return(this);
}
Transpose()
{
Matrix3d3 <T> tmp = new Matrix3d3 <T>(x[0][0],
x[1][0],
x[2][0],
x[0][1],
x[1][1],
x[2][1],
x[0][2],
x[1][2],
x[2][2]);
this = tmp;
return(this);
}
/// <summary>
/// Matrix multiplication
/// </summary>
/// <param name="v"></param>
/// <returns></returns>
public static Matrix3d3 <T> operator *(Matrix3d3 <T> v2, Matrix3d3 <T> v)
{
Matrix3d3 <T> tmp = new Matrix3d3 <T>(Numeric <T> .Zero());
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
for (int k = 0; k < 3; k++)
{
tmp.x[i][j] += v2.x[i][k] * v.x[k][j];
}
}
}
return(tmp);
}
public Matrix3d3(Matrix3d3 <T> v)
{
x = new Numeric <T> [3][];
for (int k = 0; k < 3; k++)
{
x[k] = new Numeric <T> [3];
}
x[0][0] = v.x[0][0];
x[0][1] = v.x[0][1];
x[0][2] = v.x[0][2];
x[1][0] = v.x[1][0];
x[1][1] = v.x[1][1];
x[1][2] = v.x[1][2];
x[2][0] = v.x[2][0];
x[2][1] = v.x[2][1];
x[2][2] = v.x[2][2];
}
public bool Equals(Matrix3d3 <T> other)
{
return(Equals(this, other));
}
/// <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);
}