Topic: Matrix math problem - Align coordinate system

[pkohut]

This is what I've got so far this morning, maybe 1 hour coding/API lookup and 2 hours studing Gile's code.  Any time to work on it this weekend will have to be sneaked in, as the kids are over.  One of them has been over my shoulder watching me for the last 10 minutes, and keeps asking when I'll be done.  Right Now!

Code: [Select]

struct SortedIndex
{
    SortedIndex(int nPos, const double * pVal) : m_nPos(nPos), m_pVal(pVal) {}
    int Position(void) const { return m_nPos; }

    int m_nPos;
    const double * m_pVal;
};

inline bool sort(const SortedIndex & si1, const SortedIndex & si2)
{
    return (*si1.m_pVal > *si2.m_pVal);
}

inline void Sort_I(const double * pData, std::vector<SortedIndex> & index)
{
    std::sort(index.begin(), index.end(), sort);
}

inline int Find(std::vector<SortedIndex> & index, int nVal)
{
    for(std::vector<SortedIndex>::iterator it = index.begin(); it != index.end(); ++it) {
        if(it->Position() == nVal) {
            return it->Position();
        }
    }
    return -1;
}

template<class T, int NSize>
int Giles(const Eigen::Matrix<T, NSize, NSize> & m1, const Eigen::Matrix<T, NSize, NSize> & m2, Eigen::Matrix<T, NSize, NSize> & m3)
{
    enum { nCols = NSize, mCols = NSize, };
    Eigen::Matrix<T, NSize, NSize * 2> mm;
    mm.block<NSize, NSize>(0,0) = m1;//.transpose();
    mm.block<NSize, NSize>(0, NSize) = m2;//.transpose();

    for(int i = 0; i < nCols; ++i) {

        Eigen::Matrix<T, NSize * 2, 1> row;
        Eigen::Matrix<T, NSize, 1> col;
        row = mm.row(i);
        col = mm.col(i);

        std::vector<SortedIndex> index;
        for(int i = 0; i < NSize; ++i)
            index.push_back(SortedIndex(i, &col(i)));
        Sort_I(row.data(), index);

        int nPos = Find(index, 0);

//        for(int i = 0; i < index.)
    }
    return 0;
}


Code: [Select]

int _tmain(int argc, _TCHAR * argv[])
{
    Eigen::Matrix<double, 3, 3> m1, m2, m3;

    m1 << 107, 105, 103, 99, 115, 87, 123, 125, 127;
    m2 << 253, 10, 257, 263, 50, 267, 1, 275, 277;

    int nVal = Giles<double, 3>(m1, m2, m3);
    return 0;
}


[LE3]

give a few minutes, and will post what I have in C++.... here it goes (does not verify if the determinant is a non-zero and zero error checking):

Code: [Select]

