Habe auch mal in einem Forum für Mathematik nachgefragt und ausführliche Antwort erhalten.
http://www.matheraum.de/read?t=573598
Bin wegen Umbau noch nicht dazu gekommen daran zu arbeiten, aber es sieht sehr gut aus, sodass ich es vermutlich verstehen werde.
[EDIT]
Hier meine Lösung:
struct BldPoint
{
public double wx, wy, ix, iy;
public BldPoint(double _wx,double _wy,double _ix,double _iy)
{
wx = _wx;
wy = _wy;
ix = _ix;
iy = _iy;
}
}
class Referenz
{
// Prepare some helpers:
// To store the formal product A * v, we store which element of v is used
// as coefficient for which variable of A in the j-th row as H1[j][t],
// where t = 0, 1, 2 denotes the coordinates of v.
int[][] H1 = { new int[] { 0, 1, 2 }, new int[] { 3, 4, 5 }, new int[] { 6, 7, 8 } };
// To store [w]_\times, we use H2[i][j] to specify -3, -2, ..., 3 with 0
// being 0, t > 0 being the t-th component of w (beginning with 1) and
// -t > 0 being the negative of the t-th component of w.
int[][] H2 = { new int[] { 0, -3, 2 }, new int[] { 3, 0, -1 }, new int[] { -2, 1, 0 } };
// Note that we only use the first two rows of H2!
const int ninput = 4;
const int nlineq = ninput * 2;
double[,] lineq;
BldPoint[] input;
double[][] A = new double[3][];
public Referenz(BldPoint OL, BldPoint UL, BldPoint OR, BldPoint UR)
{
A[0] = new double[3];
A[1] = new double[3];
A[2] = new double[3];
input = new BldPoint[] { OL, UL, OR, UR };
lineq = new double[nlineq, 9];
for (int i = 0; i < ninput; ++i)
{
// Clear the equations to zero
for (int j = 0; j < 2; ++j)
for (int k = 0; k < 3 * 3; ++k)
lineq[i * 2 + j, k] = 0.0;
// Prepare v and w
double[] v = new double[3];
double[] w = new double[3];
v[0] = input[i].ix;
v[1] = input[i].iy;
v[2] = 1.0;
w[0] = input[i].wx;
w[1] = input[i].wy;
w[2] = 1.0;
// Generate the linear equations
for (int j = 0; j < 2; ++j)
{
// Generate the j-th equation
for (int t = 0; t < 3; ++t)
{
int c = H2[j][t];
bool neg = c < 0;
if (neg)
c = -c;
if (c != 0)
{
// Coefficient is not zero, i.e. do something
for (int s = 0; s < 3; ++s)
lineq[i * 2 + j, H1[t][s]] += (neg ? w[c - 1] : -w[c - 1]) * v[s];
}
}
}
}
// the Gauss-Jordan algorithm ([URL]http://en.wikipedia.org/wiki/Gauss–Jordan_elimination[/URL])
const double epsilon = 1E-20; // the error tolerance
bool[] used = new bool[3 * 3]; // the pivot columns
int r = 0;
for (int c = 0; c < 3 * 3; ++c)
// Iterate the columns
{
// Find the pivot row (choose the one with the largest element)
double m = 0.0;
int pivot = -1;
for (int j = r; j < nlineq; ++j)
if (lineq[j, c] > m)
{
m = lineq[j, c];
pivot = j;
}
else if (-lineq[j, c] > m)
{
m = -lineq[j, c];
pivot = j;
}
// Check m against tolerance
if (m <= epsilon)
{
// Too small: skip column
// Clear the values to zero
for (int j = r; j < nlineq; ++j)
lineq[j, c] = 0.0;
// Mark this column as unused
used[c] = false;
}
else
{
// Mark this column as used
used[c] = true;
// Swap current with pivot row
for (int j = c; j < 3 * 3; ++j)
swap(ref lineq[r, j], ref lineq[pivot, j]);
// Normalize pivot row
for (int j = c + 1; j < 3 * 3; ++j)
lineq[r, j] /= lineq[r, c];
lineq[r, c] = 1.0;
// Eliminate the current column
for (int rr = 0; rr < nlineq; ++rr)
if (rr != r)
{
// Work on the rr-th row
for (int j = c + 1; j < 3 * 3; ++j)
lineq[rr, j] -= lineq[rr, c] * lineq[r, j];
lineq[rr, c] = 0.0;
}
// Go to the next row
++r;
}
}
// Check for right rank
if (r < 8)
{
throw new Exception("Rank is too small!?");
}
if (r > 8)
{
throw new Exception("The matrix seems to be over-determined, i.e. we obtain A = 0. We don't want that.");
}
int freecoord = 0;
for (int i = 0; i < 3 * 3; ++i)
if (!used[i])
freecoord = i;
// The free coordinate will be set to 1, the others filled from this value using the linear equations
A[freecoord / 3][freecoord % 3] = 1.0;
for (int i = 0; i < 3 * 3; ++i)
{
if (i < freecoord)
A[i / 3][i % 3] = -lineq[i,freecoord];
if (i > freecoord)
A[i / 3][i % 3] = -lineq[i - 1,freecoord];
}
}
public BldPoint Convertiere(BldPoint toConvert)
{
double[] v= new double[3], w = new double[3];
v[0] = toConvert.ix;
v[1] = toConvert.iy;
v[2] = 1.0;
// Compute w = A * v
for (int j = 0; j < 3; ++j)
{
w[j] = 0.0;
for (int k = 0; k < 3; ++k)
w[j] += A[j][k] * v[k];
}
// We should have w[2] != 0
if(w[2] == 0) throw new Exception("");
// Normalize, i.e. make sure w[2] == 1
w[0] /= w[2];
w[1] /= w[2];
w[2] = 1.0;
// Store and output
toConvert.wx = w[0];
toConvert.wy = w[1];
return toConvert;
}
void swap(ref double a, ref double b)
{
double c = b;
b = a;
a = c;
}
}
[/EDIT]
Gruß Robert
