public void substract(CMatrix4x4 m)
{
_m[0].substract(m[0]);
_m[1].substract(m[1]);
_m[2].substract(m[2]);
_m[3].substract(m[3]);
}
public void add(CMatrix4x4 m)
{
_m[0].add(m[0]);
_m[1].add(m[1]);
_m[2].add(m[2]);
_m[3].add(m[3]);
}
public CMatrix4x4(CMatrix4x4 m)
{
_m[0] = new CVector4(m._m[0]);
_m[1] = new CVector4(m._m[1]);
_m[2] = new CVector4(m._m[2]);
_m[3] = new CVector4(m._m[3]);
}
public static CMatrix3x3 realign(CXYVector target, CXYVector anchor, double [] weight)
{
double X1, Y1, X2, Y2, W, Z, C1, C2;
int k = anchor.size;
X1 = Y1 = X2 = Y2 = W = Z = C1 = C2 = 0;
while ((k--) != 0)
{
double x1 = anchor[k][0];
double y1 = anchor[k][1];
double x2 = target[k][0];
double y2 = target[k][1];
double w = weight[k];
W += w;
Z += w * (x2 * x2 + y2 * y2);
X1 += w * x1; Y1 += w * y1;
X2 += w * x2; Y2 += w * y2;
C1 += w * (x1 * x2 + y1 * y2);
C2 += w * (y1 * x2 - x1 * y2);
}
CVector4 solution =
new CMatrix4x4(
new CVector4(X2, -Y2, W, 0),
new CVector4(Y2, X2, 0, W),
new CVector4(Z, 0, X2, Y2),
new CVector4(0, Z, -Y2, X2)
).inverse().multiplyCopy(new CVector4(X1, Y1, C1, C2));
CMatrix3x3 trx = new CMatrix3x3(
new CVector3(solution[0], -solution[1], solution[2]),
new CVector3(solution[1], solution[0], solution[3]),
new CVector3(0, 0, 1));
target.transform(trx);
return(trx);
}
public CMatrix4x4 inverse()
{
CMatrix4x4 a = new CMatrix4x4(this);
CMatrix4x4 b = new CMatrix4x4();
int i, j, i1;
for (j = 0; j < 4; j++)
{
i1 = j;
for (i = j + 1; i < 4; i++)
{
if (Math.Abs(a[i][j]) > Math.Abs(a[i1][j]))
{
i1 = i;
}
}
a.swapRow(i1, j);
b.swapRow(i1, j);
if (a[j][j] == 0)
{
return(null);
}
b[j].divide(a[j][j]);
a[j].divide(a[j][j]);
for (i = 0; i < 4; i++)
{
if (i != j)
{
b[i].substract(b[j].multiplyCopy(a[i][j]));
a[i].substract(a[j].multiplyCopy(a[i][j]));
}
}
}
return(b);
}