typedef float M3[9];
static void test1 (void)
{
M3 origMat;
origMat[0] = 1.0;
origMat[1] = 3.0;
origMat[2] = 0.0;
origMat[3] = 5.0;
origMat[4] = 7.0;
origMat[5] = 0.0;
origMat[6] = 10.0;
origMat[7] = 10.0;
origMat[8] = 1.0;

M3 finalMat;
finalMat[0] = 11.0;
finalMat[1] = 13.0;
finalMat[2] = 0.0;
finalMat[3] = 17.0;
finalMat[4] = 19.0;
finalMat[5] = 0.0;
finalMat[6] = 20.0;
finalMat[7] = 5.0;
finalMat[8] = 1.0;

float det;
det = origMat[0] * (origMat[4] * origMat[8] - origMat[7] * origMat[5])
- origMat[1] * (origMat[3] * origMat[8] - origMat[6] * origMat[5])
+ origMat[2] * (origMat[3] * origMat[7] - origMat[6] * origMat[4]);

M3 invMat;
invMat[0]=(((origMat[4] * origMat[8]) - (origMat[5] * origMat[7])) / det);
invMat[1]=(-(((origMat[1] * origMat[8]) - (origMat[7] * origMat[2]))) / det);
invMat[2]=(((origMat[1] * origMat[5]) - (origMat[4] * origMat[2])) / det);

invMat[3]=(-(((origMat[3] * origMat[8]) - (origMat[5] * origMat[6]))) / det);
invMat[4]=(((origMat[0] * origMat[8]) - (origMat[6] * origMat[2])) / det);
invMat[5]=(-(((origMat[0] * origMat[5]) - (origMat[3] * origMat[2]))) / det);

invMat[6]=(((origMat[3] * origMat[7]) - (origMat[6] * origMat[4])) / det);
invMat[7]=(-(((origMat[0] * origMat[7]) - (origMat[6] * origMat[1]))) / det);
invMat[8]=(((origMat[0] * origMat[4]) - (origMat[1] * origMat[3])) / det);

M3 m;
m[0]=invMat[0] * finalMat[0] + invMat[1] * finalMat[3] + invMat[2] * finalMat[6];
m[1]=invMat[0] * finalMat[1] + invMat[1] * finalMat[4] + invMat[2] * finalMat[7];
m[2]=invMat[0] * finalMat[2] + invMat[1] * finalMat[5] + invMat[2] * finalMat[8];

m[3]=invMat[3] * finalMat[0] + invMat[4] * finalMat[3] + invMat[5] * finalMat[6];
m[4]=invMat[3] * finalMat[1] + invMat[4] * finalMat[4] + invMat[5] * finalMat[7];
m[5]=invMat[3] * finalMat[2] + invMat[4] * finalMat[5] + invMat[5] * finalMat[8];

m[6]=invMat[6] * finalMat[0] + invMat[7] * finalMat[3] + invMat[8] * finalMat[6];
m[7]=invMat[6] * finalMat[1] + invMat[7] * finalMat[4] + invMat[8] * finalMat[7];
m[8]=invMat[6] * finalMat[2] + invMat[7] * finalMat[5] + invMat[8] * finalMat[8];

acutPrintf(_T("\n (%g %g %g) (%g %g %g) (%g %g %g) \n"), m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
}

[gile]

Code: [Select]

;; GaussJordan (gile)
;; Apply the Gauss-Jordan method to two matrices
.....
(repeat (vl-position 0 (vl-sort-i col '>))
  (setq mat (append (vl-remove row mat) (list row))
row (nth cnt mat)
  )
)
.....
)

Gile super nice code!  I'm trying to figure out its magic, but am unclear what this sort does (I see what it does, just don't know why).  Can you comment on that? TIA.

Thanks Paul.

As you certainly know, while treating a row in the process, the 'pivot' can't be 0 (and if all rows first items are 0 the equations can't be solved).
So, rather than just testing if the 'pivot' is 0 and permute rows until the the 'pivot' of this index row is different from 0 (as I did in the first releases of this routine), I permute rows until the one with the greater 'pivot' absolute value becomes this index row.
This is a little more time expensive but allows a better accuracy and avoid some errors if the 'pivot' isn't 0 but very closed to (thanks to Tim Willey for reporting this bug).

[gile]

I tried to write a little C# Matrix class there with including a GaussJordan method.

[pkohut]

I've posted a question to this problem in the Eigen forum, hopefully someone will reply and say "Oh ya, just do this and that, bang."

Response from the Eigen forum:  

you just have to solve a linear system of equation m1 * result = m2. For instance you can do:
result = m1.inverse() * m2;
or:
result = m1.lu().solve(m2);

For 3x3 matrices I don't know which option is the fastest.
In both cases you need to #include <Eigen/LU>

Code: [Select]

#include <eigen\Eigen>
#include <iostream>

int _tmain(int argc, _TCHAR * argv[])
{
    Eigen::Matrix<double, 3, 3> m1, m2, m3;

    m1 << 1, 3, 0, 5, 7, 0, 10, 10, 1;
    m2 << 11, 13, 0, 17, 19, 0, 20, 5, 1;

    m3 = m1.inverse() * m2;
    cout << m3 << endl << endl;

    system("PAUSE");
    return 0;
}


Code: [Select]

Results -
-3.25 -4.25 0
4.75 5.75 0
5 -10 1

Press any key to continue . . .

Now, just need to be careful when visualizing how the data is stored in memory, and remember that Eigen defaults to column major, OpenGL prefers column major, and DirectX prefers row major.

edit: Now I feel kind of foolish for not asking yesterday if this was a Ax=B problem. I assume because I'd tried result = m1.inverse() * m2; and bVal = m1.lu().solve(m2, &m3); yesterday and didn't get the correct results that it wasn't, though my research and Lee's hint suggested it was it was. And the reason for the wrong results is I'd stuffed the arrays in the wrong order.  Thanks again everyone.

[pkohut]

I tried to write a little C# Matrix class there with including a GaussJordan method.

If we could hold pointers to the rows and columns and shuffle those around instead of the actual rows and columns, then that would be a pretty good optimization.  I had been thinking about that all day for the port of your lisp code.

[LE3]

Do not know if you tried what I posted (just need the check of the determinant value ie: if (fabs(det) < 0.0005) return;) but it is really way to easy (after seeing the results, and some coffees)

Matrix rotation = Matrix final x (Matrix original) -1 = Then you get the inverse from the original and multiplying by the final one and making sure the determinant from the original is a non-zero
Mr = Mf * (Mo)-1

Did not tested with any other values... hth

edit: Now I feel kind of foolish for not asking yesterday if this was a Ax=B problem. I assume because I'd tried result = m1.inverse() * m2; and bVal = m1.lu().solve(m2, &m3); yesterday and didn't get the correct results that it wasn't, though my research and Lee's hint suggested it was it was. And the reason for the wrong results is I'd stuffed the arrays in the wrong order. 

