Thought it might interest some comparing Giles lisp version to a C++ version. This version still needs some code refactoring done, for one it calcs column major instead of row major, hence the 2 transpose calls at the end. Also, as written, the GaussJordanElimination function requires matrices 1 x 1 bigger than the input matrices, requiring extra matrix copying. It's based on the SysMat function linked to in a earlier post.
Attached is a compile executable of the code below.
#include "stdafx.h"
#include <eigen\Eigen>
#include <iostream>
using namespace std;
template<class T, int NSize>
int GaussJordanElimination(Eigen::Matrix<T, NSize, NSize> & mm1, Eigen::Matrix<T, NSize, NSize> & mm2, T & det);
int _tmain(int argc, _TCHAR* argv[])
{
double det;
// Create 3 x 3 matrix and populate
Eigen::Matrix<double, 3, 3> m1, m2;
m1 << 1.0, 3.0, 0.0,
5.0, 7.0, 0.0,
10.0, 10.0, 1.0;
m2 << 11.0, 13.0, 0.0,
17.0, 19.0, 0.0,
20.0, 5.0, 1.0;
cout << "Calculating 3 x 3 Gauss Jordan Elimination" << endl;
cout << "Input matrix m1" << endl << m1 << endl;
cout << "Input matrix m2" << endl << m2 << endl;
if(GaussJordanElimination<double, 3>(m1, m2, det)) {
cout << "Gauss Jordan Elimination Calculated" << endl;
cout << "Output matrix: Inverse m1" << endl << m1 << endl;
cout << "Output matrix: Solution" << endl << m2 << endl;
cout << "Output: Determinant = " << det << endl;
}
else
cout << "Gauss Jordan Elimination not computed" << endl;
cout << endl;
Eigen::Matrix<double, 4, 4> m3, m4;
m3 << 1.0, 3.0, 5.0, 7.0,
9.0, 11.0, 13.0, 17.0,
19.0, 23.0, 29.0, 31.0,
32.0, 34.0, 35.0, 36.0;
m4 << 33.0, 37.0, 39.0, 41.0,
43.0, 47.0, 49.0, 51.0,
53.0, 57.0, 59.0, 71.0,
72.0, 73.0, 74.0, 75.0;
cout << "Calculating 4 x 4 Gauss Jordan Elimination" << endl;
cout << "Input matrix m3" << endl << m3 << endl;
cout << "Input matrix m4" << endl << m4 << endl;
if(GaussJordanElimination<double, 4>(m3, m4, det)) {
cout << "Gauss Jordan Elimination Calculated" << endl;
cout << "Output matrix: Inverse m3" << endl << m3 << endl;
cout << "Output matrix: Solution" << endl << m4 << endl;
cout << "Output: Determinant = " << det << endl;
}
else
cout << "Gauss Jordan Elimination not computed" << endl;
cout << endl;
system("PAUSE");
return 0;
}
template<int NSize>
inline void SetNaN(Eigen::Matrix<double, NSize, NSize> & m)
{
m.fill(_Nan._Double);
}
template<int NSize>
inline void SetNaN(Eigen::Matrix<float, NSize, NSize> & m)
{
m.fill(_Nan._Float);
}
template<class T, int NSize>
int GaussJordanElimination(Eigen::Matrix<T, NSize, NSize> & mm1, Eigen::Matrix<T, NSize, NSize> & mm2, T & det)
{
enum { nCols = NSize, mCols = NSize, };
Eigen::Matrix<T, NSize + 1, 1> pc, pl, cs;
pc.fill(0.0); pl.fill(0.0); cs.fill(0.0);
// Create matrix +1 bigger than input matrix
Eigen::Matrix<T, NSize + 1, NSize + 1> m1, m2;
// Copy mm1 matrix to m1
m1.block<NSize, NSize>(0, 0) = mm1;
// fill last row and column with 0.0
m1.row(NSize).fill(0.0);
m1.col(NSize).fill(0.0);
m2.block<NSize, NSize>(0, 0) = mm2;
m2.row(NSize).fill(0.0);
m2.col(NSize).fill(0.0);
det = 1.0;
for(int k = 0; k < nCols; ++k) {
T pv = m1(k, k);
int ik = k;
int jk = k;
T pav = fabs(pv);
for(int j, i = k; i < nCols; ++i) {
for(j = k; j < nCols; ++j) {
if(fabs(m1(i, j)) > pav) {
pv = m1(i, j);
pav = fabs(pv);
ik = i;
jk = j;
}
}
// Search terminated, the pivot is in location i=ik, j=jk
// Memorizing pivot location;
pc(k) = (T) jk;
pl(k) = (T) ik;
// Determinant det is actualised
// If det = 0, error message and stop
// Machine dependant EPSMACH equals here is 1e-20
if(ik != k) det = -det;
if(jk != k) det = -det;
det = det * pv;
if(fabs(det) < 1e-20) {
SetNaN<nCols>(mm1);
SetNaN<nCols>(mm2);
return 0;
}
// Positioning pivot in k,k
if(ik != k)
for(int i = 0; i < nCols; ++i) {
// exchange lines ik and k of matrix m1
T dTemp = m1(ik, i);
m1(ik, i) = m1(k,i);
m1(k,i) = dTemp;
}
if(mCols) {
for(i = 0; i < mCols; ++i) {
T dTemp = m2(ik, i);
m2(ik, i) = m2(k, i);
m2(k, i) = dTemp;
}
// Pivot is a correct line
if(jk != k)
for(i = 0; i < nCols; ++i) {
// exchange columns jk and k of matrix m1
T dTemp = m1(i, jk);
m1(i, jk) = m1(i, k);
m1(i, k) = dTemp;
}
// Pivot is at correct column and is located in k,k
// Column k of matrix m1 is stored in cs vector
// then column k is set to zero.
for(i = 0; i < nCols; ++i) {
cs(i) = m1(i, k);
m1(i, k) = 0.0;
}
cs(k) = 0.0;
m1(k,k) = 1.0;
// Line k of matrix m1 is modified
if(fabs(pv) < 1e-20) {
SetNaN<nCols>(mm1);
SetNaN<nCols>(mm2);
return 0;
}
for(i = 0; i < nCols; ++i)
m1(k,i) = m1(k,i) / pv;
if(mCols)
for(i = 0; i < mCols; ++i)
m2(k, i) = m2(k, i) / pv;
// Other lines of matrix m1 are modified
for(j = 0; j < nCols; ++j) {
if(j == k) j++;
for(i = 0; i < nCols; ++i)
// Line j of matrix m1 is modified
m1(j,i)= m1(j,i) - cs(j) * m1(k,i);
if(mCols)
for(i = 0; i < mCols; ++i)
m2(j, i) = m2(j, i) - cs(j) * m2(k,i);
}
// Line k is ready
} // mCols
}
} // end of k loop
// exchange lines
for(int i = nCols - 1; i >= 0; --i) {
int ik = (int) pc(i);
if(ik == i)
continue;
// exchange lines i and pc(i) of matrix m1
for(int j = 0; j < nCols; ++j) {
T dTemp = m1(i,j);
m1(i,j) = m1(ik,j);
m1(ik,j) = dTemp;
}
if(mCols) {
for(int j = 0; j < mCols; ++j) {
T dTemp = m2(i,j);
m2(i,j) = m2(ik,j);
m2(ik,j) = dTemp;
}
}
// no more exchange is necessary
// goto next line
} // for i
// exchange columns
for(int j = nCols - 1; j >= 0; --j) {
int jk = (int) pl(j);
if(jk == j)
continue;
// exchange columns j et pl(j) of matrix m1
for(int i = 0; i < nCols; ++i) {
T dTemp = m1(i,j);
m1(i,j) = m1(i, jk);
m1(i, jk) = dTemp;
}
// No more exchange is necessary
// go to next column
}
// rearrangement terminated
// for(int i = 0; i < )
mm1 = m1.block<NSize,NSize>(0,0).transpose();
mm2 = m2.block<NSize,NSize>(0,0).transpose();
return 1;
}