[pkohut]

Do not know if you tried what I posted (just need the check of the determinant value ie: if (fabs(det) < 0.0005) return;) but it is really way to easy (after seeing the results, and some coffees)

Matrix rotation = Matrix final x (Matrix original) -1 = Then you get the inverse from the original and multiplying by the final one and making sure the determinant from the original is a non-zero
Mr = Mf * (Mo)-1

I'd looked at it but thought it looked way to easy, especially since I'd tried something similar and didn't get the expected results, that being the case of "garbage in - garbage out"

[LE3]

I'd looked at it but thought it looked way to easy, especially since I'd tried something similar and didn't get the expected results, that being the case of "garbage in - garbage out"

yep... and does return the same output as the ones posted here... here is again maybe with some better format, just to have a different approach

Code: [Select]

typedef float M3[9];

static BOOL mapCoordSysM3 (M3 Mo, M3 Mf, M3 &m)
{
float det;
det = Mo[0] * (Mo[4] * Mo[8] - Mo[7] * Mo[5])
- Mo[1] * (Mo[3] * Mo[8] - Mo[6] * Mo[5])
+ Mo[2] * (Mo[3] * Mo[7] - Mo[6] * Mo[4]);

if (fabs(det) < 0.0005) return FALSE;

M3 iM;
iM[0] = (((Mo[4] * Mo[8]) - (Mo[5] * Mo[7]))    / det);
iM[1] = (-(((Mo[1] * Mo[8]) - (Mo[7] * Mo[2]))) / det);
iM[2] = (((Mo[1] * Mo[5]) - (Mo[4] * Mo[2]))    / det);

iM[3] = (-(((Mo[3] * Mo[8]) - (Mo[5] * Mo[6]))) / det);
iM[4] = (((Mo[0] * Mo[8]) - (Mo[6] * Mo[2]))    / det);
iM[5] = (-(((Mo[0] * Mo[5]) - (Mo[3] * Mo[2]))) / det);

iM[6] = (((Mo[3] * Mo[7]) - (Mo[6] * Mo[4]))    / det);
iM[7] = (-(((Mo[0] * Mo[7]) - (Mo[6] * Mo[1]))) / det);
iM[8] = (((Mo[0] * Mo[4]) - (Mo[1] * Mo[3]))    / det);

m[0] = iM[0] * Mf[0] + iM[1] * Mf[3] + iM[2] * Mf[6];
m[1] = iM[0] * Mf[1] + iM[1] * Mf[4] + iM[2] * Mf[7];
m[2] = iM[0] * Mf[2] + iM[1] * Mf[5] + iM[2] * Mf[8];

m[3] = iM[3] * Mf[0] + iM[4] * Mf[3] + iM[5] * Mf[6];
m[4] = iM[3] * Mf[1] + iM[4] * Mf[4] + iM[5] * Mf[7];
m[5] = iM[3] * Mf[2] + iM[4] * Mf[5] + iM[5] * Mf[8];

m[6] = iM[6] * Mf[0] + iM[7] * Mf[3] + iM[8] * Mf[6];
m[7] = iM[6] * Mf[1] + iM[7] * Mf[4] + iM[8] * Mf[7];
m[8] = iM[6] * Mf[2] + iM[7] * Mf[5] + iM[8] * Mf[8];
return TRUE;
}

static void usageMapCoordSysM3 (void)
{
M3 Mo;
Mo[0] = 1.0;
Mo[1] = 3.0;
Mo[2] = 0.0;
Mo[3] = 5.0;
Mo[4] = 7.0;
Mo[5] = 0.0;
Mo[6] = 10.0;
Mo[7] = 10.0;
Mo[8] = 1.0;

M3 Mf;
Mf[0] = 11.0;
Mf[1] = 13.0;
Mf[2] = 0.0;
Mf[3] = 17.0;
Mf[4] = 19.0;
Mf[5] = 0.0;
Mf[6] = 20.0;
Mf[7] = 5.0;
Mf[8] = 1.0;

M3 m;
if (mapCoordSysM3(Mo, Mf, m) == TRUE)
{
acutPrintf(_T("\n (%g %g %g) (%g %g %g) (%g %g %g) \n"),
m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
}
}

static void SDSArxCmdsTester_TEST(void)
{
usageMapCoordSysM3();
}

Command: TEST
(-3.250000 -4.250000 0.000000) (4.750000 5.750000 0.000000) (5.000000 -10.000000 1.000000)
 

[pkohut]

Finally finished the drop in replacements for AcGeMatrix2d, AcGeVector2d, and validation tests. 

As mentioned before, needed something familiar for my standalone app.  Thanks everyone for your help.