Index: orxonox/trunk/src/util/newmat/Makefile
===================================================================
--- orxonox/trunk/src/util/newmat/Makefile	(revision 4565)
+++ orxonox/trunk/src/util/newmat/Makefile	(revision 4565)
@@ -0,0 +1,184 @@
+CXX = g++
+CXXFLAGS = -O2 -Wall
+
+DIFF = ./sdiff
+PRE = ./
+MAJOR = 1
+MINOR = 0
+
+%.o:           	%.cpp
+		$(CXX) $(CXXFLAGS) -c $*.cpp
+
+everything:    	tmt example test_exc nl_ex sl_ex garch eigen 
+
+newmat_lobj = newmat1.o newmat2.o newmat3.o newmat4.o newmat5.o newmat6.o newmat7.o newmat8.o newmatex.o bandmat.o submat.o myexcept.o cholesky.o evalue.o fft.o hholder.o jacobi.o newfft.o sort.o svd.o newmatrm.o newmat9.o
+
+libnewmat.a:   	$(newmat_lobj)
+		$(AR) -cr $@ $(newmat_lobj)
+		ranlib $@
+
+tmt_obj = tmt.o tmt1.o tmt2.o tmt3.o tmt4.o tmt5.o tmt6.o tmt7.o tmt8.o tmt9.o tmta.o tmtb.o tmtc.o tmtd.o tmte.o tmtf.o tmtg.o tmth.o tmti.o tmtj.o tmtk.o tmtl.o tmtm.o
+
+tmt:           	$(tmt_obj) libnewmat.a
+		$(CXX) -o $@ $(tmt_obj) -L. -lnewmat -lm
+
+example_obj = example.o
+
+example:       	$(example_obj) libnewmat.a
+		$(CXX) -o $@ $(example_obj) -L. -lnewmat -lm
+
+test_exc_obj = test_exc.o
+
+test_exc:      	$(test_exc_obj) libnewmat.a
+		$(CXX) -o $@ $(test_exc_obj) -L. -lnewmat -lm
+
+nl_ex_obj = nl_ex.o newmatnl.o
+
+nl_ex:         	$(nl_ex_obj) libnewmat.a
+		$(CXX) -o $@ $(nl_ex_obj) -L. -lnewmat -lm
+
+sl_ex_obj = sl_ex.o solution.o myexcept.o
+
+sl_ex:         	$(sl_ex_obj)
+		$(CXX) -o $@ $(sl_ex_obj) -L. -lm
+
+garch_obj = garch.o newmatnl.o
+
+garch:         	$(garch_obj) libnewmat.a
+		$(CXX) -o $@ $(garch_obj) -L. -lnewmat -lm
+
+#Eigen: PB
+eigen_obj = eigen.o newmatnl.o
+eigen:         	$(eigen_obj) libnewmat.a
+		$(CXX) -o $@ $(eigen_obj) -L. -lnewmat -lm
+
+newmat1.o:     	newmat1.cpp newmat.h include.h boolean.h myexcept.h
+
+newmat2.o:     	newmat2.cpp include.h newmat.h newmatrc.h boolean.h myexcept.h controlw.h
+
+newmat3.o:     	newmat3.cpp include.h newmat.h newmatrc.h boolean.h myexcept.h controlw.h
+
+newmat4.o:     	newmat4.cpp include.h newmat.h newmatrc.h boolean.h myexcept.h controlw.h
+
+newmat5.o:     	newmat5.cpp include.h newmat.h newmatrc.h boolean.h myexcept.h controlw.h
+
+newmat6.o:     	newmat6.cpp include.h newmat.h newmatrc.h boolean.h myexcept.h controlw.h
+
+newmat7.o:     	newmat7.cpp include.h newmat.h newmatrc.h boolean.h myexcept.h controlw.h
+
+newmat8.o:     	newmat8.cpp include.h newmat.h newmatrc.h precisio.h boolean.h myexcept.h controlw.h
+
+newmatex.o:    	newmatex.cpp include.h newmat.h boolean.h myexcept.h
+
+bandmat.o:     	bandmat.cpp include.h newmat.h newmatrc.h boolean.h myexcept.h controlw.h
+
+submat.o:      	submat.cpp include.h newmat.h newmatrc.h boolean.h myexcept.h controlw.h
+
+myexcept.o:    	myexcept.cpp include.h boolean.h myexcept.h
+
+cholesky.o:    	cholesky.cpp include.h newmat.h boolean.h myexcept.h
+
+evalue.o:      	evalue.cpp include.h newmatap.h newmatrm.h precisio.h newmat.h boolean.h myexcept.h
+
+fft.o:         	fft.cpp include.h newmatap.h newmat.h boolean.h myexcept.h
+
+hholder.o:     	hholder.cpp include.h newmatap.h newmat.h boolean.h myexcept.h
+
+jacobi.o:      	jacobi.cpp include.h newmatap.h precisio.h newmatrm.h newmat.h boolean.h myexcept.h
+
+newfft.o:      	newfft.cpp newmatap.h newmat.h include.h boolean.h myexcept.h
+
+sort.o:        	sort.cpp include.h newmatap.h newmat.h boolean.h myexcept.h
+
+svd.o:         	svd.cpp include.h newmatap.h newmatrm.h precisio.h newmat.h boolean.h myexcept.h
+
+newmatrm.o:    	newmatrm.cpp newmat.h newmatrm.h include.h boolean.h myexcept.h
+
+newmat9.o:     	newmat9.cpp include.h newmat.h newmatio.h newmatrc.h boolean.h myexcept.h controlw.h
+
+tmt.o:         	tmt.cpp include.h newmat.h tmt.h boolean.h myexcept.h
+
+tmt1.o:        	tmt1.cpp include.h newmat.h tmt.h boolean.h myexcept.h
+
+tmt2.o:        	tmt2.cpp include.h newmat.h tmt.h boolean.h myexcept.h
+
+tmt3.o:        	tmt3.cpp include.h newmat.h tmt.h boolean.h myexcept.h
+
+tmt4.o:        	tmt4.cpp include.h newmat.h tmt.h boolean.h myexcept.h
+
+tmt5.o:        	tmt5.cpp include.h newmat.h tmt.h boolean.h myexcept.h
+
+tmt6.o:        	tmt6.cpp include.h newmatap.h tmt.h newmat.h boolean.h myexcept.h
+
+tmt7.o:        	tmt7.cpp include.h newmat.h tmt.h boolean.h myexcept.h
+
+tmt8.o:        	tmt8.cpp include.h newmatap.h tmt.h newmat.h boolean.h myexcept.h
+
+tmt9.o:        	tmt9.cpp include.h newmatap.h tmt.h newmat.h boolean.h myexcept.h
+
+tmta.o:        	tmta.cpp include.h newmatap.h tmt.h newmat.h boolean.h myexcept.h
+
+tmtb.o:        	tmtb.cpp include.h newmat.h tmt.h boolean.h myexcept.h
+
+tmtc.o:        	tmtc.cpp include.h newmat.h tmt.h boolean.h myexcept.h
+
+tmtd.o:        	tmtd.cpp include.h newmatap.h tmt.h newmat.h boolean.h myexcept.h
+
+tmte.o:        	tmte.cpp include.h newmatap.h tmt.h newmat.h boolean.h myexcept.h
+
+tmtf.o:        	tmtf.cpp include.h newmatap.h tmt.h newmat.h boolean.h myexcept.h
+
+tmtg.o:        	tmtg.cpp include.h newmatap.h tmt.h newmat.h boolean.h myexcept.h
+
+tmth.o:        	tmth.cpp include.h newmatap.h tmt.h newmat.h boolean.h myexcept.h
+
+tmti.o:        	tmti.cpp include.h newmatap.h tmt.h newmat.h boolean.h myexcept.h
+
+tmtj.o:        	tmtj.cpp include.h newmatap.h tmt.h newmat.h boolean.h myexcept.h
+
+tmtk.o:        	tmtk.cpp include.h newmatap.h newmatio.h tmt.h newmat.h boolean.h myexcept.h
+
+tmtl.o:        	tmtl.cpp newmat.h tmt.h include.h boolean.h myexcept.h
+
+tmtm.o:        	tmtm.cpp newmat.h tmt.h include.h boolean.h myexcept.h
+
+example.o:     	example.cpp newmatap.h newmatio.h newmat.h include.h boolean.h myexcept.h
+
+test_exc.o:    	test_exc.cpp newmatap.h newmatio.h newmat.h include.h boolean.h myexcept.h
+
+nl_ex.o:       	nl_ex.cpp newmatnl.h newmatio.h newmat.h include.h boolean.h myexcept.h
+
+newmatnl.o:    	newmatnl.cpp newmatap.h newmatnl.h newmat.h include.h boolean.h myexcept.h
+
+sl_ex.o:       	sl_ex.cpp include.h solution.h boolean.h myexcept.h
+
+solution.o:    	solution.cpp include.h boolean.h myexcept.h solution.h
+
+garch.o:       	garch.cpp newmatap.h newmatio.h newmatnl.h newmat.h include.h boolean.h myexcept.h
+
+eigen.o:	eigen.cpp include.h newmat.h newmatap.h newmatio.h
+
+tmt.txx:       	tmt
+		$(PRE)tmt > tmt.txx
+		$(DIFF) tmt.txt tmt.txx
+
+example.txx:   	example
+		$(PRE)example > example.txx
+		$(DIFF) example.txt example.txx
+
+test_exc.txx:  	test_exc
+		$(PRE)test_exc > test_exc.txx
+		$(DIFF) test_exc.txt test_exc.txx
+
+nl_ex.txx:     	nl_ex
+		$(PRE)nl_ex > nl_ex.txx
+		$(DIFF) nl_ex.txt nl_ex.txx
+
+sl_ex.txx:     	sl_ex
+		$(PRE)sl_ex > sl_ex.txx
+		$(DIFF) sl_ex.txt sl_ex.txx
+
+garch.txx:     	garch
+		$(PRE)garch > garch.txx
+		$(DIFF) garch.txt garch.txx
+
Index: orxonox/trunk/src/util/newmat/bandmat.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/bandmat.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/bandmat.cpp	(revision 4565)
@@ -0,0 +1,569 @@
+//$$ bandmat.cpp                     Band matrix definitions
+
+// Copyright (C) 1991,2,3,4,9: R B Davies
+
+#define WANT_MATH                    // include.h will get math fns
+
+//#define WANT_STREAM
+
+#include "include.h"
+
+#include "newmat.h"
+#include "newmatrc.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+
+
+#ifdef DO_REPORT
+#define REPORT { static ExeCounter ExeCount(__LINE__,10); ++ExeCount; }
+#else
+#define REPORT {}
+#endif
+
+static inline int my_min(int x, int y) { return x < y ? x : y; }
+static inline int my_max(int x, int y) { return x > y ? x : y; }
+
+
+BandMatrix::BandMatrix(const BaseMatrix& M)
+{
+   REPORT // CheckConversion(M);
+   // MatrixConversionCheck mcc;
+   GeneralMatrix* gmx=((BaseMatrix&)M).Evaluate(MatrixType::BM);
+   GetMatrix(gmx); CornerClear();
+}
+
+void BandMatrix::SetParameters(const GeneralMatrix* gmx)
+{
+   REPORT
+   MatrixBandWidth bw = gmx->BandWidth();
+   lower = bw.lower; upper = bw.upper;
+}
+
+void BandMatrix::ReSize(int n, int lb, int ub)
+{
+   REPORT
+   Tracer tr("BandMatrix::ReSize");
+   if (lb<0 || ub<0) Throw(ProgramException("Undefined bandwidth"));
+   lower = (lb<=n) ? lb : n-1; upper = (ub<=n) ? ub : n-1;
+   GeneralMatrix::ReSize(n,n,n*(lower+1+upper)); CornerClear();
+}
+
+// SimpleAddOK shows when we can add etc two matrices by a simple vector add
+// and when we can add one matrix into another
+// *gm must be the same type as *this
+// return 0 if simple add is OK
+// return 1 if we can add into *gm only
+// return 2 if we can add into *this only
+// return 3 if we can't add either way
+// For SP this will still be valid if we swap 1 and 2
+
+short BandMatrix::SimpleAddOK(const GeneralMatrix* gm)
+{
+   const BandMatrix* bm = (const BandMatrix*)gm;
+   if (bm->lower == lower && bm->upper == upper) { REPORT return 0; }
+   else if (bm->lower >= lower && bm->upper >= upper) { REPORT return 1; }
+   else if (bm->lower <= lower && bm->upper <= upper) { REPORT return 2; }
+   else { REPORT return 3; }
+}
+
+short SymmetricBandMatrix::SimpleAddOK(const GeneralMatrix* gm)
+{
+   const SymmetricBandMatrix* bm = (const SymmetricBandMatrix*)gm;
+   if (bm->lower == lower) { REPORT return 0; }
+   else if (bm->lower > lower) { REPORT return 1; }
+   else { REPORT return 2; }
+}
+
+void UpperBandMatrix::ReSize(int n, int lb, int ub)
+{
+   REPORT
+   if (lb != 0)
+   {
+      Tracer tr("UpperBandMatrix::ReSize");
+      Throw(ProgramException("UpperBandMatrix with non-zero lower band" ));
+   }
+   BandMatrix::ReSize(n, lb, ub);
+}
+
+void LowerBandMatrix::ReSize(int n, int lb, int ub)
+{
+   REPORT
+   if (ub != 0)
+   {
+      Tracer tr("LowerBandMatrix::ReSize");
+      Throw(ProgramException("LowerBandMatrix with non-zero upper band" ));
+   }
+   BandMatrix::ReSize(n, lb, ub);
+}
+
+void BandMatrix::ReSize(const GeneralMatrix& A)
+{
+   REPORT
+   int n = A.Nrows();
+   if (n != A.Ncols())
+   {
+      Tracer tr("BandMatrix::ReSize(GM)");
+      Throw(NotSquareException(*this));
+   }
+   MatrixBandWidth mbw = A.BandWidth();
+   ReSize(n, mbw.Lower(), mbw.Upper());
+}
+
+bool BandMatrix::SameStorageType(const GeneralMatrix& A) const
+{
+   if (Type() != A.Type()) { REPORT return false; }
+   REPORT
+   return BandWidth() == A.BandWidth();
+}
+
+void BandMatrix::ReSizeForAdd(const GeneralMatrix& A, const GeneralMatrix& B)
+{
+   REPORT
+   Tracer tr("BandMatrix::ReSizeForAdd");
+   MatrixBandWidth A_BW = A.BandWidth(); MatrixBandWidth B_BW = B.BandWidth();
+   if ((A_BW.Lower() < 0) | (A_BW.Upper() < 0) | (B_BW.Lower() < 0)
+      | (A_BW.Upper() < 0))
+         Throw(ProgramException("Can't ReSize to BandMatrix" ));
+   // already know A and B are square
+   ReSize(A.Nrows(), my_max(A_BW.Lower(), B_BW.Lower()),
+      my_max(A_BW.Upper(), B_BW.Upper()));
+}
+
+void BandMatrix::ReSizeForSP(const GeneralMatrix& A, const GeneralMatrix& B)
+{
+   REPORT
+   Tracer tr("BandMatrix::ReSizeForSP");
+   MatrixBandWidth A_BW = A.BandWidth(); MatrixBandWidth B_BW = B.BandWidth();
+   if ((A_BW.Lower() < 0) | (A_BW.Upper() < 0) | (B_BW.Lower() < 0)
+      | (A_BW.Upper() < 0))
+         Throw(ProgramException("Can't ReSize to BandMatrix" ));
+   // already know A and B are square
+   ReSize(A.Nrows(), my_min(A_BW.Lower(), B_BW.Lower()),
+      my_min(A_BW.Upper(), B_BW.Upper()));
+}
+
+
+void BandMatrix::operator=(const BaseMatrix& X)
+{
+   REPORT // CheckConversion(X);
+   // MatrixConversionCheck mcc;
+   Eq(X,MatrixType::BM); CornerClear();
+}
+
+void BandMatrix::CornerClear() const
+{
+   // set unused parts of BandMatrix to zero
+   REPORT
+   int i = lower; Real* s = store; int bw = lower + 1 + upper;
+   while (i)
+      { int j = i--; Real* sj = s; s += bw; while (j--) *sj++ = 0.0; }
+   i = upper; s = store + storage;
+   while (i)
+      { int j = i--; Real* sj = s; s -= bw; while (j--) *(--sj) = 0.0; }
+}
+
+MatrixBandWidth MatrixBandWidth::operator+(const MatrixBandWidth& bw) const
+{
+   REPORT
+   int l = bw.lower; int u = bw.upper;
+   l = (lower < 0 || l < 0) ? -1 : (lower > l) ? lower : l;
+   u = (upper < 0 || u < 0) ? -1 : (upper > u) ? upper : u;
+   return MatrixBandWidth(l,u);
+}
+
+MatrixBandWidth MatrixBandWidth::operator*(const MatrixBandWidth& bw) const
+{
+   REPORT
+   int l = bw.lower; int u = bw.upper;
+   l = (lower < 0 || l < 0) ? -1 : lower+l;
+   u = (upper < 0 || u < 0) ? -1 : upper+u;
+   return MatrixBandWidth(l,u);
+}
+
+MatrixBandWidth MatrixBandWidth::minimum(const MatrixBandWidth& bw) const
+{
+   REPORT
+   int l = bw.lower; int u = bw.upper;
+   if ((lower >= 0) && ( (l < 0) || (l > lower) )) l = lower;
+   if ((upper >= 0) && ( (u < 0) || (u > upper) )) u = upper;
+   return MatrixBandWidth(l,u);
+}
+
+UpperBandMatrix::UpperBandMatrix(const BaseMatrix& M)
+{
+   REPORT // CheckConversion(M);
+   // MatrixConversionCheck mcc;
+   GeneralMatrix* gmx=((BaseMatrix&)M).Evaluate(MatrixType::UB);
+   GetMatrix(gmx); CornerClear();
+}
+
+void UpperBandMatrix::operator=(const BaseMatrix& X)
+{
+   REPORT // CheckConversion(X);
+   // MatrixConversionCheck mcc;
+   Eq(X,MatrixType::UB); CornerClear();
+}
+
+LowerBandMatrix::LowerBandMatrix(const BaseMatrix& M)
+{
+   REPORT // CheckConversion(M);
+   // MatrixConversionCheck mcc;
+   GeneralMatrix* gmx=((BaseMatrix&)M).Evaluate(MatrixType::LB);
+   GetMatrix(gmx); CornerClear();
+}
+
+void LowerBandMatrix::operator=(const BaseMatrix& X)
+{
+   REPORT // CheckConversion(X);
+   // MatrixConversionCheck mcc;
+   Eq(X,MatrixType::LB); CornerClear();
+}
+
+BandLUMatrix::BandLUMatrix(const BaseMatrix& m)
+{
+   REPORT
+   Tracer tr("BandLUMatrix");
+   storage2 = 0; store2 = 0;  // in event of exception during build
+   GeneralMatrix* gm = ((BaseMatrix&)m).Evaluate(MatrixType::BM);
+   m1 = ((BandMatrix*)gm)->lower; m2 = ((BandMatrix*)gm)->upper;
+   GetMatrix(gm);
+   if (nrows!=ncols) Throw(NotSquareException(*this));
+   d = true; sing = false;
+   indx = new int [nrows]; MatrixErrorNoSpace(indx);
+   MONITOR_INT_NEW("Index (BndLUMat)",nrows,indx)
+   storage2 = nrows * m1;
+   store2 = new Real [storage2]; MatrixErrorNoSpace(store2);
+   MONITOR_REAL_NEW("Make (BandLUMat)",storage2,store2)
+   ludcmp();
+}
+
+BandLUMatrix::~BandLUMatrix()
+{
+   REPORT
+   MONITOR_INT_DELETE("Index (BndLUMat)",nrows,indx)
+   MONITOR_REAL_DELETE("Delete (BndLUMt)",storage2,store2)
+   delete [] indx; delete [] store2;
+}
+
+MatrixType BandLUMatrix::Type() const { REPORT return MatrixType::BC; }
+
+
+LogAndSign BandLUMatrix::LogDeterminant() const
+{
+   REPORT
+   if (sing) return 0.0;
+   Real* a = store; int w = m1+1+m2; LogAndSign sum; int i = nrows;
+   // while (i--) { sum *= *a; a += w; }
+   if (i) for (;;) { sum *= *a; if (!(--i)) break; a += w; }
+   if (!d) sum.ChangeSign(); return sum;
+}
+
+GeneralMatrix* BandMatrix::MakeSolver()
+{
+   REPORT
+   GeneralMatrix* gm = new BandLUMatrix(*this);
+   MatrixErrorNoSpace(gm); gm->ReleaseAndDelete(); return gm;
+}
+
+
+void BandLUMatrix::ludcmp()
+{
+   REPORT
+   Real* a = store2; int i = storage2;
+   // clear store2 - so unused locations are always zero -
+   // required by operator==
+   while (i--) *a++ = 0.0;
+   a = store;
+   i = m1; int j = m2; int k; int n = nrows; int w = m1 + 1 + m2;
+   while (i)
+   {
+      Real* ai = a + i;
+      k = ++j; while (k--) *a++ = *ai++;
+      k = i--; while (k--) *a++ = 0.0;
+   }
+
+   a = store; int l = m1;
+   for (k=0; k<n; k++)
+   {
+      Real x = *a; i = k; Real* aj = a;
+      if (l < n) l++;
+      for (j=k+1; j<l; j++)
+         { aj += w; if (fabs(x) < fabs(*aj)) { x = *aj; i = j; } }
+      indx[k] = i;
+      if (x==0) { sing = true; return; }
+      if (i!=k)
+      {
+         d = !d; Real* ak = a; Real* ai = store + i * w; j = w;
+         while (j--) { x = *ak; *ak++ = *ai; *ai++ = x; }
+      }
+      aj = a + w; Real* m = store2 + m1 * k;
+      for (j=k+1; j<l; j++)
+      {
+         *m++ = x = *aj / *a; i = w; Real* ak = a;
+	 while (--i) { Real* aj1 = aj++; *aj1 = *aj - x * *(++ak); }
+         *aj++ = 0.0;
+      }
+      a += w;
+   }
+}
+
+void BandLUMatrix::lubksb(Real* B, int mini)
+{
+   REPORT
+   Tracer tr("BandLUMatrix::lubksb");
+   if (sing) Throw(SingularException(*this));
+   int n = nrows; int l = m1; int w = m1 + 1 + m2;
+
+   for (int k=0; k<n; k++)
+   {
+      int i = indx[k];
+      if (i!=k) { Real x=B[k]; B[k]=B[i]; B[i]=x; }
+      if (l<n) l++;
+      Real* m = store2 + k*m1; Real* b = B+k; Real* bi = b;
+      for (i=k+1; i<l; i++)  *(++bi) -= *m++ * *b;
+   }
+
+   l = -m1;
+   for (int i = n-1; i>=mini; i--)
+   {
+      Real* b = B + i; Real* bk = b; Real x = *bk;
+      Real* a = store + w*i; Real y = *a;
+      int k = l+m1; while (k--) x -=  *(++a) * *(++bk);
+      *b = x / y;
+      if (l < m2) l++;
+   }
+}
+
+void BandLUMatrix::Solver(MatrixColX& mcout, const MatrixColX& mcin)
+{
+   REPORT
+   int i = mcin.skip; Real* el = mcin.data-i; Real* el1=el;
+   while (i--) *el++ = 0.0;
+   el += mcin.storage; i = nrows - mcin.skip - mcin.storage;
+   while (i--) *el++ = 0.0;
+   lubksb(el1, mcout.skip);
+}
+
+// Do we need check for entirely zero output?
+
+
+void UpperBandMatrix::Solver(MatrixColX& mcout,
+   const MatrixColX& mcin)
+{
+   REPORT
+   int i = mcin.skip-mcout.skip; Real* elx = mcin.data-i;
+   while (i-- > 0) *elx++ = 0.0;
+   int nr = mcin.skip+mcin.storage;
+   elx = mcin.data+mcin.storage; Real* el = elx;
+   int j = mcout.skip+mcout.storage-nr; i = nr-mcout.skip;
+   while (j-- > 0) *elx++ = 0.0;
+
+   Real* Ael = store + (upper+1)*(i-1)+1; j = 0;
+   if (i > 0) for(;;)
+   {
+      elx = el; Real sum = 0.0; int jx = j;
+      while (jx--) sum += *(--Ael) * *(--elx);
+      elx--; *elx = (*elx - sum) / *(--Ael);
+      if (--i <= 0) break;
+      if (j<upper) Ael -= upper - (++j); else el--;
+   }
+}
+
+void LowerBandMatrix::Solver(MatrixColX& mcout,
+   const MatrixColX& mcin)
+{
+   REPORT
+   int i = mcin.skip-mcout.skip; Real* elx = mcin.data-i;
+   while (i-- > 0) *elx++ = 0.0;
+   int nc = mcin.skip; i = nc+mcin.storage; elx = mcin.data+mcin.storage;
+   int nr = mcout.skip+mcout.storage; int j = nr-i; i = nr-nc;
+   while (j-- > 0) *elx++ = 0.0;
+
+   Real* el = mcin.data; Real* Ael = store + (lower+1)*nc + lower; j = 0;
+   if (i > 0) for(;;)
+   {
+      elx = el; Real sum = 0.0; int jx = j;
+      while (jx--) sum += *Ael++ * *elx++;
+      *elx = (*elx - sum) / *Ael++;
+      if (--i <= 0) break;
+      if (j<lower) Ael += lower - (++j); else el++;
+   }
+}
+
+
+LogAndSign BandMatrix::LogDeterminant() const
+{
+   REPORT
+   BandLUMatrix C(*this); return C.LogDeterminant();
+}
+
+LogAndSign LowerBandMatrix::LogDeterminant() const
+{
+   REPORT
+   int i = nrows; LogAndSign sum; Real* s = store + lower; int j = lower + 1;
+//   while (i--) { sum *= *s; s += j; }
+   if (i) for (;;) { sum *= *s; if (!(--i)) break; s += j; }
+   ((GeneralMatrix&)*this).tDelete(); return sum;
+}
+
+LogAndSign UpperBandMatrix::LogDeterminant() const
+{
+   REPORT
+   int i = nrows; LogAndSign sum; Real* s = store; int j = upper + 1;
+//   while (i--) { sum *= *s; s += j; }
+   if (i) for (;;) { sum *= *s; if (!(--i)) break; s += j; }
+   ((GeneralMatrix&)*this).tDelete(); return sum;
+}
+
+GeneralMatrix* SymmetricBandMatrix::MakeSolver()
+{
+   REPORT
+   GeneralMatrix* gm = new BandLUMatrix(*this);
+   MatrixErrorNoSpace(gm); gm->ReleaseAndDelete(); return gm;
+}
+
+SymmetricBandMatrix::SymmetricBandMatrix(const BaseMatrix& M)
+{
+   REPORT  // CheckConversion(M);
+   // MatrixConversionCheck mcc;
+   GeneralMatrix* gmx=((BaseMatrix&)M).Evaluate(MatrixType::SB);
+   GetMatrix(gmx);
+}
+
+GeneralMatrix* SymmetricBandMatrix::Transpose(TransposedMatrix*, MatrixType mt)
+{ REPORT  return Evaluate(mt); }
+
+LogAndSign SymmetricBandMatrix::LogDeterminant() const
+{
+   REPORT
+   BandLUMatrix C(*this); return C.LogDeterminant();
+}
+
+void SymmetricBandMatrix::SetParameters(const GeneralMatrix* gmx)
+{ REPORT lower = gmx->BandWidth().lower; }
+
+void SymmetricBandMatrix::ReSize(int n, int lb)
+{
+   REPORT
+   Tracer tr("SymmetricBandMatrix::ReSize");
+   if (lb<0) Throw(ProgramException("Undefined bandwidth"));
+   lower = (lb<=n) ? lb : n-1;
+   GeneralMatrix::ReSize(n,n,n*(lower+1));
+}
+
+void SymmetricBandMatrix::ReSize(const GeneralMatrix& A)
+{
+   REPORT
+   int n = A.Nrows();
+   if (n != A.Ncols())
+   {
+      Tracer tr("SymmetricBandMatrix::ReSize(GM)");
+      Throw(NotSquareException(*this));
+   }
+   MatrixBandWidth mbw = A.BandWidth(); int b = mbw.Lower();
+   if (b != mbw.Upper())
+   {
+      Tracer tr("SymmetricBandMatrix::ReSize(GM)");
+      Throw(ProgramException("Upper and lower band-widths not equal"));
+   }
+   ReSize(n, b);
+}
+
+bool SymmetricBandMatrix::SameStorageType(const GeneralMatrix& A) const
+{
+   if (Type() != A.Type()) { REPORT return false; }
+   REPORT
+   return BandWidth() == A.BandWidth();
+}
+
+void SymmetricBandMatrix::ReSizeForAdd(const GeneralMatrix& A,
+   const GeneralMatrix& B)
+{
+   REPORT
+   Tracer tr("SymmetricBandMatrix::ReSizeForAdd");
+   MatrixBandWidth A_BW = A.BandWidth(); MatrixBandWidth B_BW = B.BandWidth();
+   if ((A_BW.Lower() < 0) | (B_BW.Lower() < 0))
+         Throw(ProgramException("Can't ReSize to SymmetricBandMatrix" ));
+   // already know A and B are square
+   ReSize(A.Nrows(), my_max(A_BW.Lower(), B_BW.Lower()));
+}
+
+void SymmetricBandMatrix::ReSizeForSP(const GeneralMatrix& A,
+   const GeneralMatrix& B)
+{
+   REPORT
+   Tracer tr("SymmetricBandMatrix::ReSizeForSP");
+   MatrixBandWidth A_BW = A.BandWidth(); MatrixBandWidth B_BW = B.BandWidth();
+   if ((A_BW.Lower() < 0) | (B_BW.Lower() < 0))
+         Throw(ProgramException("Can't ReSize to SymmetricBandMatrix" ));
+   // already know A and B are square
+   ReSize(A.Nrows(), my_min(A_BW.Lower(), B_BW.Lower()));
+}
+
+
+void SymmetricBandMatrix::operator=(const BaseMatrix& X)
+{
+   REPORT // CheckConversion(X);
+   // MatrixConversionCheck mcc;
+   Eq(X,MatrixType::SB);
+}
+
+void SymmetricBandMatrix::CornerClear() const
+{
+   // set unused parts of BandMatrix to zero
+   REPORT
+   int i = lower; Real* s = store; int bw = lower + 1;
+   if (i) for(;;)
+   {
+      int j = i;
+      Real* sj = s;
+      while (j--) *sj++ = 0.0;
+      if (!(--i)) break;
+      s += bw;
+   }
+}
+
+MatrixBandWidth SymmetricBandMatrix::BandWidth() const
+   { REPORT return MatrixBandWidth(lower,lower); }
+
+inline Real square(Real x) { return x*x; }
+
+
+Real SymmetricBandMatrix::SumSquare() const
+{
+   REPORT
+   CornerClear();
+   Real sum1=0.0; Real sum2=0.0; Real* s=store; int i=nrows; int l=lower;
+   while (i--)
+      { int j = l; while (j--) sum2 += square(*s++); sum1 += square(*s++); }
+   ((GeneralMatrix&)*this).tDelete(); return sum1 + 2.0 * sum2;
+}
+
+Real SymmetricBandMatrix::SumAbsoluteValue() const
+{
+   REPORT
+   CornerClear();
+   Real sum1=0.0; Real sum2=0.0; Real* s=store; int i=nrows; int l=lower;
+   while (i--)
+      { int j = l; while (j--) sum2 += fabs(*s++); sum1 += fabs(*s++); }
+   ((GeneralMatrix&)*this).tDelete(); return sum1 + 2.0 * sum2;
+}
+
+Real SymmetricBandMatrix::Sum() const
+{
+   REPORT
+   CornerClear();
+   Real sum1=0.0; Real sum2=0.0; Real* s=store; int i=nrows; int l=lower;
+   while (i--)
+      { int j = l; while (j--) sum2 += *s++; sum1 += *s++; }
+   ((GeneralMatrix&)*this).tDelete(); return sum1 + 2.0 * sum2;
+}
+
+
+#ifdef use_namespace
+}
+#endif
+
Index: orxonox/trunk/src/util/newmat/boolean.h
===================================================================
--- orxonox/trunk/src/util/newmat/boolean.h	(revision 4565)
+++ orxonox/trunk/src/util/newmat/boolean.h	(revision 4565)
@@ -0,0 +1,40 @@
+//$$ boolean.h                       bool class
+
+// This is for compilers that do not have bool automatically defined
+
+#ifndef bool_LIB
+#define bool_LIB 0
+
+#ifdef use_namespace
+namespace RBD_COMMON {
+#endif
+
+
+class bool
+{
+	int value;
+public:
+	bool(const int b) { value = b ? 1 : 0; }
+	bool(const void* b) { value = b ? 1 : 0; }
+	bool() {}
+	operator int() const { return value; }
+	int operator!() const { return !value; }
+};
+
+
+const bool true = 1;
+const bool false = 0;
+
+
+
+// version for some older versions of gnu g++
+//#define false 0
+//#define true 1
+
+#ifdef use_namespace
+}
+#endif
+
+
+
+#endif
Index: orxonox/trunk/src/util/newmat/cholesky.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/cholesky.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/cholesky.cpp	(revision 4565)
@@ -0,0 +1,88 @@
+//$$ cholesky.cpp                     cholesky decomposition
+
+// Copyright (C) 1991,2,3,4: R B Davies
+
+#define WANT_MATH
+//#define WANT_STREAM
+
+#include "include.h"
+
+#include "newmat.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+#ifdef DO_REPORT
+#define REPORT { static ExeCounter ExeCount(__LINE__,14); ++ExeCount; }
+#else
+#define REPORT {}
+#endif
+
+/********* Cholesky decomposition of a positive definite matrix *************/
+
+// Suppose S is symmetrix and positive definite. Then there exists a unique
+// lower triangular matrix L such that L L.t() = S;
+
+inline Real square(Real x) { return x*x; }
+
+ReturnMatrix Cholesky(const SymmetricMatrix& S)
+{
+   REPORT
+   Tracer trace("Cholesky");
+   int nr = S.Nrows();
+   LowerTriangularMatrix T(nr);
+   Real* s = S.Store(); Real* t = T.Store(); Real* ti = t;
+   for (int i=0; i<nr; i++)
+   {
+      Real* tj = t; Real sum; int k;
+      for (int j=0; j<i; j++)
+      {
+         Real* tk = ti; sum = 0.0; k = j;
+         while (k--) { sum += *tj++ * *tk++; }
+         *tk = (*s++ - sum) / *tj++;
+      }
+      sum = 0.0; k = i;
+      while (k--) { sum += square(*ti++); }
+      Real d = *s++ - sum;
+      if (d<=0.0)  Throw(NPDException(S));
+      *ti++ = sqrt(d);
+   }
+   T.Release(); return T.ForReturn();
+}
+
+ReturnMatrix Cholesky(const SymmetricBandMatrix& S)
+{
+   REPORT
+   Tracer trace("Band-Cholesky");
+   int nr = S.Nrows(); int m = S.lower;
+   LowerBandMatrix T(nr,m);
+   Real* s = S.Store(); Real* t = T.Store(); Real* ti = t;
+
+   for (int i=0; i<nr; i++)
+   {
+      Real* tj = t; Real sum; int l;
+      if (i<m) { REPORT l = m-i; s += l; ti += l; l = i; }
+      else { REPORT t += (m+1); l = m; }
+
+      for (int j=0; j<l; j++)
+      {
+         Real* tk = ti; sum = 0.0; int k = j; tj += (m-j);
+         while (k--) { sum += *tj++ * *tk++; }
+         *tk = (*s++ - sum) / *tj++;
+      }
+      sum = 0.0;
+      while (l--) { sum += square(*ti++); }
+      Real d = *s++ - sum;
+      if (d<=0.0)  Throw(NPDException(S));
+      *ti++ = sqrt(d);
+   }
+
+   T.Release(); return T.ForReturn();
+}
+
+
+#ifdef use_namespace
+}
+#endif
+
Index: orxonox/trunk/src/util/newmat/controlw.h
===================================================================
--- orxonox/trunk/src/util/newmat/controlw.h	(revision 4565)
+++ orxonox/trunk/src/util/newmat/controlw.h	(revision 4565)
@@ -0,0 +1,48 @@
+//$$ controlw.h                Control word class
+
+#ifndef CONTROL_WORD_LIB
+#define CONTROL_WORD_LIB 0
+
+// for organising an int as a series of bits which indicate whether an
+// option is on or off.
+
+class ControlWord
+{
+protected:
+   int cw;                                      // the control word
+public:
+   ControlWord() : cw(0) {}                     // do nothing
+   ControlWord(int i) : cw(i) {}                // load an integer
+
+      // select specific bits (for testing at least one set)
+   ControlWord operator*(ControlWord i) const
+      { return ControlWord(cw & i.cw); }
+   void operator*=(ControlWord i)  { cw &= i.cw; }
+
+      // set bits
+   ControlWord operator+(ControlWord i) const
+      { return ControlWord(cw | i.cw); }
+   void operator+=(ControlWord i)  { cw |= i.cw; }
+
+      // reset bits
+   ControlWord operator-(ControlWord i) const
+      { return ControlWord(cw - (cw & i.cw)); }
+   void operator-=(ControlWord i) { cw -= (cw & i.cw); }
+
+      // check if all of selected bits set or reset
+   bool operator>=(ControlWord i) const { return (cw & i.cw) == i.cw; }
+   bool operator<=(ControlWord i) const { return (cw & i.cw) == cw; }
+
+      // flip selected bits
+   ControlWord operator^(ControlWord i) const
+      { return ControlWord(cw ^ i.cw); }
+   ControlWord operator~() const { return ControlWord(~cw); }
+
+      // convert to integer
+   int operator+() const { return cw; }
+   int operator!() const { return cw==0; }
+   FREE_CHECK(ControlWord)
+};
+
+
+#endif
Index: orxonox/trunk/src/util/newmat/eigen.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/eigen.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/eigen.cpp	(revision 4565)
@@ -0,0 +1,107 @@
+/*******************************************************
+ A simple program that demonstrates NewMat10 library.
+ The program defines a random symmetric matrix
+ and computes its eigendecomposition.
+ For further details read the NewMat10 Reference Manual
+********************************************************/
+
+
+#define WANT_STREAM
+#define WANT_MATH
+#define WANT_FSTREAM
+
+
+
+#include <stdlib.h>
+#include <time.h>
+#include <string.h>
+
+// the following two are needed for printing
+#include <iostream.h>
+#include <iomanip.h>
+/**************************************
+ The NewMat10 include files         */
+#include "include.h"
+#include "newmat.h"
+#include "newmatap.h"
+#include "newmatio.h"
+/***************************************/
+
+
+#ifdef use_namespace
+using namespace RBD_LIBRARIES;
+#endif
+
+int main(int argc, char **argv) {
+
+  int M = 3, N = 5;
+  Matrix X(M,N); // Define an M x N general matrix
+
+  // Fill X by random numbers between 0 and 9
+  // Note that indexing into matrices in NewMat is 1-based!
+  srand(time(NULL));
+  for (int i = 1; i <= M; ++i) {
+    for (int j = 1; j <= N; ++j) {
+      X(i,j) = rand() % 10;
+    }
+  }
+
+  SymmetricMatrix C;
+  C << X * X.t(); // fill in C by X * X^t. 
+  // Works because we *know* that the result is symmetric
+
+  cout << "The symmetrix matrix C" << endl;
+  cout << setw(5) << setprecision(0) << C << endl;
+	
+
+  // compute eigendecomposition of C
+  Matrix			V(3,3); // for eigenvectors
+  DiagonalMatrix	D(3);   // for eigenvalues
+
+  // the decomposition
+  Jacobi(C, D, V);
+	
+  // Print the result
+  cout << "The eigenvalues matrix:" << endl;
+  cout << setw(10) << setprecision(5) << D << endl;
+  cout << "The eigenvectors matrix:" << endl;
+  cout << setw(10) << setprecision(5) << V << endl;
+
+  // Check that the first eigenvector indeed has the eigenvector property
+  ColumnVector v1(3);
+  v1(1) = V(1,1);
+  v1(2) = V(2,1);
+  v1(3) = V(3,1);
+
+  ColumnVector Cv1 = C * v1;
+  ColumnVector lambda1_v1 = D(1) * v1;
+
+  cout << "The max-norm of the difference between C*v1 and lambda1*v1 is " <<
+    NormInfinity(Cv1 - lambda1_v1) << endl << endl;
+
+  // Build the inverse and check the result
+  Matrix Ci = C.i();
+  Matrix I  = Ci * C;
+
+  cout << "The inverse of C is" << endl;
+  cout << setw(10) << setprecision(5) << Ci << endl;
+  cout << "And the inverse times C is identity" << endl;
+  cout << setw(10) << setprecision(5) << I << endl;
+
+  // Example for multiple solves (see NewMat documentation)
+  ColumnVector r1(3), r2(3);
+  for (int i = 1; i <= 3; ++i) {
+    r1(i) = rand() % 10;
+    r2(i) = rand() % 10;
+  }
+  LinearEquationSolver CLU = C; // decomposes C
+  ColumnVector s1 = CLU.i() * r1;
+  ColumnVector s2 = CLU.i() * r2;
+
+  cout << "solution for right hand side r1" << endl;
+  cout << setw(10) << setprecision(5) << s1 << endl;
+  cout << "solution for right hand side r2" << endl;
+  cout << setw(10) << setprecision(5) << s2 << endl;
+	
+  return 0;
+}
Index: orxonox/trunk/src/util/newmat/evalue.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/evalue.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/evalue.cpp	(revision 4565)
@@ -0,0 +1,297 @@
+//$$evalue.cpp                           eigen-value decomposition
+
+// Copyright (C) 1991,2,3,4: R B Davies
+
+#define WANT_MATH
+
+#include "include.h"
+#include "newmatap.h"
+#include "newmatrm.h"
+#include "precisio.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+#ifdef DO_REPORT
+#define REPORT { static ExeCounter ExeCount(__LINE__,17); ++ExeCount; }
+#else
+#define REPORT {}
+#endif
+
+
+
+static void tred2(const SymmetricMatrix& A, DiagonalMatrix& D,
+   DiagonalMatrix& E, Matrix& Z)
+{
+   Tracer et("Evalue(tred2)");
+   REPORT
+   Real tol =
+      FloatingPointPrecision::Minimum()/FloatingPointPrecision::Epsilon();
+   int n = A.Nrows(); Z.ReSize(n,n); Z.Inject(A);
+   D.ReSize(n); E.ReSize(n);
+   Real* z = Z.Store(); int i;
+
+   for (i=n-1; i > 0; i--)                   // i=0 is excluded
+   {
+      Real f = Z.element(i,i-1); Real g = 0.0;
+      int k = i-1; Real* zik = z + i*n;
+      while (k--) g += square(*zik++);
+      Real h = g + square(f);
+      if (g <= tol) { REPORT E.element(i) = f; h = 0.0; }
+      else
+      {
+         REPORT
+         g = sign(-sqrt(h), f); E.element(i) = g; h -= f*g;
+         Z.element(i,i-1) = f-g; f = 0.0;
+         Real* zji = z + i; Real* zij = z + i*n; Real* ej = E.Store();
+         int j;
+         for (j=0; j<i; j++)
+         {
+            *zji = (*zij++)/h; g = 0.0;
+            Real* zjk = z + j*n; zik = z + i*n;
+            k = j; while (k--) g += *zjk++ * (*zik++);
+            k = i-j;
+            if (k) for(;;)
+               { g += *zjk * (*zik++); if (!(--k)) break; zjk += n; }
+            *ej++ = g/h; f += g * (*zji); zji += n;
+         }
+         Real hh = f / (h + h); zij = z + i*n; ej = E.Store();
+         for (j=0; j<i; j++)
+         {
+            f = *zij++; g = *ej - hh * f; *ej++ = g;
+            Real* zjk = z + j*n; Real* zik = z + i*n;
+            Real* ek = E.Store(); k = j+1;
+            while (k--)  *zjk++ -= ( f*(*ek++) + g*(*zik++) ); 
+         }
+      }
+      D.element(i) = h;
+   }
+
+   D.element(0) = 0.0; E.element(0) = 0.0;
+   for (i=0; i<n; i++)
+   {
+      if (D.element(i) != 0.0)
+      {
+         REPORT
+         for (int j=0; j<i; j++)
+         {
+            Real g = 0.0;
+            Real* zik = z + i*n; Real* zkj = z + j;
+            int k = i;
+            if (k) for (;;)
+               { g += *zik++ * (*zkj); if (!(--k)) break; zkj += n; }
+            Real* zki = z + i; zkj = z + j;
+            k = i;
+            if (k) for (;;)
+               { *zkj -= g * (*zki); if (!(--k)) break; zkj += n; zki += n; }
+         }
+      }
+      Real* zij = z + i*n; Real* zji = z + i;
+      int j = i;
+      if (j) for (;;)
+         { *zij++ = 0.0; *zji = 0.0; if (!(--j)) break; zji += n; }
+      D.element(i) = *zij; *zij = 1.0;
+   }
+}
+
+static void tql2(DiagonalMatrix& D, DiagonalMatrix& E, Matrix& Z)
+{
+   Tracer et("Evalue(tql2)");
+   REPORT
+   Real eps = FloatingPointPrecision::Epsilon();
+   int n = D.Nrows(); Real* z = Z.Store(); int l;
+   for (l=1; l<n; l++) E.element(l-1) = E.element(l);
+   Real b = 0.0; Real f = 0.0; E.element(n-1) = 0.0;
+   for (l=0; l<n; l++)
+   {
+      int i,j;
+      Real& dl = D.element(l); Real& el = E.element(l);
+      Real h = eps * ( fabs(dl) + fabs(el) );
+      if (b < h) { REPORT b = h; }
+      int m;
+      for (m=l; m<n; m++) if (fabs(E.element(m)) <= b) break;
+      bool test = false;
+      for (j=0; j<30; j++)
+      {
+         if (m==l) { REPORT test = true; break; }
+         Real& dl1 = D.element(l+1);
+         Real g = dl; Real p = (dl1-g) / (2.0*el); Real r = sqrt(p*p + 1.0);
+         dl = el / (p < 0.0 ? p-r : p+r); Real h = g - dl; f += h;
+         Real* dlx = &dl1; i = n-l-1; while (i--) *dlx++ -= h;
+
+         p = D.element(m); Real c = 1.0; Real s = 0.0;
+         for (i=m-1; i>=l; i--)
+         {
+            Real ei = E.element(i); Real di = D.element(i);
+            Real& ei1 = E.element(i+1);
+            g = c * ei; h = c * p;
+            if ( fabs(p) >= fabs(ei))
+            {
+               REPORT
+               c = ei / p; r = sqrt(c*c + 1.0);
+               ei1 = s*p*r; s = c/r; c = 1.0/r;
+            }
+            else
+            {
+               REPORT
+               c = p / ei; r = sqrt(c*c + 1.0);
+               ei1 = s * ei * r; s = 1.0/r; c /= r;
+            }
+            p = c * di - s*g; D.element(i+1) = h + s * (c*g + s*di);
+
+            Real* zki = z + i; Real* zki1 = zki + 1; int k = n;
+            if (k) for (;;)
+            {
+               REPORT
+               h = *zki1; *zki1 = s*(*zki) + c*h; *zki = c*(*zki) - s*h;
+               if (!(--k)) break;
+               zki += n; zki1 += n;
+            }
+         }
+         el = s*p; dl = c*p;
+         if (fabs(el) <= b) { REPORT; test = true; break; }
+      }
+      if (!test) Throw ( ConvergenceException(D) );
+      dl += f;
+   }
+/*
+   for (int i=0; i<n; i++)
+   {
+      int k = i; Real p = D.element(i);
+      for (int j=i+1; j<n; j++)
+         { if (D.element(j) < p) { k = j; p = D.element(j); } }
+      if (k != i)
+      {
+         D.element(k) = D.element(i); D.element(i) = p; int j = n;
+         Real* zji = z + i; Real* zjk = z + k;
+         if (j) for(;;)
+         {
+            p = *zji; *zji = *zjk; *zjk = p;
+            if (!(--j)) break;
+            zji += n; zjk += n;
+         }
+      }
+   }
+*/
+}
+
+static void tred3(const SymmetricMatrix& X, DiagonalMatrix& D,
+   DiagonalMatrix& E, SymmetricMatrix& A)
+{
+   Tracer et("Evalue(tred3)");
+   REPORT
+   Real tol =
+      FloatingPointPrecision::Minimum()/FloatingPointPrecision::Epsilon();
+   int n = X.Nrows(); A = X; D.ReSize(n); E.ReSize(n);
+   Real* ei = E.Store() + n;
+   for (int i = n-1; i >= 0; i--)
+   {
+      Real h = 0.0; Real f = - FloatingPointPrecision::Maximum();
+      Real* d = D.Store(); Real* a = A.Store() + (i*(i+1))/2; int k = i;
+      while (k--) { f = *a++; *d++ = f; h += square(f); }
+      if (h <= tol) { REPORT *(--ei) = 0.0; h = 0.0; }
+      else
+      {
+         REPORT
+         Real g = sign(-sqrt(h), f); *(--ei) = g; h -= f*g;
+         f -= g; *(d-1) = f; *(a-1) = f; f = 0.0;
+         Real* dj = D.Store(); Real* ej = E.Store(); int j;
+         for (j = 0; j < i; j++)
+         {
+            Real* dk = D.Store(); Real* ak = A.Store()+(j*(j+1))/2;
+            Real g = 0.0; k = j;
+            while (k--)  g += *ak++ * *dk++;
+            k = i-j; int l = j; 
+            if (k) for (;;) { g += *ak * *dk++; if (!(--k)) break; ak += ++l; }
+            g /= h; *ej++ = g; f += g * *dj++;
+         }  
+         Real hh = f / (2 * h); Real* ak = A.Store();
+         dj = D.Store(); ej = E.Store();
+         for (j = 0; j < i; j++)
+         {
+            f = *dj++; g = *ej - hh * f; *ej++ = g;
+            Real* dk = D.Store(); Real* ek = E.Store(); k = j+1;
+            while (k--) { *ak++ -= (f * *ek++ + g * *dk++); }
+         }
+      }
+      *d = *a; *a = h;
+   }
+}
+
+static void tql1(DiagonalMatrix& D, DiagonalMatrix& E)
+{
+   Tracer et("Evalue(tql1)");
+   REPORT
+   Real eps = FloatingPointPrecision::Epsilon();
+   int n = D.Nrows(); int l;
+   for (l=1; l<n; l++) E.element(l-1) = E.element(l);
+   Real b = 0.0; Real f = 0.0; E.element(n-1) = 0.0;
+   for (l=0; l<n; l++)
+   {
+      int i,j;
+      Real& dl = D.element(l); Real& el = E.element(l);
+      Real h = eps * ( fabs(dl) + fabs(el) );
+      if (b < h) b = h;
+      int m;
+      for (m=l; m<n; m++) if (fabs(E.element(m)) <= b) break;
+      bool test = false;
+      for (j=0; j<30; j++)
+      {
+         if (m==l) { REPORT test = true; break; }
+         Real& dl1 = D.element(l+1);
+         Real g = dl; Real p = (dl1-g) / (2.0*el); Real r = sqrt(p*p + 1.0);
+         dl = el / (p < 0.0 ? p-r : p+r); Real h = g - dl; f += h;
+         Real* dlx = &dl1; i = n-l-1; while (i--) *dlx++ -= h;
+
+         p = D.element(m); Real c = 1.0; Real s = 0.0;
+         for (i=m-1; i>=l; i--)
+         {
+            Real ei = E.element(i); Real di = D.element(i);
+            Real& ei1 = E.element(i+1);
+            g = c * ei; h = c * p;
+            if ( fabs(p) >= fabs(ei))
+            {
+               REPORT
+               c = ei / p; r = sqrt(c*c + 1.0);
+               ei1 = s*p*r; s = c/r; c = 1.0/r;
+            }
+            else
+            {
+               REPORT
+               c = p / ei; r = sqrt(c*c + 1.0);
+               ei1 = s * ei * r; s = 1.0/r; c /= r;
+            }
+            p = c * di - s*g; D.element(i+1) = h + s * (c*g + s*di);
+         }
+         el = s*p; dl = c*p;
+         if (fabs(el) <= b) { REPORT test = true; break; }
+      }
+      if (!test) Throw ( ConvergenceException(D) );
+      Real p = dl + f;
+      test = false;
+      for (i=l; i>0; i--)
+      {
+         if (p < D.element(i-1)) { REPORT D.element(i) = D.element(i-1); }
+         else { REPORT test = true; break; }
+      }
+      if (!test) i=0;
+      D.element(i) = p;
+   }
+}
+
+void EigenValues(const SymmetricMatrix& A, DiagonalMatrix& D, Matrix& Z)
+{ REPORT DiagonalMatrix E; tred2(A, D, E, Z); tql2(D, E, Z); SortSV(D,Z,true); }
+
+void EigenValues(const SymmetricMatrix& X, DiagonalMatrix& D)
+{ REPORT DiagonalMatrix E; SymmetricMatrix A; tred3(X,D,E,A); tql1(D,E); }
+
+void EigenValues(const SymmetricMatrix& X, DiagonalMatrix& D,
+   SymmetricMatrix& A)
+{ REPORT DiagonalMatrix E; tred3(X,D,E,A); tql1(D,E); }
+
+
+#ifdef use_namespace
+}
+#endif
+
Index: orxonox/trunk/src/util/newmat/example.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/example.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/example.cpp	(revision 4565)
@@ -0,0 +1,345 @@
+//$$ example.cpp                             Example of use of matrix package
+
+#define WANT_STREAM                  // include.h will get stream fns
+#define WANT_MATH                    // include.h will get math fns
+                                     // newmatap.h will get include.h
+
+#include "newmatap.h"                // need matrix applications
+
+#include "newmatio.h"                // need matrix output routines
+
+#ifdef use_namespace
+using namespace NEWMAT;              // access NEWMAT namespace
+#endif
+
+
+// demonstration of matrix package on linear regression problem
+
+
+void test1(Real* y, Real* x1, Real* x2, int nobs, int npred)
+{
+   cout << "\n\nTest 1 - traditional, bad\n";
+
+   // traditional sum of squares and products method of calculation
+   // but not adjusting means; maybe subject to round-off error
+
+   // make matrix of predictor values with 1s into col 1 of matrix
+   int npred1 = npred+1;        // number of cols including col of ones.
+   Matrix X(nobs,npred1);
+   X.Column(1) = 1.0;
+
+   // load x1 and x2 into X
+   //    [use << rather than = when loading arrays]
+   X.Column(2) << x1;  X.Column(3) << x2;
+
+   // vector of Y values
+   ColumnVector Y(nobs); Y << y;
+
+   // form sum of squares and product matrix
+   //    [use << rather than = for copying Matrix into SymmetricMatrix]
+   SymmetricMatrix SSQ; SSQ << X.t() * X;
+
+   // calculate estimate
+   //    [bracket last two terms to force this multiplication first]
+   //    [ .i() means inverse, but inverse is not explicity calculated]
+   ColumnVector A = SSQ.i() * (X.t() * Y);
+
+   // Get variances of estimates from diagonal elements of inverse of SSQ
+   // get inverse of SSQ - we need it for finding D
+   DiagonalMatrix D; D << SSQ.i();
+   ColumnVector V = D.AsColumn();
+
+   // Calculate fitted values and residuals
+   ColumnVector Fitted = X * A;
+   ColumnVector Residual = Y - Fitted;
+   Real ResVar = Residual.SumSquare() / (nobs-npred1);
+
+   // Get diagonals of Hat matrix (an expensive way of doing this)
+   DiagonalMatrix Hat;  Hat << X * (X.t() * X).i() * X.t();
+
+   // print out answers
+   cout << "\nEstimates and their standard errors\n\n";
+
+   // make vector of standard errors
+   ColumnVector SE(npred1);
+   for (int i=1; i<=npred1; i++) SE(i) = sqrt(V(i)*ResVar);
+   // use concatenation function to form matrix and use matrix print
+   // to get two columns
+   cout << setw(11) << setprecision(5) << (A | SE) << endl;
+
+   cout << "\nObservations, fitted value, residual value, hat value\n";
+
+   // use concatenation again; select only columns 2 to 3 of X
+   cout << setw(9) << setprecision(3) <<
+     (X.Columns(2,3) | Y | Fitted | Residual | Hat.AsColumn());
+   cout << "\n\n";
+}
+
+void test2(Real* y, Real* x1, Real* x2, int nobs, int npred)
+{
+   cout << "\n\nTest 2 - traditional, OK\n";
+
+   // traditional sum of squares and products method of calculation
+   // with subtraction of means - less subject to round-off error
+   // than test1
+
+   // make matrix of predictor values
+   Matrix X(nobs,npred);
+
+   // load x1 and x2 into X
+   //    [use << rather than = when loading arrays]
+   X.Column(1) << x1;  X.Column(2) << x2;
+
+   // vector of Y values
+   ColumnVector Y(nobs); Y << y;
+
+   // make vector of 1s
+   ColumnVector Ones(nobs); Ones = 1.0;
+
+   // calculate means (averages) of x1 and x2 [ .t() takes transpose]
+   RowVector M = Ones.t() * X / nobs;
+
+   // and subtract means from x1 and x1
+   Matrix XC(nobs,npred);
+   XC = X - Ones * M;
+
+   // do the same to Y [use Sum to get sum of elements]
+   ColumnVector YC(nobs);
+   Real m = Sum(Y) / nobs;  YC = Y - Ones * m;
+
+   // form sum of squares and product matrix
+   //    [use << rather than = for copying Matrix into SymmetricMatrix]
+   SymmetricMatrix SSQ; SSQ << XC.t() * XC;
+
+   // calculate estimate
+   //    [bracket last two terms to force this multiplication first]
+   //    [ .i() means inverse, but inverse is not explicity calculated]
+   ColumnVector A = SSQ.i() * (XC.t() * YC);
+
+   // calculate estimate of constant term
+   //    [AsScalar converts 1x1 matrix to Real]
+   Real a = m - (M * A).AsScalar();
+
+   // Get variances of estimates from diagonal elements of inverse of SSQ
+   //    [ we are taking inverse of SSQ - we need it for finding D ]
+   Matrix ISSQ = SSQ.i(); DiagonalMatrix D; D << ISSQ;
+   ColumnVector V = D.AsColumn();
+   Real v = 1.0/nobs + (M * ISSQ * M.t()).AsScalar();
+					    // for calc variance of const
+
+   // Calculate fitted values and residuals
+   int npred1 = npred+1;
+   ColumnVector Fitted = X * A + a;
+   ColumnVector Residual = Y - Fitted;
+   Real ResVar = Residual.SumSquare() / (nobs-npred1);
+
+   // Get diagonals of Hat matrix (an expensive way of doing this)
+   Matrix X1(nobs,npred1); X1.Column(1)<<Ones; X1.Columns(2,npred1)<<X;
+   DiagonalMatrix Hat;  Hat << X1 * (X1.t() * X1).i() * X1.t();
+
+   // print out answers
+   cout << "\nEstimates and their standard errors\n\n";
+   cout.setf(ios::fixed, ios::floatfield);
+   cout << setw(11) << setprecision(5)  << a << " ";
+   cout << setw(11) << setprecision(5)  << sqrt(v*ResVar) << endl;
+   // make vector of standard errors
+   ColumnVector SE(npred);
+   for (int i=1; i<=npred; i++) SE(i) = sqrt(V(i)*ResVar);
+   // use concatenation function to form matrix and use matrix print
+   // to get two columns
+   cout << setw(11) << setprecision(5) << (A | SE) << endl;
+   cout << "\nObservations, fitted value, residual value, hat value\n";
+   cout << setw(9) << setprecision(3) <<
+     (X | Y | Fitted | Residual | Hat.AsColumn());
+   cout << "\n\n";
+}
+
+void test3(Real* y, Real* x1, Real* x2, int nobs, int npred)
+{
+   cout << "\n\nTest 3 - Cholesky\n";
+
+   // traditional sum of squares and products method of calculation
+   // with subtraction of means - using Cholesky decomposition
+
+   Matrix X(nobs,npred);
+   X.Column(1) << x1;  X.Column(2) << x2;
+   ColumnVector Y(nobs); Y << y;
+   ColumnVector Ones(nobs); Ones = 1.0;
+   RowVector M = Ones.t() * X / nobs;
+   Matrix XC(nobs,npred);
+   XC = X - Ones * M;
+   ColumnVector YC(nobs);
+   Real m = Sum(Y) / nobs;  YC = Y - Ones * m;
+   SymmetricMatrix SSQ; SSQ << XC.t() * XC;
+
+   // Cholesky decomposition of SSQ
+   LowerTriangularMatrix L = Cholesky(SSQ);
+
+   // calculate estimate
+   ColumnVector A = L.t().i() * (L.i() * (XC.t() * YC));
+
+   // calculate estimate of constant term
+   Real a = m - (M * A).AsScalar();
+
+   // Get variances of estimates from diagonal elements of invoice of SSQ
+   DiagonalMatrix D; D << L.t().i() * L.i();
+   ColumnVector V = D.AsColumn();
+   Real v = 1.0/nobs + (L.i() * M.t()).SumSquare();
+
+   // Calculate fitted values and residuals
+   int npred1 = npred+1;
+   ColumnVector Fitted = X * A + a;
+   ColumnVector Residual = Y - Fitted;
+   Real ResVar = Residual.SumSquare() / (nobs-npred1);
+
+   // Get diagonals of Hat matrix (an expensive way of doing this)
+   Matrix X1(nobs,npred1); X1.Column(1)<<Ones; X1.Columns(2,npred1)<<X;
+   DiagonalMatrix Hat;  Hat << X1 * (X1.t() * X1).i() * X1.t();
+
+   // print out answers
+   cout << "\nEstimates and their standard errors\n\n";
+   cout.setf(ios::fixed, ios::floatfield);
+   cout << setw(11) << setprecision(5)  << a << " ";
+   cout << setw(11) << setprecision(5)  << sqrt(v*ResVar) << endl;
+   ColumnVector SE(npred);
+   for (int i=1; i<=npred; i++) SE(i) = sqrt(V(i)*ResVar);
+   cout << setw(11) << setprecision(5) << (A | SE) << endl;
+   cout << "\nObservations, fitted value, residual value, hat value\n";
+   cout << setw(9) << setprecision(3) <<
+      (X | Y | Fitted | Residual | Hat.AsColumn());
+   cout << "\n\n";
+}
+
+void test4(Real* y, Real* x1, Real* x2, int nobs, int npred)
+{
+   cout << "\n\nTest 4 - QR triangularisation\n";
+
+   // QR triangularisation method
+ 
+   // load data - 1s into col 1 of matrix
+   int npred1 = npred+1;
+   Matrix X(nobs,npred1); ColumnVector Y(nobs);
+   X.Column(1) = 1.0;  X.Column(2) << x1;  X.Column(3) << x2;  Y << y;
+
+   // do Householder triangularisation
+   // no need to deal with constant term separately
+   Matrix X1 = X;                 // Want copy of matrix
+   ColumnVector Y1 = Y;
+   UpperTriangularMatrix U; ColumnVector M;
+   QRZ(X1, U); QRZ(X1, Y1, M);    // Y1 now contains resids
+   ColumnVector A = U.i() * M;
+   ColumnVector Fitted = X * A;
+   Real ResVar = Y1.SumSquare() / (nobs-npred1);
+
+   // get variances of estimates
+   U = U.i(); DiagonalMatrix D; D << U * U.t();
+
+   // Get diagonals of Hat matrix
+   DiagonalMatrix Hat;  Hat << X1 * X1.t();
+
+   // print out answers
+   cout << "\nEstimates and their standard errors\n\n";
+   ColumnVector SE(npred1);
+   for (int i=1; i<=npred1; i++) SE(i) = sqrt(D(i)*ResVar);
+   cout << setw(11) << setprecision(5) << (A | SE) << endl;
+   cout << "\nObservations, fitted value, residual value, hat value\n";
+   cout << setw(9) << setprecision(3) << 
+      (X.Columns(2,3) | Y | Fitted | Y1 | Hat.AsColumn());
+   cout << "\n\n";
+}
+
+void test5(Real* y, Real* x1, Real* x2, int nobs, int npred)
+{
+   cout << "\n\nTest 5 - singular value\n";
+
+   // Singular value decomposition method
+ 
+   // load data - 1s into col 1 of matrix
+   int npred1 = npred+1;
+   Matrix X(nobs,npred1); ColumnVector Y(nobs);
+   X.Column(1) = 1.0;  X.Column(2) << x1;  X.Column(3) << x2;  Y << y;
+
+   // do SVD
+   Matrix U, V; DiagonalMatrix D;
+   SVD(X,D,U,V);                              // X = U * D * V.t()
+   ColumnVector Fitted = U.t() * Y;
+   ColumnVector A = V * ( D.i() * Fitted );
+   Fitted = U * Fitted;
+   ColumnVector Residual = Y - Fitted;
+   Real ResVar = Residual.SumSquare() / (nobs-npred1);
+
+   // get variances of estimates
+   D << V * (D * D).i() * V.t();
+
+   // Get diagonals of Hat matrix
+   DiagonalMatrix Hat;  Hat << U * U.t();
+
+   // print out answers
+   cout << "\nEstimates and their standard errors\n\n";
+   ColumnVector SE(npred1);
+   for (int i=1; i<=npred1; i++) SE(i) = sqrt(D(i)*ResVar);
+   cout << setw(11) << setprecision(5) << (A | SE) << endl;
+   cout << "\nObservations, fitted value, residual value, hat value\n";
+   cout << setw(9) << setprecision(3) << 
+      (X.Columns(2,3) | Y | Fitted | Residual | Hat.AsColumn());
+   cout << "\n\n";
+}
+
+int main()
+{
+   cout << "\nDemonstration of Matrix package\n";
+   cout << "\nPrint a real number (may help lost memory test): " << 3.14159265 << "\n";
+
+   // Test for any memory not deallocated after running this program
+   Real* s1; { ColumnVector A(8000); s1 = A.Store(); }
+
+   {
+      // the data
+
+#ifndef ATandT
+      Real y[]  = { 8.3, 5.5, 8.0, 8.5, 5.7, 4.4, 6.3, 7.9, 9.1 };
+      Real x1[] = { 2.4, 1.8, 2.4, 3.0, 2.0, 1.2, 2.0, 2.7, 3.6 };
+      Real x2[] = { 1.7, 0.9, 1.6, 1.9, 0.5, 0.6, 1.1, 1.0, 0.5 };
+#else             // for compilers that do not understand aggregrates
+      Real y[9], x1[9], x2[9];
+      y[0]=8.3; y[1]=5.5; y[2]=8.0; y[3]=8.5; y[4]=5.7;
+      y[5]=4.4; y[6]=6.3; y[7]=7.9; y[8]=9.1;
+      x1[0]=2.4; x1[1]=1.8; x1[2]=2.4; x1[3]=3.0; x1[4]=2.0;
+      x1[5]=1.2; x1[6]=2.0; x1[7]=2.7; x1[8]=3.6;
+      x2[0]=1.7; x2[1]=0.9; x2[2]=1.6; x2[3]=1.9; x2[4]=0.5;
+      x2[5]=0.6; x2[6]=1.1; x2[7]=1.0; x2[8]=0.5;
+#endif
+
+      int nobs = 9;                           // number of observations
+      int npred = 2;                          // number of predictor values
+
+      // we want to find the values of a,a1,a2 to give the best
+      // fit of y[i] with a0 + a1*x1[i] + a2*x2[i]
+      // Also print diagonal elements of hat matrix, X*(X.t()*X).i()*X.t()
+
+      // this example demonstrates five methods of calculation
+
+      Try
+      {
+         test1(y, x1, x2, nobs, npred);
+         test2(y, x1, x2, nobs, npred);
+         test3(y, x1, x2, nobs, npred);
+         test4(y, x1, x2, nobs, npred);
+         test5(y, x1, x2, nobs, npred);
+      }
+      CatchAll { cout << Exception::what(); }
+   }
+
+#ifdef DO_FREE_CHECK
+   FreeCheck::Status();
+#endif
+   Real* s2; { ColumnVector A(8000); s2 = A.Store(); }
+   cout << "\n\nThe following test does not work with all compilers - see documentation\n";
+   cout << "Checking for lost memory: "
+      << (unsigned long)s1 << " " << (unsigned long)s2 << " ";
+   if (s1 != s2) cout << " - error\n"; else cout << " - ok\n";
+
+   return 0;
+
+}
+
Index: orxonox/trunk/src/util/newmat/fft.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/fft.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/fft.cpp	(revision 4565)
@@ -0,0 +1,450 @@
+//$$ fft.cpp                         Fast fourier transform
+
+// Copyright (C) 1991,2,3,4,8: R B Davies
+
+
+#define WANT_MATH
+// #define WANT_STREAM
+
+#include "include.h"
+
+#include "newmatap.h"
+
+// #include "newmatio.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+#ifdef DO_REPORT
+#define REPORT { static ExeCounter ExeCount(__LINE__,19); ++ExeCount; }
+#else
+#define REPORT {}
+#endif
+
+static void cossin(int n, int d, Real& c, Real& s)
+// calculate cos(twopi*n/d) and sin(twopi*n/d)
+// minimise roundoff error
+{
+   REPORT
+   long n4 = n * 4; int sector = (int)floor( (Real)n4 / (Real)d + 0.5 );
+   n4 -= sector * d;
+   if (sector < 0) { REPORT sector = 3 - (3 - sector) % 4; }
+   else  { REPORT sector %= 4; }
+   Real ratio = 1.5707963267948966192 * (Real)n4 / (Real)d;
+
+   switch (sector)
+   {
+   case 0: REPORT c =  cos(ratio); s =  sin(ratio); break;
+   case 1: REPORT c = -sin(ratio); s =  cos(ratio); break;
+   case 2: REPORT c = -cos(ratio); s = -sin(ratio); break;
+   case 3: REPORT c =  sin(ratio); s = -cos(ratio); break;
+   }
+}
+
+static void fftstep(ColumnVector& A, ColumnVector& B, ColumnVector& X,
+   ColumnVector& Y, int after, int now, int before)
+{
+   REPORT
+   Tracer trace("FFT(step)");
+   // const Real twopi = 6.2831853071795864769;
+   const int gamma = after * before;  const int delta = now * after;
+   // const Real angle = twopi / delta;  Real temp;
+   // Real r_omega = cos(angle);  Real i_omega = -sin(angle);
+   Real r_arg = 1.0;  Real i_arg = 0.0;
+   Real* x = X.Store();  Real* y = Y.Store();   // pointers to array storage
+   const int m = A.Nrows() - gamma;
+
+   for (int j = 0; j < now; j++)
+   {
+      Real* a = A.Store(); Real* b = B.Store(); // pointers to array storage
+      Real* x1 = x; Real* y1 = y; x += after; y += after;
+      for (int ia = 0; ia < after; ia++)
+      {
+         // generate sins & cosines explicitly rather than iteratively
+         // for more accuracy; but slower
+         cossin(-(j*after+ia), delta, r_arg, i_arg);
+
+         Real* a1 = a++; Real* b1 = b++; Real* x2 = x1++; Real* y2 = y1++;
+         if (now==2)
+         {
+            REPORT int ib = before;
+            if (ib) for (;;)
+            {
+               REPORT
+               Real* a2 = m + a1; Real* b2 = m + b1; a1 += after; b1 += after;
+               Real r_value = *a2; Real i_value = *b2;
+               *x2 = r_value * r_arg - i_value * i_arg + *(a2-gamma);
+               *y2 = r_value * i_arg + i_value * r_arg + *(b2-gamma);
+               if (!(--ib)) break;
+               x2 += delta; y2 += delta;
+            }
+         }
+         else
+         {
+            REPORT int ib = before;
+            if (ib) for (;;)
+            {
+               REPORT
+               Real* a2 = m + a1; Real* b2 = m + b1; a1 += after; b1 += after;
+               Real r_value = *a2; Real i_value = *b2;
+               int in = now-1; while (in--)
+               {
+                  // it should be possible to make this faster
+                  // hand code for now = 2,3,4,5,8
+                  // use symmetry to halve number of operations
+                  a2 -= gamma; b2 -= gamma;  Real temp = r_value;
+                  r_value = r_value * r_arg - i_value * i_arg + *a2;
+                  i_value = temp    * i_arg + i_value * r_arg + *b2;
+               }
+               *x2 = r_value; *y2 = i_value;
+               if (!(--ib)) break;
+               x2 += delta; y2 += delta;
+            }
+         }
+
+         // temp = r_arg;
+         // r_arg = r_arg * r_omega - i_arg * i_omega;
+         // i_arg = temp  * i_omega + i_arg * r_omega;
+
+      }
+   }
+}
+
+
+void FFTI(const ColumnVector& U, const ColumnVector& V,
+   ColumnVector& X, ColumnVector& Y)
+{
+   // Inverse transform
+   Tracer trace("FFTI");
+   REPORT
+   FFT(U,-V,X,Y);
+   const Real n = X.Nrows(); X /= n; Y /= (-n);
+}
+
+void RealFFT(const ColumnVector& U, ColumnVector& X, ColumnVector& Y)
+{
+   // Fourier transform of a real series
+   Tracer trace("RealFFT");
+   REPORT
+   const int n = U.Nrows();                     // length of arrays
+   const int n2 = n / 2;
+   if (n != 2 * n2)
+      Throw(ProgramException("Vector length not multiple of 2", U));
+   ColumnVector A(n2), B(n2);
+   Real* a = A.Store(); Real* b = B.Store(); Real* u = U.Store(); int i = n2;
+   while (i--) { *a++ = *u++; *b++ = *u++; }
+   FFT(A,B,A,B);
+   int n21 = n2 + 1;
+   X.ReSize(n21); Y.ReSize(n21);
+   i = n2 - 1;
+   a = A.Store(); b = B.Store();              // first els of A and B
+   Real* an = a + i; Real* bn = b + i;        // last els of A and B
+   Real* x = X.Store(); Real* y = Y.Store();  // first els of X and Y
+   Real* xn = x + n2; Real* yn = y + n2;      // last els of X and Y
+
+   *x++ = *a + *b; *y++ = 0.0;                // first complex element
+   *xn-- = *a++ - *b++; *yn-- = 0.0;          // last complex element
+
+   int j = -1; i = n2/2;
+   while (i--)
+   {
+      Real c,s; cossin(j--,n,c,s);
+      Real am = *a - *an; Real ap = *a++ + *an--;
+      Real bm = *b - *bn; Real bp = *b++ + *bn--;
+      Real samcbp = s * am + c * bp; Real sbpcam = s * bp - c * am;
+      *x++  =  0.5 * ( ap + samcbp); *y++  =  0.5 * ( bm + sbpcam);
+      *xn-- =  0.5 * ( ap - samcbp); *yn-- =  0.5 * (-bm + sbpcam);
+   }
+}
+
+void RealFFTI(const ColumnVector& A, const ColumnVector& B, ColumnVector& U)
+{
+   // inverse of a Fourier transform of a real series
+   Tracer trace("RealFFTI");
+   REPORT
+   const int n21 = A.Nrows();                     // length of arrays
+   if (n21 != B.Nrows() || n21 == 0)
+      Throw(ProgramException("Vector lengths unequal or zero", A, B));
+   const int n2 = n21 - 1;  const int n = 2 * n2;  int i = n2 - 1;
+
+   ColumnVector X(n2), Y(n2);
+   Real* a = A.Store(); Real* b = B.Store();  // first els of A and B
+   Real* an = a + n2;   Real* bn = b + n2;    // last els of A and B
+   Real* x = X.Store(); Real* y = Y.Store();  // first els of X and Y
+   Real* xn = x + i;    Real* yn = y + i;     // last els of X and Y
+
+   Real hn = 0.5 / n2;
+   *x++  = hn * (*a + *an);  *y++  = - hn * (*a - *an);
+   a++; an--; b++; bn--;
+   int j = -1;  i = n2/2;
+   while (i--)
+   {
+      Real c,s; cossin(j--,n,c,s);
+      Real am = *a - *an; Real ap = *a++ + *an--;
+      Real bm = *b - *bn; Real bp = *b++ + *bn--;
+      Real samcbp = s * am - c * bp; Real sbpcam = s * bp + c * am;
+      *x++  =  hn * ( ap + samcbp); *y++  =  - hn * ( bm + sbpcam);
+      *xn-- =  hn * ( ap - samcbp); *yn-- =  - hn * (-bm + sbpcam);
+   }
+   FFT(X,Y,X,Y);             // have done inverting elsewhere
+   U.ReSize(n); i = n2;
+   x = X.Store(); y = Y.Store(); Real* u = U.Store();
+   while (i--) { *u++ = *x++; *u++ = - *y++; }
+}
+
+void FFT(const ColumnVector& U, const ColumnVector& V,
+   ColumnVector& X, ColumnVector& Y)
+{
+   // from Carl de Boor (1980), Siam J Sci Stat Comput, 1 173-8
+   // but first try Sande and Gentleman
+   Tracer trace("FFT");
+   REPORT
+   const int n = U.Nrows();                     // length of arrays
+   if (n != V.Nrows() || n == 0)
+      Throw(ProgramException("Vector lengths unequal or zero", U, V));
+   if (n == 1) { REPORT X = U; Y = V; return; }
+
+   // see if we can use the newfft routine
+   if (!FFT_Controller::OnlyOldFFT && FFT_Controller::CanFactor(n))
+   {
+      REPORT
+      X = U; Y = V;
+      if ( FFT_Controller::ar_1d_ft(n,X.Store(),Y.Store()) ) return;
+   }
+
+   ColumnVector B = V;
+   ColumnVector A = U;
+   X.ReSize(n); Y.ReSize(n);
+   const int nextmx = 8;
+#ifndef ATandT
+   int prime[8] = { 2,3,5,7,11,13,17,19 };
+#else
+   int prime[8];
+   prime[0]=2; prime[1]=3; prime[2]=5; prime[3]=7;
+   prime[4]=11; prime[5]=13; prime[6]=17; prime[7]=19;
+#endif
+   int after = 1; int before = n; int next = 0; bool inzee = true;
+   int now = 0; int b1;             // initialised to keep gnu happy
+
+   do
+   {
+      for (;;)
+      {
+	 if (next < nextmx) { REPORT now = prime[next]; }
+	 b1 = before / now;  if (b1 * now == before) { REPORT break; }
+	 next++; now += 2;
+      }
+      before = b1;
+
+      if (inzee) { REPORT fftstep(A, B, X, Y, after, now, before); }
+      else { REPORT fftstep(X, Y, A, B, after, now, before); }
+
+      inzee = !inzee; after *= now;
+   }
+   while (before != 1);
+
+   if (inzee) { REPORT A.Release(); X = A; B.Release(); Y = B; }
+}
+
+// Trigonometric transforms
+// see Charles Van Loan (1992) "Computational frameworks for the fast
+// Fourier transform" published by SIAM; section 4.4.
+
+void DCT_II(const ColumnVector& U, ColumnVector& V)
+{
+   // Discrete cosine transform, type II, of a real series
+   Tracer trace("DCT_II");
+   REPORT
+   const int n = U.Nrows();                     // length of arrays
+   const int n2 = n / 2; const int n4 = n * 4;
+   if (n != 2 * n2)
+      Throw(ProgramException("Vector length not multiple of 2", U));
+   ColumnVector A(n);
+   Real* a = A.Store(); Real* b = a + n; Real* u = U.Store();
+   int i = n2;
+   while (i--) { *a++ = *u++; *(--b) = *u++; }
+   ColumnVector X, Y;
+   RealFFT(A, X, Y); A.CleanUp();
+   V.ReSize(n);
+   Real* x = X.Store(); Real* y = Y.Store();
+   Real* v = V.Store(); Real* w = v + n;
+   *v = *x;
+   int k = 0; i = n2;
+   while (i--)
+   {
+      Real c, s; cossin(++k, n4, c, s);
+      Real xi = *(++x); Real yi = *(++y);
+      *(++v) = xi * c + yi * s; *(--w) = xi * s - yi * c;
+   }
+}
+
+void DCT_II_inverse(const ColumnVector& V, ColumnVector& U)
+{
+   // Inverse of discrete cosine transform, type II
+   Tracer trace("DCT_II_inverse");
+   REPORT
+   const int n = V.Nrows();                     // length of array
+   const int n2 = n / 2; const int n4 = n * 4; const int n21 = n2 + 1;
+   if (n != 2 * n2)
+      Throw(ProgramException("Vector length not multiple of 2", V));
+   ColumnVector X(n21), Y(n21);
+   Real* x = X.Store(); Real* y = Y.Store();
+   Real* v = V.Store(); Real* w = v + n;
+   *x = *v; *y = 0.0;
+   int i = n2; int k = 0;
+   while (i--)
+   {
+      Real c, s; cossin(++k, n4, c, s);
+      Real vi = *(++v); Real wi = *(--w);
+      *(++x) = vi * c + wi * s; *(++y) = vi * s - wi * c;
+   }
+   ColumnVector A; RealFFTI(X, Y, A);
+   X.CleanUp(); Y.CleanUp(); U.ReSize(n);
+   Real* a = A.Store(); Real* b = a + n; Real* u = U.Store();
+   i = n2;
+   while (i--) { *u++ = *a++; *u++ = *(--b); }
+}
+
+void DST_II(const ColumnVector& U, ColumnVector& V)
+{
+   // Discrete sine transform, type II, of a real series
+   Tracer trace("DST_II");
+   REPORT
+   const int n = U.Nrows();                     // length of arrays
+   const int n2 = n / 2; const int n4 = n * 4;
+   if (n != 2 * n2)
+      Throw(ProgramException("Vector length not multiple of 2", U));
+   ColumnVector A(n);
+   Real* a = A.Store(); Real* b = a + n; Real* u = U.Store();
+   int i = n2;
+   while (i--) { *a++ = *u++; *(--b) = -(*u++); }
+   ColumnVector X, Y;
+   RealFFT(A, X, Y); A.CleanUp();
+   V.ReSize(n);
+   Real* x = X.Store(); Real* y = Y.Store();
+   Real* v = V.Store(); Real* w = v + n;
+   *(--w) = *x;
+   int k = 0; i = n2;
+   while (i--)
+   {
+      Real c, s; cossin(++k, n4, c, s);
+      Real xi = *(++x); Real yi = *(++y);
+      *v++ = xi * s - yi * c; *(--w) = xi * c + yi * s;
+   }
+}
+
+void DST_II_inverse(const ColumnVector& V, ColumnVector& U)
+{
+   // Inverse of discrete sine transform, type II
+   Tracer trace("DST_II_inverse");
+   REPORT
+   const int n = V.Nrows();                     // length of array
+   const int n2 = n / 2; const int n4 = n * 4; const int n21 = n2 + 1;
+   if (n != 2 * n2)
+      Throw(ProgramException("Vector length not multiple of 2", V));
+   ColumnVector X(n21), Y(n21);
+   Real* x = X.Store(); Real* y = Y.Store();
+   Real* v = V.Store(); Real* w = v + n;
+   *x = *(--w); *y = 0.0;
+   int i = n2; int k = 0;
+   while (i--)
+   {
+      Real c, s; cossin(++k, n4, c, s);
+      Real vi = *v++; Real wi = *(--w);
+      *(++x) = vi * s + wi * c; *(++y) = - vi * c + wi * s;
+   }
+   ColumnVector A; RealFFTI(X, Y, A);
+   X.CleanUp(); Y.CleanUp(); U.ReSize(n);
+   Real* a = A.Store(); Real* b = a + n; Real* u = U.Store();
+   i = n2;
+   while (i--) { *u++ = *a++; *u++ = -(*(--b)); }
+}
+
+void DCT_inverse(const ColumnVector& V, ColumnVector& U)
+{
+   // Inverse of discrete cosine transform, type I
+   Tracer trace("DCT_inverse");
+   REPORT
+   const int n = V.Nrows()-1;                     // length of transform
+   const int n2 = n / 2; const int n21 = n2 + 1;
+   if (n != 2 * n2)
+      Throw(ProgramException("Vector length not multiple of 2", V));
+   ColumnVector X(n21), Y(n21);
+   Real* x = X.Store(); Real* y = Y.Store(); Real* v = V.Store();
+   Real vi = *v++; *x++ = vi; *y++ = 0.0;
+   Real sum1 = vi / 2.0; Real sum2 = sum1; vi = *v++;
+   int i = n2-1;
+   while (i--)
+   {
+      Real vi2 = *v++; sum1 += vi2 + vi; sum2 += vi2 - vi;
+      *x++ = vi2; vi2 = *v++; *y++ = vi - vi2; vi = vi2;
+   }
+   sum1 += vi; sum2 -= vi;
+   vi = *v; *x = vi; *y = 0.0; vi /= 2.0; sum1 += vi; sum2 += vi;
+   ColumnVector A; RealFFTI(X, Y, A);
+   X.CleanUp(); Y.CleanUp(); U.ReSize(n+1);
+   Real* a = A.Store(); Real* b = a + n; Real* u = U.Store(); v = u + n;
+   i = n2; int k = 0; *u++ = sum1 / n2; *v-- = sum2 / n2;
+   while (i--)
+   {
+      Real s = sin(1.5707963267948966192 * (++k) / n2);
+      Real ai = *(++a); Real bi = *(--b);
+      Real bz = (ai - bi) / 4 / s; Real az = (ai + bi) / 2;
+      *u++ = az - bz; *v-- = az + bz;
+   }
+}
+
+void DCT(const ColumnVector& U, ColumnVector& V)
+{
+   // Discrete cosine transform, type I
+   Tracer trace("DCT");
+   REPORT
+   DCT_inverse(U, V);
+   V *= (V.Nrows()-1)/2;
+}
+
+void DST_inverse(const ColumnVector& V, ColumnVector& U)
+{
+   // Inverse of discrete sine transform, type I
+   Tracer trace("DST_inverse");
+   REPORT
+   const int n = V.Nrows()-1;                     // length of transform
+   const int n2 = n / 2; const int n21 = n2 + 1;
+   if (n != 2 * n2)
+      Throw(ProgramException("Vector length not multiple of 2", V));
+   ColumnVector X(n21), Y(n21);
+   Real* x = X.Store(); Real* y = Y.Store(); Real* v = V.Store();
+   Real vi = *(++v); *x++ = 2 * vi; *y++ = 0.0;
+   int i = n2-1;
+   while (i--) { *y++ = *(++v); Real vi2 = *(++v); *x++ = vi2 - vi; vi = vi2; }
+   *x = -2 * vi; *y = 0.0;
+   ColumnVector A; RealFFTI(X, Y, A);
+   X.CleanUp(); Y.CleanUp(); U.ReSize(n+1);
+   Real* a = A.Store(); Real* b = a + n; Real* u = U.Store(); v = u + n;
+   i = n2; int k = 0; *u++ = 0.0; *v-- = 0.0;
+   while (i--)
+   {
+      Real s = sin(1.5707963267948966192 * (++k) / n2);
+      Real ai = *(++a); Real bi = *(--b);
+      Real az = (ai + bi) / 4 / s; Real bz = (ai - bi) / 2;
+      *u++ = az - bz; *v-- = az + bz;
+   }
+}
+
+void DST(const ColumnVector& U, ColumnVector& V)
+{
+   // Discrete sine transform, type I
+   Tracer trace("DST");
+   REPORT
+   DST_inverse(U, V);
+   V *= (V.Nrows()-1)/2;
+}
+
+
+
+#ifdef use_namespace
+}
+#endif
+
+
Index: orxonox/trunk/src/util/newmat/garch.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/garch.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/garch.cpp	(revision 4565)
@@ -0,0 +1,175 @@
+
+#define WANT_STREAM
+#define WANT_MATH
+#define WANT_FSTREAM
+
+#include "newmatap.h"
+#include "newmatio.h"
+#include "newmatnl.h"
+
+#ifdef use_namespace
+using namespace RBD_LIBRARIES;
+#endif
+
+// This is a demonstration of a special case of the Garch model
+// Observe two series X and Y of length n
+// and suppose
+//    Y(i) = beta * X(i) + epsilon(i)
+// where epsilon(i) is normally distributed with zero mean and variance =
+//    h(i) = alpha0 + alpha1 * square(epsilon(i-1)) + beta1 * h(i-1).
+// Then this program is supposed to estimate beta, alpha0, alpha1, beta1
+// The Garch model is supposed to model something like an instability
+// in the stock or options market following an unexpected result.
+// alpha1 determines the size of the instability and beta1 determines how
+// quickly is dies away.
+// We should, at least, have an X of several columns and beta as a vector
+
+inline Real square(Real x) { return x*x; }
+
+// the class that defines the GARCH log-likelihood
+
+class GARCH11_LL : public LL_D_FI
+{
+   ColumnVector Y;                 // Y values
+   ColumnVector X;                 // X values
+   ColumnVector D;                 // derivatives of loglikelihood
+   SymmetricMatrix D2;             // - approximate second derivatives
+   int n;                          // number of observations
+   Real beta, alpha0, alpha1, beta1;
+                                   // the parameters
+
+public:
+
+   GARCH11_LL(const ColumnVector& y, const ColumnVector& x)
+      : Y(y), X(x), n(y.Nrows()) {}
+                                   // constructor - load Y and X values
+
+   void Set(const ColumnVector& p) // set parameter values
+   {
+      para = p;
+      beta = para(1); alpha0 = para(2);
+      alpha1 = para(3); beta1 = para(4);
+   }
+
+   bool IsValid();                 // are parameters valid
+   Real LogLikelihood();           // return the loglikelihood
+   ReturnMatrix Derivatives();     // derivatives of log-likelihood
+   ReturnMatrix FI();              // Fisher Information matrix
+};
+
+bool GARCH11_LL::IsValid()
+{ return alpha0>0 && alpha1>0 && beta1>0 && (alpha1+beta1)<1.0; }
+
+Real GARCH11_LL::LogLikelihood()
+{
+//   cout << endl << "                           ";
+//   cout << setw(10) << setprecision(5) << beta;
+//   cout << setw(10) << setprecision(5) << alpha0;
+//   cout << setw(10) << setprecision(5) << alpha1;
+//   cout << setw(10) << setprecision(5) << beta1;
+//   cout << endl;
+   ColumnVector H(n);              // residual variances
+   ColumnVector U = Y - X * beta;  // the residuals
+   ColumnVector LH(n);     // derivative of log-likelihood wrt H
+			   // each row corresponds to one observation
+   LH(1)=0;
+   Matrix Hderiv(n,4);     // rectangular matrix of derivatives
+			   // of H wrt parameters
+			   // each row corresponds to one observation
+			   // each column to one of the parameters
+
+   // Regard Y(1) as fixed and don't include in likelihood
+   // then put in an expected value of H(1) in place of actual value
+   //   which we don't know. Use
+   // E{H(i)} = alpha0 + alpha1 * E{square(epsilon(i-1))} + beta1 * E{H(i-1)}
+   // and  E{square(epsilon(i-1))} = E{H(i-1)} = E{H(i)}
+   Real denom = (1-alpha1-beta1);
+   H(1) = alpha0/denom;    // the expected value of H
+   Hderiv(1,1) = 0;
+   Hderiv(1,2) = 1.0 / denom;
+   Hderiv(1,3) = alpha0 / square(denom);
+   Hderiv(1,4) = Hderiv(1,3);
+   Real LL = 0.0;          // the log likelihood
+   Real sum1 = 0;          // for forming derivative wrt beta
+   Real sum2 = 0;          // for forming second derivative wrt beta
+   for (int i=2; i<=n; i++)
+   {
+      Real u1 = U(i-1); Real h1 = H(i-1);
+      Real h = alpha0 + alpha1*square(u1) + beta1*h1; // variance of this obsv.
+      H(i) = h; Real u = U(i);
+      LL += log(h) + square(u) / h;        // -2 * log likelihood
+      // Hderiv are derivatives of h with respect to the parameters
+      // need to allow for h1 depending on parameters
+      Hderiv(i,1) = -2*u1*alpha1*X(i-1) + beta1*Hderiv(i-1,1);  // beta
+      Hderiv(i,2) = 1 + beta1*Hderiv(i-1,2);                    // alpha0
+      Hderiv(i,3) = square(u1) + beta1*Hderiv(i-1,3);           // alpha1
+      Hderiv(i,4) = h1 + beta1*Hderiv(i-1,4);                   // beta1
+      LH(i) = -0.5 * (1/h - square(u/h));
+      sum1 += u * X(i)/ h;
+      sum2 += square(X(i)) / h;
+   }
+   D = Hderiv.t()*LH;         // derivatives of likelihood wrt parameters
+   D(1) += sum1;              // add on deriv wrt beta from square(u) term
+//   cout << setw(10) << setprecision(5) << D << endl;
+
+   // do minus expected value of second derivatives
+   if (wg)                    // do only if second derivatives wanted
+   {
+      Hderiv.Row(1) = 0.0;
+      Hderiv = H.AsDiagonal().i() * Hderiv;
+      D2 << Hderiv.t() * Hderiv;  D2 = D2 / 2.0;
+      D2(1,1) += sum2;
+//      cout << setw(10) << setprecision(5) << D2 << endl;
+//      DiagonalMatrix DX; EigenValues(D2,DX);
+//      cout << setw(10) << setprecision(5) << DX << endl;
+
+   }
+   return -0.5 * LL;
+}
+
+ReturnMatrix GARCH11_LL::Derivatives()
+{ return D; }
+
+ReturnMatrix GARCH11_LL::FI()
+{
+   if (!wg) cout << endl << "unexpected call of FI" << endl;
+   return D2;
+}
+
+
+
+int main()
+{
+   // get data
+   ifstream fin("garch.dat");
+   if (!fin) { cout << "cannot find garch.dat\n"; exit(1); }
+   int n; fin >> n;            // series length
+   // Y contains the dependant variable, X the predictor variable
+   ColumnVector Y(n), X(n);
+   int i;
+   for (i=1; i<=n; i++) fin >> Y(i) >> X(i);
+   cout << "Read " << n << " data points - begin fit\n\n";
+   // now do the fit
+   ColumnVector H(n);
+   GARCH11_LL garch11(Y,X);                  // loglikehood "object"
+   MLE_D_FI mle_d_fi(garch11,100,0.0001);    // mle "object"
+   ColumnVector Para(4);                     // to hold the parameters
+   Para << 0.0 << 0.1 << 0.1 << 0.1;         // starting values
+      // (Should change starting values to a more intelligent formula)
+   mle_d_fi.Fit(Para);                       // do the fit
+   ColumnVector SE;
+   mle_d_fi.GetStandardErrors(SE);
+   cout << "\n\n";
+   cout << "estimates and standard errors\n";
+   cout << setw(15) << setprecision(5) << (Para | SE) << endl << endl;
+   SymmetricMatrix Corr;
+   mle_d_fi.GetCorrelations(Corr);
+   cout << "correlation matrix\n";
+   cout << setw(10) << setprecision(2) << Corr << endl << endl;
+   cout << "inverse of correlation matrix\n";
+   cout << setw(10) << setprecision(2) << Corr.i() << endl << endl;
+   return 0;
+}
+
+
+
Index: orxonox/trunk/src/util/newmat/hholder.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/hholder.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/hholder.cpp	(revision 4565)
@@ -0,0 +1,199 @@
+//$$ hholder.cpp                                   QR decomposition
+
+// Copyright (C) 1991,2,3,4: R B Davies
+
+#define WANT_MATH
+
+#include "include.h"
+
+#include "newmatap.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+#ifdef DO_REPORT
+#define REPORT { static ExeCounter ExeCount(__LINE__,16); ++ExeCount; }
+#else
+#define REPORT {}
+#endif
+
+
+/*************************** QR decompositions ***************************/
+
+inline Real square(Real x) { return x*x; }
+
+void QRZT(Matrix& X, LowerTriangularMatrix& L)
+{
+   REPORT
+	Tracer et("QZT(1)");
+   int n = X.Ncols(); int s = X.Nrows(); L.ReSize(s);
+   Real* xi = X.Store(); int k;
+   for (int i=0; i<s; i++)
+   {
+      Real sum = 0.0;
+      Real* xi0=xi; k=n; while(k--) { sum += square(*xi++); }
+      sum = sqrt(sum);
+      L.element(i,i) = sum;
+      if (sum==0.0) Throw(SingularException(L));
+      Real* xj0=xi0; k=n; while(k--) { *xj0++ /= sum; }
+      for (int j=i+1; j<s; j++)
+      {
+         sum=0.0;
+         xi=xi0; Real* xj=xj0; k=n; while(k--) { sum += *xi++ * *xj++; }
+         xi=xi0; k=n; while(k--) { *xj0++ -= sum * *xi++; }
+         L.element(j,i) = sum;
+      }
+   }
+}
+
+void QRZT(const Matrix& X, Matrix& Y, Matrix& M)
+{
+   REPORT
+   Tracer et("QRZT(2)");
+   int n = X.Ncols(); int s = X.Nrows(); int t = Y.Nrows();
+   if (Y.Ncols() != n)
+      { Throw(ProgramException("Unequal row lengths",X,Y)); }
+   M.ReSize(t,s);
+   Real* xi = X.Store(); int k;
+   for (int i=0; i<s; i++)
+   {
+      Real* xj0 = Y.Store(); Real* xi0 = xi;
+      for (int j=0; j<t; j++)
+      {
+         Real sum=0.0;
+         xi=xi0; Real* xj=xj0; k=n; while(k--) { sum += *xi++ * *xj++; }
+         xi=xi0; k=n; while(k--) { *xj0++ -= sum * *xi++; }
+         M.element(j,i) = sum;
+      }
+   }
+}
+
+/*
+void QRZ(Matrix& X, UpperTriangularMatrix& U)
+{
+	Tracer et("QRZ(1)");
+	int n = X.Nrows(); int s = X.Ncols(); U.ReSize(s);
+	Real* xi0 = X.Store(); int k;
+	for (int i=0; i<s; i++)
+	{
+		Real sum = 0.0;
+		Real* xi = xi0; k=n; while(k--) { sum += square(*xi); xi+=s; }
+		sum = sqrt(sum);
+		U.element(i,i) = sum;
+		if (sum==0.0) Throw(SingularException(U));
+		Real* xj0=xi0; k=n; while(k--) { *xj0 /= sum; xj0+=s; }
+		xj0 = xi0;
+		for (int j=i+1; j<s; j++)
+		{
+			sum=0.0;
+			xi=xi0; k=n; xj0++; Real* xj=xj0;
+			while(k--) { sum += *xi * *xj; xi+=s; xj+=s; }
+			xi=xi0; k=n; xj=xj0;
+			while(k--) { *xj -= sum * *xi; xj+=s; xi+=s; }
+			U.element(i,j) = sum;
+		}
+		xi0++;
+	}
+}
+*/
+
+void QRZ(Matrix& X, UpperTriangularMatrix& U)
+{
+   REPORT
+   Tracer et("QRZ(1)");
+   int n = X.Nrows(); int s = X.Ncols(); U.ReSize(s); U = 0.0;
+   Real* xi0 = X.Store(); Real* u0 = U.Store(); Real* u;
+   int j, k; int J = s; int i = s;
+   while (i--)
+   {
+      Real* xj0 = xi0; Real* xi = xi0; k = n;
+      if (k) for (;;)
+      {
+         u = u0; Real Xi = *xi; Real* xj = xj0;
+         j = J; while(j--) *u++ += Xi * *xj++;
+         if (!(--k)) break;
+         xi += s; xj0 += s;
+      }
+
+      Real sum = sqrt(*u0); *u0 = sum; u = u0+1;
+      if (sum == 0.0) Throw(SingularException(U));
+      int J1 = J-1; j = J1; while(j--) *u++ /= sum;
+
+      xj0 = xi0; xi = xi0++; k = n;
+      if (k) for (;;)
+      {
+         u = u0+1; Real Xi = *xi; Real* xj = xj0;
+         Xi /= sum; *xj++ = Xi;
+         j = J1; while(j--) *xj++ -= *u++ * Xi;
+         if (!(--k)) break;
+	      xi += s; xj0 += s;
+      }
+      u0 += J--;
+   }
+}
+
+void QRZ(const Matrix& X, Matrix& Y, Matrix& M)
+{
+   REPORT
+   Tracer et("QRZ(2)");
+   int n = X.Nrows(); int s = X.Ncols(); int t = Y.Ncols();
+   if (Y.Nrows() != n)
+      { Throw(ProgramException("Unequal column lengths",X,Y)); }
+   M.ReSize(s,t); M = 0;Real* m0 = M.Store(); Real* m;
+   Real* xi0 = X.Store();
+   int j, k; int i = s;
+   while (i--)
+   {
+      Real* xj0 = Y.Store(); Real* xi = xi0; k = n;
+      if (k) for (;;)
+      {
+         m = m0; Real Xi = *xi; Real* xj = xj0;
+         j = t; while(j--) *m++ += Xi * *xj++;
+         if (!(--k)) break;
+         xi += s; xj0 += t;
+      }
+
+      xj0 = Y.Store(); xi = xi0++; k = n;
+      if (k) for (;;)
+      {
+         m = m0; Real Xi = *xi; Real* xj = xj0;
+         j = t; while(j--) *xj++ -= *m++ * Xi;
+         if (!(--k)) break;
+         xi += s; xj0 += t;
+      }
+      m0 += t;
+   }
+}
+
+/*
+
+void QRZ(const Matrix& X, Matrix& Y, Matrix& M)
+{
+	Tracer et("QRZ(2)");
+	int n = X.Nrows(); int s = X.Ncols(); int t = Y.Ncols();
+	if (Y.Nrows() != n)
+	{ Throw(ProgramException("Unequal column lengths",X,Y)); }
+	M.ReSize(s,t);
+	Real* xi0 = X.Store(); int k;
+	for (int i=0; i<s; i++)
+	{
+		Real* xj0 = Y.Store();
+		for (int j=0; j<t; j++)
+		{
+			Real sum=0.0;
+			Real* xi=xi0; Real* xj=xj0; k=n;
+			while(k--) { sum += *xi * *xj; xi+=s; xj+=t; }
+			xi=xi0; k=n; xj=xj0++;
+			while(k--) { *xj -= sum * *xi; xj+=t; xi+=s; }
+			M.element(i,j) = sum;
+		}
+		xi0++;
+	}
+}
+*/
+
+#ifdef use_namespace
+}
+#endif
+
Index: orxonox/trunk/src/util/newmat/include.h
===================================================================
--- orxonox/trunk/src/util/newmat/include.h	(revision 4565)
+++ orxonox/trunk/src/util/newmat/include.h	(revision 4565)
@@ -0,0 +1,336 @@
+//$$ include.h           include files required by various versions of C++
+
+#ifndef INCLUDE_LIB
+#define INCLUDE_LIB
+
+//#define use_namespace                   // define name spaces
+
+//#define SETUP_C_SUBSCRIPTS              // allow element access via A[i][j]
+
+//#define OPT_COMPATIBLE                  // for use with opt++
+
+// Activate just one of the following 3 statements
+
+//#define SimulateExceptions              // use simulated exceptions
+#define UseExceptions                   // use C++ exceptions
+//#define DisableExceptions               // do not use exceptions
+
+
+//#define TEMPS_DESTROYED_QUICKLY         // for compilers that delete
+					// temporaries too quickly
+
+//#define TEMPS_DESTROYED_QUICKLY_R       // the same thing but applied
+					// to return from functions only
+
+//#define DO_FREE_CHECK                   // check news and deletes balance
+
+#define USING_DOUBLE                    // elements of type double
+//#define USING_FLOAT                   // elements of type float
+
+#define bool_LIB 0                      // for compatibility with my older libraries
+
+//#define ios_format_flags ios::fmtflags  // for Gnu 3 and Intel for Linux
+
+
+//#define _STANDARD_                    // using standard library
+
+//#define use_float_h                   // use float.h for precision data
+
+
+//#define HAS_INT64                     // if unsigned _int64 is recognised
+                                        // used by newran03
+                                        
+// comment out next line if Exception causes a problem
+#define TypeDefException
+
+//*********************** end of options set by user ********************
+
+
+// for Gnu C++ version 3
+#if defined __GNUG__ && __GNUG__ >= 3
+   #define _STANDARD_                   // use standard library
+   #define ios_format_flags ios::fmtflags
+#endif
+
+// for Intel C++ for Linux
+#if defined __ICC
+   #define _STANDARD_                   // use standard library
+   #define ios_format_flags ios::fmtflags
+#endif
+
+
+#ifdef _STANDARD_                       // using standard library
+   #include <cstdlib>
+   #ifdef _MSC_VER
+      #include <limits>                 // for VC++6
+   #endif
+   #ifdef WANT_STREAM
+      #include <iostream>
+      #include <iomanip>
+   #endif
+   #ifdef WANT_MATH
+      #include <cmath>
+   #endif
+   #ifdef WANT_STRING
+      #include <cstring>
+   #endif
+   #ifdef WANT_TIME
+      #include <ctime>
+   #endif
+   #ifdef WANT_FSTREAM
+      #include <fstream>
+   #endif
+   using namespace std;
+#else
+
+#define DEFAULT_HEADER                  // use AT&T style header
+                                        // if no other compiler is recognised
+
+#ifdef _MSC_VER                         // Microsoft
+   #include <stdlib.h>
+
+//   reactivate these statements to run under MSC version 7.0
+//   typedef int jmp_buf[9];
+//   extern "C"
+//   {
+//      int __cdecl setjmp(jmp_buf);
+//      void __cdecl longjmp(jmp_buf, int);
+//   }
+
+   #ifdef WANT_STREAM
+      #include <iostream.h>
+      #include <iomanip.h>
+   #endif
+   #ifdef WANT_MATH
+      #include <math.h>
+      #include <float.h>
+   #endif
+   #ifdef WANT_STRING
+      #include <string.h>
+   #endif
+   #ifdef WANT_TIME
+      #include <time.h>
+   #endif
+   #ifdef WANT_FSTREAM
+      #include <fstream.h>
+   #endif
+   #undef DEFAULT_HEADER
+#endif
+
+#ifdef __ZTC__                          // Zortech
+   #include <stdlib.h>
+   #ifdef WANT_STREAM
+      #include <iostream.hpp>
+      #include <iomanip.hpp>
+      #define flush ""                  // not defined in iomanip?
+   #endif
+   #ifdef WANT_MATH
+      #include <math.h>
+      #include <float.h>
+   #endif
+   #ifdef WANT_STRING
+      #include <string.h>
+   #endif
+   #ifdef WANT_TIME
+      #include <time.h>
+   #endif
+   #ifdef WANT_FSTREAM
+      #include <fstream.h>
+   #endif
+   #undef DEFAULT_HEADER
+#endif
+
+#if defined __BCPLUSPLUS__ || defined __TURBOC__  // Borland or Turbo
+   #include <stdlib.h>
+   #ifdef WANT_STREAM
+      #include <iostream.h>
+      #include <iomanip.h>
+   #endif
+   #ifdef WANT_MATH
+      #include <math.h>
+      #include <float.h>            // Borland has both float and values
+                                    // but values.h returns +INF for
+                                    // MAXDOUBLE in BC5
+   #endif
+   #ifdef WANT_STRING
+      #include <string.h>
+   #endif
+   #ifdef WANT_TIME
+      #include <time.h>
+   #endif
+   #ifdef WANT_FSTREAM
+      #include <fstream.h>
+   #endif
+   #undef DEFAULT_HEADER
+#endif
+
+#ifdef __GNUG__                         // Gnu C++
+   #include <stdlib.h>
+   #ifdef WANT_STREAM
+      #include <iostream.h>
+      #include <iomanip.h>
+   #endif
+   #ifdef WANT_MATH
+      #include <math.h>
+      #include <float.h>
+   #endif
+   #ifdef WANT_STRING
+      #include <string.h>
+   #endif
+   #ifdef WANT_TIME
+      #include <time.h>
+   #endif
+   #ifdef WANT_FSTREAM
+      #include <fstream.h>
+   #endif
+   #undef DEFAULT_HEADER
+#endif
+
+#ifdef __WATCOMC__                      // Watcom C/C++
+   #include <stdlib.h>
+   #ifdef WANT_STREAM
+      #include <iostream.h>
+      #include <iomanip.h>
+   #endif
+   #ifdef WANT_MATH
+      #include <math.h>
+      #include <float.h>
+   #endif
+   #ifdef WANT_STRING
+      #include <string.h>
+   #endif
+   #ifdef WANT_TIME
+      #include <time.h>
+   #endif
+   #ifdef WANT_FSTREAM
+      #include <fstream.h>
+   #endif
+   #undef DEFAULT_HEADER
+#endif
+
+
+#ifdef macintosh                        // MPW C++ on the Mac
+#include <stdlib.h>
+#ifdef WANT_STREAM
+#include <iostream.h>
+#include <iomanip.h>
+#endif
+#ifdef WANT_MATH
+#include <float.h>
+#include <math.h>
+#endif
+#ifdef WANT_STRING
+#include <string.h>
+#endif
+#ifdef WANT_TIME
+#include <time.h>
+#endif
+#ifdef WANT_FSTREAM
+#include <fstream.h>
+#endif
+#undef DEFAULT_HEADER
+#endif
+
+#ifdef use_float_h                      // use float.h for precision values
+#include <stdlib.h>
+#ifdef WANT_STREAM
+#include <iostream.h>
+#include <iomanip.h>
+#endif
+#ifdef WANT_MATH
+#include <float.h>
+#include <math.h>
+#endif
+#ifdef WANT_STRING
+#include <string.h>
+#endif
+#ifdef WANT_TIME
+#include <time.h>
+#endif
+#ifdef WANT_FSTREAM
+#include <fstream.h>
+#endif
+#undef DEFAULT_HEADER
+#endif
+
+
+#ifdef DEFAULT_HEADER                   // for example AT&T
+#define ATandT
+#include <stdlib.h>
+#ifdef WANT_STREAM
+#include <iostream.h>
+#include <iomanip.h>
+#endif
+#ifdef WANT_MATH
+#include <math.h>
+#define SystemV                         // use System V
+#include <values.h>
+#endif
+#ifdef WANT_STRING
+#include <string.h>
+#endif
+#ifdef WANT_TIME
+#include <time.h>
+#endif
+#ifdef WANT_FSTREAM
+#include <fstream.h>
+#endif
+#endif                                  // DEFAULT_HEADER
+
+#endif                                  // _STANDARD_
+
+#ifdef use_namespace
+namespace RBD_COMMON {
+#endif
+
+
+#ifdef USING_FLOAT                      // set precision type to float
+typedef float Real;
+typedef double long_Real;
+#endif
+
+#ifdef USING_DOUBLE                     // set precision type to double
+typedef double Real;
+typedef long double long_Real;
+#endif
+
+
+// This is for (very old) compilers that do not have bool automatically defined
+
+#ifndef bool_LIB
+#define bool_LIB 0
+
+class bool
+{
+	int value;
+public:
+	bool(const int b) { value = b ? 1 : 0; }
+	bool(const void* b) { value = b ? 1 : 0; }
+	bool() {}
+	operator int() const { return value; }
+	int operator!() const { return !value; }
+};
+
+
+const bool true = 1;
+const bool false = 0;
+
+#endif
+
+
+
+#ifdef use_namespace
+}
+#endif
+
+
+#ifdef use_namespace
+namespace RBD_COMMON {}
+namespace RBD_LIBRARIES                 // access all my libraries
+{
+   using namespace RBD_COMMON;
+}
+#endif
+
+
+#endif
Index: orxonox/trunk/src/util/newmat/jacobi.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/jacobi.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/jacobi.cpp	(revision 4565)
@@ -0,0 +1,123 @@
+//$$jacobi.cpp                           jacobi eigenvalue analysis
+
+// Copyright (C) 1991,2,3,4: R B Davies
+
+
+//#define WANT_STREAM
+
+
+#define WANT_MATH
+
+#include "include.h"
+#include "newmatap.h"
+#include "precisio.h"
+#include "newmatrm.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+#ifdef DO_REPORT
+#define REPORT { static ExeCounter ExeCount(__LINE__,18); ++ExeCount; }
+#else
+#define REPORT {}
+#endif
+
+
+void Jacobi(const SymmetricMatrix& X, DiagonalMatrix& D, SymmetricMatrix& A,
+   Matrix& V, bool eivec)
+{
+   Real epsilon = FloatingPointPrecision::Epsilon();
+   Tracer et("Jacobi");
+   REPORT
+   int n = X.Nrows(); DiagonalMatrix B(n), Z(n); D.ReSize(n); A = X;
+   if (eivec) { REPORT V.ReSize(n,n); D = 1.0; V = D; }
+   B << A; D = B; Z = 0.0; A.Inject(Z);
+   bool converged = false;
+   for (int i=1; i<=50; i++)
+   {
+      Real sm=0.0; Real* a = A.Store(); int p = A.Storage();
+      while (p--) sm += fabs(*a++);            // have previously zeroed diags
+      if (sm==0.0) { REPORT converged = true; break; }
+      Real tresh = (i<4) ? 0.2 * sm / square(n) : 0.0; a = A.Store();
+      for (p = 0; p < n; p++)
+      {
+         Real* ap1 = a + (p*(p+1))/2;
+         Real& zp = Z.element(p); Real& dp = D.element(p);
+         for (int q = p+1; q < n; q++)
+         {
+            Real* ap = ap1; Real* aq = a + (q*(q+1))/2;
+            Real& zq = Z.element(q); Real& dq = D.element(q);
+            Real& apq = A.element(q,p);
+            Real g = 100 * fabs(apq); Real adp = fabs(dp); Real adq = fabs(dq);
+
+            if (i>4 && g < epsilon*adp && g < epsilon*adq) { REPORT apq = 0.0; }
+            else if (fabs(apq) > tresh)
+            {
+               REPORT
+               Real t; Real h = dq - dp; Real ah = fabs(h);
+               if (g < epsilon*ah) { REPORT t = apq / h; }
+               else
+               {
+                  REPORT
+                  Real theta = 0.5 * h / apq;
+                  t = 1.0 / ( fabs(theta) + sqrt(1.0 + square(theta)) );
+                  if (theta<0.0) { REPORT t = -t; }
+               }
+               Real c = 1.0 / sqrt(1.0 + square(t)); Real s = t * c;
+               Real tau = s / (1.0 + c); h = t * apq;
+               zp -= h; zq += h; dp -= h; dq += h; apq = 0.0;
+               int j = p;
+               while (j--)
+               {
+                  g = *ap; h = *aq;
+                  *ap++ = g-s*(h+g*tau); *aq++ = h+s*(g-h*tau);
+               }
+               int ip = p+1; j = q-ip; ap += ip++; aq++;
+               while (j--)
+               {
+                  g = *ap; h = *aq;
+                  *ap = g-s*(h+g*tau); *aq++ = h+s*(g-h*tau);
+                  ap += ip++;
+               }
+               if (q < n-1)             // last loop is non-empty
+               {
+                  int iq = q+1; j = n-iq; ap += ip++; aq += iq++;
+                  for (;;)
+                  {
+                     g = *ap; h = *aq;
+                     *ap = g-s*(h+g*tau); *aq = h+s*(g-h*tau);
+                     if (!(--j)) break;
+                     ap += ip++; aq += iq++;
+                  }
+               }
+               if (eivec)
+               {
+                  REPORT
+                  RectMatrixCol VP(V,p); RectMatrixCol VQ(V,q);
+                  Rotate(VP, VQ, tau, s);
+               }
+            }
+         }
+      }
+      B = B + Z; D = B; Z = 0.0;
+   }
+   if (!converged) Throw(ConvergenceException(X));
+   if (eivec) SortSV(D, V, true);
+   else SortAscending(D);
+}
+
+void Jacobi(const SymmetricMatrix& X, DiagonalMatrix& D)
+{ REPORT SymmetricMatrix A; Matrix V; Jacobi(X,D,A,V,false); }
+
+void Jacobi(const SymmetricMatrix& X, DiagonalMatrix& D, SymmetricMatrix& A)
+{ REPORT Matrix V; Jacobi(X,D,A,V,false); }
+
+void Jacobi(const SymmetricMatrix& X, DiagonalMatrix& D, Matrix& V)
+{ REPORT SymmetricMatrix A; Jacobi(X,D,A,V,true); }
+
+
+#ifdef use_namespace
+}
+#endif
+
Index: orxonox/trunk/src/util/newmat/myexcept.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/myexcept.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/myexcept.cpp	(revision 4565)
@@ -0,0 +1,485 @@
+//$$myexcept.cpp                        Exception handler
+
+// Copyright (C) 1993,4,6: R B Davies
+
+
+#define WANT_STREAM                    // include.h will get stream fns
+#define WANT_STRING
+
+#include "include.h"                   // include standard files
+#include "boolean.h"
+
+
+#include "myexcept.h"                  // for exception handling
+
+#ifdef use_namespace
+namespace RBD_COMMON {
+#endif
+
+
+//#define REG_DEREG                    // for print out uses of new/delete
+//#define CLEAN_LIST                   // to print entries being added to
+                                       // or deleted from cleanup list
+
+#ifdef SimulateExceptions
+
+void Throw()
+{
+   for (Janitor* jan = JumpBase::jl->janitor; jan; jan = jan->NextJanitor)
+      jan->CleanUp();
+   JumpItem* jx = JumpBase::jl->ji;    // previous jumpbase;
+   if ( !jx ) { Terminate(); }         // jl was initial JumpItem
+   JumpBase::jl = jx;                  // drop down a level; cannot be in front
+                                       // of previous line
+   Tracer::last = JumpBase::jl->trace;
+   longjmp(JumpBase::jl->env, 1);
+}
+
+#endif                                 // end of simulate exceptions
+
+
+unsigned long Exception::Select;
+char* Exception::what_error;
+int Exception::SoFar;
+int Exception::LastOne;
+
+Exception::Exception(const char* a_what)
+{
+   Select++; SoFar = 0;
+   if (!what_error)                   // make space for exception message
+   {
+      LastOne = 511;
+      what_error = new char[512];
+      if (!what_error)                // fail to make space
+      {
+         LastOne = 0;
+         what_error = (char *)"No heap space for exception message\n";
+      }
+   }
+   AddMessage("\n\nAn exception has been thrown\n");
+   AddMessage(a_what);
+   if (a_what) Tracer::AddTrace();
+}
+
+void Exception::AddMessage(const char* a_what)
+{
+   if (a_what)
+   {
+      int l = strlen(a_what); int r = LastOne - SoFar;
+      if (l < r) { strcpy(what_error+SoFar, a_what); SoFar += l; }
+      else if (r > 0)
+      {
+         strncpy(what_error+SoFar, a_what, r);
+         what_error[LastOne] = 0;
+         SoFar = LastOne;
+      }
+   }
+}
+
+void Exception::AddInt(int value)
+{
+   bool negative;
+   if (value == 0) { AddMessage("0"); return; }
+   else if (value < 0) { value = -value; negative = true; }
+   else negative = false;
+   int n = 0; int v = value;        // how many digits will we need?
+   while (v > 0) { v /= 10; n++; }
+   if (negative) n++;
+   if (LastOne-SoFar < n) { AddMessage("***"); return; }
+
+   SoFar += n; n = SoFar; what_error[n] = 0;
+   while (value > 0)
+   {
+      int nv = value / 10; int rm = value - nv * 10;  value = nv;
+      what_error[--n] = (char)(rm + '0');
+   }
+   if (negative) what_error[--n] = '-';
+   return;
+}
+
+void Tracer::PrintTrace()
+{
+   cout << "\n";
+   for (Tracer* et = last; et; et=et->previous)
+      cout << "  * " << et->entry << "\n";
+}
+
+void Tracer::AddTrace()
+{
+   if (last)
+   {
+      Exception::AddMessage("Trace: ");
+      Exception::AddMessage(last->entry);
+      for (Tracer* et = last->previous; et; et=et->previous)
+      {
+         Exception::AddMessage("; ");
+         Exception::AddMessage(et->entry);
+      }
+      Exception::AddMessage(".\n");
+   }
+}
+
+#ifdef SimulateExceptions
+
+
+Janitor::Janitor()
+{
+   if (do_not_link)
+   {
+      do_not_link = false; NextJanitor = 0; OnStack = false;
+#ifdef CLEAN_LIST
+      cout << "Not added to clean-list " << (unsigned long)this << "\n";
+#endif
+   }
+   else
+   {
+      OnStack = true;
+#ifdef CLEAN_LIST
+      cout << "Add to       clean-list " << (unsigned long)this << "\n";
+#endif
+      NextJanitor = JumpBase::jl->janitor; JumpBase::jl->janitor=this;
+   }
+}
+
+Janitor::~Janitor()
+{
+   // expect the item to be deleted to be first on list
+   // but must be prepared to search list
+   if (OnStack)
+   {
+#ifdef CLEAN_LIST
+      cout << "Delete from  clean-list " << (unsigned long)this << "\n";
+#endif
+      Janitor* lastjan = JumpBase::jl->janitor;
+      if (this == lastjan) JumpBase::jl->janitor = NextJanitor;
+      else
+      {
+	 for (Janitor* jan = lastjan->NextJanitor; jan;
+	    jan = lastjan->NextJanitor)
+	 {
+	    if (jan==this)
+	       { lastjan->NextJanitor = jan->NextJanitor; return; }
+	    lastjan=jan;
+	 }
+
+	 Throw(Exception(
+"Cannot resolve memory linked list\nSee notes in myexcept.cpp for details\n"
+         ));
+
+
+// This message occurs when a call to ~Janitor() occurs, apparently
+// without a corresponding call to Janitor(). This could happen if my
+// way of deciding whether a constructor is being called by new
+// fails.
+
+// It may happen if you are using my simulated exceptions and also have
+// your compiler s exceptions turned on.
+
+// It can also happen if you have a class derived from Janitor
+// which does not include a copy constructor [ eg X(const &X) ].
+// Possibly also if delete is applied an object on the stack (ie not
+// called by new). Otherwise, it is a bug in myexcept or your compiler.
+// If you do not #define TEMPS_DESTROYED_QUICKLY you will get this
+// error with Microsoft C 7.0. There are probably situations where
+// you will get this when you do define TEMPS_DESTROYED_QUICKLY. This
+// is a bug in MSC. Beware of "operator" statements for defining
+// conversions; particularly for converting from a Base class to a
+// Derived class.
+
+// You may get away with simply deleting this error message and Throw
+// statement if you can not find a better way of overcoming the
+// problem. In any case please tell me if you get this error message,
+// particularly for compilers apart from Microsoft C 7.0.
+
+
+      }
+   }
+}
+
+JumpItem* JumpBase::jl;              // will be set to zero
+jmp_buf JumpBase::env;
+bool Janitor::do_not_link;           // will be set to false
+
+
+int JanitorInitializer::ref_count;
+
+JanitorInitializer::JanitorInitializer()
+{
+   if (ref_count++ == 0) new JumpItem;
+                                    // need JumpItem at head of list
+}
+
+#endif                              // end of SimulateExceptions
+
+Tracer* Tracer::last;               // will be set to zero
+
+
+void Terminate()
+{
+   cout << "\n\nThere has been an exception with no handler - exiting";
+   const char* what = Exception::what();
+   if (what) cout << what << "\n";
+   exit(1);
+}
+
+
+
+#ifdef DO_FREE_CHECK
+// Routines for tracing whether new and delete calls are balanced
+
+FreeCheckLink::FreeCheckLink() : next(FreeCheck::next)
+   { FreeCheck::next = this; }
+
+FCLClass::FCLClass(void* t, char* name) : ClassName(name) { ClassStore=t; }
+
+FCLRealArray::FCLRealArray(void* t, char* o, int s)
+  : Operation(o), size(s) { ClassStore=t; }
+
+FCLIntArray::FCLIntArray(void* t, char* o, int s)
+  : Operation(o), size(s) { ClassStore=t; }
+
+FreeCheckLink* FreeCheck::next;
+int FreeCheck::BadDelete;
+
+void FCLClass::Report()
+{ cout << "   " << ClassName << "   " << (unsigned long)ClassStore << "\n"; }
+
+void FCLRealArray::Report()
+{
+   cout << "   " << Operation << "   " << (unsigned long)ClassStore <<
+      "   " << size << "\n";
+}
+
+void FCLIntArray::Report()
+{
+   cout << "   " << Operation << "   " << (unsigned long)ClassStore <<
+      "   " << size << "\n";
+}
+
+void FreeCheck::Register(void* t, char* name)
+{
+   FCLClass* f = new FCLClass(t,name);
+   if (!f) { cout << "Out of memory in FreeCheck\n"; exit(1); }
+#ifdef REG_DEREG
+   cout << "Registering   " << name << "   " << (unsigned long)t << "\n";
+#endif
+}
+
+void FreeCheck::RegisterR(void* t, char* o, int s)
+{
+   FCLRealArray* f = new FCLRealArray(t,o,s);
+   if (!f) { cout << "Out of memory in FreeCheck\n"; exit(1); }
+#ifdef REG_DEREG
+   cout << o << "   " << s << "   " << (unsigned long)t << "\n";
+#endif
+}
+
+void FreeCheck::RegisterI(void* t, char* o, int s)
+{
+   FCLIntArray* f = new FCLIntArray(t,o,s);
+   if (!f) { cout << "Out of memory in FreeCheck\n"; exit(1); }
+#ifdef REG_DEREG
+   cout << o << "   " << s << "   " << (unsigned long)t << "\n";
+#endif
+}
+
+void FreeCheck::DeRegister(void* t, char* name)
+{
+   FreeCheckLink* last = 0;
+#ifdef REG_DEREG
+   cout << "Deregistering " << name << "   " << (unsigned long)t << "\n";
+#endif
+   for (FreeCheckLink* fcl = next; fcl; fcl = fcl->next)
+   {
+      if (fcl->ClassStore==t)
+      {
+	 if (last) last->next = fcl->next; else next = fcl->next;
+	 delete fcl; return;
+      }
+      last = fcl;
+   }
+   cout << "\nRequest to delete non-existent object of class and location:\n";
+   cout << "   " << name << "   " << (unsigned long)t << "\n";
+   BadDelete++;
+   Tracer::PrintTrace();
+   cout << "\n";
+}
+
+void FreeCheck::DeRegisterR(void* t, char* o, int s)
+{
+   FreeCheckLink* last = 0;
+#ifdef REG_DEREG
+   cout << o << "   " << s << "   " << (unsigned long)t << "\n";
+#endif
+   for (FreeCheckLink* fcl = next; fcl; fcl = fcl->next)
+   {
+      if (fcl->ClassStore==t)
+      {
+	 if (last) last->next = fcl->next; else next = fcl->next;
+	 if (s >= 0 && ((FCLRealArray*)fcl)->size != s)
+	 {
+	    cout << "\nArray sizes do not agree:\n";
+	    cout << "   " << o << "   " << (unsigned long)t
+	       << "   " << ((FCLRealArray*)fcl)->size << "   " << s << "\n";
+	    Tracer::PrintTrace();
+	    cout << "\n";
+	 }
+	 delete fcl; return;
+      }
+      last = fcl;
+   }
+   cout << "\nRequest to delete non-existent real array:\n";
+   cout << "   " << o << "   " << (unsigned long)t << "   " << s << "\n";
+   BadDelete++;
+   Tracer::PrintTrace();
+   cout << "\n";
+}
+
+void FreeCheck::DeRegisterI(void* t, char* o, int s)
+{
+   FreeCheckLink* last = 0;
+#ifdef REG_DEREG
+   cout << o << "   " << s << "   " << (unsigned long)t << "\n";
+#endif
+   for (FreeCheckLink* fcl = next; fcl; fcl = fcl->next)
+   {
+      if (fcl->ClassStore==t)
+      {
+	 if (last) last->next = fcl->next; else next = fcl->next;
+	 if (s >= 0 && ((FCLIntArray*)fcl)->size != s)
+	 {
+	    cout << "\nArray sizes do not agree:\n";
+	    cout << "   " << o << "   " << (unsigned long)t
+	       << "   " << ((FCLIntArray*)fcl)->size << "   " << s << "\n";
+	    Tracer::PrintTrace();
+	    cout << "\n";
+	 }
+	 delete fcl; return;
+      }
+      last = fcl;
+   }
+   cout << "\nRequest to delete non-existent int array:\n";
+   cout << "   " << o << "   " << (unsigned long)t << "   " << s << "\n";
+   BadDelete++;
+   Tracer::PrintTrace();
+   cout << "\n";
+}
+
+void FreeCheck::Status()
+{
+   if (next)
+   {
+      cout << "\nObjects of the following classes remain undeleted:\n";
+      for (FreeCheckLink* fcl = next; fcl; fcl = fcl->next) fcl->Report();
+      cout << "\n";
+   }
+   else cout << "\nNo objects remain undeleted\n\n";
+   if (BadDelete)
+   {
+      cout << "\nThere were " << BadDelete << 
+         " requests to delete non-existent items\n\n";
+   }
+}
+
+#endif                            // end of DO_FREE_CHECK
+
+// derived exception bodies
+
+Logic_error::Logic_error(const char* a_what) : Exception()
+{
+   Select = Exception::Select;
+   AddMessage("Logic error:- "); AddMessage(a_what);
+   if (a_what) Tracer::AddTrace();
+}
+
+Runtime_error::Runtime_error(const char* a_what)
+   : Exception()
+{
+   Select = Exception::Select;
+   AddMessage("Runtime error:- "); AddMessage(a_what);
+   if (a_what) Tracer::AddTrace();
+}
+
+Domain_error::Domain_error(const char* a_what) : Logic_error()
+{
+   Select = Exception::Select;
+   AddMessage("domain error\n"); AddMessage(a_what);
+   if (a_what) Tracer::AddTrace();
+}
+
+Invalid_argument::Invalid_argument(const char* a_what) : Logic_error()
+{
+   Select = Exception::Select;
+   AddMessage("invalid argument\n"); AddMessage(a_what);
+   if (a_what) Tracer::AddTrace();
+}
+
+Length_error::Length_error(const char* a_what) : Logic_error()
+{
+   Select = Exception::Select;
+   AddMessage("length error\n"); AddMessage(a_what);
+   if (a_what) Tracer::AddTrace();
+}
+
+Out_of_range::Out_of_range(const char* a_what) : Logic_error()
+{
+   Select = Exception::Select;
+   AddMessage("out of range\n"); AddMessage(a_what);
+   if (a_what) Tracer::AddTrace();
+}
+
+//Bad_cast::Bad_cast(const char* a_what) : Logic_error()
+//{
+//   Select = Exception::Select;
+//   AddMessage("bad cast\n"); AddMessage(a_what);
+//   if (a_what) Tracer::AddTrace();
+//}
+
+//Bad_typeid::Bad_typeid(const char* a_what) : Logic_error()
+//{
+//   Select = Exception::Select;
+//   AddMessage("bad type id.\n"); AddMessage(a_what);
+//   if (a_what) Tracer::AddTrace();
+//}
+
+Range_error::Range_error(const char* a_what) : Runtime_error()
+{
+   Select = Exception::Select;
+   AddMessage("range error\n"); AddMessage(a_what);
+   if (a_what) Tracer::AddTrace();
+}
+
+Overflow_error::Overflow_error(const char* a_what) : Runtime_error()
+{
+   Select = Exception::Select;
+   AddMessage("overflow error\n"); AddMessage(a_what);
+   if (a_what) Tracer::AddTrace();
+}
+
+Bad_alloc::Bad_alloc(const char* a_what) : Exception()
+{
+   Select = Exception::Select;
+   AddMessage("bad allocation\n"); AddMessage(a_what);
+   if (a_what) Tracer::AddTrace();
+}
+
+
+
+
+unsigned long Logic_error::Select;
+unsigned long Runtime_error::Select;
+unsigned long Domain_error::Select;
+unsigned long Invalid_argument::Select;
+unsigned long Length_error::Select;
+unsigned long Out_of_range::Select;
+//unsigned long Bad_cast::Select;
+//unsigned long Bad_typeid::Select;
+unsigned long Range_error::Select;
+unsigned long Overflow_error::Select;
+unsigned long Bad_alloc::Select;
+
+#ifdef use_namespace
+}
+#endif
+
+
Index: orxonox/trunk/src/util/newmat/myexcept.h
===================================================================
--- orxonox/trunk/src/util/newmat/myexcept.h	(revision 4565)
+++ orxonox/trunk/src/util/newmat/myexcept.h	(revision 4565)
@@ -0,0 +1,430 @@
+//$$ myexcept.h                                  Exception handling classes
+
+
+// A set of classes to simulate exceptions in C++
+//
+//   Partially copied from Carlos Vidal s article in the C users  journal
+//   September 1992, pp 19-28
+//
+//   Operations defined
+//      Try {     }
+//      Throw ( exception object )
+//      ReThrow
+//      Catch ( exception class ) {      }
+//      CatchAll {      }
+//      CatchAndThrow
+//
+//   All catch lists must end with a CatchAll or CatchAndThrow statement
+//   but not both.
+//
+//   When exceptions are finally implemented replace Try, Throw(E), Rethrow,
+//   Catch, CatchAll, CatchAndThrow by try, throw E, throw, catch,
+//   catch(...), and {}.
+//
+//   All exception classes must be derived from Exception, have no non-static
+//   variables and must include the statement
+//
+//      static unsigned long Select;
+//
+//   Any constructor in one of these exception classes must include
+//
+//      Select = Exception::Select;
+//
+//   For each exceptions class, EX_1, some .cpp file must include
+//
+//      unsigned long EX_1::Select;
+//
+
+
+#ifndef EXCEPTION_LIB
+#define EXCEPTION_LIB
+
+#ifdef use_namespace
+namespace RBD_COMMON {
+#endif
+
+
+void Terminate();
+
+
+//********** classes for setting up exceptions and reporting ************//
+
+class Exception;
+
+class Tracer                             // linked list showing how
+{                                        // we got here
+   const char* entry;
+   Tracer* previous;
+public:
+   Tracer(const char*);
+   ~Tracer();
+   void ReName(const char*);
+   static void PrintTrace();             // for printing trace
+   static void AddTrace();               // insert trace in exception record
+   static Tracer* last;                  // points to Tracer list
+   friend class Exception;
+};
+
+
+class Exception                          // The base exception class
+{
+protected:
+   static char* what_error;              // error message
+   static int SoFar;                     // no. characters already entered
+   static int LastOne;                   // last location in error buffer
+public:
+   static void AddMessage(const char* a_what);
+                                         // messages about exception
+   static void AddInt(int value);        // integer to error message
+   static unsigned long Select;          // for identifying exception
+   Exception(const char* a_what = 0);
+   static const char* what() { return what_error; }
+                                         // for getting error message
+};
+
+
+inline Tracer::Tracer(const char* e)
+   : entry(e), previous(last) { last = this; }
+
+inline Tracer::~Tracer() { last = previous; }
+
+inline void Tracer::ReName(const char* e) { entry=e; }
+
+#ifdef SimulateExceptions                // SimulateExceptions
+
+#include <setjmp.h>
+
+
+//************* the definitions of Try, Throw and Catch *****************//
+
+
+class JumpItem;
+class Janitor;
+
+class JumpBase         // pointer to a linked list of jmp_buf s
+{
+public:
+   static JumpItem *jl;
+   static jmp_buf env;
+};
+
+class JumpItem         // an item in a linked list of jmp_buf s
+{
+public:
+   JumpItem *ji;
+   jmp_buf env;
+   Tracer* trace;                     // to keep check on Tracer items
+   Janitor* janitor;                  // list of items for cleanup
+   JumpItem() : ji(JumpBase::jl), trace(0), janitor(0)
+      { JumpBase::jl = this; }
+   ~JumpItem() { JumpBase::jl = ji; }
+};
+
+void Throw();
+
+inline void Throw(const Exception&) { Throw(); }
+
+#define Try                                             \
+   if (!setjmp( JumpBase::jl->env )) {                  \
+   JumpBase::jl->trace = Tracer::last;               \
+   JumpItem JI387256156;
+
+#define ReThrow Throw()
+
+#define Catch(EXCEPTION)                                \
+   } else if (Exception::Select == EXCEPTION::Select) {
+
+#define CatchAll } else
+
+#define CatchAndThrow  } else Throw();
+
+
+//****************** cleanup heap following Throw ***********************//
+
+class Janitor
+{
+protected:
+   static bool do_not_link;                  // set when new is called
+   bool OnStack;                             // false if created by new
+public:
+   Janitor* NextJanitor;
+   virtual void CleanUp() {}
+   Janitor();
+   virtual ~Janitor();
+};
+
+
+// The tiresome old trick for initializing the Janitor class
+// this is needed for classes derived from Janitor which have objects
+// declared globally
+
+class JanitorInitializer
+{
+public:
+   JanitorInitializer();
+private:
+   static int ref_count;
+};
+
+static JanitorInitializer JanInit;
+
+#endif                                // end of SimulateExceptions
+
+#ifdef UseExceptions
+
+#define Try try
+#define Throw(E) throw E
+#define ReThrow throw
+#define Catch catch
+#define CatchAll catch(...)
+#define CatchAndThrow {}
+
+#endif                                // end of UseExceptions
+
+
+#ifdef DisableExceptions              // Disable exceptions
+
+#define Try {
+#define ReThrow Throw()
+#define Catch(EXCEPTION) } if (false) {
+#define CatchAll } if (false)
+#define CatchAndThrow }
+
+inline void Throw() { Terminate(); }
+inline void Throw(const Exception&) { Terminate(); }
+
+
+#endif                                // end of DisableExceptions
+
+#ifndef SimulateExceptions            // ! SimulateExceptions
+
+class Janitor                         // a dummy version
+{
+public:
+   virtual void CleanUp() {}
+   Janitor() {}
+   virtual ~Janitor() {}
+};
+
+#endif                                // end of ! SimulateExceptions
+
+
+//******************** FREE_CHECK and NEW_DELETE ***********************//
+
+#ifdef DO_FREE_CHECK                          // DO_FREE_CHECK
+// Routines for tracing whether new and delete calls are balanced
+
+class FreeCheck;
+
+class FreeCheckLink
+{
+protected:
+   FreeCheckLink* next;
+   void* ClassStore;
+   FreeCheckLink();
+   virtual void Report()=0;                   // print details of link
+   friend class FreeCheck;
+};
+
+class FCLClass : public FreeCheckLink         // for registering objects
+{
+   char* ClassName;
+   FCLClass(void* t, char* name);
+   void Report();
+   friend class FreeCheck;
+};
+
+class FCLRealArray : public FreeCheckLink     // for registering real arrays
+{
+   char* Operation;
+   int size;
+   FCLRealArray(void* t, char* o, int s);
+   void Report();
+   friend class FreeCheck;
+};
+
+class FCLIntArray : public FreeCheckLink     // for registering int arrays
+{
+   char* Operation;
+   int size;
+   FCLIntArray(void* t, char* o, int s);
+   void Report();
+   friend class FreeCheck;
+};
+
+
+class FreeCheck
+{
+   static FreeCheckLink* next;
+   static int BadDelete;
+public:
+   static void Register(void*, char*);
+   static void DeRegister(void*, char*);
+   static void RegisterR(void*, char*, int);
+   static void DeRegisterR(void*, char*, int);
+   static void RegisterI(void*, char*, int);
+   static void DeRegisterI(void*, char*, int);
+   static void Status();
+   friend class FreeCheckLink;
+   friend class FCLClass;
+   friend class FCLRealArray;
+   friend class FCLIntArray;
+};
+
+#define FREE_CHECK(Class)                                                  \
+public:                                                                    \
+   void* operator new(size_t size)                                         \
+   {                                                                       \
+      void* t = ::operator new(size); FreeCheck::Register(t,#Class);       \
+      return t;                                                            \
+   }                                                                       \
+   void operator delete(void* t)                                           \
+   { FreeCheck::DeRegister(t,#Class); ::operator delete(t); }
+
+
+#ifdef SimulateExceptions         // SimulateExceptions
+
+#define NEW_DELETE(Class)                                                  \
+public:                                                                    \
+   void* operator new(size_t size)                                         \
+   {                                                                       \
+      do_not_link=true;                                                    \
+      void* t = ::operator new(size); FreeCheck::Register(t,#Class);       \
+      return t;                                                            \
+   }                                                                       \
+   void operator delete(void* t)                                           \
+   { FreeCheck::DeRegister(t,#Class); ::operator delete(t); }
+
+
+#endif                           // end of SimulateExceptions
+
+
+#define MONITOR_REAL_NEW(Operation, Size, Pointer)                         \
+	FreeCheck::RegisterR(Pointer, Operation, Size);
+#define MONITOR_INT_NEW(Operation, Size, Pointer)                          \
+	FreeCheck::RegisterI(Pointer, Operation, Size);
+#define MONITOR_REAL_DELETE(Operation, Size, Pointer)                      \
+	FreeCheck::DeRegisterR(Pointer, Operation, Size);
+#define MONITOR_INT_DELETE(Operation, Size, Pointer)                       \
+	FreeCheck::DeRegisterI(Pointer, Operation, Size);
+
+#else                            // DO_FREE_CHECK not defined
+
+#define FREE_CHECK(Class) public:
+#define MONITOR_REAL_NEW(Operation, Size, Pointer) {}
+#define MONITOR_INT_NEW(Operation, Size, Pointer) {}
+#define MONITOR_REAL_DELETE(Operation, Size, Pointer) {}
+#define MONITOR_INT_DELETE(Operation, Size, Pointer) {}
+
+
+#ifdef SimulateExceptions         // SimulateExceptions
+
+
+#define NEW_DELETE(Class)                                                  \
+public:                                                                    \
+	void* operator new(size_t size)                                    \
+	{ do_not_link=true; void* t = ::operator new(size); return t; }    \
+	void operator delete(void* t) { ::operator delete(t); }
+
+#endif                            // end of SimulateExceptions
+
+#endif                            // end of ! DO_FREE_CHECK
+
+#ifndef SimulateExceptions        // ! SimulateExceptions
+
+#define NEW_DELETE(Class) FREE_CHECK(Class)
+
+#endif                            // end of ! SimulateExceptions
+
+
+//********************* derived exceptions ******************************//
+
+class Logic_error : public Exception
+{
+public:
+   static unsigned long Select;
+   Logic_error(const char* a_what = 0);
+};
+
+class Runtime_error : public Exception
+{
+public:
+   static unsigned long Select;
+   Runtime_error(const char* a_what = 0);
+};
+
+class Domain_error : public Logic_error
+{
+public:
+   static unsigned long Select;
+   Domain_error(const char* a_what = 0);
+};
+
+class Invalid_argument : public Logic_error
+{
+public:
+   static unsigned long Select;
+   Invalid_argument(const char* a_what = 0);
+};
+
+class Length_error : public Logic_error
+{
+public:
+   static unsigned long Select;
+   Length_error(const char* a_what = 0);
+};
+
+class Out_of_range : public Logic_error
+{
+public:
+   static unsigned long Select;
+   Out_of_range(const char* a_what = 0);
+};
+
+//class Bad_cast : public Logic_error
+//{
+//public:
+//   static unsigned long Select;
+//   Bad_cast(const char* a_what = 0);
+//};
+
+//class Bad_typeid : public Logic_error
+//{
+//public:
+//   static unsigned long Select;
+//   Bad_typeid(const char* a_what = 0);
+//};
+
+class Range_error : public Runtime_error
+{
+public:
+   static unsigned long Select;
+   Range_error(const char* a_what = 0);
+};
+
+class Overflow_error : public Runtime_error
+{
+public:
+   static unsigned long Select;
+   Overflow_error(const char* a_what = 0);
+};
+
+class Bad_alloc : public Exception
+{
+public:
+   static unsigned long Select;
+   Bad_alloc(const char* a_what = 0);
+};
+
+#ifdef use_namespace
+}
+#endif
+
+
+#endif                            // end of EXCEPTION_LIB
+
+
+// body file: myexcept.cpp
+
+
+
Index: orxonox/trunk/src/util/newmat/newfft.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/newfft.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/newfft.cpp	(revision 4565)
@@ -0,0 +1,1059 @@
+//$ newfft.cpp
+
+// This is originally by Sande and Gentleman in 1967! I have translated from
+// Fortran into C and a little bit of C++.
+
+// It takes about twice as long as fftw
+// (http://theory.lcs.mit.edu/~fftw/homepage.html)
+// but is much shorter than fftw  and so despite its age
+// might represent a reasonable
+// compromise between speed and complexity.
+// If you really need the speed get fftw.
+
+
+//    THIS SUBROUTINE WAS WRITTEN BY G.SANDE OF PRINCETON UNIVERSITY AND
+//    W.M.GENTLMAN OF THE BELL TELEPHONE LAB.  IT WAS BROUGHT TO LONDON
+//    BY DR. M.D. GODFREY AT THE IMPERIAL COLLEGE AND WAS ADAPTED FOR
+//    BURROUGHS 6700 BY D. R. BRILLINGER AND J. PEMBERTON
+//    IT REPRESENTS THE STATE OF THE ART OF COMPUTING COMPLETE FINITE
+//    DISCRETE FOURIER TRANSFORMS AS OF NOV.1967.
+//    OTHER PROGRAMS REQUIRED.
+//                                 ONLY THOSE SUBROUTINES INCLUDED HERE.
+//                      USAGE.
+//       CALL AR1DFT(N,X,Y)
+//            WHERE  N IS THE NUMBER OF POINTS IN THE SEQUENCE .
+//                   X - IS A ONE-DIMENSIONAL ARRAY CONTAINING THE REAL
+//                       PART OF THE SEQUENCE.
+//                   Y - IS A ONE-DIMENSIONAL ARRAY CONTAINING THE
+//                       IMAGINARY PART OF THE SEQUENCE.
+//    THE TRANSFORM IS RETURNED IN X AND Y.
+//            METHOD
+//               FOR A GENERAL DISCUSSION OF THESE TRANSFORMS AND OF
+//    THE FAST METHOD FOR COMPUTING THEM, SEE GENTLEMAN AND SANDE,
+//    @FAST FOURIER TRANSFORMS - FOR FUN AND PROFIT,@ 1966 FALL JOINT
+//    COMPUTER CONFERENCE.
+//    THIS PROGRAM COMPUTES THIS FOR A COMPLEX SEQUENCE Z(T) OF LENGTH
+//    N WHOSE ELEMENTS ARE STORED AT(X(I) , Y(I)) AND RETURNS THE
+//    TRANSFORM COEFFICIENTS AT (X(I), Y(I)).
+//        DESCRIPTION
+//    AR1DFT IS A HIGHLY MODULAR ROUTINE CAPABLE OF COMPUTING IN PLACE
+//    THE COMPLETE FINITE DISCRETE FOURIER TRANSFORM  OF A ONE-
+//    DIMENSIONAL SEQUENCE OF RATHER GENERAL LENGTH N.
+//       THE MAIN ROUTINE , AR1DFT ITSELF, FACTORS N. IT THEN CALLS ON
+//    ON GR 1D FT TO COMPUTE THE ACTUAL TRANSFORMS, USING THESE FACTORS.
+//    THIS GR 1D FT DOES, CALLING AT EACH STAGE ON THE APPROPRIATE KERN
+//    EL R2FTK, R4FTK, R8FTK, R16FTK, R3FTK, R5FTK, OR RPFTK TO PERFORM
+//    THE COMPUTATIONS FOR THIS PASS OVER THE SEQUENCE, DEPENDING ON
+//    WHETHER THE CORRESPONDING FACTOR IS 2, 4, 8, 16, 3, 5, OR SOME
+//    MORE GENERAL PRIME P. WHEN GR1DFT IS FINISHED THE TRANSFORM IS
+//    COMPUTED, HOWEVER, THE RESULTS ARE STORED IN "DIGITS REVERSED"
+//    ORDER. AR1DFT THEREFORE, CALLS UPON GR 1S FS TO SORT THEM OUT.
+//    TO RETURN TO THE FACTORIZATION, SINGLETON HAS POINTED OUT THAT
+//    THE TRANSFORMS ARE MORE EFFICIENT IF THE SAMPLE SIZE N, IS OF THE
+//    FORM B*A**2 AND B CONSISTS OF A SINGLE FACTOR.  IN SUCH A CASE
+//    IF WE PROCESS THE FACTORS IN THE ORDER ABA  THEN
+//    THE REORDERING CAN BE DONE AS FAST IN PLACE, AS WITH SCRATCH
+//    STORAGE.  BUT AS B BECOMES MORE COMPLICATED, THE COST OF THE DIGIT
+//    REVERSING DUE TO B PART BECOMES VERY EXPENSIVE IF WE TRY TO DO IT
+//    IN PLACE.  IN SUCH A CASE IT MIGHT BE BETTER TO USE EXTRA STORAGE
+//    A ROUTINE TO DO THIS IS, HOWEVER, NOT INCLUDED HERE.
+//    ANOTHER FEATURE INFLUENCING THE FACTORIZATION IS THAT FOR ANY FIXED
+//    FACTOR N WE CAN PREPARE A SPECIAL KERNEL WHICH WILL COMPUTE
+//    THAT STAGE OF THE TRANSFORM MORE EFFICIENTLY THAN WOULD A KERNEL
+//    FOR GENERAL FACTORS, ESPECIALLY IF THE GENERAL KERNEL HAD TO BE
+//    APPLIED SEVERAL TIMES. FOR EXAMPLE, FACTORS OF 4 ARE MORE
+//    EFFICIENT THAN FACTORS OF 2, FACTORS OF 8 MORE EFFICIENT THAN 4,ETC
+//    ON THE OTHER HAND DIMINISHING RETURNS RAPIDLY SET IN, ESPECIALLY
+//    SINCE THE LENGTH OF THE KERNEL FOR A SPECIAL CASE IS ROUGHLY
+//    PROPORTIONAL TO THE FACTOR IT DEALS WITH. HENCE THESE PROBABLY ARE
+//    ALL THE KERNELS WE WISH TO HAVE.
+//            RESTRICTIONS.
+//    AN UNFORTUNATE FEATURE OF THE SORTING PROBLEM IS THAT THE MOST
+//    EFFICIENT WAY TO DO IT IS WITH NESTED DO LOOPS, ONE FOR EACH
+//    FACTOR. THIS PUTS A RESTRICTION ON N AS TO HOW MANY FACTORS IT
+//    CAN HAVE.  CURRENTLY THE LIMIT IS 16, BUT THE LIMIT CAN BE READILY
+//    RAISED IF NECESSARY.
+//    A SECOND RESTRICTION OF THE PROGRAM IS THAT LOCAL STORAGE OF THE
+//    THE ORDER P**2 IS REQUIRED BY THE GENERAL KERNEL RPFTK, SO SOME
+//    LIMIT MUST BE SET ON P.  CURRENTLY THIS IS 19, BUT IT CAN BE INCRE
+//    INCREASED BY TRIVIAL CHANGES.
+//       OTHER COMMENTS.
+//(1) THE ROUTINE IS ADAPTED TO CHECK WHETHER A GIVEN N WILL MEET THE
+//    ABOVE FACTORING REQUIREMENTS AN, IF NOT, TO RETURN THE NEXT HIGHER
+//    NUMBER, NX, SAY, WHICH WILL MEET THESE REQUIREMENTS.
+//    THIS CAN BE ACCHIEVED BY   A STATEMENT OF THE FORM
+//            CALL FACTR(N,X,Y).
+//    IF A DIFFERENT N, SAY NX, IS RETURNED THEN THE TRANSFORMS COULD BE
+//    OBTAINED BY EXTENDING THE SIZE OF THE X-ARRAY AND Y-ARRAY TO NX,
+//    AND SETTING X(I) = Y(I) = 0., FOR I = N+1, NX.
+//(2) IF THE SEQUENCE Z IS ONLY A REAL SEQUENCE, THEN THE IMAGINARY PART
+//    Y(I)=0., THIS WILL RETURN THE COSINE TRANSFORM OF THE REAL SEQUENCE
+//    IN X, AND THE SINE TRANSFORM IN Y.
+
+
+#define WANT_STREAM
+
+#define WANT_MATH
+
+#include "newmatap.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+#ifdef DO_REPORT
+#define REPORT { static ExeCounter ExeCount(__LINE__,20); ++ExeCount; }
+#else
+#define REPORT {}
+#endif
+
+inline Real square(Real x) { return x*x; }
+inline int square(int x) { return x*x; }
+
+static void GR_1D_FS (int PTS, int N_SYM, int N_UN_SYM,
+   const SimpleIntArray& SYM, int P_SYM, const SimpleIntArray& UN_SYM,
+   Real* X, Real* Y);
+static void GR_1D_FT (int N, int N_FACTOR, const SimpleIntArray& FACTOR,
+   Real* X, Real* Y);
+static void R_P_FTK (int N, int M, int P, Real* X, Real* Y);
+static void R_2_FTK (int N, int M, Real* X0, Real* Y0, Real* X1, Real* Y1);
+static void R_3_FTK (int N, int M, Real* X0, Real* Y0, Real* X1, Real* Y1,
+   Real* X2, Real* Y2);
+static void R_4_FTK (int N, int M,
+   Real* X0, Real* Y0, Real* X1, Real* Y1,
+   Real* X2, Real* Y2, Real* X3, Real* Y3);
+static void R_5_FTK (int N, int M,
+   Real* X0, Real* Y0, Real* X1, Real* Y1, Real* X2, Real* Y2,
+   Real* X3, Real* Y3, Real* X4, Real* Y4);
+static void R_8_FTK (int N, int M,
+   Real* X0, Real* Y0, Real* X1, Real* Y1,
+   Real* X2, Real* Y2, Real* X3, Real* Y3,
+   Real* X4, Real* Y4, Real* X5, Real* Y5,
+   Real* X6, Real* Y6, Real* X7, Real* Y7);
+static void R_16_FTK (int N, int M,
+   Real* X0, Real* Y0, Real* X1, Real* Y1,
+   Real* X2, Real* Y2, Real* X3, Real* Y3,
+   Real* X4, Real* Y4, Real* X5, Real* Y5,
+   Real* X6, Real* Y6, Real* X7, Real* Y7,
+   Real* X8, Real* Y8, Real* X9, Real* Y9,
+   Real* X10, Real* Y10, Real* X11, Real* Y11,
+   Real* X12, Real* Y12, Real* X13, Real* Y13,
+   Real* X14, Real* Y14, Real* X15, Real* Y15);
+static int BitReverse(int x, int prod, int n, const SimpleIntArray& f);
+
+
+bool FFT_Controller::ar_1d_ft (int PTS, Real* X, Real *Y)
+{
+//    ARBITRARY RADIX ONE DIMENSIONAL FOURIER TRANSFORM
+
+   REPORT
+
+   int  F,J,N,NF,P,PMAX,P_SYM,P_TWO,Q,R,TWO_GRP;
+
+   // NP is maximum number of squared factors allows PTS up to 2**32 at least
+   // NQ is number of not-squared factors - increase if we increase PMAX
+   const int NP = 16, NQ = 10;
+   SimpleIntArray PP(NP), QQ(NQ);
+
+   TWO_GRP=16; PMAX=19;
+
+   // PMAX is the maximum factor size
+   // TWO_GRP is the maximum power of 2 handled as a single factor
+   // Doesn't take advantage of combining powers of 2 when calculating
+   // number of factors
+
+   if (PTS<=1) return true;
+   N=PTS; P_SYM=1; F=2; P=0; Q=0;
+
+   // P counts the number of squared factors
+   // Q counts the number of the rest
+   // R = 0 for no non-squared factors; 1 otherwise
+
+   // FACTOR holds all the factors - non-squared ones in the middle
+   //   - length is 2*P+Q
+   // SYM also holds all the factors but with the non-squared ones
+   //   multiplied together - length is 2*P+R
+   // PP holds the values of the squared factors - length is P
+   // QQ holds the values of the rest - length is Q
+
+   // P_SYM holds the product of the squared factors
+
+   // find the factors - load into PP and QQ
+   while (N > 1)
+   {
+      bool fail = true;
+      for (J=F; J<=PMAX; J++)
+         if (N % J == 0) { fail = false; F=J; break; }
+      if (fail || P >= NP || Q >= NQ) return false; // can't factor
+      N /= F;
+      if (N % F != 0) QQ[Q++] = F;
+      else { N /= F; PP[P++] = F; P_SYM *= F; }
+   }
+
+   R = (Q == 0) ? 0 : 1;  // R = 0 if no not-squared factors, 1 otherwise
+
+   NF = 2*P + Q;
+   SimpleIntArray FACTOR(NF + 1), SYM(2*P + R);
+   FACTOR[NF] = 0;                // we need this in the "combine powers of 2"
+
+   // load into SYM and FACTOR
+   for (J=0; J<P; J++)
+      { SYM[J]=FACTOR[J]=PP[P-1-J]; FACTOR[P+Q+J]=SYM[P+R+J]=PP[J]; }
+
+   if (Q>0)
+   {
+      REPORT
+      for (J=0; J<Q; J++) FACTOR[P+J]=QQ[J];
+      SYM[P]=PTS/square(P_SYM);
+   }
+
+   // combine powers of 2
+   P_TWO = 1;
+   for (J=0; J < NF; J++)
+   {
+      if (FACTOR[J]!=2) continue;
+      P_TWO=P_TWO*2; FACTOR[J]=1;
+      if (P_TWO<TWO_GRP && FACTOR[J+1]==2) continue;
+      FACTOR[J]=P_TWO; P_TWO=1;
+   }
+
+   if (P==0) R=0;
+   if (Q<=1) Q=0;
+
+   // do the analysis
+   GR_1D_FT(PTS,NF,FACTOR,X,Y);                 // the transform
+   GR_1D_FS(PTS,2*P+R,Q,SYM,P_SYM,QQ,X,Y);      // the reshuffling
+
+   return true;
+
+}
+
+static void GR_1D_FS (int PTS, int N_SYM, int N_UN_SYM,
+   const SimpleIntArray& SYM, int P_SYM, const SimpleIntArray& UN_SYM,
+   Real* X, Real* Y)
+{
+//    GENERAL RADIX ONE DIMENSIONAL FOURIER SORT
+
+// PTS = number of points
+// N_SYM = length of SYM
+// N_UN_SYM = length of UN_SYM
+// SYM: squared factors + product of non-squared factors + squared factors
+// P_SYM = product of squared factors (each included only once)
+// UN_SYM: not-squared factors
+
+   REPORT
+
+   Real T;
+   int  JJ,KK,P_UN_SYM;
+
+   // I have replaced the multiple for-loop used by Sande-Gentleman code
+   // by the following code which does not limit the number of factors
+
+   if (N_SYM > 0)
+   {
+      REPORT
+      SimpleIntArray U(N_SYM);
+      for(MultiRadixCounter MRC(N_SYM, SYM, U); !MRC.Finish(); ++MRC)
+      {
+         if (MRC.Swap())
+         {
+            int P = MRC.Reverse(); int JJ = MRC.Counter(); Real T;
+            T=X[JJ]; X[JJ]=X[P]; X[P]=T; T=Y[JJ]; Y[JJ]=Y[P]; Y[P]=T;
+         }
+      }
+   }
+
+   int J,JL,K,L,M,MS;
+
+   // UN_SYM contains the non-squared factors
+   // I have replaced the Sande-Gentleman code as it runs into
+   // integer overflow problems
+   // My code (and theirs) would be improved by using a bit array
+   // as suggested by Van Loan
+
+   if (N_UN_SYM==0) { REPORT return; }
+   P_UN_SYM=PTS/square(P_SYM); JL=(P_UN_SYM-3)*P_SYM; MS=P_UN_SYM*P_SYM;
+
+   for (J = P_SYM; J<=JL; J+=P_SYM)
+   {
+      K=J;
+      do K = P_SYM * BitReverse(K / P_SYM, P_UN_SYM, N_UN_SYM, UN_SYM);
+      while (K<J);
+
+      if (K!=J)
+      {
+         REPORT
+         for (L=0; L<P_SYM; L++) for (M=L; M<PTS; M+=MS)
+         {
+            JJ=M+J; KK=M+K;
+            T=X[JJ]; X[JJ]=X[KK]; X[KK]=T; T=Y[JJ]; Y[JJ]=Y[KK]; Y[KK]=T;
+         }
+      }
+   }
+
+   return;
+}
+
+static void GR_1D_FT (int N, int N_FACTOR, const SimpleIntArray& FACTOR,
+   Real* X, Real* Y)
+{
+//    GENERAL RADIX ONE DIMENSIONAL FOURIER TRANSFORM;
+
+   REPORT
+
+   int  M = N;
+
+   for (int i = 0; i < N_FACTOR; i++)
+   {
+      int P = FACTOR[i]; M /= P;
+
+      switch(P)
+      {
+      case 1: REPORT break;
+      case 2: REPORT R_2_FTK (N,M,X,Y,X+M,Y+M); break;
+      case 3: REPORT R_3_FTK (N,M,X,Y,X+M,Y+M,X+2*M,Y+2*M); break;
+      case 4: REPORT R_4_FTK (N,M,X,Y,X+M,Y+M,X+2*M,Y+2*M,X+3*M,Y+3*M); break;
+      case 5:
+         REPORT
+         R_5_FTK (N,M,X,Y,X+M,Y+M,X+2*M,Y+2*M,X+3*M,Y+3*M,X+4*M,Y+4*M);
+         break;
+      case 8:
+         REPORT
+         R_8_FTK (N,M,X,Y,X+M,Y+M,X+2*M,Y+2*M,
+            X+3*M,Y+3*M,X+4*M,Y+4*M,X+5*M,Y+5*M,
+            X+6*M,Y+6*M,X+7*M,Y+7*M);
+         break;
+      case 16:
+         REPORT
+         R_16_FTK (N,M,X,Y,X+M,Y+M,X+2*M,Y+2*M,
+            X+3*M,Y+3*M,X+4*M,Y+4*M,X+5*M,Y+5*M,
+            X+6*M,Y+6*M,X+7*M,Y+7*M,X+8*M,Y+8*M,
+            X+9*M,Y+9*M,X+10*M,Y+10*M,X+11*M,Y+11*M,
+            X+12*M,Y+12*M,X+13*M,Y+13*M,X+14*M,Y+14*M,
+            X+15*M,Y+15*M);
+         break;
+      default: REPORT R_P_FTK (N,M,P,X,Y); break;
+      }
+   }
+
+}
+
+static void R_P_FTK (int N, int M, int P, Real* X, Real* Y)
+//    RADIX PRIME FOURIER TRANSFORM KERNEL;
+// X and Y are treated as M * P matrices with Fortran storage
+{
+   REPORT
+   bool NO_FOLD,ZERO;
+   Real ANGLE,IS,IU,RS,RU,T,TWOPI,XT,YT;
+   int  J,JJ,K0,K,M_OVER_2,MP,PM,PP,U,V;
+
+   Real AA [9][9], BB [9][9];
+   Real A [18], B [18], C [18], S [18];
+   Real IA [9], IB [9], RA [9], RB [9];
+
+   TWOPI=8.0*atan(1.0);
+   M_OVER_2=M/2+1; MP=M*P; PP=P/2; PM=P-1;
+
+   for (U=0; U<PP; U++)
+   {
+      ANGLE=TWOPI*Real(U+1)/Real(P);
+      JJ=P-U-2;
+      A[U]=cos(ANGLE); B[U]=sin(ANGLE);
+      A[JJ]=A[U]; B[JJ]= -B[U];
+   }
+
+   for (U=1; U<=PP; U++)
+   {
+      for (V=1; V<=PP; V++)
+         { JJ=U*V-U*V/P*P; AA[V-1][U-1]=A[JJ-1]; BB[V-1][U-1]=B[JJ-1]; }
+   }
+
+   for (J=0; J<M_OVER_2; J++)
+   {
+      NO_FOLD = (J==0 || 2*J==M);
+      K0=J;
+      ANGLE=TWOPI*Real(J)/Real(MP); ZERO=ANGLE==0.0;
+      C[0]=cos(ANGLE); S[0]=sin(ANGLE);
+      for (U=1; U<PM; U++)
+      {
+         C[U]=C[U-1]*C[0]-S[U-1]*S[0];
+         S[U]=S[U-1]*C[0]+C[U-1]*S[0];
+      }
+      goto L700;
+   L500:
+      REPORT
+      if (NO_FOLD) { REPORT goto L1500; }
+      REPORT
+      NO_FOLD=true; K0=M-J;
+      for (U=0; U<PM; U++)
+         { T=C[U]*A[U]+S[U]*B[U]; S[U]= -S[U]*A[U]+C[U]*B[U]; C[U]=T; }
+   L700:
+      REPORT
+      for (K=K0; K<N; K+=MP)
+      {
+         XT=X[K]; YT=Y[K];
+         for (U=1; U<=PP; U++)
+         {
+            RA[U-1]=XT; IA[U-1]=YT;
+            RB[U-1]=0.0; IB[U-1]=0.0;
+         }
+         for (U=1; U<=PP; U++)
+         {
+            JJ=P-U;
+            RS=X[K+M*U]+X[K+M*JJ]; IS=Y[K+M*U]+Y[K+M*JJ];
+            RU=X[K+M*U]-X[K+M*JJ]; IU=Y[K+M*U]-Y[K+M*JJ];
+            XT=XT+RS; YT=YT+IS;
+            for (V=0; V<PP; V++)
+            {
+               RA[V]=RA[V]+RS*AA[V][U-1]; IA[V]=IA[V]+IS*AA[V][U-1];
+               RB[V]=RB[V]+RU*BB[V][U-1]; IB[V]=IB[V]+IU*BB[V][U-1];
+            }
+         }
+         X[K]=XT; Y[K]=YT;
+         for (U=1; U<=PP; U++)
+         {
+            if (!ZERO)
+            {
+               REPORT
+               XT=RA[U-1]+IB[U-1]; YT=IA[U-1]-RB[U-1];
+               X[K+M*U]=XT*C[U-1]+YT*S[U-1]; Y[K+M*U]=YT*C[U-1]-XT*S[U-1];
+               JJ=P-U;
+               XT=RA[U-1]-IB[U-1]; YT=IA[U-1]+RB[U-1];
+               X[K+M*JJ]=XT*C[JJ-1]+YT*S[JJ-1];
+               Y[K+M*JJ]=YT*C[JJ-1]-XT*S[JJ-1];
+            }
+            else
+            {
+               REPORT
+               X[K+M*U]=RA[U-1]+IB[U-1]; Y[K+M*U]=IA[U-1]-RB[U-1];
+               JJ=P-U;
+               X[K+M*JJ]=RA[U-1]-IB[U-1]; Y[K+M*JJ]=IA[U-1]+RB[U-1];
+            }
+         }
+      }
+      goto L500;
+L1500: ;
+   }
+   return;
+}
+
+static void R_2_FTK (int N, int M, Real* X0, Real* Y0, Real* X1, Real* Y1)
+//    RADIX TWO FOURIER TRANSFORM KERNEL;
+{
+   REPORT
+   bool NO_FOLD,ZERO;
+   int  J,K,K0,M2,M_OVER_2;
+   Real ANGLE,C,IS,IU,RS,RU,S,TWOPI;
+
+   M2=M*2; M_OVER_2=M/2+1;
+   TWOPI=8.0*atan(1.0);
+
+   for (J=0; J<M_OVER_2; J++)
+   {
+      NO_FOLD = (J==0 || 2*J==M);
+      K0=J;
+      ANGLE=TWOPI*Real(J)/Real(M2); ZERO=ANGLE==0.0;
+      C=cos(ANGLE); S=sin(ANGLE);
+      goto L200;
+   L100:
+      REPORT
+      if (NO_FOLD) { REPORT goto L600; }
+      REPORT
+      NO_FOLD=true; K0=M-J; C= -C;
+   L200:
+      REPORT
+      for (K=K0; K<N; K+=M2)
+      {
+         RS=X0[K]+X1[K]; IS=Y0[K]+Y1[K];
+         RU=X0[K]-X1[K]; IU=Y0[K]-Y1[K];
+         X0[K]=RS; Y0[K]=IS;
+         if (!ZERO) { X1[K]=RU*C+IU*S; Y1[K]=IU*C-RU*S; }
+         else { X1[K]=RU; Y1[K]=IU; }
+      }
+      goto L100;
+   L600: ;
+   }
+
+   return;
+}
+
+static void R_3_FTK (int N, int M, Real* X0, Real* Y0, Real* X1, Real* Y1,
+   Real* X2, Real* Y2)
+//    RADIX THREE FOURIER TRANSFORM KERNEL
+{
+   REPORT
+   bool NO_FOLD,ZERO;
+   int  J,K,K0,M3,M_OVER_2;
+   Real ANGLE,A,B,C1,C2,S1,S2,T,TWOPI;
+   Real I0,I1,I2,IA,IB,IS,R0,R1,R2,RA,RB,RS;
+
+   M3=M*3; M_OVER_2=M/2+1; TWOPI=8.0*atan(1.0);
+   A=cos(TWOPI/3.0); B=sin(TWOPI/3.0);
+
+   for (J=0; J<M_OVER_2; J++)
+   {
+      NO_FOLD = (J==0 || 2*J==M);
+      K0=J;
+      ANGLE=TWOPI*Real(J)/Real(M3); ZERO=ANGLE==0.0;
+      C1=cos(ANGLE); S1=sin(ANGLE);
+      C2=C1*C1-S1*S1; S2=S1*C1+C1*S1;
+      goto L200;
+   L100:
+      REPORT
+      if (NO_FOLD) { REPORT goto L600; }
+      REPORT
+      NO_FOLD=true; K0=M-J;
+      T=C1*A+S1*B; S1=C1*B-S1*A; C1=T;
+      T=C2*A-S2*B; S2= -C2*B-S2*A; C2=T;
+   L200:
+      REPORT
+      for (K=K0; K<N; K+=M3)
+      {
+         R0 = X0[K]; I0 = Y0[K];
+         RS=X1[K]+X2[K]; IS=Y1[K]+Y2[K];
+         X0[K]=R0+RS; Y0[K]=I0+IS;
+         RA=R0+RS*A; IA=I0+IS*A;
+         RB=(X1[K]-X2[K])*B; IB=(Y1[K]-Y2[K])*B;
+         if (!ZERO)
+         {
+            REPORT
+            R1=RA+IB; I1=IA-RB; R2=RA-IB; I2=IA+RB;
+            X1[K]=R1*C1+I1*S1; Y1[K]=I1*C1-R1*S1;
+            X2[K]=R2*C2+I2*S2; Y2[K]=I2*C2-R2*S2;
+         }
+         else { REPORT X1[K]=RA+IB; Y1[K]=IA-RB; X2[K]=RA-IB; Y2[K]=IA+RB; }
+      }
+      goto L100;
+   L600: ;
+   }
+
+   return;
+}
+
+static void R_4_FTK (int N, int M,
+   Real* X0, Real* Y0, Real* X1, Real* Y1,
+   Real* X2, Real* Y2, Real* X3, Real* Y3)
+//    RADIX FOUR FOURIER TRANSFORM KERNEL
+{
+   REPORT
+   bool NO_FOLD,ZERO;
+   int  J,K,K0,M4,M_OVER_2;
+   Real ANGLE,C1,C2,C3,S1,S2,S3,T,TWOPI;
+   Real I1,I2,I3,IS0,IS1,IU0,IU1,R1,R2,R3,RS0,RS1,RU0,RU1;
+
+   M4=M*4; M_OVER_2=M/2+1;
+   TWOPI=8.0*atan(1.0);
+
+   for (J=0; J<M_OVER_2; J++)
+   {
+      NO_FOLD = (J==0 || 2*J==M);
+      K0=J;
+      ANGLE=TWOPI*Real(J)/Real(M4); ZERO=ANGLE==0.0;
+      C1=cos(ANGLE); S1=sin(ANGLE);
+      C2=C1*C1-S1*S1; S2=S1*C1+C1*S1;
+      C3=C2*C1-S2*S1; S3=S2*C1+C2*S1;
+      goto L200;
+   L100:
+      REPORT
+      if (NO_FOLD) { REPORT goto L600; }
+      REPORT
+      NO_FOLD=true; K0=M-J;
+      T=C1; C1=S1; S1=T;
+      C2= -C2;
+      T=C3; C3= -S3; S3= -T;
+   L200:
+      REPORT
+      for (K=K0; K<N; K+=M4)
+      {
+         RS0=X0[K]+X2[K]; IS0=Y0[K]+Y2[K];
+         RU0=X0[K]-X2[K]; IU0=Y0[K]-Y2[K];
+         RS1=X1[K]+X3[K]; IS1=Y1[K]+Y3[K];
+         RU1=X1[K]-X3[K]; IU1=Y1[K]-Y3[K];
+         X0[K]=RS0+RS1; Y0[K]=IS0+IS1;
+         if (!ZERO)
+         {
+            REPORT
+            R1=RU0+IU1; I1=IU0-RU1;
+            R2=RS0-RS1; I2=IS0-IS1;
+            R3=RU0-IU1; I3=IU0+RU1;
+            X2[K]=R1*C1+I1*S1; Y2[K]=I1*C1-R1*S1;
+            X1[K]=R2*C2+I2*S2; Y1[K]=I2*C2-R2*S2;
+            X3[K]=R3*C3+I3*S3; Y3[K]=I3*C3-R3*S3;
+         }
+         else
+         {
+            REPORT
+            X2[K]=RU0+IU1; Y2[K]=IU0-RU1;
+            X1[K]=RS0-RS1; Y1[K]=IS0-IS1;
+            X3[K]=RU0-IU1; Y3[K]=IU0+RU1;
+         }
+      }
+      goto L100;
+   L600: ;
+   }
+
+   return;
+}
+
+static void R_5_FTK (int N, int M,
+   Real* X0, Real* Y0, Real* X1, Real* Y1, Real* X2, Real* Y2,
+   Real* X3, Real* Y3, Real* X4, Real* Y4)
+//    RADIX FIVE FOURIER TRANSFORM KERNEL
+
+{
+   REPORT
+   bool NO_FOLD,ZERO;
+   int  J,K,K0,M5,M_OVER_2;
+   Real ANGLE,A1,A2,B1,B2,C1,C2,C3,C4,S1,S2,S3,S4,T,TWOPI;
+   Real R0,R1,R2,R3,R4,RA1,RA2,RB1,RB2,RS1,RS2,RU1,RU2;
+   Real I0,I1,I2,I3,I4,IA1,IA2,IB1,IB2,IS1,IS2,IU1,IU2;
+
+   M5=M*5; M_OVER_2=M/2+1;
+   TWOPI=8.0*atan(1.0);
+   A1=cos(TWOPI/5.0); B1=sin(TWOPI/5.0);
+   A2=cos(2.0*TWOPI/5.0); B2=sin(2.0*TWOPI/5.0);
+
+   for (J=0; J<M_OVER_2; J++)
+   {
+      NO_FOLD = (J==0 || 2*J==M);
+      K0=J;
+      ANGLE=TWOPI*Real(J)/Real(M5); ZERO=ANGLE==0.0;
+      C1=cos(ANGLE); S1=sin(ANGLE);
+      C2=C1*C1-S1*S1; S2=S1*C1+C1*S1;
+      C3=C2*C1-S2*S1; S3=S2*C1+C2*S1;
+      C4=C2*C2-S2*S2; S4=S2*C2+C2*S2;
+      goto L200;
+   L100:
+      REPORT
+      if (NO_FOLD) { REPORT goto L600; }
+      REPORT
+      NO_FOLD=true; K0=M-J;
+      T=C1*A1+S1*B1; S1=C1*B1-S1*A1; C1=T;
+      T=C2*A2+S2*B2; S2=C2*B2-S2*A2; C2=T;
+      T=C3*A2-S3*B2; S3= -C3*B2-S3*A2; C3=T;
+      T=C4*A1-S4*B1; S4= -C4*B1-S4*A1; C4=T;
+   L200:
+      REPORT
+      for (K=K0; K<N; K+=M5)
+      {
+         R0=X0[K]; I0=Y0[K];
+         RS1=X1[K]+X4[K]; IS1=Y1[K]+Y4[K];
+         RU1=X1[K]-X4[K]; IU1=Y1[K]-Y4[K];
+         RS2=X2[K]+X3[K]; IS2=Y2[K]+Y3[K];
+         RU2=X2[K]-X3[K]; IU2=Y2[K]-Y3[K];
+         X0[K]=R0+RS1+RS2; Y0[K]=I0+IS1+IS2;
+         RA1=R0+RS1*A1+RS2*A2; IA1=I0+IS1*A1+IS2*A2;
+         RA2=R0+RS1*A2+RS2*A1; IA2=I0+IS1*A2+IS2*A1;
+         RB1=RU1*B1+RU2*B2; IB1=IU1*B1+IU2*B2;
+         RB2=RU1*B2-RU2*B1; IB2=IU1*B2-IU2*B1;
+         if (!ZERO)
+         {
+            REPORT
+            R1=RA1+IB1; I1=IA1-RB1;
+            R2=RA2+IB2; I2=IA2-RB2;
+            R3=RA2-IB2; I3=IA2+RB2;
+            R4=RA1-IB1; I4=IA1+RB1;
+            X1[K]=R1*C1+I1*S1; Y1[K]=I1*C1-R1*S1;
+            X2[K]=R2*C2+I2*S2; Y2[K]=I2*C2-R2*S2;
+            X3[K]=R3*C3+I3*S3; Y3[K]=I3*C3-R3*S3;
+            X4[K]=R4*C4+I4*S4; Y4[K]=I4*C4-R4*S4;
+         }
+         else
+         {
+            REPORT
+            X1[K]=RA1+IB1; Y1[K]=IA1-RB1;
+            X2[K]=RA2+IB2; Y2[K]=IA2-RB2;
+            X3[K]=RA2-IB2; Y3[K]=IA2+RB2;
+            X4[K]=RA1-IB1; Y4[K]=IA1+RB1;
+         }
+      }
+      goto L100;
+   L600: ;
+   }
+
+   return;
+}
+
+static void R_8_FTK (int N, int M,
+   Real* X0, Real* Y0, Real* X1, Real* Y1,
+   Real* X2, Real* Y2, Real* X3, Real* Y3,
+   Real* X4, Real* Y4, Real* X5, Real* Y5,
+   Real* X6, Real* Y6, Real* X7, Real* Y7)
+//    RADIX EIGHT FOURIER TRANSFORM KERNEL
+{
+   REPORT
+   bool NO_FOLD,ZERO;
+   int  J,K,K0,M8,M_OVER_2;
+   Real ANGLE,C1,C2,C3,C4,C5,C6,C7,E,S1,S2,S3,S4,S5,S6,S7,T,TWOPI;
+   Real R1,R2,R3,R4,R5,R6,R7,RS0,RS1,RS2,RS3,RU0,RU1,RU2,RU3;
+   Real I1,I2,I3,I4,I5,I6,I7,IS0,IS1,IS2,IS3,IU0,IU1,IU2,IU3;
+   Real RSS0,RSS1,RSU0,RSU1,RUS0,RUS1,RUU0,RUU1;
+   Real ISS0,ISS1,ISU0,ISU1,IUS0,IUS1,IUU0,IUU1;
+
+   M8=M*8; M_OVER_2=M/2+1;
+   TWOPI=8.0*atan(1.0); E=cos(TWOPI/8.0);
+
+   for (J=0;J<M_OVER_2;J++)
+   {
+      NO_FOLD= (J==0 || 2*J==M);
+      K0=J;
+      ANGLE=TWOPI*Real(J)/Real(M8); ZERO=ANGLE==0.0;
+      C1=cos(ANGLE); S1=sin(ANGLE);
+      C2=C1*C1-S1*S1; S2=C1*S1+S1*C1;
+      C3=C2*C1-S2*S1; S3=S2*C1+C2*S1;
+      C4=C2*C2-S2*S2; S4=S2*C2+C2*S2;
+      C5=C4*C1-S4*S1; S5=S4*C1+C4*S1;
+      C6=C4*C2-S4*S2; S6=S4*C2+C4*S2;
+      C7=C4*C3-S4*S3; S7=S4*C3+C4*S3;
+      goto L200;
+   L100:
+      REPORT
+      if (NO_FOLD) { REPORT goto L600; }
+      REPORT
+      NO_FOLD=true; K0=M-J;
+      T=(C1+S1)*E; S1=(C1-S1)*E; C1=T;
+      T=S2; S2=C2; C2=T;
+      T=(-C3+S3)*E; S3=(C3+S3)*E; C3=T;
+      C4= -C4;
+      T= -(C5+S5)*E; S5=(-C5+S5)*E; C5=T;
+      T= -S6; S6= -C6; C6=T;
+      T=(C7-S7)*E; S7= -(C7+S7)*E; C7=T;
+   L200:
+      REPORT
+      for (K=K0; K<N; K+=M8)
+      {
+         RS0=X0[K]+X4[K]; IS0=Y0[K]+Y4[K];
+         RU0=X0[K]-X4[K]; IU0=Y0[K]-Y4[K];
+         RS1=X1[K]+X5[K]; IS1=Y1[K]+Y5[K];
+         RU1=X1[K]-X5[K]; IU1=Y1[K]-Y5[K];
+         RS2=X2[K]+X6[K]; IS2=Y2[K]+Y6[K];
+         RU2=X2[K]-X6[K]; IU2=Y2[K]-Y6[K];
+         RS3=X3[K]+X7[K]; IS3=Y3[K]+Y7[K];
+         RU3=X3[K]-X7[K]; IU3=Y3[K]-Y7[K];
+         RSS0=RS0+RS2; ISS0=IS0+IS2;
+         RSU0=RS0-RS2; ISU0=IS0-IS2;
+         RSS1=RS1+RS3; ISS1=IS1+IS3;
+         RSU1=RS1-RS3; ISU1=IS1-IS3;
+         RUS0=RU0-IU2; IUS0=IU0+RU2;
+         RUU0=RU0+IU2; IUU0=IU0-RU2;
+         RUS1=RU1-IU3; IUS1=IU1+RU3;
+         RUU1=RU1+IU3; IUU1=IU1-RU3;
+         T=(RUS1+IUS1)*E; IUS1=(IUS1-RUS1)*E; RUS1=T;
+         T=(RUU1+IUU1)*E; IUU1=(IUU1-RUU1)*E; RUU1=T;
+         X0[K]=RSS0+RSS1; Y0[K]=ISS0+ISS1;
+         if (!ZERO)
+         {
+            REPORT
+            R1=RUU0+RUU1; I1=IUU0+IUU1;
+            R2=RSU0+ISU1; I2=ISU0-RSU1;
+            R3=RUS0+IUS1; I3=IUS0-RUS1;
+            R4=RSS0-RSS1; I4=ISS0-ISS1;
+            R5=RUU0-RUU1; I5=IUU0-IUU1;
+            R6=RSU0-ISU1; I6=ISU0+RSU1;
+            R7=RUS0-IUS1; I7=IUS0+RUS1;
+            X4[K]=R1*C1+I1*S1; Y4[K]=I1*C1-R1*S1;
+            X2[K]=R2*C2+I2*S2; Y2[K]=I2*C2-R2*S2;
+            X6[K]=R3*C3+I3*S3; Y6[K]=I3*C3-R3*S3;
+            X1[K]=R4*C4+I4*S4; Y1[K]=I4*C4-R4*S4;
+            X5[K]=R5*C5+I5*S5; Y5[K]=I5*C5-R5*S5;
+            X3[K]=R6*C6+I6*S6; Y3[K]=I6*C6-R6*S6;
+            X7[K]=R7*C7+I7*S7; Y7[K]=I7*C7-R7*S7;
+         }
+         else
+         {
+            REPORT
+            X4[K]=RUU0+RUU1; Y4[K]=IUU0+IUU1;
+            X2[K]=RSU0+ISU1; Y2[K]=ISU0-RSU1;
+            X6[K]=RUS0+IUS1; Y6[K]=IUS0-RUS1;
+            X1[K]=RSS0-RSS1; Y1[K]=ISS0-ISS1;
+            X5[K]=RUU0-RUU1; Y5[K]=IUU0-IUU1;
+            X3[K]=RSU0-ISU1; Y3[K]=ISU0+RSU1;
+            X7[K]=RUS0-IUS1; Y7[K]=IUS0+RUS1;
+         }
+      }
+      goto L100;
+   L600: ;
+   }
+
+   return;
+}
+
+static void R_16_FTK (int N, int M,
+   Real* X0, Real* Y0, Real* X1, Real* Y1,
+   Real* X2, Real* Y2, Real* X3, Real* Y3,
+   Real* X4, Real* Y4, Real* X5, Real* Y5,
+   Real* X6, Real* Y6, Real* X7, Real* Y7,
+   Real* X8, Real* Y8, Real* X9, Real* Y9,
+   Real* X10, Real* Y10, Real* X11, Real* Y11,
+   Real* X12, Real* Y12, Real* X13, Real* Y13,
+   Real* X14, Real* Y14, Real* X15, Real* Y15)
+//    RADIX SIXTEEN FOURIER TRANSFORM KERNEL
+{
+   REPORT
+   bool NO_FOLD,ZERO;
+   int  J,K,K0,M16,M_OVER_2;
+   Real ANGLE,EI1,ER1,E2,EI3,ER3,EI5,ER5,T,TWOPI;
+   Real RS0,RS1,RS2,RS3,RS4,RS5,RS6,RS7;
+   Real IS0,IS1,IS2,IS3,IS4,IS5,IS6,IS7;
+   Real RU0,RU1,RU2,RU3,RU4,RU5,RU6,RU7;
+   Real IU0,IU1,IU2,IU3,IU4,IU5,IU6,IU7;
+   Real RUS0,RUS1,RUS2,RUS3,RUU0,RUU1,RUU2,RUU3;
+   Real ISS0,ISS1,ISS2,ISS3,ISU0,ISU1,ISU2,ISU3;
+   Real RSS0,RSS1,RSS2,RSS3,RSU0,RSU1,RSU2,RSU3;
+   Real IUS0,IUS1,IUS2,IUS3,IUU0,IUU1,IUU2,IUU3;
+   Real RSSS0,RSSS1,RSSU0,RSSU1,RSUS0,RSUS1,RSUU0,RSUU1;
+   Real ISSS0,ISSS1,ISSU0,ISSU1,ISUS0,ISUS1,ISUU0,ISUU1;
+   Real RUSS0,RUSS1,RUSU0,RUSU1,RUUS0,RUUS1,RUUU0,RUUU1;
+   Real IUSS0,IUSS1,IUSU0,IUSU1,IUUS0,IUUS1,IUUU0,IUUU1;
+   Real R1,R2,R3,R4,R5,R6,R7,R8,R9,R10,R11,R12,R13,R14,R15;
+   Real I1,I2,I3,I4,I5,I6,I7,I8,I9,I10,I11,I12,I13,I14,I15;
+   Real C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,C12,C13,C14,C15;
+   Real S1,S2,S3,S4,S5,S6,S7,S8,S9,S10,S11,S12,S13,S14,S15;
+
+   M16=M*16; M_OVER_2=M/2+1;
+   TWOPI=8.0*atan(1.0);
+   ER1=cos(TWOPI/16.0); EI1=sin(TWOPI/16.0);
+   E2=cos(TWOPI/8.0);
+   ER3=cos(3.0*TWOPI/16.0); EI3=sin(3.0*TWOPI/16.0);
+   ER5=cos(5.0*TWOPI/16.0); EI5=sin(5.0*TWOPI/16.0);
+
+   for (J=0; J<M_OVER_2; J++)
+   {
+      NO_FOLD = (J==0 || 2*J==M);
+      K0=J;
+      ANGLE=TWOPI*Real(J)/Real(M16);
+      ZERO=ANGLE==0.0;
+      C1=cos(ANGLE); S1=sin(ANGLE);
+      C2=C1*C1-S1*S1; S2=C1*S1+S1*C1;
+      C3=C2*C1-S2*S1; S3=S2*C1+C2*S1;
+      C4=C2*C2-S2*S2; S4=S2*C2+C2*S2;
+      C5=C4*C1-S4*S1; S5=S4*C1+C4*S1;
+      C6=C4*C2-S4*S2; S6=S4*C2+C4*S2;
+      C7=C4*C3-S4*S3; S7=S4*C3+C4*S3;
+      C8=C4*C4-S4*S4; S8=C4*S4+S4*C4;
+      C9=C8*C1-S8*S1; S9=S8*C1+C8*S1;
+      C10=C8*C2-S8*S2; S10=S8*C2+C8*S2;
+      C11=C8*C3-S8*S3; S11=S8*C3+C8*S3;
+      C12=C8*C4-S8*S4; S12=S8*C4+C8*S4;
+      C13=C8*C5-S8*S5; S13=S8*C5+C8*S5;
+      C14=C8*C6-S8*S6; S14=S8*C6+C8*S6;
+      C15=C8*C7-S8*S7; S15=S8*C7+C8*S7;
+      goto L200;
+   L100:
+      REPORT
+      if (NO_FOLD) { REPORT goto L600; }
+      REPORT
+      NO_FOLD=true; K0=M-J;
+      T=C1*ER1+S1*EI1; S1= -S1*ER1+C1*EI1; C1=T;
+      T=(C2+S2)*E2; S2=(C2-S2)*E2; C2=T;
+      T=C3*ER3+S3*EI3; S3= -S3*ER3+C3*EI3; C3=T;
+      T=S4; S4=C4; C4=T;
+      T=S5*ER1-C5*EI1; S5=C5*ER1+S5*EI1; C5=T;
+      T=(-C6+S6)*E2; S6=(C6+S6)*E2; C6=T;
+      T=S7*ER3-C7*EI3; S7=C7*ER3+S7*EI3; C7=T;
+      C8= -C8;
+      T= -(C9*ER1+S9*EI1); S9=S9*ER1-C9*EI1; C9=T;
+      T= -(C10+S10)*E2; S10=(-C10+S10)*E2; C10=T;
+      T= -(C11*ER3+S11*EI3); S11=S11*ER3-C11*EI3; C11=T;
+      T= -S12; S12= -C12; C12=T;
+      T= -S13*ER1+C13*EI1; S13= -(C13*ER1+S13*EI1); C13=T;
+      T=(C14-S14)*E2; S14= -(C14+S14)*E2; C14=T;
+      T= -S15*ER3+C15*EI3; S15= -(C15*ER3+S15*EI3); C15=T;
+   L200:
+      REPORT
+      for (K=K0; K<N; K+=M16)
+      {
+         RS0=X0[K]+X8[K]; IS0=Y0[K]+Y8[K];
+         RU0=X0[K]-X8[K]; IU0=Y0[K]-Y8[K];
+         RS1=X1[K]+X9[K]; IS1=Y1[K]+Y9[K];
+         RU1=X1[K]-X9[K]; IU1=Y1[K]-Y9[K];
+         RS2=X2[K]+X10[K]; IS2=Y2[K]+Y10[K];
+         RU2=X2[K]-X10[K]; IU2=Y2[K]-Y10[K];
+         RS3=X3[K]+X11[K]; IS3=Y3[K]+Y11[K];
+         RU3=X3[K]-X11[K]; IU3=Y3[K]-Y11[K];
+         RS4=X4[K]+X12[K]; IS4=Y4[K]+Y12[K];
+         RU4=X4[K]-X12[K]; IU4=Y4[K]-Y12[K];
+         RS5=X5[K]+X13[K]; IS5=Y5[K]+Y13[K];
+         RU5=X5[K]-X13[K]; IU5=Y5[K]-Y13[K];
+         RS6=X6[K]+X14[K]; IS6=Y6[K]+Y14[K];
+         RU6=X6[K]-X14[K]; IU6=Y6[K]-Y14[K];
+         RS7=X7[K]+X15[K]; IS7=Y7[K]+Y15[K];
+         RU7=X7[K]-X15[K]; IU7=Y7[K]-Y15[K];
+         RSS0=RS0+RS4; ISS0=IS0+IS4;
+         RSS1=RS1+RS5; ISS1=IS1+IS5;
+         RSS2=RS2+RS6; ISS2=IS2+IS6;
+         RSS3=RS3+RS7; ISS3=IS3+IS7;
+         RSU0=RS0-RS4; ISU0=IS0-IS4;
+         RSU1=RS1-RS5; ISU1=IS1-IS5;
+         RSU2=RS2-RS6; ISU2=IS2-IS6;
+         RSU3=RS3-RS7; ISU3=IS3-IS7;
+         RUS0=RU0-IU4; IUS0=IU0+RU4;
+         RUS1=RU1-IU5; IUS1=IU1+RU5;
+         RUS2=RU2-IU6; IUS2=IU2+RU6;
+         RUS3=RU3-IU7; IUS3=IU3+RU7;
+         RUU0=RU0+IU4; IUU0=IU0-RU4;
+         RUU1=RU1+IU5; IUU1=IU1-RU5;
+         RUU2=RU2+IU6; IUU2=IU2-RU6;
+         RUU3=RU3+IU7; IUU3=IU3-RU7;
+         T=(RSU1+ISU1)*E2; ISU1=(ISU1-RSU1)*E2; RSU1=T;
+         T=(RSU3+ISU3)*E2; ISU3=(ISU3-RSU3)*E2; RSU3=T;
+         T=RUS1*ER3+IUS1*EI3; IUS1=IUS1*ER3-RUS1*EI3; RUS1=T;
+         T=(RUS2+IUS2)*E2; IUS2=(IUS2-RUS2)*E2; RUS2=T;
+         T=RUS3*ER5+IUS3*EI5; IUS3=IUS3*ER5-RUS3*EI5; RUS3=T;
+         T=RUU1*ER1+IUU1*EI1; IUU1=IUU1*ER1-RUU1*EI1; RUU1=T;
+         T=(RUU2+IUU2)*E2; IUU2=(IUU2-RUU2)*E2; RUU2=T;
+         T=RUU3*ER3+IUU3*EI3; IUU3=IUU3*ER3-RUU3*EI3; RUU3=T;
+         RSSS0=RSS0+RSS2; ISSS0=ISS0+ISS2;
+         RSSS1=RSS1+RSS3; ISSS1=ISS1+ISS3;
+         RSSU0=RSS0-RSS2; ISSU0=ISS0-ISS2;
+         RSSU1=RSS1-RSS3; ISSU1=ISS1-ISS3;
+         RSUS0=RSU0-ISU2; ISUS0=ISU0+RSU2;
+         RSUS1=RSU1-ISU3; ISUS1=ISU1+RSU3;
+         RSUU0=RSU0+ISU2; ISUU0=ISU0-RSU2;
+         RSUU1=RSU1+ISU3; ISUU1=ISU1-RSU3;
+         RUSS0=RUS0-IUS2; IUSS0=IUS0+RUS2;
+         RUSS1=RUS1-IUS3; IUSS1=IUS1+RUS3;
+         RUSU0=RUS0+IUS2; IUSU0=IUS0-RUS2;
+         RUSU1=RUS1+IUS3; IUSU1=IUS1-RUS3;
+         RUUS0=RUU0+RUU2; IUUS0=IUU0+IUU2;
+         RUUS1=RUU1+RUU3; IUUS1=IUU1+IUU3;
+         RUUU0=RUU0-RUU2; IUUU0=IUU0-IUU2;
+         RUUU1=RUU1-RUU3; IUUU1=IUU1-IUU3;
+         X0[K]=RSSS0+RSSS1; Y0[K]=ISSS0+ISSS1;
+         if (!ZERO)
+         {
+            REPORT
+            R1=RUUS0+RUUS1; I1=IUUS0+IUUS1;
+            R2=RSUU0+RSUU1; I2=ISUU0+ISUU1;
+            R3=RUSU0+RUSU1; I3=IUSU0+IUSU1;
+            R4=RSSU0+ISSU1; I4=ISSU0-RSSU1;
+            R5=RUUU0+IUUU1; I5=IUUU0-RUUU1;
+            R6=RSUS0+ISUS1; I6=ISUS0-RSUS1;
+            R7=RUSS0+IUSS1; I7=IUSS0-RUSS1;
+            R8=RSSS0-RSSS1; I8=ISSS0-ISSS1;
+            R9=RUUS0-RUUS1; I9=IUUS0-IUUS1;
+            R10=RSUU0-RSUU1; I10=ISUU0-ISUU1;
+            R11=RUSU0-RUSU1; I11=IUSU0-IUSU1;
+            R12=RSSU0-ISSU1; I12=ISSU0+RSSU1;
+            R13=RUUU0-IUUU1; I13=IUUU0+RUUU1;
+            R14=RSUS0-ISUS1; I14=ISUS0+RSUS1;
+            R15=RUSS0-IUSS1; I15=IUSS0+RUSS1;
+            X8[K]=R1*C1+I1*S1; Y8[K]=I1*C1-R1*S1;
+            X4[K]=R2*C2+I2*S2; Y4[K]=I2*C2-R2*S2;
+            X12[K]=R3*C3+I3*S3; Y12[K]=I3*C3-R3*S3;
+            X2[K]=R4*C4+I4*S4; Y2[K]=I4*C4-R4*S4;
+            X10[K]=R5*C5+I5*S5; Y10[K]=I5*C5-R5*S5;
+            X6[K]=R6*C6+I6*S6; Y6[K]=I6*C6-R6*S6;
+            X14[K]=R7*C7+I7*S7; Y14[K]=I7*C7-R7*S7;
+            X1[K]=R8*C8+I8*S8; Y1[K]=I8*C8-R8*S8;
+            X9[K]=R9*C9+I9*S9; Y9[K]=I9*C9-R9*S9;
+            X5[K]=R10*C10+I10*S10; Y5[K]=I10*C10-R10*S10;
+            X13[K]=R11*C11+I11*S11; Y13[K]=I11*C11-R11*S11;
+            X3[K]=R12*C12+I12*S12; Y3[K]=I12*C12-R12*S12;
+            X11[K]=R13*C13+I13*S13; Y11[K]=I13*C13-R13*S13;
+            X7[K]=R14*C14+I14*S14; Y7[K]=I14*C14-R14*S14;
+            X15[K]=R15*C15+I15*S15; Y15[K]=I15*C15-R15*S15;
+         }
+         else
+         {
+            REPORT
+            X8[K]=RUUS0+RUUS1; Y8[K]=IUUS0+IUUS1;
+            X4[K]=RSUU0+RSUU1; Y4[K]=ISUU0+ISUU1;
+            X12[K]=RUSU0+RUSU1; Y12[K]=IUSU0+IUSU1;
+            X2[K]=RSSU0+ISSU1; Y2[K]=ISSU0-RSSU1;
+            X10[K]=RUUU0+IUUU1; Y10[K]=IUUU0-RUUU1;
+            X6[K]=RSUS0+ISUS1; Y6[K]=ISUS0-RSUS1;
+            X14[K]=RUSS0+IUSS1; Y14[K]=IUSS0-RUSS1;
+            X1[K]=RSSS0-RSSS1; Y1[K]=ISSS0-ISSS1;
+            X9[K]=RUUS0-RUUS1; Y9[K]=IUUS0-IUUS1;
+            X5[K]=RSUU0-RSUU1; Y5[K]=ISUU0-ISUU1;
+            X13[K]=RUSU0-RUSU1; Y13[K]=IUSU0-IUSU1;
+            X3[K]=RSSU0-ISSU1; Y3[K]=ISSU0+RSSU1;
+            X11[K]=RUUU0-IUUU1; Y11[K]=IUUU0+RUUU1;
+            X7[K]=RSUS0-ISUS1; Y7[K]=ISUS0+RSUS1;
+            X15[K]=RUSS0-IUSS1; Y15[K]=IUSS0+RUSS1;
+         }
+      }
+      goto L100;
+   L600: ;
+   }
+
+   return;
+}
+
+// can the number of points be factorised sufficiently
+// for the fft to run
+
+bool FFT_Controller::CanFactor(int PTS)
+{
+   REPORT
+   const int NP = 16, NQ = 10, PMAX=19;
+
+   if (PTS<=1) { REPORT return true; }
+
+   int N = PTS, F = 2, P = 0, Q = 0;
+
+   while (N > 1)
+   {
+      bool fail = true;
+      for (int J = F; J <= PMAX; J++)
+         if (N % J == 0) { fail = false; F=J; break; }
+      if (fail || P >= NP || Q >= NQ) { REPORT return false; }
+      N /= F;
+      if (N % F != 0) Q++; else { N /= F; P++; }
+   }
+
+   return true;    // can factorise
+
+}
+
+bool FFT_Controller::OnlyOldFFT;         // static variable
+
+// **************************** multi radix counter **********************
+
+MultiRadixCounter::MultiRadixCounter(int nx, const SimpleIntArray& rx,
+   SimpleIntArray& vx)
+   : Radix(rx), Value(vx), n(nx), reverse(0),
+      product(1), counter(0), finish(false)
+{
+   REPORT for (int k = 0; k < n; k++) { Value[k] = 0; product *= Radix[k]; }
+}
+
+void MultiRadixCounter::operator++()
+{
+   REPORT
+   counter++; int p = product;
+   for (int k = 0; k < n; k++)
+   {
+      Value[k]++; int p1 = p / Radix[k]; reverse += p1;
+      if (Value[k] == Radix[k]) { REPORT Value[k] = 0; reverse -= p; p = p1; }
+      else { REPORT return; }
+   }
+   finish = true;
+}
+
+
+static int BitReverse(int x, int prod, int n, const SimpleIntArray& f)
+{
+   // x = c[0]+f[0]*(c[1]+f[1]*(c[2]+...
+   // return c[n-1]+f[n-1]*(c[n-2]+f[n-2]*(c[n-3]+...
+   // prod is the product of the f[i]
+   // n is the number of f[i] (don't assume f has the correct length)
+
+   REPORT
+   const int* d = f.Data() + n; int sum = 0; int q = 1;
+   while (n--)
+   {
+      prod /= *(--d);
+      int c = x / prod; x-= c * prod;
+      sum += q * c; q *= *d;
+   }
+   return sum;
+}
+
+
+#ifdef use_namespace
+}
+#endif
+
+
Index: orxonox/trunk/src/util/newmat/newmat.h
===================================================================
--- orxonox/trunk/src/util/newmat/newmat.h	(revision 4565)
+++ orxonox/trunk/src/util/newmat/newmat.h	(revision 4565)
@@ -0,0 +1,1797 @@
+//$$ newmat.h           definition file for new version of matrix package
+
+// Copyright (C) 1991,2,3,4,7,2000,2002: R B Davies
+
+#ifndef NEWMAT_LIB
+#define NEWMAT_LIB 0
+
+#include "include.h"
+
+#include "boolean.h"
+#include "myexcept.h"
+
+
+#ifdef use_namespace
+namespace NEWMAT { using namespace RBD_COMMON; }
+namespace RBD_LIBRARIES { using namespace NEWMAT; }
+namespace NEWMAT {
+#endif
+
+//#define DO_REPORT                     // to activate REPORT
+
+#ifdef NO_LONG_NAMES
+#define UpperTriangularMatrix UTMatrix
+#define LowerTriangularMatrix LTMatrix
+#define SymmetricMatrix SMatrix
+#define DiagonalMatrix DMatrix
+#define BandMatrix BMatrix
+#define UpperBandMatrix UBMatrix
+#define LowerBandMatrix LBMatrix
+#define SymmetricBandMatrix SBMatrix
+#define BandLUMatrix BLUMatrix
+#endif
+
+#ifndef TEMPS_DESTROYED_QUICKLY_R
+#define ReturnMatrix ReturnMatrixX
+#else
+#define ReturnMatrix ReturnMatrixX&
+#endif
+
+// ************************** general utilities ****************************/
+
+class GeneralMatrix;
+
+void MatrixErrorNoSpace(void*);                 // no space handler
+
+class LogAndSign
+// Return from LogDeterminant function
+//    - value of the log plus the sign (+, - or 0)
+{
+   Real log_value;
+   int sign;
+public:
+   LogAndSign() { log_value=0.0; sign=1; }
+   LogAndSign(Real);
+   void operator*=(Real);
+   void PowEq(int k);  // raise to power of k
+   void ChangeSign() { sign = -sign; }
+   Real LogValue() const { return log_value; }
+   int Sign() const { return sign; }
+   Real Value() const;
+   FREE_CHECK(LogAndSign)
+};
+
+// the following class is for counting the number of times a piece of code
+// is executed. It is used for locating any code not executed by test
+// routines. Use turbo GREP locate all places this code is called and
+// check which ones are not accessed.
+// Somewhat implementation dependent as it relies on "cout" still being
+// present when ExeCounter objects are destructed.
+
+#ifdef DO_REPORT
+
+class ExeCounter
+{
+   int line;                                    // code line number
+   int fileid;                                  // file identifier
+   long nexe;                                   // number of executions
+   static int nreports;                         // number of reports
+public:
+   ExeCounter(int,int);
+   void operator++() { nexe++; }
+   ~ExeCounter();                               // prints out reports
+};
+
+#endif
+
+
+// ************************** class MatrixType *****************************/
+
+// Is used for finding the type of a matrix resulting from the binary operations
+// +, -, * and identifying what conversions are permissible.
+// This class must be updated when new matrix types are added.
+
+class GeneralMatrix;                            // defined later
+class BaseMatrix;                               // defined later
+class MatrixInput;                              // defined later
+
+class MatrixType
+{
+public:
+   enum Attribute {  Valid     = 1,
+                     Diagonal  = 2,             // order of these is important
+                     Symmetric = 4,
+                     Band      = 8,
+                     Lower     = 16,
+                     Upper     = 32,
+                     LUDeco    = 64,
+                     Ones      = 128 };
+
+   enum            { US = 0,
+                     UT = Valid + Upper,
+                     LT = Valid + Lower,
+                     Rt = Valid,
+                     Sm = Valid + Symmetric,
+                     Dg = Valid + Diagonal + Band + Lower + Upper + Symmetric,
+                     Id = Valid + Diagonal + Band + Lower + Upper + Symmetric
+                        + Ones,
+                     RV = Valid,     //   do not separate out
+                     CV = Valid,     //   vectors
+                     BM = Valid + Band,
+                     UB = Valid + Band + Upper,
+                     LB = Valid + Band + Lower,
+                     SB = Valid + Band + Symmetric,
+                     Ct = Valid + LUDeco,
+                     BC = Valid + Band + LUDeco
+                   };
+
+
+   static int nTypes() { return 10; }           // number of different types
+					       // exclude Ct, US, BC
+public:
+   int attribute;
+   bool DataLossOK;                            // true if data loss is OK when
+                                               // this represents a destination
+public:
+   MatrixType () : DataLossOK(false) {}
+   MatrixType (int i) : attribute(i), DataLossOK(false) {}
+   MatrixType (int i, bool dlok) : attribute(i), DataLossOK(dlok) {}
+   MatrixType (const MatrixType& mt)
+      : attribute(mt.attribute), DataLossOK(mt.DataLossOK) {}
+   void operator=(const MatrixType& mt)
+      { attribute = mt.attribute; DataLossOK = mt.DataLossOK; }
+   void SetDataLossOK() { DataLossOK = true; }
+   int operator+() const { return attribute; }
+   MatrixType operator+(MatrixType mt) const
+      { return MatrixType(attribute & mt.attribute); }
+   MatrixType operator*(const MatrixType&) const;
+   MatrixType SP(const MatrixType&) const;
+   MatrixType KP(const MatrixType&) const;
+   MatrixType operator|(const MatrixType& mt) const
+      { return MatrixType(attribute & mt.attribute & Valid); }
+   MatrixType operator&(const MatrixType& mt) const
+      { return MatrixType(attribute & mt.attribute & Valid); }
+   bool operator>=(MatrixType mt) const
+      { return ( attribute & mt.attribute ) == attribute; }
+   bool operator<(MatrixType mt) const         // for MS Visual C++ 4
+      { return ( attribute & mt.attribute ) != attribute; }
+   bool operator==(MatrixType t) const
+      { return (attribute == t.attribute); }
+   bool operator!=(MatrixType t) const
+      { return (attribute != t.attribute); }
+   bool operator!() const { return (attribute & Valid) == 0; }
+   MatrixType i() const;                       // type of inverse
+   MatrixType t() const;                       // type of transpose
+   MatrixType AddEqualEl() const               // Add constant to matrix
+      { return MatrixType(attribute & (Valid + Symmetric)); }
+   MatrixType MultRHS() const;                 // type for rhs of multiply
+   MatrixType sub() const                      // type of submatrix
+      { return MatrixType(attribute & Valid); }
+   MatrixType ssub() const                     // type of sym submatrix
+      { return MatrixType(attribute); }        // not for selection matrix
+   GeneralMatrix* New() const;                 // new matrix of given type
+   GeneralMatrix* New(int,int,BaseMatrix*) const;
+                                               // new matrix of given type
+   const char* Value() const;                  // to print type
+   friend bool Rectangular(MatrixType a, MatrixType b, MatrixType c);
+   friend bool Compare(const MatrixType&, MatrixType&);
+                                               // compare and check conv.
+   bool IsBand() const { return (attribute & Band) != 0; }
+   bool IsDiagonal() const { return (attribute & Diagonal) != 0; }
+   bool IsSymmetric() const { return (attribute & Symmetric) != 0; }
+   bool CannotConvert() const { return (attribute & LUDeco) != 0; }
+                                               // used by operator== 
+   FREE_CHECK(MatrixType)
+};
+
+
+// *********************** class MatrixBandWidth ***********************/
+
+class MatrixBandWidth
+{
+public:
+   int lower;
+   int upper;
+   MatrixBandWidth(const int l, const int u) : lower(l), upper (u) {}
+   MatrixBandWidth(const int i) : lower(i), upper(i) {}
+   MatrixBandWidth operator+(const MatrixBandWidth&) const;
+   MatrixBandWidth operator*(const MatrixBandWidth&) const;
+   MatrixBandWidth minimum(const MatrixBandWidth&) const;
+   MatrixBandWidth t() const { return MatrixBandWidth(upper,lower); }
+   bool operator==(const MatrixBandWidth& bw) const
+      { return (lower == bw.lower) && (upper == bw.upper); }
+   bool operator!=(const MatrixBandWidth& bw) const { return !operator==(bw); }
+   int Upper() const { return upper; }
+   int Lower() const { return lower; }
+   FREE_CHECK(MatrixBandWidth)
+};
+
+
+// ********************* Array length specifier ************************/
+
+// This class is introduced to avoid constructors such as
+//   ColumnVector(int)
+// being used for conversions
+
+class ArrayLengthSpecifier
+{
+   int value;
+public:
+   int Value() const { return value; }
+   ArrayLengthSpecifier(int l) : value(l) {}
+};
+
+// ************************* Matrix routines ***************************/
+
+
+class MatrixRowCol;                             // defined later
+class MatrixRow;
+class MatrixCol;
+class MatrixColX;
+
+class GeneralMatrix;                            // defined later
+class AddedMatrix;
+class MultipliedMatrix;
+class SubtractedMatrix;
+class SPMatrix;
+class KPMatrix;
+class ConcatenatedMatrix;
+class StackedMatrix;
+class SolvedMatrix;
+class ShiftedMatrix;
+class NegShiftedMatrix;
+class ScaledMatrix;
+class TransposedMatrix;
+class ReversedMatrix;
+class NegatedMatrix;
+class InvertedMatrix;
+class RowedMatrix;
+class ColedMatrix;
+class DiagedMatrix;
+class MatedMatrix;
+class GetSubMatrix;
+class ReturnMatrixX;
+class Matrix;
+class nricMatrix;
+class RowVector;
+class ColumnVector;
+class SymmetricMatrix;
+class UpperTriangularMatrix;
+class LowerTriangularMatrix;
+class DiagonalMatrix;
+class CroutMatrix;
+class BandMatrix;
+class LowerBandMatrix;
+class UpperBandMatrix;
+class SymmetricBandMatrix;
+class LinearEquationSolver;
+class GenericMatrix;
+
+
+#define MatrixTypeUnSp 0
+//static MatrixType MatrixTypeUnSp(MatrixType::US);
+//						// AT&T needs this
+
+class BaseMatrix : public Janitor               // base of all matrix classes
+{
+protected:
+   virtual int search(const BaseMatrix*) const = 0;
+						// count number of times matrix
+   						// is referred to
+
+public:
+   virtual GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp) = 0;
+						// evaluate temporary
+   // for old version of G++
+   //   virtual GeneralMatrix* Evaluate(MatrixType mt) = 0;
+   //   GeneralMatrix* Evaluate() { return Evaluate(MatrixTypeUnSp); }
+#ifndef TEMPS_DESTROYED_QUICKLY
+   AddedMatrix operator+(const BaseMatrix&) const;    // results of operations
+   MultipliedMatrix operator*(const BaseMatrix&) const;
+   SubtractedMatrix operator-(const BaseMatrix&) const;
+   ConcatenatedMatrix operator|(const BaseMatrix&) const;
+   StackedMatrix operator&(const BaseMatrix&) const;
+   ShiftedMatrix operator+(Real) const;
+   ScaledMatrix operator*(Real) const;
+   ScaledMatrix operator/(Real) const;
+   ShiftedMatrix operator-(Real) const;
+   TransposedMatrix t() const;
+//   TransposedMatrix t;
+   NegatedMatrix operator-() const;                   // change sign of elements
+   ReversedMatrix Reverse() const;
+   InvertedMatrix i() const;
+//   InvertedMatrix i;
+   RowedMatrix AsRow() const;
+   ColedMatrix AsColumn() const;
+   DiagedMatrix AsDiagonal() const;
+   MatedMatrix AsMatrix(int,int) const;
+   GetSubMatrix SubMatrix(int,int,int,int) const;
+   GetSubMatrix SymSubMatrix(int,int) const;
+   GetSubMatrix Row(int) const;
+   GetSubMatrix Rows(int,int) const;
+   GetSubMatrix Column(int) const;
+   GetSubMatrix Columns(int,int) const;
+#else
+   AddedMatrix& operator+(const BaseMatrix&) const;    // results of operations
+   MultipliedMatrix& operator*(const BaseMatrix&) const;
+   SubtractedMatrix& operator-(const BaseMatrix&) const;
+   ConcatenatedMatrix& operator|(const BaseMatrix&) const;
+   StackedMatrix& operator&(const BaseMatrix&) const;
+   ShiftedMatrix& operator+(Real) const;
+   ScaledMatrix& operator*(Real) const;
+   ScaledMatrix& operator/(Real) const;
+   ShiftedMatrix& operator-(Real) const;
+   TransposedMatrix& t() const;
+//   TransposedMatrix& t;
+   NegatedMatrix& operator-() const;                   // change sign of elements
+   ReversedMatrix& Reverse() const;
+   InvertedMatrix& i() const;
+//   InvertedMatrix& i;
+   RowedMatrix& AsRow() const;
+   ColedMatrix& AsColumn() const;
+   DiagedMatrix& AsDiagonal() const;
+   MatedMatrix& AsMatrix(int,int) const;
+   GetSubMatrix& SubMatrix(int,int,int,int) const;
+   GetSubMatrix& SymSubMatrix(int,int) const;
+   GetSubMatrix& Row(int) const;
+   GetSubMatrix& Rows(int,int) const;
+   GetSubMatrix& Column(int) const;
+   GetSubMatrix& Columns(int,int) const;
+#endif
+   Real AsScalar() const;                      // conversion of 1 x 1 matrix
+   virtual LogAndSign LogDeterminant() const;
+   Real Determinant() const;
+   virtual Real SumSquare() const;
+   Real NormFrobenius() const;
+   virtual Real SumAbsoluteValue() const;
+   virtual Real Sum() const;
+   virtual Real MaximumAbsoluteValue() const;
+   virtual Real MaximumAbsoluteValue1(int& i) const;
+   virtual Real MaximumAbsoluteValue2(int& i, int& j) const;
+   virtual Real MinimumAbsoluteValue() const;
+   virtual Real MinimumAbsoluteValue1(int& i) const;
+   virtual Real MinimumAbsoluteValue2(int& i, int& j) const;
+   virtual Real Maximum() const;
+   virtual Real Maximum1(int& i) const;
+   virtual Real Maximum2(int& i, int& j) const;
+   virtual Real Minimum() const;
+   virtual Real Minimum1(int& i) const;
+   virtual Real Minimum2(int& i, int& j) const;
+   virtual Real Trace() const;
+   Real Norm1() const;
+   Real NormInfinity() const;
+   virtual MatrixBandWidth BandWidth() const;  // bandwidths of band matrix
+   virtual void CleanUp() {}                   // to clear store
+   void IEQND() const;                         // called by ineq. ops
+//   virtual ReturnMatrix Reverse() const;       // reverse order of elements
+
+
+//protected:
+//   BaseMatrix() : t(this), i(this) {}
+
+   friend class GeneralMatrix;
+   friend class Matrix;
+   friend class nricMatrix;
+   friend class RowVector;
+   friend class ColumnVector;
+   friend class SymmetricMatrix;
+   friend class UpperTriangularMatrix;
+   friend class LowerTriangularMatrix;
+   friend class DiagonalMatrix;
+   friend class CroutMatrix;
+   friend class BandMatrix;
+   friend class LowerBandMatrix;
+   friend class UpperBandMatrix;
+   friend class SymmetricBandMatrix;
+   friend class AddedMatrix;
+   friend class MultipliedMatrix;
+   friend class SubtractedMatrix;
+   friend class SPMatrix;
+   friend class KPMatrix;
+   friend class ConcatenatedMatrix;
+   friend class StackedMatrix;
+   friend class SolvedMatrix;
+   friend class ShiftedMatrix;
+   friend class NegShiftedMatrix;
+   friend class ScaledMatrix;
+   friend class TransposedMatrix;
+   friend class ReversedMatrix;
+   friend class NegatedMatrix;
+   friend class InvertedMatrix;
+   friend class RowedMatrix;
+   friend class ColedMatrix;
+   friend class DiagedMatrix;
+   friend class MatedMatrix;
+   friend class GetSubMatrix;
+   friend class ReturnMatrixX;
+   friend class LinearEquationSolver;
+   friend class GenericMatrix;
+   NEW_DELETE(BaseMatrix)
+};
+
+
+// ***************************** working classes **************************/
+
+class GeneralMatrix : public BaseMatrix         // declarable matrix types
+{
+   virtual GeneralMatrix* Image() const;        // copy of matrix
+protected:
+   int tag;                                     // shows whether can reuse
+   int nrows, ncols;                            // dimensions
+   int storage;                                 // total store required
+   Real* store;                                 // point to store (0=not set)
+   GeneralMatrix();                             // initialise with no store
+   GeneralMatrix(ArrayLengthSpecifier);         // constructor getting store
+   void Add(GeneralMatrix*, Real);              // sum of GM and Real
+   void Add(Real);                              // add Real to this
+   void NegAdd(GeneralMatrix*, Real);           // Real - GM
+   void NegAdd(Real);                           // this = this - Real
+   void Multiply(GeneralMatrix*, Real);         // product of GM and Real
+   void Multiply(Real);                         // multiply this by Real
+   void Negate(GeneralMatrix*);                 // change sign
+   void Negate();                               // change sign
+   void ReverseElements();                      // internal reverse of elements
+   void ReverseElements(GeneralMatrix*);        // reverse order of elements
+   void operator=(Real);                        // set matrix to constant
+   Real* GetStore();                            // get store or copy
+   GeneralMatrix* BorrowStore(GeneralMatrix*, MatrixType);
+                                                // temporarily access store
+   void GetMatrix(const GeneralMatrix*);        // used by = and initialise
+   void Eq(const BaseMatrix&, MatrixType);      // used by =
+   void Eq(const BaseMatrix&, MatrixType, bool);// used by <<
+   void Eq2(const BaseMatrix&, MatrixType);     // cut down version of Eq
+   int search(const BaseMatrix*) const;
+   virtual GeneralMatrix* Transpose(TransposedMatrix*, MatrixType);
+   void CheckConversion(const BaseMatrix&);     // check conversion OK
+   void ReSize(int, int, int);                  // change dimensions
+   virtual short SimpleAddOK(const GeneralMatrix* gm) { return 0; }
+             // see bandmat.cpp for explanation
+public:
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+   virtual MatrixType Type() const = 0;         // type of a matrix
+   int Nrows() const { return nrows; }          // get dimensions
+   int Ncols() const { return ncols; }
+   int Storage() const { return storage; }
+   Real* Store() const { return store; }
+   virtual ~GeneralMatrix();                    // delete store if set
+   void tDelete();                              // delete if tag permits
+   bool reuse();                                // true if tag allows reuse
+   void Protect() { tag=-1; }                   // cannot delete or reuse
+   int Tag() const { return tag; }
+   bool IsZero() const;                         // test matrix has all zeros
+   void Release() { tag=1; }                    // del store after next use
+   void Release(int t) { tag=t; }               // del store after t accesses
+   void ReleaseAndDelete() { tag=0; }           // delete matrix after use
+   void operator<<(const Real*);                // assignment from an array
+   void operator<<(const BaseMatrix& X)
+      { Eq(X,this->Type(),true); }              // = without checking type
+   void Inject(const GeneralMatrix&);           // copy stored els only
+   void operator+=(const BaseMatrix&);
+   void operator-=(const BaseMatrix&);
+   void operator*=(const BaseMatrix&);
+   void operator|=(const BaseMatrix&);
+   void operator&=(const BaseMatrix&);
+   void operator+=(Real);
+   void operator-=(Real r) { operator+=(-r); }
+   void operator*=(Real);
+   void operator/=(Real r) { operator*=(1.0/r); }
+   virtual GeneralMatrix* MakeSolver();         // for solving
+   virtual void Solver(MatrixColX&, const MatrixColX&) {}
+   virtual void GetRow(MatrixRowCol&) = 0;      // Get matrix row
+   virtual void RestoreRow(MatrixRowCol&) {}    // Restore matrix row
+   virtual void NextRow(MatrixRowCol&);         // Go to next row
+   virtual void GetCol(MatrixRowCol&) = 0;      // Get matrix col
+   virtual void GetCol(MatrixColX&) = 0;        // Get matrix col
+   virtual void RestoreCol(MatrixRowCol&) {}    // Restore matrix col
+   virtual void RestoreCol(MatrixColX&) {}      // Restore matrix col
+   virtual void NextCol(MatrixRowCol&);         // Go to next col
+   virtual void NextCol(MatrixColX&);           // Go to next col
+   Real SumSquare() const;
+   Real SumAbsoluteValue() const;
+   Real Sum() const;
+   Real MaximumAbsoluteValue1(int& i) const;
+   Real MinimumAbsoluteValue1(int& i) const;
+   Real Maximum1(int& i) const;
+   Real Minimum1(int& i) const;
+   Real MaximumAbsoluteValue() const;
+   Real MaximumAbsoluteValue2(int& i, int& j) const;
+   Real MinimumAbsoluteValue() const;
+   Real MinimumAbsoluteValue2(int& i, int& j) const;
+   Real Maximum() const;
+   Real Maximum2(int& i, int& j) const;
+   Real Minimum() const;
+   Real Minimum2(int& i, int& j) const;
+   LogAndSign LogDeterminant() const;
+   virtual bool IsEqual(const GeneralMatrix&) const;
+                                                // same type, same values
+   void CheckStore() const;                     // check store is non-zero
+   virtual void SetParameters(const GeneralMatrix*) {}
+                                                // set parameters in GetMatrix
+   operator ReturnMatrix() const;               // for building a ReturnMatrix
+   ReturnMatrix ForReturn() const;
+   virtual bool SameStorageType(const GeneralMatrix& A) const;
+   virtual void ReSizeForAdd(const GeneralMatrix& A, const GeneralMatrix& B);
+   virtual void ReSizeForSP(const GeneralMatrix& A, const GeneralMatrix& B);
+   virtual void ReSize(const GeneralMatrix& A);
+   MatrixInput operator<<(Real);                // for loading a list
+   MatrixInput operator<<(int f);
+//   ReturnMatrix Reverse() const;                // reverse order of elements
+   void CleanUp();                              // to clear store
+
+   friend class Matrix;
+   friend class nricMatrix;
+   friend class SymmetricMatrix;
+   friend class UpperTriangularMatrix;
+   friend class LowerTriangularMatrix;
+   friend class DiagonalMatrix;
+   friend class CroutMatrix;
+   friend class RowVector;
+   friend class ColumnVector;
+   friend class BandMatrix;
+   friend class LowerBandMatrix;
+   friend class UpperBandMatrix;
+   friend class SymmetricBandMatrix;
+   friend class BaseMatrix;
+   friend class AddedMatrix;
+   friend class MultipliedMatrix;
+   friend class SubtractedMatrix;
+   friend class SPMatrix;
+   friend class KPMatrix;
+   friend class ConcatenatedMatrix;
+   friend class StackedMatrix;
+   friend class SolvedMatrix;
+   friend class ShiftedMatrix;
+   friend class NegShiftedMatrix;
+   friend class ScaledMatrix;
+   friend class TransposedMatrix;
+   friend class ReversedMatrix;
+   friend class NegatedMatrix;
+   friend class InvertedMatrix;
+   friend class RowedMatrix;
+   friend class ColedMatrix;
+   friend class DiagedMatrix;
+   friend class MatedMatrix;
+   friend class GetSubMatrix;
+   friend class ReturnMatrixX;
+   friend class LinearEquationSolver;
+   friend class GenericMatrix;
+   NEW_DELETE(GeneralMatrix)
+};
+
+
+
+class Matrix : public GeneralMatrix             // usual rectangular matrix
+{
+   GeneralMatrix* Image() const;                // copy of matrix
+public:
+   Matrix() {}
+   ~Matrix() {}
+   Matrix(int, int);                            // standard declaration
+   Matrix(const BaseMatrix&);                   // evaluate BaseMatrix
+   void operator=(const BaseMatrix&);
+   void operator=(Real f) { GeneralMatrix::operator=(f); }
+   void operator=(const Matrix& m) { operator=((const BaseMatrix&)m); }
+   MatrixType Type() const;
+   Real& operator()(int, int);                  // access element
+   Real& element(int, int);                     // access element
+   Real operator()(int, int) const;            // access element
+   Real element(int, int) const;               // access element
+#ifdef SETUP_C_SUBSCRIPTS
+   Real* operator[](int m) { return store+m*ncols; }
+   const Real* operator[](int m) const { return store+m*ncols; }
+#endif
+   Matrix(const Matrix& gm) { GetMatrix(&gm); }
+   GeneralMatrix* MakeSolver();
+   Real Trace() const;
+   void GetRow(MatrixRowCol&);
+   void GetCol(MatrixRowCol&);
+   void GetCol(MatrixColX&);
+   void RestoreCol(MatrixRowCol&);
+   void RestoreCol(MatrixColX&);
+   void NextRow(MatrixRowCol&);
+   void NextCol(MatrixRowCol&);
+   void NextCol(MatrixColX&);
+   virtual void ReSize(int,int);           // change dimensions
+      // virtual so we will catch it being used in a vector called as a matrix
+   void ReSize(const GeneralMatrix& A);
+   Real MaximumAbsoluteValue2(int& i, int& j) const;
+   Real MinimumAbsoluteValue2(int& i, int& j) const;
+   Real Maximum2(int& i, int& j) const;
+   Real Minimum2(int& i, int& j) const;
+   friend Real DotProduct(const Matrix& A, const Matrix& B);
+   NEW_DELETE(Matrix)
+};
+
+class nricMatrix : public Matrix                // for use with Numerical
+                                                // Recipes in C
+{
+   GeneralMatrix* Image() const;                // copy of matrix
+   Real** row_pointer;                          // points to rows
+   void MakeRowPointer();                       // build rowpointer
+   void DeleteRowPointer();
+public:
+   nricMatrix() {}
+   nricMatrix(int m, int n)                     // standard declaration
+      :  Matrix(m,n) { MakeRowPointer(); }
+   nricMatrix(const BaseMatrix& bm)             // evaluate BaseMatrix
+      :  Matrix(bm) { MakeRowPointer(); }
+   void operator=(const BaseMatrix& bm)
+      { DeleteRowPointer(); Matrix::operator=(bm); MakeRowPointer(); }
+   void operator=(Real f) { GeneralMatrix::operator=(f); }
+   void operator=(const nricMatrix& m) { operator=((const BaseMatrix&)m); }
+   void operator<<(const BaseMatrix& X)
+      { DeleteRowPointer(); Eq(X,this->Type(),true); MakeRowPointer(); }
+   nricMatrix(const nricMatrix& gm) { GetMatrix(&gm); MakeRowPointer(); }
+   void ReSize(int m, int n)               // change dimensions
+      { DeleteRowPointer(); Matrix::ReSize(m,n); MakeRowPointer(); }
+   void ReSize(const GeneralMatrix& A);
+   ~nricMatrix() { DeleteRowPointer(); }
+   Real** nric() const { CheckStore(); return row_pointer-1; }
+   void CleanUp();                                // to clear store
+   NEW_DELETE(nricMatrix)
+};
+
+class SymmetricMatrix : public GeneralMatrix
+{
+   GeneralMatrix* Image() const;                // copy of matrix
+public:
+   SymmetricMatrix() {}
+   ~SymmetricMatrix() {}
+   SymmetricMatrix(ArrayLengthSpecifier);
+   SymmetricMatrix(const BaseMatrix&);
+   void operator=(const BaseMatrix&);
+   void operator=(Real f) { GeneralMatrix::operator=(f); }
+   void operator=(const SymmetricMatrix& m) { operator=((const BaseMatrix&)m); }
+   Real& operator()(int, int);                  // access element
+   Real& element(int, int);                     // access element
+   Real operator()(int, int) const;             // access element
+   Real element(int, int) const;                // access element
+#ifdef SETUP_C_SUBSCRIPTS
+   Real* operator[](int m) { return store+(m*(m+1))/2; }
+   const Real* operator[](int m) const { return store+(m*(m+1))/2; }
+#endif
+   MatrixType Type() const;
+   SymmetricMatrix(const SymmetricMatrix& gm) { GetMatrix(&gm); }
+   Real SumSquare() const;
+   Real SumAbsoluteValue() const;
+   Real Sum() const;
+   Real Trace() const;
+   void GetRow(MatrixRowCol&);
+   void GetCol(MatrixRowCol&);
+   void GetCol(MatrixColX&);
+   void RestoreCol(MatrixRowCol&) {}
+   void RestoreCol(MatrixColX&);
+	GeneralMatrix* Transpose(TransposedMatrix*, MatrixType);
+   void ReSize(int);                       // change dimensions
+   void ReSize(const GeneralMatrix& A);
+   NEW_DELETE(SymmetricMatrix)
+};
+
+class UpperTriangularMatrix : public GeneralMatrix
+{
+   GeneralMatrix* Image() const;                // copy of matrix
+public:
+   UpperTriangularMatrix() {}
+   ~UpperTriangularMatrix() {}
+   UpperTriangularMatrix(ArrayLengthSpecifier);
+   void operator=(const BaseMatrix&);
+   void operator=(const UpperTriangularMatrix& m)
+      { operator=((const BaseMatrix&)m); }
+   UpperTriangularMatrix(const BaseMatrix&);
+   UpperTriangularMatrix(const UpperTriangularMatrix& gm) { GetMatrix(&gm); }
+   void operator=(Real f) { GeneralMatrix::operator=(f); }
+   Real& operator()(int, int);                  // access element
+   Real& element(int, int);                     // access element
+   Real operator()(int, int) const;             // access element
+   Real element(int, int) const;                // access element
+#ifdef SETUP_C_SUBSCRIPTS
+   Real* operator[](int m) { return store+m*ncols-(m*(m+1))/2; }
+   const Real* operator[](int m) const { return store+m*ncols-(m*(m+1))/2; }
+#endif
+   MatrixType Type() const;
+   GeneralMatrix* MakeSolver() { return this; } // for solving
+   void Solver(MatrixColX&, const MatrixColX&);
+   LogAndSign LogDeterminant() const;
+   Real Trace() const;
+   void GetRow(MatrixRowCol&);
+   void GetCol(MatrixRowCol&);
+   void GetCol(MatrixColX&);
+   void RestoreCol(MatrixRowCol&);
+   void RestoreCol(MatrixColX& c) { RestoreCol((MatrixRowCol&)c); }
+   void NextRow(MatrixRowCol&);
+   void ReSize(int);                       // change dimensions
+   void ReSize(const GeneralMatrix& A);
+   MatrixBandWidth BandWidth() const;
+   NEW_DELETE(UpperTriangularMatrix)
+};
+
+class LowerTriangularMatrix : public GeneralMatrix
+{
+   GeneralMatrix* Image() const;                // copy of matrix
+public:
+   LowerTriangularMatrix() {}
+   ~LowerTriangularMatrix() {}
+   LowerTriangularMatrix(ArrayLengthSpecifier);
+   LowerTriangularMatrix(const LowerTriangularMatrix& gm) { GetMatrix(&gm); }
+   LowerTriangularMatrix(const BaseMatrix& M);
+   void operator=(const BaseMatrix&);
+   void operator=(Real f) { GeneralMatrix::operator=(f); }
+   void operator=(const LowerTriangularMatrix& m)
+      { operator=((const BaseMatrix&)m); }
+   Real& operator()(int, int);                  // access element
+   Real& element(int, int);                     // access element
+   Real operator()(int, int) const;             // access element
+   Real element(int, int) const;                // access element
+#ifdef SETUP_C_SUBSCRIPTS
+   Real* operator[](int m) { return store+(m*(m+1))/2; }
+   const Real* operator[](int m) const { return store+(m*(m+1))/2; }
+#endif
+   MatrixType Type() const;
+   GeneralMatrix* MakeSolver() { return this; } // for solving
+   void Solver(MatrixColX&, const MatrixColX&);
+   LogAndSign LogDeterminant() const;
+   Real Trace() const;
+   void GetRow(MatrixRowCol&);
+   void GetCol(MatrixRowCol&);
+   void GetCol(MatrixColX&);
+   void RestoreCol(MatrixRowCol&);
+   void RestoreCol(MatrixColX& c) { RestoreCol((MatrixRowCol&)c); }
+   void NextRow(MatrixRowCol&);
+   void ReSize(int);                       // change dimensions
+   void ReSize(const GeneralMatrix& A);
+   MatrixBandWidth BandWidth() const;
+   NEW_DELETE(LowerTriangularMatrix)
+};
+
+class DiagonalMatrix : public GeneralMatrix
+{
+   GeneralMatrix* Image() const;                // copy of matrix
+public:
+   DiagonalMatrix() {}
+   ~DiagonalMatrix() {}
+   DiagonalMatrix(ArrayLengthSpecifier);
+   DiagonalMatrix(const BaseMatrix&);
+   DiagonalMatrix(const DiagonalMatrix& gm) { GetMatrix(&gm); }
+   void operator=(const BaseMatrix&);
+   void operator=(Real f) { GeneralMatrix::operator=(f); }
+   void operator=(const DiagonalMatrix& m) { operator=((const BaseMatrix&)m); }
+   Real& operator()(int, int);                  // access element
+   Real& operator()(int);                       // access element
+   Real operator()(int, int) const;             // access element
+   Real operator()(int) const;
+   Real& element(int, int);                     // access element
+   Real& element(int);                          // access element
+   Real element(int, int) const;                // access element
+   Real element(int) const;                     // access element
+#ifdef SETUP_C_SUBSCRIPTS
+   Real& operator[](int m) { return store[m]; }
+   const Real& operator[](int m) const { return store[m]; }
+#endif
+   MatrixType Type() const;
+
+   LogAndSign LogDeterminant() const;
+   Real Trace() const;
+   void GetRow(MatrixRowCol&);
+   void GetCol(MatrixRowCol&);
+   void GetCol(MatrixColX&);
+   void NextRow(MatrixRowCol&);
+   void NextCol(MatrixRowCol&);
+   void NextCol(MatrixColX&);
+   GeneralMatrix* MakeSolver() { return this; } // for solving
+   void Solver(MatrixColX&, const MatrixColX&);
+   GeneralMatrix* Transpose(TransposedMatrix*, MatrixType);
+   void ReSize(int);                       // change dimensions
+   void ReSize(const GeneralMatrix& A);
+   Real* nric() const
+      { CheckStore(); return store-1; }         // for use by NRIC
+   MatrixBandWidth BandWidth() const;
+//   ReturnMatrix Reverse() const;                // reverse order of elements
+   NEW_DELETE(DiagonalMatrix)
+};
+
+class RowVector : public Matrix
+{
+   GeneralMatrix* Image() const;                // copy of matrix
+public:
+   RowVector() { nrows = 1; }
+   ~RowVector() {}
+   RowVector(ArrayLengthSpecifier n) : Matrix(1,n.Value()) {}
+   RowVector(const BaseMatrix&);
+   RowVector(const RowVector& gm) { GetMatrix(&gm); }
+   void operator=(const BaseMatrix&);
+   void operator=(Real f) { GeneralMatrix::operator=(f); }
+   void operator=(const RowVector& m) { operator=((const BaseMatrix&)m); }
+   Real& operator()(int);                       // access element
+   Real& element(int);                          // access element
+   Real operator()(int) const;                  // access element
+   Real element(int) const;                     // access element
+#ifdef SETUP_C_SUBSCRIPTS
+   Real& operator[](int m) { return store[m]; }
+   const Real& operator[](int m) const { return store[m]; }
+#endif
+   MatrixType Type() const;
+   void GetCol(MatrixRowCol&);
+   void GetCol(MatrixColX&);
+   void NextCol(MatrixRowCol&);
+   void NextCol(MatrixColX&);
+   void RestoreCol(MatrixRowCol&) {}
+   void RestoreCol(MatrixColX& c);
+   GeneralMatrix* Transpose(TransposedMatrix*, MatrixType);
+   void ReSize(int);                       // change dimensions
+   void ReSize(int,int);                   // in case access is matrix
+   void ReSize(const GeneralMatrix& A);
+   Real* nric() const
+      { CheckStore(); return store-1; }         // for use by NRIC
+   void CleanUp();                              // to clear store
+   // friend ReturnMatrix GetMatrixRow(Matrix& A, int row);
+   NEW_DELETE(RowVector)
+};
+
+class ColumnVector : public Matrix
+{
+   GeneralMatrix* Image() const;                // copy of matrix
+public:
+   ColumnVector() { ncols = 1; }
+   ~ColumnVector() {}
+   ColumnVector(ArrayLengthSpecifier n) : Matrix(n.Value(),1) {}
+   ColumnVector(const BaseMatrix&);
+   ColumnVector(const ColumnVector& gm) { GetMatrix(&gm); }
+   void operator=(const BaseMatrix&);
+   void operator=(Real f) { GeneralMatrix::operator=(f); }
+   void operator=(const ColumnVector& m) { operator=((const BaseMatrix&)m); }
+   Real& operator()(int);                       // access element
+   Real& element(int);                          // access element
+   Real operator()(int) const;                  // access element
+   Real element(int) const;                     // access element
+#ifdef SETUP_C_SUBSCRIPTS
+   Real& operator[](int m) { return store[m]; }
+   const Real& operator[](int m) const { return store[m]; }
+#endif
+   MatrixType Type() const;
+   GeneralMatrix* Transpose(TransposedMatrix*, MatrixType);
+   void ReSize(int);                       // change dimensions
+   void ReSize(int,int);                   // in case access is matrix
+   void ReSize(const GeneralMatrix& A);
+   Real* nric() const
+      { CheckStore(); return store-1; }         // for use by NRIC
+   void CleanUp();                              // to clear store
+//   ReturnMatrix Reverse() const;                // reverse order of elements
+   NEW_DELETE(ColumnVector)
+};
+
+class CroutMatrix : public GeneralMatrix        // for LU decomposition
+{
+   int* indx;
+   bool d;
+   bool sing;
+   void ludcmp();
+public:
+   CroutMatrix(const BaseMatrix&);
+   MatrixType Type() const;
+   void lubksb(Real*, int=0);
+   ~CroutMatrix();
+   GeneralMatrix* MakeSolver() { return this; } // for solving
+   LogAndSign LogDeterminant() const;
+   void Solver(MatrixColX&, const MatrixColX&);
+   void GetRow(MatrixRowCol&);
+   void GetCol(MatrixRowCol&);
+   void GetCol(MatrixColX& c) { GetCol((MatrixRowCol&)c); }
+   void operator=(const BaseMatrix&);
+   void operator=(const CroutMatrix& m) { operator=((const BaseMatrix&)m); }
+   void CleanUp();                                // to clear store
+   bool IsEqual(const GeneralMatrix&) const;
+   bool IsSingular() const { return sing; }
+   NEW_DELETE(CroutMatrix)
+};
+
+// ***************************** band matrices ***************************/
+
+class BandMatrix : public GeneralMatrix         // band matrix
+{
+   GeneralMatrix* Image() const;                // copy of matrix
+protected:
+   void CornerClear() const;                    // set unused elements to zero
+   short SimpleAddOK(const GeneralMatrix* gm);
+public:
+   int lower, upper;                            // band widths
+   BandMatrix() { lower=0; upper=0; CornerClear(); }
+   ~BandMatrix() {}
+   BandMatrix(int n,int lb,int ub) { ReSize(n,lb,ub); CornerClear(); }
+                                                // standard declaration
+   BandMatrix(const BaseMatrix&);               // evaluate BaseMatrix
+   void operator=(const BaseMatrix&);
+   void operator=(Real f) { GeneralMatrix::operator=(f); }
+   void operator=(const BandMatrix& m) { operator=((const BaseMatrix&)m); }
+   MatrixType Type() const;
+   Real& operator()(int, int);                  // access element
+   Real& element(int, int);                     // access element
+   Real operator()(int, int) const;             // access element
+   Real element(int, int) const;                // access element
+#ifdef SETUP_C_SUBSCRIPTS
+   Real* operator[](int m) { return store+(upper+lower)*m+lower; }
+   const Real* operator[](int m) const { return store+(upper+lower)*m+lower; }
+#endif
+   BandMatrix(const BandMatrix& gm) { GetMatrix(&gm); }
+   LogAndSign LogDeterminant() const;
+   GeneralMatrix* MakeSolver();
+   Real Trace() const;
+   Real SumSquare() const { CornerClear(); return GeneralMatrix::SumSquare(); }
+   Real SumAbsoluteValue() const
+      { CornerClear(); return GeneralMatrix::SumAbsoluteValue(); }
+   Real Sum() const
+      { CornerClear(); return GeneralMatrix::Sum(); }
+   Real MaximumAbsoluteValue() const
+      { CornerClear(); return GeneralMatrix::MaximumAbsoluteValue(); }
+   Real MinimumAbsoluteValue() const
+      { int i, j; return GeneralMatrix::MinimumAbsoluteValue2(i, j); }
+   Real Maximum() const { int i, j; return GeneralMatrix::Maximum2(i, j); }
+   Real Minimum() const { int i, j; return GeneralMatrix::Minimum2(i, j); }
+   void GetRow(MatrixRowCol&);
+   void GetCol(MatrixRowCol&);
+   void GetCol(MatrixColX&);
+   void RestoreCol(MatrixRowCol&);
+   void RestoreCol(MatrixColX& c) { RestoreCol((MatrixRowCol&)c); }
+   void NextRow(MatrixRowCol&);
+   virtual void ReSize(int, int, int);             // change dimensions
+   void ReSize(const GeneralMatrix& A);
+   bool SameStorageType(const GeneralMatrix& A) const;
+   void ReSizeForAdd(const GeneralMatrix& A, const GeneralMatrix& B);
+   void ReSizeForSP(const GeneralMatrix& A, const GeneralMatrix& B);
+   MatrixBandWidth BandWidth() const;
+   void SetParameters(const GeneralMatrix*);
+   MatrixInput operator<<(Real);                // will give error
+   MatrixInput operator<<(int f);
+   void operator<<(const Real* r);              // will give error
+      // the next is included because Zortech and Borland
+      // cannot find the copy in GeneralMatrix
+   void operator<<(const BaseMatrix& X) { GeneralMatrix::operator<<(X); }
+   NEW_DELETE(BandMatrix)
+};
+
+class UpperBandMatrix : public BandMatrix       // upper band matrix
+{
+   GeneralMatrix* Image() const;                // copy of matrix
+public:
+   UpperBandMatrix() {}
+   ~UpperBandMatrix() {}
+   UpperBandMatrix(int n, int ubw)              // standard declaration
+      : BandMatrix(n, 0, ubw) {}
+   UpperBandMatrix(const BaseMatrix&);          // evaluate BaseMatrix
+   void operator=(const BaseMatrix&);
+   void operator=(Real f) { GeneralMatrix::operator=(f); }
+   void operator=(const UpperBandMatrix& m)
+      { operator=((const BaseMatrix&)m); }
+   MatrixType Type() const;
+   UpperBandMatrix(const UpperBandMatrix& gm) { GetMatrix(&gm); }
+   GeneralMatrix* MakeSolver() { return this; }
+   void Solver(MatrixColX&, const MatrixColX&);
+   LogAndSign LogDeterminant() const;
+   void ReSize(int, int, int);             // change dimensions
+   void ReSize(int n,int ubw)              // change dimensions
+      { BandMatrix::ReSize(n,0,ubw); }
+   void ReSize(const GeneralMatrix& A) { BandMatrix::ReSize(A); }
+   Real& operator()(int, int);
+   Real operator()(int, int) const;
+   Real& element(int, int);
+   Real element(int, int) const;
+#ifdef SETUP_C_SUBSCRIPTS
+   Real* operator[](int m) { return store+upper*m; }
+   const Real* operator[](int m) const { return store+upper*m; }
+#endif
+   NEW_DELETE(UpperBandMatrix)
+};
+
+class LowerBandMatrix : public BandMatrix       // upper band matrix
+{
+   GeneralMatrix* Image() const;                // copy of matrix
+public:
+   LowerBandMatrix() {}
+   ~LowerBandMatrix() {}
+   LowerBandMatrix(int n, int lbw)              // standard declaration
+      : BandMatrix(n, lbw, 0) {}
+   LowerBandMatrix(const BaseMatrix&);          // evaluate BaseMatrix
+   void operator=(const BaseMatrix&);
+   void operator=(Real f) { GeneralMatrix::operator=(f); }
+   void operator=(const LowerBandMatrix& m)
+      { operator=((const BaseMatrix&)m); }
+   MatrixType Type() const;
+   LowerBandMatrix(const LowerBandMatrix& gm) { GetMatrix(&gm); }
+   GeneralMatrix* MakeSolver() { return this; }
+   void Solver(MatrixColX&, const MatrixColX&);
+   LogAndSign LogDeterminant() const;
+   void ReSize(int, int, int);             // change dimensions
+   void ReSize(int n,int lbw)             // change dimensions
+      { BandMatrix::ReSize(n,lbw,0); }
+   void ReSize(const GeneralMatrix& A) { BandMatrix::ReSize(A); }
+   Real& operator()(int, int);
+   Real operator()(int, int) const;
+   Real& element(int, int);
+   Real element(int, int) const;
+#ifdef SETUP_C_SUBSCRIPTS
+   Real* operator[](int m) { return store+lower*(m+1); }
+   const Real* operator[](int m) const { return store+lower*(m+1); }
+#endif
+   NEW_DELETE(LowerBandMatrix)
+};
+
+class SymmetricBandMatrix : public GeneralMatrix
+{
+   GeneralMatrix* Image() const;                // copy of matrix
+   void CornerClear() const;                    // set unused elements to zero
+   short SimpleAddOK(const GeneralMatrix* gm);
+public:
+   int lower;                                   // lower band width
+   SymmetricBandMatrix() { lower=0; CornerClear(); }
+   ~SymmetricBandMatrix() {}
+   SymmetricBandMatrix(int n, int lb) { ReSize(n,lb); CornerClear(); }
+   SymmetricBandMatrix(const BaseMatrix&);
+   void operator=(const BaseMatrix&);
+   void operator=(Real f) { GeneralMatrix::operator=(f); }
+   void operator=(const SymmetricBandMatrix& m)
+      { operator=((const BaseMatrix&)m); }
+   Real& operator()(int, int);                  // access element
+   Real& element(int, int);                     // access element
+   Real operator()(int, int) const;             // access element
+   Real element(int, int) const;                // access element
+#ifdef SETUP_C_SUBSCRIPTS
+   Real* operator[](int m) { return store+lower*(m+1); }
+   const Real* operator[](int m) const { return store+lower*(m+1); }
+#endif
+   MatrixType Type() const;
+   SymmetricBandMatrix(const SymmetricBandMatrix& gm) { GetMatrix(&gm); }
+   GeneralMatrix* MakeSolver();
+   Real SumSquare() const;
+   Real SumAbsoluteValue() const;
+   Real Sum() const;
+   Real MaximumAbsoluteValue() const
+      { CornerClear(); return GeneralMatrix::MaximumAbsoluteValue(); }
+   Real MinimumAbsoluteValue() const
+      { int i, j; return GeneralMatrix::MinimumAbsoluteValue2(i, j); }
+   Real Maximum() const { int i, j; return GeneralMatrix::Maximum2(i, j); }
+   Real Minimum() const { int i, j; return GeneralMatrix::Minimum2(i, j); }
+   Real Trace() const;
+   LogAndSign LogDeterminant() const;
+   void GetRow(MatrixRowCol&);
+   void GetCol(MatrixRowCol&);
+   void GetCol(MatrixColX&);
+   void RestoreCol(MatrixRowCol&) {}
+   void RestoreCol(MatrixColX&);
+   GeneralMatrix* Transpose(TransposedMatrix*, MatrixType);
+   void ReSize(int,int);                       // change dimensions
+   void ReSize(const GeneralMatrix& A);
+   bool SameStorageType(const GeneralMatrix& A) const;
+   void ReSizeForAdd(const GeneralMatrix& A, const GeneralMatrix& B);
+   void ReSizeForSP(const GeneralMatrix& A, const GeneralMatrix& B);
+   MatrixBandWidth BandWidth() const;
+   void SetParameters(const GeneralMatrix*);
+   NEW_DELETE(SymmetricBandMatrix)
+};
+
+class BandLUMatrix : public GeneralMatrix
+// for LU decomposition of band matrix
+{
+   int* indx;
+   bool d;
+   bool sing;                                   // true if singular
+   Real* store2;
+   int storage2;
+   void ludcmp();
+   int m1,m2;                                   // lower and upper
+public:
+   BandLUMatrix(const BaseMatrix&);
+   MatrixType Type() const;
+   void lubksb(Real*, int=0);
+   ~BandLUMatrix();
+   GeneralMatrix* MakeSolver() { return this; } // for solving
+   LogAndSign LogDeterminant() const;
+   void Solver(MatrixColX&, const MatrixColX&);
+   void GetRow(MatrixRowCol&);
+   void GetCol(MatrixRowCol&);
+   void GetCol(MatrixColX& c) { GetCol((MatrixRowCol&)c); }
+   void operator=(const BaseMatrix&);
+   void operator=(const BandLUMatrix& m) { operator=((const BaseMatrix&)m); }
+   void CleanUp();                                // to clear store
+   bool IsEqual(const GeneralMatrix&) const;
+   bool IsSingular() const { return sing; }
+   NEW_DELETE(BandLUMatrix)
+};
+
+// ************************** special matrices ****************************
+
+class IdentityMatrix : public GeneralMatrix
+{
+   GeneralMatrix* Image() const;          // copy of matrix
+public:
+   IdentityMatrix() {}
+   ~IdentityMatrix() {}
+   IdentityMatrix(ArrayLengthSpecifier n) : GeneralMatrix(1)
+      { nrows = ncols = n.Value(); *store = 1; }
+   IdentityMatrix(const IdentityMatrix& gm) { GetMatrix(&gm); }
+   IdentityMatrix(const BaseMatrix&);
+   void operator=(const BaseMatrix&);
+   void operator=(Real f) { GeneralMatrix::operator=(f); }
+   MatrixType Type() const;
+
+   LogAndSign LogDeterminant() const;
+   Real Trace() const;
+   Real SumSquare() const;
+   Real SumAbsoluteValue() const;
+   Real Sum() const { return Trace(); }
+   void GetRow(MatrixRowCol&);
+   void GetCol(MatrixRowCol&);
+   void GetCol(MatrixColX&);
+   void NextRow(MatrixRowCol&);
+   void NextCol(MatrixRowCol&);
+   void NextCol(MatrixColX&);
+   GeneralMatrix* MakeSolver() { return this; } // for solving
+   void Solver(MatrixColX&, const MatrixColX&);
+   GeneralMatrix* Transpose(TransposedMatrix*, MatrixType);
+   void ReSize(int n);
+   void ReSize(const GeneralMatrix& A);
+   MatrixBandWidth BandWidth() const;
+//   ReturnMatrix Reverse() const;                // reverse order of elements
+   NEW_DELETE(IdentityMatrix)
+};
+
+
+
+
+// ************************** GenericMatrix class ************************/
+
+class GenericMatrix : public BaseMatrix
+{
+   GeneralMatrix* gm;
+   int search(const BaseMatrix* bm) const;
+   friend class BaseMatrix;
+public:
+   GenericMatrix() : gm(0) {}
+   GenericMatrix(const BaseMatrix& bm)
+      { gm = ((BaseMatrix&)bm).Evaluate(); gm = gm->Image(); }
+   GenericMatrix(const GenericMatrix& bm)
+      { gm = bm.gm->Image(); }
+   void operator=(const GenericMatrix&);
+   void operator=(const BaseMatrix&);
+   void operator+=(const BaseMatrix&);
+   void operator-=(const BaseMatrix&);
+   void operator*=(const BaseMatrix&);
+   void operator|=(const BaseMatrix&);
+   void operator&=(const BaseMatrix&);
+   void operator+=(Real);
+   void operator-=(Real r) { operator+=(-r); }
+   void operator*=(Real);
+   void operator/=(Real r) { operator*=(1.0/r); }
+   ~GenericMatrix() { delete gm; }
+   void CleanUp() { delete gm; gm = 0; }
+   void Release() { gm->Release(); }
+   GeneralMatrix* Evaluate(MatrixType = MatrixTypeUnSp);
+   MatrixBandWidth BandWidth() const;
+   NEW_DELETE(GenericMatrix)
+};
+
+// *************************** temporary classes *************************/
+
+class MultipliedMatrix : public BaseMatrix
+{
+protected:
+   // if these union statements cause problems, simply remove them
+   // and declare the items individually
+   union { const BaseMatrix* bm1; GeneralMatrix* gm1; };
+						  // pointers to summands
+   union { const BaseMatrix* bm2; GeneralMatrix* gm2; };
+   MultipliedMatrix(const BaseMatrix* bm1x, const BaseMatrix* bm2x)
+      : bm1(bm1x),bm2(bm2x) {}
+   int search(const BaseMatrix*) const;
+   friend class BaseMatrix;
+   friend class GeneralMatrix;
+   friend class GenericMatrix;
+public:
+   ~MultipliedMatrix() {}
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+   MatrixBandWidth BandWidth() const;
+   NEW_DELETE(MultipliedMatrix)
+};
+
+class AddedMatrix : public MultipliedMatrix
+{
+protected:
+   AddedMatrix(const BaseMatrix* bm1x, const BaseMatrix* bm2x)
+      : MultipliedMatrix(bm1x,bm2x) {}
+
+   friend class BaseMatrix;
+   friend class GeneralMatrix;
+   friend class GenericMatrix;
+public:
+   ~AddedMatrix() {}
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+   MatrixBandWidth BandWidth() const;
+   NEW_DELETE(AddedMatrix)
+};
+
+class SPMatrix : public AddedMatrix
+{
+protected:
+   SPMatrix(const BaseMatrix* bm1x, const BaseMatrix* bm2x)
+      : AddedMatrix(bm1x,bm2x) {}
+
+   friend class BaseMatrix;
+   friend class GeneralMatrix;
+   friend class GenericMatrix;
+public:
+   ~SPMatrix() {}
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+   MatrixBandWidth BandWidth() const;
+
+#ifndef TEMPS_DESTROYED_QUICKLY
+   friend SPMatrix SP(const BaseMatrix&, const BaseMatrix&);
+#else
+   friend SPMatrix& SP(const BaseMatrix&, const BaseMatrix&);
+#endif
+
+   NEW_DELETE(SPMatrix)
+};
+
+class KPMatrix : public MultipliedMatrix
+{
+protected:
+   KPMatrix(const BaseMatrix* bm1x, const BaseMatrix* bm2x)
+      : MultipliedMatrix(bm1x,bm2x) {}
+
+   friend class BaseMatrix;
+   friend class GeneralMatrix;
+   friend class GenericMatrix;
+public:
+   ~KPMatrix() {}
+   MatrixBandWidth BandWidth() const;
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+#ifndef TEMPS_DESTROYED_QUICKLY
+   friend KPMatrix KP(const BaseMatrix&, const BaseMatrix&);
+#else
+   friend KPMatrix& KP(const BaseMatrix&, const BaseMatrix&);
+#endif
+   NEW_DELETE(KPMatrix)
+};
+
+class ConcatenatedMatrix : public MultipliedMatrix
+{
+protected:
+   ConcatenatedMatrix(const BaseMatrix* bm1x, const BaseMatrix* bm2x)
+      : MultipliedMatrix(bm1x,bm2x) {}
+
+   friend class BaseMatrix;
+   friend class GeneralMatrix;
+   friend class GenericMatrix;
+public:
+   ~ConcatenatedMatrix() {}
+   MatrixBandWidth BandWidth() const;
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+   NEW_DELETE(ConcatenatedMatrix)
+};
+
+class StackedMatrix : public ConcatenatedMatrix
+{
+protected:
+   StackedMatrix(const BaseMatrix* bm1x, const BaseMatrix* bm2x)
+      : ConcatenatedMatrix(bm1x,bm2x) {}
+
+   friend class BaseMatrix;
+   friend class GeneralMatrix;
+   friend class GenericMatrix;
+public:
+   ~StackedMatrix() {}
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+   NEW_DELETE(StackedMatrix)
+};
+
+class SolvedMatrix : public MultipliedMatrix
+{
+   SolvedMatrix(const BaseMatrix* bm1x, const BaseMatrix* bm2x)
+      : MultipliedMatrix(bm1x,bm2x) {}
+   friend class BaseMatrix;
+   friend class InvertedMatrix;                        // for operator*
+public:
+   ~SolvedMatrix() {}
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+   MatrixBandWidth BandWidth() const;
+   NEW_DELETE(SolvedMatrix)
+};
+
+class SubtractedMatrix : public AddedMatrix
+{
+   SubtractedMatrix(const BaseMatrix* bm1x, const BaseMatrix* bm2x)
+      : AddedMatrix(bm1x,bm2x) {}
+   friend class BaseMatrix;
+   friend class GeneralMatrix;
+   friend class GenericMatrix;
+public:
+   ~SubtractedMatrix() {}
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+   NEW_DELETE(SubtractedMatrix)
+};
+
+class ShiftedMatrix : public BaseMatrix
+{
+protected:
+   union { const BaseMatrix* bm; GeneralMatrix* gm; };
+   Real f;
+   ShiftedMatrix(const BaseMatrix* bmx, Real fx) : bm(bmx),f(fx) {}
+   int search(const BaseMatrix*) const;
+   friend class BaseMatrix;
+   friend class GeneralMatrix;
+   friend class GenericMatrix;
+public:
+   ~ShiftedMatrix() {}
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+#ifndef TEMPS_DESTROYED_QUICKLY
+   friend ShiftedMatrix operator+(Real f, const BaseMatrix& BM);
+//      { return ShiftedMatrix(&BM, f); }
+#endif
+   NEW_DELETE(ShiftedMatrix)
+};
+
+class NegShiftedMatrix : public ShiftedMatrix
+{
+protected:
+   NegShiftedMatrix(Real fx, const BaseMatrix* bmx) : ShiftedMatrix(bmx,fx) {}
+   friend class BaseMatrix;
+   friend class GeneralMatrix;
+   friend class GenericMatrix;
+public:
+   ~NegShiftedMatrix() {}
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+#ifndef TEMPS_DESTROYED_QUICKLY
+   friend NegShiftedMatrix operator-(Real, const BaseMatrix&);
+#else
+   friend NegShiftedMatrix& operator-(Real, const BaseMatrix&);
+#endif
+   NEW_DELETE(NegShiftedMatrix)
+};
+
+class ScaledMatrix : public ShiftedMatrix
+{
+   ScaledMatrix(const BaseMatrix* bmx, Real fx) : ShiftedMatrix(bmx,fx) {}
+   friend class BaseMatrix;
+   friend class GeneralMatrix;
+   friend class GenericMatrix;
+public:
+   ~ScaledMatrix() {}
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+   MatrixBandWidth BandWidth() const;
+#ifndef TEMPS_DESTROYED_QUICKLY
+   friend ScaledMatrix operator*(Real f, const BaseMatrix& BM);
+      //{ return ScaledMatrix(&BM, f); }
+#endif
+   NEW_DELETE(ScaledMatrix)
+};
+
+class NegatedMatrix : public BaseMatrix
+{
+protected:
+   union { const BaseMatrix* bm; GeneralMatrix* gm; };
+   NegatedMatrix(const BaseMatrix* bmx) : bm(bmx) {}
+   int search(const BaseMatrix*) const;
+private:
+   friend class BaseMatrix;
+public:
+   ~NegatedMatrix() {}
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+   MatrixBandWidth BandWidth() const;
+   NEW_DELETE(NegatedMatrix)
+};
+
+class TransposedMatrix : public NegatedMatrix
+{
+   TransposedMatrix(const BaseMatrix* bmx) : NegatedMatrix(bmx) {}
+   friend class BaseMatrix;
+public:
+   ~TransposedMatrix() {}
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+   MatrixBandWidth BandWidth() const;
+   NEW_DELETE(TransposedMatrix)
+};
+
+class ReversedMatrix : public NegatedMatrix
+{
+   ReversedMatrix(const BaseMatrix* bmx) : NegatedMatrix(bmx) {}
+   friend class BaseMatrix;
+public:
+   ~ReversedMatrix() {}
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+   NEW_DELETE(ReversedMatrix)
+};
+
+class InvertedMatrix : public NegatedMatrix
+{
+   InvertedMatrix(const BaseMatrix* bmx) : NegatedMatrix(bmx) {}
+public:
+   ~InvertedMatrix() {}
+#ifndef TEMPS_DESTROYED_QUICKLY
+   SolvedMatrix operator*(const BaseMatrix&) const;       // inverse(A) * B
+   ScaledMatrix operator*(Real t) const { return BaseMatrix::operator*(t); }
+#else
+   SolvedMatrix& operator*(const BaseMatrix&);            // inverse(A) * B
+   ScaledMatrix& operator*(Real t) const { return BaseMatrix::operator*(t); }
+#endif
+   friend class BaseMatrix;
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+   MatrixBandWidth BandWidth() const;
+   NEW_DELETE(InvertedMatrix)
+};
+
+class RowedMatrix : public NegatedMatrix
+{
+   RowedMatrix(const BaseMatrix* bmx) : NegatedMatrix(bmx) {}
+   friend class BaseMatrix;
+public:
+   ~RowedMatrix() {}
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+   MatrixBandWidth BandWidth() const;
+   NEW_DELETE(RowedMatrix)
+};
+
+class ColedMatrix : public NegatedMatrix
+{
+   ColedMatrix(const BaseMatrix* bmx) : NegatedMatrix(bmx) {}
+   friend class BaseMatrix;
+public:
+   ~ColedMatrix() {}
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+   MatrixBandWidth BandWidth() const;
+   NEW_DELETE(ColedMatrix)
+};
+
+class DiagedMatrix : public NegatedMatrix
+{
+   DiagedMatrix(const BaseMatrix* bmx) : NegatedMatrix(bmx) {}
+   friend class BaseMatrix;
+public:
+   ~DiagedMatrix() {}
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+   MatrixBandWidth BandWidth() const;
+   NEW_DELETE(DiagedMatrix)
+};
+
+class MatedMatrix : public NegatedMatrix
+{
+   int nr, nc;
+   MatedMatrix(const BaseMatrix* bmx, int nrx, int ncx)
+      : NegatedMatrix(bmx), nr(nrx), nc(ncx) {}
+   friend class BaseMatrix;
+public:
+   ~MatedMatrix() {}
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+   MatrixBandWidth BandWidth() const;
+   NEW_DELETE(MatedMatrix)
+};
+
+class ReturnMatrixX : public BaseMatrix    // for matrix return
+{
+   GeneralMatrix* gm;
+   int search(const BaseMatrix*) const;
+public:
+   ~ReturnMatrixX() {}
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+   friend class BaseMatrix;
+#ifdef TEMPS_DESTROYED_QUICKLY_R
+   ReturnMatrixX(const ReturnMatrixX& tm);
+#else
+   ReturnMatrixX(const ReturnMatrixX& tm) : gm(tm.gm) {}
+#endif
+   ReturnMatrixX(const GeneralMatrix* gmx) : gm((GeneralMatrix*&)gmx) {}
+//   ReturnMatrixX(GeneralMatrix&);
+   MatrixBandWidth BandWidth() const;
+   NEW_DELETE(ReturnMatrixX)
+};
+
+
+// ************************** submatrices ******************************/
+
+class GetSubMatrix : public NegatedMatrix
+{
+   int row_skip;
+   int row_number;
+   int col_skip;
+   int col_number;
+   bool IsSym;
+
+   GetSubMatrix
+      (const BaseMatrix* bmx, int rs, int rn, int cs, int cn, bool is)
+      : NegatedMatrix(bmx),
+      row_skip(rs), row_number(rn), col_skip(cs), col_number(cn), IsSym(is) {}
+   void SetUpLHS();
+   friend class BaseMatrix;
+public:
+   GetSubMatrix(const GetSubMatrix& g)
+      : NegatedMatrix(g.bm), row_skip(g.row_skip), row_number(g.row_number),
+      col_skip(g.col_skip), col_number(g.col_number), IsSym(g.IsSym) {}
+   ~GetSubMatrix() {}
+   GeneralMatrix* Evaluate(MatrixType mt=MatrixTypeUnSp);
+   void operator=(const BaseMatrix&);
+   void operator+=(const BaseMatrix&);
+   void operator-=(const BaseMatrix&);
+   void operator=(const GetSubMatrix& m) { operator=((const BaseMatrix&)m); }
+   void operator<<(const BaseMatrix&);
+   void operator<<(const Real*);                // copy from array
+   MatrixInput operator<<(Real);                // for loading a list
+   MatrixInput operator<<(int f);
+   void operator=(Real);                        // copy from constant
+   void operator+=(Real);                       // add constant
+   void operator-=(Real r) { operator+=(-r); }  // subtract constant
+   void operator*=(Real);                       // multiply by constant
+   void operator/=(Real r) { operator*=(1.0/r); } // divide by constant
+   void Inject(const GeneralMatrix&);           // copy stored els only
+   MatrixBandWidth BandWidth() const;
+   NEW_DELETE(GetSubMatrix)
+};
+
+// ******************** linear equation solving ****************************/
+
+class LinearEquationSolver : public BaseMatrix
+{
+   GeneralMatrix* gm;
+   int search(const BaseMatrix*) const { return 0; }
+   friend class BaseMatrix;
+public:
+   LinearEquationSolver(const BaseMatrix& bm);
+   ~LinearEquationSolver() { delete gm; }
+   void CleanUp() { delete gm; } 
+   GeneralMatrix* Evaluate(MatrixType) { return gm; }
+   // probably should have an error message if MatrixType != UnSp
+   NEW_DELETE(LinearEquationSolver)
+};
+
+// ************************** matrix input *******************************/
+
+class MatrixInput          // for reading a list of values into a matrix
+                           // the difficult part is detecting a mismatch
+                           // in the number of elements
+{
+   int n;                  // number values still to be read
+   Real* r;                // pointer to next location to be read to
+public:
+   MatrixInput(const MatrixInput& mi) : n(mi.n), r(mi.r) {}
+   MatrixInput(int nx, Real* rx) : n(nx), r(rx) {}
+   ~MatrixInput();
+   MatrixInput operator<<(Real);
+   MatrixInput operator<<(int f);
+   friend class GeneralMatrix;
+};
+
+
+
+// **************** a very simple integer array class ********************/
+
+// A minimal array class to imitate a C style array but giving dynamic storage
+// mostly intended for internal use by newmat
+
+class SimpleIntArray : public Janitor
+{
+protected:
+   int* a;                    // pointer to the array
+   int n;                     // length of the array
+public:
+   SimpleIntArray(int xn);    // build an array length xn
+   ~SimpleIntArray();         // return the space to memory
+   int& operator[](int i);    // access element of the array - start at 0
+   int operator[](int i) const;
+			      // access element of constant array
+   void operator=(int ai);    // set the array equal to a constant
+   void operator=(const SimpleIntArray& b);
+			      // copy the elements of an array
+   SimpleIntArray(const SimpleIntArray& b);
+			      // make a new array equal to an existing one
+   int Size() const { return n; }
+			      // return the size of the array
+   int* Data() { return a; }  // pointer to the data
+   const int* Data() const { return a; }
+			      // pointer to the data
+   void ReSize(int i, bool keep = false);
+                              // change length, keep data if keep = true
+   void CleanUp() { ReSize(0); }
+   NEW_DELETE(SimpleIntArray)
+};
+
+// *************************** exceptions ********************************/
+
+class NPDException : public Runtime_error     // Not positive definite
+{
+public:
+   static unsigned long Select;          // for identifying exception
+   NPDException(const GeneralMatrix&);
+};
+
+class ConvergenceException : public Runtime_error
+{
+public:
+   static unsigned long Select;          // for identifying exception
+   ConvergenceException(const GeneralMatrix& A);
+   ConvergenceException(const char* c);
+};
+
+class SingularException : public Runtime_error
+{
+public:
+   static unsigned long Select;          // for identifying exception
+   SingularException(const GeneralMatrix& A);
+};
+
+class OverflowException : public Runtime_error
+{
+public:
+   static unsigned long Select;          // for identifying exception
+   OverflowException(const char* c);
+};
+
+class ProgramException : public Logic_error
+{
+protected:
+   ProgramException();
+public:
+   static unsigned long Select;          // for identifying exception
+   ProgramException(const char* c);
+   ProgramException(const char* c, const GeneralMatrix&);
+   ProgramException(const char* c, const GeneralMatrix&, const GeneralMatrix&);
+   ProgramException(const char* c, MatrixType, MatrixType);
+};
+
+class IndexException : public Logic_error
+{
+public:
+   static unsigned long Select;          // for identifying exception
+   IndexException(int i, const GeneralMatrix& A);
+   IndexException(int i, int j, const GeneralMatrix& A);
+   // next two are for access via element function
+   IndexException(int i, const GeneralMatrix& A, bool);
+   IndexException(int i, int j, const GeneralMatrix& A, bool);
+};
+
+class VectorException : public Logic_error    // cannot convert to vector
+{
+public:
+   static unsigned long Select;          // for identifying exception
+   VectorException();
+   VectorException(const GeneralMatrix& A);
+};
+
+class NotSquareException : public Logic_error
+{
+public:
+   static unsigned long Select;          // for identifying exception
+   NotSquareException(const GeneralMatrix& A);
+};
+
+class SubMatrixDimensionException : public Logic_error
+{
+public:
+   static unsigned long Select;          // for identifying exception
+   SubMatrixDimensionException();
+};
+
+class IncompatibleDimensionsException : public Logic_error
+{
+public:
+   static unsigned long Select;          // for identifying exception
+   IncompatibleDimensionsException();
+   IncompatibleDimensionsException(const GeneralMatrix&, const GeneralMatrix&);
+};
+
+class NotDefinedException : public Logic_error
+{
+public:
+   static unsigned long Select;          // for identifying exception
+   NotDefinedException(const char* op, const char* matrix);
+};
+
+class CannotBuildException : public Logic_error
+{
+public:
+   static unsigned long Select;          // for identifying exception
+   CannotBuildException(const char* matrix);
+};
+
+
+class InternalException : public Logic_error
+{
+public:
+   static unsigned long Select;          // for identifying exception
+   InternalException(const char* c);
+};
+
+// ************************ functions ************************************ //
+
+bool operator==(const GeneralMatrix& A, const GeneralMatrix& B);
+bool operator==(const BaseMatrix& A, const BaseMatrix& B);
+inline bool operator!=(const GeneralMatrix& A, const GeneralMatrix& B)
+   { return ! (A==B); }
+inline bool operator!=(const BaseMatrix& A, const BaseMatrix& B)
+   { return ! (A==B); }
+
+   // inequality operators are dummies included for compatibility
+   // with STL. They throw an exception if actually called.
+inline bool operator<=(const BaseMatrix& A, const BaseMatrix&)
+   { A.IEQND(); return true; }
+inline bool operator>=(const BaseMatrix& A, const BaseMatrix&)
+   { A.IEQND(); return true; }
+inline bool operator<(const BaseMatrix& A, const BaseMatrix&)
+   { A.IEQND(); return true; }
+inline bool operator>(const BaseMatrix& A, const BaseMatrix&)
+   { A.IEQND(); return true; }
+
+bool IsZero(const BaseMatrix& A);
+
+
+// ********************* inline functions ******************************** //
+
+
+inline LogAndSign LogDeterminant(const BaseMatrix& B)
+   { return B.LogDeterminant(); }
+inline Real Determinant(const BaseMatrix& B)
+   { return B.Determinant(); }
+inline Real SumSquare(const BaseMatrix& B) { return B.SumSquare(); }
+inline Real NormFrobenius(const BaseMatrix& B) { return B.NormFrobenius(); }
+inline Real Trace(const BaseMatrix& B) { return B.Trace(); }
+inline Real SumAbsoluteValue(const BaseMatrix& B)
+   { return B.SumAbsoluteValue(); }
+inline Real Sum(const BaseMatrix& B)
+   { return B.Sum(); }
+inline Real MaximumAbsoluteValue(const BaseMatrix& B)
+   { return B.MaximumAbsoluteValue(); }
+inline Real MinimumAbsoluteValue(const BaseMatrix& B)
+   { return B.MinimumAbsoluteValue(); }
+inline Real Maximum(const BaseMatrix& B) { return B.Maximum(); }
+inline Real Minimum(const BaseMatrix& B) { return B.Minimum(); }
+inline Real Norm1(const BaseMatrix& B) { return B.Norm1(); }
+inline Real Norm1(RowVector& RV) { return RV.MaximumAbsoluteValue(); }
+inline Real NormInfinity(const BaseMatrix& B) { return B.NormInfinity(); }
+inline Real NormInfinity(ColumnVector& CV)
+   { return CV.MaximumAbsoluteValue(); }
+inline bool IsZero(const GeneralMatrix& A) { return A.IsZero(); }
+
+#ifdef TEMPS_DESTROYED_QUICKLY
+inline ShiftedMatrix& operator+(Real f, const BaseMatrix& BM)
+   { return BM + f; }
+inline ScaledMatrix& operator*(Real f, const BaseMatrix& BM)
+   { return BM * f; }
+#endif
+
+// these are moved out of the class definitions because of a problem
+// with the Intel 8.1 compiler
+#ifndef TEMPS_DESTROYED_QUICKLY
+   inline ShiftedMatrix operator+(Real f, const BaseMatrix& BM)
+      { return ShiftedMatrix(&BM, f); }
+   inline ScaledMatrix operator*(Real f, const BaseMatrix& BM)
+      { return ScaledMatrix(&BM, f); }
+#endif
+
+
+inline MatrixInput MatrixInput::operator<<(int f) { return *this << (Real)f; }
+inline MatrixInput GeneralMatrix::operator<<(int f) { return *this << (Real)f; }
+inline MatrixInput BandMatrix::operator<<(int f) { return *this << (Real)f; }
+inline MatrixInput GetSubMatrix::operator<<(int f) { return *this << (Real)f; }
+
+
+
+#ifdef use_namespace
+}
+#endif
+
+
+#endif
+
+// body file: newmat1.cpp
+// body file: newmat2.cpp
+// body file: newmat3.cpp
+// body file: newmat4.cpp
+// body file: newmat5.cpp
+// body file: newmat6.cpp
+// body file: newmat7.cpp
+// body file: newmat8.cpp
+// body file: newmatex.cpp
+// body file: bandmat.cpp
+// body file: submat.cpp
+
+
+
+
+
+
+
Index: orxonox/trunk/src/util/newmat/newmat1.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/newmat1.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/newmat1.cpp	(revision 4565)
@@ -0,0 +1,177 @@
+//$$ newmat1.cpp   Matrix type functions
+
+// Copyright (C) 1991,2,3,4: R B Davies
+
+//#define WANT_STREAM
+
+#include "newmat.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+#ifdef DO_REPORT
+#define REPORT { static ExeCounter ExeCount(__LINE__,1); ++ExeCount; }
+#else
+#define REPORT {}
+#endif
+
+
+/************************* MatrixType functions *****************************/
+
+
+// all operations to return MatrixTypes which correspond to valid types
+// of matrices.
+// Eg: if it has the Diagonal attribute, then it must also have
+// Upper, Lower, Band and Symmetric.
+
+
+MatrixType MatrixType::operator*(const MatrixType& mt) const
+{
+   REPORT
+   int a = attribute & mt.attribute & ~Symmetric;
+   a |= (a & Diagonal) * 31;                   // recognise diagonal
+   return MatrixType(a);
+}
+
+MatrixType MatrixType::SP(const MatrixType& mt) const
+// elementwise product
+// Lower, Upper, Diag, Band if only one is
+// Symmetric, Ones, Valid (and Real) if both are
+// Need to include Lower & Upper => Diagonal
+// Will need to include both Skew => Symmetric
+{
+   REPORT
+   int a = ((attribute | mt.attribute) & ~(Symmetric + Valid + Ones))
+      | (attribute & mt.attribute);
+   if ((a & Lower) != 0  &&  (a & Upper) != 0) a |= Diagonal;
+   a |= (a & Diagonal) * 31;                   // recognise diagonal
+   return MatrixType(a);
+}
+
+MatrixType MatrixType::KP(const MatrixType& mt) const
+// Kronecker product
+// Lower, Upper, Diag, Symmetric, Band, Valid if both are
+// Do not treat Band separately
+// Ones is complicated so leave this out
+{
+   REPORT
+   int a = (attribute & mt.attribute) & ~Ones;
+   return MatrixType(a);
+}
+
+MatrixType MatrixType::i() const               // type of inverse
+{
+   REPORT
+   int a = attribute & ~(Band+LUDeco);
+   a |= (a & Diagonal) * 31;                   // recognise diagonal
+   return MatrixType(a);
+}
+
+MatrixType MatrixType::t() const
+// swap lower and upper attributes
+// assume Upper is in bit above Lower
+{
+   REPORT
+   int a = attribute;
+   a ^= (((a >> 1) ^ a) & Lower) * 3;
+   return MatrixType(a);
+}
+
+MatrixType MatrixType::MultRHS() const
+{
+   REPORT
+   // remove symmetric attribute unless diagonal
+   return (attribute >= Dg) ? attribute : (attribute & ~Symmetric);
+}
+
+bool Rectangular(MatrixType a, MatrixType b, MatrixType c)
+{
+   REPORT
+   return
+      ((a.attribute | b.attribute | c.attribute) & ~MatrixType::Valid) == 0;
+}
+
+const char* MatrixType::Value() const
+{
+// make a string with the name of matrix with the given attributes
+   switch (attribute)
+   {
+   case Valid:                              REPORT return "Rect ";
+   case Valid+Symmetric:                    REPORT return "Sym  ";
+   case Valid+Band:                         REPORT return "Band ";
+   case Valid+Symmetric+Band:               REPORT return "SmBnd";
+   case Valid+Upper:                        REPORT return "UT   ";
+   case Valid+Diagonal+Symmetric+Band+Upper+Lower:
+                                            REPORT return "Diag ";
+   case Valid+Diagonal+Symmetric+Band+Upper+Lower+Ones:
+                                            REPORT return "Ident";
+   case Valid+Band+Upper:                   REPORT return "UpBnd";
+   case Valid+Lower:                        REPORT return "LT   ";
+   case Valid+Band+Lower:                   REPORT return "LwBnd";
+   default:
+      REPORT
+      if (!(attribute & Valid))             return "UnSp ";
+      if (attribute & LUDeco)
+         return (attribute & Band) ?     "BndLU" : "Crout";
+                                            return "?????";
+   }
+}
+
+
+GeneralMatrix* MatrixType::New(int nr, int nc, BaseMatrix* bm) const
+{
+// make a new matrix with the given attributes
+
+   Tracer tr("New"); GeneralMatrix* gm=0;   // initialised to keep gnu happy
+   switch (attribute)
+   {
+   case Valid:
+      REPORT
+      if (nc==1) { gm = new ColumnVector(nr); break; }
+      if (nr==1) { gm = new RowVector(nc); break; }
+      gm = new Matrix(nr, nc); break;
+
+   case Valid+Symmetric:
+      REPORT gm = new SymmetricMatrix(nr); break;
+
+   case Valid+Band:
+      {
+         REPORT
+         MatrixBandWidth bw = bm->BandWidth();
+         gm = new BandMatrix(nr,bw.lower,bw.upper); break;
+      }
+
+   case Valid+Symmetric+Band:
+      REPORT gm = new SymmetricBandMatrix(nr,bm->BandWidth().lower); break;
+
+   case Valid+Upper:
+      REPORT gm = new UpperTriangularMatrix(nr); break;
+
+   case Valid+Diagonal+Symmetric+Band+Upper+Lower:
+      REPORT gm = new DiagonalMatrix(nr); break;
+
+   case Valid+Band+Upper:
+      REPORT gm = new UpperBandMatrix(nr,bm->BandWidth().upper); break;
+
+   case Valid+Lower:
+      REPORT gm = new LowerTriangularMatrix(nr); break;
+
+   case Valid+Band+Lower:
+      REPORT gm = new LowerBandMatrix(nr,bm->BandWidth().lower); break;
+
+   case Valid+Diagonal+Symmetric+Band+Upper+Lower+Ones:
+      REPORT gm = new IdentityMatrix(nr); break;
+
+   default:
+      Throw(ProgramException("Invalid matrix type"));
+   }
+   
+   MatrixErrorNoSpace(gm); gm->Protect(); return gm;
+}
+
+
+#ifdef use_namespace
+}
+#endif
+
Index: orxonox/trunk/src/util/newmat/newmat2.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/newmat2.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/newmat2.cpp	(revision 4565)
@@ -0,0 +1,633 @@
+//$$ newmat2.cpp      Matrix row and column operations
+
+// Copyright (C) 1991,2,3,4: R B Davies
+
+#define WANT_MATH
+
+#include "include.h"
+
+#include "newmat.h"
+#include "newmatrc.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+
+#ifdef DO_REPORT
+#define REPORT { static ExeCounter ExeCount(__LINE__,2); ++ExeCount; }
+#else
+#define REPORT {}
+#endif
+
+//#define MONITOR(what,storage,store) { cout << what << " " << storage << " at " << (long)store << "\n"; }
+
+#define MONITOR(what,store,storage) {}
+
+/************************** Matrix Row/Col functions ************************/
+
+void MatrixRowCol::Add(const MatrixRowCol& mrc)
+{
+   // THIS += mrc
+   REPORT
+   int f = mrc.skip; int l = f + mrc.storage; int lx = skip + storage;
+   if (f < skip) f = skip; if (l > lx) l = lx; l -= f;
+   if (l<=0) return;
+   Real* elx=data+(f-skip); Real* el=mrc.data+(f-mrc.skip);
+   while (l--) *elx++ += *el++;
+}
+
+void MatrixRowCol::AddScaled(const MatrixRowCol& mrc, Real x)
+{
+   REPORT
+   // THIS += (mrc * x)
+   int f = mrc.skip; int l = f + mrc.storage; int lx = skip + storage;
+   if (f < skip) f = skip; if (l > lx) l = lx; l -= f;
+   if (l<=0) return;
+   Real* elx=data+(f-skip); Real* el=mrc.data+(f-mrc.skip);
+   while (l--) *elx++ += *el++ * x;
+}
+
+void MatrixRowCol::Sub(const MatrixRowCol& mrc)
+{
+   REPORT
+   // THIS -= mrc
+   int f = mrc.skip; int l = f + mrc.storage; int lx = skip + storage;
+   if (f < skip) f = skip; if (l > lx) l = lx; l -= f;
+   if (l<=0) return;
+   Real* elx=data+(f-skip); Real* el=mrc.data+(f-mrc.skip);
+   while (l--) *elx++ -= *el++;
+}
+
+void MatrixRowCol::Inject(const MatrixRowCol& mrc)
+// copy stored elements only
+{
+   REPORT
+   int f = mrc.skip; int l = f + mrc.storage; int lx = skip + storage;
+   if (f < skip) f = skip; if (l > lx) l = lx; l -= f;
+   if (l<=0) return;
+   Real* elx=data+(f-skip); Real* ely=mrc.data+(f-mrc.skip);
+   while (l--) *elx++ = *ely++;
+}
+
+Real DotProd(const MatrixRowCol& mrc1, const MatrixRowCol& mrc2)
+{
+   REPORT                                         // not accessed
+   int f = mrc1.skip; int f2 = mrc2.skip;
+   int l = f + mrc1.storage; int l2 = f2 + mrc2.storage;
+   if (f < f2) f = f2; if (l > l2) l = l2; l -= f;
+   if (l<=0) return 0.0;
+
+   Real* el1=mrc1.data+(f-mrc1.skip); Real* el2=mrc2.data+(f-mrc2.skip);
+   Real sum = 0.0;
+   while (l--) sum += *el1++ * *el2++;
+   return sum;
+}
+
+void MatrixRowCol::Add(const MatrixRowCol& mrc1, const MatrixRowCol& mrc2)
+{
+   // THIS = mrc1 + mrc2
+   int f = skip; int l = skip + storage;
+   int f1 = mrc1.skip; int l1 = f1 + mrc1.storage;
+   if (f1<f) f1=f; if (l1>l) l1=l;
+   int f2 = mrc2.skip; int l2 = f2 + mrc2.storage;
+   if (f2<f) f2=f; if (l2>l) l2=l;
+   Real* el = data + (f-skip);
+   Real* el1 = mrc1.data+(f1-mrc1.skip); Real* el2 = mrc2.data+(f2-mrc2.skip);
+   if (f1<f2)
+   {
+      int i = f1-f; while (i--) *el++ = 0.0;
+      if (l1<=f2)                              // disjoint
+      {
+         REPORT                                // not accessed
+         i = l1-f1;     while (i--) *el++ = *el1++;
+         i = f2-l1;     while (i--) *el++ = 0.0;
+         i = l2-f2;     while (i--) *el++ = *el2++;
+         i = l-l2;      while (i--) *el++ = 0.0;
+      }
+      else
+      {
+         i = f2-f1;    while (i--) *el++ = *el1++;
+         if (l1<=l2)
+         {
+            REPORT
+            i = l1-f2; while (i--) *el++ = *el1++ + *el2++;
+            i = l2-l1; while (i--) *el++ = *el2++;
+            i = l-l2;  while (i--) *el++ = 0.0;
+         }
+         else
+         {
+            REPORT
+            i = l2-f2; while (i--) *el++ = *el1++ + *el2++;
+            i = l1-l2; while (i--) *el++ = *el1++;
+            i = l-l1;  while (i--) *el++ = 0.0;
+         }
+      }
+   }
+   else
+   {
+      int i = f2-f; while (i--) *el++ = 0.0;
+      if (l2<=f1)                              // disjoint
+      {
+         REPORT                                // not accessed
+         i = l2-f2;     while (i--) *el++ = *el2++;
+         i = f1-l2;     while (i--) *el++ = 0.0;
+         i = l1-f1;     while (i--) *el++ = *el1++;
+         i = l-l1;      while (i--) *el++ = 0.0;
+      }
+      else
+      {
+         i = f1-f2;    while (i--) *el++ = *el2++;
+         if (l2<=l1)
+         {
+            REPORT
+            i = l2-f1; while (i--) *el++ = *el1++ + *el2++;
+            i = l1-l2; while (i--) *el++ = *el1++;
+            i = l-l1;  while (i--) *el++ = 0.0;
+         }
+         else
+         {
+            REPORT
+            i = l1-f1; while (i--) *el++ = *el1++ + *el2++;
+            i = l2-l1; while (i--) *el++ = *el2++;
+            i = l-l2;  while (i--) *el++ = 0.0;
+         }
+      }
+   }
+}
+
+void MatrixRowCol::Sub(const MatrixRowCol& mrc1, const MatrixRowCol& mrc2)
+{
+   // THIS = mrc1 - mrc2
+   int f = skip; int l = skip + storage;
+   int f1 = mrc1.skip; int l1 = f1 + mrc1.storage;
+   if (f1<f) f1=f; if (l1>l) l1=l;
+   int f2 = mrc2.skip; int l2 = f2 + mrc2.storage;
+   if (f2<f) f2=f; if (l2>l) l2=l;
+   Real* el = data + (f-skip);
+   Real* el1 = mrc1.data+(f1-mrc1.skip); Real* el2 = mrc2.data+(f2-mrc2.skip);
+   if (f1<f2)
+   {
+      int i = f1-f; while (i--) *el++ = 0.0;
+      if (l1<=f2)                              // disjoint
+      {
+         REPORT                                // not accessed
+         i = l1-f1;     while (i--) *el++ = *el1++;
+         i = f2-l1;     while (i--) *el++ = 0.0;
+         i = l2-f2;     while (i--) *el++ = - *el2++;
+         i = l-l2;      while (i--) *el++ = 0.0;
+      }
+      else
+      {
+         i = f2-f1;    while (i--) *el++ = *el1++;
+         if (l1<=l2)
+         {
+            REPORT
+            i = l1-f2; while (i--) *el++ = *el1++ - *el2++;
+            i = l2-l1; while (i--) *el++ = - *el2++;
+            i = l-l2;  while (i--) *el++ = 0.0;
+         }
+         else
+         {
+            REPORT
+            i = l2-f2; while (i--) *el++ = *el1++ - *el2++;
+            i = l1-l2; while (i--) *el++ = *el1++;
+            i = l-l1;  while (i--) *el++ = 0.0;
+         }
+      }
+   }
+   else
+   {
+      int i = f2-f; while (i--) *el++ = 0.0;
+      if (l2<=f1)                              // disjoint
+      {
+         REPORT                                // not accessed
+         i = l2-f2;     while (i--) *el++ = - *el2++;
+         i = f1-l2;     while (i--) *el++ = 0.0;
+         i = l1-f1;     while (i--) *el++ = *el1++;
+         i = l-l1;      while (i--) *el++ = 0.0;
+      }
+      else
+      {
+         i = f1-f2;    while (i--) *el++ = - *el2++;
+         if (l2<=l1)
+         {
+            REPORT
+            i = l2-f1; while (i--) *el++ = *el1++ - *el2++;
+            i = l1-l2; while (i--) *el++ = *el1++;
+            i = l-l1;  while (i--) *el++ = 0.0;
+         }
+         else
+         {
+            REPORT
+            i = l1-f1; while (i--) *el++ = *el1++ - *el2++;
+            i = l2-l1; while (i--) *el++ = - *el2++;
+            i = l-l2;  while (i--) *el++ = 0.0;
+         }
+      }
+   }
+}
+
+
+void MatrixRowCol::Add(const MatrixRowCol& mrc1, Real x)
+{
+   // THIS = mrc1 + x
+   REPORT
+   if (!storage) return;
+   int f = mrc1.skip; int l = f + mrc1.storage; int lx = skip + storage;
+   if (f < skip) { f = skip; if (l < f) l = f; }
+   if (l > lx) { l = lx; if (f > lx) f = lx; }
+
+   Real* elx = data; Real* ely = mrc1.data+(f-mrc1.skip);
+
+   int l1 = f-skip;  while (l1--) *elx++ = x;
+       l1 = l-f;     while (l1--) *elx++ = *ely++ + x;
+       lx -= l;      while (lx--) *elx++ = x;
+}
+
+void MatrixRowCol::NegAdd(const MatrixRowCol& mrc1, Real x)
+{
+   // THIS = x - mrc1
+   REPORT
+   if (!storage) return;
+   int f = mrc1.skip; int l = f + mrc1.storage; int lx = skip + storage;
+   if (f < skip) { f = skip; if (l < f) l = f; }
+   if (l > lx) { l = lx; if (f > lx) f = lx; }
+
+   Real* elx = data; Real* ely = mrc1.data+(f-mrc1.skip);
+
+   int l1 = f-skip;  while (l1--) *elx++ = x;
+       l1 = l-f;     while (l1--) *elx++ = x - *ely++;
+       lx -= l;      while (lx--) *elx++ = x;
+}
+
+void MatrixRowCol::RevSub(const MatrixRowCol& mrc1)
+{
+   // THIS = mrc - THIS
+   REPORT
+   if (!storage) return;
+   int f = mrc1.skip; int l = f + mrc1.storage; int lx = skip + storage;
+   if (f < skip) { f = skip; if (l < f) l = f; }
+   if (l > lx) { l = lx; if (f > lx) f = lx; }
+
+   Real* elx = data; Real* ely = mrc1.data+(f-mrc1.skip);
+
+   int l1 = f-skip;  while (l1--) { *elx = - *elx; elx++; }
+       l1 = l-f;     while (l1--) { *elx = *ely++ - *elx; elx++; }
+       lx -= l;      while (lx--) { *elx = - *elx; elx++; }
+}
+
+void MatrixRowCol::ConCat(const MatrixRowCol& mrc1, const MatrixRowCol& mrc2)
+{
+   // THIS = mrc1 | mrc2
+   REPORT
+   int f1 = mrc1.skip; int l1 = f1 + mrc1.storage; int lx = skip + storage;
+   if (f1 < skip) { f1 = skip; if (l1 < f1) l1 = f1; }
+   if (l1 > lx) { l1 = lx; if (f1 > lx) f1 = lx; }
+
+   Real* elx = data;
+
+   int i = f1-skip;  while (i--) *elx++ =0.0;
+   i = l1-f1;
+   if (i)                       // in case f1 would take ely out of range
+      { Real* ely = mrc1.data+(f1-mrc1.skip);  while (i--) *elx++ = *ely++; }
+
+   int f2 = mrc2.skip; int l2 = f2 + mrc2.storage; i = mrc1.length;
+   int skipx = l1 - i; lx -= i; // addresses rel to second seg, maybe -ve
+   if (f2 < skipx) { f2 = skipx; if (l2 < f2) l2 = f2; }
+   if (l2 > lx) { l2 = lx; if (f2 > lx) f2 = lx; }
+
+   i = f2-skipx; while (i--) *elx++ = 0.0;
+   i = l2-f2;
+   if (i)                       // in case f2 would take ely out of range
+      { Real* ely = mrc2.data+(f2-mrc2.skip); while (i--) *elx++ = *ely++; }
+   lx -= l2;     while (lx--) *elx++ = 0.0;
+}
+
+void MatrixRowCol::Multiply(const MatrixRowCol& mrc1)
+// element by element multiply into
+{
+   REPORT
+   if (!storage) return;
+   int f = mrc1.skip; int l = f + mrc1.storage; int lx = skip + storage;
+   if (f < skip) { f = skip; if (l < f) l = f; }
+   if (l > lx) { l = lx; if (f > lx) f = lx; }
+
+   Real* elx = data; Real* ely = mrc1.data+(f-mrc1.skip);
+
+   int l1 = f-skip;  while (l1--) *elx++ = 0;
+       l1 = l-f;     while (l1--) *elx++ *= *ely++;
+       lx -= l;      while (lx--) *elx++ = 0;
+}
+
+void MatrixRowCol::Multiply(const MatrixRowCol& mrc1, const MatrixRowCol& mrc2)
+// element by element multiply
+{
+   int f = skip; int l = skip + storage;
+   int f1 = mrc1.skip; int l1 = f1 + mrc1.storage;
+   if (f1<f) f1=f; if (l1>l) l1=l;
+   int f2 = mrc2.skip; int l2 = f2 + mrc2.storage;
+   if (f2<f) f2=f; if (l2>l) l2=l;
+   Real* el = data + (f-skip); int i;
+   if (f1<f2) f1 = f2; if (l1>l2) l1 = l2;
+   if (l1<=f1) { REPORT i = l-f; while (i--) *el++ = 0.0; }  // disjoint
+   else
+   {
+      REPORT
+      Real* el1 = mrc1.data+(f1-mrc1.skip);
+      Real* el2 = mrc2.data+(f1-mrc2.skip);
+      i = f1-f ;    while (i--) *el++ = 0.0;
+      i = l1-f1;    while (i--) *el++ = *el1++ * *el2++;
+      i = l-l1;     while (i--) *el++ = 0.0;
+   }
+}
+
+void MatrixRowCol::KP(const MatrixRowCol& mrc1, const MatrixRowCol& mrc2)
+// row for Kronecker product
+{
+   int f = skip; int s = storage; Real* el = data; int i;
+
+   i = mrc1.skip * mrc2.length;
+   if (i > f)
+   {
+      i -= f; f = 0; if (i > s) { i = s; s = 0; }  else s -= i;
+      while (i--) *el++ = 0.0;
+      if (s == 0) return;
+   }
+   else f -= i;
+
+   i = mrc1.storage; Real* el1 = mrc1.data;
+   int mrc2_skip = mrc2.skip; int mrc2_storage = mrc2.storage;
+   int mrc2_length = mrc2.length;
+   int mrc2_remain = mrc2_length - mrc2_skip - mrc2_storage;
+   while (i--)
+   {
+      int j; Real* el2 = mrc2.data; Real vel1 = *el1;
+      if (f == 0 && mrc2_length <= s)
+      {
+         j = mrc2_skip; s -= j;    while (j--) *el++ = 0.0;
+         j = mrc2_storage; s -= j; while (j--) *el++ = vel1 * *el2++;
+         j = mrc2_remain; s -= j;  while (j--) *el++ = 0.0;
+      }
+      else if (f >= mrc2_length) f -= mrc2_length;
+      else
+      {
+         j = mrc2_skip;
+         if (j > f)
+         {
+            j -= f; f = 0; if (j > s) { j = s; s = 0; } else s -= j;
+            while (j--) *el++ = 0.0;
+         }
+         else f -= j;
+
+         j = mrc2_storage;
+         if (j > f)
+         {
+            j -= f; el2 += f; f = 0; if (j > s) { j = s; s = 0; } else s -= j;
+            while (j--) *el++ = vel1 * *el2++;
+         }
+         else f -= j;
+
+         j = mrc2_remain;
+         if (j > f)
+         {
+            j -= f; f = 0; if (j > s) { j = s; s = 0; } else s -= j;
+            while (j--) *el++ = 0.0;
+         }
+         else f -= j;
+      }
+      if (s == 0) return;
+      ++el1;
+   }
+
+   i = (mrc1.length - mrc1.skip - mrc1.storage) * mrc2.length;
+   if (i > f)
+   {
+      i -= f; if (i > s) i = s;
+      while (i--) *el++ = 0.0;
+   }
+}
+
+
+void MatrixRowCol::Copy(const MatrixRowCol& mrc1)
+{
+   // THIS = mrc1
+   REPORT
+   if (!storage) return;
+   int f = mrc1.skip; int l = f + mrc1.storage; int lx = skip + storage;
+   if (f < skip) { f = skip; if (l < f) l = f; }
+   if (l > lx) { l = lx; if (f > lx) f = lx; }
+
+   Real* elx = data; Real* ely = 0;
+
+   if (l-f) ely = mrc1.data+(f-mrc1.skip);
+
+   int l1 = f-skip;  while (l1--) *elx++ = 0.0;
+       l1 = l-f;     while (l1--) *elx++ = *ely++;
+       lx -= l;      while (lx--) *elx++ = 0.0;
+}
+
+void MatrixRowCol::CopyCheck(const MatrixRowCol& mrc1)
+// Throw an exception if this would lead to a loss of data
+{
+   REPORT
+   if (!storage) return;
+   int f = mrc1.skip; int l = f + mrc1.storage; int lx = skip + storage;
+   if (f < skip || l > lx) Throw(ProgramException("Illegal Conversion"));
+
+   Real* elx = data; Real* ely = mrc1.data+(f-mrc1.skip);
+
+   int l1 = f-skip;  while (l1--) *elx++ = 0.0;
+       l1 = l-f;     while (l1--) *elx++ = *ely++;
+       lx -= l;      while (lx--) *elx++ = 0.0;
+}
+
+void MatrixRowCol::Check(const MatrixRowCol& mrc1)
+// Throw an exception if +=, -=, copy etc would lead to a loss of data
+{
+   REPORT
+   int f = mrc1.skip; int l = f + mrc1.storage; int lx = skip + storage;
+   if (f < skip || l > lx) Throw(ProgramException("Illegal Conversion"));
+}
+
+void MatrixRowCol::Check()
+// Throw an exception if +=, -= of constant would lead to a loss of data
+// that is: check full row is present
+// may not be appropriate for symmetric matrices
+{
+   REPORT
+   if (skip!=0 || storage!=length)
+      Throw(ProgramException("Illegal Conversion"));
+}
+
+void MatrixRowCol::Negate(const MatrixRowCol& mrc1)
+{
+   // THIS = -mrc1
+   REPORT
+   if (!storage) return;
+   int f = mrc1.skip; int l = f + mrc1.storage; int lx = skip + storage;
+   if (f < skip) { f = skip; if (l < f) l = f; }
+   if (l > lx) { l = lx; if (f > lx) f = lx; }
+
+   Real* elx = data; Real* ely = mrc1.data+(f-mrc1.skip);
+
+   int l1 = f-skip;  while (l1--) *elx++ = 0.0;
+       l1 = l-f;     while (l1--) *elx++ = - *ely++;
+       lx -= l;      while (lx--) *elx++ = 0.0;
+}
+
+void MatrixRowCol::Multiply(const MatrixRowCol& mrc1, Real s)
+{
+   // THIS = mrc1 * s
+   REPORT
+   if (!storage) return;
+   int f = mrc1.skip; int l = f + mrc1.storage; int lx = skip + storage;
+   if (f < skip) { f = skip; if (l < f) l = f; }
+   if (l > lx) { l = lx; if (f > lx) f = lx; }
+
+   Real* elx = data; Real* ely = mrc1.data+(f-mrc1.skip);
+
+   int l1 = f-skip;  while (l1--) *elx++ = 0.0;
+       l1 = l-f;     while (l1--) *elx++ = *ely++ * s;
+       lx -= l;      while (lx--) *elx++ = 0.0;
+}
+
+void DiagonalMatrix::Solver(MatrixColX& mrc, const MatrixColX& mrc1)
+{
+   // mrc = mrc / mrc1   (elementwise)
+   REPORT
+   int f = mrc1.skip; int f0 = mrc.skip;
+   int l = f + mrc1.storage; int lx = f0 + mrc.storage;
+   if (f < f0) { f = f0; if (l < f) l = f; }
+   if (l > lx) { l = lx; if (f > lx) f = lx; }
+
+   Real* elx = mrc.data; Real* eld = store+f;
+
+   int l1 = f-f0;    while (l1--) *elx++ = 0.0;
+       l1 = l-f;     while (l1--) *elx++ /= *eld++;
+       lx -= l;      while (lx--) *elx++ = 0.0;
+   // Solver makes sure input and output point to same memory
+}
+
+void IdentityMatrix::Solver(MatrixColX& mrc, const MatrixColX& mrc1)
+{
+   // mrc = mrc / mrc1   (elementwise)
+   REPORT
+   int f = mrc1.skip; int f0 = mrc.skip;
+   int l = f + mrc1.storage; int lx = f0 + mrc.storage;
+   if (f < f0) { f = f0; if (l < f) l = f; }
+   if (l > lx) { l = lx; if (f > lx) f = lx; }
+
+   Real* elx = mrc.data; Real eldv = *store;
+
+   int l1 = f-f0;    while (l1--) *elx++ = 0.0;
+       l1 = l-f;     while (l1--) *elx++ /= eldv;
+       lx -= l;      while (lx--) *elx++ = 0.0;
+   // Solver makes sure input and output point to same memory
+}
+
+void MatrixRowCol::Copy(const Real*& r)
+{
+   // THIS = *r
+   REPORT
+   Real* elx = data; const Real* ely = r+skip; r += length;
+   int l = storage; while (l--) *elx++ = *ely++;
+}
+
+void MatrixRowCol::Copy(Real r)
+{
+   // THIS = r
+   REPORT  Real* elx = data; int l = storage; while (l--) *elx++ = r;
+}
+
+void MatrixRowCol::Zero()
+{
+   // THIS = 0
+   REPORT  Real* elx = data; int l = storage; while (l--) *elx++ = 0;
+}
+
+void MatrixRowCol::Multiply(Real r)
+{
+   // THIS *= r
+   REPORT  Real* elx = data; int l = storage; while (l--) *elx++ *= r;
+}
+
+void MatrixRowCol::Add(Real r)
+{
+   // THIS += r
+   REPORT
+   Real* elx = data; int l = storage; while (l--) *elx++ += r;
+}
+
+Real MatrixRowCol::SumAbsoluteValue()
+{
+   REPORT
+   Real sum = 0.0; Real* elx = data; int l = storage;
+   while (l--) sum += fabs(*elx++);
+   return sum;
+}
+
+// max absolute value of r and elements of row/col
+// we use <= or >= in all of these so we are sure of getting
+// r reset at least once.
+Real MatrixRowCol::MaximumAbsoluteValue1(Real r, int& i)
+{
+   REPORT
+   Real* elx = data; int l = storage; int li = -1;
+   while (l--) { Real f = fabs(*elx++); if (r <= f) { r = f; li = l; } }
+   i = (li >= 0) ? storage - li + skip : 0;
+   return r;
+}
+
+// min absolute value of r and elements of row/col
+Real MatrixRowCol::MinimumAbsoluteValue1(Real r, int& i)
+{
+   REPORT
+   Real* elx = data; int l = storage; int li = -1;
+   while (l--) { Real f = fabs(*elx++); if (r >= f) { r = f; li = l; } }
+   i = (li >= 0) ? storage - li + skip : 0;
+   return r;
+}
+
+// max value of r and elements of row/col
+Real MatrixRowCol::Maximum1(Real r, int& i)
+{
+   REPORT
+   Real* elx = data; int l = storage; int li = -1;
+   while (l--) { Real f = *elx++; if (r <= f) { r = f; li = l; } }
+   i = (li >= 0) ? storage - li + skip : 0;
+   return r;
+}
+
+// min value of r and elements of row/col
+Real MatrixRowCol::Minimum1(Real r, int& i)
+{
+   REPORT
+   Real* elx = data; int l = storage; int li = -1;
+   while (l--) { Real f = *elx++; if (r >= f) { r = f; li = l; } }
+   i = (li >= 0) ? storage - li + skip : 0;
+   return r;
+}
+
+Real MatrixRowCol::Sum()
+{
+   REPORT
+   Real sum = 0.0; Real* elx = data; int l = storage;
+   while (l--) sum += *elx++;
+   return sum;
+}
+
+void MatrixRowCol::SubRowCol(MatrixRowCol& mrc, int skip1, int l1) const
+{
+   mrc.length = l1;  int d = skip - skip1;
+   if (d<0) { mrc.skip = 0; mrc.data = data - d; }
+   else  { mrc.skip = d; mrc.data = data; }
+   d = skip + storage - skip1;
+   d = ((l1 < d) ? l1 : d) - mrc.skip;  mrc.storage = (d < 0) ? 0 : d;
+   mrc.cw = 0;
+}
+
+#ifdef use_namespace
+}
+#endif
+
Index: orxonox/trunk/src/util/newmat/newmat3.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/newmat3.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/newmat3.cpp	(revision 4565)
@@ -0,0 +1,842 @@
+//$$ newmat3.cpp        Matrix get and restore rows and columns
+
+// Copyright (C) 1991,2,3,4: R B Davies
+
+//#define WANT_STREAM
+
+#include "include.h"
+
+#include "newmat.h"
+#include "newmatrc.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+
+#ifdef DO_REPORT
+#define REPORT { static ExeCounter ExeCount(__LINE__,3); ++ExeCount; }
+#else
+#define REPORT {}
+#endif
+
+//#define MONITOR(what,storage,store)
+//   { cout << what << " " << storage << " at " << (long)store << "\n"; }
+
+#define MONITOR(what,store,storage) {}
+
+
+// Control bits codes for GetRow, GetCol, RestoreRow, RestoreCol
+//
+// LoadOnEntry:
+//    Load data into MatrixRow or Col dummy array under GetRow or GetCol
+// StoreOnExit:
+//    Restore data to original matrix under RestoreRow or RestoreCol
+// DirectPart:
+//    Load or restore only part directly stored; must be set with StoreOnExit
+//    Still have decide how to handle this with symmetric
+// StoreHere:
+//    used in columns only - store data at supplied storage address;
+//    used for GetCol, NextCol & RestoreCol. No need to fill out zeros
+// HaveStore:
+//    dummy array has been assigned (internal use only).
+
+// For symmetric matrices, treat columns as rows unless StoreHere is set;
+// then stick to columns as this will give better performance for doing
+// inverses
+
+// How components are used:
+
+// Use rows wherever possible in preference to columns
+
+// Columns without StoreHere are used in in-exact transpose, sum column,
+// multiply a column vector, and maybe in future access to column,
+// additional multiply functions, add transpose
+
+// Columns with StoreHere are used in exact transpose (not symmetric matrices
+// or vectors, load only)
+
+// Columns with MatrixColX (Store to full column) are used in inverse and solve
+
+// Functions required for each matrix class
+
+// GetRow(MatrixRowCol& mrc)
+// GetCol(MatrixRowCol& mrc)
+// GetCol(MatrixColX& mrc)
+// RestoreRow(MatrixRowCol& mrc)
+// RestoreCol(MatrixRowCol& mrc)
+// RestoreCol(MatrixColX& mrc)
+// NextRow(MatrixRowCol& mrc)
+// NextCol(MatrixRowCol& mrc)
+// NextCol(MatrixColX& mrc)
+
+// The Restore routines assume StoreOnExit has already been checked
+// Defaults for the Next routines are given below
+// Assume cannot have both !DirectPart && StoreHere for MatrixRowCol routines
+
+
+// Default NextRow and NextCol:
+// will work as a default but need to override NextRow for efficiency
+
+void GeneralMatrix::NextRow(MatrixRowCol& mrc)
+{
+   REPORT
+   if (+(mrc.cw*StoreOnExit)) { REPORT this->RestoreRow(mrc); }
+   mrc.rowcol++;
+   if (mrc.rowcol<nrows) { REPORT this->GetRow(mrc); }
+   else { REPORT mrc.cw -= StoreOnExit; }
+}
+
+void GeneralMatrix::NextCol(MatrixRowCol& mrc)
+{
+   REPORT                                                // 423
+   if (+(mrc.cw*StoreOnExit)) { REPORT this->RestoreCol(mrc); }
+   mrc.rowcol++;
+   if (mrc.rowcol<ncols) { REPORT this->GetCol(mrc); }
+   else { REPORT mrc.cw -= StoreOnExit; }
+}
+
+void GeneralMatrix::NextCol(MatrixColX& mrc)
+{
+   REPORT                                                // 423
+   if (+(mrc.cw*StoreOnExit)) { REPORT this->RestoreCol(mrc); }
+   mrc.rowcol++;
+   if (mrc.rowcol<ncols) { REPORT this->GetCol(mrc); }
+   else { REPORT mrc.cw -= StoreOnExit; }
+}
+
+
+// routines for matrix
+
+void Matrix::GetRow(MatrixRowCol& mrc)
+{
+   REPORT
+   mrc.skip=0; mrc.storage=mrc.length=ncols; mrc.data=store+mrc.rowcol*ncols;
+}
+
+
+void Matrix::GetCol(MatrixRowCol& mrc)
+{
+   REPORT
+   mrc.skip=0; mrc.storage=mrc.length=nrows;
+   if ( ncols==1 && !(mrc.cw*StoreHere) )      // ColumnVector
+      { REPORT mrc.data=store; }
+   else
+   {
+      Real* ColCopy;
+      if ( !(mrc.cw*(HaveStore+StoreHere)) )
+      {
+         REPORT
+         ColCopy = new Real [nrows]; MatrixErrorNoSpace(ColCopy);
+         MONITOR_REAL_NEW("Make (MatGetCol)",nrows,ColCopy)
+         mrc.data = ColCopy; mrc.cw += HaveStore;
+      }
+      else { REPORT ColCopy = mrc.data; }
+      if (+(mrc.cw*LoadOnEntry))
+      {
+         REPORT
+         Real* Mstore = store+mrc.rowcol; int i=nrows;
+         //while (i--) { *ColCopy++ = *Mstore; Mstore+=ncols; }
+         if (i) for (;;)
+            { *ColCopy++ = *Mstore; if (!(--i)) break; Mstore+=ncols; }
+      }
+   }
+}
+
+void Matrix::GetCol(MatrixColX& mrc)
+{
+   REPORT
+   mrc.skip=0; mrc.storage=nrows; mrc.length=nrows;
+   if (+(mrc.cw*LoadOnEntry))
+   {
+      REPORT  Real* ColCopy = mrc.data;
+      Real* Mstore = store+mrc.rowcol; int i=nrows;
+      //while (i--) { *ColCopy++ = *Mstore; Mstore+=ncols; }
+      if (i) for (;;)
+          { *ColCopy++ = *Mstore; if (!(--i)) break; Mstore+=ncols; }
+   }
+}
+
+void Matrix::RestoreCol(MatrixRowCol& mrc)
+{
+   // always check StoreOnExit before calling RestoreCol
+   REPORT                                   // 429
+   if (+(mrc.cw*HaveStore))
+   {
+      REPORT                                // 426
+      Real* Mstore = store+mrc.rowcol; int i=nrows;
+      Real* Cstore = mrc.data;
+      // while (i--) { *Mstore = *Cstore++; Mstore+=ncols; }
+      if (i) for (;;)
+          { *Mstore = *Cstore++; if (!(--i)) break; Mstore+=ncols; }
+   }
+}
+
+void Matrix::RestoreCol(MatrixColX& mrc)
+{
+   REPORT
+   Real* Mstore = store+mrc.rowcol; int i=nrows; Real* Cstore = mrc.data;
+   // while (i--) { *Mstore = *Cstore++; Mstore+=ncols; }
+   if (i) for (;;)
+      { *Mstore = *Cstore++; if (!(--i)) break; Mstore+=ncols; }
+}
+
+void Matrix::NextRow(MatrixRowCol& mrc) { REPORT mrc.IncrMat(); }  // 1808
+
+void Matrix::NextCol(MatrixRowCol& mrc)
+{
+   REPORT                                        // 632
+   if (+(mrc.cw*StoreOnExit)) { REPORT RestoreCol(mrc); }
+   mrc.rowcol++;
+   if (mrc.rowcol<ncols)
+   {
+      if (+(mrc.cw*LoadOnEntry))
+      {
+         REPORT
+         Real* ColCopy = mrc.data;
+         Real* Mstore = store+mrc.rowcol; int i=nrows;
+         //while (i--) { *ColCopy++ = *Mstore; Mstore+=ncols; }
+         if (i) for (;;)
+            { *ColCopy++ = *Mstore; if (!(--i)) break; Mstore+=ncols; }
+      }
+   }
+   else { REPORT mrc.cw -= StoreOnExit; }
+}
+
+void Matrix::NextCol(MatrixColX& mrc)
+{
+   REPORT
+   if (+(mrc.cw*StoreOnExit)) { REPORT RestoreCol(mrc); }
+   mrc.rowcol++;
+   if (mrc.rowcol<ncols)
+   {
+      if (+(mrc.cw*LoadOnEntry))
+      {
+         REPORT
+         Real* ColCopy = mrc.data;
+         Real* Mstore = store+mrc.rowcol; int i=nrows;
+         // while (i--) { *ColCopy++ = *Mstore; Mstore+=ncols; }
+         if (i) for (;;)
+            { *ColCopy++ = *Mstore; if (!(--i)) break; Mstore+=ncols; }
+      }
+   }
+   else { REPORT mrc.cw -= StoreOnExit; }
+}
+
+// routines for diagonal matrix
+
+void DiagonalMatrix::GetRow(MatrixRowCol& mrc)
+{
+   REPORT
+   mrc.skip=mrc.rowcol; mrc.storage=1;
+   mrc.data=store+mrc.skip; mrc.length=ncols;
+}
+
+void DiagonalMatrix::GetCol(MatrixRowCol& mrc)
+{
+   REPORT
+   mrc.skip=mrc.rowcol; mrc.storage=1; mrc.length=nrows;
+   if (+(mrc.cw*StoreHere))              // should not happen
+      Throw(InternalException("DiagonalMatrix::GetCol(MatrixRowCol&)"));
+   else  { REPORT mrc.data=store+mrc.skip; }
+                                                      // not accessed
+}
+
+void DiagonalMatrix::GetCol(MatrixColX& mrc)
+{
+   REPORT
+   mrc.skip=mrc.rowcol; mrc.storage=1; mrc.length=nrows;
+   mrc.data = mrc.store+mrc.skip;
+   *(mrc.data)=*(store+mrc.skip);
+}
+
+void DiagonalMatrix::NextRow(MatrixRowCol& mrc) { REPORT mrc.IncrDiag(); }
+						      // 800
+
+void DiagonalMatrix::NextCol(MatrixRowCol& mrc) { REPORT mrc.IncrDiag(); }
+                        // not accessed
+
+void DiagonalMatrix::NextCol(MatrixColX& mrc)
+{
+   REPORT
+   if (+(mrc.cw*StoreOnExit))
+      { REPORT *(store+mrc.rowcol)=*(mrc.data); }
+   mrc.IncrDiag();
+   int t1 = +(mrc.cw*LoadOnEntry);
+   if (t1 && mrc.rowcol < ncols)
+      { REPORT *(mrc.data)=*(store+mrc.rowcol); }
+}
+
+// routines for upper triangular matrix
+
+void UpperTriangularMatrix::GetRow(MatrixRowCol& mrc)
+{
+   REPORT
+   int row = mrc.rowcol; mrc.skip=row; mrc.length=ncols;
+   mrc.storage=ncols-row; mrc.data=store+(row*(2*ncols-row+1))/2;
+}
+
+
+void UpperTriangularMatrix::GetCol(MatrixRowCol& mrc)
+{
+   REPORT
+   mrc.skip=0; int i=mrc.rowcol+1; mrc.storage=i;
+   mrc.length=nrows; Real* ColCopy;
+   if ( !(mrc.cw*(StoreHere+HaveStore)) )
+   {
+      REPORT                                              // not accessed
+      ColCopy = new Real [nrows]; MatrixErrorNoSpace(ColCopy);
+      MONITOR_REAL_NEW("Make (UT GetCol)",nrows,ColCopy)
+      mrc.data = ColCopy; mrc.cw += HaveStore;
+   }
+   else { REPORT ColCopy = mrc.data; }
+   if (+(mrc.cw*LoadOnEntry))
+   {
+      REPORT
+      Real* Mstore = store+mrc.rowcol; int j = ncols;
+      // while (i--) { *ColCopy++ = *Mstore; Mstore += --j; }
+      if (i) for (;;)
+         { *ColCopy++ = *Mstore; if (!(--i)) break; Mstore += --j; }
+   }
+}
+
+void UpperTriangularMatrix::GetCol(MatrixColX& mrc)
+{
+   REPORT
+   mrc.skip=0; int i=mrc.rowcol+1; mrc.storage=i;
+   mrc.length=nrows;
+   if (+(mrc.cw*LoadOnEntry))
+   {
+      REPORT
+      Real* ColCopy = mrc.data;
+      Real* Mstore = store+mrc.rowcol; int j = ncols;
+      // while (i--) { *ColCopy++ = *Mstore; Mstore += --j; }
+      if (i) for (;;)
+         { *ColCopy++ = *Mstore; if (!(--i)) break; Mstore += --j; }
+   }
+}
+
+void UpperTriangularMatrix::RestoreCol(MatrixRowCol& mrc)
+{
+  REPORT
+  Real* Mstore = store+mrc.rowcol; int i=mrc.rowcol+1; int j = ncols;
+  Real* Cstore = mrc.data;
+  // while (i--) { *Mstore = *Cstore++; Mstore += --j; }
+  if (i) for (;;)
+     { *Mstore = *Cstore++; if (!(--i)) break; Mstore += --j; }
+}
+
+void UpperTriangularMatrix::NextRow(MatrixRowCol& mrc) { REPORT mrc.IncrUT(); }
+						      // 722
+
+// routines for lower triangular matrix
+
+void LowerTriangularMatrix::GetRow(MatrixRowCol& mrc)
+{
+   REPORT
+   int row=mrc.rowcol; mrc.skip=0; mrc.storage=row+1; mrc.length=ncols;
+   mrc.data=store+(row*(row+1))/2;
+}
+
+void LowerTriangularMatrix::GetCol(MatrixRowCol& mrc)
+{
+   REPORT
+   int col=mrc.rowcol; mrc.skip=col; mrc.length=nrows;
+   int i=nrows-col; mrc.storage=i; Real* ColCopy;
+   if ( +(mrc.cw*(StoreHere+HaveStore)) )
+      { REPORT  ColCopy = mrc.data; }
+   else
+   {
+      REPORT                                            // not accessed
+      ColCopy = new Real [nrows]; MatrixErrorNoSpace(ColCopy);
+      MONITOR_REAL_NEW("Make (LT GetCol)",nrows,ColCopy)
+      mrc.cw += HaveStore; mrc.data = ColCopy;
+   }
+
+   if (+(mrc.cw*LoadOnEntry))
+   {
+      REPORT
+      Real* Mstore = store+(col*(col+3))/2;
+      // while (i--) { *ColCopy++ = *Mstore; Mstore += ++col; }
+      if (i) for (;;)
+         { *ColCopy++ = *Mstore; if (!(--i)) break; Mstore += ++col; }
+   }
+}
+
+void LowerTriangularMatrix::GetCol(MatrixColX& mrc)
+{
+   REPORT
+   int col=mrc.rowcol; mrc.skip=col; mrc.length=nrows;
+   int i=nrows-col; mrc.storage=i; mrc.data = mrc.store + col;
+
+   if (+(mrc.cw*LoadOnEntry))
+   {
+      REPORT  Real* ColCopy = mrc.data;
+      Real* Mstore = store+(col*(col+3))/2;
+      // while (i--) { *ColCopy++ = *Mstore; Mstore += ++col; }
+      if (i) for (;;)
+         { *ColCopy++ = *Mstore; if (!(--i)) break; Mstore += ++col; }
+   }
+}
+
+void LowerTriangularMatrix::RestoreCol(MatrixRowCol& mrc)
+{
+   REPORT
+   int col=mrc.rowcol; Real* Cstore = mrc.data;
+   Real* Mstore = store+(col*(col+3))/2; int i=nrows-col;
+   //while (i--) { *Mstore = *Cstore++; Mstore += ++col; }
+   if (i) for (;;)
+      { *Mstore = *Cstore++; if (!(--i)) break; Mstore += ++col; }
+}
+
+void LowerTriangularMatrix::NextRow(MatrixRowCol& mrc) { REPORT mrc.IncrLT(); }
+					                 //712
+
+// routines for symmetric matrix
+
+void SymmetricMatrix::GetRow(MatrixRowCol& mrc)
+{
+   REPORT                                                //571
+   mrc.skip=0; int row=mrc.rowcol; mrc.length=ncols;
+   if (+(mrc.cw*DirectPart))
+      { REPORT mrc.storage=row+1; mrc.data=store+(row*(row+1))/2; }
+   else
+   {
+      // do not allow StoreOnExit and !DirectPart
+      if (+(mrc.cw*StoreOnExit))
+         Throw(InternalException("SymmetricMatrix::GetRow(MatrixRowCol&)"));
+      mrc.storage=ncols; Real* RowCopy;
+      if (!(mrc.cw*HaveStore))
+      {
+         REPORT
+         RowCopy = new Real [ncols]; MatrixErrorNoSpace(RowCopy);
+         MONITOR_REAL_NEW("Make (SymGetRow)",ncols,RowCopy)
+         mrc.cw += HaveStore; mrc.data = RowCopy;
+      }
+      else { REPORT RowCopy = mrc.data; }
+      if (+(mrc.cw*LoadOnEntry))
+      {
+         REPORT                                         // 544
+         Real* Mstore = store+(row*(row+1))/2; int i = row;
+         while (i--) *RowCopy++ = *Mstore++;
+         i = ncols-row;
+         // while (i--) { *RowCopy++ = *Mstore; Mstore += ++row; }
+         if (i) for (;;)
+            { *RowCopy++ = *Mstore; if (!(--i)) break; Mstore += ++row; }
+      }
+   }
+}
+
+void SymmetricMatrix::GetCol(MatrixRowCol& mrc)
+{
+   // do not allow StoreHere
+   if (+(mrc.cw*StoreHere))
+      Throw(InternalException("SymmetricMatrix::GetCol(MatrixRowCol&)"));
+
+   int col=mrc.rowcol; mrc.length=nrows;
+   REPORT
+   mrc.skip=0;
+   if (+(mrc.cw*DirectPart))    // actually get row ??
+      { REPORT mrc.storage=col+1; mrc.data=store+(col*(col+1))/2; }
+   else
+   {
+      // do not allow StoreOnExit and !DirectPart
+      if (+(mrc.cw*StoreOnExit))
+         Throw(InternalException("SymmetricMatrix::GetCol(MatrixRowCol&)"));
+
+      mrc.storage=ncols; Real* ColCopy;
+      if ( +(mrc.cw*HaveStore)) { REPORT ColCopy = mrc.data; }
+      else
+      {
+         REPORT                                      // not accessed
+         ColCopy = new Real [ncols]; MatrixErrorNoSpace(ColCopy);
+         MONITOR_REAL_NEW("Make (SymGetCol)",ncols,ColCopy)
+         mrc.cw += HaveStore; mrc.data = ColCopy;
+      }
+      if (+(mrc.cw*LoadOnEntry))
+      {
+         REPORT
+         Real* Mstore = store+(col*(col+1))/2; int i = col;
+         while (i--) *ColCopy++ = *Mstore++;
+         i = ncols-col;
+         // while (i--) { *ColCopy++ = *Mstore; Mstore += ++col; }
+         if (i) for (;;)
+            { *ColCopy++ = *Mstore; if (!(--i)) break; Mstore += ++col; }
+      }
+   }
+}
+
+void SymmetricMatrix::GetCol(MatrixColX& mrc)
+{
+   int col=mrc.rowcol; mrc.length=nrows;
+   if (+(mrc.cw*DirectPart))
+   {
+      REPORT
+      mrc.skip=col; int i=nrows-col; mrc.storage=i;
+      mrc.data = mrc.store+col;
+      if (+(mrc.cw*LoadOnEntry))
+      {
+         REPORT                           // not accessed
+         Real* ColCopy = mrc.data;
+         Real* Mstore = store+(col*(col+3))/2;
+         // while (i--) { *ColCopy++ = *Mstore; Mstore += ++col; }
+         if (i) for (;;)
+            { *ColCopy++ = *Mstore; if (!(--i)) break; Mstore += ++col; }
+      }
+   }
+   else
+   {
+      REPORT
+      // do not allow StoreOnExit and !DirectPart
+      if (+(mrc.cw*StoreOnExit))
+         Throw(InternalException("SymmetricMatrix::GetCol(MatrixColX&)"));
+
+      mrc.skip=0; mrc.storage=ncols;
+      if (+(mrc.cw*LoadOnEntry))
+      {
+         REPORT
+         Real* ColCopy = mrc.data;
+         Real* Mstore = store+(col*(col+1))/2; int i = col;
+         while (i--) *ColCopy++ = *Mstore++;
+         i = ncols-col;
+         // while (i--) { *ColCopy++ = *Mstore; Mstore += ++col; }
+         if (i) for (;;)
+            { *ColCopy++ = *Mstore; if (!(--i)) break; Mstore += ++col; }
+      }
+   }
+}
+
+// Do not need RestoreRow because we do not allow !DirectPart && StoreOnExit
+
+void SymmetricMatrix::RestoreCol(MatrixColX& mrc)
+{
+   REPORT
+   // Really do restore column
+   int col=mrc.rowcol; Real* Cstore = mrc.data;
+   Real* Mstore = store+(col*(col+3))/2; int i = nrows-col;
+   // while (i--) { *Mstore = *Cstore++; Mstore+= ++col; }
+   if (i) for (;;)
+      { *Mstore = *Cstore++; if (!(--i)) break; Mstore+= ++col; }
+
+}
+
+// routines for row vector
+
+void RowVector::GetCol(MatrixRowCol& mrc)
+{
+   REPORT
+   // do not allow StoreHere
+   if (+(mrc.cw*StoreHere))
+      Throw(InternalException("RowVector::GetCol(MatrixRowCol&)"));
+
+   mrc.skip=0; mrc.storage=1; mrc.length=nrows; mrc.data = store+mrc.rowcol;
+
+}
+
+void RowVector::GetCol(MatrixColX& mrc)
+{
+   REPORT
+   mrc.skip=0; mrc.storage=1; mrc.length=nrows;
+   if (mrc.cw >= LoadOnEntry)
+      { REPORT *(mrc.data) = *(store+mrc.rowcol); }
+
+}
+
+void RowVector::NextCol(MatrixRowCol& mrc)
+{ REPORT mrc.rowcol++; mrc.data++; }
+
+void RowVector::NextCol(MatrixColX& mrc)
+{
+   if (+(mrc.cw*StoreOnExit)) { REPORT *(store+mrc.rowcol)=*(mrc.data); }
+
+   mrc.rowcol++;
+   if (mrc.rowcol < ncols)
+   {
+      if (+(mrc.cw*LoadOnEntry)) { REPORT *(mrc.data)=*(store+mrc.rowcol); }
+   }
+   else { REPORT mrc.cw -= StoreOnExit; }
+}
+
+void RowVector::RestoreCol(MatrixColX& mrc)
+   { REPORT *(store+mrc.rowcol)=*(mrc.data); }           // not accessed
+
+
+// routines for band matrices
+
+void BandMatrix::GetRow(MatrixRowCol& mrc)
+{
+   REPORT
+   int r = mrc.rowcol; int w = lower+1+upper; mrc.length=ncols;
+   int s = r-lower;
+   if (s<0) { mrc.data = store+(r*w-s); w += s; s = 0; }
+   else mrc.data = store+r*w;
+   mrc.skip = s; s += w-ncols; if (s>0) w -= s; mrc.storage = w;
+}
+
+// should make special versions of this for upper and lower band matrices
+
+void BandMatrix::NextRow(MatrixRowCol& mrc)
+{
+   REPORT
+   int r = ++mrc.rowcol;
+   if (r<=lower) { mrc.storage++; mrc.data += lower+upper; }
+   else  { mrc.skip++; mrc.data += lower+upper+1; }
+   if (r>=ncols-upper) mrc.storage--;
+}
+
+void BandMatrix::GetCol(MatrixRowCol& mrc)
+{
+   REPORT
+   int c = mrc.rowcol; int n = lower+upper; int w = n+1;
+   mrc.length=nrows; Real* ColCopy;
+   int b; int s = c-upper;
+   if (s<=0) { w += s; s = 0; b = c+lower; } else b = s*w+n;
+   mrc.skip = s; s += w-nrows; if (s>0) w -= s; mrc.storage = w;
+   if ( +(mrc.cw*(StoreHere+HaveStore)) )
+      { REPORT ColCopy = mrc.data; }
+   else
+   {
+      REPORT
+      ColCopy = new Real [n+1]; MatrixErrorNoSpace(ColCopy);
+      MONITOR_REAL_NEW("Make (BMGetCol)",n+1,ColCopy)
+      mrc.cw += HaveStore; mrc.data = ColCopy;
+   }
+
+   if (+(mrc.cw*LoadOnEntry))
+   {
+      REPORT
+      Real* Mstore = store+b;
+      // while (w--) { *ColCopy++ = *Mstore; Mstore+=n; }
+      if (w) for (;;)
+         { *ColCopy++ = *Mstore; if (!(--w)) break; Mstore+=n; }
+   }
+}
+
+void BandMatrix::GetCol(MatrixColX& mrc)
+{
+   REPORT
+   int c = mrc.rowcol; int n = lower+upper; int w = n+1;
+   mrc.length=nrows; int b; int s = c-upper;
+   if (s<=0) { w += s; s = 0; b = c+lower; } else b = s*w+n;
+   mrc.skip = s; s += w-nrows; if (s>0) w -= s; mrc.storage = w;
+   mrc.data = mrc.store+mrc.skip;
+
+   if (+(mrc.cw*LoadOnEntry))
+   {
+      REPORT
+      Real* ColCopy = mrc.data; Real* Mstore = store+b;
+      // while (w--) { *ColCopy++ = *Mstore; Mstore+=n; }
+      if (w) for (;;)
+         { *ColCopy++ = *Mstore; if (!(--w)) break; Mstore+=n; }
+   }
+}
+
+void BandMatrix::RestoreCol(MatrixRowCol& mrc)
+{
+   REPORT
+   int c = mrc.rowcol; int n = lower+upper; int s = c-upper;
+   Real* Mstore = store + ((s<=0) ? c+lower : s*n+s+n);
+   Real* Cstore = mrc.data;
+   int w = mrc.storage;
+   // while (w--) { *Mstore = *Cstore++; Mstore += n; }
+   if (w) for (;;)
+      { *Mstore = *Cstore++; if (!(--w)) break; Mstore += n; }
+}
+
+// routines for symmetric band matrix
+
+void SymmetricBandMatrix::GetRow(MatrixRowCol& mrc)
+{
+   REPORT
+   int r=mrc.rowcol; int s = r-lower; int w1 = lower+1; int o = r*w1;
+   mrc.length = ncols;
+   if (s<0) { w1 += s; o -= s; s = 0; }
+   mrc.skip = s;
+
+   if (+(mrc.cw*DirectPart))
+      { REPORT  mrc.data = store+o; mrc.storage = w1; }
+   else
+   {
+      // do not allow StoreOnExit and !DirectPart
+      if (+(mrc.cw*StoreOnExit))
+         Throw(InternalException("SymmetricBandMatrix::GetRow(MatrixRowCol&)"));
+      int w = w1+lower; s += w-ncols; Real* RowCopy;
+      if (s>0) w -= s; mrc.storage = w; int w2 = w-w1;
+      if (!(mrc.cw*HaveStore))
+      {
+         REPORT
+         RowCopy = new Real [2*lower+1]; MatrixErrorNoSpace(RowCopy);
+         MONITOR_REAL_NEW("Make (SmBGetRow)",2*lower+1,RowCopy)
+         mrc.cw += HaveStore; mrc.data = RowCopy;
+      }
+      else { REPORT  RowCopy = mrc.data; }
+
+      if (+(mrc.cw*LoadOnEntry))
+      {
+	      REPORT
+         Real* Mstore = store+o;
+         while (w1--) *RowCopy++ = *Mstore++;
+         Mstore--;
+         while (w2--) { Mstore += lower; *RowCopy++ = *Mstore; }
+      }
+   }
+}
+
+void SymmetricBandMatrix::GetCol(MatrixRowCol& mrc)
+{
+   // do not allow StoreHere
+   if (+(mrc.cw*StoreHere))
+      Throw(InternalException("SymmetricBandMatrix::GetCol(MatrixRowCol&)"));
+
+   int c=mrc.rowcol; int w1 = lower+1; mrc.length=nrows;
+   REPORT
+   int s = c-lower; int o = c*w1;
+   if (s<0) { w1 += s; o -= s; s = 0; }
+   mrc.skip = s;
+
+   if (+(mrc.cw*DirectPart))
+   { REPORT  mrc.data = store+o; mrc.storage = w1; }
+   else
+   {
+      // do not allow StoreOnExit and !DirectPart
+      if (+(mrc.cw*StoreOnExit))
+         Throw(InternalException("SymmetricBandMatrix::GetCol(MatrixRowCol&)"));
+      int w = w1+lower; s += w-ncols; Real* ColCopy;
+      if (s>0) w -= s; mrc.storage = w; int w2 = w-w1;
+
+      if ( +(mrc.cw*HaveStore) ) { REPORT ColCopy = mrc.data; }
+      else
+      {
+         REPORT ColCopy = new Real [2*lower+1]; MatrixErrorNoSpace(ColCopy);
+         MONITOR_REAL_NEW("Make (SmBGetCol)",2*lower+1,ColCopy)
+         mrc.cw += HaveStore; mrc.data = ColCopy;
+      }
+
+      if (+(mrc.cw*LoadOnEntry))
+      {
+         REPORT
+         Real* Mstore = store+o;
+         while (w1--) *ColCopy++ = *Mstore++;
+         Mstore--;
+         while (w2--) { Mstore += lower; *ColCopy++ = *Mstore; }
+      }
+   }
+}
+
+void SymmetricBandMatrix::GetCol(MatrixColX& mrc)
+{
+   int c=mrc.rowcol; int w1 = lower+1; mrc.length=nrows;
+   if (+(mrc.cw*DirectPart))
+   {
+      REPORT
+      int b = c*w1+lower;
+      mrc.skip = c; c += w1-nrows; w1 -= c; mrc.storage = w1;
+      Real* ColCopy = mrc.data = mrc.store+mrc.skip;
+      if (+(mrc.cw*LoadOnEntry))
+      {
+         REPORT
+         Real* Mstore = store+b;
+         // while (w1--) { *ColCopy++ = *Mstore; Mstore += lower; }
+         if (w1) for (;;)
+            { *ColCopy++ = *Mstore; if (!(--w1)) break; Mstore += lower; }
+      }
+   }
+   else
+   {
+      REPORT
+      // do not allow StoreOnExit and !DirectPart
+      if (+(mrc.cw*StoreOnExit))
+         Throw(InternalException("SymmetricBandMatrix::GetCol(MatrixColX&)"));
+      int s = c-lower; int o = c*w1;
+      if (s<0) { w1 += s; o -= s; s = 0; }
+      mrc.skip = s;
+
+      int w = w1+lower; s += w-ncols;
+      if (s>0) w -= s; mrc.storage = w; int w2 = w-w1;
+
+      Real* ColCopy = mrc.data = mrc.store+mrc.skip;
+
+      if (+(mrc.cw*LoadOnEntry))
+      {
+         REPORT
+         Real* Mstore = store+o;
+         while (w1--) *ColCopy++ = *Mstore++;
+         Mstore--;
+         while (w2--) { Mstore += lower; *ColCopy++ = *Mstore; }
+      }
+
+   }
+}
+
+void SymmetricBandMatrix::RestoreCol(MatrixColX& mrc)
+{
+   REPORT
+   int c = mrc.rowcol;
+   Real* Mstore = store + c*lower+c+lower;
+   Real* Cstore = mrc.data; int w = mrc.storage;
+   // while (w--) { *Mstore = *Cstore++; Mstore += lower; }
+   if (w) for (;;)
+      { *Mstore = *Cstore++; if (!(--w)) break; Mstore += lower; }
+}
+
+// routines for identity matrix
+
+void IdentityMatrix::GetRow(MatrixRowCol& mrc)
+{
+   REPORT
+   mrc.skip=mrc.rowcol; mrc.storage=1; mrc.data=store; mrc.length=ncols;
+}
+
+void IdentityMatrix::GetCol(MatrixRowCol& mrc)
+{
+   REPORT
+   mrc.skip=mrc.rowcol; mrc.storage=1; mrc.length=nrows;
+   if (+(mrc.cw*StoreHere))              // should not happen
+      Throw(InternalException("IdentityMatrix::GetCol(MatrixRowCol&)"));
+   else  { REPORT mrc.data=store; }
+}
+
+void IdentityMatrix::GetCol(MatrixColX& mrc)
+{
+   REPORT
+   mrc.skip=mrc.rowcol; mrc.storage=1; mrc.length=nrows;
+   mrc.data = mrc.store+mrc.skip; *(mrc.data)=*store;
+}
+
+void IdentityMatrix::NextRow(MatrixRowCol& mrc) { REPORT mrc.IncrId(); }
+
+void IdentityMatrix::NextCol(MatrixRowCol& mrc) { REPORT mrc.IncrId(); }
+
+void IdentityMatrix::NextCol(MatrixColX& mrc)
+{
+   REPORT
+   if (+(mrc.cw*StoreOnExit)) { REPORT *store=*(mrc.data); }
+   mrc.IncrDiag();            // must increase mrc.data so need IncrDiag
+   int t1 = +(mrc.cw*LoadOnEntry);
+   if (t1 && mrc.rowcol < ncols) { REPORT *(mrc.data)=*store; }
+}
+
+
+
+
+// *************************** destructors *******************************
+
+MatrixRowCol::~MatrixRowCol()
+{
+   if (+(cw*HaveStore))
+   {
+      MONITOR_REAL_DELETE("Free    (RowCol)",-1,data)  // do not know length
+      delete [] data;
+   }
+}
+
+MatrixRow::~MatrixRow() { if (+(cw*StoreOnExit)) gm->RestoreRow(*this); }
+
+MatrixCol::~MatrixCol() { if (+(cw*StoreOnExit)) gm->RestoreCol(*this); }
+
+MatrixColX::~MatrixColX() { if (+(cw*StoreOnExit)) gm->RestoreCol(*this); }
+
+#ifdef use_namespace
+}
+#endif
+
Index: orxonox/trunk/src/util/newmat/newmat4.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/newmat4.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/newmat4.cpp	(revision 4565)
@@ -0,0 +1,950 @@
+//$$ newmat4.cpp       Constructors, ReSize, basic utilities
+
+// Copyright (C) 1991,2,3,4,8,9: R B Davies
+
+#include "include.h"
+
+#include "newmat.h"
+#include "newmatrc.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+
+
+#ifdef DO_REPORT
+#define REPORT { static ExeCounter ExeCount(__LINE__,4); ++ExeCount; }
+#else
+#define REPORT {}
+#endif
+
+
+#define DO_SEARCH                   // search for LHS of = in RHS
+
+// ************************* general utilities *************************/
+
+static int tristore(int n)                    // elements in triangular matrix
+{ return (n*(n+1))/2; }
+
+
+// **************************** constructors ***************************/
+
+GeneralMatrix::GeneralMatrix()
+{ store=0; storage=0; nrows=0; ncols=0; tag=-1; }
+
+GeneralMatrix::GeneralMatrix(ArrayLengthSpecifier s)
+{
+   REPORT
+   storage=s.Value(); tag=-1;
+   if (storage)
+   {
+      store = new Real [storage]; MatrixErrorNoSpace(store);
+      MONITOR_REAL_NEW("Make (GenMatrix)",storage,store)
+   }
+   else store = 0;
+}
+
+Matrix::Matrix(int m, int n) : GeneralMatrix(m*n)
+{ REPORT nrows=m; ncols=n; }
+
+SymmetricMatrix::SymmetricMatrix(ArrayLengthSpecifier n)
+   : GeneralMatrix(tristore(n.Value()))
+{ REPORT nrows=n.Value(); ncols=n.Value(); }
+
+UpperTriangularMatrix::UpperTriangularMatrix(ArrayLengthSpecifier n)
+   : GeneralMatrix(tristore(n.Value()))
+{ REPORT nrows=n.Value(); ncols=n.Value(); }
+
+LowerTriangularMatrix::LowerTriangularMatrix(ArrayLengthSpecifier n)
+   : GeneralMatrix(tristore(n.Value()))
+{ REPORT nrows=n.Value(); ncols=n.Value(); }
+
+DiagonalMatrix::DiagonalMatrix(ArrayLengthSpecifier m) : GeneralMatrix(m)
+{ REPORT nrows=m.Value(); ncols=m.Value(); }
+
+Matrix::Matrix(const BaseMatrix& M)
+{
+   REPORT // CheckConversion(M);
+   // MatrixConversionCheck mcc;
+   GeneralMatrix* gmx=((BaseMatrix&)M).Evaluate(MatrixType::Rt);
+   GetMatrix(gmx);
+}
+
+RowVector::RowVector(const BaseMatrix& M) : Matrix(M)
+{
+   if (nrows!=1)
+   {
+      Tracer tr("RowVector");
+      Throw(VectorException(*this));
+   }
+}
+
+ColumnVector::ColumnVector(const BaseMatrix& M) : Matrix(M)
+{
+   if (ncols!=1)
+   {
+      Tracer tr("ColumnVector");
+      Throw(VectorException(*this));
+   }
+}
+
+SymmetricMatrix::SymmetricMatrix(const BaseMatrix& M)
+{
+   REPORT  // CheckConversion(M);
+   // MatrixConversionCheck mcc;
+   GeneralMatrix* gmx=((BaseMatrix&)M).Evaluate(MatrixType::Sm);
+   GetMatrix(gmx);
+}
+
+UpperTriangularMatrix::UpperTriangularMatrix(const BaseMatrix& M)
+{
+   REPORT // CheckConversion(M);
+   // MatrixConversionCheck mcc;
+   GeneralMatrix* gmx=((BaseMatrix&)M).Evaluate(MatrixType::UT);
+   GetMatrix(gmx);
+}
+
+LowerTriangularMatrix::LowerTriangularMatrix(const BaseMatrix& M)
+{
+   REPORT // CheckConversion(M);
+   // MatrixConversionCheck mcc;
+   GeneralMatrix* gmx=((BaseMatrix&)M).Evaluate(MatrixType::LT);
+   GetMatrix(gmx);
+}
+
+DiagonalMatrix::DiagonalMatrix(const BaseMatrix& M)
+{
+   REPORT //CheckConversion(M);
+   // MatrixConversionCheck mcc;
+   GeneralMatrix* gmx=((BaseMatrix&)M).Evaluate(MatrixType::Dg);
+   GetMatrix(gmx);
+}
+
+IdentityMatrix::IdentityMatrix(const BaseMatrix& M)
+{
+   REPORT //CheckConversion(M);
+   // MatrixConversionCheck mcc;
+   GeneralMatrix* gmx=((BaseMatrix&)M).Evaluate(MatrixType::Id);
+   GetMatrix(gmx);
+}
+
+GeneralMatrix::~GeneralMatrix()
+{
+   if (store)
+   {
+      MONITOR_REAL_DELETE("Free (GenMatrix)",storage,store)
+      delete [] store;
+   }
+}
+
+CroutMatrix::CroutMatrix(const BaseMatrix& m)
+{
+   REPORT
+   Tracer tr("CroutMatrix");
+   indx = 0;                     // in case of exception at next line
+   GeneralMatrix* gm = ((BaseMatrix&)m).Evaluate(MatrixType::Rt);
+   GetMatrix(gm);
+   if (nrows!=ncols) { CleanUp(); Throw(NotSquareException(*gm)); }
+   d=true; sing=false;
+   indx=new int [nrows]; MatrixErrorNoSpace(indx);
+   MONITOR_INT_NEW("Index (CroutMat)",nrows,indx)
+   ludcmp();
+}
+
+CroutMatrix::~CroutMatrix()
+{
+   MONITOR_INT_DELETE("Index (CroutMat)",nrows,indx)
+   delete [] indx;
+}
+
+//ReturnMatrixX::ReturnMatrixX(GeneralMatrix& gmx)
+//{
+//   REPORT
+//   gm = gmx.Image(); gm->ReleaseAndDelete();
+//}
+
+#ifndef TEMPS_DESTROYED_QUICKLY_R
+
+GeneralMatrix::operator ReturnMatrixX() const
+{
+   REPORT
+   GeneralMatrix* gm = Image(); gm->ReleaseAndDelete();
+   return ReturnMatrixX(gm);
+}
+
+#else
+
+GeneralMatrix::operator ReturnMatrixX&() const
+{
+   REPORT
+   GeneralMatrix* gm = Image(); gm->ReleaseAndDelete();
+   ReturnMatrixX* x = new ReturnMatrixX(gm);
+   MatrixErrorNoSpace(x); return *x;
+}
+
+#endif
+
+#ifndef TEMPS_DESTROYED_QUICKLY_R
+
+ReturnMatrixX GeneralMatrix::ForReturn() const
+{
+   REPORT
+   GeneralMatrix* gm = Image(); gm->ReleaseAndDelete();
+   return ReturnMatrixX(gm);
+}
+
+#else
+
+ReturnMatrixX& GeneralMatrix::ForReturn() const
+{
+   REPORT
+   GeneralMatrix* gm = Image(); gm->ReleaseAndDelete();
+   ReturnMatrixX* x = new ReturnMatrixX(gm);
+   MatrixErrorNoSpace(x); return *x;
+}
+
+#endif
+
+// ************************** ReSize matrices ***************************/
+
+void GeneralMatrix::ReSize(int nr, int nc, int s)
+{
+   REPORT
+   if (store)
+   {
+      MONITOR_REAL_DELETE("Free (ReDimensi)",storage,store)
+      delete [] store;
+   }
+   storage=s; nrows=nr; ncols=nc; tag=-1;
+   if (s)
+   {
+      store = new Real [storage]; MatrixErrorNoSpace(store);
+      MONITOR_REAL_NEW("Make (ReDimensi)",storage,store)
+   }
+   else store = 0;
+}
+
+void Matrix::ReSize(int nr, int nc)
+{ REPORT GeneralMatrix::ReSize(nr,nc,nr*nc); }
+
+void SymmetricMatrix::ReSize(int nr)
+{ REPORT GeneralMatrix::ReSize(nr,nr,tristore(nr)); }
+
+void UpperTriangularMatrix::ReSize(int nr)
+{ REPORT GeneralMatrix::ReSize(nr,nr,tristore(nr)); }
+
+void LowerTriangularMatrix::ReSize(int nr)
+{ REPORT GeneralMatrix::ReSize(nr,nr,tristore(nr)); }
+
+void DiagonalMatrix::ReSize(int nr)
+{ REPORT GeneralMatrix::ReSize(nr,nr,nr); }
+
+void RowVector::ReSize(int nc)
+{ REPORT GeneralMatrix::ReSize(1,nc,nc); }
+
+void ColumnVector::ReSize(int nr)
+{ REPORT GeneralMatrix::ReSize(nr,1,nr); }
+
+void RowVector::ReSize(int nr, int nc)
+{
+   Tracer tr("RowVector::ReSize");
+   if (nr != 1) Throw(VectorException(*this));
+   REPORT GeneralMatrix::ReSize(1,nc,nc);
+}
+
+void ColumnVector::ReSize(int nr, int nc)
+{
+   Tracer tr("ColumnVector::ReSize");
+   if (nc != 1) Throw(VectorException(*this));
+   REPORT GeneralMatrix::ReSize(nr,1,nr);
+}
+
+void IdentityMatrix::ReSize(int nr)
+{ REPORT GeneralMatrix::ReSize(nr,nr,1); *store = 1; }
+
+
+void Matrix::ReSize(const GeneralMatrix& A)
+{ REPORT  ReSize(A.Nrows(), A.Ncols()); }
+
+void nricMatrix::ReSize(const GeneralMatrix& A)
+{ REPORT  ReSize(A.Nrows(), A.Ncols()); }
+
+void ColumnVector::ReSize(const GeneralMatrix& A)
+{ REPORT  ReSize(A.Nrows(), A.Ncols()); }
+
+void RowVector::ReSize(const GeneralMatrix& A)
+{ REPORT  ReSize(A.Nrows(), A.Ncols()); }
+
+void SymmetricMatrix::ReSize(const GeneralMatrix& A)
+{
+   REPORT
+   int n = A.Nrows();
+   if (n != A.Ncols())
+   {
+      Tracer tr("SymmetricMatrix::ReSize(GM)");
+      Throw(NotSquareException(*this));
+   }
+   ReSize(n);
+}
+
+void DiagonalMatrix::ReSize(const GeneralMatrix& A)
+{
+   REPORT
+   int n = A.Nrows();
+   if (n != A.Ncols())
+   {
+      Tracer tr("DiagonalMatrix::ReSize(GM)");
+      Throw(NotSquareException(*this));
+   }
+   ReSize(n);
+}
+
+void UpperTriangularMatrix::ReSize(const GeneralMatrix& A)
+{
+   REPORT
+   int n = A.Nrows();
+   if (n != A.Ncols())
+   {
+      Tracer tr("UpperTriangularMatrix::ReSize(GM)");
+      Throw(NotSquareException(*this));
+   }
+   ReSize(n);
+}
+
+void LowerTriangularMatrix::ReSize(const GeneralMatrix& A)
+{
+   REPORT
+   int n = A.Nrows();
+   if (n != A.Ncols())
+   {
+      Tracer tr("LowerTriangularMatrix::ReSize(GM)");
+      Throw(NotSquareException(*this));
+   }
+   ReSize(n);
+}
+
+void IdentityMatrix::ReSize(const GeneralMatrix& A)
+{
+   REPORT
+   int n = A.Nrows();
+   if (n != A.Ncols())
+   {
+      Tracer tr("IdentityMatrix::ReSize(GM)");
+      Throw(NotSquareException(*this));
+   }
+   ReSize(n);
+}
+
+void GeneralMatrix::ReSize(const GeneralMatrix&)
+{
+   REPORT
+   Tracer tr("GeneralMatrix::ReSize(GM)");
+   Throw(NotDefinedException("ReSize", "this type of matrix"));
+}
+
+void GeneralMatrix::ReSizeForAdd(const GeneralMatrix& A, const GeneralMatrix&)
+{ REPORT ReSize(A); }
+
+void GeneralMatrix::ReSizeForSP(const GeneralMatrix& A, const GeneralMatrix&)
+{ REPORT ReSize(A); }
+
+
+// ************************* SameStorageType ******************************/
+
+// SameStorageType checks A and B have same storage type including bandwidth
+// It does not check same dimensions since we assume this is already done
+
+bool GeneralMatrix::SameStorageType(const GeneralMatrix& A) const
+{
+   REPORT
+   return Type() == A.Type();
+}
+
+
+// ******************* manipulate types, storage **************************/
+
+int GeneralMatrix::search(const BaseMatrix* s) const
+{ REPORT return (s==this) ? 1 : 0; }
+
+int GenericMatrix::search(const BaseMatrix* s) const
+{ REPORT return gm->search(s); }
+
+int MultipliedMatrix::search(const BaseMatrix* s) const
+{ REPORT return bm1->search(s) + bm2->search(s); }
+
+int ShiftedMatrix::search(const BaseMatrix* s) const
+{ REPORT return bm->search(s); }
+
+int NegatedMatrix::search(const BaseMatrix* s) const
+{ REPORT return bm->search(s); }
+
+int ReturnMatrixX::search(const BaseMatrix* s) const
+{ REPORT return (s==gm) ? 1 : 0; }
+
+MatrixType Matrix::Type() const { return MatrixType::Rt; }
+MatrixType SymmetricMatrix::Type() const { return MatrixType::Sm; }
+MatrixType UpperTriangularMatrix::Type() const { return MatrixType::UT; }
+MatrixType LowerTriangularMatrix::Type() const { return MatrixType::LT; }
+MatrixType DiagonalMatrix::Type() const { return MatrixType::Dg; }
+MatrixType RowVector::Type() const { return MatrixType::RV; }
+MatrixType ColumnVector::Type() const { return MatrixType::CV; }
+MatrixType CroutMatrix::Type() const { return MatrixType::Ct; }
+MatrixType BandMatrix::Type() const { return MatrixType::BM; }
+MatrixType UpperBandMatrix::Type() const { return MatrixType::UB; }
+MatrixType LowerBandMatrix::Type() const { return MatrixType::LB; }
+MatrixType SymmetricBandMatrix::Type() const { return MatrixType::SB; }
+
+MatrixType IdentityMatrix::Type() const { return MatrixType::Id; }
+
+
+
+MatrixBandWidth BaseMatrix::BandWidth() const { REPORT return -1; }
+MatrixBandWidth DiagonalMatrix::BandWidth() const { REPORT return 0; }
+MatrixBandWidth IdentityMatrix::BandWidth() const { REPORT return 0; }
+
+MatrixBandWidth UpperTriangularMatrix::BandWidth() const
+   { REPORT return MatrixBandWidth(0,-1); }
+
+MatrixBandWidth LowerTriangularMatrix::BandWidth() const
+   { REPORT return MatrixBandWidth(-1,0); }
+
+MatrixBandWidth BandMatrix::BandWidth() const
+   { REPORT return MatrixBandWidth(lower,upper); }
+
+MatrixBandWidth GenericMatrix::BandWidth()const
+   { REPORT return gm->BandWidth(); }
+
+MatrixBandWidth AddedMatrix::BandWidth() const
+   { REPORT return gm1->BandWidth() + gm2->BandWidth(); }
+
+MatrixBandWidth SPMatrix::BandWidth() const
+   { REPORT return gm1->BandWidth().minimum(gm2->BandWidth()); }
+
+MatrixBandWidth KPMatrix::BandWidth() const
+{
+   int lower, upper;
+   MatrixBandWidth bw1 = gm1->BandWidth(), bw2 = gm2->BandWidth();
+   if (bw1.Lower() < 0)
+   {
+      if (bw2.Lower() < 0) { REPORT lower = -1; }
+      else { REPORT lower = bw2.Lower() + (gm1->Nrows() - 1) * gm2->Nrows(); }
+   }
+   else
+   {
+      if (bw2.Lower() < 0)
+         { REPORT lower = (1 + bw1.Lower()) * gm2->Nrows() - 1; }
+      else { REPORT lower = bw2.Lower() + bw1.Lower() * gm2->Nrows(); }
+   }
+   if (bw1.Upper() < 0)
+   {
+      if (bw2.Upper() < 0) { REPORT upper = -1; }
+      else { REPORT upper = bw2.Upper() + (gm1->Nrows() - 1) * gm2->Nrows(); }
+   }
+   else
+   {
+      if (bw2.Upper() < 0)
+         { REPORT upper = (1 + bw1.Upper()) * gm2->Nrows() - 1; }
+      else { REPORT upper = bw2.Upper() + bw1.Upper() * gm2->Nrows(); }
+   }
+   return MatrixBandWidth(lower, upper);
+}
+
+MatrixBandWidth MultipliedMatrix::BandWidth() const
+{ REPORT return gm1->BandWidth() * gm2->BandWidth(); }
+
+MatrixBandWidth ConcatenatedMatrix::BandWidth() const { REPORT return -1; }
+
+MatrixBandWidth SolvedMatrix::BandWidth() const
+{
+   if (+gm1->Type() & MatrixType::Diagonal)
+      { REPORT return gm2->BandWidth(); }
+   else { REPORT return -1; }
+}
+
+MatrixBandWidth ScaledMatrix::BandWidth() const
+   { REPORT return gm->BandWidth(); }
+
+MatrixBandWidth NegatedMatrix::BandWidth() const
+   { REPORT return gm->BandWidth(); }
+
+MatrixBandWidth TransposedMatrix::BandWidth() const
+   { REPORT return gm->BandWidth().t(); }
+
+MatrixBandWidth InvertedMatrix::BandWidth() const
+{
+   if (+gm->Type() & MatrixType::Diagonal)
+      { REPORT return MatrixBandWidth(0,0); }
+   else { REPORT return -1; }
+}
+
+MatrixBandWidth RowedMatrix::BandWidth() const { REPORT return -1; }
+MatrixBandWidth ColedMatrix::BandWidth() const { REPORT return -1; }
+MatrixBandWidth DiagedMatrix::BandWidth() const { REPORT return 0; }
+MatrixBandWidth MatedMatrix::BandWidth() const { REPORT return -1; }
+MatrixBandWidth ReturnMatrixX::BandWidth() const
+   { REPORT return gm->BandWidth(); }
+
+MatrixBandWidth GetSubMatrix::BandWidth() const
+{
+
+   if (row_skip==col_skip && row_number==col_number)
+      { REPORT return gm->BandWidth(); }
+   else { REPORT return MatrixBandWidth(-1); }
+}
+
+// ********************** the memory managment tools **********************/
+
+//  Rules regarding tDelete, reuse, GetStore
+//    All matrices processed during expression evaluation must be subject
+//    to exactly one of reuse(), tDelete(), GetStore() or BorrowStore().
+//    If reuse returns true the matrix must be reused.
+//    GetMatrix(gm) always calls gm->GetStore()
+//    gm->Evaluate obeys rules
+//    bm->Evaluate obeys rules for matrices in bm structure
+
+void GeneralMatrix::tDelete()
+{
+   if (tag<0)
+   {
+      if (tag<-1) { REPORT store=0; delete this; return; }  // borrowed
+      else { REPORT return; }
+   }
+   if (tag==1)
+   {
+      if (store)
+      {
+         REPORT  MONITOR_REAL_DELETE("Free   (tDelete)",storage,store)
+         delete [] store;
+      }
+      store=0; CleanUp(); tag=-1; return;
+   }
+   if (tag==0) { REPORT delete this; return; }
+   REPORT tag--; return;
+}
+
+static void BlockCopy(int n, Real* from, Real* to)
+{
+   REPORT
+   int i = (n >> 3);
+   while (i--)
+   {
+      *to++ = *from++; *to++ = *from++; *to++ = *from++; *to++ = *from++;
+      *to++ = *from++; *to++ = *from++; *to++ = *from++; *to++ = *from++;
+   }
+   i = n & 7; while (i--) *to++ = *from++;
+}
+
+bool GeneralMatrix::reuse()
+{
+   if (tag<-1)
+   {
+      if (storage)
+      {
+         REPORT
+         Real* s = new Real [storage]; MatrixErrorNoSpace(s);
+         MONITOR_REAL_NEW("Make     (reuse)",storage,s)
+         BlockCopy(storage, store, s); store=s;
+      }
+      else { REPORT store = 0; CleanUp(); }
+      tag=0; return true;
+   }
+   if (tag<0) { REPORT return false; }
+   if (tag<=1)  { REPORT return true; }
+   REPORT tag--; return false;
+}
+
+Real* GeneralMatrix::GetStore()
+{
+   if (tag<0 || tag>1)
+   {
+      Real* s;
+      if (storage)
+      {
+         s = new Real [storage]; MatrixErrorNoSpace(s);
+         MONITOR_REAL_NEW("Make  (GetStore)",storage,s)
+         BlockCopy(storage, store, s);
+      }
+      else s = 0;
+      if (tag>1) { REPORT tag--; }
+      else if (tag < -1) { REPORT store=0; delete this; } // borrowed store
+      else { REPORT }
+      return s;
+   }
+   Real* s=store; store=0;
+   if (tag==0) { REPORT delete this; }
+   else { REPORT CleanUp(); tag=-1; }
+   return s;
+}
+
+void GeneralMatrix::GetMatrix(const GeneralMatrix* gmx)
+{
+   REPORT  tag=-1; nrows=gmx->Nrows(); ncols=gmx->Ncols();
+   storage=gmx->storage; SetParameters(gmx);
+   store=((GeneralMatrix*)gmx)->GetStore();
+}
+
+GeneralMatrix* GeneralMatrix::BorrowStore(GeneralMatrix* gmx, MatrixType mt)
+// Copy storage of *this to storage of *gmx. Then convert to type mt.
+// If mt == 0 just let *gmx point to storage of *this if tag==-1.
+{
+   if (!mt)
+   {
+      if (tag == -1) { REPORT gmx->tag = -2; gmx->store = store; }
+      else { REPORT gmx->tag = 0; gmx->store = GetStore(); }
+   }
+   else if (Compare(gmx->Type(),mt))
+   { REPORT  gmx->tag = 0; gmx->store = GetStore(); }
+   else
+   {
+      REPORT gmx->tag = -2; gmx->store = store;
+      gmx = gmx->Evaluate(mt); gmx->tag = 0; tDelete();
+   }
+
+   return gmx;
+}
+
+void GeneralMatrix::Eq(const BaseMatrix& X, MatrixType mt)
+// Count number of references to this in X.
+// If zero delete storage in this;
+// otherwise tag this to show when storage can be deleted
+// evaluate X and copy to this
+{
+#ifdef DO_SEARCH
+   int counter=X.search(this);
+   if (counter==0)
+   {
+      REPORT
+      if (store)
+      {
+         MONITOR_REAL_DELETE("Free (operator=)",storage,store)
+         REPORT delete [] store; storage=0; store = 0;
+      }
+   }
+   else { REPORT Release(counter); }
+   GeneralMatrix* gmx = ((BaseMatrix&)X).Evaluate(mt);
+   if (gmx!=this) { REPORT GetMatrix(gmx); }
+   else { REPORT }
+   Protect();
+#else
+   GeneralMatrix* gmx = ((BaseMatrix&)X).Evaluate(mt);
+   if (gmx!=this)
+   {
+      REPORT
+      if (store)
+      {
+         MONITOR_REAL_DELETE("Free (operator=)",storage,store)
+         REPORT delete [] store; storage=0; store = 0;
+      }
+      GetMatrix(gmx);
+   }
+   else { REPORT }
+   Protect();
+#endif
+}
+
+// version to work with operator<<
+void GeneralMatrix::Eq(const BaseMatrix& X, MatrixType mt, bool ldok)
+{
+   REPORT
+   if (ldok) mt.SetDataLossOK();
+   Eq(X, mt);
+}
+
+void GeneralMatrix::Eq2(const BaseMatrix& X, MatrixType mt)
+// a cut down version of Eq for use with += etc.
+// we know BaseMatrix points to two GeneralMatrix objects,
+// the first being this (may be the same).
+// we know tag has been set correctly in each.
+{
+   GeneralMatrix* gmx = ((BaseMatrix&)X).Evaluate(mt);
+   if (gmx!=this) { REPORT GetMatrix(gmx); }  // simplify GetMatrix ?
+   else { REPORT }
+   Protect();
+}
+
+void GeneralMatrix::Inject(const GeneralMatrix& X)
+// copy stored values of X; otherwise leave els of *this unchanged
+{
+   REPORT
+   Tracer tr("Inject");
+   if (nrows != X.nrows || ncols != X.ncols)
+      Throw(IncompatibleDimensionsException());
+   MatrixRow mr((GeneralMatrix*)&X, LoadOnEntry);
+   MatrixRow mrx(this, LoadOnEntry+StoreOnExit+DirectPart);
+   int i=nrows;
+   while (i--) { mrx.Inject(mr); mrx.Next(); mr.Next(); }
+}
+
+// ************* checking for data loss during conversion *******************/
+
+bool Compare(const MatrixType& source, MatrixType& destination)
+{
+   if (!destination) { destination=source; return true; }
+   if (destination==source) return true;
+   if (!destination.DataLossOK && !(destination>=source))
+      Throw(ProgramException("Illegal Conversion", source, destination));
+   return false;
+}
+
+// ************* Make a copy of a matrix on the heap *********************/
+
+GeneralMatrix* Matrix::Image() const
+{
+   REPORT
+   GeneralMatrix* gm = new Matrix(*this); MatrixErrorNoSpace(gm);
+   return gm;
+}
+
+GeneralMatrix* SymmetricMatrix::Image() const
+{
+   REPORT
+   GeneralMatrix* gm = new SymmetricMatrix(*this); MatrixErrorNoSpace(gm);
+   return gm;
+}
+
+GeneralMatrix* UpperTriangularMatrix::Image() const
+{
+   REPORT
+   GeneralMatrix* gm = new UpperTriangularMatrix(*this);
+   MatrixErrorNoSpace(gm); return gm;
+}
+
+GeneralMatrix* LowerTriangularMatrix::Image() const
+{
+   REPORT
+   GeneralMatrix* gm = new LowerTriangularMatrix(*this);
+   MatrixErrorNoSpace(gm); return gm;
+}
+
+GeneralMatrix* DiagonalMatrix::Image() const
+{
+   REPORT
+   GeneralMatrix* gm = new DiagonalMatrix(*this); MatrixErrorNoSpace(gm);
+   return gm;
+}
+
+GeneralMatrix* RowVector::Image() const
+{
+   REPORT
+   GeneralMatrix* gm = new RowVector(*this); MatrixErrorNoSpace(gm);
+   return gm;
+}
+
+GeneralMatrix* ColumnVector::Image() const
+{
+   REPORT
+   GeneralMatrix* gm = new ColumnVector(*this); MatrixErrorNoSpace(gm);
+   return gm;
+}
+
+GeneralMatrix* BandMatrix::Image() const
+{
+   REPORT
+   GeneralMatrix* gm = new BandMatrix(*this); MatrixErrorNoSpace(gm);
+   return gm;
+}
+
+GeneralMatrix* UpperBandMatrix::Image() const
+{
+   REPORT
+   GeneralMatrix* gm = new UpperBandMatrix(*this); MatrixErrorNoSpace(gm);
+   return gm;
+}
+
+GeneralMatrix* LowerBandMatrix::Image() const
+{
+   REPORT
+   GeneralMatrix* gm = new LowerBandMatrix(*this); MatrixErrorNoSpace(gm);
+   return gm;
+}
+
+GeneralMatrix* SymmetricBandMatrix::Image() const
+{
+   REPORT
+   GeneralMatrix* gm = new SymmetricBandMatrix(*this); MatrixErrorNoSpace(gm);
+   return gm;
+}
+
+GeneralMatrix* nricMatrix::Image() const
+{
+   REPORT
+   GeneralMatrix* gm = new nricMatrix(*this); MatrixErrorNoSpace(gm);
+   return gm;
+}
+
+GeneralMatrix* IdentityMatrix::Image() const
+{
+   REPORT
+   GeneralMatrix* gm = new IdentityMatrix(*this); MatrixErrorNoSpace(gm);
+   return gm;
+}
+
+GeneralMatrix* GeneralMatrix::Image() const
+{
+   bool dummy = true;
+   if (dummy)                                   // get rid of warning message
+      Throw(InternalException("Cannot apply Image to this matrix type"));
+   return 0;
+}
+
+
+// *********************** nricMatrix routines *****************************/
+
+void nricMatrix::MakeRowPointer()
+{
+   if (nrows > 0)
+   {
+      row_pointer = new Real* [nrows]; MatrixErrorNoSpace(row_pointer);
+      Real* s = Store() - 1; int i = nrows; Real** rp = row_pointer;
+      if (i) for (;;) { *rp++ = s; if (!(--i)) break; s+=ncols; }
+   }
+   else row_pointer = 0;
+}
+
+void nricMatrix::DeleteRowPointer()
+{ if (nrows) delete [] row_pointer; }
+
+void GeneralMatrix::CheckStore() const
+{
+   if (!store)
+      Throw(ProgramException("NRIC accessing matrix with unset dimensions"));
+}
+
+
+// *************************** CleanUp routines *****************************/
+
+void GeneralMatrix::CleanUp()
+{
+   // set matrix dimensions to zero, delete storage
+   REPORT
+   if (store && storage)
+   {
+      MONITOR_REAL_DELETE("Free (CleanUp)    ",storage,store)
+      REPORT delete [] store;
+   }
+   store=0; storage=0; nrows=0; ncols=0;
+}
+
+void nricMatrix::CleanUp()
+{ DeleteRowPointer(); GeneralMatrix::CleanUp(); }
+
+void RowVector::CleanUp()
+{ GeneralMatrix::CleanUp(); nrows=1; }
+
+void ColumnVector::CleanUp()
+{ GeneralMatrix::CleanUp(); ncols=1; }
+
+void CroutMatrix::CleanUp()
+{
+   if (nrows) delete [] indx;
+   GeneralMatrix::CleanUp();
+}
+
+void BandLUMatrix::CleanUp()
+{
+   if (nrows) delete [] indx;
+   if (storage2) delete [] store2;
+   GeneralMatrix::CleanUp();
+}
+
+// ************************ simple integer array class ***********************
+
+// construct a new array of length xn. Check that xn is non-negative and
+// that space is available
+
+SimpleIntArray::SimpleIntArray(int xn) : n(xn)
+{
+   if (n < 0) Throw(Logic_error("invalid array length"));
+   else if (n == 0) { REPORT  a = 0; }
+   else { REPORT  a = new int [n]; if (!a) Throw(Bad_alloc()); }
+}
+
+// destroy an array - return its space to memory
+
+SimpleIntArray::~SimpleIntArray() { REPORT  if (a) delete [] a; }
+
+// access an element of an array; return a "reference" so elements
+// can be modified.
+// check index is within range
+// in this array class the index runs from 0 to n-1
+
+int& SimpleIntArray::operator[](int i)
+{
+   REPORT
+   if (i < 0 || i >= n) Throw(Logic_error("array index out of range"));
+   return a[i];
+}
+
+// same thing again but for arrays declared constant so we can't
+// modify its elements
+
+int SimpleIntArray::operator[](int i) const
+{
+   REPORT
+   if (i < 0 || i >= n) Throw(Logic_error("array index out of range"));
+   return a[i];
+}
+
+// set all the elements equal to a given value
+
+void SimpleIntArray::operator=(int ai)
+   { REPORT  for (int i = 0; i < n; i++) a[i] = ai; }
+
+// set the elements equal to those of another array.
+// check the arrays are of the same length
+
+void SimpleIntArray::operator=(const SimpleIntArray& b)
+{
+   REPORT
+   if (b.n != n) Throw(Logic_error("array lengths differ in copy"));
+   for (int i = 0; i < n; i++) a[i] = b.a[i];
+}
+
+// construct a new array equal to an existing array
+// check that space is available
+
+SimpleIntArray::SimpleIntArray(const SimpleIntArray& b) : n(b.n)
+{
+   if (n == 0) { REPORT  a = 0; }
+   else
+   {
+      REPORT
+      a = new int [n]; if (!a) Throw(Bad_alloc());
+      for (int i = 0; i < n; i++) a[i] = b.a[i];
+   }
+}
+
+// change the size of an array; optionally copy data from old array to
+// new array
+
+void SimpleIntArray::ReSize(int n1, bool keep)
+{
+   if (n1 == n) { REPORT  return; }
+   else if (n1 == 0) { REPORT  n = 0; delete [] a; a = 0; }
+   else if (n == 0)
+      { REPORT  a = new int [n1]; if (!a) Throw(Bad_alloc()); n = n1; }
+   else
+   {
+      int* a1 = a;
+      if (keep)
+      {
+         REPORT
+         a = new int [n1]; if (!a) Throw(Bad_alloc());
+         if (n > n1) n = n1;
+         for (int i = 0; i < n; i++) a[i] = a1[i];
+         n = n1; delete [] a1;
+      }
+      else
+      {
+         REPORT  n = n1; delete [] a1;
+         a = new int [n]; if (!a) Throw(Bad_alloc());
+      }
+   }
+}
+
+
+#ifdef use_namespace
+}
+#endif
+
Index: orxonox/trunk/src/util/newmat/newmat5.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/newmat5.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/newmat5.cpp	(revision 4565)
@@ -0,0 +1,563 @@
+//$$ newmat5.cpp         Transpose, evaluate etc
+
+// Copyright (C) 1991,2,3,4: R B Davies
+
+//#define WANT_STREAM
+
+#include "include.h"
+
+#include "newmat.h"
+#include "newmatrc.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+
+#ifdef DO_REPORT
+#define REPORT { static ExeCounter ExeCount(__LINE__,5); ++ExeCount; }
+#else
+#define REPORT {}
+#endif
+
+
+/************************ carry out operations ******************************/
+
+
+GeneralMatrix* GeneralMatrix::Transpose(TransposedMatrix* tm, MatrixType mt)
+{
+   GeneralMatrix* gm1;
+
+   if (Compare(Type().t(),mt))
+   {
+      REPORT
+      gm1 = mt.New(ncols,nrows,tm);
+      for (int i=0; i<ncols; i++)
+      {
+         MatrixRow mr(gm1, StoreOnExit+DirectPart, i);
+         MatrixCol mc(this, mr.Data(), LoadOnEntry, i);
+      }
+   }
+   else
+   {
+      REPORT
+      gm1 = mt.New(ncols,nrows,tm);
+      MatrixRow mr(this, LoadOnEntry);
+      MatrixCol mc(gm1, StoreOnExit+DirectPart);
+      int i = nrows;
+      while (i--) { mc.Copy(mr); mr.Next(); mc.Next(); }
+   }
+   tDelete(); gm1->ReleaseAndDelete(); return gm1;
+}
+
+GeneralMatrix* SymmetricMatrix::Transpose(TransposedMatrix*, MatrixType mt)
+{ REPORT  return Evaluate(mt); }
+
+
+GeneralMatrix* DiagonalMatrix::Transpose(TransposedMatrix*, MatrixType mt)
+{ REPORT return Evaluate(mt); }
+
+GeneralMatrix* ColumnVector::Transpose(TransposedMatrix*, MatrixType mt)
+{
+   REPORT
+   GeneralMatrix* gmx = new RowVector; MatrixErrorNoSpace(gmx);
+   gmx->nrows = 1; gmx->ncols = gmx->storage = storage;
+   return BorrowStore(gmx,mt);
+}
+
+GeneralMatrix* RowVector::Transpose(TransposedMatrix*, MatrixType mt)
+{
+   REPORT
+   GeneralMatrix* gmx = new ColumnVector; MatrixErrorNoSpace(gmx);
+   gmx->ncols = 1; gmx->nrows = gmx->storage = storage;
+   return BorrowStore(gmx,mt);
+}
+
+GeneralMatrix* IdentityMatrix::Transpose(TransposedMatrix*, MatrixType mt)
+{ REPORT return Evaluate(mt); }
+
+GeneralMatrix* GeneralMatrix::Evaluate(MatrixType mt)
+{
+   if (Compare(this->Type(),mt)) { REPORT return this; }
+   REPORT
+   GeneralMatrix* gmx = mt.New(nrows,ncols,this);
+   MatrixRow mr(this, LoadOnEntry);
+   MatrixRow mrx(gmx, StoreOnExit+DirectPart);
+   int i=nrows;
+   while (i--) { mrx.Copy(mr); mrx.Next(); mr.Next(); }
+   tDelete(); gmx->ReleaseAndDelete(); return gmx;
+}
+
+GeneralMatrix* GenericMatrix::Evaluate(MatrixType mt)
+   { REPORT  return gm->Evaluate(mt); }
+
+GeneralMatrix* ShiftedMatrix::Evaluate(MatrixType mt)
+{
+   gm=((BaseMatrix*&)bm)->Evaluate();
+   int nr=gm->Nrows(); int nc=gm->Ncols();
+   Compare(gm->Type().AddEqualEl(),mt);
+   if (!(mt==gm->Type()))
+   {
+      REPORT
+      GeneralMatrix* gmx = mt.New(nr,nc,this);
+      MatrixRow mr(gm, LoadOnEntry);
+      MatrixRow mrx(gmx, StoreOnExit+DirectPart);
+      while (nr--) { mrx.Add(mr,f); mrx.Next(); mr.Next(); }
+      gmx->ReleaseAndDelete(); gm->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+      return gmx;
+   }
+   else if (gm->reuse())
+   {
+      REPORT gm->Add(f);
+#ifdef TEMPS_DESTROYED_QUICKLY
+      GeneralMatrix* gmx = gm; delete this; return gmx;
+#else
+      return gm;
+#endif
+   }
+   else
+   {
+      REPORT GeneralMatrix* gmy = gm->Type().New(nr,nc,this);
+      gmy->ReleaseAndDelete(); gmy->Add(gm,f);
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+      return gmy;
+   }
+}
+
+GeneralMatrix* NegShiftedMatrix::Evaluate(MatrixType mt)
+{
+   gm=((BaseMatrix*&)bm)->Evaluate();
+   int nr=gm->Nrows(); int nc=gm->Ncols();
+   Compare(gm->Type().AddEqualEl(),mt);
+   if (!(mt==gm->Type()))
+   {
+      REPORT
+      GeneralMatrix* gmx = mt.New(nr,nc,this);
+      MatrixRow mr(gm, LoadOnEntry);
+      MatrixRow mrx(gmx, StoreOnExit+DirectPart);
+      while (nr--) { mrx.NegAdd(mr,f); mrx.Next(); mr.Next(); }
+      gmx->ReleaseAndDelete(); gm->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+      return gmx;
+   }
+   else if (gm->reuse())
+   {
+      REPORT gm->NegAdd(f);
+#ifdef TEMPS_DESTROYED_QUICKLY
+      GeneralMatrix* gmx = gm; delete this; return gmx;
+#else
+      return gm;
+#endif
+   }
+   else
+   {
+      REPORT GeneralMatrix* gmy = gm->Type().New(nr,nc,this);
+      gmy->ReleaseAndDelete(); gmy->NegAdd(gm,f);
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+      return gmy;
+   }
+}
+
+GeneralMatrix* ScaledMatrix::Evaluate(MatrixType mt)
+{
+   gm=((BaseMatrix*&)bm)->Evaluate();
+   int nr=gm->Nrows(); int nc=gm->Ncols();
+   if (Compare(gm->Type(),mt))
+   {
+      if (gm->reuse())
+      {
+         REPORT gm->Multiply(f);
+#ifdef TEMPS_DESTROYED_QUICKLY
+         GeneralMatrix* gmx = gm; delete this; return gmx;
+#else
+         return gm;
+#endif
+      }
+      else
+      {
+         REPORT GeneralMatrix* gmx = gm->Type().New(nr,nc,this);
+         gmx->ReleaseAndDelete(); gmx->Multiply(gm,f);
+#ifdef TEMPS_DESTROYED_QUICKLY
+         delete this;
+#endif
+         return gmx;
+      }
+   }
+   else
+   {
+      REPORT
+      GeneralMatrix* gmx = mt.New(nr,nc,this);
+      MatrixRow mr(gm, LoadOnEntry);
+      MatrixRow mrx(gmx, StoreOnExit+DirectPart);
+      while (nr--) { mrx.Multiply(mr,f); mrx.Next(); mr.Next(); }
+      gmx->ReleaseAndDelete(); gm->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+      return gmx;
+   }
+}
+
+GeneralMatrix* NegatedMatrix::Evaluate(MatrixType mt)
+{
+   gm=((BaseMatrix*&)bm)->Evaluate();
+   int nr=gm->Nrows(); int nc=gm->Ncols();
+   if (Compare(gm->Type(),mt))
+   {
+      if (gm->reuse())
+      {
+         REPORT gm->Negate();
+#ifdef TEMPS_DESTROYED_QUICKLY
+         GeneralMatrix* gmx = gm; delete this; return gmx;
+#else
+         return gm;
+#endif
+      }
+      else
+      {
+         REPORT
+         GeneralMatrix* gmx = gm->Type().New(nr,nc,this);
+         gmx->ReleaseAndDelete(); gmx->Negate(gm);
+#ifdef TEMPS_DESTROYED_QUICKLY
+         delete this;
+#endif
+         return gmx;
+      }
+   }
+   else
+   {
+      REPORT
+      GeneralMatrix* gmx = mt.New(nr,nc,this);
+      MatrixRow mr(gm, LoadOnEntry);
+      MatrixRow mrx(gmx, StoreOnExit+DirectPart);
+      while (nr--) { mrx.Negate(mr); mrx.Next(); mr.Next(); }
+      gmx->ReleaseAndDelete(); gm->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+      return gmx;
+   }
+}
+
+GeneralMatrix* ReversedMatrix::Evaluate(MatrixType mt)
+{
+   gm=((BaseMatrix*&)bm)->Evaluate(); GeneralMatrix* gmx;
+
+   if ((gm->Type()).IsBand() && ! (gm->Type()).IsDiagonal())
+   {
+      gm->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+      Throw(NotDefinedException("Reverse", "band matrices"));
+   }
+
+   if (gm->reuse()) { REPORT gm->ReverseElements(); gmx = gm; }
+   else
+   {
+      REPORT
+      gmx = gm->Type().New(gm->Nrows(), gm->Ncols(), this);
+      gmx->ReverseElements(gm); gmx->ReleaseAndDelete();
+   }
+#ifdef TEMPS_DESTROYED_QUICKLY
+   delete this;
+#endif
+   return gmx->Evaluate(mt); // target matrix is different type?
+
+}
+
+GeneralMatrix* TransposedMatrix::Evaluate(MatrixType mt)
+{
+   REPORT
+   gm=((BaseMatrix*&)bm)->Evaluate();
+   Compare(gm->Type().t(),mt);
+   GeneralMatrix* gmx=gm->Transpose(this, mt);
+#ifdef TEMPS_DESTROYED_QUICKLY
+   delete this;
+#endif
+   return gmx;
+}
+
+GeneralMatrix* RowedMatrix::Evaluate(MatrixType mt)
+{
+   gm = ((BaseMatrix*&)bm)->Evaluate();
+   GeneralMatrix* gmx = new RowVector; MatrixErrorNoSpace(gmx);
+   gmx->nrows = 1; gmx->ncols = gmx->storage = gm->storage;
+#ifdef TEMPS_DESTROYED_QUICKLY
+   GeneralMatrix* gmy = gm; delete this; return gmy->BorrowStore(gmx,mt);
+#else
+   return gm->BorrowStore(gmx,mt);
+#endif
+}
+
+GeneralMatrix* ColedMatrix::Evaluate(MatrixType mt)
+{
+   gm = ((BaseMatrix*&)bm)->Evaluate();
+   GeneralMatrix* gmx = new ColumnVector; MatrixErrorNoSpace(gmx);
+   gmx->ncols = 1; gmx->nrows = gmx->storage = gm->storage;
+#ifdef TEMPS_DESTROYED_QUICKLY
+   GeneralMatrix* gmy = gm; delete this; return gmy->BorrowStore(gmx,mt);
+#else
+   return gm->BorrowStore(gmx,mt);
+#endif
+}
+
+GeneralMatrix* DiagedMatrix::Evaluate(MatrixType mt)
+{
+   gm = ((BaseMatrix*&)bm)->Evaluate();
+   GeneralMatrix* gmx = new DiagonalMatrix; MatrixErrorNoSpace(gmx);
+   gmx->nrows = gmx->ncols = gmx->storage = gm->storage;
+#ifdef TEMPS_DESTROYED_QUICKLY
+   GeneralMatrix* gmy = gm; delete this; return gmy->BorrowStore(gmx,mt);
+#else
+   return gm->BorrowStore(gmx,mt);
+#endif
+}
+
+GeneralMatrix* MatedMatrix::Evaluate(MatrixType mt)
+{
+   Tracer tr("MatedMatrix::Evaluate");
+   gm = ((BaseMatrix*&)bm)->Evaluate();
+   GeneralMatrix* gmx = new Matrix; MatrixErrorNoSpace(gmx);
+   gmx->nrows = nr; gmx->ncols = nc; gmx->storage = gm->storage;
+   if (nr*nc != gmx->storage)
+      Throw(IncompatibleDimensionsException());
+#ifdef TEMPS_DESTROYED_QUICKLY
+   GeneralMatrix* gmy = gm; delete this; return gmy->BorrowStore(gmx,mt);
+#else
+   return gm->BorrowStore(gmx,mt);
+#endif
+}
+
+GeneralMatrix* GetSubMatrix::Evaluate(MatrixType mt)
+{
+   REPORT
+   Tracer tr("SubMatrix(evaluate)");
+   gm = ((BaseMatrix*&)bm)->Evaluate();
+   if (row_number < 0) row_number = gm->Nrows();
+   if (col_number < 0) col_number = gm->Ncols();
+   if (row_skip+row_number > gm->Nrows() || col_skip+col_number > gm->Ncols())
+   {
+      gm->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+      Throw(SubMatrixDimensionException());
+   }
+   if (IsSym) Compare(gm->Type().ssub(), mt);
+   else Compare(gm->Type().sub(), mt);
+   GeneralMatrix* gmx = mt.New(row_number, col_number, this);
+   int i = row_number;
+   MatrixRow mr(gm, LoadOnEntry, row_skip); 
+   MatrixRow mrx(gmx, StoreOnExit+DirectPart);
+   MatrixRowCol sub;
+   while (i--)
+   {
+      mr.SubRowCol(sub, col_skip, col_number);   // put values in sub
+      mrx.Copy(sub); mrx.Next(); mr.Next();
+   }
+   gmx->ReleaseAndDelete(); gm->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+   delete this;
+#endif
+   return gmx;
+}   
+
+
+GeneralMatrix* ReturnMatrixX::Evaluate(MatrixType mt)
+{
+#ifdef TEMPS_DESTROYED_QUICKLY_R
+   GeneralMatrix* gmx = gm; delete this; return gmx->Evaluate(mt);
+#else
+   return gm->Evaluate(mt);
+#endif
+}
+
+
+void GeneralMatrix::Add(GeneralMatrix* gm1, Real f)
+{
+   REPORT
+   Real* s1=gm1->store; Real* s=store; int i=(storage >> 2);
+   while (i--)
+   { *s++ = *s1++ + f; *s++ = *s1++ + f; *s++ = *s1++ + f; *s++ = *s1++ + f; }
+   i = storage & 3; while (i--) *s++ = *s1++ + f;
+}
+   
+void GeneralMatrix::Add(Real f)
+{
+   REPORT
+   Real* s=store; int i=(storage >> 2);
+   while (i--) { *s++ += f; *s++ += f; *s++ += f; *s++ += f; }
+   i = storage & 3; while (i--) *s++ += f;
+}
+   
+void GeneralMatrix::NegAdd(GeneralMatrix* gm1, Real f)
+{
+   REPORT
+   Real* s1=gm1->store; Real* s=store; int i=(storage >> 2);
+   while (i--)
+   { *s++ = f - *s1++; *s++ = f - *s1++; *s++ = f - *s1++; *s++ = f - *s1++; }
+   i = storage & 3; while (i--) *s++ = f - *s1++;
+}
+   
+void GeneralMatrix::NegAdd(Real f)
+{
+   REPORT
+   Real* s=store; int i=(storage >> 2);
+   while (i--)
+   {
+      *s = f - *s; s++; *s = f - *s; s++;
+      *s = f - *s; s++; *s = f - *s; s++;
+   }
+   i = storage & 3; while (i--)  { *s = f - *s; s++; }
+}
+   
+void GeneralMatrix::Negate(GeneralMatrix* gm1)
+{
+   // change sign of elements
+   REPORT
+   Real* s1=gm1->store; Real* s=store; int i=(storage >> 2);
+   while (i--)
+   { *s++ = -(*s1++); *s++ = -(*s1++); *s++ = -(*s1++); *s++ = -(*s1++); }
+   i = storage & 3; while(i--) *s++ = -(*s1++);
+}
+   
+void GeneralMatrix::Negate()
+{
+   REPORT
+   Real* s=store; int i=(storage >> 2);
+   while (i--)
+   { *s = -(*s); s++; *s = -(*s); s++; *s = -(*s); s++; *s = -(*s); s++; }
+   i = storage & 3; while(i--) { *s = -(*s); s++; }
+}
+   
+void GeneralMatrix::Multiply(GeneralMatrix* gm1, Real f)
+{
+   REPORT
+   Real* s1=gm1->store; Real* s=store;  int i=(storage >> 2);
+   while (i--)
+   { *s++ = *s1++ * f; *s++ = *s1++ * f; *s++ = *s1++ * f; *s++ = *s1++ * f; }
+   i = storage & 3; while (i--) *s++ = *s1++ * f;
+}
+   
+void GeneralMatrix::Multiply(Real f)
+{
+   REPORT
+   Real* s=store; int i=(storage >> 2);
+   while (i--) { *s++ *= f; *s++ *= f; *s++ *= f; *s++ *= f; }
+   i = storage & 3; while (i--) *s++ *= f;
+}
+   
+
+/************************ MatrixInput routines ****************************/
+
+// int MatrixInput::n;          // number values still to be read
+// Real* MatrixInput::r;        // pointer to next location to be read to
+
+MatrixInput MatrixInput::operator<<(Real f)
+{
+   REPORT
+   Tracer et("MatrixInput");
+   if (n<=0) Throw(ProgramException("List of values too long"));
+   *r = f; int n1 = n-1; n=0;   // n=0 so we won't trigger exception
+   return MatrixInput(n1, r+1);
+}
+
+
+MatrixInput GeneralMatrix::operator<<(Real f)
+{
+   REPORT
+   Tracer et("MatrixInput");
+   int n = Storage();
+   if (n<=0) Throw(ProgramException("Loading data to zero length matrix"));
+   Real* r; r = Store(); *r = f; n--;
+   return MatrixInput(n, r+1);
+}
+
+MatrixInput GetSubMatrix::operator<<(Real f)
+{
+   REPORT
+   Tracer et("MatrixInput (GetSubMatrix)");
+   SetUpLHS();
+   if (row_number != 1 || col_skip != 0 || col_number != gm->Ncols())
+   {
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+      Throw(ProgramException("MatrixInput requires complete rows"));
+   }
+   MatrixRow mr(gm, DirectPart, row_skip);  // to pick up location and length
+   int n = mr.Storage();
+   if (n<=0)
+   {
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+      Throw(ProgramException("Loading data to zero length row"));
+   }
+   Real* r; r = mr.Data(); *r = f; n--;
+   if (+(mr.cw*HaveStore))
+   {
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+      Throw(ProgramException("Fails with this matrix type"));
+   }
+#ifdef TEMPS_DESTROYED_QUICKLY
+   delete this;
+#endif
+   return MatrixInput(n, r+1);
+}
+
+MatrixInput::~MatrixInput()
+{
+   REPORT
+   Tracer et("MatrixInput");
+   if (n!=0) Throw(ProgramException("A list of values was too short"));
+}
+
+MatrixInput BandMatrix::operator<<(Real)
+{
+   Tracer et("MatrixInput");
+   bool dummy = true;
+   if (dummy)                                   // get rid of warning message
+      Throw(ProgramException("Cannot use list read with a BandMatrix"));
+   return MatrixInput(0, 0);
+}
+
+void BandMatrix::operator<<(const Real*)
+{ Throw(ProgramException("Cannot use array read with a BandMatrix")); }
+
+// ************************* Reverse order of elements ***********************
+
+void GeneralMatrix::ReverseElements(GeneralMatrix* gm)
+{
+   // reversing into a new matrix
+   REPORT
+   int n = Storage(); Real* rx = Store() + n; Real* x = gm->Store();
+   while (n--) *(--rx) = *(x++);
+}
+
+void GeneralMatrix::ReverseElements()
+{
+   // reversing in place
+   REPORT
+   int n = Storage(); Real* x = Store(); Real* rx = x + n;
+   n /= 2;
+   while (n--) { Real t = *(--rx); *rx = *x; *(x++) = t; }
+}
+
+
+#ifdef use_namespace
+}
+#endif
+
Index: orxonox/trunk/src/util/newmat/newmat6.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/newmat6.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/newmat6.cpp	(revision 4565)
@@ -0,0 +1,1124 @@
+//$$ newmat6.cpp            Operators, element access, submatrices
+
+// Copyright (C) 1991,2,3,4: R B Davies
+
+#include "include.h"
+
+#include "newmat.h"
+#include "newmatrc.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+
+
+#ifdef DO_REPORT
+#define REPORT { static ExeCounter ExeCount(__LINE__,6); ++ExeCount; }
+#else
+#define REPORT {}
+#endif
+
+/*************************** general utilities *************************/
+
+static int tristore(int n)                      // els in triangular matrix
+{ return (n*(n+1))/2; }
+
+
+/****************************** operators *******************************/
+
+Real& Matrix::operator()(int m, int n)
+{
+   REPORT
+   if (m<=0 || m>nrows || n<=0 || n>ncols)
+      Throw(IndexException(m,n,*this));
+   return store[(m-1)*ncols+n-1];
+}
+
+Real& SymmetricMatrix::operator()(int m, int n)
+{
+   REPORT
+   if (m<=0 || n<=0 || m>nrows || n>ncols)
+      Throw(IndexException(m,n,*this));
+   if (m>=n) return store[tristore(m-1)+n-1];
+   else return store[tristore(n-1)+m-1];
+}
+
+Real& UpperTriangularMatrix::operator()(int m, int n)
+{
+   REPORT
+   if (m<=0 || n<m || n>ncols)
+      Throw(IndexException(m,n,*this));
+   return store[(m-1)*ncols+n-1-tristore(m-1)];
+}
+
+Real& LowerTriangularMatrix::operator()(int m, int n)
+{
+   REPORT
+   if (n<=0 || m<n || m>nrows)
+      Throw(IndexException(m,n,*this));
+   return store[tristore(m-1)+n-1];
+}
+
+Real& DiagonalMatrix::operator()(int m, int n)
+{
+   REPORT
+   if (n<=0 || m!=n || m>nrows || n>ncols)
+      Throw(IndexException(m,n,*this));
+   return store[n-1];
+}
+
+Real& DiagonalMatrix::operator()(int m)
+{
+   REPORT
+   if (m<=0 || m>nrows) Throw(IndexException(m,*this));
+   return store[m-1];
+}
+
+Real& ColumnVector::operator()(int m)
+{
+   REPORT
+   if (m<=0 || m> nrows) Throw(IndexException(m,*this));
+   return store[m-1];
+}
+
+Real& RowVector::operator()(int n)
+{
+   REPORT
+   if (n<=0 || n> ncols) Throw(IndexException(n,*this));
+   return store[n-1];
+}
+
+Real& BandMatrix::operator()(int m, int n)
+{
+   REPORT
+   int w = upper+lower+1; int i = lower+n-m;
+   if (m<=0 || m>nrows || n<=0 || n>ncols || i<0 || i>=w)
+      Throw(IndexException(m,n,*this));
+   return store[w*(m-1)+i];
+}
+
+Real& UpperBandMatrix::operator()(int m, int n)
+{
+   REPORT
+   int w = upper+1; int i = n-m;
+   if (m<=0 || m>nrows || n<=0 || n>ncols || i<0 || i>=w)
+      Throw(IndexException(m,n,*this));
+   return store[w*(m-1)+i];
+}
+
+Real& LowerBandMatrix::operator()(int m, int n)
+{
+   REPORT
+   int w = lower+1; int i = lower+n-m;
+   if (m<=0 || m>nrows || n<=0 || n>ncols || i<0 || i>=w)
+      Throw(IndexException(m,n,*this));
+   return store[w*(m-1)+i];
+}
+
+Real& SymmetricBandMatrix::operator()(int m, int n)
+{
+   REPORT
+   int w = lower+1;
+   if (m>=n)
+   {
+      REPORT
+      int i = lower+n-m;
+      if ( m>nrows || n<=0 || i<0 )
+         Throw(IndexException(m,n,*this));
+      return store[w*(m-1)+i];
+   }
+   else
+   {
+      REPORT
+      int i = lower+m-n;
+      if ( n>nrows || m<=0 || i<0 )
+         Throw(IndexException(m,n,*this));
+      return store[w*(n-1)+i];
+   }
+}
+
+
+Real Matrix::operator()(int m, int n) const
+{
+   REPORT
+   if (m<=0 || m>nrows || n<=0 || n>ncols)
+      Throw(IndexException(m,n,*this));
+   return store[(m-1)*ncols+n-1];
+}
+
+Real SymmetricMatrix::operator()(int m, int n) const
+{
+   REPORT
+   if (m<=0 || n<=0 || m>nrows || n>ncols)
+      Throw(IndexException(m,n,*this));
+   if (m>=n) return store[tristore(m-1)+n-1];
+   else return store[tristore(n-1)+m-1];
+}
+
+Real UpperTriangularMatrix::operator()(int m, int n) const
+{
+   REPORT
+   if (m<=0 || n<m || n>ncols)
+      Throw(IndexException(m,n,*this));
+   return store[(m-1)*ncols+n-1-tristore(m-1)];
+}
+
+Real LowerTriangularMatrix::operator()(int m, int n) const
+{
+   REPORT
+   if (n<=0 || m<n || m>nrows)
+      Throw(IndexException(m,n,*this));
+   return store[tristore(m-1)+n-1];
+}
+
+Real DiagonalMatrix::operator()(int m, int n) const
+{
+   REPORT
+   if (n<=0 || m!=n || m>nrows || n>ncols)
+      Throw(IndexException(m,n,*this));
+   return store[n-1];
+}
+
+Real DiagonalMatrix::operator()(int m) const
+{
+   REPORT
+   if (m<=0 || m>nrows) Throw(IndexException(m,*this));
+   return store[m-1];
+}
+
+Real ColumnVector::operator()(int m) const
+{
+   REPORT
+   if (m<=0 || m> nrows) Throw(IndexException(m,*this));
+   return store[m-1];
+}
+
+Real RowVector::operator()(int n) const
+{
+   REPORT
+   if (n<=0 || n> ncols) Throw(IndexException(n,*this));
+   return store[n-1];
+}
+
+Real BandMatrix::operator()(int m, int n) const
+{
+   REPORT
+   int w = upper+lower+1; int i = lower+n-m;
+   if (m<=0 || m>nrows || n<=0 || n>ncols || i<0 || i>=w)
+      Throw(IndexException(m,n,*this));
+   return store[w*(m-1)+i];
+}
+
+Real UpperBandMatrix::operator()(int m, int n) const
+{
+   REPORT
+   int w = upper+1; int i = n-m;
+   if (m<=0 || m>nrows || n<=0 || n>ncols || i<0 || i>=w)
+      Throw(IndexException(m,n,*this));
+   return store[w*(m-1)+i];
+}
+
+Real LowerBandMatrix::operator()(int m, int n) const
+{
+   REPORT
+   int w = lower+1; int i = lower+n-m;
+   if (m<=0 || m>nrows || n<=0 || n>ncols || i<0 || i>=w)
+      Throw(IndexException(m,n,*this));
+   return store[w*(m-1)+i];
+}
+
+Real SymmetricBandMatrix::operator()(int m, int n) const
+{
+   REPORT
+   int w = lower+1;
+   if (m>=n)
+   {
+      REPORT
+      int i = lower+n-m;
+      if ( m>nrows || n<=0 || i<0 )
+         Throw(IndexException(m,n,*this));
+      return store[w*(m-1)+i];
+   }
+   else
+   {
+      REPORT
+      int i = lower+m-n;
+      if ( n>nrows || m<=0 || i<0 )
+         Throw(IndexException(m,n,*this));
+      return store[w*(n-1)+i];
+   }
+}
+
+
+Real BaseMatrix::AsScalar() const
+{
+   REPORT
+   GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
+
+   if (gm->nrows!=1 || gm->ncols!=1)
+   {
+      Tracer tr("AsScalar");
+      Try
+         { Throw(ProgramException("Cannot convert to scalar", *gm)); }
+      CatchAll { gm->tDelete(); ReThrow; }
+   }
+
+   Real x = *(gm->store); gm->tDelete(); return x;
+}
+
+#ifdef TEMPS_DESTROYED_QUICKLY
+
+AddedMatrix& BaseMatrix::operator+(const BaseMatrix& bm) const
+{
+   REPORT
+   AddedMatrix* x = new AddedMatrix(this, &bm);
+   MatrixErrorNoSpace(x);
+   return *x;
+}
+
+SPMatrix& SP(const BaseMatrix& bm1,const BaseMatrix& bm2)
+{
+   REPORT
+   SPMatrix* x = new SPMatrix(&bm1, &bm2);
+   MatrixErrorNoSpace(x);
+   return *x;
+}
+
+KPMatrix& KP(const BaseMatrix& bm1,const BaseMatrix& bm2)
+{
+   REPORT
+   KPMatrix* x = new KPMatrix(&bm1, &bm2);
+   MatrixErrorNoSpace(x);
+   return *x;
+}
+
+MultipliedMatrix& BaseMatrix::operator*(const BaseMatrix& bm) const
+{
+   REPORT
+   MultipliedMatrix* x = new MultipliedMatrix(this, &bm);
+   MatrixErrorNoSpace(x);
+   return *x;
+}
+
+ConcatenatedMatrix& BaseMatrix::operator|(const BaseMatrix& bm) const
+{
+   REPORT
+   ConcatenatedMatrix* x = new ConcatenatedMatrix(this, &bm);
+   MatrixErrorNoSpace(x);
+   return *x;
+}
+
+StackedMatrix& BaseMatrix::operator&(const BaseMatrix& bm) const
+{
+   REPORT
+   StackedMatrix* x = new StackedMatrix(this, &bm);
+   MatrixErrorNoSpace(x);
+   return *x;
+}
+
+//SolvedMatrix& InvertedMatrix::operator*(const BaseMatrix& bmx) const
+SolvedMatrix& InvertedMatrix::operator*(const BaseMatrix& bmx)
+{
+   REPORT
+   SolvedMatrix* x;
+   Try { x = new SolvedMatrix(bm, &bmx); MatrixErrorNoSpace(x); }
+   CatchAll { delete this; ReThrow; }
+   delete this;                // since we are using bm rather than this
+   return *x;
+}
+
+SubtractedMatrix& BaseMatrix::operator-(const BaseMatrix& bm) const
+{
+   REPORT
+   SubtractedMatrix* x = new SubtractedMatrix(this, &bm);
+   MatrixErrorNoSpace(x);
+   return *x;
+}
+
+ShiftedMatrix& BaseMatrix::operator+(Real f) const
+{
+   REPORT
+   ShiftedMatrix* x = new ShiftedMatrix(this, f);
+   MatrixErrorNoSpace(x);
+   return *x;
+}
+
+NegShiftedMatrix& operator-(Real f,const BaseMatrix& bm1)
+{
+   REPORT
+   NegShiftedMatrix* x = new NegShiftedMatrix(f, &bm1);
+   MatrixErrorNoSpace(x);
+   return *x;
+}
+
+ScaledMatrix& BaseMatrix::operator*(Real f) const
+{
+   REPORT
+   ScaledMatrix* x = new ScaledMatrix(this, f);
+   MatrixErrorNoSpace(x);
+   return *x;
+}
+
+ScaledMatrix& BaseMatrix::operator/(Real f) const
+{
+   REPORT
+   ScaledMatrix* x = new ScaledMatrix(this, 1.0/f);
+   MatrixErrorNoSpace(x);
+   return *x;
+}
+
+ShiftedMatrix& BaseMatrix::operator-(Real f) const
+{
+   REPORT
+   ShiftedMatrix* x = new ShiftedMatrix(this, -f);
+   MatrixErrorNoSpace(x);
+   return *x;
+}
+
+TransposedMatrix& BaseMatrix::t() const
+{
+   REPORT
+   TransposedMatrix* x = new TransposedMatrix(this);
+   MatrixErrorNoSpace(x);
+   return *x;
+}
+
+NegatedMatrix& BaseMatrix::operator-() const
+{
+   REPORT
+   NegatedMatrix* x = new NegatedMatrix(this);
+   MatrixErrorNoSpace(x);
+   return *x;
+}
+
+ReversedMatrix& BaseMatrix::Reverse() const
+{
+   REPORT
+   ReversedMatrix* x = new ReversedMatrix(this);
+   MatrixErrorNoSpace(x);
+   return *x;
+}
+
+InvertedMatrix& BaseMatrix::i() const
+{
+   REPORT
+   InvertedMatrix* x = new InvertedMatrix(this);
+   MatrixErrorNoSpace(x);
+   return *x;
+}
+
+RowedMatrix& BaseMatrix::AsRow() const
+{
+   REPORT
+   RowedMatrix* x = new RowedMatrix(this);
+   MatrixErrorNoSpace(x);
+   return *x;
+}
+
+ColedMatrix& BaseMatrix::AsColumn() const
+{
+   REPORT
+   ColedMatrix* x = new ColedMatrix(this);
+   MatrixErrorNoSpace(x);
+   return *x;
+}
+
+DiagedMatrix& BaseMatrix::AsDiagonal() const
+{
+   REPORT
+   DiagedMatrix* x = new DiagedMatrix(this);
+   MatrixErrorNoSpace(x);
+   return *x;
+}
+
+MatedMatrix& BaseMatrix::AsMatrix(int nrx, int ncx) const
+{
+   REPORT
+   MatedMatrix* x = new MatedMatrix(this,nrx,ncx);
+   MatrixErrorNoSpace(x);
+   return *x;
+}
+
+#else
+
+AddedMatrix BaseMatrix::operator+(const BaseMatrix& bm) const
+{ REPORT return AddedMatrix(this, &bm); }
+
+SPMatrix SP(const BaseMatrix& bm1,const BaseMatrix& bm2)
+{ REPORT return SPMatrix(&bm1, &bm2); }
+
+KPMatrix KP(const BaseMatrix& bm1,const BaseMatrix& bm2)
+{ REPORT return KPMatrix(&bm1, &bm2); }
+
+MultipliedMatrix BaseMatrix::operator*(const BaseMatrix& bm) const
+{ REPORT return MultipliedMatrix(this, &bm); }
+
+ConcatenatedMatrix BaseMatrix::operator|(const BaseMatrix& bm) const
+{ REPORT return ConcatenatedMatrix(this, &bm); }
+
+StackedMatrix BaseMatrix::operator&(const BaseMatrix& bm) const
+{ REPORT return StackedMatrix(this, &bm); }
+
+SolvedMatrix InvertedMatrix::operator*(const BaseMatrix& bmx) const
+{ REPORT return SolvedMatrix(bm, &bmx); }
+
+SubtractedMatrix BaseMatrix::operator-(const BaseMatrix& bm) const
+{ REPORT return SubtractedMatrix(this, &bm); }
+
+ShiftedMatrix BaseMatrix::operator+(Real f) const
+{ REPORT return ShiftedMatrix(this, f); }
+
+NegShiftedMatrix operator-(Real f, const BaseMatrix& bm)
+{ REPORT return NegShiftedMatrix(f, &bm); }
+
+ScaledMatrix BaseMatrix::operator*(Real f) const
+{ REPORT return ScaledMatrix(this, f); }
+
+ScaledMatrix BaseMatrix::operator/(Real f) const
+{ REPORT return ScaledMatrix(this, 1.0/f); }
+
+ShiftedMatrix BaseMatrix::operator-(Real f) const
+{ REPORT return ShiftedMatrix(this, -f); }
+
+TransposedMatrix BaseMatrix::t() const
+{ REPORT return TransposedMatrix(this); }
+
+NegatedMatrix BaseMatrix::operator-() const
+{ REPORT return NegatedMatrix(this); }
+
+ReversedMatrix BaseMatrix::Reverse() const
+{ REPORT return ReversedMatrix(this); }
+
+InvertedMatrix BaseMatrix::i() const
+{ REPORT return InvertedMatrix(this); }
+
+
+RowedMatrix BaseMatrix::AsRow() const
+{ REPORT return RowedMatrix(this); }
+
+ColedMatrix BaseMatrix::AsColumn() const
+{ REPORT return ColedMatrix(this); }
+
+DiagedMatrix BaseMatrix::AsDiagonal() const
+{ REPORT return DiagedMatrix(this); }
+
+MatedMatrix BaseMatrix::AsMatrix(int nrx, int ncx) const
+{ REPORT return MatedMatrix(this,nrx,ncx); }
+
+#endif
+
+void GeneralMatrix::operator=(Real f)
+{ REPORT int i=storage; Real* s=store; while (i--) { *s++ = f; } }
+
+void Matrix::operator=(const BaseMatrix& X)
+{
+   REPORT //CheckConversion(X);
+   // MatrixConversionCheck mcc;
+   Eq(X,MatrixType::Rt);
+} 
+
+void RowVector::operator=(const BaseMatrix& X)
+{
+   REPORT  // CheckConversion(X);
+   // MatrixConversionCheck mcc;
+   Eq(X,MatrixType::RV);
+   if (nrows!=1)
+      { Tracer tr("RowVector(=)"); Throw(VectorException(*this)); }
+}
+
+void ColumnVector::operator=(const BaseMatrix& X)
+{
+   REPORT //CheckConversion(X);
+   // MatrixConversionCheck mcc;
+   Eq(X,MatrixType::CV);
+   if (ncols!=1)
+      { Tracer tr("ColumnVector(=)"); Throw(VectorException(*this)); }
+}
+
+void SymmetricMatrix::operator=(const BaseMatrix& X)
+{
+   REPORT // CheckConversion(X);
+   // MatrixConversionCheck mcc;
+   Eq(X,MatrixType::Sm);
+}
+
+void UpperTriangularMatrix::operator=(const BaseMatrix& X)
+{
+   REPORT //CheckConversion(X);
+   // MatrixConversionCheck mcc;
+   Eq(X,MatrixType::UT);
+}
+
+void LowerTriangularMatrix::operator=(const BaseMatrix& X)
+{
+   REPORT //CheckConversion(X);
+   // MatrixConversionCheck mcc;
+   Eq(X,MatrixType::LT);
+}
+
+void DiagonalMatrix::operator=(const BaseMatrix& X)
+{
+   REPORT // CheckConversion(X);
+   // MatrixConversionCheck mcc;
+   Eq(X,MatrixType::Dg);
+}
+
+void IdentityMatrix::operator=(const BaseMatrix& X)
+{
+   REPORT // CheckConversion(X);
+   // MatrixConversionCheck mcc;
+   Eq(X,MatrixType::Id);
+}
+
+void GeneralMatrix::operator<<(const Real* r)
+{
+   REPORT
+   int i = storage; Real* s=store;
+   while(i--) *s++ = *r++;
+}
+
+
+void GenericMatrix::operator=(const GenericMatrix& bmx)
+{
+   if (&bmx != this) { REPORT if (gm) delete gm; gm = bmx.gm->Image();}
+   else { REPORT }
+   gm->Protect();
+}
+
+void GenericMatrix::operator=(const BaseMatrix& bmx)
+{
+   if (gm)
+   {
+      int counter=bmx.search(gm);
+      if (counter==0) { REPORT delete gm; gm=0; }
+      else { REPORT gm->Release(counter); }
+   }
+   else { REPORT }
+   GeneralMatrix* gmx = ((BaseMatrix&)bmx).Evaluate();
+   if (gmx != gm) { REPORT if (gm) delete gm; gm = gmx->Image(); }
+   else { REPORT }
+   gm->Protect();
+}
+
+
+/*************************** += etc ***************************************/
+
+// will also need versions for SubMatrix
+
+
+
+// GeneralMatrix operators
+
+void GeneralMatrix::operator+=(const BaseMatrix& X)
+{
+   REPORT
+   Tracer tr("GeneralMatrix::operator+=");
+   // MatrixConversionCheck mcc;
+   Protect();                                   // so it cannot get deleted
+						// during Evaluate
+   GeneralMatrix* gm = ((BaseMatrix&)X).Evaluate();
+#ifdef TEMPS_DESTROYED_QUICKLY
+   AddedMatrix* am = new AddedMatrix(this,gm);
+   MatrixErrorNoSpace(am);
+   if (gm==this) Release(2); else Release();
+   Eq2(*am,Type());
+#else
+   AddedMatrix am(this,gm);
+   if (gm==this) Release(2); else Release();
+   Eq2(am,Type());
+#endif
+}
+
+void GeneralMatrix::operator-=(const BaseMatrix& X)
+{
+   REPORT
+   Tracer tr("GeneralMatrix::operator-=");
+   // MatrixConversionCheck mcc;
+   Protect();                                   // so it cannot get deleted
+						// during Evaluate
+   GeneralMatrix* gm = ((BaseMatrix&)X).Evaluate();
+#ifdef TEMPS_DESTROYED_QUICKLY
+   SubtractedMatrix* am = new SubtractedMatrix(this,gm);
+   MatrixErrorNoSpace(am);
+   if (gm==this) Release(2); else Release();
+   Eq2(*am,Type());
+#else
+   SubtractedMatrix am(this,gm);
+   if (gm==this) Release(2); else Release();
+   Eq2(am,Type());
+#endif
+}
+
+void GeneralMatrix::operator*=(const BaseMatrix& X)
+{
+   REPORT
+   Tracer tr("GeneralMatrix::operator*=");
+   // MatrixConversionCheck mcc;
+   Protect();                                   // so it cannot get deleted
+						// during Evaluate
+   GeneralMatrix* gm = ((BaseMatrix&)X).Evaluate();
+#ifdef TEMPS_DESTROYED_QUICKLY
+   MultipliedMatrix* am = new MultipliedMatrix(this,gm);
+   MatrixErrorNoSpace(am);
+   if (gm==this) Release(2); else Release();
+   Eq2(*am,Type());
+#else
+   MultipliedMatrix am(this,gm);
+   if (gm==this) Release(2); else Release();
+   Eq2(am,Type());
+#endif
+}
+
+void GeneralMatrix::operator|=(const BaseMatrix& X)
+{
+   REPORT
+   Tracer tr("GeneralMatrix::operator|=");
+   // MatrixConversionCheck mcc;
+   Protect();                                   // so it cannot get deleted
+						// during Evaluate
+   GeneralMatrix* gm = ((BaseMatrix&)X).Evaluate();
+#ifdef TEMPS_DESTROYED_QUICKLY
+   ConcatenatedMatrix* am = new ConcatenatedMatrix(this,gm);
+   MatrixErrorNoSpace(am);
+   if (gm==this) Release(2); else Release();
+   Eq2(*am,Type());
+#else
+   ConcatenatedMatrix am(this,gm);
+   if (gm==this) Release(2); else Release();
+   Eq2(am,Type());
+#endif
+}
+
+void GeneralMatrix::operator&=(const BaseMatrix& X)
+{
+   REPORT
+   Tracer tr("GeneralMatrix::operator&=");
+   // MatrixConversionCheck mcc;
+   Protect();                                   // so it cannot get deleted
+						// during Evaluate
+   GeneralMatrix* gm = ((BaseMatrix&)X).Evaluate();
+#ifdef TEMPS_DESTROYED_QUICKLY
+   StackedMatrix* am = new StackedMatrix(this,gm);
+   MatrixErrorNoSpace(am);
+   if (gm==this) Release(2); else Release();
+   Eq2(*am,Type());
+#else
+   StackedMatrix am(this,gm);
+   if (gm==this) Release(2); else Release();
+   Eq2(am,Type());
+#endif
+}
+
+void GeneralMatrix::operator+=(Real r)
+{
+   REPORT
+   Tracer tr("GeneralMatrix::operator+=(Real)");
+   // MatrixConversionCheck mcc;
+#ifdef TEMPS_DESTROYED_QUICKLY
+   ShiftedMatrix* am = new ShiftedMatrix(this,r);
+   MatrixErrorNoSpace(am);
+   Release(); Eq2(*am,Type());
+#else
+   ShiftedMatrix am(this,r);
+   Release(); Eq2(am,Type());
+#endif
+}
+
+void GeneralMatrix::operator*=(Real r)
+{
+   REPORT
+   Tracer tr("GeneralMatrix::operator*=(Real)");
+   // MatrixConversionCheck mcc;
+#ifdef TEMPS_DESTROYED_QUICKLY
+   ScaledMatrix* am = new ScaledMatrix(this,r);
+   MatrixErrorNoSpace(am);
+   Release(); Eq2(*am,Type());
+#else
+   ScaledMatrix am(this,r);
+   Release(); Eq2(am,Type());
+#endif
+}
+
+
+// Generic matrix operators
+
+void GenericMatrix::operator+=(const BaseMatrix& X)
+{
+   REPORT
+   Tracer tr("GenericMatrix::operator+=");
+   if (!gm) Throw(ProgramException("GenericMatrix is null"));
+   gm->Protect();            // so it cannot get deleted during Evaluate
+   GeneralMatrix* gmx = ((BaseMatrix&)X).Evaluate();
+#ifdef TEMPS_DESTROYED_QUICKLY
+   AddedMatrix* am = new AddedMatrix(gm,gmx);
+   MatrixErrorNoSpace(am);
+   if (gmx==gm) gm->Release(2); else gm->Release();
+   GeneralMatrix* gmy = am->Evaluate();
+#else
+   AddedMatrix am(gm,gmx);
+   if (gmx==gm) gm->Release(2); else gm->Release();
+   GeneralMatrix* gmy = am.Evaluate();
+#endif
+   if (gmy != gm) { REPORT delete gm; gm = gmy->Image(); }
+   else { REPORT }
+   gm->Protect();
+}
+
+void GenericMatrix::operator-=(const BaseMatrix& X)
+{
+   REPORT
+   Tracer tr("GenericMatrix::operator-=");
+   if (!gm) Throw(ProgramException("GenericMatrix is null"));
+   gm->Protect();            // so it cannot get deleted during Evaluate
+   GeneralMatrix* gmx = ((BaseMatrix&)X).Evaluate();
+#ifdef TEMPS_DESTROYED_QUICKLY
+   SubtractedMatrix* am = new SubtractedMatrix(gm,gmx);
+   MatrixErrorNoSpace(am);
+   if (gmx==gm) gm->Release(2); else gm->Release();
+   GeneralMatrix* gmy = am->Evaluate();
+#else
+   SubtractedMatrix am(gm,gmx);
+   if (gmx==gm) gm->Release(2); else gm->Release();
+   GeneralMatrix* gmy = am.Evaluate();
+#endif
+   if (gmy != gm) { REPORT delete gm; gm = gmy->Image(); }
+   else { REPORT }
+   gm->Protect();
+}
+
+void GenericMatrix::operator*=(const BaseMatrix& X)
+{
+   REPORT
+   Tracer tr("GenericMatrix::operator*=");
+   if (!gm) Throw(ProgramException("GenericMatrix is null"));
+   gm->Protect();            // so it cannot get deleted during Evaluate
+   GeneralMatrix* gmx = ((BaseMatrix&)X).Evaluate();
+#ifdef TEMPS_DESTROYED_QUICKLY
+   MultipliedMatrix* am = new MultipliedMatrix(gm,gmx);
+   MatrixErrorNoSpace(am);
+   if (gmx==gm) gm->Release(2); else gm->Release();
+   GeneralMatrix* gmy = am->Evaluate();
+#else
+   MultipliedMatrix am(gm,gmx);
+   if (gmx==gm) gm->Release(2); else gm->Release();
+   GeneralMatrix* gmy = am.Evaluate();
+#endif
+   if (gmy != gm) { REPORT delete gm; gm = gmy->Image(); }
+   else { REPORT }
+   gm->Protect();
+}
+
+void GenericMatrix::operator|=(const BaseMatrix& X)
+{
+   REPORT
+   Tracer tr("GenericMatrix::operator|=");
+   if (!gm) Throw(ProgramException("GenericMatrix is null"));
+   gm->Protect();            // so it cannot get deleted during Evaluate
+   GeneralMatrix* gmx = ((BaseMatrix&)X).Evaluate();
+#ifdef TEMPS_DESTROYED_QUICKLY
+   ConcatenatedMatrix* am = new ConcatenatedMatrix(gm,gmx);
+   MatrixErrorNoSpace(am);
+   if (gmx==gm) gm->Release(2); else gm->Release();
+   GeneralMatrix* gmy = am->Evaluate();
+#else
+   ConcatenatedMatrix am(gm,gmx);
+   if (gmx==gm) gm->Release(2); else gm->Release();
+   GeneralMatrix* gmy = am.Evaluate();
+#endif
+   if (gmy != gm) { REPORT delete gm; gm = gmy->Image(); }
+   else { REPORT }
+   gm->Protect();
+}
+
+void GenericMatrix::operator&=(const BaseMatrix& X)
+{
+   REPORT
+   Tracer tr("GenericMatrix::operator&=");
+   if (!gm) Throw(ProgramException("GenericMatrix is null"));
+   gm->Protect();            // so it cannot get deleted during Evaluate
+   GeneralMatrix* gmx = ((BaseMatrix&)X).Evaluate();
+#ifdef TEMPS_DESTROYED_QUICKLY
+   StackedMatrix* am = new StackedMatrix(gm,gmx);
+   MatrixErrorNoSpace(am);
+   if (gmx==gm) gm->Release(2); else gm->Release();
+   GeneralMatrix* gmy = am->Evaluate();
+#else
+   StackedMatrix am(gm,gmx);
+   if (gmx==gm) gm->Release(2); else gm->Release();
+   GeneralMatrix* gmy = am.Evaluate();
+#endif
+   if (gmy != gm) { REPORT delete gm; gm = gmy->Image(); }
+   else { REPORT }
+   gm->Protect();
+}
+
+void GenericMatrix::operator+=(Real r)
+{
+   REPORT
+   Tracer tr("GenericMatrix::operator+= (Real)");
+   if (!gm) Throw(ProgramException("GenericMatrix is null"));
+#ifdef TEMPS_DESTROYED_QUICKLY
+   ShiftedMatrix* am = new ShiftedMatrix(gm,r);
+   MatrixErrorNoSpace(am);
+   gm->Release();
+   GeneralMatrix* gmy = am->Evaluate();
+#else
+   ShiftedMatrix am(gm,r);
+   gm->Release();
+   GeneralMatrix* gmy = am.Evaluate();
+#endif
+   if (gmy != gm) { REPORT delete gm; gm = gmy->Image(); }
+   else { REPORT }
+   gm->Protect();
+}
+
+void GenericMatrix::operator*=(Real r)
+{
+   REPORT
+   Tracer tr("GenericMatrix::operator*= (Real)");
+   if (!gm) Throw(ProgramException("GenericMatrix is null"));
+#ifdef TEMPS_DESTROYED_QUICKLY
+   ScaledMatrix* am = new ScaledMatrix(gm,r);
+   MatrixErrorNoSpace(am);
+   gm->Release();
+   GeneralMatrix* gmy = am->Evaluate();
+#else
+   ScaledMatrix am(gm,r);
+   gm->Release();
+   GeneralMatrix* gmy = am.Evaluate();
+#endif
+   if (gmy != gm) { REPORT delete gm; gm = gmy->Image(); }
+   else { REPORT }
+   gm->Protect();
+}
+
+
+/************************* element access *********************************/
+
+Real& Matrix::element(int m, int n)
+{
+   REPORT
+   if (m<0 || m>= nrows || n<0 || n>= ncols)
+      Throw(IndexException(m,n,*this,true));
+   return store[m*ncols+n];
+}
+
+Real Matrix::element(int m, int n) const
+{
+   REPORT
+   if (m<0 || m>= nrows || n<0 || n>= ncols)
+      Throw(IndexException(m,n,*this,true));
+   return store[m*ncols+n];
+}
+
+Real& SymmetricMatrix::element(int m, int n)
+{
+   REPORT
+   if (m<0 || n<0 || m >= nrows || n>=ncols)
+      Throw(IndexException(m,n,*this,true));
+   if (m>=n) return store[tristore(m)+n];
+   else return store[tristore(n)+m];
+}
+
+Real SymmetricMatrix::element(int m, int n) const
+{
+   REPORT
+   if (m<0 || n<0 || m >= nrows || n>=ncols)
+      Throw(IndexException(m,n,*this,true));
+   if (m>=n) return store[tristore(m)+n];
+   else return store[tristore(n)+m];
+}
+
+Real& UpperTriangularMatrix::element(int m, int n)
+{
+   REPORT
+   if (m<0 || n<m || n>=ncols)
+      Throw(IndexException(m,n,*this,true));
+   return store[m*ncols+n-tristore(m)];
+}
+
+Real UpperTriangularMatrix::element(int m, int n) const
+{
+   REPORT
+   if (m<0 || n<m || n>=ncols)
+      Throw(IndexException(m,n,*this,true));
+   return store[m*ncols+n-tristore(m)];
+}
+
+Real& LowerTriangularMatrix::element(int m, int n)
+{
+   REPORT
+   if (n<0 || m<n || m>=nrows)
+      Throw(IndexException(m,n,*this,true));
+   return store[tristore(m)+n];
+}
+
+Real LowerTriangularMatrix::element(int m, int n) const
+{
+   REPORT
+   if (n<0 || m<n || m>=nrows)
+      Throw(IndexException(m,n,*this,true));
+   return store[tristore(m)+n];
+}
+
+Real& DiagonalMatrix::element(int m, int n)
+{
+   REPORT
+   if (n<0 || m!=n || m>=nrows || n>=ncols)
+      Throw(IndexException(m,n,*this,true));
+   return store[n];
+}
+
+Real DiagonalMatrix::element(int m, int n) const
+{
+   REPORT
+   if (n<0 || m!=n || m>=nrows || n>=ncols)
+      Throw(IndexException(m,n,*this,true));
+   return store[n];
+}
+
+Real& DiagonalMatrix::element(int m)
+{
+   REPORT
+   if (m<0 || m>=nrows) Throw(IndexException(m,*this,true));
+   return store[m];
+}
+
+Real DiagonalMatrix::element(int m) const
+{
+   REPORT
+   if (m<0 || m>=nrows) Throw(IndexException(m,*this,true));
+   return store[m];
+}
+
+Real& ColumnVector::element(int m)
+{
+   REPORT
+   if (m<0 || m>= nrows) Throw(IndexException(m,*this,true));
+   return store[m];
+}
+
+Real ColumnVector::element(int m) const
+{
+   REPORT
+   if (m<0 || m>= nrows) Throw(IndexException(m,*this,true));
+   return store[m];
+}
+
+Real& RowVector::element(int n)
+{
+   REPORT
+   if (n<0 || n>= ncols)  Throw(IndexException(n,*this,true));
+   return store[n];
+}
+
+Real RowVector::element(int n) const
+{
+   REPORT
+   if (n<0 || n>= ncols)  Throw(IndexException(n,*this,true));
+   return store[n];
+}
+
+Real& BandMatrix::element(int m, int n)
+{
+   REPORT
+   int w = upper+lower+1; int i = lower+n-m;
+   if (m<0 || m>= nrows || n<0 || n>= ncols || i<0 || i>=w)
+      Throw(IndexException(m,n,*this,true));
+   return store[w*m+i];
+}
+
+Real BandMatrix::element(int m, int n) const
+{
+   REPORT
+   int w = upper+lower+1; int i = lower+n-m;
+   if (m<0 || m>= nrows || n<0 || n>= ncols || i<0 || i>=w)
+      Throw(IndexException(m,n,*this,true));
+   return store[w*m+i];
+}
+
+Real& UpperBandMatrix::element(int m, int n)
+{
+   REPORT
+   int w = upper+1; int i = n-m;
+   if (m<0 || m>= nrows || n<0 || n>= ncols || i<0 || i>=w)
+      Throw(IndexException(m,n,*this,true));
+   return store[w*m+i];
+}
+
+Real UpperBandMatrix::element(int m, int n) const
+{
+   REPORT
+   int w = upper+1; int i = n-m;
+   if (m<0 || m>= nrows || n<0 || n>= ncols || i<0 || i>=w)
+      Throw(IndexException(m,n,*this,true));
+   return store[w*m+i];
+}
+
+Real& LowerBandMatrix::element(int m, int n)
+{
+   REPORT
+   int w = lower+1; int i = lower+n-m;
+   if (m<0 || m>= nrows || n<0 || n>= ncols || i<0 || i>=w)
+      Throw(IndexException(m,n,*this,true));
+   return store[w*m+i];
+}
+
+Real LowerBandMatrix::element(int m, int n) const
+{
+   REPORT
+   int w = lower+1; int i = lower+n-m;
+   if (m<0 || m>= nrows || n<0 || n>= ncols || i<0 || i>=w)
+      Throw(IndexException(m,n,*this,true));
+   return store[w*m+i];
+}
+
+Real& SymmetricBandMatrix::element(int m, int n)
+{
+   REPORT
+   int w = lower+1;
+   if (m>=n)
+   {
+      REPORT
+      int i = lower+n-m;
+      if ( m>=nrows || n<0 || i<0 )
+         Throw(IndexException(m,n,*this,true));
+      return store[w*m+i];
+   }
+   else
+   {
+      REPORT
+      int i = lower+m-n;
+      if ( n>=nrows || m<0 || i<0 )
+         Throw(IndexException(m,n,*this,true));
+      return store[w*n+i];
+   }
+}
+
+Real SymmetricBandMatrix::element(int m, int n) const
+{
+   REPORT
+   int w = lower+1;
+   if (m>=n)
+   {
+      REPORT
+      int i = lower+n-m;
+      if ( m>=nrows || n<0 || i<0 )
+         Throw(IndexException(m,n,*this,true));
+      return store[w*m+i];
+   }
+   else
+   {
+      REPORT
+      int i = lower+m-n;
+      if ( n>=nrows || m<0 || i<0 )
+         Throw(IndexException(m,n,*this,true));
+      return store[w*n+i];
+   }
+}
+
+#ifdef use_namespace
+}
+#endif
+
Index: orxonox/trunk/src/util/newmat/newmat7.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/newmat7.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/newmat7.cpp	(revision 4565)
@@ -0,0 +1,1052 @@
+//$$ newmat7.cpp     Invert, solve, binary operations
+
+// Copyright (C) 1991,2,3,4: R B Davies
+
+#include "include.h"
+
+#include "newmat.h"
+#include "newmatrc.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+
+#ifdef DO_REPORT
+#define REPORT { static ExeCounter ExeCount(__LINE__,7); ++ExeCount; }
+#else
+#define REPORT {}
+#endif
+
+
+//***************************** solve routines ******************************/
+
+GeneralMatrix* GeneralMatrix::MakeSolver()
+{
+   REPORT
+   GeneralMatrix* gm = new CroutMatrix(*this);
+   MatrixErrorNoSpace(gm); gm->ReleaseAndDelete(); return gm;
+}
+
+GeneralMatrix* Matrix::MakeSolver()
+{
+   REPORT
+   GeneralMatrix* gm = new CroutMatrix(*this);
+   MatrixErrorNoSpace(gm); gm->ReleaseAndDelete(); return gm;
+}
+
+void CroutMatrix::Solver(MatrixColX& mcout, const MatrixColX& mcin)
+{
+   REPORT
+   int i = mcin.skip; Real* el = mcin.data-i; Real* el1 = el;
+   while (i--) *el++ = 0.0;
+   el += mcin.storage; i = nrows - mcin.skip - mcin.storage;
+   while (i--) *el++ = 0.0;
+   lubksb(el1, mcout.skip);
+}
+
+
+// Do we need check for entirely zero output?
+
+void UpperTriangularMatrix::Solver(MatrixColX& mcout,
+   const MatrixColX& mcin)
+{
+   REPORT
+   int i = mcin.skip-mcout.skip; Real* elx = mcin.data-i;
+   while (i-- > 0) *elx++ = 0.0;
+   int nr = mcin.skip+mcin.storage;
+   elx = mcin.data+mcin.storage; Real* el = elx;
+   int j = mcout.skip+mcout.storage-nr; int nc = ncols-nr; i = nr-mcout.skip;
+   while (j-- > 0) *elx++ = 0.0;
+   Real* Ael = store + (nr*(2*ncols-nr+1))/2; j = 0;
+   while (i-- > 0)
+   {
+      elx = el; Real sum = 0.0; int jx = j++; Ael -= nc;
+      while (jx--) sum += *(--Ael) * *(--elx);
+      elx--; *elx = (*elx - sum) / *(--Ael);
+   }
+}
+
+void LowerTriangularMatrix::Solver(MatrixColX& mcout,
+   const MatrixColX& mcin)
+{
+   REPORT
+   int i = mcin.skip-mcout.skip; Real* elx = mcin.data-i;
+   while (i-- > 0) *elx++ = 0.0;
+   int nc = mcin.skip; i = nc+mcin.storage; elx = mcin.data+mcin.storage;
+   int nr = mcout.skip+mcout.storage; int j = nr-i; i = nr-nc;
+   while (j-- > 0) *elx++ = 0.0;
+   Real* el = mcin.data; Real* Ael = store + (nc*(nc+1))/2; j = 0;
+   while (i-- > 0)
+   {
+      elx = el; Real sum = 0.0; int jx = j++; Ael += nc;
+      while (jx--) sum += *Ael++ * *elx++;
+      *elx = (*elx - sum) / *Ael++;
+   }
+}
+
+//******************* carry out binary operations *************************/
+
+static GeneralMatrix*
+   GeneralMult(GeneralMatrix*,GeneralMatrix*,MultipliedMatrix*,MatrixType);
+static GeneralMatrix*
+   GeneralSolv(GeneralMatrix*,GeneralMatrix*,BaseMatrix*,MatrixType);
+static GeneralMatrix*
+   GeneralSolvI(GeneralMatrix*,BaseMatrix*,MatrixType);
+static GeneralMatrix*
+   GeneralKP(GeneralMatrix*,GeneralMatrix*,KPMatrix*,MatrixType);
+
+GeneralMatrix* MultipliedMatrix::Evaluate(MatrixType mt)
+{
+   REPORT
+   gm2 = ((BaseMatrix*&)bm2)->Evaluate();
+   gm2 = gm2->Evaluate(gm2->Type().MultRHS());     // no symmetric on RHS
+   gm1=((BaseMatrix*&)bm1)->Evaluate();
+#ifdef TEMPS_DESTROYED_QUICKLY
+   GeneralMatrix* gmx;
+   Try { gmx = GeneralMult(gm1, gm2, this, mt); }
+   CatchAll { delete this; ReThrow; }
+   delete this; return gmx;
+#else
+   return GeneralMult(gm1, gm2, this, mt);
+#endif
+}
+
+GeneralMatrix* SolvedMatrix::Evaluate(MatrixType mt)
+{
+   REPORT
+   gm1=((BaseMatrix*&)bm1)->Evaluate();
+   gm2=((BaseMatrix*&)bm2)->Evaluate();
+#ifdef TEMPS_DESTROYED_QUICKLY
+   GeneralMatrix* gmx;
+   Try { gmx = GeneralSolv(gm1,gm2,this,mt); }
+   CatchAll { delete this; ReThrow; }
+   delete this; return gmx;
+#else
+   return GeneralSolv(gm1,gm2,this,mt);
+#endif
+}
+
+GeneralMatrix* KPMatrix::Evaluate(MatrixType mt)
+{
+   REPORT
+   gm1=((BaseMatrix*&)bm1)->Evaluate();
+   gm2=((BaseMatrix*&)bm2)->Evaluate();
+#ifdef TEMPS_DESTROYED_QUICKLY
+   GeneralMatrix* gmx;
+   Try { gmx = GeneralKP(gm1,gm2,this,mt); }
+   CatchAll { delete this; ReThrow; }
+   delete this; return gmx;
+#else
+   return GeneralKP(gm1,gm2,this,mt);
+#endif
+}
+
+// routines for adding or subtracting matrices of identical storage structure
+
+static void Add(GeneralMatrix* gm, GeneralMatrix* gm1, GeneralMatrix* gm2)
+{
+   REPORT
+   Real* s1=gm1->Store(); Real* s2=gm2->Store();
+   Real* s=gm->Store(); int i=gm->Storage() >> 2;
+   while (i--)
+   {
+       *s++ = *s1++ + *s2++; *s++ = *s1++ + *s2++;
+       *s++ = *s1++ + *s2++; *s++ = *s1++ + *s2++;
+   }
+   i=gm->Storage() & 3; while (i--) *s++ = *s1++ + *s2++;
+}
+
+static void Add(GeneralMatrix* gm, GeneralMatrix* gm2)
+{
+   REPORT
+   Real* s2=gm2->Store(); Real* s=gm->Store(); int i=gm->Storage() >> 2;
+   while (i--)
+   { *s++ += *s2++; *s++ += *s2++; *s++ += *s2++; *s++ += *s2++; }
+   i=gm->Storage() & 3; while (i--) *s++ += *s2++;
+}
+
+static void Subtract(GeneralMatrix* gm, GeneralMatrix* gm1, GeneralMatrix* gm2)
+{
+   REPORT
+   Real* s1=gm1->Store(); Real* s2=gm2->Store();
+   Real* s=gm->Store(); int i=gm->Storage() >> 2;
+   while (i--)
+   {
+       *s++ = *s1++ - *s2++; *s++ = *s1++ - *s2++;
+       *s++ = *s1++ - *s2++; *s++ = *s1++ - *s2++;
+   }
+   i=gm->Storage() & 3; while (i--) *s++ = *s1++ - *s2++;
+}
+
+static void Subtract(GeneralMatrix* gm, GeneralMatrix* gm2)
+{
+   REPORT
+   Real* s2=gm2->Store(); Real* s=gm->Store(); int i=gm->Storage() >> 2;
+   while (i--)
+   { *s++ -= *s2++; *s++ -= *s2++; *s++ -= *s2++; *s++ -= *s2++; }
+   i=gm->Storage() & 3; while (i--) *s++ -= *s2++;
+}
+
+static void ReverseSubtract(GeneralMatrix* gm, GeneralMatrix* gm2)
+{
+   REPORT
+   Real* s2=gm2->Store(); Real* s=gm->Store(); int i=gm->Storage() >> 2;
+   while (i--)
+   {
+      *s = *s2++ - *s; s++; *s = *s2++ - *s; s++;
+      *s = *s2++ - *s; s++; *s = *s2++ - *s; s++;
+   }
+   i=gm->Storage() & 3; while (i--) { *s = *s2++ - *s; s++; }
+}
+
+static void SP(GeneralMatrix* gm, GeneralMatrix* gm1, GeneralMatrix* gm2)
+{
+   REPORT
+   Real* s1=gm1->Store(); Real* s2=gm2->Store();
+   Real* s=gm->Store(); int i=gm->Storage() >> 2;
+   while (i--)
+   {
+       *s++ = *s1++ * *s2++; *s++ = *s1++ * *s2++;
+       *s++ = *s1++ * *s2++; *s++ = *s1++ * *s2++;
+   }
+   i=gm->Storage() & 3; while (i--) *s++ = *s1++ * *s2++;
+}
+
+static void SP(GeneralMatrix* gm, GeneralMatrix* gm2)
+{
+   REPORT
+   Real* s2=gm2->Store(); Real* s=gm->Store(); int i=gm->Storage() >> 2;
+   while (i--)
+   { *s++ *= *s2++; *s++ *= *s2++; *s++ *= *s2++; *s++ *= *s2++; }
+   i=gm->Storage() & 3; while (i--) *s++ *= *s2++;
+}
+
+// routines for adding or subtracting matrices of different storage structure
+
+static void AddDS(GeneralMatrix* gm, GeneralMatrix* gm1, GeneralMatrix* gm2)
+{
+   REPORT
+   int nr = gm->Nrows();
+   MatrixRow mr1(gm1, LoadOnEntry); MatrixRow mr2(gm2, LoadOnEntry);
+   MatrixRow mr(gm, StoreOnExit+DirectPart);
+   while (nr--) { mr.Add(mr1,mr2); mr1.Next(); mr2.Next(); mr.Next(); }
+}
+
+static void AddDS(GeneralMatrix* gm, GeneralMatrix* gm2)
+// Add into first argument
+{
+   REPORT
+   int nr = gm->Nrows();
+   MatrixRow mr(gm, StoreOnExit+LoadOnEntry+DirectPart);
+   MatrixRow mr2(gm2, LoadOnEntry);
+   while (nr--) { mr.Add(mr2); mr.Next(); mr2.Next(); }
+}
+
+static void SubtractDS
+   (GeneralMatrix* gm, GeneralMatrix* gm1, GeneralMatrix* gm2)
+{
+   REPORT
+   int nr = gm->Nrows();
+   MatrixRow mr1(gm1, LoadOnEntry); MatrixRow mr2(gm2, LoadOnEntry);
+   MatrixRow mr(gm, StoreOnExit+DirectPart);
+   while (nr--) { mr.Sub(mr1,mr2); mr1.Next(); mr2.Next(); mr.Next(); }
+}
+
+static void SubtractDS(GeneralMatrix* gm, GeneralMatrix* gm2)
+{
+   REPORT
+   int nr = gm->Nrows();
+   MatrixRow mr(gm, LoadOnEntry+StoreOnExit+DirectPart);
+   MatrixRow mr2(gm2, LoadOnEntry);
+   while (nr--) { mr.Sub(mr2); mr.Next(); mr2.Next(); }
+}
+
+static void ReverseSubtractDS(GeneralMatrix* gm, GeneralMatrix* gm2)
+{
+   REPORT
+   int nr = gm->Nrows();
+   MatrixRow mr(gm, LoadOnEntry+StoreOnExit+DirectPart);
+   MatrixRow mr2(gm2, LoadOnEntry);
+   while (nr--) { mr.RevSub(mr2); mr2.Next(); mr.Next(); }
+}
+
+static void SPDS(GeneralMatrix* gm, GeneralMatrix* gm1, GeneralMatrix* gm2)
+{
+   REPORT
+   int nr = gm->Nrows();
+   MatrixRow mr1(gm1, LoadOnEntry); MatrixRow mr2(gm2, LoadOnEntry);
+   MatrixRow mr(gm, StoreOnExit+DirectPart);
+   while (nr--) { mr.Multiply(mr1,mr2); mr1.Next(); mr2.Next(); mr.Next(); }
+}
+
+static void SPDS(GeneralMatrix* gm, GeneralMatrix* gm2)
+// SP into first argument
+{
+   REPORT
+   int nr = gm->Nrows();
+   MatrixRow mr(gm, StoreOnExit+LoadOnEntry+DirectPart);
+   MatrixRow mr2(gm2, LoadOnEntry);
+   while (nr--) { mr.Multiply(mr2); mr.Next(); mr2.Next(); }
+}
+
+static GeneralMatrix* GeneralMult1(GeneralMatrix* gm1, GeneralMatrix* gm2,
+   MultipliedMatrix* mm, MatrixType mtx)
+{
+   REPORT
+   Tracer tr("GeneralMult1");
+   int nr=gm1->Nrows(); int nc=gm2->Ncols();
+   if (gm1->Ncols() !=gm2->Nrows())
+      Throw(IncompatibleDimensionsException(*gm1, *gm2));
+   GeneralMatrix* gmx = mtx.New(nr,nc,mm);
+
+   MatrixCol mcx(gmx, StoreOnExit+DirectPart);
+   MatrixCol mc2(gm2, LoadOnEntry);
+   while (nc--)
+   {
+      MatrixRow mr1(gm1, LoadOnEntry, mcx.Skip());
+      Real* el = mcx.Data();                         // pointer to an element
+      int n = mcx.Storage();
+      while (n--) { *(el++) = DotProd(mr1,mc2); mr1.Next(); }
+      mc2.Next(); mcx.Next();
+   }
+   gmx->ReleaseAndDelete(); gm1->tDelete(); gm2->tDelete(); return gmx;
+}
+
+static GeneralMatrix* GeneralMult2(GeneralMatrix* gm1, GeneralMatrix* gm2,
+   MultipliedMatrix* mm, MatrixType mtx)
+{
+   // version that accesses by row only - not good for thin matrices
+   // or column vectors in right hand term.
+   REPORT
+   Tracer tr("GeneralMult2");
+   int nr=gm1->Nrows(); int nc=gm2->Ncols();
+   if (gm1->Ncols() !=gm2->Nrows())
+      Throw(IncompatibleDimensionsException(*gm1, *gm2));
+   GeneralMatrix* gmx = mtx.New(nr,nc,mm);
+
+   MatrixRow mrx(gmx, LoadOnEntry+StoreOnExit+DirectPart);
+   MatrixRow mr1(gm1, LoadOnEntry);
+   while (nr--)
+   {
+      MatrixRow mr2(gm2, LoadOnEntry, mr1.Skip());
+      Real* el = mr1.Data();                         // pointer to an element
+      int n = mr1.Storage();
+      mrx.Zero();
+      while (n--) { mrx.AddScaled(mr2, *el++); mr2.Next(); }
+      mr1.Next(); mrx.Next();
+   }
+   gmx->ReleaseAndDelete(); gm1->tDelete(); gm2->tDelete(); return gmx;
+}
+
+static GeneralMatrix* mmMult(GeneralMatrix* gm1, GeneralMatrix* gm2)
+{
+   // matrix multiplication for type Matrix only
+   REPORT
+   Tracer tr("MatrixMult");
+
+   int nr=gm1->Nrows(); int ncr=gm1->Ncols(); int nc=gm2->Ncols();
+   if (ncr != gm2->Nrows()) Throw(IncompatibleDimensionsException(*gm1,*gm2));
+
+   Matrix* gm = new Matrix(nr,nc); MatrixErrorNoSpace(gm);
+
+   Real* s1=gm1->Store(); Real* s2=gm2->Store(); Real* s=gm->Store();
+
+   if (ncr)
+   {
+      while (nr--)
+      {
+         Real* s2x = s2; int j = ncr;
+         Real* sx = s; Real f = *s1++; int k = nc;
+         while (k--) *sx++ = f * *s2x++;
+         while (--j)
+            { sx = s; f = *s1++; k = nc; while (k--) *sx++ += f * *s2x++; }
+         s = sx;
+      }
+   }
+   else *gm = 0.0;
+
+   gm->ReleaseAndDelete(); gm1->tDelete(); gm2->tDelete(); return gm;
+}
+
+static GeneralMatrix* GeneralMult(GeneralMatrix* gm1, GeneralMatrix* gm2,
+   MultipliedMatrix* mm, MatrixType mtx)
+{
+   if ( Rectangular(gm1->Type(), gm2->Type(), mtx))
+   {
+      REPORT
+      return mmMult(gm1, gm2);
+   }
+   else
+   {
+      REPORT
+      Compare(gm1->Type() * gm2->Type(),mtx);
+      int nr = gm2->Nrows(); int nc = gm2->Ncols();
+      if (nc <= 5 && nr > nc) { REPORT return GeneralMult1(gm1, gm2, mm, mtx); }
+      else { REPORT return GeneralMult2(gm1, gm2, mm, mtx); }
+   }
+}
+
+static GeneralMatrix* GeneralKP(GeneralMatrix* gm1, GeneralMatrix* gm2,
+   KPMatrix* kp, MatrixType mtx)
+{
+   REPORT
+   Tracer tr("GeneralKP");
+   int nr1 = gm1->Nrows(); int nc1 = gm1->Ncols();
+   int nr2 = gm2->Nrows(); int nc2 = gm2->Ncols();
+   Compare((gm1->Type()).KP(gm2->Type()),mtx);
+   GeneralMatrix* gmx = mtx.New(nr1*nr2, nc1*nc2, kp);
+   MatrixRow mrx(gmx, LoadOnEntry+StoreOnExit+DirectPart);
+   MatrixRow mr1(gm1, LoadOnEntry);
+   for (int i = 1; i <= nr1; ++i)
+   {
+      MatrixRow mr2(gm2, LoadOnEntry);
+      for (int j = 1; j <= nr2; ++j)
+         { mrx.KP(mr1,mr2); mr2.Next(); mrx.Next(); }
+      mr1.Next();
+   }
+   gmx->ReleaseAndDelete(); gm1->tDelete(); gm2->tDelete(); return gmx;
+}
+
+static GeneralMatrix* GeneralSolv(GeneralMatrix* gm1, GeneralMatrix* gm2,
+   BaseMatrix* sm, MatrixType mtx)
+{
+   REPORT
+   Tracer tr("GeneralSolv");
+   Compare(gm1->Type().i() * gm2->Type(),mtx);
+   int nr = gm1->Nrows();
+   if (nr != gm1->Ncols()) Throw(NotSquareException(*gm1));
+   int nc = gm2->Ncols();
+   if (gm1->Ncols() != gm2->Nrows())
+      Throw(IncompatibleDimensionsException(*gm1, *gm2));
+   GeneralMatrix* gmx = mtx.New(nr,nc,sm); MatrixErrorNoSpace(gmx);
+   Real* r = new Real [nr]; MatrixErrorNoSpace(r);
+   MONITOR_REAL_NEW("Make   (GenSolv)",nr,r)
+   GeneralMatrix* gms = gm1->MakeSolver();
+   Try
+   {
+
+      MatrixColX mcx(gmx, r, StoreOnExit+DirectPart);   // copy to and from r
+         // this must be inside Try so mcx is destroyed before gmx
+      MatrixColX mc2(gm2, r, LoadOnEntry);
+      while (nc--) { gms->Solver(mcx, mc2); mcx.Next(); mc2.Next(); }
+   }
+   CatchAll
+   {
+      if (gms) gms->tDelete();
+      delete gmx;                   // <--------------------
+      gm2->tDelete();
+      MONITOR_REAL_DELETE("Delete (GenSolv)",nr,r)
+                          // ATandT version 2.1 gives an internal error
+      delete [] r;
+      ReThrow;
+   }
+   gms->tDelete(); gmx->ReleaseAndDelete(); gm2->tDelete();
+   MONITOR_REAL_DELETE("Delete (GenSolv)",nr,r)
+                          // ATandT version 2.1 gives an internal error
+   delete [] r;
+   return gmx;
+}
+
+// version for inverses - gm2 is identity
+static GeneralMatrix* GeneralSolvI(GeneralMatrix* gm1, BaseMatrix* sm,
+   MatrixType mtx)
+{
+   REPORT
+   Tracer tr("GeneralSolvI");
+   Compare(gm1->Type().i(),mtx);
+   int nr = gm1->Nrows();
+   if (nr != gm1->Ncols()) Throw(NotSquareException(*gm1));
+   int nc = nr;
+   // DiagonalMatrix I(nr); I = 1;
+   IdentityMatrix I(nr);
+   GeneralMatrix* gmx = mtx.New(nr,nc,sm); MatrixErrorNoSpace(gmx);
+   Real* r = new Real [nr]; MatrixErrorNoSpace(r);
+   MONITOR_REAL_NEW("Make   (GenSolvI)",nr,r)
+   GeneralMatrix* gms = gm1->MakeSolver();
+   Try
+   {
+
+      MatrixColX mcx(gmx, r, StoreOnExit+DirectPart);   // copy to and from r
+         // this must be inside Try so mcx is destroyed before gmx
+      MatrixColX mc2(&I, r, LoadOnEntry);
+      while (nc--) { gms->Solver(mcx, mc2); mcx.Next(); mc2.Next(); }
+   }
+   CatchAll
+   {
+      if (gms) gms->tDelete();
+      delete gmx;
+      MONITOR_REAL_DELETE("Delete (GenSolvI)",nr,r)
+                          // ATandT version 2.1 gives an internal error
+      delete [] r;
+      ReThrow;
+   }
+   gms->tDelete(); gmx->ReleaseAndDelete();
+   MONITOR_REAL_DELETE("Delete (GenSolvI)",nr,r)
+                          // ATandT version 2.1 gives an internal error
+   delete [] r;
+   return gmx;
+}
+
+GeneralMatrix* InvertedMatrix::Evaluate(MatrixType mtx)
+{
+   // Matrix Inversion - use solve routines
+   Tracer tr("InvertedMatrix::Evaluate");
+   REPORT
+   gm=((BaseMatrix*&)bm)->Evaluate();
+#ifdef TEMPS_DESTROYED_QUICKLY
+	GeneralMatrix* gmx;
+	Try { gmx = GeneralSolvI(gm,this,mtx); }
+   CatchAll { delete this; ReThrow; }
+   delete this; return gmx;
+#else
+   return GeneralSolvI(gm,this,mtx);
+#endif
+}
+
+//*************************** New versions ************************
+
+GeneralMatrix* AddedMatrix::Evaluate(MatrixType mtd)
+{
+   REPORT
+   Tracer tr("AddedMatrix::Evaluate");
+   gm1=((BaseMatrix*&)bm1)->Evaluate(); gm2=((BaseMatrix*&)bm2)->Evaluate();
+   int nr=gm1->Nrows(); int nc=gm1->Ncols();
+   if (nr!=gm2->Nrows() || nc!=gm2->Ncols())
+   {
+      Try { Throw(IncompatibleDimensionsException(*gm1, *gm2)); }
+      CatchAll
+      {
+         gm1->tDelete(); gm2->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+         delete this;
+#endif
+         ReThrow;
+      }
+   }
+   MatrixType mt1 = gm1->Type(), mt2 = gm2->Type(); MatrixType mts = mt1 + mt2;
+   if (!mtd) { REPORT mtd = mts; }
+   else if (!(mtd.DataLossOK || mtd >= mts))
+   {
+      REPORT
+      gm1->tDelete(); gm2->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+      Throw(ProgramException("Illegal Conversion", mts, mtd));
+   }
+   GeneralMatrix* gmx;
+   bool c1 = (mtd == mt1), c2 = (mtd == mt2);
+   if ( c1 && c2 && (gm1->SimpleAddOK(gm2) == 0) )
+   {
+      if (gm1->reuse()) { REPORT Add(gm1,gm2); gm2->tDelete(); gmx = gm1; }
+      else if (gm2->reuse()) { REPORT Add(gm2,gm1); gmx = gm2; }
+      else
+      {
+         REPORT
+         // what if new throws an exception
+         Try { gmx = mt1.New(nr,nc,this); }
+         CatchAll
+         {
+#ifdef TEMPS_DESTROYED_QUICKLY
+            delete this;
+#endif
+            ReThrow;
+         }
+         gmx->ReleaseAndDelete(); Add(gmx,gm1,gm2);
+      }
+   }
+   else
+   {
+      if (c1 && c2)
+      {
+         short SAO = gm1->SimpleAddOK(gm2);
+         if (SAO & 1) { REPORT c1 = false; }
+         if (SAO & 2) { REPORT c2 = false; }
+      }
+      if (c1 && gm1->reuse() )               // must have type test first
+         { REPORT AddDS(gm1,gm2); gm2->tDelete(); gmx = gm1; }
+      else if (c2 && gm2->reuse() )
+         { REPORT AddDS(gm2,gm1); if (!c1) gm1->tDelete(); gmx = gm2; }
+      else
+      {
+         REPORT
+         Try { gmx = mtd.New(nr,nc,this); }
+         CatchAll
+         {
+            if (!c1) gm1->tDelete(); if (!c2) gm2->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+            delete this;
+#endif
+            ReThrow;
+         }
+         AddDS(gmx,gm1,gm2);
+         if (!c1) gm1->tDelete(); if (!c2) gm2->tDelete();
+         gmx->ReleaseAndDelete();
+      }
+   }
+#ifdef TEMPS_DESTROYED_QUICKLY
+   delete this;
+#endif
+   return gmx;
+}
+
+GeneralMatrix* SubtractedMatrix::Evaluate(MatrixType mtd)
+{
+   REPORT
+   Tracer tr("SubtractedMatrix::Evaluate");
+   gm1=((BaseMatrix*&)bm1)->Evaluate(); gm2=((BaseMatrix*&)bm2)->Evaluate();
+   int nr=gm1->Nrows(); int nc=gm1->Ncols();
+   if (nr!=gm2->Nrows() || nc!=gm2->Ncols())
+   {
+      Try { Throw(IncompatibleDimensionsException(*gm1, *gm2)); }
+      CatchAll
+      {
+         gm1->tDelete(); gm2->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+         delete this;
+#endif
+         ReThrow;
+      }
+   }
+   MatrixType mt1 = gm1->Type(), mt2 = gm2->Type(); MatrixType mts = mt1 + mt2;
+   if (!mtd) { REPORT mtd = mts; }
+   else if (!(mtd.DataLossOK || mtd >= mts))
+   {
+      gm1->tDelete(); gm2->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+      Throw(ProgramException("Illegal Conversion", mts, mtd));
+   }
+   GeneralMatrix* gmx;
+   bool c1 = (mtd == mt1), c2 = (mtd == mt2);
+   if ( c1 && c2 && (gm1->SimpleAddOK(gm2) == 0) )
+   {
+      if (gm1->reuse()) { REPORT Subtract(gm1,gm2); gm2->tDelete(); gmx = gm1; }
+      else if (gm2->reuse()) { REPORT ReverseSubtract(gm2,gm1); gmx = gm2; }
+      else
+      {
+         REPORT
+         Try { gmx = mt1.New(nr,nc,this); }
+         CatchAll
+         {
+#ifdef TEMPS_DESTROYED_QUICKLY
+            delete this;
+#endif
+            ReThrow;
+         }
+         gmx->ReleaseAndDelete(); Subtract(gmx,gm1,gm2);
+      }
+   }
+   else
+   {
+      if (c1 && c2)
+      {
+         short SAO = gm1->SimpleAddOK(gm2);
+         if (SAO & 1) { REPORT c1 = false; }
+         if (SAO & 2) { REPORT c2 = false; }
+      }
+      if (c1 && gm1->reuse() )               // must have type test first
+         { REPORT SubtractDS(gm1,gm2); gm2->tDelete(); gmx = gm1; }
+      else if (c2 && gm2->reuse() )
+      {
+         REPORT ReverseSubtractDS(gm2,gm1);
+         if (!c1) gm1->tDelete(); gmx = gm2;
+      }
+      else
+      {
+         REPORT
+         // what if New throws and exception
+         Try { gmx = mtd.New(nr,nc,this); }
+         CatchAll
+         {
+            if (!c1) gm1->tDelete(); if (!c2) gm2->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+            delete this;
+#endif
+            ReThrow;
+         }
+         SubtractDS(gmx,gm1,gm2);
+         if (!c1) gm1->tDelete(); if (!c2) gm2->tDelete();
+         gmx->ReleaseAndDelete();
+      }
+   }
+#ifdef TEMPS_DESTROYED_QUICKLY
+   delete this;
+#endif
+   return gmx;
+}
+
+GeneralMatrix* SPMatrix::Evaluate(MatrixType mtd)
+{
+   REPORT
+   Tracer tr("SPMatrix::Evaluate");
+   gm1=((BaseMatrix*&)bm1)->Evaluate(); gm2=((BaseMatrix*&)bm2)->Evaluate();
+   int nr=gm1->Nrows(); int nc=gm1->Ncols();
+   if (nr!=gm2->Nrows() || nc!=gm2->Ncols())
+   {
+      Try { Throw(IncompatibleDimensionsException(*gm1, *gm2)); }
+      CatchAll
+      {
+         gm1->tDelete(); gm2->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+         delete this;
+#endif
+         ReThrow;
+      }
+   }
+   MatrixType mt1 = gm1->Type(), mt2 = gm2->Type();
+   MatrixType mts = mt1.SP(mt2);
+   if (!mtd) { REPORT mtd = mts; }
+   else if (!(mtd.DataLossOK || mtd >= mts))
+   {
+      gm1->tDelete(); gm2->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+      Throw(ProgramException("Illegal Conversion", mts, mtd));
+   }
+   GeneralMatrix* gmx;
+   bool c1 = (mtd == mt1), c2 = (mtd == mt2);
+   if ( c1 && c2 && (gm1->SimpleAddOK(gm2) == 0) )
+   {
+      if (gm1->reuse()) { REPORT SP(gm1,gm2); gm2->tDelete(); gmx = gm1; }
+      else if (gm2->reuse()) { REPORT SP(gm2,gm1); gmx = gm2; }
+      else
+      {
+         REPORT
+         Try { gmx = mt1.New(nr,nc,this); }
+         CatchAll
+         {
+#ifdef TEMPS_DESTROYED_QUICKLY
+            delete this;
+#endif
+            ReThrow;
+         }
+         gmx->ReleaseAndDelete(); SP(gmx,gm1,gm2);
+      }
+   }
+   else
+   {
+      if (c1 && c2)
+      {
+         short SAO = gm1->SimpleAddOK(gm2);
+         if (SAO & 1) { REPORT c2 = false; }    // c1 and c2 swapped
+         if (SAO & 2) { REPORT c1 = false; }
+      }
+      if (c1 && gm1->reuse() )               // must have type test first
+         { REPORT SPDS(gm1,gm2); gm2->tDelete(); gmx = gm1; }
+      else if (c2 && gm2->reuse() )
+         { REPORT SPDS(gm2,gm1); if (!c1) gm1->tDelete(); gmx = gm2; }
+      else
+      {
+         REPORT
+         // what if New throws and exception
+         Try { gmx = mtd.New(nr,nc,this); }
+         CatchAll
+         {
+            if (!c1) gm1->tDelete(); if (!c2) gm2->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+            delete this;
+#endif
+            ReThrow;
+         }
+         SPDS(gmx,gm1,gm2);
+         if (!c1) gm1->tDelete(); if (!c2) gm2->tDelete();
+         gmx->ReleaseAndDelete();
+      }
+   }
+#ifdef TEMPS_DESTROYED_QUICKLY
+   delete this;
+#endif
+   return gmx;
+}
+
+
+
+//*************************** norm functions ****************************/
+
+Real BaseMatrix::Norm1() const
+{
+   // maximum of sum of absolute values of a column
+   REPORT
+   GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
+   int nc = gm->Ncols(); Real value = 0.0;
+   MatrixCol mc(gm, LoadOnEntry);
+   while (nc--)
+      { Real v = mc.SumAbsoluteValue(); if (value < v) value = v; mc.Next(); }
+   gm->tDelete(); return value;
+}
+
+Real BaseMatrix::NormInfinity() const
+{
+   // maximum of sum of absolute values of a row
+   REPORT
+   GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
+   int nr = gm->Nrows(); Real value = 0.0;
+   MatrixRow mr(gm, LoadOnEntry);
+   while (nr--)
+      { Real v = mr.SumAbsoluteValue(); if (value < v) value = v; mr.Next(); }
+   gm->tDelete(); return value;
+}
+
+//********************** Concatenation and stacking *************************/
+
+GeneralMatrix* ConcatenatedMatrix::Evaluate(MatrixType mtx)
+{
+   REPORT
+   Tracer tr("Concatenate");
+#ifdef TEMPS_DESTROYED_QUICKLY
+      Try
+      {
+         gm2 = ((BaseMatrix*&)bm2)->Evaluate();
+         gm1 = ((BaseMatrix*&)bm1)->Evaluate();
+         Compare(gm1->Type() | gm2->Type(),mtx);
+         int nr=gm1->Nrows(); int nc = gm1->Ncols() + gm2->Ncols();
+         if (nr != gm2->Nrows())
+            Throw(IncompatibleDimensionsException(*gm1, *gm2));
+         GeneralMatrix* gmx = mtx.New(nr,nc,this);
+         MatrixRow mr1(gm1, LoadOnEntry); MatrixRow mr2(gm2, LoadOnEntry);
+         MatrixRow mr(gmx, StoreOnExit+DirectPart);
+         while (nr--) { mr.ConCat(mr1,mr2); mr1.Next(); mr2.Next(); mr.Next(); }
+         gmx->ReleaseAndDelete(); gm1->tDelete(); gm2->tDelete(); delete this;
+         return gmx;
+      }
+      CatchAll  { delete this; ReThrow; }
+#ifndef UseExceptions
+      return 0;
+#endif
+#else
+      gm2 = ((BaseMatrix*&)bm2)->Evaluate();
+      gm1 = ((BaseMatrix*&)bm1)->Evaluate();
+      Compare(gm1->Type() | gm2->Type(),mtx);
+      int nr=gm1->Nrows(); int nc = gm1->Ncols() + gm2->Ncols();
+      if (nr != gm2->Nrows())
+         Throw(IncompatibleDimensionsException(*gm1, *gm2));
+      GeneralMatrix* gmx = mtx.New(nr,nc,this);
+      MatrixRow mr1(gm1, LoadOnEntry); MatrixRow mr2(gm2, LoadOnEntry);
+      MatrixRow mr(gmx, StoreOnExit+DirectPart);
+      while (nr--) { mr.ConCat(mr1,mr2); mr1.Next(); mr2.Next(); mr.Next(); }
+      gmx->ReleaseAndDelete(); gm1->tDelete(); gm2->tDelete(); return gmx;
+#endif
+}
+
+GeneralMatrix* StackedMatrix::Evaluate(MatrixType mtx)
+{
+   REPORT
+   Tracer tr("Stack");
+#ifdef TEMPS_DESTROYED_QUICKLY
+      Try
+      {
+         gm2 = ((BaseMatrix*&)bm2)->Evaluate();
+         gm1 = ((BaseMatrix*&)bm1)->Evaluate();
+         Compare(gm1->Type() & gm2->Type(),mtx);
+         int nc=gm1->Ncols();
+         int nr1 = gm1->Nrows(); int nr2 = gm2->Nrows();
+         if (nc != gm2->Ncols())
+            Throw(IncompatibleDimensionsException(*gm1, *gm2));
+         GeneralMatrix* gmx = mtx.New(nr1+nr2,nc,this);
+         MatrixRow mr1(gm1, LoadOnEntry); MatrixRow mr2(gm2, LoadOnEntry);
+         MatrixRow mr(gmx, StoreOnExit+DirectPart);
+         while (nr1--) { mr.Copy(mr1); mr1.Next(); mr.Next(); }
+         while (nr2--) { mr.Copy(mr2); mr2.Next(); mr.Next(); }
+         gmx->ReleaseAndDelete(); gm1->tDelete(); gm2->tDelete(); delete this;
+         return gmx;
+      }
+      CatchAll  { delete this; ReThrow; }
+#ifndef UseExceptions
+      return 0;
+#endif
+#else
+      gm2 = ((BaseMatrix*&)bm2)->Evaluate();
+      gm1 = ((BaseMatrix*&)bm1)->Evaluate();
+      Compare(gm1->Type() & gm2->Type(),mtx);
+      int nc=gm1->Ncols();
+      int nr1 = gm1->Nrows(); int nr2 = gm2->Nrows();
+      if (nc != gm2->Ncols())
+         Throw(IncompatibleDimensionsException(*gm1, *gm2));
+      GeneralMatrix* gmx = mtx.New(nr1+nr2,nc,this);
+      MatrixRow mr1(gm1, LoadOnEntry); MatrixRow mr2(gm2, LoadOnEntry);
+      MatrixRow mr(gmx, StoreOnExit+DirectPart);
+      while (nr1--) { mr.Copy(mr1); mr1.Next(); mr.Next(); }
+      while (nr2--) { mr.Copy(mr2); mr2.Next(); mr.Next(); }
+      gmx->ReleaseAndDelete(); gm1->tDelete(); gm2->tDelete(); return gmx;
+#endif
+}
+
+// ************************* equality of matrices ******************** //
+
+static bool RealEqual(Real* s1, Real* s2, int n)
+{
+   int i = n >> 2;
+   while (i--)
+   {
+      if (*s1++ != *s2++) return false; if (*s1++ != *s2++) return false;
+      if (*s1++ != *s2++) return false; if (*s1++ != *s2++) return false;
+   }
+   i = n & 3; while (i--) if (*s1++ != *s2++) return false;
+   return true;
+}
+
+static bool intEqual(int* s1, int* s2, int n)
+{
+   int i = n >> 2;
+   while (i--)
+   {
+      if (*s1++ != *s2++) return false; if (*s1++ != *s2++) return false;
+      if (*s1++ != *s2++) return false; if (*s1++ != *s2++) return false;
+   }
+   i = n & 3; while (i--) if (*s1++ != *s2++) return false;
+   return true;
+}
+
+
+bool operator==(const BaseMatrix& A, const BaseMatrix& B)
+{
+   Tracer tr("BaseMatrix ==");
+   REPORT
+   GeneralMatrix* gmA = ((BaseMatrix&)A).Evaluate();
+   GeneralMatrix* gmB = ((BaseMatrix&)B).Evaluate();
+
+   if (gmA == gmB)                            // same matrix
+      { REPORT gmA->tDelete(); return true; }
+
+   if ( gmA->Nrows() != gmB->Nrows() || gmA->Ncols() != gmB->Ncols() )
+                                              // different dimensions
+      { REPORT gmA->tDelete(); gmB->tDelete(); return false; }
+
+   // check for CroutMatrix or BandLUMatrix
+   MatrixType AType = gmA->Type(); MatrixType BType = gmB->Type();
+   if (AType.CannotConvert() || BType.CannotConvert() )
+   {
+      REPORT
+      bool bx = gmA->IsEqual(*gmB);
+      gmA->tDelete(); gmB->tDelete();
+      return bx;
+   }
+
+   // is matrix storage the same
+   // will need to modify if further matrix structures are introduced
+   if (AType == BType && gmA->BandWidth() == gmB->BandWidth())
+   {                                          // compare store
+      REPORT
+      bool bx = RealEqual(gmA->Store(),gmB->Store(),gmA->Storage());
+      gmA->tDelete(); gmB->tDelete();
+      return bx;
+   }
+
+   // matrix storage different - just subtract
+   REPORT  return IsZero(*gmA-*gmB);
+}
+
+bool operator==(const GeneralMatrix& A, const GeneralMatrix& B)
+{
+   Tracer tr("GeneralMatrix ==");
+   // May or may not call tDeletes
+   REPORT
+
+   if (&A == &B)                              // same matrix
+      { REPORT return true; }
+
+   if ( A.Nrows() != B.Nrows() || A.Ncols() != B.Ncols() )
+      { REPORT return false; }                // different dimensions
+
+   // check for CroutMatrix or BandLUMatrix
+   MatrixType AType = A.Type(); MatrixType BType = B.Type();
+   if (AType.CannotConvert() || BType.CannotConvert() )
+      { REPORT  return A.IsEqual(B); }
+
+   // is matrix storage the same
+   // will need to modify if further matrix structures are introduced
+   if (AType == BType && A.BandWidth() == B.BandWidth())
+      { REPORT return RealEqual(A.Store(),B.Store(),A.Storage()); }
+
+   // matrix storage different - just subtract
+   REPORT  return IsZero(A-B);
+}
+
+bool GeneralMatrix::IsZero() const
+{
+   REPORT
+   Real* s=store; int i = storage >> 2;
+   while (i--)
+   {
+      if (*s++) return false; if (*s++) return false;
+      if (*s++) return false; if (*s++) return false;
+   }
+   i = storage & 3; while (i--) if (*s++) return false;
+   return true;
+}
+
+bool IsZero(const BaseMatrix& A)
+{
+   Tracer tr("BaseMatrix::IsZero");
+   REPORT
+   GeneralMatrix* gm1 = 0; bool bx;
+   Try { gm1=((BaseMatrix&)A).Evaluate(); bx = gm1->IsZero(); }
+   CatchAll { if (gm1) gm1->tDelete(); ReThrow; }
+   gm1->tDelete();
+   return bx;
+}
+
+// IsEqual functions - insist matrices are of same type
+// as well as equal values to be equal
+
+bool GeneralMatrix::IsEqual(const GeneralMatrix& A) const
+{
+   Tracer tr("GeneralMatrix IsEqual");
+   if (A.Type() != Type())                       // not same types
+      { REPORT return false; }
+   if (&A == this)                               // same matrix
+      { REPORT  return true; }
+   if (A.nrows != nrows || A.ncols != ncols)
+                                                 // different dimensions
+   { REPORT return false; }
+   // is matrix storage the same - compare store
+   REPORT
+   return RealEqual(A.store,store,storage);
+}
+
+bool CroutMatrix::IsEqual(const GeneralMatrix& A) const
+{
+   Tracer tr("CroutMatrix IsEqual");
+   if (A.Type() != Type())                       // not same types
+      { REPORT return false; }
+   if (&A == this)                               // same matrix
+      { REPORT  return true; }
+   if (A.nrows != nrows || A.ncols != ncols)
+                                                 // different dimensions
+   { REPORT return false; }
+   // is matrix storage the same - compare store
+   REPORT
+   return RealEqual(A.store,store,storage)
+      && intEqual(((CroutMatrix&)A).indx, indx, nrows);
+}
+
+
+bool BandLUMatrix::IsEqual(const GeneralMatrix& A) const
+{
+   Tracer tr("BandLUMatrix IsEqual");
+   if (A.Type() != Type())                       // not same types
+      { REPORT  return false; }
+   if (&A == this)                               // same matrix
+      { REPORT  return true; }
+   if ( A.Nrows() != nrows || A.Ncols() != ncols
+      || ((BandLUMatrix&)A).m1 != m1 || ((BandLUMatrix&)A).m2 != m2 )
+                                                 // different dimensions
+   { REPORT  return false; }
+
+   // matrix storage the same - compare store
+   REPORT
+   return RealEqual(A.Store(),store,storage)
+      && RealEqual(((BandLUMatrix&)A).store2,store2,storage2)
+      && intEqual(((BandLUMatrix&)A).indx, indx, nrows);
+}
+
+
+#ifdef use_namespace
+}
+#endif
+
+
Index: orxonox/trunk/src/util/newmat/newmat8.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/newmat8.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/newmat8.cpp	(revision 4565)
@@ -0,0 +1,734 @@
+//$$ newmat8.cpp         Advanced LU transform, scalar functions
+
+// Copyright (C) 1991,2,3,4,8: R B Davies
+
+#define WANT_MATH
+
+#include "include.h"
+
+#include "newmat.h"
+#include "newmatrc.h"
+#include "precisio.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+
+#ifdef DO_REPORT
+#define REPORT { static ExeCounter ExeCount(__LINE__,8); ++ExeCount; }
+#else
+#define REPORT {}
+#endif
+
+
+/************************** LU transformation ****************************/
+
+void CroutMatrix::ludcmp()
+// LU decomposition from Golub & Van Loan, algorithm 3.4.1, (the "outer
+// product" version).
+// This replaces the code derived from Numerical Recipes in C in previous
+// versions of newmat and being row oriented runs much faster with large
+// matrices.
+{
+   REPORT
+   Tracer trace( "Crout(ludcmp)" ); sing = false;
+   Real* akk = store;                    // runs down diagonal
+
+   Real big = fabs(*akk); int mu = 0; Real* ai = akk; int k;
+
+   for (k = 1; k < nrows; k++)
+   {
+      ai += nrows; const Real trybig = fabs(*ai);
+      if (big < trybig) { big = trybig; mu = k; }
+   }
+
+
+   if (nrows) for (k = 0;;)
+   {
+      /*
+      int mu1;
+      {
+         Real big = fabs(*akk); mu1 = k; Real* ai = akk; int i;
+
+         for (i = k+1; i < nrows; i++)
+         {
+            ai += nrows; const Real trybig = fabs(*ai);
+            if (big < trybig) { big = trybig; mu1 = i; }
+         }
+      }
+      if (mu1 != mu) cout << k << " " << mu << " " << mu1 << endl;
+      */
+
+      indx[k] = mu;
+
+      if (mu != k)                       //row swap
+      {
+         Real* a1 = store + nrows * k; Real* a2 = store + nrows * mu; d = !d;
+         int j = nrows;
+         while (j--) { const Real temp = *a1; *a1++ = *a2; *a2++ = temp; }
+      }
+
+      Real diag = *akk; big = 0; mu = k + 1;
+      if (diag != 0)
+      {
+         ai = akk; int i = nrows - k - 1;
+         while (i--)
+         {
+            ai += nrows; Real* al = ai; Real mult = *al / diag; *al = mult;
+            int l = nrows - k - 1; Real* aj = akk;
+            // work out the next pivot as part of this loop
+            // this saves a column operation
+            if (l-- != 0)
+            {
+               *(++al) -= (mult * *(++aj));
+               const Real trybig = fabs(*al);
+               if (big < trybig) { big = trybig; mu = nrows - i - 1; }
+               while (l--) *(++al) -= (mult * *(++aj));
+            }
+         }
+      }
+      else sing = true;
+      if (++k == nrows) break;          // so next line won't overflow
+      akk += nrows + 1;
+   }
+}
+
+void CroutMatrix::lubksb(Real* B, int mini)
+{
+   REPORT
+   // this has been adapted from Numerical Recipes in C. The code has been
+   // substantially streamlined, so I do not think much of the original
+   // copyright remains. However there is not much opportunity for
+   // variation in the code, so it is still similar to the NR code.
+   // I follow the NR code in skipping over initial zeros in the B vector.
+
+   Tracer trace("Crout(lubksb)");
+   if (sing) Throw(SingularException(*this));
+   int i, j, ii = nrows;            // ii initialised : B might be all zeros
+
+
+   // scan for first non-zero in B
+   for (i = 0; i < nrows; i++)
+   {
+      int ip = indx[i]; Real temp = B[ip]; B[ip] = B[i]; B[i] = temp;
+      if (temp != 0.0) { ii = i; break; }
+   }
+
+   Real* bi; Real* ai;
+   i = ii + 1;
+
+   if (i < nrows)
+   {
+      bi = B + ii; ai = store + ii + i * nrows;
+      for (;;)
+      {
+         int ip = indx[i]; Real sum = B[ip]; B[ip] = B[i];
+         Real* aij = ai; Real* bj = bi; j = i - ii;
+         while (j--) sum -= *aij++ * *bj++;
+         B[i] = sum;
+         if (++i == nrows) break;
+         ai += nrows;
+      }
+   }
+
+   ai = store + nrows * nrows;
+
+   for (i = nrows - 1; i >= mini; i--)
+   {
+      Real* bj = B+i; ai -= nrows; Real* ajx = ai+i;
+      Real sum = *bj; Real diag = *ajx;
+      j = nrows - i; while(--j) sum -= *(++ajx) * *(++bj);
+      B[i] = sum / diag;
+   }
+}
+
+/****************************** scalar functions ****************************/
+
+inline Real square(Real x) { return x*x; }
+
+Real GeneralMatrix::SumSquare() const
+{
+   REPORT
+   Real sum = 0.0; int i = storage; Real* s = store;
+   while (i--) sum += square(*s++);
+   ((GeneralMatrix&)*this).tDelete(); return sum;
+}
+
+Real GeneralMatrix::SumAbsoluteValue() const
+{
+   REPORT
+   Real sum = 0.0; int i = storage; Real* s = store;
+   while (i--) sum += fabs(*s++);
+   ((GeneralMatrix&)*this).tDelete(); return sum;
+}
+
+Real GeneralMatrix::Sum() const
+{
+   REPORT
+   Real sum = 0.0; int i = storage; Real* s = store;
+   while (i--) sum += *s++;
+   ((GeneralMatrix&)*this).tDelete(); return sum;
+}
+
+// maxima and minima
+
+// There are three sets of routines
+// MaximumAbsoluteValue, MinimumAbsoluteValue, Maximum, Minimum
+// ... these find just the maxima and minima
+// MaximumAbsoluteValue1, MinimumAbsoluteValue1, Maximum1, Minimum1
+// ... these find the maxima and minima and their locations in a
+//     one dimensional object
+// MaximumAbsoluteValue2, MinimumAbsoluteValue2, Maximum2, Minimum2
+// ... these find the maxima and minima and their locations in a
+//     two dimensional object
+
+// If the matrix has no values throw an exception
+
+// If we do not want the location find the maximum or minimum on the
+// array stored by GeneralMatrix
+// This won't work for BandMatrices. We call ClearCorner for
+// MaximumAbsoluteValue but for the others use the AbsoluteMinimumValue2
+// version and discard the location.
+
+// For one dimensional objects, when we want the location of the
+// maximum or minimum, work with the array stored by GeneralMatrix
+
+// For two dimensional objects where we want the location of the maximum or
+// minimum proceed as follows:
+
+// For rectangular matrices use the array stored by GeneralMatrix and
+// deduce the location from the location in the GeneralMatrix
+
+// For other two dimensional matrices use the Matrix Row routine to find the
+// maximum or minimum for each row.
+
+static void NullMatrixError(const GeneralMatrix* gm)
+{
+   ((GeneralMatrix&)*gm).tDelete();
+   Throw(ProgramException("Maximum or minimum of null matrix"));
+}
+
+Real GeneralMatrix::MaximumAbsoluteValue() const
+{
+   REPORT
+   if (storage == 0) NullMatrixError(this);
+   Real maxval = 0.0; int l = storage; Real* s = store;
+   while (l--) { Real a = fabs(*s++); if (maxval < a) maxval = a; }
+   ((GeneralMatrix&)*this).tDelete(); return maxval;
+}
+
+Real GeneralMatrix::MaximumAbsoluteValue1(int& i) const
+{
+   REPORT
+   if (storage == 0) NullMatrixError(this);
+   Real maxval = 0.0; int l = storage; Real* s = store; int li = storage;
+   while (l--)
+      { Real a = fabs(*s++); if (maxval <= a) { maxval = a; li = l; }  }
+   i = storage - li;
+   ((GeneralMatrix&)*this).tDelete(); return maxval;
+}
+
+Real GeneralMatrix::MinimumAbsoluteValue() const
+{
+   REPORT
+   if (storage == 0) NullMatrixError(this);
+   int l = storage - 1; Real* s = store; Real minval = fabs(*s++);
+   while (l--) { Real a = fabs(*s++); if (minval > a) minval = a; }
+   ((GeneralMatrix&)*this).tDelete(); return minval;
+}
+
+Real GeneralMatrix::MinimumAbsoluteValue1(int& i) const
+{
+   REPORT
+   if (storage == 0) NullMatrixError(this);
+   int l = storage - 1; Real* s = store; Real minval = fabs(*s++); int li = l;
+   while (l--)
+      { Real a = fabs(*s++); if (minval >= a) { minval = a; li = l; }  }
+   i = storage - li;
+   ((GeneralMatrix&)*this).tDelete(); return minval;
+}
+
+Real GeneralMatrix::Maximum() const
+{
+   REPORT
+   if (storage == 0) NullMatrixError(this);
+   int l = storage - 1; Real* s = store; Real maxval = *s++;
+   while (l--) { Real a = *s++; if (maxval < a) maxval = a; }
+   ((GeneralMatrix&)*this).tDelete(); return maxval;
+}
+
+Real GeneralMatrix::Maximum1(int& i) const
+{
+   REPORT
+   if (storage == 0) NullMatrixError(this);
+   int l = storage - 1; Real* s = store; Real maxval = *s++; int li = l;
+   while (l--) { Real a = *s++; if (maxval <= a) { maxval = a; li = l; } }
+   i = storage - li;
+   ((GeneralMatrix&)*this).tDelete(); return maxval;
+}
+
+Real GeneralMatrix::Minimum() const
+{
+   REPORT
+   if (storage == 0) NullMatrixError(this);
+   int l = storage - 1; Real* s = store; Real minval = *s++;
+   while (l--) { Real a = *s++; if (minval > a) minval = a; }
+   ((GeneralMatrix&)*this).tDelete(); return minval;
+}
+
+Real GeneralMatrix::Minimum1(int& i) const
+{
+   REPORT
+   if (storage == 0) NullMatrixError(this);
+   int l = storage - 1; Real* s = store; Real minval = *s++; int li = l;
+   while (l--) { Real a = *s++; if (minval >= a) { minval = a; li = l; } }
+   i = storage - li;
+   ((GeneralMatrix&)*this).tDelete(); return minval;
+}
+
+Real GeneralMatrix::MaximumAbsoluteValue2(int& i, int& j) const
+{
+   REPORT
+   if (storage == 0) NullMatrixError(this);
+   Real maxval = 0.0; int nr = Nrows();
+   MatrixRow mr((GeneralMatrix*)this, LoadOnEntry+DirectPart);
+   for (int r = 1; r <= nr; r++)
+   {
+      int c; maxval = mr.MaximumAbsoluteValue1(maxval, c);
+      if (c > 0) { i = r; j = c; }
+      mr.Next();
+   }
+   ((GeneralMatrix&)*this).tDelete(); return maxval;
+}
+
+Real GeneralMatrix::MinimumAbsoluteValue2(int& i, int& j) const
+{
+   REPORT
+   if (storage == 0)  NullMatrixError(this);
+   Real minval = FloatingPointPrecision::Maximum(); int nr = Nrows();
+   MatrixRow mr((GeneralMatrix*)this, LoadOnEntry+DirectPart);
+   for (int r = 1; r <= nr; r++)
+   {
+      int c; minval = mr.MinimumAbsoluteValue1(minval, c);
+      if (c > 0) { i = r; j = c; }
+      mr.Next();
+   }
+   ((GeneralMatrix&)*this).tDelete(); return minval;
+}
+
+Real GeneralMatrix::Maximum2(int& i, int& j) const
+{
+   REPORT
+   if (storage == 0) NullMatrixError(this);
+   Real maxval = -FloatingPointPrecision::Maximum(); int nr = Nrows();
+   MatrixRow mr((GeneralMatrix*)this, LoadOnEntry+DirectPart);
+   for (int r = 1; r <= nr; r++)
+   {
+      int c; maxval = mr.Maximum1(maxval, c);
+      if (c > 0) { i = r; j = c; }
+      mr.Next();
+   }
+   ((GeneralMatrix&)*this).tDelete(); return maxval;
+}
+
+Real GeneralMatrix::Minimum2(int& i, int& j) const
+{
+   REPORT
+   if (storage == 0) NullMatrixError(this);
+   Real minval = FloatingPointPrecision::Maximum(); int nr = Nrows();
+   MatrixRow mr((GeneralMatrix*)this, LoadOnEntry+DirectPart);
+   for (int r = 1; r <= nr; r++)
+   {
+      int c; minval = mr.Minimum1(minval, c);
+      if (c > 0) { i = r; j = c; }
+      mr.Next();
+   }
+   ((GeneralMatrix&)*this).tDelete(); return minval;
+}
+
+Real Matrix::MaximumAbsoluteValue2(int& i, int& j) const
+{
+   REPORT
+   int k; Real m = GeneralMatrix::MaximumAbsoluteValue1(k); k--;
+   i = k / Ncols(); j = k - i * Ncols(); i++; j++;
+   return m;
+}
+
+Real Matrix::MinimumAbsoluteValue2(int& i, int& j) const
+{
+   REPORT
+   int k; Real m = GeneralMatrix::MinimumAbsoluteValue1(k); k--;
+   i = k / Ncols(); j = k - i * Ncols(); i++; j++;
+   return m;
+}
+
+Real Matrix::Maximum2(int& i, int& j) const
+{
+   REPORT
+   int k; Real m = GeneralMatrix::Maximum1(k); k--;
+   i = k / Ncols(); j = k - i * Ncols(); i++; j++;
+   return m;
+}
+
+Real Matrix::Minimum2(int& i, int& j) const
+{
+   REPORT
+   int k; Real m = GeneralMatrix::Minimum1(k); k--;
+   i = k / Ncols(); j = k - i * Ncols(); i++; j++;
+   return m;
+}
+
+Real SymmetricMatrix::SumSquare() const
+{
+   REPORT
+   Real sum1 = 0.0; Real sum2 = 0.0; Real* s = store; int nr = nrows;
+   for (int i = 0; i<nr; i++)
+   {
+      int j = i;
+      while (j--) sum2 += square(*s++);
+      sum1 += square(*s++);
+   }
+   ((GeneralMatrix&)*this).tDelete(); return sum1 + 2.0 * sum2;
+}
+
+Real SymmetricMatrix::SumAbsoluteValue() const
+{
+   REPORT
+   Real sum1 = 0.0; Real sum2 = 0.0; Real* s = store; int nr = nrows;
+   for (int i = 0; i<nr; i++)
+   {
+      int j = i;
+      while (j--) sum2 += fabs(*s++);
+      sum1 += fabs(*s++);
+   }
+   ((GeneralMatrix&)*this).tDelete(); return sum1 + 2.0 * sum2;
+}
+
+Real IdentityMatrix::SumAbsoluteValue() const
+   { REPORT  return fabs(Trace()); }    // no need to do tDelete?
+
+Real SymmetricMatrix::Sum() const
+{
+   REPORT
+   Real sum1 = 0.0; Real sum2 = 0.0; Real* s = store; int nr = nrows;
+   for (int i = 0; i<nr; i++)
+   {
+      int j = i;
+      while (j--) sum2 += *s++;
+      sum1 += *s++;
+   }
+   ((GeneralMatrix&)*this).tDelete(); return sum1 + 2.0 * sum2;
+}
+
+Real IdentityMatrix::SumSquare() const
+{
+   Real sum = *store * *store * nrows;
+   ((GeneralMatrix&)*this).tDelete(); return sum;
+}
+
+
+Real BaseMatrix::SumSquare() const
+{
+   REPORT GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
+   Real s = gm->SumSquare(); return s;
+}
+
+Real BaseMatrix::NormFrobenius() const
+   { REPORT  return sqrt(SumSquare()); }
+
+Real BaseMatrix::SumAbsoluteValue() const
+{
+   REPORT GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
+   Real s = gm->SumAbsoluteValue(); return s;
+}
+
+Real BaseMatrix::Sum() const
+{
+   REPORT GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
+   Real s = gm->Sum(); return s;
+}
+
+Real BaseMatrix::MaximumAbsoluteValue() const
+{
+   REPORT GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
+   Real s = gm->MaximumAbsoluteValue(); return s;
+}
+
+Real BaseMatrix::MaximumAbsoluteValue1(int& i) const
+{
+   REPORT GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
+   Real s = gm->MaximumAbsoluteValue1(i); return s;
+}
+
+Real BaseMatrix::MaximumAbsoluteValue2(int& i, int& j) const
+{
+   REPORT GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
+   Real s = gm->MaximumAbsoluteValue2(i, j); return s;
+}
+
+Real BaseMatrix::MinimumAbsoluteValue() const
+{
+   REPORT GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
+   Real s = gm->MinimumAbsoluteValue(); return s;
+}
+
+Real BaseMatrix::MinimumAbsoluteValue1(int& i) const
+{
+   REPORT GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
+   Real s = gm->MinimumAbsoluteValue1(i); return s;
+}
+
+Real BaseMatrix::MinimumAbsoluteValue2(int& i, int& j) const
+{
+   REPORT GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
+   Real s = gm->MinimumAbsoluteValue2(i, j); return s;
+}
+
+Real BaseMatrix::Maximum() const
+{
+   REPORT GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
+   Real s = gm->Maximum(); return s;
+}
+
+Real BaseMatrix::Maximum1(int& i) const
+{
+   REPORT GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
+   Real s = gm->Maximum1(i); return s;
+}
+
+Real BaseMatrix::Maximum2(int& i, int& j) const
+{
+   REPORT GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
+   Real s = gm->Maximum2(i, j); return s;
+}
+
+Real BaseMatrix::Minimum() const
+{
+   REPORT GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
+   Real s = gm->Minimum(); return s;
+}
+
+Real BaseMatrix::Minimum1(int& i) const
+{
+   REPORT GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
+   Real s = gm->Minimum1(i); return s;
+}
+
+Real BaseMatrix::Minimum2(int& i, int& j) const
+{
+   REPORT GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
+   Real s = gm->Minimum2(i, j); return s;
+}
+
+Real DotProduct(const Matrix& A, const Matrix& B)
+{
+   REPORT
+   int n = A.storage;
+   if (n != B.storage) Throw(IncompatibleDimensionsException(A,B));
+   Real sum = 0.0; Real* a = A.store; Real* b = B.store;
+   while (n--) sum += *a++ * *b++;
+   return sum;
+}
+
+Real Matrix::Trace() const
+{
+   REPORT
+   Tracer trace("Trace");
+   int i = nrows; int d = i+1;
+   if (i != ncols) Throw(NotSquareException(*this));
+   Real sum = 0.0; Real* s = store;
+//   while (i--) { sum += *s; s += d; }
+   if (i) for (;;) { sum += *s; if (!(--i)) break; s += d; }
+   ((GeneralMatrix&)*this).tDelete(); return sum;
+}
+
+Real DiagonalMatrix::Trace() const
+{
+   REPORT
+   int i = nrows; Real sum = 0.0; Real* s = store;
+   while (i--) sum += *s++;
+   ((GeneralMatrix&)*this).tDelete(); return sum;
+}
+
+Real SymmetricMatrix::Trace() const
+{
+   REPORT
+   int i = nrows; Real sum = 0.0; Real* s = store; int j = 2;
+   // while (i--) { sum += *s; s += j++; }
+   if (i) for (;;) { sum += *s; if (!(--i)) break; s += j++; }
+   ((GeneralMatrix&)*this).tDelete(); return sum;
+}
+
+Real LowerTriangularMatrix::Trace() const
+{
+   REPORT
+   int i = nrows; Real sum = 0.0; Real* s = store; int j = 2;
+   // while (i--) { sum += *s; s += j++; }
+   if (i) for (;;) { sum += *s; if (!(--i)) break; s += j++; }
+   ((GeneralMatrix&)*this).tDelete(); return sum;
+}
+
+Real UpperTriangularMatrix::Trace() const
+{
+   REPORT
+   int i = nrows; Real sum = 0.0; Real* s = store;
+   while (i) { sum += *s; s += i--; }             // won t cause a problem
+   ((GeneralMatrix&)*this).tDelete(); return sum;
+}
+
+Real BandMatrix::Trace() const
+{
+   REPORT
+   int i = nrows; int w = lower+upper+1;
+   Real sum = 0.0; Real* s = store+lower;
+   // while (i--) { sum += *s; s += w; }
+   if (i) for (;;) { sum += *s; if (!(--i)) break; s += w; }
+   ((GeneralMatrix&)*this).tDelete(); return sum;
+}
+
+Real SymmetricBandMatrix::Trace() const
+{
+   REPORT
+   int i = nrows; int w = lower+1;
+   Real sum = 0.0; Real* s = store+lower;
+   // while (i--) { sum += *s; s += w; }
+   if (i) for (;;) { sum += *s; if (!(--i)) break; s += w; }
+   ((GeneralMatrix&)*this).tDelete(); return sum;
+}
+
+Real IdentityMatrix::Trace() const
+{
+   Real sum = *store * nrows;
+   ((GeneralMatrix&)*this).tDelete(); return sum;
+}
+
+
+Real BaseMatrix::Trace() const
+{
+   REPORT
+   MatrixType Diag = MatrixType::Dg; Diag.SetDataLossOK();
+   GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate(Diag);
+   Real sum = gm->Trace(); return sum;
+}
+
+void LogAndSign::operator*=(Real x)
+{
+   if (x > 0.0) { log_value += log(x); }
+   else if (x < 0.0) { log_value += log(-x); sign = -sign; }
+   else sign = 0;
+}
+
+void LogAndSign::PowEq(int k)
+{
+   if (sign)
+   {
+      log_value *= k;
+      if ( (k & 1) == 0 ) sign = 1;
+   }
+}
+
+Real LogAndSign::Value() const
+{
+   Tracer et("LogAndSign::Value");
+   if (log_value >= FloatingPointPrecision::LnMaximum())
+      Throw(OverflowException("Overflow in exponential"));
+   return sign * exp(log_value);
+}
+
+LogAndSign::LogAndSign(Real f)
+{
+   if (f == 0.0) { log_value = 0.0; sign = 0; return; }
+   else if (f < 0.0) { sign = -1; f = -f; }
+   else sign = 1;
+   log_value = log(f);
+}
+
+LogAndSign DiagonalMatrix::LogDeterminant() const
+{
+   REPORT
+   int i = nrows; LogAndSign sum; Real* s = store;
+   while (i--) sum *= *s++;
+   ((GeneralMatrix&)*this).tDelete(); return sum;
+}
+
+LogAndSign LowerTriangularMatrix::LogDeterminant() const
+{
+   REPORT
+   int i = nrows; LogAndSign sum; Real* s = store; int j = 2;
+   // while (i--) { sum *= *s; s += j++; }
+   if (i) for(;;) { sum *= *s; if (!(--i)) break; s += j++; }
+   ((GeneralMatrix&)*this).tDelete(); return sum;
+}
+
+LogAndSign UpperTriangularMatrix::LogDeterminant() const
+{
+   REPORT
+   int i = nrows; LogAndSign sum; Real* s = store;
+   while (i) { sum *= *s; s += i--; }
+   ((GeneralMatrix&)*this).tDelete(); return sum;
+}
+
+LogAndSign IdentityMatrix::LogDeterminant() const
+{
+   REPORT
+   int i = nrows; LogAndSign sum;
+   if (i > 0) { sum = *store; sum.PowEq(i); }
+   ((GeneralMatrix&)*this).tDelete(); return sum;
+}
+
+LogAndSign BaseMatrix::LogDeterminant() const
+{
+   REPORT GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
+   LogAndSign sum = gm->LogDeterminant(); return sum;
+}
+
+LogAndSign GeneralMatrix::LogDeterminant() const
+{
+   REPORT
+   Tracer tr("LogDeterminant");
+   if (nrows != ncols) Throw(NotSquareException(*this));
+   CroutMatrix C(*this); return C.LogDeterminant();
+}
+
+LogAndSign CroutMatrix::LogDeterminant() const
+{
+   REPORT
+   if (sing) return 0.0;
+   int i = nrows; int dd = i+1; LogAndSign sum; Real* s = store;
+   if (i) for(;;)
+   {
+      sum *= *s;
+      if (!(--i)) break;
+      s += dd;
+   }
+   if (!d) sum.ChangeSign(); return sum;
+
+}
+
+Real BaseMatrix::Determinant() const
+{
+   REPORT
+   Tracer tr("Determinant");
+   REPORT GeneralMatrix* gm = ((BaseMatrix&)*this).Evaluate();
+   LogAndSign ld = gm->LogDeterminant();
+   return ld.Value();
+}
+
+
+
+
+
+LinearEquationSolver::LinearEquationSolver(const BaseMatrix& bm)
+: gm( ( ((BaseMatrix&)bm).Evaluate() )->MakeSolver() )
+{
+   if (gm==&bm) { REPORT  gm = gm->Image(); }
+   // want a copy if  *gm is actually bm
+   else { REPORT  gm->Protect(); }
+}
+
+
+#ifdef use_namespace
+}
+#endif
+
Index: orxonox/trunk/src/util/newmat/newmat9.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/newmat9.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/newmat9.cpp	(revision 4565)
@@ -0,0 +1,76 @@
+//$$ newmat9.cpp         Input and output
+
+// Copyright (C) 1991,2,3,4: R B Davies
+
+
+#define WANT_STREAM
+
+#include "include.h"
+
+#include "newmat.h"
+#include "newmatio.h"
+#include "newmatrc.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+
+
+#ifdef DO_REPORT
+#define REPORT { static ExeCounter ExeCount(__LINE__,9); ++ExeCount; }
+#else
+#define REPORT {}
+#endif
+
+// for G++ 3.01
+#ifndef ios_format_flags
+#define ios_format_flags long
+#endif
+
+ostream& operator<<(ostream& s, const BaseMatrix& X)
+{
+   GeneralMatrix* gm = ((BaseMatrix&)X).Evaluate(); operator<<(s, *gm);
+   gm->tDelete(); return s;
+}
+
+
+ostream& operator<<(ostream& s, const GeneralMatrix& X)
+{
+   MatrixRow mr((GeneralMatrix*)&X, LoadOnEntry);
+   int w = s.width();  int nr = X.Nrows();  ios_format_flags f = s.flags();
+   s.setf(ios::fixed, ios::floatfield);
+   for (int i=1; i<=nr; i++)
+   {
+      int skip = mr.skip;  int storage = mr.storage;
+      Real* store = mr.data;  skip *= w+1;
+      while (skip--) s << " ";
+      while (storage--) { s.width(w); s << *store++ << " "; }
+//      while (storage--) s << setw(w) << *store++ << " ";
+      mr.Next();  s << "\n";
+   }
+   s << flush;  s.flags(f); return s;
+}
+
+// include this stuff if you are using an old version of G++
+// with an incomplete io library
+
+/*
+
+ostream& operator<<(ostream& os, Omanip_precision i)
+   { os.precision(i.x); return os; }
+
+Omanip_precision setprecision(int i) { return Omanip_precision(i); }
+
+ostream& operator<<(ostream& os, Omanip_width i)
+   { os.width(i.x); return os; }
+
+Omanip_width setw(int i) { return Omanip_width(i); }
+
+*/
+
+#ifdef use_namespace
+}
+#endif
+
+
Index: orxonox/trunk/src/util/newmat/newmatap.h
===================================================================
--- orxonox/trunk/src/util/newmat/newmatap.h	(revision 4565)
+++ orxonox/trunk/src/util/newmat/newmatap.h	(revision 4565)
@@ -0,0 +1,171 @@
+//$$ newmatap.h           definition file for matrix package applications
+
+// Copyright (C) 1991,2,3,4,8: R B Davies
+
+#ifndef NEWMATAP_LIB
+#define NEWMATAP_LIB 0
+
+#include "newmat.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+
+// ************************** applications *****************************/
+
+
+void QRZT(Matrix&, LowerTriangularMatrix&);
+
+void QRZT(const Matrix&, Matrix&, Matrix&);
+
+void QRZ(Matrix&, UpperTriangularMatrix&);
+
+void QRZ(const Matrix&, Matrix&, Matrix&);
+
+inline void HHDecompose(Matrix& X, LowerTriangularMatrix& L)
+{ QRZT(X,L); }
+
+inline void HHDecompose(const Matrix& X, Matrix& Y, Matrix& M)
+{ QRZT(X, Y, M); }
+
+ReturnMatrix Cholesky(const SymmetricMatrix&);
+
+ReturnMatrix Cholesky(const SymmetricBandMatrix&);
+
+void SVD(const Matrix&, DiagonalMatrix&, Matrix&, Matrix&,
+    bool=true, bool=true);
+
+void SVD(const Matrix&, DiagonalMatrix&);
+
+inline void SVD(const Matrix& A, DiagonalMatrix& D, Matrix& U,
+   bool withU = true) { SVD(A, D, U, U, withU, false); }
+
+void SortSV(DiagonalMatrix& D, Matrix& U, bool ascending = false);
+
+void SortSV(DiagonalMatrix& D, Matrix& U, Matrix& V, bool ascending = false);
+
+void Jacobi(const SymmetricMatrix&, DiagonalMatrix&);
+
+void Jacobi(const SymmetricMatrix&, DiagonalMatrix&, SymmetricMatrix&);
+
+void Jacobi(const SymmetricMatrix&, DiagonalMatrix&, Matrix&);
+
+void Jacobi(const SymmetricMatrix&, DiagonalMatrix&, SymmetricMatrix&,
+   Matrix&, bool=true);
+
+void EigenValues(const SymmetricMatrix&, DiagonalMatrix&);
+
+void EigenValues(const SymmetricMatrix&, DiagonalMatrix&, SymmetricMatrix&);
+
+void EigenValues(const SymmetricMatrix&, DiagonalMatrix&, Matrix&);
+
+class SymmetricEigenAnalysis
+// not implemented yet
+{
+public:
+   SymmetricEigenAnalysis(const SymmetricMatrix&);
+private:
+   DiagonalMatrix diag;
+   DiagonalMatrix offdiag;
+   SymmetricMatrix backtransform;
+   FREE_CHECK(SymmetricEigenAnalysis)
+};
+
+void SortAscending(GeneralMatrix&);
+
+void SortDescending(GeneralMatrix&);
+
+
+// class for deciding which fft to use and containing new fft function
+class FFT_Controller
+{
+public:
+   static bool OnlyOldFFT;
+   static bool ar_1d_ft (int PTS, Real* X, Real *Y);
+   static bool CanFactor(int PTS);
+};
+
+void FFT(const ColumnVector&, const ColumnVector&,
+   ColumnVector&, ColumnVector&);
+
+void FFTI(const ColumnVector&, const ColumnVector&,
+   ColumnVector&, ColumnVector&);
+
+void RealFFT(const ColumnVector&, ColumnVector&, ColumnVector&);
+
+void RealFFTI(const ColumnVector&, const ColumnVector&, ColumnVector&);
+
+void DCT_II(const ColumnVector&, ColumnVector&);
+
+void DCT_II_inverse(const ColumnVector&, ColumnVector&);
+
+void DST_II(const ColumnVector&, ColumnVector&);
+
+void DST_II_inverse(const ColumnVector&, ColumnVector&);
+
+void DCT(const ColumnVector&, ColumnVector&);
+
+void DCT_inverse(const ColumnVector&, ColumnVector&);
+
+void DST(const ColumnVector&, ColumnVector&);
+
+void DST_inverse(const ColumnVector&, ColumnVector&);
+
+// This class is used by the new FFT program
+
+// Suppose an integer is expressed as a sequence of digits with each
+// digit having a different radix.
+// This class supposes we are counting with this multi-radix number
+// but also keeps track of the number with the digits (and radices)
+// reversed.
+// The integer starts at zero
+// operator++() increases it by 1
+// Counter gives the number of increments
+// Reverse() gives the value with the digits in reverse order
+// Swap is true if reverse is less than counter
+// Finish is true when we have done a complete cycle and are back at zero
+
+class MultiRadixCounter
+{
+   const SimpleIntArray& Radix;
+                              // radix of each digit
+                              // n-1 highest order, 0 lowest order
+   SimpleIntArray& Value;     // value of each digit
+   const int n;               // number of digits
+   int reverse;               // value when order of digits is reversed
+   int product;               // product of radices
+   int counter;               // counter
+   bool finish;               // true when we have gone over whole range
+public:
+   MultiRadixCounter(int nx, const SimpleIntArray& rx,
+      SimpleIntArray& vx);
+   void operator++();         // increment the multi-radix counter
+   bool Swap() const { return reverse < counter; }
+   bool Finish() const { return finish; }
+   int Reverse() const { return reverse; }
+   int Counter() const { return counter; }
+};
+
+
+#ifdef use_namespace
+}
+#endif
+
+
+
+#endif
+
+// body file: cholesky.cpp
+// body file: evalue.cpp
+// body file: fft.cpp
+// body file: hholder.cpp
+// body file: jacobi.cpp
+// body file: newfft.cpp
+// body file: sort.cpp
+// body file: svd.cpp
+
+
+
+
+
Index: orxonox/trunk/src/util/newmat/newmatex.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/newmatex.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/newmatex.cpp	(revision 4565)
@@ -0,0 +1,307 @@
+//$$ newmatex.cpp                    Exception handler
+
+// Copyright (C) 1992,3,4,7: R B Davies
+
+#define WANT_STREAM                  // include.h will get stream fns
+
+#include "include.h"                 // include standard files
+#include "newmat.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+unsigned long OverflowException::Select;
+unsigned long SingularException::Select;
+unsigned long NPDException::Select;
+unsigned long ConvergenceException::Select;
+unsigned long ProgramException::Select;
+unsigned long IndexException::Select;
+unsigned long VectorException::Select;
+unsigned long NotSquareException::Select;
+unsigned long SubMatrixDimensionException::Select;
+unsigned long IncompatibleDimensionsException::Select;
+unsigned long NotDefinedException::Select;
+unsigned long CannotBuildException::Select;
+unsigned long InternalException::Select;
+
+
+
+static void MatrixDetails(const GeneralMatrix& A)
+// write matrix details to Exception buffer
+{
+   MatrixBandWidth bw = A.BandWidth(); int ubw = bw.upper; int lbw = bw.lower;
+   Exception::AddMessage("MatrixType = ");
+   Exception::AddMessage(A.Type().Value());
+   Exception::AddMessage("  # Rows = "); Exception::AddInt(A.Nrows());
+   Exception::AddMessage("; # Cols = "); Exception::AddInt(A.Ncols());
+   if (lbw >=0)
+      { Exception::AddMessage("; lower BW = "); Exception::AddInt(lbw); }
+   if (ubw >=0)
+      { Exception::AddMessage("; upper BW = "); Exception::AddInt(ubw); }
+   Exception::AddMessage("\n");
+}
+
+NPDException::NPDException(const GeneralMatrix& A)
+   : Runtime_error()
+{
+   Select = Exception::Select;
+   AddMessage("detected by Newmat: matrix not positive definite\n\n");
+   MatrixDetails(A);
+   Tracer::AddTrace();
+}
+
+SingularException::SingularException(const GeneralMatrix& A)
+   : Runtime_error()
+{
+   Select = Exception::Select;
+   AddMessage("detected by Newmat: matrix is singular\n\n");
+   MatrixDetails(A);
+   Tracer::AddTrace();
+}
+
+ConvergenceException::ConvergenceException(const GeneralMatrix& A)
+   : Runtime_error()
+{
+   Select = Exception::Select;
+   AddMessage("detected by Newmat: process fails to converge\n\n");
+   MatrixDetails(A);
+   Tracer::AddTrace();
+}
+
+ConvergenceException::ConvergenceException(const char* c) : Runtime_error()
+{
+   Select = Exception::Select;
+   AddMessage("detected by Newmat: ");
+   AddMessage(c); AddMessage("\n\n");
+   if (c) Tracer::AddTrace();
+}
+
+OverflowException::OverflowException(const char* c) : Runtime_error()
+{
+   Select = Exception::Select;
+   AddMessage("detected by Newmat: ");
+   AddMessage(c); AddMessage("\n\n");
+   if (c) Tracer::AddTrace();
+}
+
+ProgramException::ProgramException(const char* c) : Logic_error()
+{
+   Select = Exception::Select;
+   AddMessage("detected by Newmat: ");
+   AddMessage(c); AddMessage("\n\n");
+   if (c) Tracer::AddTrace();
+}
+
+ProgramException::ProgramException(const char* c, const GeneralMatrix& A)
+   : Logic_error()
+{
+   Select = Exception::Select;
+   AddMessage("detected by Newmat: ");
+   AddMessage(c); AddMessage("\n\n");
+   MatrixDetails(A);
+   if (c) Tracer::AddTrace();
+}
+
+ProgramException::ProgramException(const char* c, const GeneralMatrix& A,
+   const GeneralMatrix& B) : Logic_error()
+{
+   Select = Exception::Select;
+   AddMessage("detected by Newmat: ");
+   AddMessage(c); AddMessage("\n\n");
+   MatrixDetails(A); MatrixDetails(B);
+   if (c) Tracer::AddTrace();
+}
+
+ProgramException::ProgramException(const char* c, MatrixType a, MatrixType b)
+   : Logic_error()
+{
+   Select = Exception::Select;
+   AddMessage("detected by Newmat: ");
+   AddMessage(c); AddMessage("\nMatrixTypes = ");
+   AddMessage(a.Value()); AddMessage("; ");
+   AddMessage(b.Value()); AddMessage("\n\n");
+   if (c) Tracer::AddTrace();
+}
+
+VectorException::VectorException() : Logic_error()
+{
+   Select = Exception::Select;
+   AddMessage("detected by Newmat: cannot convert matrix to vector\n\n");
+   Tracer::AddTrace();
+}
+
+VectorException::VectorException(const GeneralMatrix& A)
+   : Logic_error()
+{
+   Select = Exception::Select;
+   AddMessage("detected by Newmat: cannot convert matrix to vector\n\n");
+   MatrixDetails(A);
+   Tracer::AddTrace();
+}
+
+NotSquareException::NotSquareException(const GeneralMatrix& A)
+   : Logic_error()
+{
+   Select = Exception::Select;
+   AddMessage("detected by Newmat: matrix is not square\n\n");
+   MatrixDetails(A);
+   Tracer::AddTrace();
+}
+
+SubMatrixDimensionException::SubMatrixDimensionException()
+   : Logic_error()
+{
+   Select = Exception::Select;
+   AddMessage("detected by Newmat: incompatible submatrix dimension\n\n");
+   Tracer::AddTrace();
+}
+
+IncompatibleDimensionsException::IncompatibleDimensionsException()
+   : Logic_error()
+{
+   Select = Exception::Select;
+   AddMessage("detected by Newmat: incompatible dimensions\n\n");
+   Tracer::AddTrace();
+}
+
+IncompatibleDimensionsException::IncompatibleDimensionsException
+   (const GeneralMatrix& A, const GeneralMatrix& B)
+      : Logic_error()
+{
+   Select = Exception::Select;
+   AddMessage("detected by Newmat: incompatible dimensions\n\n");
+   MatrixDetails(A); MatrixDetails(B);
+   Tracer::AddTrace();
+}
+
+NotDefinedException::NotDefinedException(const char* op, const char* matrix)
+   : Logic_error()
+{
+   Select = Exception::Select;
+   AddMessage("detected by Newmat: ");
+   AddMessage(op);
+   AddMessage(" not defined for ");
+   AddMessage(matrix);
+   AddMessage("\n\n");
+   Tracer::AddTrace();
+}
+
+CannotBuildException::CannotBuildException(const char* matrix)
+   : Logic_error()
+{
+   Select = Exception::Select;
+   AddMessage("detected by Newmat: cannot build matrix type ");
+   AddMessage(matrix); AddMessage("\n\n");
+   Tracer::AddTrace();
+}
+
+IndexException::IndexException(int i, const GeneralMatrix& A)
+   : Logic_error()
+{
+   Select = Exception::Select;
+   AddMessage("detected by Newmat: index error: requested index = ");
+   AddInt(i); AddMessage("\n\n");
+   MatrixDetails(A);
+   Tracer::AddTrace();
+}
+
+IndexException::IndexException(int i, int j, const GeneralMatrix& A)
+   : Logic_error()
+{
+   Select = Exception::Select;
+   AddMessage("detected by Newmat: index error: requested indices = ");
+   AddInt(i); AddMessage(", "); AddInt(j);
+   AddMessage("\n\n");
+   MatrixDetails(A);
+   Tracer::AddTrace();
+}
+
+
+IndexException::IndexException(int i, const GeneralMatrix& A, bool)
+   : Logic_error()
+{
+   Select = Exception::Select;
+   AddMessage("detected by Newmat: element error: requested index (wrt 0) = ");
+   AddInt(i);
+   AddMessage("\n\n");
+   MatrixDetails(A);
+   Tracer::AddTrace();
+}
+
+IndexException::IndexException(int i, int j, const GeneralMatrix& A, bool)
+   : Logic_error()
+{
+   Select = Exception::Select;
+   AddMessage(
+      "detected by Newmat: element error: requested indices (wrt 0) = ");
+   AddInt(i); AddMessage(", "); AddInt(j);
+   AddMessage("\n\n");
+   MatrixDetails(A);
+   Tracer::AddTrace();
+}
+
+InternalException::InternalException(const char* c) : Logic_error()
+{
+   Select = Exception::Select;
+   AddMessage("internal error detected by Newmat: please inform author\n");
+   AddMessage(c); AddMessage("\n\n");
+   Tracer::AddTrace();
+}
+
+
+
+
+/************************* ExeCounter functions *****************************/
+
+#ifdef DO_REPORT
+
+int ExeCounter::nreports;                      // will be set to zero
+
+ExeCounter::ExeCounter(int xl, int xf) : line(xl), fileid(xf), nexe(0) {}
+
+ExeCounter::~ExeCounter()
+{
+   nreports++;
+   cout << "REPORT  " << setw(6) << nreports << "  "
+      << setw(6) << fileid << "  " << setw(6) << line
+      << "  " << setw(6) << nexe << "\n";
+}
+
+#endif
+
+/**************************** error handler *******************************/
+
+void MatrixErrorNoSpace(void* v) { if (!v) Throw(Bad_alloc()); }
+// throw exception if v is null
+
+
+
+
+/************************* miscellanous errors ***************************/
+
+
+void CroutMatrix::GetRow(MatrixRowCol&)
+   { Throw(NotDefinedException("GetRow","Crout")); }
+void CroutMatrix::GetCol(MatrixRowCol&)
+   { Throw(NotDefinedException("GetCol","Crout")); }
+void CroutMatrix::operator=(const BaseMatrix&)
+   { Throw(NotDefinedException("=","Crout")); }
+void BandLUMatrix::GetRow(MatrixRowCol&)
+   { Throw(NotDefinedException("GetRow","BandLUMatrix")); }
+void BandLUMatrix::GetCol(MatrixRowCol&)
+   { Throw(NotDefinedException("GetCol","BandLUMatrix")); }
+void BandLUMatrix::operator=(const BaseMatrix&)
+   { Throw(NotDefinedException("=","BandLUMatrix")); }
+void BaseMatrix::IEQND() const
+   { Throw(NotDefinedException("inequalities", "matrices")); }
+#ifdef TEMPS_DESTROYED_QUICKLY_R
+   ReturnMatrixX::ReturnMatrixX(const ReturnMatrixX& tm)
+     : gm(tm.gm) { Throw(ProgramException("ReturnMatrixX error")); }
+#endif
+
+
+#ifdef use_namespace
+}
+#endif
+
Index: orxonox/trunk/src/util/newmat/newmatio.h
===================================================================
--- orxonox/trunk/src/util/newmat/newmatio.h	(revision 4565)
+++ orxonox/trunk/src/util/newmat/newmatio.h	(revision 4565)
@@ -0,0 +1,61 @@
+//$$ newmatio.h           definition file for matrix package input/output
+
+// Copyright (C) 1991,2,3,4: R B Davies
+
+#ifndef NEWMATIO_LIB
+#define NEWMATIO_LIB 0
+
+#ifndef WANT_STREAM
+#define WANT_STREAM
+#endif
+
+#include "newmat.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+
+
+/**************************** input/output *****************************/
+
+ostream& operator<<(ostream&, const BaseMatrix&);
+
+ostream& operator<<(ostream&, const GeneralMatrix&);
+
+
+/*  Use in some old versions of G++ without complete iomanipulators
+
+class Omanip_precision
+{
+   int x;
+public:
+   Omanip_precision(int i) : x(i) {}
+   friend ostream& operator<<(ostream& os, Omanip_precision i);
+};
+
+
+Omanip_precision setprecision(int i);
+
+class Omanip_width
+{
+   int x;
+public:
+   Omanip_width(int i) : x(i) {}
+   friend ostream& operator<<(ostream& os, Omanip_width i);
+};
+
+Omanip_width setw(int i);
+
+*/
+
+#ifdef use_namespace
+}
+#endif
+
+
+
+#endif
+
+// body file: newmat9.cpp
+
Index: orxonox/trunk/src/util/newmat/newmatnl.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/newmatnl.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/newmatnl.cpp	(revision 4565)
@@ -0,0 +1,258 @@
+//$$ newmatnl.cpp         Non-linear optimisation
+
+// Copyright (C) 1993,4,5,6: R B Davies
+
+
+#define WANT_MATH
+#define WANT_STREAM
+
+#include "newmatap.h"
+#include "newmatnl.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+
+
+void FindMaximum2::Fit(ColumnVector& Theta, int n_it)
+{
+   Tracer tr("FindMaximum2::Fit");
+   enum State {Start, Restart, Continue, Interpolate, Extrapolate,
+      Fail, Convergence};
+   State TheState = Start;
+   Real z,w,x,x2,g,l1,l2,l3,d1,d2=0,d3;
+   ColumnVector Theta1, Theta2, Theta3;
+   int np = Theta.Nrows();
+   ColumnVector H1(np), H3, HP(np), K, K1(np);
+   bool oorg, conv;
+   int counter = 0;
+   Theta1 = Theta; HP = 0.0; g = 0.0;
+
+   // This is really a set of gotos and labels, but they do not work
+   // correctly in AT&T C++ and Sun 4.01 C++.
+
+   for(;;)
+   {
+      switch (TheState)
+      {
+      case Start:
+	 tr.ReName("FindMaximum2::Fit/Start");
+	 Value(Theta1, true, l1, oorg);
+	 if (oorg) Throw(ProgramException("invalid starting value\n"));
+
+      case Restart:
+	 tr.ReName("FindMaximum2::Fit/ReStart");
+	 conv = NextPoint(H1, d1);
+	 if (conv) { TheState = Convergence; break; }
+	 if (counter++ > n_it) { TheState = Fail; break; }
+
+	 z = 1.0 / sqrt(d1);
+	 H3 = H1 * z; K = (H3 - HP) * g; HP = H3;
+	 g = 0.0;                     // de-activate to use curved projection
+	 if (g==0.0) K1 = 0.0; else K1 = K * 0.2 + K1 * 0.6;
+	 // (K - K1) * alpha + K1 * (1 - alpha)
+	 //     = K * alpha + K1 * (1 - 2 * alpha)
+	 K = K1 * d1; g = z;
+
+      case Continue:
+	 tr.ReName("FindMaximum2::Fit/Continue");
+	 Theta2 = Theta1 + H1 + K;
+	 Value(Theta2, false, l2, oorg);
+	 if (counter++ > n_it) { TheState = Fail; break; }
+	 if (oorg)
+	 {
+	    H1 *= 0.5; K *= 0.25; d1 *= 0.5; g *= 2.0;
+	    TheState =  Continue; break;
+	 }
+	 d2 = LastDerivative(H1 + K * 2.0);
+
+      case Interpolate:
+	 tr.ReName("FindMaximum2::Fit/Interpolate");
+	 z = d1 + d2 - 3.0 * (l2 - l1);
+	 w = z * z - d1 * d2;
+	 if (w < 0.0) { TheState = Extrapolate; break; }
+	 w = z + sqrt(w);
+	 if (1.5 * w + d1 < 0.0)
+	    { TheState = Extrapolate; break; }
+	 if (d2 > 0.0 && l2 > l1 && w > 0.0)
+	    { TheState = Extrapolate; break; }
+	 x = d1 / (w + d1); x2 = x * x; g /= x;
+	 Theta3 = Theta1 + H1 * x + K * x2;
+	 Value(Theta3, true, l3, oorg);
+	 if (counter++ > n_it) { TheState = Fail; break; }
+	 if (oorg)
+	 {
+	    if (x <= 1.0)
+	       { x *= 0.5; x2 = x*x; g *= 2.0; d1 *= x; H1 *= x; K *= x2; }
+	    else
+	    {
+	       x = 0.5 * (x-1.0); x2 = x*x; Theta1 = Theta2;
+	       H1 = (H1 + K * 2.0) * x;
+	       K *= x2; g = 0.0; d1 = x * d2; l1 = l2;
+	    }
+	    TheState = Continue; break;
+	 }
+
+	 if (l3 >= l1 && l3 >= l2)
+	    { Theta1 = Theta3; l1 = l3; TheState =  Restart; break; }
+
+	 d3 = LastDerivative(H1 + K * 2.0);
+	 if (l1 > l2)
+	    { H1 *= x; K *= x2; Theta2 = Theta3; d1 *= x; d2 = d3*x; }
+	 else
+	 {
+	    Theta1 = Theta2; Theta2 = Theta3;
+	    x -= 1.0; x2 = x*x; g = 0.0; H1 = (H1 + K * 2.0) * x;
+	    K *= x2; l1 = l2; l2 = l3; d1 = x*d2; d2 = x*d3;
+	    if (d1 <= 0.0) { TheState = Start; break; }
+	 }
+	 TheState =  Interpolate; break;
+
+      case Extrapolate:
+	 tr.ReName("FindMaximum2::Fit/Extrapolate");
+	 Theta1 = Theta2; g = 0.0; K *= 4.0; H1 = (H1 * 2.0 + K);
+	 d1 = 2.0 * d2; l1 = l2;
+	 TheState = Continue; break;
+
+      case Fail:
+	 Throw(ConvergenceException(Theta));
+
+      case Convergence:
+	 Theta = Theta1; return;
+      }
+   }
+}
+
+
+
+void NonLinearLeastSquares::Value
+   (const ColumnVector& Parameters, bool, Real& v, bool& oorg)
+{
+   Tracer tr("NonLinearLeastSquares::Value");
+   Y.ReSize(n_obs); X.ReSize(n_obs,n_param);
+   // put the fitted values in Y, the derivatives in X.
+   Pred.Set(Parameters);
+   if (!Pred.IsValid()) { oorg=true; return; }
+   for (int i=1; i<=n_obs; i++)
+   {
+      Y(i) = Pred(i);
+      X.Row(i) = Pred.Derivatives();
+   }
+   if (!Pred.IsValid()) { oorg=true; return; }  // check afterwards as well
+   Y = *DataPointer - Y; Real ssq = Y.SumSquare();
+   errorvar =  ssq / (n_obs - n_param);
+   cout << "\n" << setw(15) << setprecision(10) << " " << errorvar;
+   Derivs = Y.t() * X;          // get the derivative and stash it
+   oorg = false; v = -0.5 * ssq;
+}
+
+bool NonLinearLeastSquares::NextPoint(ColumnVector& Adj, Real& test)
+{
+   Tracer tr("NonLinearLeastSquares::NextPoint");
+   QRZ(X, U); QRZ(X, Y, M);     // do the QR decomposition
+   test = M.SumSquare();
+   cout << " " << setw(15) << setprecision(10)
+      << test << " " << Y.SumSquare() / (n_obs - n_param);
+   Adj = U.i() * M;
+   if (test < errorvar * criterion) return true;
+   else return false;
+}
+
+Real NonLinearLeastSquares::LastDerivative(const ColumnVector& H)
+{ return (Derivs * H).AsScalar(); }
+
+void NonLinearLeastSquares::Fit(const ColumnVector& Data,
+   ColumnVector& Parameters)
+{
+   Tracer tr("NonLinearLeastSquares::Fit");
+   n_param = Parameters.Nrows(); n_obs = Data.Nrows();
+   DataPointer = &Data;
+   FindMaximum2::Fit(Parameters, Lim);
+   cout << "\nConverged\n";
+}
+
+void NonLinearLeastSquares::MakeCovariance()
+{
+   if (Covariance.Nrows()==0)
+   {
+      UpperTriangularMatrix UI = U.i();
+      Covariance << UI * UI.t() * errorvar;
+      SE << Covariance;                 // get diagonals
+      for (int i = 1; i<=n_param; i++) SE(i) = sqrt(SE(i));
+   }
+}
+
+void NonLinearLeastSquares::GetStandardErrors(ColumnVector& SEX)
+   { MakeCovariance(); SEX = SE.AsColumn(); }
+
+void NonLinearLeastSquares::GetCorrelations(SymmetricMatrix& Corr)
+   { MakeCovariance(); Corr << SE.i() * Covariance * SE.i(); }
+
+void NonLinearLeastSquares::GetHatDiagonal(DiagonalMatrix& Hat) const
+{
+   Hat.ReSize(n_obs);
+   for (int i = 1; i<=n_obs; i++) Hat(i) = X.Row(i).SumSquare();
+}
+
+
+// the MLE_D_FI routines
+
+void MLE_D_FI::Value
+   (const ColumnVector& Parameters, bool wg, Real& v, bool& oorg)
+{
+   Tracer tr("MLE_D_FI::Value");
+   if (!LL.IsValid(Parameters,wg)) { oorg=true; return; }
+   v = LL.LogLikelihood();
+   if (!LL.IsValid()) { oorg=true; return; }     // check validity again
+   cout << "\n" << setw(20) << setprecision(10) << v;
+   oorg = false;
+   Derivs = LL.Derivatives();                    // Get derivatives
+}
+
+bool MLE_D_FI::NextPoint(ColumnVector& Adj, Real& test)
+{
+   Tracer tr("MLE_D_FI::NextPoint");
+   SymmetricMatrix FI = LL.FI();
+   LT = Cholesky(FI);
+   ColumnVector Adj1 = LT.i() * Derivs;
+   Adj = LT.t().i() * Adj1;
+   test = SumSquare(Adj1);
+   cout << "   " << setw(20) << setprecision(10) << test;
+   return (test < Criterion);
+}
+
+Real MLE_D_FI::LastDerivative(const ColumnVector& H)
+{ return (Derivs.t() * H).AsScalar(); }
+
+void MLE_D_FI::Fit(ColumnVector& Parameters)
+{
+   Tracer tr("MLE_D_FI::Fit");
+   FindMaximum2::Fit(Parameters,Lim);
+   cout << "\nConverged\n";
+}
+  
+void MLE_D_FI::MakeCovariance()
+{
+   if (Covariance.Nrows()==0)
+   {
+      LowerTriangularMatrix LTI = LT.i();
+      Covariance << LTI.t() * LTI;
+      SE << Covariance;                // get diagonal
+      int n = Covariance.Nrows();
+      for (int i=1; i <= n; i++) SE(i) = sqrt(SE(i));
+   }
+}
+
+void MLE_D_FI::GetStandardErrors(ColumnVector& SEX)
+{ MakeCovariance(); SEX = SE.AsColumn(); }
+   
+void MLE_D_FI::GetCorrelations(SymmetricMatrix& Corr)
+{ MakeCovariance(); Corr << SE.i() * Covariance * SE.i(); }
+
+
+
+#ifdef use_namespace
+}
+#endif
+
Index: orxonox/trunk/src/util/newmat/newmatnl.h
===================================================================
--- orxonox/trunk/src/util/newmat/newmatnl.h	(revision 4565)
+++ orxonox/trunk/src/util/newmat/newmatnl.h	(revision 4565)
@@ -0,0 +1,322 @@
+//$$ newmatnl.h           definition file for non-linear optimisation
+
+// Copyright (C) 1993,4,5: R B Davies
+
+#ifndef NEWMATNL_LIB
+#define NEWMATNL_LIB 0
+
+#include "newmat.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+
+
+/*
+
+This is a beginning of a series of classes for non-linear optimisation.
+
+At present there are two classes. FindMaximum2 is the basic optimisation
+strategy when one is doing an optimisation where one has first
+derivatives and estimates of the second derivatives. Class
+NonLinearLeastSquares is derived from FindMaximum2. This provides the
+functions that calculate function values and derivatives.
+
+A third class is now added. This is for doing maximum-likelihood when
+you have first derviatives and something like the Fisher Information
+matrix (eg the variance covariance matrix of the first derivatives or
+minus the second derivatives - this matrix is assumed to be positive
+definite).
+
+
+
+   class FindMaximum2
+
+Suppose T is the ColumnVector of parameters, F(T) the function we want
+to maximise, D(T) the ColumnVector of derivatives of F with respect to
+T, and S(T) the matrix of second derivatives.
+
+Then the basic iteration is given a value of T, update it to
+
+           T - S.i() * D
+
+where .i() denotes inverse.
+
+If F was quadratic this would give exactly the right answer (except it
+might get a minimum rather than a maximum). Since F is not usually
+quadratic, the simple procedure would be to recalculate S and D with the
+new value of T and keep iterating until the process converges. This is
+known as the method of conjugate gradients.
+
+In practice, this method may not converge. FindMaximum2 considers an
+iteration of the form
+
+           T - x * S.i() * D
+
+where x is a number. It tries x = 1 and uses the values of F and its
+slope with respect to x at x = 0 and x = 1 to fit a cubic in x. It then
+choses x to maximise the resulting function. This gives our new value of
+T. The program checks that the value of F is getting better and carries
+out a variety of strategies if it is not.
+
+The program also has a second strategy. If the successive values of T
+seem to be lying along a curve - eg we are following along a curved
+ridge, the program will try to fit this ridge and project along it. This
+does not work at present and is commented out.
+
+FindMaximum2 has three virtual functions which need to be over-ridden by
+a derived class.
+
+   void Value(const ColumnVector& T, bool wg, Real& f, bool& oorg);
+
+T is the column vector of parameters. The function returns the value of
+the function to f, but may instead set oorg to true if the parameter
+values are not valid. If wg is true it may also calculate and store the
+second derivative information.
+
+   bool NextPoint(ColumnVector& H, Real& d);
+
+Using the value of T provided in the previous call of Value, find the
+conjugate gradients adjustment to T, that is - S.i() * D. Also return
+
+           d = D.t() * S.i() * D.
+
+NextPoint should return true if it considers that the process has
+converged (d very small) and false otherwise. The previous call of Value
+will have set wg to true, so that S will be available.
+
+   Real LastDerivative(const ColumnVector& H);
+
+Return the scalar product of H and the vector of derivatives at the last
+value of T.
+
+The function Fit is the function that calls the iteration.
+
+   void Fit(ColumnVector&, int);
+
+The arguments are the trial parameter values as a ColumnVector and the
+maximum number of iterations. The program calls a DataException if the
+initial parameters are not valid and a ConvergenceException if the
+process fails to converge.
+
+
+   class NonLinearLeastSquares
+
+This class is derived from FindMaximum2 and carries out a non-linear
+least squares fit. It uses a QR decomposition to carry out the
+operations required by FindMaximum2.
+
+A prototype class R1_Col_I_D is provided. The user needs to derive a
+class from this which includes functions the predicted value of each
+observation its derivatives. An object from this class has to be
+provided to class NonLinearLeastSquares.
+
+Suppose we observe n normal random variables with the same unknown
+variance and such the i-th one has expected value given by f(i,P)
+where P is a column vector of unknown parameters and f is a known
+function. We wish to estimate P.
+
+First derive a class from R1_Col_I_D and override Real operator()(int i)
+to give the value of the function f in terms of i and the ColumnVector
+para defined in class R1_CoL_I_D. Also override ReturnMatrix
+Derivatives() to give the derivates of f at para and the value of i
+used in the preceeding call to operator(). Return the result as a
+RowVector. Construct an object from this class. Suppose in what follows
+it is called pred.
+
+Now constuct a NonLinearLeastSquaresObject accessing pred and optionally
+an iteration limit and an accuracy critierion.
+
+   NonLinearLeastSquares NLLS(pred, 1000, 0.0001);
+
+The accuracy critierion should be somewhat less than one and 0.0001 is
+about the smallest sensible value.
+
+Define a ColumnVector P containing a guess at the value of the unknown
+parameter, and a ColumnVector Y containing the unknown data. Call
+
+   NLLS.Fit(Y,P);
+
+If the process converges, P will contain the estimates of the unknown
+parameters. If it does not converge an exception will be generated.
+
+The following member functions can be called after you have done a fit.
+
+Real ResidualVariance() const;
+
+The estimate of the variance of the observations.
+
+void GetResiduals(ColumnVector& Z) const;
+
+The residuals of the individual observations.
+
+void GetStandardErrors(ColumnVector&);
+
+The standard errors of the observations.
+
+void GetCorrelations(SymmetricMatrix&);
+
+The correlations of the observations.
+
+void GetHatDiagonal(DiagonalMatrix&) const;
+
+Forms a diagonal matrix of values between 0 and 1. If the i-th value is
+larger than, say 0.2, then the i-th data value could have an undue
+influence on your estimates.
+
+
+*/
+
+class FindMaximum2
+{
+   virtual void Value(const ColumnVector&, bool, Real&, bool&) = 0;
+   virtual bool NextPoint(ColumnVector&, Real&) = 0;
+   virtual Real LastDerivative(const ColumnVector&) = 0;
+public:
+   void Fit(ColumnVector&, int);
+   virtual ~FindMaximum2() {}            // to keep gnu happy
+};
+
+class R1_Col_I_D
+{
+   // The prototype for a Real function of a ColumnVector and an
+   // integer.
+   // You need to derive your function from this one and put in your
+   // function for operator() and Derivatives() at least.
+   // You may also want to set up a constructor to enter in additional
+   // parameter values (that will not vary during the solve).
+
+protected:
+   ColumnVector para;                 // Current x value
+
+public:
+   virtual bool IsValid() { return true; }
+                                       // is the current x value OK
+   virtual Real operator()(int i) = 0; // i-th function value at current para
+   virtual void Set(const ColumnVector& X) { para = X; }
+                                       // set current para
+   bool IsValid(const ColumnVector& X)
+      { Set(X); return IsValid(); }
+                                       // set para, check OK
+   Real operator()(int i, const ColumnVector& X)
+      { Set(X); return operator()(i); }
+                                       // set para, return value
+   virtual ReturnMatrix Derivatives() = 0;
+                                       // return derivatives as RowVector
+   virtual ~R1_Col_I_D() {}            // to keep gnu happy
+};
+
+
+class NonLinearLeastSquares : public FindMaximum2
+{
+   // these replace the corresponding functions in FindMaximum2
+   void Value(const ColumnVector&, bool, Real&, bool&);
+   bool NextPoint(ColumnVector&, Real&);
+   Real LastDerivative(const ColumnVector&);
+
+   Matrix X;                         // the things we need to do the
+   ColumnVector Y;                   // QR triangularisation
+   UpperTriangularMatrix U;          // see the write-up in newmata.txt
+   ColumnVector M;
+   Real errorvar, criterion;
+   int n_obs, n_param;
+   const ColumnVector* DataPointer;
+   RowVector Derivs;
+   SymmetricMatrix Covariance;
+   DiagonalMatrix SE;
+   R1_Col_I_D& Pred;                 // Reference to predictor object
+   int Lim;                          // maximum number of iterations
+
+public:
+   NonLinearLeastSquares(R1_Col_I_D& pred, int lim=1000, Real crit=0.0001)
+      : criterion(crit), Pred(pred), Lim(lim) {}
+   void Fit(const ColumnVector&, ColumnVector&);
+   Real ResidualVariance() const { return errorvar; }
+   void GetResiduals(ColumnVector& Z) const { Z = Y; }
+   void GetStandardErrors(ColumnVector&);
+   void GetCorrelations(SymmetricMatrix&);
+   void GetHatDiagonal(DiagonalMatrix&) const;
+
+private:
+   void MakeCovariance();
+};
+
+
+// The next class is the prototype class for calculating the
+// log-likelihood.
+// I assume first derivatives are available and something like the 
+// Fisher Information or variance/covariance matrix of the first
+// derivatives or minus the matrix of second derivatives is
+// available. This matrix must be positive definite.
+
+class LL_D_FI
+{
+protected:
+   ColumnVector para;                  // current parameter values
+   bool wg;                         // true if FI matrix wanted
+
+public:
+   virtual void Set(const ColumnVector& X) { para = X; }
+                                       // set parameter values
+   virtual void WG(bool wgx) { wg = wgx; }
+                                       // set wg
+
+   virtual bool IsValid() { return true; }
+                                       // return true is para is OK
+   bool IsValid(const ColumnVector& X, bool wgx=true)
+      { Set(X); WG(wgx); return IsValid(); }
+
+   virtual Real LogLikelihood() = 0;   // return the loglikelihhod
+   Real LogLikelihood(const ColumnVector& X, bool wgx=true)
+      { Set(X); WG(wgx); return LogLikelihood(); }
+
+   virtual ReturnMatrix Derivatives() = 0;
+                                       // column vector of derivatives
+   virtual ReturnMatrix FI() = 0;      // Fisher Information matrix
+   virtual ~LL_D_FI() {}               // to keep gnu happy
+};
+
+// This is the class for doing the maximum likelihood estimation
+
+class MLE_D_FI : public FindMaximum2
+{
+   // these replace the corresponding functions in FindMaximum2
+   void Value(const ColumnVector&, bool, Real&, bool&);
+   bool NextPoint(ColumnVector&, Real&);
+   Real LastDerivative(const ColumnVector&);
+
+   // the things we need for the analysis
+   LL_D_FI& LL;                        // reference to log-likelihood
+   int Lim;                            // maximum number of iterations
+   Real Criterion;                     // convergence criterion
+   ColumnVector Derivs;                // for the derivatives
+   LowerTriangularMatrix LT;           // Cholesky decomposition of FI
+   SymmetricMatrix Covariance;
+   DiagonalMatrix SE;
+
+public:
+   MLE_D_FI(LL_D_FI& ll, int lim=1000, Real criterion=0.0001)
+      : LL(ll), Lim(lim), Criterion(criterion) {}
+   void Fit(ColumnVector& Parameters);
+   void GetStandardErrors(ColumnVector&);
+   void GetCorrelations(SymmetricMatrix&);
+
+private:
+   void MakeCovariance();
+};
+
+
+#ifdef use_namespace
+}
+#endif
+
+
+
+#endif
+
+// body file: newmatnl.cpp
+
+
+
+
Index: orxonox/trunk/src/util/newmat/newmatrc.h
===================================================================
--- orxonox/trunk/src/util/newmat/newmatrc.h	(revision 4565)
+++ orxonox/trunk/src/util/newmat/newmatrc.h	(revision 4565)
@@ -0,0 +1,170 @@
+//$$ newmatrc.h              definition file for row/column classes
+
+// Copyright (C) 1991,2,3,4,7: R B Davies
+
+#ifndef NEWMATRC_LIB
+#define NEWMATRC_LIB 0
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+#include "controlw.h"
+
+
+/************** classes MatrixRowCol, MatrixRow, MatrixCol *****************/
+
+// Used for accessing the rows and columns of matrices
+// All matrix classes must provide routines for calculating matrix rows and
+// columns. Assume rows can be found very efficiently.
+
+enum LSF { LoadOnEntry=1,StoreOnExit=2,DirectPart=4,StoreHere=8,HaveStore=16 };
+
+
+class LoadAndStoreFlag : public ControlWord
+{
+public:
+   LoadAndStoreFlag() {}
+   LoadAndStoreFlag(int i) : ControlWord(i) {}
+   LoadAndStoreFlag(LSF lsf) : ControlWord(lsf) {}
+   LoadAndStoreFlag(const ControlWord& cwx) : ControlWord(cwx) {}
+};
+
+class MatrixRowCol
+// the row or column of a matrix
+{
+public:                                        // these are public to avoid
+                                               // numerous friend statements
+   int length;                                 // row or column length
+   int skip;                                   // initial number of zeros
+   int storage;                                // number of stored elements
+   int rowcol;                                 // row or column number
+   GeneralMatrix* gm;                          // pointer to parent matrix
+   Real* data;                                 // pointer to local storage
+   LoadAndStoreFlag cw;                        // Load? Store? Is a Copy?
+   void IncrMat() { rowcol++; data += storage; }   // used by NextRow
+   void IncrDiag() { rowcol++; skip++; data++; }
+   void IncrId() { rowcol++; skip++; }
+   void IncrUT() { rowcol++; data += storage; storage--; skip++; }
+   void IncrLT() { rowcol++; data += storage; storage++; }
+
+public:
+   void Zero();                                // set elements to zero
+   void Add(const MatrixRowCol&);              // add a row/col
+   void AddScaled(const MatrixRowCol&, Real);  // add a multiple of a row/col
+   void Add(const MatrixRowCol&, const MatrixRowCol&);
+                                               // add two rows/cols
+   void Add(const MatrixRowCol&, Real);        // add a row/col
+   void NegAdd(const MatrixRowCol&, Real);     // Real - a row/col
+   void Sub(const MatrixRowCol&);              // subtract a row/col
+   void Sub(const MatrixRowCol&, const MatrixRowCol&);
+					       // sub a row/col from another
+   void RevSub(const MatrixRowCol&);           // subtract from a row/col
+   void ConCat(const MatrixRowCol&, const MatrixRowCol&);
+                                               // concatenate two row/cols
+   void Multiply(const MatrixRowCol&);         // multiply a row/col
+   void Multiply(const MatrixRowCol&, const MatrixRowCol&);
+                                               // multiply two row/cols
+   void KP(const MatrixRowCol&, const MatrixRowCol&);
+                                               // Kronecker Product two row/cols
+   void Copy(const MatrixRowCol&);             // copy a row/col
+   void CopyCheck(const MatrixRowCol&);        // ... check for data loss
+   void Check(const MatrixRowCol&);            // just check for data loss
+   void Check();                               // check full row/col present
+   void Copy(const Real*&);                    // copy from an array
+   void Copy(Real);                            // copy from constant
+   void Add(Real);                             // add a constant
+   void Multiply(Real);                        // multiply by constant
+   Real SumAbsoluteValue();                    // sum of absolute values
+   Real MaximumAbsoluteValue1(Real r, int& i); // maximum of absolute values
+   Real MinimumAbsoluteValue1(Real r, int& i); // minimum of absolute values
+   Real Maximum1(Real r, int& i);              // maximum
+   Real Minimum1(Real r, int& i);              // minimum
+   Real Sum();                                 // sum of values
+   void Inject(const MatrixRowCol&);           // copy stored els of a row/col
+   void Negate(const MatrixRowCol&);           // change sign of a row/col
+   void Multiply(const MatrixRowCol&, Real);   // scale a row/col
+   friend Real DotProd(const MatrixRowCol&, const MatrixRowCol&);
+                                               // sum of pairwise product
+   Real* Data() { return data; }
+   int Skip() { return skip; }                 // number of elements skipped
+   int Storage() { return storage; }           // number of elements stored
+   int Length() { return length; }             // length of row or column
+   void Skip(int i) { skip=i; }
+   void Storage(int i) { storage=i; }
+   void Length(int i) { length=i; }
+   void SubRowCol(MatrixRowCol&, int, int) const;
+					       // get part of a row or column
+   MatrixRowCol() {}                           // to stop warning messages
+   ~MatrixRowCol();
+   FREE_CHECK(MatrixRowCol)
+};
+
+class MatrixRow : public MatrixRowCol
+{
+public:
+   // bodies for these are inline at the end of this .h file
+   MatrixRow(GeneralMatrix*, LoadAndStoreFlag, int=0);
+                                               // extract a row
+   ~MatrixRow();
+   void Next();                                // get next row
+   FREE_CHECK(MatrixRow)
+};
+
+class MatrixCol : public MatrixRowCol
+{
+public:
+   // bodies for these are inline at the end of this .h file
+   MatrixCol(GeneralMatrix*, LoadAndStoreFlag, int=0);
+                                               // extract a col
+   MatrixCol(GeneralMatrix*, Real*, LoadAndStoreFlag, int=0);
+                                               // store/retrieve a col
+   ~MatrixCol();
+   void Next();                                // get next row
+   FREE_CHECK(MatrixCol)
+};
+
+// MatrixColX is an alternative to MatrixCol where the complete
+// column is stored externally
+
+class MatrixColX : public MatrixRowCol
+{
+public:
+   // bodies for these are inline at the end of this .h file
+   MatrixColX(GeneralMatrix*, Real*, LoadAndStoreFlag, int=0);
+                                               // store/retrieve a col
+   ~MatrixColX();
+   void Next();                                // get next row
+   Real* store;                                // pointer to local storage
+                                               //    less skip
+   FREE_CHECK(MatrixColX)
+};
+
+/**************************** inline bodies ****************************/
+
+inline MatrixRow::MatrixRow(GeneralMatrix* gmx, LoadAndStoreFlag cwx, int row)
+{ gm=gmx; cw=cwx; rowcol=row; gm->GetRow(*this); }
+
+inline void MatrixRow::Next() { gm->NextRow(*this); }
+
+inline MatrixCol::MatrixCol(GeneralMatrix* gmx, LoadAndStoreFlag cwx, int col)
+{ gm=gmx; cw=cwx; rowcol=col; gm->GetCol(*this); }
+
+inline MatrixCol::MatrixCol(GeneralMatrix* gmx, Real* r,
+   LoadAndStoreFlag cwx, int col)
+{ gm=gmx; data=r; cw=cwx+StoreHere; rowcol=col; gm->GetCol(*this); }
+
+inline MatrixColX::MatrixColX(GeneralMatrix* gmx, Real* r,
+   LoadAndStoreFlag cwx, int col)
+{ gm=gmx; store=data=r; cw=cwx+StoreHere; rowcol=col; gm->GetCol(*this); }
+
+
+inline void MatrixCol::Next() { gm->NextCol(*this); }
+
+inline void MatrixColX::Next() { gm->NextCol(*this); }
+
+#ifdef use_namespace
+}
+#endif
+
+#endif
Index: orxonox/trunk/src/util/newmat/newmatrm.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/newmatrm.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/newmatrm.cpp	(revision 4565)
@@ -0,0 +1,189 @@
+//$$newmatrm.cpp                         rectangular matrix operations
+
+// Copyright (C) 1991,2,3,4: R B Davies
+
+
+
+#include "newmat.h"
+#include "newmatrm.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+#ifdef DO_REPORT
+#define REPORT { static ExeCounter ExeCount(__LINE__,12); ++ExeCount; }
+#else
+#define REPORT {}
+#endif
+
+
+// operations on rectangular matrices
+
+
+void RectMatrixRow::Reset (const Matrix& M, int row, int skip, int length)
+{
+   REPORT
+   RectMatrixRowCol::Reset
+      ( M.Store()+row*M.Ncols()+skip, length, 1, M.Ncols() );
+}
+
+void RectMatrixRow::Reset (const Matrix& M, int row)
+{
+   REPORT
+   RectMatrixRowCol::Reset( M.Store()+row*M.Ncols(), M.Ncols(), 1, M.Ncols() );
+}
+
+void RectMatrixCol::Reset (const Matrix& M, int skip, int col, int length)
+{
+   REPORT
+   RectMatrixRowCol::Reset
+      ( M.Store()+col+skip*M.Ncols(), length, M.Ncols(), 1 );
+}
+
+void RectMatrixCol::Reset (const Matrix& M, int col)
+{
+   REPORT
+   RectMatrixRowCol::Reset( M.Store()+col, M.Nrows(), M.Ncols(), 1 );
+}
+
+
+Real RectMatrixRowCol::SumSquare() const
+{
+   REPORT
+   long_Real sum = 0.0; int i = n; Real* s = store; int d = spacing;
+   // while (i--) { sum += (long_Real)*s * *s; s += d; }
+   if (i) for(;;)
+      { sum += (long_Real)*s * *s; if (!(--i)) break; s += d; }
+   return (Real)sum;
+}
+
+Real RectMatrixRowCol::operator*(const RectMatrixRowCol& rmrc) const
+{
+   REPORT
+   long_Real sum = 0.0; int i = n;
+   Real* s = store; int d = spacing;
+   Real* s1 = rmrc.store; int d1 = rmrc.spacing;
+   if (i!=rmrc.n)
+   {
+      Tracer tr("newmatrm");
+      Throw(InternalException("Dimensions differ in *"));
+   }
+   // while (i--) { sum += (long_Real)*s * *s1; s += d; s1 += d1; }
+   if (i) for(;;)
+      { sum += (long_Real)*s * *s1; if (!(--i)) break; s += d; s1 += d1; }
+   return (Real)sum;
+}
+
+void RectMatrixRowCol::AddScaled(const RectMatrixRowCol& rmrc, Real r)
+{
+   REPORT
+   int i = n; Real* s = store; int d = spacing;
+   Real* s1 = rmrc.store; int d1 = rmrc.spacing;
+   if (i!=rmrc.n)
+   {
+      Tracer tr("newmatrm");
+      Throw(InternalException("Dimensions differ in AddScaled"));
+   }
+   // while (i--) { *s += *s1 * r; s += d; s1 += d1; }
+   if (i) for (;;)
+      { *s += *s1 * r; if (!(--i)) break; s += d; s1 += d1; }
+}
+
+void RectMatrixRowCol::Divide(const RectMatrixRowCol& rmrc, Real r)
+{
+   REPORT
+   int i = n; Real* s = store; int d = spacing;
+   Real* s1 = rmrc.store; int d1 = rmrc.spacing;
+   if (i!=rmrc.n)
+   {
+      Tracer tr("newmatrm");
+      Throw(InternalException("Dimensions differ in Divide"));
+   }
+   // while (i--) { *s = *s1 / r; s += d; s1 += d1; }
+   if (i) for (;;) { *s = *s1 / r; if (!(--i)) break; s += d; s1 += d1; }
+}
+
+void RectMatrixRowCol::Divide(Real r)
+{
+   REPORT
+   int i = n; Real* s = store; int d = spacing;
+   // while (i--) { *s /= r; s += d; }
+   if (i) for (;;) { *s /= r; if (!(--i)) break; s += d; }
+}
+
+void RectMatrixRowCol::Negate()
+{
+   REPORT
+   int i = n; Real* s = store; int d = spacing;
+   // while (i--) { *s = - *s; s += d; }
+   if (i) for (;;) { *s = - *s; if (!(--i)) break; s += d; }
+}
+
+void RectMatrixRowCol::Zero()
+{
+   REPORT
+   int i = n; Real* s = store; int d = spacing;
+   // while (i--) { *s = 0.0; s += d; }
+   if (i) for (;;) { *s = 0.0; if (!(--i)) break; s += d; }
+}
+
+void ComplexScale(RectMatrixCol& U, RectMatrixCol& V, Real x, Real y)
+{
+   REPORT
+   int n = U.n;
+   if (n != V.n)
+   {
+      Tracer tr("newmatrm");
+      Throw(InternalException("Dimensions differ in ComplexScale"));
+   }
+   Real* u = U.store; Real* v = V.store; 
+   int su = U.spacing; int sv = V.spacing;
+   //while (n--)
+   //{
+   //   Real z = *u * x - *v * y;  *v =  *u * y + *v * x;  *u = z;
+   //   u += su;  v += sv;
+   //}
+   if (n) for (;;)
+   {
+      Real z = *u * x - *v * y;  *v =  *u * y + *v * x;  *u = z;
+      if (!(--n)) break;
+      u += su;  v += sv;
+   }
+}
+
+void Rotate(RectMatrixCol& U, RectMatrixCol& V, Real tau, Real s)
+{
+   REPORT
+   //  (U, V) = (U, V) * (c, s)  where  tau = s/(1+c), c^2 + s^2 = 1
+   int n = U.n;
+   if (n != V.n)
+   {
+      Tracer tr("newmatrm");
+      Throw(InternalException("Dimensions differ in Rotate"));
+   }
+   Real* u = U.store; Real* v = V.store;
+   int su = U.spacing; int sv = V.spacing;
+   //while (n--)
+   //{
+   //   Real zu = *u; Real zv = *v;
+   //   *u -= s * (zv + zu * tau); *v += s * (zu - zv * tau);
+   //   u += su;  v += sv;
+   //}
+   if (n) for(;;)
+   {
+      Real zu = *u; Real zv = *v;
+      *u -= s * (zv + zu * tau); *v += s * (zu - zv * tau);
+      if (!(--n)) break;
+      u += su;  v += sv;
+   }
+}
+
+
+
+
+#ifdef use_namespace
+}
+#endif
+
+
Index: orxonox/trunk/src/util/newmat/newmatrm.h
===================================================================
--- orxonox/trunk/src/util/newmat/newmatrm.h	(revision 4565)
+++ orxonox/trunk/src/util/newmat/newmatrm.h	(revision 4565)
@@ -0,0 +1,122 @@
+//$$newmatrm.h                            rectangular matrix operations
+
+// Copyright (C) 1991,2,3,4: R B Davies
+
+#ifndef NEWMATRM_LIB
+#define NEWMATRM_LIB 0
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+// operations on rectangular matrices
+
+class RectMatrixCol;
+
+class RectMatrixRowCol
+// a class for accessing rows and columns of rectangular matrices
+{
+protected:
+#ifdef use_namespace              // to make namespace work
+public:
+#endif
+   Real* store;                   // pointer to storage
+   int n;                         // number of elements
+   int spacing;                   // space between elements
+   int shift;                     // space between cols or rows
+   RectMatrixRowCol(Real* st, int nx, int sp, int sh)
+      : store(st), n(nx), spacing(sp), shift(sh) {}
+   void Reset(Real* st, int nx, int sp, int sh)
+      { store=st; n=nx; spacing=sp; shift=sh; }
+public:
+   Real operator*(const RectMatrixRowCol&) const;         // dot product
+   void AddScaled(const RectMatrixRowCol&, Real);         // add scaled
+   void Divide(const RectMatrixRowCol&, Real);            // scaling
+   void Divide(Real);                                     // scaling
+   void Negate();                                         // change sign
+   void Zero();                                           // zero row col
+   Real& operator[](int i) { return *(store+i*spacing); } // element
+   Real SumSquare() const;                                // sum of squares
+   Real& First() { return *store; }                       // get first element
+   void DownDiag() { store += (shift+spacing); n--; }
+   void UpDiag() { store -= (shift+spacing); n++; }
+   friend void ComplexScale(RectMatrixCol&, RectMatrixCol&, Real, Real);
+   friend void Rotate(RectMatrixCol&, RectMatrixCol&, Real, Real);
+   FREE_CHECK(RectMatrixRowCol)
+};
+
+class RectMatrixRow : public RectMatrixRowCol
+{
+public:
+   RectMatrixRow(const Matrix&, int, int, int);
+   RectMatrixRow(const Matrix&, int);
+   void Reset(const Matrix&, int, int, int);
+   void Reset(const Matrix&, int);
+   Real& operator[](int i) { return *(store+i); }
+   void Down() { store += shift; }
+   void Right() { store++; n--; }
+   void Up() { store -= shift; }
+   void Left() { store--; n++; }
+   FREE_CHECK(RectMatrixRow)
+};
+
+class RectMatrixCol : public RectMatrixRowCol
+{
+public:
+   RectMatrixCol(const Matrix&, int, int, int);
+   RectMatrixCol(const Matrix&, int);
+   void Reset(const Matrix&, int, int, int);
+   void Reset(const Matrix&, int);
+   void Down() { store += spacing; n--; }
+   void Right() { store++; }
+   void Up() { store -= spacing; n++; }
+   void Left() { store--; }
+   friend void ComplexScale(RectMatrixCol&, RectMatrixCol&, Real, Real);
+   friend void Rotate(RectMatrixCol&, RectMatrixCol&, Real, Real);
+   FREE_CHECK(RectMatrixCol)
+};
+
+class RectMatrixDiag : public RectMatrixRowCol
+{
+public:
+   RectMatrixDiag(const DiagonalMatrix& D)
+      : RectMatrixRowCol(D.Store(), D.Nrows(), 1, 1) {}
+   Real& operator[](int i) { return *(store+i); }
+   void DownDiag() { store++; n--; }
+   void UpDiag() { store--; n++; }
+   FREE_CHECK(RectMatrixDiag)
+};
+
+
+inline RectMatrixRow::RectMatrixRow
+   (const Matrix& M, int row, int skip, int length)
+   : RectMatrixRowCol( M.Store()+row*M.Ncols()+skip, length, 1, M.Ncols() ) {}
+
+inline RectMatrixRow::RectMatrixRow (const Matrix& M, int row)
+   : RectMatrixRowCol( M.Store()+row*M.Ncols(), M.Ncols(), 1, M.Ncols() ) {}
+
+inline RectMatrixCol::RectMatrixCol
+   (const Matrix& M, int skip, int col, int length)
+   : RectMatrixRowCol( M.Store()+col+skip*M.Ncols(), length, M.Ncols(), 1 ) {}
+
+inline RectMatrixCol::RectMatrixCol (const Matrix& M, int col)
+   : RectMatrixRowCol( M.Store()+col, M.Nrows(), M.Ncols(), 1 ) {}
+
+inline Real square(Real x) { return x*x; }
+inline Real sign(Real x, Real y)
+   { return (y>=0) ? x : -x; }                    // assume x >=0
+
+
+
+
+
+
+#ifdef use_namespace
+}
+#endif
+
+#endif
+
+// body file: newmatrm.cpp
+
+
Index: orxonox/trunk/src/util/newmat/nl_ex.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/nl_ex.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/nl_ex.cpp	(revision 4565)
@@ -0,0 +1,93 @@
+// This is an example of a non-linear least squares fit. The example
+// is from "Nonlinear estimation" by Gavin Ross (Springer,1990), p 63.
+// There are better ways of doing the fit in this case so this
+// example is just an example.
+
+// The model is E(y) = a + b exp(-kx) and there are 6 data points.
+
+#define WANT_STREAM
+#define WANT_MATH
+#include "newmatnl.h"
+#include "newmatio.h"
+
+#ifdef use_namespace
+using namespace RBD_LIBRARIES;
+#endif
+
+
+// first define the class describing the predictor function
+
+class Model_3pe : public R1_Col_I_D
+{
+   ColumnVector x_values;         // the values of "x"
+   RowVector deriv;               // values of derivatives
+public:
+   Model_3pe(const ColumnVector& X_Values)
+      : x_values(X_Values) { deriv.ReSize(3); }
+											 // load X data
+   Real operator()(int);
+   bool IsValid() { return para(3)>0; }
+                                  // require "k" > 0
+   ReturnMatrix Derivatives() { return deriv; }
+};
+
+Real Model_3pe::operator()(int i)
+{
+   Real a = para(1); Real b = para(2); Real k = para(3);
+   Real xvi = x_values(i);
+   Real e = exp(-k * xvi);
+   deriv(1) = 1.0;                    // calculate derivatives
+   deriv(2) = e;
+   deriv(3) = - b * e * xvi;
+   return a + b * e;                  // function value
+}
+
+int main()
+{
+   {
+      // Get the data
+      ColumnVector X(6);
+      ColumnVector Y(6);
+      X << 1   << 2   <<  3   <<  4   <<  6   <<  8;
+      Y << 3.2 << 7.9 << 11.1 << 14.5 << 16.7 << 18.3;
+
+
+      // Do the fit
+      Model_3pe model(X);                // the model object
+      NonLinearLeastSquares NLLS(model); // the non-linear least squares
+                                         // object
+      ColumnVector Para(3);              // for the parameters
+      Para << 9 << -6 << .5;             // trial values of parameters
+      cout << "Fitting parameters\n";
+      NLLS.Fit(Y,Para);                  // do the fit
+
+      // Inspect the results
+      ColumnVector SE;                   // for the standard errors
+      NLLS.GetStandardErrors(SE);
+      cout << "\n\nEstimates and standard errors\n" <<
+         setw(10) << setprecision(2) << (Para | SE) << endl;
+      Real ResidualSD = sqrt(NLLS.ResidualVariance());
+      cout << "\nResidual s.d. = " << setw(10) << setprecision(2) <<
+         ResidualSD << endl;
+      SymmetricMatrix Correlations;
+      NLLS.GetCorrelations(Correlations);
+      cout << "\nCorrelationMatrix\n" <<
+         setw(10) << setprecision(2) << Correlations << endl;
+      ColumnVector Residuals;
+      NLLS.GetResiduals(Residuals);
+      DiagonalMatrix Hat;
+      NLLS.GetHatDiagonal(Hat);
+      cout << "\nX, Y, Residual, Hat\n" << setw(10) << setprecision(2) <<
+         (X | Y | Residuals | Hat.AsColumn()) << endl;
+      // recover var/cov matrix
+      SymmetricMatrix D;
+      D << SE.AsDiagonal() * Correlations * SE.AsDiagonal();
+      cout << "\nVar/cov\n" << setw(14) << setprecision(4) << D << endl;
+   }
+
+#ifdef DO_FREE_CHECK
+   FreeCheck::Status();
+#endif
+ 
+   return 0;
+}
Index: orxonox/trunk/src/util/newmat/precisio.h
===================================================================
--- orxonox/trunk/src/util/newmat/precisio.h	(revision 4565)
+++ orxonox/trunk/src/util/newmat/precisio.h	(revision 4565)
@@ -0,0 +1,241 @@
+//$$ precisio.h                          floating point constants
+
+#ifndef PRECISION_LIB
+#define PRECISION_LIB 0
+
+#ifdef _STANDARD_                 // standard library available
+#include <limits>
+#endif
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+#ifdef _STANDARD_                 // standard library available
+
+using namespace std;
+	
+class FloatingPointPrecision
+{
+public:
+   static int Dig()              // number of decimal digits or precision
+      { return numeric_limits<Real>::digits10 ; }
+
+   static Real Epsilon()         // smallest number such that 1+Eps!=Eps
+      { return numeric_limits<Real>::epsilon(); }
+
+   static int Mantissa()         // bits in mantisa
+      { return numeric_limits<Real>::digits; }
+
+   static Real Maximum()         // maximum value
+      { return numeric_limits<Real>::max(); }
+
+   static int MaximumDecimalExponent()  // maximum decimal exponent
+      { return numeric_limits<Real>::max_exponent10; }
+
+   static int MaximumExponent()  // maximum binary exponent
+      { return numeric_limits<Real>::max_exponent; }
+
+   static Real LnMaximum()       // natural log of maximum
+      { return (Real)log(Maximum()); }
+
+   static Real Minimum()         // minimum positive value
+      { return numeric_limits<Real>::min(); } 
+
+   static int MinimumDecimalExponent() // minimum decimal exponent
+      { return numeric_limits<Real>::min_exponent10; }
+
+   static int MinimumExponent()  // minimum binary exponent
+      { return numeric_limits<Real>::min_exponent; }
+
+   static Real LnMinimum()       // natural log of minimum
+      { return (Real)log(Minimum()); }
+
+   static int Radix()            // exponent radix
+      { return numeric_limits<Real>::radix; }
+
+   static int Rounds()           // addition rounding (1 = does round)
+   {
+	  return numeric_limits<Real>::round_style ==
+		 round_to_nearest ? 1 : 0;
+   }
+
+};
+
+
+#else                              // _STANDARD_ not defined
+
+#ifndef SystemV                    // if there is float.h
+
+
+#ifdef USING_FLOAT
+
+
+class FloatingPointPrecision
+{
+public:
+   static int Dig()
+      { return FLT_DIG; }        // number of decimal digits or precision
+
+   static Real Epsilon()
+      { return FLT_EPSILON; }    // smallest number such that 1+Eps!=Eps
+
+   static int Mantissa()
+      { return FLT_MANT_DIG; }   // bits in mantisa
+
+   static Real Maximum()
+      { return FLT_MAX; }        // maximum value
+
+   static int MaximumDecimalExponent()
+      { return FLT_MAX_10_EXP; } // maximum decimal exponent
+
+   static int MaximumExponent()
+      { return FLT_MAX_EXP; }    // maximum binary exponent
+
+   static Real LnMaximum()
+      { return (Real)log(Maximum()); } // natural log of maximum
+
+   static Real Minimum()
+      { return FLT_MIN; }        // minimum positive value
+
+   static int MinimumDecimalExponent()
+      { return FLT_MIN_10_EXP; } // minimum decimal exponent
+
+   static int MinimumExponent()
+      { return FLT_MIN_EXP; }    // minimum binary exponent
+
+   static Real LnMinimum()
+      { return (Real)log(Minimum()); } // natural log of minimum
+
+   static int Radix()
+      { return FLT_RADIX; }      // exponent radix
+
+   static int Rounds()
+      { return FLT_ROUNDS; }     // addition rounding (1 = does round)
+
+};
+
+#endif                           // USING_FLOAT
+
+
+#ifdef USING_DOUBLE
+
+class FloatingPointPrecision
+{
+public:
+
+   static int Dig()
+      { return DBL_DIG; }        // number of decimal digits or precision
+
+   static Real Epsilon()
+      { return DBL_EPSILON; }    // smallest number such that 1+Eps!=Eps
+
+   static int Mantissa()
+      { return DBL_MANT_DIG; }   // bits in mantisa
+
+   static Real Maximum()
+      { return DBL_MAX; }        // maximum value
+
+   static int MaximumDecimalExponent()
+      { return DBL_MAX_10_EXP; } // maximum decimal exponent
+
+   static int MaximumExponent()
+      { return DBL_MAX_EXP; }    // maximum binary exponent
+
+   static Real LnMaximum()
+      { return (Real)log(Maximum()); } // natural log of maximum
+
+   static Real Minimum()
+   {
+//#ifdef __BCPLUSPLUS__
+//       return 2.225074e-308;     // minimum positive value
+//#else
+       return DBL_MIN;
+//#endif
+   }
+
+   static int MinimumDecimalExponent()
+      { return DBL_MIN_10_EXP; } // minimum decimal exponent
+
+   static int MinimumExponent()
+      { return DBL_MIN_EXP; }    // minimum binary exponent
+
+   static Real LnMinimum()
+      { return (Real)log(Minimum()); } // natural log of minimum
+
+
+   static int Radix()
+      { return FLT_RADIX; }      // exponent radix
+
+   static int Rounds()
+      { return FLT_ROUNDS; }     // addition rounding (1 = does round)
+
+};
+
+#endif                             // USING_DOUBLE
+
+#else                              // if there is no float.h
+
+#ifdef USING_FLOAT
+
+class FloatingPointPrecision
+{
+public:
+
+   static Real Epsilon()
+      { return pow(2.0,(int)(1-FSIGNIF)); }
+                                   // smallest number such that 1+Eps!=Eps
+
+   static Real Maximum()
+      { return MAXFLOAT; }            // maximum value
+
+   static Real LnMaximum()
+      { return (Real)log(Maximum()); }  // natural log of maximum
+
+   static Real Minimum()
+      { return MINFLOAT; }             // minimum positive value
+
+   static Real LnMinimum()
+      { return (Real)log(Minimum()); }  // natural log of minimum
+
+};
+
+#endif                                  // USING_FLOAT
+
+
+#ifdef USING_DOUBLE
+
+class FloatingPointPrecision
+{
+public:
+
+   static Real Epsilon()
+      { return pow(2.0,(int)(1-DSIGNIF)); }
+                                      // smallest number such that 1+Eps!=Eps
+
+   static Real Maximum()
+      { return MAXDOUBLE; }           // maximum value
+
+   static Real LnMaximum()
+      { return LN_MAXDOUBLE; }        // natural log of maximum
+
+   static Real Minimum()
+      { return MINDOUBLE; }
+
+   static Real LnMinimum()
+      { return LN_MINDOUBLE; }        // natural log of minimum
+};
+
+#endif                                // USING_DOUBLE
+
+#endif                                // SystemV
+
+#endif                                // _STANDARD_
+
+#ifdef use_namespace
+}
+#endif                                // use_namespace
+
+
+
+#endif                                // PRECISION_LIB
Index: orxonox/trunk/src/util/newmat/sl_ex.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/sl_ex.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/sl_ex.cpp	(revision 4565)
@@ -0,0 +1,33 @@
+// This is an example of the use of solution to find the cube root of 
+// the integers -10 to 10
+
+// you will need to compile and link solution.cpp and except.cpp
+
+#define WANT_STREAM
+#define WANT_MATH
+
+#include "include.h"
+#include "solution.h"
+
+#ifdef use_namespace
+using namespace RBD_LIBRARIES;
+#endif
+
+
+// the cube class
+
+class Cube : public R1_R1
+{ Real operator()() { return x*x*x; } };
+
+int main()
+{
+   // construct the Cube object
+   Cube cube;
+   // and then the solve object
+   OneDimSolve cube_root(cube);
+   // Now do the solves
+   for (int i=-10; i<=10; i++)
+      cout << i << "   "  << cube_root.Solve(i,0,1.5) << endl;
+   return 0;
+}
+
Index: orxonox/trunk/src/util/newmat/solution.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/solution.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/solution.cpp	(revision 4565)
@@ -0,0 +1,200 @@
+//$$ solution.cpp                    // solve routines
+
+// Copyright (C) 1994: R B Davies
+
+
+#define WANT_STREAM                  // include.h will get stream fns
+#define WANT_MATH                    // include.h will get math fns
+
+#include "include.h"
+#include "boolean.h"
+#include "myexcept.h"
+
+#include "solution.h"
+
+#ifdef use_namespace
+namespace RBD_COMMON {
+#endif
+
+
+void R1_R1::Set(Real X)
+{
+   if ((!minXinf && X <= minX) || (!maxXinf && X >= maxX))
+       Throw(SolutionException("X value out of range"));
+   x = X; xSet = true;
+}
+
+R1_R1::operator Real()
+{
+   if (!xSet) Throw(SolutionException("Value of X not set"));
+   Real y = operator()();
+   return y;
+}
+
+unsigned long SolutionException::Select;
+
+SolutionException::SolutionException(const char* a_what) : Exception()
+{
+   Select = Exception::Select;
+   AddMessage("Error detected by solution package\n");
+   AddMessage(a_what); AddMessage("\n");
+   if (a_what) Tracer::AddTrace();
+};
+
+inline Real square(Real x) { return x*x; }
+
+void OneDimSolve::LookAt(int V)
+{
+   lim--;
+   if (!lim) Throw(SolutionException("Does not converge"));
+   Last = V;
+   Real yy = function(x[V]) - YY;
+   Finish = (fabs(yy) <= accY) || (Captured && fabs(x[L]-x[U]) <= accX );
+   y[V] = vpol*yy;
+}
+
+void OneDimSolve::HFlip() { hpol=-hpol; State(U,C,L); }
+
+void OneDimSolve::VFlip()
+   { vpol = -vpol; y[0] = -y[0]; y[1] = -y[1]; y[2] = -y[2]; }
+
+void OneDimSolve::Flip()
+{
+   hpol=-hpol; vpol=-vpol; State(U,C,L);
+   y[0] = -y[0]; y[1] = -y[1]; y[2] = -y[2];
+}
+
+void OneDimSolve::State(int I, int J, int K) { L=I; C=J; U=K; }
+
+void OneDimSolve::Linear(int I, int J, int K)
+{
+   x[J] = (x[I]*y[K] - x[K]*y[I])/(y[K] - y[I]);
+   // cout << "Linear\n";
+}
+
+void OneDimSolve::Quadratic(int I, int J, int K)
+{
+   // result to overwrite I
+   Real YJK, YIK, YIJ, XKI, XKJ;
+   YJK = y[J] - y[K]; YIK = y[I] - y[K]; YIJ = y[I] - y[J];
+   XKI = (x[K] - x[I]);
+   XKJ = (x[K]*y[J] - x[J]*y[K])/YJK;
+   if ( square(YJK/YIK)>(x[K] - x[J])/XKI ||
+      square(YIJ/YIK)>(x[J] - x[I])/XKI )
+   {
+      x[I] = XKJ;
+      // cout << "Quadratic - exceptional\n";
+   }
+   else
+   {
+      XKI = (x[K]*y[I] - x[I]*y[K])/YIK;
+      x[I] = (XKJ*y[I] - XKI*y[J])/YIJ;
+      // cout << "Quadratic - normal\n";
+   }
+}
+
+Real OneDimSolve::Solve(Real Y, Real X, Real Dev, int Lim)
+{
+   enum Loop { start, captured1, captured2, binary, finish };
+   Tracer et("OneDimSolve::Solve");
+   lim=Lim; Captured = false;
+   if (Dev==0.0) Throw(SolutionException("Dev is zero"));
+   L=0; C=1; U=2; vpol=1; hpol=1; y[C]=0.0; y[U]=0.0;
+   if (Dev<0.0) { hpol=-1; Dev = -Dev; }
+   YY=Y;                                // target value
+   x[L] = X;                            // initial trial value
+   if (!function.IsValid(X))
+      Throw(SolutionException("Starting value is invalid"));
+   Loop TheLoop = start;
+   for (;;)
+   {
+      switch (TheLoop)
+      {
+      case start:
+         LookAt(L); if (Finish) { TheLoop = finish; break; }
+         if (y[L]>0.0) VFlip();               // so Y[L] < 0
+
+         x[U] = X + Dev * hpol;
+         if (!function.maxXinf && x[U] > function.maxX)
+            x[U] = (function.maxX + X) / 2.0;
+         if (!function.minXinf && x[U] < function.minX)
+            x[U] = (function.minX + X) / 2.0;
+
+         LookAt(U); if (Finish) { TheLoop = finish; break; }
+         if (y[U] > 0.0) { TheLoop = captured1; Captured = true; break; }
+         if (y[U] == y[L])
+            Throw(SolutionException("Function is flat"));
+         if (y[U] < y[L]) HFlip();             // Change direction
+         State(L,U,C);
+         for (i=0; i<20; i++)
+         {
+            // cout << "Searching for crossing point\n";
+            // Have L C then crossing point, Y[L]<Y[C]<0
+            x[U] = x[C] + Dev * hpol;
+            if (!function.maxXinf && x[U] > function.maxX)
+            x[U] = (function.maxX + x[C]) / 2.0;
+            if (!function.minXinf && x[U] < function.minX)
+            x[U] = (function.minX + x[C]) / 2.0;
+
+            LookAt(U); if (Finish) { TheLoop = finish; break; }
+            if (y[U] > 0) { TheLoop = captured2; Captured = true; break; }
+            if (y[U] < y[C])
+                Throw(SolutionException("Function is not monotone"));
+            Dev *= 2.0;
+            State(C,U,L);
+         }
+         if (TheLoop != start ) break;
+         Throw(SolutionException("Cannot locate a crossing point"));
+
+      case captured1:
+         // cout << "Captured - 1\n";
+         // We have 2 points L and U with crossing between them
+         Linear(L,C,U);                   // linear interpolation
+                                          // - result to C
+         LookAt(C); if (Finish) { TheLoop = finish; break; }
+         if (y[C] > 0.0) Flip();            // Want y[C] < 0
+         if (y[C] < 0.5*y[L]) { State(C,L,U); TheLoop = binary; break; }
+
+      case captured2:
+         // cout << "Captured - 2\n";
+         // We have L,C before crossing, U after crossing
+         Quadratic(L,C,U);                // quad interpolation
+                                          // - result to L
+         State(C,L,U);
+         if ((x[C] - x[L])*hpol <= 0.0 || (x[C] - x[U])*hpol >= 0.0)
+            { TheLoop = captured1; break; }
+         LookAt(C); if (Finish) { TheLoop = finish; break; }
+         // cout << "Through first stage\n";
+         if (y[C] > 0.0) Flip();
+         if (y[C] > 0.5*y[L]) { TheLoop = captured2; break; }
+         else { State(C,L,U); TheLoop = captured1; break; }
+
+      case binary:
+         // We have L, U around crossing - do binary search
+         // cout << "Binary\n";
+         for (i=3; i; i--)
+         {
+            x[C] = 0.5*(x[L]+x[U]);
+            LookAt(C); if (Finish) { TheLoop = finish; break; }
+            if (y[C]>0.0) State(L,U,C); else State(C,L,U);
+         }
+         if (TheLoop != binary) break;
+         TheLoop = captured1; break;
+
+      case finish:
+	 return x[Last];
+
+      }
+   }
+}
+
+bool R1_R1::IsValid(Real X)
+{
+   Set(X);
+   return (minXinf || x > minX) && (maxXinf || x < maxX);
+}
+
+#ifdef use_namespace
+}
+#endif
+
Index: orxonox/trunk/src/util/newmat/solution.h
===================================================================
--- orxonox/trunk/src/util/newmat/solution.h	(revision 4565)
+++ orxonox/trunk/src/util/newmat/solution.h	(revision 4565)
@@ -0,0 +1,94 @@
+//$$ solution.h                      // solve routines
+
+#include "boolean.h"
+#include "myexcept.h"
+
+#ifdef use_namespace
+namespace RBD_COMMON {
+#endif
+
+
+// Solve the equation f(x)=y for x where f is a monotone continuous
+// function of x
+// Essentially Brent s method
+
+// You need to derive a class from R1_R1 and override "operator()"
+// with the function you want to solve.
+// Use an object from this class in OneDimSolve
+
+class R1_R1
+{
+   // the prototype for a Real function of a Real variable
+   // you need to derive your function from this one and put in your
+   // function for operator() at least. You probably also want to set up a
+   // constructor to put in additional parameter values (e.g. that will not
+   // vary during a solve)
+
+protected:
+   Real x;                             // Current x value
+   bool xSet;                          // true if a value assigned to x
+
+public:
+   Real minX, maxX;                    // range of value x
+   bool minXinf, maxXinf;              // true if these are infinite
+   R1_R1() : xSet(false), minXinf(true), maxXinf(true) {}
+   virtual Real operator()() = 0;      // function value at current x
+                                       // set current x
+   virtual void Set(Real X);           // set x, check OK
+   Real operator()(Real X) { Set(X); return operator()(); }
+                                       // set x, return value
+   virtual bool IsValid(Real X);
+   operator Real();                    // implicit conversion
+};
+
+class SolutionException : public Exception
+{
+public:
+   static unsigned long Select;
+   SolutionException(const char* a_what = 0);
+};
+
+class OneDimSolve
+{
+   R1_R1& function;                     // reference to the function
+   Real accX;                           // accuracy in X direction
+   Real accY;                           // accuracy in Y direction
+   int lim;                             // maximum number of iterations
+
+public:
+   OneDimSolve(R1_R1& f, Real AccY = 0.0001, Real AccX = 0.0)
+      : function(f), accX(AccX), accY(AccY) {}
+                       // f is an R1_R1 function
+   Real Solve(Real Y, Real X, Real Dev, int Lim=100);
+                       // Solve for x in Y=f(x)
+                       // X is the initial trial value of x
+                       // X+Dev is the second trial value
+                       // program returns a value of x such that
+                       // |Y-f(x)| <= accY or |f.inv(Y)-x| <= accX
+
+private:
+   Real x[3], y[3];                         // Trial values of X and Y
+   int L,C,U,Last;                          // Locations of trial values
+   int vpol, hpol;                          // polarities
+   Real YY;                                 // target value
+   int i;
+   void LookAt(int);                        // get new value of function
+   bool Finish;                             // true if LookAt finds conv.
+   bool Captured;                           // true when target surrounded
+   void VFlip();
+   void HFlip();
+   void Flip();
+   void State(int I, int J, int K);
+   void Linear(int, int, int);
+   void Quadratic(int, int, int);
+};
+
+
+#ifdef use_namespace
+}
+#endif
+
+// body file: solution.cpp
+
+
+
Index: orxonox/trunk/src/util/newmat/sort.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/sort.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/sort.cpp	(revision 4565)
@@ -0,0 +1,272 @@
+//$$ sort.cpp                            Sorting
+
+// Copyright (C) 1991,2,3,4: R B Davies
+
+#define WANT_MATH
+
+#include "include.h"
+
+#include "newmatap.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+#ifdef DO_REPORT
+#define REPORT { static ExeCounter ExeCount(__LINE__,13); ++ExeCount; }
+#else
+#define REPORT {}
+#endif
+
+/******************************** Quick sort ********************************/
+
+// Quicksort.
+// Essentially the method described in Sedgewick s algorithms in C++
+// My version is still partially recursive, unlike Segewick s, but the
+// smallest segment of each split is used in the recursion, so it should
+// not overlead the stack.
+
+// If the process does not seems to be converging an exception is thrown.
+
+
+#define DoSimpleSort 17            // when to switch to insert sort
+#define MaxDepth 50                // maximum recursion depth
+
+static void MyQuickSortDescending(Real* first, Real* last, int depth);
+static void InsertionSortDescending(Real* first, const int length,
+   int guard);
+static Real SortThreeDescending(Real* a, Real* b, Real* c);
+
+static void MyQuickSortAscending(Real* first, Real* last, int depth);
+static void InsertionSortAscending(Real* first, const int length,
+   int guard);
+
+
+void SortDescending(GeneralMatrix& GM)
+{
+   REPORT
+   Tracer et("QuickSortDescending");
+
+   Real* data = GM.Store(); int max = GM.Storage();
+
+   if (max > DoSimpleSort) MyQuickSortDescending(data, data + max - 1, 0);
+   InsertionSortDescending(data, max, DoSimpleSort);
+
+}
+
+static Real SortThreeDescending(Real* a, Real* b, Real* c)
+{
+   // sort *a, *b, *c; return *b; optimise for already sorted
+   if (*a >= *b)
+   {
+      if (*b >= *c) { REPORT return *b; }
+      else if (*a >= *c) { REPORT Real x = *c; *c = *b; *b = x; return x; }
+      else { REPORT Real x = *a; *a = *c; *c = *b; *b = x; return x; }
+   }
+   else if (*c >= *b) { REPORT Real x = *c; *c = *a; *a = x; return *b; }
+   else if (*a >= *c) { REPORT Real x = *a; *a = *b; *b = x; return x; }
+   else { REPORT Real x = *c; *c = *a; *a = *b; *b = x; return x; }
+}
+
+static void InsertionSortDescending(Real* first, const int length,
+   int guard)
+// guard gives the length of the sequence to scan to find first
+// element (eg = length)
+{
+   REPORT
+   if (length <= 1) return;
+
+   // scan for first element
+   Real* f = first; Real v = *f; Real* h = f;
+   if (guard > length) { REPORT guard = length; }
+   int i = guard - 1;
+   while (i--) if (v < *(++f)) { v = *f; h = f; }
+   *h = *first; *first = v;
+
+   // do the sort
+   i = length - 1; f = first;
+   while (i--)
+   {
+      Real* g = f++; h = f; v = *h;
+      while (*g < v) *h-- = *g--;
+      *h = v;
+   }
+}
+
+static void MyQuickSortDescending(Real* first, Real* last, int depth)
+{
+   REPORT
+   for (;;)
+   {
+      const int length = last - first + 1;
+      if (length < DoSimpleSort) { REPORT return; }
+      if (depth++ > MaxDepth)
+         Throw(ConvergenceException("QuickSortDescending fails: "));
+      Real* centre = first + length/2;
+      const Real test = SortThreeDescending(first, centre, last);
+      Real* f = first; Real* l = last;
+      for (;;)
+      {
+         while (*(++f) > test) {}
+         while (*(--l) < test) {}
+         if (l <= f) break;
+         const Real temp = *f; *f = *l; *l = temp;
+      }
+      if (f > centre)
+         { REPORT MyQuickSortDescending(l+1, last, depth); last = f-1; }
+      else { REPORT MyQuickSortDescending(first, f-1, depth); first = l+1; }
+   }
+}
+
+void SortAscending(GeneralMatrix& GM)
+{
+   REPORT
+   Tracer et("QuickSortAscending");
+
+   Real* data = GM.Store(); int max = GM.Storage();
+
+   if (max > DoSimpleSort) MyQuickSortAscending(data, data + max - 1, 0);
+   InsertionSortAscending(data, max, DoSimpleSort);
+
+}
+
+static void InsertionSortAscending(Real* first, const int length,
+   int guard)
+// guard gives the length of the sequence to scan to find first
+// element (eg guard = length)
+{
+   REPORT
+   if (length <= 1) return;
+
+   // scan for first element
+   Real* f = first; Real v = *f; Real* h = f;
+   if (guard > length) { REPORT guard = length; }
+   int i = guard - 1;
+   while (i--) if (v > *(++f)) { v = *f; h = f; }
+   *h = *first; *first = v;
+
+   // do the sort
+   i = length - 1; f = first;
+   while (i--)
+   {
+      Real* g = f++; h = f; v = *h;
+      while (*g > v) *h-- = *g--;
+      *h = v;
+   }
+}
+static void MyQuickSortAscending(Real* first, Real* last, int depth)
+{
+   REPORT
+   for (;;)
+   {
+      const int length = last - first + 1;
+      if (length < DoSimpleSort) { REPORT return; }
+      if (depth++ > MaxDepth)
+         Throw(ConvergenceException("QuickSortAscending fails: "));
+      Real* centre = first + length/2;
+      const Real test = SortThreeDescending(last, centre, first);
+      Real* f = first; Real* l = last;
+      for (;;)
+      {
+         while (*(++f) < test) {}
+         while (*(--l) > test) {}
+         if (l <= f) break;
+         const Real temp = *f; *f = *l; *l = temp;
+      }
+      if (f > centre)
+         { REPORT MyQuickSortAscending(l+1, last, depth); last = f-1; }
+      else { REPORT MyQuickSortAscending(first, f-1, depth); first = l+1; }
+   }
+}
+
+//********* sort diagonal matrix & rearrange matrix columns ****************
+
+// used by SVD
+
+// these are for sorting singular values - should be updated with faster
+// sorts that handle exchange of columns better
+// however time is probably not significant compared with SVD time
+
+void SortSV(DiagonalMatrix& D, Matrix& U, bool ascending)
+{
+   REPORT
+   Tracer trace("SortSV_DU");
+   int m = U.Nrows(); int n = U.Ncols();
+   if (n != D.Nrows()) Throw(IncompatibleDimensionsException(D,U));
+   Real* u = U.Store();
+   for (int i=0; i<n; i++)
+   {
+      int k = i; Real p = D.element(i);
+      if (ascending)
+      {
+         for (int j=i+1; j<n; j++)
+            { if (D.element(j) < p) { k = j; p = D.element(j); } }
+      }
+      else
+      {
+         for (int j=i+1; j<n; j++)
+         { if (D.element(j) > p) { k = j; p = D.element(j); } }
+      }
+      if (k != i)
+      {
+         D.element(k) = D.element(i); D.element(i) = p; int j = m;
+         Real* uji = u + i; Real* ujk = u + k;
+         if (j) for(;;)
+         {
+            p = *uji; *uji = *ujk; *ujk = p;
+            if (!(--j)) break;
+            uji += n; ujk += n;
+         }
+      }
+   }
+}
+
+void SortSV(DiagonalMatrix& D, Matrix& U, Matrix& V, bool ascending)
+{
+   REPORT
+   Tracer trace("SortSV_DUV");
+   int mu = U.Nrows(); int mv = V.Nrows(); int n = D.Nrows();
+   if (n != U.Ncols()) Throw(IncompatibleDimensionsException(D,U));
+   if (n != V.Ncols()) Throw(IncompatibleDimensionsException(D,V));
+   Real* u = U.Store(); Real* v = V.Store();
+   for (int i=0; i<n; i++)
+   {
+      int k = i; Real p = D.element(i);
+      if (ascending)
+      {
+         for (int j=i+1; j<n; j++)
+            { if (D.element(j) < p) { k = j; p = D.element(j); } }
+      }
+      else
+      {
+         for (int j=i+1; j<n; j++)
+         { if (D.element(j) > p) { k = j; p = D.element(j); } }
+      }
+      if (k != i)
+      {
+         D.element(k) = D.element(i); D.element(i) = p;
+         Real* uji = u + i; Real* ujk = u + k;
+         Real* vji = v + i; Real* vjk = v + k;
+         int j = mu;
+         if (j) for(;;)
+         {
+            p = *uji; *uji = *ujk; *ujk = p; if (!(--j)) break;
+            uji += n; ujk += n;
+         }
+         j = mv;
+         if (j) for(;;)
+         {
+            p = *vji; *vji = *vjk; *vjk = p; if (!(--j)) break;
+            vji += n; vjk += n;
+         }
+      }
+   }
+}
+
+
+
+
+#ifdef use_namespace
+}
+#endif
+
Index: orxonox/trunk/src/util/newmat/submat.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/submat.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/submat.cpp	(revision 4565)
@@ -0,0 +1,429 @@
+//$$ submat.cpp                         submatrices
+
+// Copyright (C) 1991,2,3,4: R B Davies
+
+#include "include.h"
+
+#include "newmat.h"
+#include "newmatrc.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+#ifdef DO_REPORT
+#define REPORT { static ExeCounter ExeCount(__LINE__,11); ++ExeCount; }
+#else
+#define REPORT {}
+#endif
+
+
+/****************************** submatrices *********************************/
+
+#ifdef TEMPS_DESTROYED_QUICKLY
+GetSubMatrix& BaseMatrix::SubMatrix(int first_row, int last_row, int first_col,
+   int last_col) const
+#else
+GetSubMatrix BaseMatrix::SubMatrix(int first_row, int last_row, int first_col,
+   int last_col) const
+#endif
+{
+   REPORT
+   Tracer tr("SubMatrix");
+   int a = first_row - 1; int b = last_row - first_row + 1;
+   int c = first_col - 1; int d = last_col - first_col + 1;
+   if (a<0 || b<0 || c<0 || d<0) Throw(SubMatrixDimensionException());
+                             // allow zero rows or columns
+#ifdef TEMPS_DESTROYED_QUICKLY
+   GetSubMatrix* x = new GetSubMatrix(this, a, b, c, d, false);
+   MatrixErrorNoSpace(x);
+   return *x;
+#else
+   return GetSubMatrix(this, a, b, c, d, false);
+#endif
+}
+
+#ifdef TEMPS_DESTROYED_QUICKLY
+GetSubMatrix& BaseMatrix::SymSubMatrix(int first_row, int last_row) const
+#else
+GetSubMatrix BaseMatrix::SymSubMatrix(int first_row, int last_row) const
+#endif
+{
+   REPORT
+   Tracer tr("SubMatrix(symmetric)");
+   int a = first_row - 1; int b = last_row - first_row + 1;
+   if (a<0 || b<0) Throw(SubMatrixDimensionException());
+                             // allow zero rows or columns
+#ifdef TEMPS_DESTROYED_QUICKLY
+   GetSubMatrix* x = new GetSubMatrix(this, a, b, a, b, true);
+   MatrixErrorNoSpace(x);
+   return *x;
+#else
+   return GetSubMatrix( this, a, b, a, b, true);
+#endif
+}
+
+#ifdef TEMPS_DESTROYED_QUICKLY
+GetSubMatrix& BaseMatrix::Row(int first_row) const
+#else
+GetSubMatrix BaseMatrix::Row(int first_row) const
+#endif
+{
+   REPORT
+   Tracer tr("SubMatrix(row)");
+   int a = first_row - 1;
+   if (a<0) Throw(SubMatrixDimensionException());
+#ifdef TEMPS_DESTROYED_QUICKLY
+   GetSubMatrix* x = new GetSubMatrix(this, a, 1, 0, -1, false);
+   MatrixErrorNoSpace(x);
+   return *x;
+#else
+   return GetSubMatrix(this, a, 1, 0, -1, false);
+#endif
+}
+
+#ifdef TEMPS_DESTROYED_QUICKLY
+GetSubMatrix& BaseMatrix::Rows(int first_row, int last_row) const
+#else
+GetSubMatrix BaseMatrix::Rows(int first_row, int last_row) const
+#endif
+{
+   REPORT
+   Tracer tr("SubMatrix(rows)");
+   int a = first_row - 1; int b = last_row - first_row + 1;
+   if (a<0 || b<0) Throw(SubMatrixDimensionException());
+                             // allow zero rows or columns
+#ifdef TEMPS_DESTROYED_QUICKLY
+   GetSubMatrix* x = new GetSubMatrix(this, a, b, 0, -1, false);
+   MatrixErrorNoSpace(x);
+   return *x;
+#else
+   return GetSubMatrix(this, a, b, 0, -1, false);
+#endif
+}
+
+#ifdef TEMPS_DESTROYED_QUICKLY
+GetSubMatrix& BaseMatrix::Column(int first_col) const
+#else
+GetSubMatrix BaseMatrix::Column(int first_col) const
+#endif
+{
+   REPORT
+   Tracer tr("SubMatrix(column)");
+   int c = first_col - 1;
+   if (c<0) Throw(SubMatrixDimensionException());
+#ifdef TEMPS_DESTROYED_QUICKLY
+   GetSubMatrix* x = new GetSubMatrix(this, 0, -1, c, 1, false);
+   MatrixErrorNoSpace(x);
+   return *x;
+#else
+   return GetSubMatrix(this, 0, -1, c, 1, false);
+#endif
+}
+
+#ifdef TEMPS_DESTROYED_QUICKLY
+GetSubMatrix& BaseMatrix::Columns(int first_col, int last_col) const
+#else
+GetSubMatrix BaseMatrix::Columns(int first_col, int last_col) const
+#endif
+{
+   REPORT
+   Tracer tr("SubMatrix(columns)");
+   int c = first_col - 1; int d = last_col - first_col + 1;
+   if (c<0 || d<0) Throw(SubMatrixDimensionException());
+                             // allow zero rows or columns
+#ifdef TEMPS_DESTROYED_QUICKLY
+   GetSubMatrix* x = new GetSubMatrix(this, 0, -1, c, d, false);
+   MatrixErrorNoSpace(x);
+   return *x;
+#else
+   return GetSubMatrix(this, 0, -1, c, d, false);
+#endif
+}
+
+void GetSubMatrix::SetUpLHS()
+{
+   REPORT
+   Tracer tr("SubMatrix(LHS)");
+   const BaseMatrix* bm1 = bm;
+   GeneralMatrix* gm1 = ((BaseMatrix*&)bm)->Evaluate();
+   if ((BaseMatrix*)gm1!=bm1)
+      Throw(ProgramException("Invalid LHS"));
+   if (row_number < 0) row_number = gm1->Nrows();
+   if (col_number < 0) col_number = gm1->Ncols();
+   if (row_skip+row_number > gm1->Nrows()
+      || col_skip+col_number > gm1->Ncols())
+         Throw(SubMatrixDimensionException());
+}
+
+void GetSubMatrix::operator<<(const BaseMatrix& bmx)
+{
+   REPORT
+   Tracer tr("SubMatrix(<<)"); GeneralMatrix* gmx = 0;
+   Try
+   {
+      SetUpLHS(); gmx = ((BaseMatrix&)bmx).Evaluate();
+      if (row_number != gmx->Nrows() || col_number != gmx->Ncols())
+         Throw(IncompatibleDimensionsException());
+      MatrixRow mrx(gmx, LoadOnEntry);
+      MatrixRow mr(gm, LoadOnEntry+StoreOnExit+DirectPart, row_skip);
+                                     // do need LoadOnEntry
+      MatrixRowCol sub; int i = row_number;
+      while (i--)
+      {
+         mr.SubRowCol(sub, col_skip, col_number);   // put values in sub
+         sub.Copy(mrx); mr.Next(); mrx.Next();
+      }
+      gmx->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+   }
+
+   CatchAll
+   {
+      if (gmx) gmx->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+      ReThrow;
+   }
+}
+
+void GetSubMatrix::operator=(const BaseMatrix& bmx)
+{
+   REPORT
+   Tracer tr("SubMatrix(=)"); GeneralMatrix* gmx = 0;
+   // MatrixConversionCheck mcc;         // Check for loss of info
+   Try
+   {
+      SetUpLHS(); gmx = ((BaseMatrix&)bmx).Evaluate();
+      if (row_number != gmx->Nrows() || col_number != gmx->Ncols())
+         Throw(IncompatibleDimensionsException());
+      LoadAndStoreFlag lasf =
+         (  row_skip == col_skip
+            && gm->Type().IsSymmetric()
+            && gmx->Type().IsSymmetric() )
+        ? LoadOnEntry+DirectPart
+        : LoadOnEntry;
+      MatrixRow mrx(gmx, lasf);
+      MatrixRow mr(gm, LoadOnEntry+StoreOnExit+DirectPart, row_skip);
+                                     // do need LoadOnEntry
+      MatrixRowCol sub; int i = row_number;
+      while (i--)
+      {
+         mr.SubRowCol(sub, col_skip, col_number);   // put values in sub
+         sub.CopyCheck(mrx); mr.Next(); mrx.Next();
+      }
+      gmx->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+   }
+
+   CatchAll
+   {
+      if (gmx) gmx->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+      ReThrow;
+   }
+}
+
+void GetSubMatrix::operator<<(const Real* r)
+{
+   REPORT
+   Tracer tr("SubMatrix(<<Real*)");
+   SetUpLHS();
+   if (row_skip+row_number > gm->Nrows() || col_skip+col_number > gm->Ncols())
+      Throw(SubMatrixDimensionException());
+   MatrixRow mr(gm, LoadOnEntry+StoreOnExit+DirectPart, row_skip);
+                                  // do need LoadOnEntry
+   MatrixRowCol sub; int i = row_number;
+   while (i--)
+   {
+      mr.SubRowCol(sub, col_skip, col_number);   // put values in sub
+      sub.Copy(r); mr.Next();
+   }
+#ifdef TEMPS_DESTROYED_QUICKLY
+   delete this;
+#endif
+}
+
+void GetSubMatrix::operator=(Real r)
+{
+   REPORT
+   Tracer tr("SubMatrix(=Real)");
+   SetUpLHS();
+   MatrixRow mr(gm, LoadOnEntry+StoreOnExit+DirectPart, row_skip);
+                                  // do need LoadOnEntry
+   MatrixRowCol sub; int i = row_number;
+   while (i--)
+   {
+      mr.SubRowCol(sub, col_skip, col_number);   // put values in sub
+      sub.Copy(r); mr.Next();
+   }
+#ifdef TEMPS_DESTROYED_QUICKLY
+   delete this;
+#endif
+}
+
+void GetSubMatrix::Inject(const GeneralMatrix& gmx)
+{
+   REPORT
+   Tracer tr("SubMatrix(inject)");
+   SetUpLHS();
+   if (row_number != gmx.Nrows() || col_number != gmx.Ncols())
+      Throw(IncompatibleDimensionsException());
+   MatrixRow mrx((GeneralMatrix*)(&gmx), LoadOnEntry);
+   MatrixRow mr(gm, LoadOnEntry+StoreOnExit+DirectPart, row_skip);
+                                  // do need LoadOnEntry
+   MatrixRowCol sub; int i = row_number;
+   while (i--)
+   {
+      mr.SubRowCol(sub, col_skip, col_number);   // put values in sub
+      sub.Inject(mrx); mr.Next(); mrx.Next();
+   }
+#ifdef TEMPS_DESTROYED_QUICKLY
+   delete this;
+#endif
+}
+
+void GetSubMatrix::operator+=(const BaseMatrix& bmx)
+{
+   REPORT
+   Tracer tr("SubMatrix(+=)"); GeneralMatrix* gmx = 0;
+   // MatrixConversionCheck mcc;         // Check for loss of info
+   Try
+   {
+      SetUpLHS(); gmx = ((BaseMatrix&)bmx).Evaluate();
+      if (row_number != gmx->Nrows() || col_number != gmx->Ncols())
+         Throw(IncompatibleDimensionsException());
+      MatrixRow mrx(gmx, LoadOnEntry);
+      MatrixRow mr(gm, LoadOnEntry+StoreOnExit+DirectPart, row_skip);
+                                     // do need LoadOnEntry
+      MatrixRowCol sub; int i = row_number;
+      while (i--)
+      {
+         mr.SubRowCol(sub, col_skip, col_number);   // put values in sub
+         sub.Check(mrx);                            // check for loss of info
+         sub.Add(mrx); mr.Next(); mrx.Next();
+      }
+      gmx->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+   }
+
+   CatchAll
+   {
+      if (gmx) gmx->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+      ReThrow;
+   }
+}
+
+void GetSubMatrix::operator-=(const BaseMatrix& bmx)
+{
+   REPORT
+   Tracer tr("SubMatrix(-=)"); GeneralMatrix* gmx = 0;
+   // MatrixConversionCheck mcc;         // Check for loss of info
+   Try
+   {
+      SetUpLHS(); gmx = ((BaseMatrix&)bmx).Evaluate();
+      if (row_number != gmx->Nrows() || col_number != gmx->Ncols())
+         Throw(IncompatibleDimensionsException());
+      MatrixRow mrx(gmx, LoadOnEntry);
+      MatrixRow mr(gm, LoadOnEntry+StoreOnExit+DirectPart, row_skip);
+                                     // do need LoadOnEntry
+      MatrixRowCol sub; int i = row_number;
+      while (i--)
+      {
+         mr.SubRowCol(sub, col_skip, col_number);   // put values in sub
+         sub.Check(mrx);                            // check for loss of info
+         sub.Sub(mrx); mr.Next(); mrx.Next();
+      }
+      gmx->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+   }
+
+   CatchAll
+   {
+      if (gmx) gmx->tDelete();
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+      ReThrow;
+   }
+}
+
+void GetSubMatrix::operator+=(Real r)
+{
+   REPORT
+   Tracer tr("SubMatrix(+= or -= Real)");
+   // MatrixConversionCheck mcc;         // Check for loss of info
+   Try
+   {
+      SetUpLHS();
+      MatrixRow mr(gm, LoadOnEntry+StoreOnExit+DirectPart, row_skip);
+                                     // do need LoadOnEntry
+      MatrixRowCol sub; int i = row_number;
+      while (i--)
+      {
+         mr.SubRowCol(sub, col_skip, col_number);   // put values in sub
+         sub.Check();                               // check for loss of info
+         sub.Add(r); mr.Next();
+      }
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+   }
+
+   CatchAll
+   {
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+      ReThrow;
+   }
+}
+
+void GetSubMatrix::operator*=(Real r)
+{
+   REPORT
+   Tracer tr("SubMatrix(*= or /= Real)");
+   // MatrixConversionCheck mcc;         // Check for loss of info
+   Try
+   {
+      SetUpLHS();
+      MatrixRow mr(gm, LoadOnEntry+StoreOnExit+DirectPart, row_skip);
+                                     // do need LoadOnEntry
+      MatrixRowCol sub; int i = row_number;
+      while (i--)
+      {
+         mr.SubRowCol(sub, col_skip, col_number);   // put values in sub
+         sub.Multiply(r); mr.Next();
+      }
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+   }
+
+   CatchAll
+   {
+#ifdef TEMPS_DESTROYED_QUICKLY
+      delete this;
+#endif
+      ReThrow;
+   }
+}
+
+#ifdef use_namespace
+}
+#endif
+
Index: orxonox/trunk/src/util/newmat/svd.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/svd.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/svd.cpp	(revision 4565)
@@ -0,0 +1,227 @@
+//$$svd.cpp                           singular value decomposition
+
+// Copyright (C) 1991,2,3,4,5: R B Davies
+// Updated 17 July, 1995
+
+#define WANT_MATH
+
+#include "include.h"
+#include "newmatap.h"
+#include "newmatrm.h"
+#include "precisio.h"
+
+#ifdef use_namespace
+namespace NEWMAT {
+#endif
+
+#ifdef DO_REPORT
+#define REPORT { static ExeCounter ExeCount(__LINE__,15); ++ExeCount; }
+#else
+#define REPORT {}
+#endif
+
+
+static Real pythag(Real f, Real g, Real& c, Real& s)
+// return z=sqrt(f*f+g*g), c=f/z, s=g/z
+// set c=1,s=0 if z==0
+// avoid floating point overflow or divide by zero
+{
+   if (f==0 && g==0) { c=1.0; s=0.0; return 0.0; }
+   Real af = f>=0 ? f : -f;
+   Real ag = g>=0 ? g : -g;
+   if (ag<af)
+   {
+      REPORT
+      Real h = g/f; Real sq = sqrt(1.0+h*h);
+      if (f<0) sq = -sq;           // make return value non-negative
+      c = 1.0/sq; s = h/sq; return sq*f;
+   }
+   else
+   {
+      REPORT
+      Real h = f/g; Real sq = sqrt(1.0+h*h);
+      if (g<0) sq = -sq;
+      s = 1.0/sq; c = h/sq; return sq*g;
+   }
+}
+
+
+void SVD(const Matrix& A, DiagonalMatrix& Q, Matrix& U, Matrix& V,
+   bool withU, bool withV)
+// from Wilkinson and Reinsch: "Handbook of Automatic Computation"
+{
+   REPORT
+   Tracer trace("SVD");
+   Real eps = FloatingPointPrecision::Epsilon();
+   Real tol = FloatingPointPrecision::Minimum()/eps;
+
+   int m = A.Nrows(); int n = A.Ncols();
+   if (m<n)
+      Throw(ProgramException("Want no. Rows >= no. Cols", A));
+   if (withV && &U == &V)
+      Throw(ProgramException("Need different matrices for U and V", U, V));
+   U = A; Real g = 0.0; Real f,h; Real x = 0.0; int i;
+   RowVector E(n); RectMatrixRow EI(E,0); Q.ReSize(n);
+   RectMatrixCol UCI(U,0); RectMatrixRow URI(U,0,1,n-1);
+
+   if (n) for (i=0;;)
+   {
+      EI.First() = g; Real ei = g; EI.Right(); Real s = UCI.SumSquare();
+      if (s<tol) { REPORT Q.element(i) = 0.0; }
+      else
+      {
+         REPORT
+         f = UCI.First(); g = -sign(sqrt(s), f); h = f*g-s; UCI.First() = f-g;
+         Q.element(i) = g; RectMatrixCol UCJ = UCI; int j=n-i;
+         while (--j) { UCJ.Right(); UCJ.AddScaled(UCI, (UCI*UCJ)/h); }
+      }
+
+      s = URI.SumSquare();
+      if (s<tol) { REPORT g = 0.0; }
+      else
+      {
+         REPORT
+         f = URI.First(); g = -sign(sqrt(s), f); URI.First() = f-g;
+         EI.Divide(URI,f*g-s); RectMatrixRow URJ = URI; int j=m-i;
+         while (--j) { URJ.Down(); URJ.AddScaled(EI, URI*URJ); }
+      }
+
+      Real y = fabs(Q.element(i)) + fabs(ei); if (x<y) { REPORT x = y; }
+      if (++i == n) { REPORT break; }
+      UCI.DownDiag(); URI.DownDiag();
+   }
+
+   if (withV)
+   {
+      REPORT
+      V.ReSize(n,n); V = 0.0; RectMatrixCol VCI(V,n-1,n-1,1);
+      if (n) { VCI.First() = 1.0; g=E.element(n-1); if (n!=1) URI.UpDiag(); }
+      for (i=n-2; i>=0; i--)
+      {
+         VCI.Left();
+         if (g!=0.0)
+         {
+            VCI.Divide(URI, URI.First()*g); int j = n-i;
+            RectMatrixCol VCJ = VCI;
+            while (--j) { VCJ.Right(); VCJ.AddScaled( VCI, (URI*VCJ) ); }
+         }
+         VCI.Zero(); VCI.Up(); VCI.First() = 1.0; g=E.element(i);
+         if (i==0) break;
+         URI.UpDiag();
+      }
+   }
+
+   if (withU)
+   {
+      REPORT
+      for (i=n-1; i>=0; i--)
+      {
+         g = Q.element(i); URI.Reset(U,i,i+1,n-i-1); URI.Zero();
+         if (g!=0.0)
+         {
+            h=UCI.First()*g; int j=n-i; RectMatrixCol UCJ = UCI;
+            while (--j)
+            {
+               UCJ.Right(); UCI.Down(); UCJ.Down(); Real s = UCI*UCJ;
+               UCI.Up(); UCJ.Up(); UCJ.AddScaled(UCI,s/h);
+            }
+            UCI.Divide(g);
+         }
+         else UCI.Zero();
+         UCI.First() += 1.0;
+         if (i==0) break;
+         UCI.UpDiag();
+      }
+   }
+
+   eps *= x;
+   for (int k=n-1; k>=0; k--)
+   {
+      Real z = -FloatingPointPrecision::Maximum(); // to keep Gnu happy
+      Real y; int limit = 50; int l = 0;
+      while (limit--)
+      {
+         Real c, s; int i; int l1=k; bool tfc=false;
+         for (l=k; l>=0; l--)
+         {
+//          if (fabs(E.element(l))<=eps) goto test_f_convergence;
+            if (fabs(E.element(l))<=eps) { REPORT tfc=true; break; }
+            if (fabs(Q.element(l-1))<=eps) { REPORT l1=l; break; }
+            REPORT
+         }
+         if (!tfc)
+         {
+            REPORT
+            l=l1; l1=l-1; s = -1.0; c = 0.0;
+            for (i=l; i<=k; i++)
+            {
+               f = - s * E.element(i); E.element(i) *= c;
+//             if (fabs(f)<=eps) goto test_f_convergence;
+               if (fabs(f)<=eps) { REPORT break; }
+               g = Q.element(i); h = pythag(g,f,c,s); Q.element(i) = h;
+               if (withU)
+               {
+                  REPORT
+                  RectMatrixCol UCI(U,i); RectMatrixCol UCJ(U,l1);
+                  ComplexScale(UCJ, UCI, c, s);
+               }
+            }
+         }
+//       test_f_convergence: z = Q.element(k); if (l==k) goto convergence;
+         z = Q.element(k);  if (l==k) { REPORT break; }
+
+         x = Q.element(l); y = Q.element(k-1);
+         g = E.element(k-1); h = E.element(k);
+         f = ((y-z)*(y+z) + (g-h)*(g+h)) / (2*h*y);
+         if (f>1)         { REPORT g = f * sqrt(1 + square(1/f)); }
+         else if (f<-1)   { REPORT g = -f * sqrt(1 + square(1/f)); }
+         else             { REPORT g = sqrt(f*f + 1); }
+            { REPORT f = ((x-z)*(x+z) + h*(y / ((f<0.0) ? f-g : f+g)-h)) / x; }
+
+         c = 1.0; s = 1.0;
+         for (i=l+1; i<=k; i++)
+         {
+            g = E.element(i); y = Q.element(i); h = s*g; g *= c;
+            z = pythag(f,h,c,s); E.element(i-1) = z;
+            f = x*c + g*s; g = -x*s + g*c; h = y*s; y *= c;
+            if (withV)
+            {
+               REPORT
+               RectMatrixCol VCI(V,i); RectMatrixCol VCJ(V,i-1);
+               ComplexScale(VCI, VCJ, c, s);
+            }
+            z = pythag(f,h,c,s); Q.element(i-1) = z;
+            f = c*g + s*y; x = -s*g + c*y;
+            if (withU)
+            {
+               REPORT
+               RectMatrixCol UCI(U,i); RectMatrixCol UCJ(U,i-1);
+               ComplexScale(UCI, UCJ, c, s);
+            }
+         }
+         E.element(l) = 0.0; E.element(k) = f; Q.element(k) = x;
+      }
+      if (l!=k) { Throw(ConvergenceException(A)); }
+// convergence:
+      if (z < 0.0)
+      {
+         REPORT
+         Q.element(k) = -z;
+         if (withV) { RectMatrixCol VCI(V,k); VCI.Negate(); }
+      }
+   }
+   if (withU & withV) SortSV(Q, U, V);
+   else if (withU) SortSV(Q, U);
+   else if (withV) SortSV(Q, V);
+   else SortDescending(Q);
+}
+
+void SVD(const Matrix& A, DiagonalMatrix& D)
+{ REPORT Matrix U; SVD(A, D, U, U, false, false); }
+
+
+
+#ifdef use_namespace
+}
+#endif
+
Index: orxonox/trunk/src/util/newmat/test_exc.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/test_exc.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/test_exc.cpp	(revision 4565)
@@ -0,0 +1,186 @@
+#define WANT_STREAM
+
+#include "newmatap.h"
+#include "newmatio.h"              // to help namespace with VC++ 5
+
+#ifdef use_namespace
+using namespace RBD_LIBRARIES;
+#endif
+
+//#include <except.h>             // if you want to use set_terminate
+
+/**************************** test exceptions ******************************/
+
+
+int main()
+{
+   // activate the next expression if you want to use compiler supported
+   // exceptions and you want Terminate to catch uncaught exceptions
+   // set_terminate(Terminate);
+   Real* s1; Real* s2; Real* s3; Real* s4;
+   // Forces cout to allocate memory at beginning
+   cout << "\nThis tests the exception system, so you will get\n" <<
+      "a long list of error messages\n\n";
+   cout << "\nPrint a real number (may help lost memory test): " << 3.14159265 << "\n";
+   // Throw exception to set up exception buffer
+   Try { Throw(Exception("Just a dummy\n")); }
+   CatchAll {};
+   { Matrix A1(40,200); s1 = A1.Store(); }
+   { Matrix A1(1,1); s3 = A1.Store(); }
+   {
+      Tracer et("Test");
+
+      Try
+      {
+         Tracer et("Try block");
+
+
+
+         cout << "-----------------------------------------\n\n";
+         Matrix A(2,3), B(4,5); A = 1; B = 2;
+         cout << "Incompatible dimensions\n";
+         et.ReName("Block A");
+         Try { Matrix C = A + B; }
+         CatchAll { cout << Exception::what() << endl; }
+         cout << "-----------------------------------------\n\n";
+
+         cout << "Bad index\n";
+         et.ReName("Block B");
+         Try { Real f = A(3,3); cout << f << endl; }
+         CatchAll { cout << Exception::what() << endl; }
+         cout << "-----------------------------------------\n\n";
+
+         cout << "Illegal conversion\n";
+         et.ReName("Block C");
+         Try { UpperTriangularMatrix U = A; }
+         CatchAll { cout << Exception::what() << endl; }
+         cout << "-----------------------------------------\n\n";
+
+         cout << "Invert non-square matrix - 1\n";
+         et.ReName("Block D");
+         Try { CroutMatrix X = A; }
+         CatchAll { cout << Exception::what() << endl; }
+         cout << "-----------------------------------------\n\n";
+
+         cout << "Invert non-square matrix - 2\n";
+         et.ReName("Block E");
+         Try { Matrix X = A.i(); }
+         CatchAll { cout << Exception::what() << endl; }
+         cout << "-----------------------------------------\n\n";
+
+         cout << "Non 1x1 matrix to scalar\n";
+         et.ReName("Block F");
+         Try { Real f = A.AsScalar(); cout << f << endl; }
+         CatchAll { cout << Exception::what() << endl; }
+         cout << "-----------------------------------------\n\n";
+
+         cout << "Matrix to vector\n";
+         et.ReName("Block G");
+         Try { ColumnVector CV = A;}
+         CatchAll { cout << Exception::what() << endl; }
+         cout << "-----------------------------------------\n\n";
+
+         cout << "Invert singular matrix\n";
+         et.ReName("Block H");
+         Try { Matrix X(2,2); X<<1<<2<<2<<4; X = X.i(); }
+         CatchAll { cout << Exception::what() << endl; }
+         cout << "-----------------------------------------\n\n";
+
+         cout << "SubMatrix error\n";
+         et.ReName("Block I");
+         Try { Matrix X = A.Row(3); }
+         CatchAll { cout << Exception::what() << endl; }
+         cout << "-----------------------------------------\n\n";
+
+         cout << "SubMatrix error\n";
+         et.ReName("Block J");
+         Try { Matrix X = A.Row(0); }
+         CatchAll { cout << Exception::what() << endl; }
+         cout << "-----------------------------------------\n\n";
+
+         cout << "Cholesky error\n";
+         et.ReName("Block K");
+         Try
+         {
+            SymmetricMatrix SM(50); SM = 10;
+            LowerTriangularMatrix L = Cholesky(SM);
+         }
+         CatchAll { cout << Exception::what() << endl; }
+         cout << "-----------------------------------------\n\n";
+
+         cout << "Inequality error\n";
+         et.ReName("Block L");
+         Try
+         {
+            Matrix A(10,10), B(10,10); A = 10; B = 20;
+            if ( A < B) A = B;
+         }
+         CatchAll { cout << Exception::what() << endl; }
+         cout << "-----------------------------------------\n\n";
+
+         cout << "Maximum of empty matrix\n";
+         et.ReName("Block M");
+         Try
+         {
+            Matrix A(10,20); A = 5; Matrix B=A.Rows(6,5);
+            MaximumAbsoluteValue(B);
+         }
+         CatchAll { cout << Exception::what() << endl; }
+         cout << "-----------------------------------------\n\n";
+
+         cout << "Incorrectly ReSizing band matrix\n";
+         et.ReName("Block N");
+         Try
+         {
+            BandMatrix A(20,5,3); A = 5; UpperBandMatrix B;
+            B.ReSize(A);
+         }
+         CatchAll { cout << Exception::what() << endl; }
+         cout << "-----------------------------------------\n\n";
+
+         cout << "Incorrectly ReSizing symmetric band matrix\n";
+         et.ReName("Block M");
+         Try
+         {
+            BandMatrix A(20,5,3); A = 5; SymmetricBandMatrix B;
+            B.ReSize(A);
+         }
+         CatchAll { cout << Exception::what() << endl; }
+         cout << "-----------------------------------------\n\n";
+
+         cout << "ReSize CroutMatrix\n";
+         et.ReName("Block O");
+         Try
+         {
+            Matrix A(3,3); A = 0; A(1,1) = A(2,2) = A(3,3) = 1;
+            CroutMatrix B = A;;
+            B.ReSize(A);
+         }
+         CatchAll { cout << Exception::what() << endl; }
+         cout << "-----------------------------------------\n\n";
+
+      }
+      CatchAll { cout << "\nException generated in test program\n\n"; }
+   }
+
+   cout << "\nEnd test\n";
+   { Matrix A1(40,200); s2 = A1.Store(); }
+   cout << "\n(The following memory checks are probably not valid with all\n";
+   cout << "compilers - see documentation)\n";
+   cout << "\nChecking for lost memory: "
+      << (unsigned long)s1 << " " << (unsigned long)s2 << " ";
+   if (s1 != s2) cout << " - error\n"; else cout << " - ok\n";
+   { Matrix A1(1,1); s4 = A1.Store(); }
+   cout << "\nChecking for lost memory: "
+      << (unsigned long)s3 << " " << (unsigned long)s4 << " ";
+   if (s3 != s4) cout << " - error\n\n"; else cout << " - ok\n\n";
+
+
+#ifdef DO_FREE_CHECK
+   FreeCheck::Status();
+#endif
+
+//   Throw(Runtime_error("Exception outside try block"));
+
+   return 0;
+}
Index: orxonox/trunk/src/util/newmat/tmt.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmt.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmt.cpp	(revision 4565)
@@ -0,0 +1,426 @@
+#define WANT_STREAM
+
+#include "include.h"
+
+#include "newmat.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+
+/**************************** test program ******************************/
+
+
+class PrintCounter
+{
+   int count;
+   const char* s;
+public:
+   ~PrintCounter();
+   PrintCounter(const char * sx) : count(0), s(sx) {}
+   void operator++() { count++; }
+};
+
+PrintCounter PCZ("Number of non-zero matrices (should be 1) = ");
+PrintCounter PCN("Number of matrices tested                 = ");
+
+PrintCounter::~PrintCounter()
+{ cout << s << count << "\n"; }
+
+
+void Print(const Matrix& X)
+{
+   ++PCN;
+   cout << "\nMatrix type: " << X.Type().Value() << " (";
+   cout << X.Nrows() << ", ";
+   cout << X.Ncols() << ")\n\n";
+   if (X.IsZero()) { cout << "All elements are zero\n" << flush; return; }
+   int nr=X.Nrows(); int nc=X.Ncols();
+   for (int i=1; i<=nr; i++)
+   {
+      for (int j=1; j<=nc; j++)  cout << X(i,j) << "\t";
+      cout << "\n";
+   }
+   cout << flush; ++PCZ;
+}
+
+void Print(const UpperTriangularMatrix& X)
+{
+   ++PCN;
+   cout << "\nMatrix type: " << X.Type().Value() << " (";
+   cout << X.Nrows() << ", ";
+   cout << X.Ncols() << ")\n\n";
+   if (X.IsZero()) { cout << "All elements are zero\n" << flush; return; }
+   int nr=X.Nrows(); int nc=X.Ncols();
+   for (int i=1; i<=nr; i++)
+   {
+      int j;
+      for (j=1; j<i; j++) cout << "\t";
+      for (j=i; j<=nc; j++)  cout << X(i,j) << "\t";
+      cout << "\n";
+   }
+   cout << flush; ++PCZ;
+}
+
+void Print(const DiagonalMatrix& X)
+{
+   ++PCN;
+   cout << "\nMatrix type: " << X.Type().Value() << " (";
+   cout << X.Nrows() << ", ";
+   cout << X.Ncols() << ")\n\n";
+   if (X.IsZero()) { cout << "All elements are zero\n" << flush; return; }
+   int nr=X.Nrows(); int nc=X.Ncols();
+   for (int i=1; i<=nr; i++)
+   {
+      for (int j=1; j<i; j++) cout << "\t";
+      if (i<=nc) cout << X(i,i) << "\t";
+      cout << "\n";
+   }
+   cout << flush; ++PCZ;
+}
+
+void Print(const SymmetricMatrix& X)
+{
+   ++PCN;
+   cout << "\nMatrix type: " << X.Type().Value() << " (";
+   cout << X.Nrows() << ", ";
+   cout << X.Ncols() << ")\n\n";
+   if (X.IsZero()) { cout << "All elements are zero\n" << flush; return; }
+   int nr=X.Nrows(); int nc=X.Ncols();
+   for (int i=1; i<=nr; i++)
+   {
+      int j;
+      for (j=1; j<i; j++) cout << X(j,i) << "\t";
+      for (j=i; j<=nc; j++)  cout << X(i,j) << "\t";
+      cout << "\n";
+   }
+   cout << flush; ++PCZ;
+}
+
+void Print(const LowerTriangularMatrix& X)
+{
+   ++PCN;
+   cout << "\nMatrix type: " << X.Type().Value() << " (";
+   cout << X.Nrows() << ", ";
+   cout << X.Ncols() << ")\n\n";
+   if (X.IsZero()) { cout << "All elements are zero\n" << flush; return; }
+   int nr=X.Nrows();
+   for (int i=1; i<=nr; i++)
+   {
+      for (int j=1; j<=i; j++) cout << X(i,j) << "\t";
+      cout << "\n";
+   }
+   cout << flush; ++PCZ;
+}
+
+
+void Clean(Matrix& A, Real c)
+{
+   int nr = A.Nrows(); int nc = A.Ncols();
+   for (int i=1; i<=nr; i++)
+   {
+      for (int j=1; j<=nc; j++)
+      { Real a = A(i,j); if ((a < c) && (a > -c)) A(i,j) = 0.0; }
+   }
+}
+
+void Clean(DiagonalMatrix& A, Real c)
+{
+   int nr = A.Nrows();
+   for (int i=1; i<=nr; i++)
+   { Real a = A(i,i); if ((a < c) && (a > -c)) A(i,i) = 0.0; }
+}
+
+void PentiumCheck(Real N, Real D)
+{
+   Real R = N / D;
+   R = R * D - N;
+   if ( R > 1 || R < -1)
+      cout << "Pentium error detected: % error = " << 100 * R / N << "\n";
+}
+
+
+
+//*************************** main program **********************************
+
+void TestTypeAdd();                            // test +
+void TestTypeMult();                           // test *
+void TestTypeConcat();                         // test |
+void TestTypeSP();                             // test SP
+void TestTypeKP();                             // test KP
+void TestTypeOrder();                          // test >=
+
+
+int main()
+{
+   Real* s1; Real* s2; Real* s3; Real* s4;
+   cout << "\nBegin test\n";   // Forces cout to allocate memory at beginning
+   cout << "Now print a real number: " << 3.14159265 << endl;
+   // Throw exception to set up exception buffer
+#ifndef DisableExceptions
+   Try { Throw(Exception("Just a dummy\n")); }
+   CatchAll {}
+#else
+   cout << "Not doing exceptions\n";
+#endif
+   { Matrix A1(40,200); s1 = A1.Store(); }
+   { Matrix A1(1,1); s3 = A1.Store(); }
+   {
+      Tracer et("Matrix test program");
+
+      Matrix A(25,150);
+      {
+         int i;
+         RowVector A(8);
+         for (i=1;i<=7;i++) A(i)=0.0; A(8)=1.0;
+         Print(A);
+      }
+      cout << "\n";
+
+      TestTypeAdd(); TestTypeMult(); TestTypeConcat();
+      TestTypeSP(); TestTypeKP(); TestTypeOrder();
+
+
+      Try { 
+         trymat1();
+         trymat2();
+         trymat3();
+         trymat4();
+         trymat5();
+         trymat6();
+         trymat7();
+         trymat8();
+         trymat9();
+         trymata();
+         trymatb();
+         trymatc();
+         trymatd();
+         trymate();
+         trymatf();
+         trymatg();
+         trymath();
+         trymati();
+         trymatj();
+         trymatk();
+         trymatl();
+         trymatm();
+
+         cout << "\nEnd of tests\n";
+      }
+      CatchAll
+      {
+         cout << "\nTest program fails - exception generated\n\n";
+         cout << Exception::what();
+      }
+
+
+   }
+
+   { Matrix A1(40,200); s2 = A1.Store(); }
+   cout << "\n(The following memory checks are probably not valid with all\n";
+   cout << "compilers - see documentation)\n";
+   cout << "\nChecking for lost memory: "
+      << (unsigned long)s1 << " " << (unsigned long)s2 << " ";
+   if (s1 != s2) cout << " - error\n"; else cout << " - ok\n";
+   { Matrix A1(1,1); s4 = A1.Store(); }
+   cout << "\nChecking for lost memory: "
+      << (unsigned long)s3 << " " << (unsigned long)s4 << " ";
+   if (s3 != s4) cout << " - error\n\n"; else cout << " - ok\n\n";
+
+   // check for Pentium bug
+   PentiumCheck(4195835L,3145727L);
+   PentiumCheck(5244795L,3932159L);
+
+#ifdef DO_FREE_CHECK
+   FreeCheck::Status();
+#endif
+   return 0;
+}
+
+
+
+
+//************************ test type manipulation **************************/
+
+
+// These functions may cause problems for Glockenspiel 2.0c; they are used
+// only for testing so you can delete them
+
+
+void TestTypeAdd()
+{
+   MatrixType list[10];
+   list[0] = MatrixType::UT;
+   list[1] = MatrixType::LT;
+   list[2] = MatrixType::Rt;
+   list[3] = MatrixType::Sm;
+   list[4] = MatrixType::Dg;
+   list[5] = MatrixType::BM;
+   list[6] = MatrixType::UB;
+   list[7] = MatrixType::LB;
+   list[8] = MatrixType::SB;
+   list[9] = MatrixType::Id;
+
+   cout << "+     ";
+   int i;
+   for (i=0; i<MatrixType::nTypes(); i++) cout << list[i].Value() << " ";
+   cout << "\n";
+   for (i=0; i<MatrixType::nTypes(); i++)
+	{
+		cout << list[i].Value() << " ";
+      for (int j=0; j<MatrixType::nTypes(); j++)
+         cout << (list[j]+list[i]).Value() << " ";
+      cout << "\n";
+   }
+   cout << "\n";
+}
+
+void TestTypeMult()
+{
+   MatrixType list[10];
+   list[0] = MatrixType::UT;
+   list[1] = MatrixType::LT;
+   list[2] = MatrixType::Rt;
+   list[3] = MatrixType::Sm;
+   list[4] = MatrixType::Dg;
+   list[5] = MatrixType::BM;
+   list[6] = MatrixType::UB;
+   list[7] = MatrixType::LB;
+   list[8] = MatrixType::SB;
+   list[9] = MatrixType::Id;
+
+   cout << "*     ";
+   int i;
+   for (i=0; i<MatrixType::nTypes(); i++)
+      cout << list[i].Value() << " ";
+   cout << "\n";
+   for (i=0; i<MatrixType::nTypes(); i++)
+   {
+		cout << list[i].Value() << " ";
+      for (int j=0; j<MatrixType::nTypes(); j++)
+         cout << (list[j]*list[i]).Value() << " ";
+      cout << "\n";
+   }
+   cout << "\n";
+}
+
+void TestTypeConcat()
+{
+   MatrixType list[10];
+   list[0] = MatrixType::UT;
+   list[1] = MatrixType::LT;
+   list[2] = MatrixType::Rt;
+   list[3] = MatrixType::Sm;
+   list[4] = MatrixType::Dg;
+   list[5] = MatrixType::BM;
+   list[6] = MatrixType::UB;
+   list[7] = MatrixType::LB;
+   list[8] = MatrixType::SB;
+   list[9] = MatrixType::Id;
+
+   cout << "|     ";
+   int i;
+   for (i=0; i<MatrixType::nTypes(); i++)
+      cout << list[i].Value() << " ";
+   cout << "\n";
+   for (i=0; i<MatrixType::nTypes(); i++)
+   {
+		cout << list[i].Value() << " ";
+      for (int j=0; j<MatrixType::nTypes(); j++)
+         cout << (list[j] | list[i]).Value() << " ";
+      cout << "\n";
+   }
+   cout << "\n";
+}
+
+void TestTypeSP()
+{
+   MatrixType list[10];
+   list[0] = MatrixType::UT;
+   list[1] = MatrixType::LT;
+   list[2] = MatrixType::Rt;
+   list[3] = MatrixType::Sm;
+   list[4] = MatrixType::Dg;
+   list[5] = MatrixType::BM;
+   list[6] = MatrixType::UB;
+   list[7] = MatrixType::LB;
+   list[8] = MatrixType::SB;
+   list[9] = MatrixType::Id;
+
+   cout << "SP    ";
+   int i;
+   for (i=0; i<MatrixType::nTypes(); i++)
+		cout << list[i].Value() << " ";
+   cout << "\n";
+   for (i=0; i<MatrixType::nTypes(); i++)
+   {
+		cout << list[i].Value() << " ";
+      for (int j=0; j<MatrixType::nTypes(); j++)
+         cout << (list[j].SP(list[i])).Value() << " ";
+      cout << "\n";
+   }
+   cout << "\n";
+}
+
+void TestTypeKP()
+{
+   MatrixType list[10];
+   list[0] = MatrixType::UT;
+   list[1] = MatrixType::LT;
+   list[2] = MatrixType::Rt;
+   list[3] = MatrixType::Sm;
+   list[4] = MatrixType::Dg;
+   list[5] = MatrixType::BM;
+   list[6] = MatrixType::UB;
+   list[7] = MatrixType::LB;
+   list[8] = MatrixType::SB;
+   list[9] = MatrixType::Id;
+
+   cout << "KP    ";
+   int i;
+   for (i=0; i<MatrixType::nTypes(); i++)
+		cout << list[i].Value() << " ";
+   cout << "\n";
+   for (i=0; i<MatrixType::nTypes(); i++)
+   {
+		cout << list[i].Value() << " ";
+      for (int j=0; j<MatrixType::nTypes(); j++)
+         cout << (list[j].KP(list[i])).Value() << " ";
+      cout << "\n";
+   }
+   cout << "\n";
+}
+
+void TestTypeOrder()
+{
+   MatrixType list[10];
+   list[0] = MatrixType::UT;
+   list[1] = MatrixType::LT;
+   list[2] = MatrixType::Rt;
+   list[3] = MatrixType::Sm;
+   list[4] = MatrixType::Dg;
+   list[5] = MatrixType::BM;
+   list[6] = MatrixType::UB;
+   list[7] = MatrixType::LB;
+   list[8] = MatrixType::SB;
+   list[9] = MatrixType::Id;
+
+   cout << ">=    ";
+   int i;
+   for (i = 0; i<MatrixType::nTypes(); i++)
+      cout << list[i].Value() << " ";
+   cout << "\n";
+   for (i=0; i<MatrixType::nTypes(); i++)
+   {
+      cout << list[i].Value() << " ";
+      for (int j=0; j<MatrixType::nTypes(); j++)
+	 cout << ((list[j]>=list[i]) ? "Yes   " : "No    ");
+      cout << "\n";
+   }
+   cout << "\n";
+}
+
+
Index: orxonox/trunk/src/util/newmat/tmt.h
===================================================================
--- orxonox/trunk/src/util/newmat/tmt.h	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmt.h	(revision 4565)
@@ -0,0 +1,60 @@
+// definition file for test programs
+
+//#define DONT_DO_NRIC         // activate if running a bounds checker
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+// print time between construction and destruction
+class time_lapse
+{
+   double start_time;
+public:
+   time_lapse();
+   ~time_lapse();
+};
+
+void Print(const Matrix& X);
+void Print(const UpperTriangularMatrix& X);
+void Print(const DiagonalMatrix& X);
+void Print(const SymmetricMatrix& X);
+void Print(const LowerTriangularMatrix& X);
+
+void Clean(Matrix&, Real);
+void Clean(DiagonalMatrix&, Real);
+
+void trymat1(); void trymat2(); void trymat3();
+void trymat4(); void trymat5(); void trymat6();
+void trymat7(); void trymat8(); void trymat9();
+void trymata(); void trymatb(); void trymatc();
+void trymatd(); void trymate(); void trymatf();
+void trymatg(); void trymath(); void trymati();
+void trymatj(); void trymatk(); void trymatl();
+void trymatm();
+
+
+// body file: tmt.cpp
+// body file: tmt1.cpp
+// body file: tmt2.cpp
+// body file: tmt3.cpp
+// body file: tmt4.cpp
+// body file: tmt5.cpp
+// body file: tmt6.cpp
+// body file: tmt7.cpp
+// body file: tmt8.cpp
+// body file: tmt9.cpp
+// body file: tmta.cpp
+// body file: tmtb.cpp
+// body file: tmtc.cpp
+// body file: tmtd.cpp
+// body file: tmte.cpp
+// body file: tmtf.cpp
+// body file: tmtg.cpp
+// body file: tmth.cpp
+// body file: tmti.cpp
+// body file: tmtj.cpp
+// body file: tmtk.cpp
+// body file: tmtl.cpp
+// body file: tmtm.cpp
+
Index: orxonox/trunk/src/util/newmat/tmt1.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmt1.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmt1.cpp	(revision 4565)
@@ -0,0 +1,207 @@
+
+#define WANT_STREAM
+
+
+
+#include "include.h"
+
+#include "newmat.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+
+/**************************** test program ******************************/
+
+
+void trymat1()
+{
+//   cout << "\nFirst test of Matrix package\n\n";
+   Tracer et("First test of Matrix package");
+   Tracer::PrintTrace();
+   {
+      Tracer et1("Stage 1");
+      int i,j;
+
+      LowerTriangularMatrix L(10);
+      for (i=1;i<=10;i++) for (j=1;j<=i;j++) L(i,j)=2.0+i*i+j;
+      SymmetricMatrix S(10);
+      for (i=1;i<=10;i++) for (j=1;j<=i;j++) S(i,j)=i*j+1.0;
+      SymmetricMatrix S1 = S / 2.0;
+      S = S1 * 2.0;
+      UpperTriangularMatrix U=L.t()*2.0;
+      Print(LowerTriangularMatrix(L-U.t()*0.5));
+      DiagonalMatrix D(10);
+      for (i=1;i<=10;i++) D(i,i)=(i-4)*(i-5)*(i-6);
+      Matrix M=(S+U-D+L)*(L+U-D+S);
+      DiagonalMatrix DD=D*D;
+      LowerTriangularMatrix LD=L*D;
+      // expressions split for Turbo C
+      Matrix M1 = S*L + U*L - D*L + L*L + 10.0;
+      { M1 = M1 + S*U + U*U - D*U + L*U - S*D; }
+      { M1 = M1 - U*D + DD - LD + S*S; }
+      { M1 = M1 + U*S - D*S + L*S - 10.0; }
+      M=M1-M;
+      Print(M);
+   }
+   {
+      Tracer et1("Stage 2");
+      int i,j;
+
+      LowerTriangularMatrix L(9);
+      for (i=1;i<=9;i++) for (j=1;j<=i;j++) L(i,j)=1.0+j;
+      UpperTriangularMatrix U1(9);
+      for (j=1;j<=9;j++) for (i=1;i<=j;i++) U1(i,j)=1.0+i;
+      LowerTriangularMatrix LX(9);
+      for (i=1;i<=9;i++) for (j=1;j<=i;j++) LX(i,j)=1.0+i*i;
+      UpperTriangularMatrix UX(9);
+      for (j=1;j<=9;j++) for (i=1;i<=j;i++) UX(i,j)=1.0+j*j;
+      {
+         L=L+LX/0.5;   L=L-LX*3.0;   L=LX*2.0+L;
+         U1=U1+UX*2.0; U1=U1-UX*3.0; U1=UX*2.0+U1;
+      }
+
+
+      SymmetricMatrix S(9);
+      for (i=1;i<=9;i++) for (j=1;j<=i;j++) S(i,j)=i*i+j;
+      {
+         SymmetricMatrix S1 = S;
+         S=S1+5.0;
+         S=S-3.0;
+      }
+
+      DiagonalMatrix D(9);
+      for (i=1;i<=9;i++) D(i,i)=S(i,i);
+      UpperTriangularMatrix U=L.t()*2.0;
+      {
+         U1=U1*2.0 - U;  Print(U1);
+         L=L*2.0-D; U=U-D;
+      }
+      Matrix M=U+L; S=S*2.0; M=S-M; Print(M);
+   }
+   {
+      Tracer et1("Stage 3");
+      int i,j;
+      Matrix M(10,3), N(10,3);
+      for (i = 1; i<=10; i++) for (j = 1; j<=3; j++)
+         {  M(i,j) = 2*i-j; N(i,j) = i*j + 20; }
+      Matrix MN = M + N, M1;
+
+      M1 = M; M1 += N; M1 -= MN; Print(M1);
+      M1 = M; M1 += M1; M1 = M1 - M * 2; Print(M1);
+      M1 = M; M1 += N * 2; M1 -= (MN + N); Print(M1);
+      M1 = M; M1 -= M1; Print(M1);
+      M1 = M; M1 -= MN + M1; M1 += N + M; Print(M1);
+      M1 = M; M1 -= 5; M1 -= M; M1 *= 0.2; M1 = M1 + 1; Print(M1);
+      Matrix NT = N.t();
+      M1 = M; M1 *= NT; M1 -= M * N.t(); Print(M1);
+      M = M * M.t();
+      DiagonalMatrix D(10); D = 2;
+      M1 = M; M1 += D; M1 -= M; M1 = M1 - D; Print(M1);
+      M1 = M; M1 -= D; M1 -= M; M1 = M1 + D; Print(M1);
+      M1 = M; M1 *= D; M1 /= 2; M1 -= M; Print(M1);
+      SymmetricMatrix SM; SM << M;
+      // UpperTriangularMatrix SM; SM << M;
+      SM += 10; M1 = SM - M; M1 /=10; M1 = M1 - 1; Print(M1);
+   }
+   {
+      Tracer et1("Stage 4");
+      int i,j;
+      Matrix M(10,3), N(10,5);
+      for (i = 1; i<=10; i++) for (j = 1; j<=3; j++) M(i,j) = 2*i-j;
+      for (i = 1; i<=10; i++) for (j = 1; j<=5; j++) N(i,j) = i*j + 20;
+      Matrix M1;
+      M1 = M; M1 |= N; M1 &= N | M;
+      M1 -= (M | N) & (N | M); Print(M1);
+      M1 = M; M1 |= M1; M1 &= M1;
+      M1 -= (M | M) & (M | M); Print(M1);
+
+   }
+   {
+      Tracer et1("Stage 5");
+      int i,j;
+      BandMatrix BM1(10,2,3), BM2(10,4,1); Matrix M1(10,10), M2(10,10);
+      for (i=1;i<=10;i++) for (j=1;j<=10;j++)
+        { M1(i,j) = 0.5*i+j*j-50; M2(i,j) = (i*101 + j*103) % 13; }
+      BM1.Inject(M1); BM2.Inject(M2);
+      BandMatrix BM = BM1; BM += BM2;
+      Matrix M1X = BM1; Matrix M2X = BM2; Matrix MX = BM;
+      MX -= M1X + M2X; Print(MX);
+      MX = BM1; MX += BM2; MX -= M1X; MX -= M2X; Print(MX);
+      SymmetricBandMatrix SM1; SM1 << BM1 * BM1.t(); 
+      SymmetricBandMatrix SM2; SM2 << BM2 * BM2.t();
+      SM1 *= 5.5;
+      M1X *= M1X.t(); M1X *= 5.5; M2X *= M2X.t();
+      SM1 -= SM2; M1 = SM1 - M1X + M2X; Print(M1);
+      M1 = BM1; BM1 *= SM1; M1 = M1 * SM1 - BM1; Print(M1); 
+      M1 = BM1; BM1 -= SM1; M1 = M1 - SM1 - BM1; Print(M1); 
+      M1 = BM1; BM1 += SM1; M1 = M1 + SM1 - BM1; Print(M1); 
+      
+   }
+   {
+      Tracer et1("Stage 6");
+      int i,j;
+      Matrix M(10,10), N(10,10);
+      for (i = 1; i<=10; i++) for (j = 1; j<=10; j++)
+         {  M(i,j) = 2*i-j; N(i,j) = i*j + 20; }
+      GenericMatrix GM = M;
+      GM += N; Matrix M1 = GM - N - M; Print(M1);
+      DiagonalMatrix D(10); D = 3;
+      GM = D; GM += N; GM += M; GM += D;
+      M1 = D*2 - GM + M + N; Print(M1);
+      GM = D; GM *= 4; GM += 16; GM /= 8; GM -= 2;
+      GM -= D / 2; M1 = GM; Print(M1);
+      GM = D; GM *= M; GM *= N; GM /= 3; M1 = M*N - GM; Print(M1);
+      GM = D; GM |= M; GM &= N | D; M1 = GM - ((D | M) & (N | D));
+      Print(M1);
+      GM = M; M1 = M; GM += 5; GM *= 3; M *= 3; M += 15; M1 = GM - M;
+      Print(M1);
+      D.ReSize(10); for (i = 1; i<=10; i++) D(i) = i;
+      M1 = D + 10; GM = D; GM += 10; M1 -= GM; Print(M1);
+      GM = M; GM -= D; M1 = GM; GM = D; GM -= M; M1 += GM; Print(M1);
+      GM = M; GM *= D; M1 = GM; GM = D; GM *= M.t();
+      M1 -= GM.t(); Print(M1);
+      GM = M; GM += 2 * GM; GM -= 3 * M; M1 = GM; Print(M1);
+      GM = M; GM |= GM; GM -= (M | M); M1 = GM; Print(M1);
+      GM = M; GM &= GM; GM -= (M & M); M1 = GM; Print(M1);
+      M1 = M; M1 = (M1.t() & M.t()) - (M | M).t(); Print(M1);
+      M1 = M; M1 = (M1.t() | M.t()) - (M & M).t(); Print(M1);
+
+   }
+
+   {
+      Tracer et1("Stage 7");
+      // test for bug in MS VC5
+      int n = 3;
+      int k; int j;
+      Matrix A(n,n), B(n,n);
+
+      //first version - MS VC++ 5 mis-compiles if optimisation is on
+      for (k=1; k<=n; k++)
+      {
+         for (j = 1; j <= n; j++) A(k,j) = ((k-1) * (2*j-1));
+      }
+
+      //second version
+      for (k=1; k<=n; k++)
+      {
+         const int k1 = k-1;          // otherwise Visual C++ 5 fails
+         for (j = 1; j <= n; j++) B(k,j) = (k1 * (2*j-1));
+      }
+
+      if (A != B)
+      {
+         cout << "\nVisual C++ version 5 compiler error?";
+         cout << "\nTurn off optimisation";
+      }
+
+      A -= B; Print(A);
+
+   }
+
+//   cout << "\nEnd of first test\n";
+}
+
Index: orxonox/trunk/src/util/newmat/tmt2.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmt2.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmt2.cpp	(revision 4565)
@@ -0,0 +1,287 @@
+
+//#define WANT_STREAM
+
+
+#include "include.h"
+
+#include "newmat.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+
+/**************************** test program ******************************/
+
+
+void trymat2()
+{
+//   cout << "\nSecond test of Matrix package\n\n";
+   Tracer et("Second test of Matrix package");
+   Tracer::PrintTrace();
+
+   int i,j;
+
+   Matrix M(3,5);
+   for (i=1; i<=3; i++) for (j=1; j<=5; j++) M(i,j) = 100*i + j;
+   Matrix X(8,10);
+   for (i=1; i<=8; i++) for (j=1; j<=10; j++) X(i,j) = 1000*i + 10*j;
+   Matrix Y = X; Matrix Z = X;
+   { X.SubMatrix(2,4,3,7) << M; }
+   for (i=1; i<=3; i++) for (j=1; j<=5; j++) Y(i+1,j+2) = 100*i + j;
+   Print(Matrix(X-Y));
+
+
+   Real a[15]; Real* r = a;
+   for (i=1; i<=3; i++) for (j=1; j<=5; j++) *r++ = 100*i + j;
+   { Z.SubMatrix(2,4,3,7) << a; }
+   Print(Matrix(Z-Y));
+
+   { M=33; X.SubMatrix(2,4,3,7) << M; }
+   { Z.SubMatrix(2,4,3,7) = 33; }
+   Print(Matrix(Z-X));
+
+   for (i=1; i<=8; i++) for (j=1; j<=10; j++) X(i,j) = 1000*i + 10*j;
+   Y = X;
+   UpperTriangularMatrix U(5);
+   for (i=1; i<=5; i++) for (j=i; j<=5; j++) U(i,j) = 100*i + j;
+   { X.SubMatrix(3,7,5,9) << U; }
+   for (i=1; i<=5; i++) for (j=i; j<=5; j++) Y(i+2,j+4) = 100*i + j;
+   for (i=1; i<=5; i++) for (j=1; j<i; j++) Y(i+2,j+4) = 0.0;
+   Print(Matrix(X-Y));
+   for (i=1; i<=8; i++) for (j=1; j<=10; j++) X(i,j) = 1000*i + 10*j;
+   Y = X;
+   for (i=1; i<=5; i++) for (j=i; j<=5; j++) U(i,j) = 100*i + j;
+   { X.SubMatrix(3,7,5,9).Inject(U); }
+   for (i=1; i<=5; i++) for (j=i; j<=5; j++) Y(i+2,j+4) = 100*i + j;
+   Print(Matrix(X-Y));
+
+
+   // test growing and shrinking a vector
+   {
+      ColumnVector V(100);
+      for (i=1;i<=100;i++) V(i) = i*i+i;
+      V = V.Rows(1,50);               // to get first 50 vlaues.
+
+      {
+         V.Release(); ColumnVector VX=V;
+         V.ReSize(100); V = 0.0; V.Rows(1,50)=VX;
+      }                               // V now length 100
+
+      M=V; M=100;                     // to make sure V will hold its values
+      for (i=1;i<=50;i++) V(i) -= i*i+i;
+      Print(V);
+
+
+	   // test redimensioning vectors with two dimensions given
+      ColumnVector CV1(10); CV1 = 10;
+      ColumnVector CV2(5); CV2.ReSize(10,1); CV2 = 10;
+      V = CV1-CV2; Print(V);
+
+      RowVector RV1(20); RV1 = 100;
+      RowVector RV2; RV2.ReSize(1,20); RV2 = 100;
+      V = (RV1-RV2).t(); Print(V);
+
+      X.ReSize(4,7);
+      for (i=1; i<=4; i++) for (j=1; j<=7; j++) X(i,j) = 1000*i + 10*j;
+      Y = 10.5 * X;
+      Z = 7.25 - Y;
+      M = Z + X * 10.5 - 7.25;
+      Print(M);
+      Y = 2.5 * X;
+      Z = 9.25 + Y;
+      M = Z - X * 2.5 - 9.25;
+      Print(M);
+      U.ReSize(8);
+      for (i=1; i<=8; i++) for (j=i; j<=8; j++) U(i,j) = 100*i + j;
+      Y = 100 - U;
+      M = Y + U - 100;
+      Print(M);
+   }
+
+   {
+      SymmetricMatrix S,T;
+
+      S << (U + U.t());
+      T = 100 - S; M = T + S - 100; Print(M);
+      T = 100 - 2 * S; M = T + S * 2 - 100; Print(M);
+      X = 100 - 2 * S; M = X + S * 2 - 100; Print(M);
+      T = S; T = 100 - T; M = T + S - 100; Print(M);
+   }
+
+   // test new
+   {
+      ColumnVector CV1; RowVector RV1;
+      Matrix* MX; MX = new Matrix; if (!MX) Throw(Bad_alloc("New fails "));
+      MX->ReSize(10,20);
+      for (i = 1; i <= 10; i++) for (j = 1; j <= 20; j++)
+         (*MX)(i,j) = 100 * i + j;
+      ColumnVector* CV = new ColumnVector(10);
+      if (!CV) Throw(Bad_alloc("New fails "));
+      *CV << 1 << 2 << 3 << 4 << 5 << 6 << 7 << 8 << 9 << 10;
+      RowVector* RV =  new RowVector(CV->t() | (*CV + 10).t());
+      if (!RV) Throw(Bad_alloc("New fails "));
+      CV1 = ColumnVector(10); CV1 = 1; RV1 = RowVector(20); RV1 = 1;
+      *MX -= 100 * *CV * RV1 + CV1 * *RV;
+      Print(*MX);
+      delete MX; delete CV; delete RV;
+   }
+
+
+   // test copying of vectors and matrices with no elements
+   {
+      ColumnVector dims(16);
+      Matrix M1; Matrix M2 = M1; Print(M2);
+      dims(1) = M2.Nrows(); dims(2) = M2.Ncols();
+      dims(3) = (Real)(unsigned long)M2.Store(); dims(4) = M2.Storage();
+      M2 = M1;
+      dims(5) = M2.Nrows(); dims(6) = M2.Ncols();
+      dims(7) = (Real)(unsigned long)M2.Store(); dims(8) = M2.Storage();
+      M2.ReSize(10,20); M2.CleanUp();
+      dims(9) = M2.Nrows(); dims(10) = M2.Ncols();
+      dims(11) = (Real)(unsigned long)M2.Store(); dims(12) = M2.Storage();
+      M2.ReSize(20,10); M2.ReSize(0,0);
+      dims(13) = M2.Nrows(); dims(14) = M2.Ncols();
+      dims(15) = (Real)(unsigned long)M2.Store(); dims(16) = M2.Storage();
+      Print(dims);
+   }
+
+   {
+      ColumnVector dims(16);
+      ColumnVector M1; ColumnVector M2 = M1; Print(M2);
+      dims(1) = M2.Nrows(); dims(2) = M2.Ncols()-1;
+      dims(3) = (Real)(unsigned long)M2.Store(); dims(4) = M2.Storage();
+      M2 = M1;
+      dims(5) = M2.Nrows(); dims(6) = M2.Ncols()-1;
+      dims(7) = (Real)(unsigned long)M2.Store(); dims(8) = M2.Storage();
+      M2.ReSize(10); M2.CleanUp();
+      dims(9) = M2.Nrows(); dims(10) = M2.Ncols()-1;
+      dims(11) = (Real)(unsigned long)M2.Store(); dims(12) = M2.Storage();
+      M2.ReSize(10); M2.ReSize(0);
+      dims(13) = M2.Nrows(); dims(14) = M2.Ncols()-1;
+      dims(15) = (Real)(unsigned long)M2.Store(); dims(16) = M2.Storage();
+      Print(dims);
+   }
+
+   {
+      ColumnVector dims(16);
+      RowVector M1; RowVector M2 = M1; Print(M2);
+      dims(1) = M2.Nrows()-1; dims(2) = M2.Ncols();
+      dims(3) = (Real)(unsigned long)M2.Store(); dims(4) = M2.Storage();
+      M2 = M1;
+      dims(5) = M2.Nrows()-1; dims(6) = M2.Ncols();
+      dims(7) = (Real)(unsigned long)M2.Store(); dims(8) = M2.Storage();
+      M2.ReSize(10); M2.CleanUp();
+      dims(9) = M2.Nrows()-1; dims(10) = M2.Ncols();
+      dims(11) = (Real)(unsigned long)M2.Store(); dims(12) = M2.Storage();
+      M2.ReSize(10); M2.ReSize(0);
+      dims(13) = M2.Nrows()-1; dims(14) = M2.Ncols();
+      dims(15) = (Real)(unsigned long)M2.Store(); dims(16) = M2.Storage();
+      Print(dims);
+   }
+
+   // test identity matrix
+   {
+      Matrix M;
+      IdentityMatrix I(10); DiagonalMatrix D(10); D = 1;
+      M = I; M -= D; Print(M);
+      D -= I; Print(D);
+      ColumnVector X(8);
+      D = 1;
+      X(1) = Sum(D) - Sum(I);
+      X(2) = SumAbsoluteValue(D) - SumAbsoluteValue(I);
+      X(3) = SumSquare(D) - SumSquare(I);
+      X(4) = Trace(D) - Trace(I);
+      X(5) = Maximum(D) - Maximum(I);
+      X(6) = Minimum(D) - Minimum(I);
+      X(7) = LogDeterminant(D).LogValue() - LogDeterminant(I).LogValue();
+      X(8) = LogDeterminant(D).Sign() - LogDeterminant(I).Sign();
+      Clean(X,0.00000001); Print(X);
+
+      for (i = 1; i <= 10; i++) for (j = 1; j <= 10; j++)
+         M(i,j) = 100 * i + j;
+      Matrix N;
+      N = M * I - M; Print(N);
+      N = I * M - M; Print(N);
+      N = M * I.i() - M; Print(N);
+      N = I.i() * M - M; Print(N);
+      N = I.i(); N -= I; Print(N);
+      N = I.t(); N -= I; Print(N);
+      N = I.t(); N += (-I); Print(N); // <----------------
+      D = I; N = D; D = 1; N -= D; Print(N);
+      N = I; D = 1; N -= D; Print(N);
+      N = M + 2 * IdentityMatrix(10); N -= (M + 2 * D); Print(N);
+
+      I *= 4;
+
+      D = 4;
+
+      X.ReSize(14);
+      X(1) = Sum(D) - Sum(I);
+      X(2) = SumAbsoluteValue(D) - SumAbsoluteValue(I);
+      X(3) = SumSquare(D) - SumSquare(I);
+      X(4) = Trace(D) - Trace(I);
+      X(5) = Maximum(D) - Maximum(I);
+      X(6) = Minimum(D) - Minimum(I);
+      X(7) = LogDeterminant(D).LogValue() - LogDeterminant(I).LogValue();  // <--
+      X(8) = LogDeterminant(D).Sign() - LogDeterminant(I).Sign();
+      int i,j;
+      X(9) = I.Maximum1(i) - 4; X(10) = i-1;
+      X(11) = I.Maximum2(i,j) - 4; X(12) = i-10; X(13) = j-10;
+      X(14) = I.Nrows() - 10;
+      Clean(X,0.00000001); Print(X);
+
+
+      N = D.i();
+      N += I / (-16);
+      Print(N);
+      N = M * I - 4 * M; Print(N);
+      N = I * M - 4 * M; Print(N);
+      N = M * I.i() - 0.25 * M; Print(N);
+      N = I.i() * M - 0.25 * M; Print(N);
+      N = I.i(); N -= I * 0.0625; Print(N);
+      N = I.i(); N = N - 0.0625 * I; Print(N);
+      N = I.t(); N -= I; Print(N);
+      D = I * 2; N = D; D = 1; N -= 8 * D; Print(N);
+      N = I * 2; N -= 8 * D; Print(N);
+      N = 0.5 * I + M; N -= M; N -= 2.0 * D; Print(N);
+
+      IdentityMatrix J(10); J = 8;
+      D = 4;
+      DiagonalMatrix E(10); E = 8;
+      N = (I + J) - (D + E); Print(N);
+      N = (5*I + 3*J) - (5*D + 3*E); Print(N);
+      N = (-I + J) - (-D + E); Print(N);
+      N = (I - J) - (D - E); Print(N);
+      N = (I | J) - (D | E); Print(N);
+      N = (I & J) - (D & E); Print(N);
+      N = SP(I,J) - SP(D,E); Print(N);
+      N = D.SubMatrix(2,5,3,8) - I.SubMatrix(2,5,3,8); Print(N);
+
+      N = M; N.Inject(I); D << M; N -= (M + I); N += D; Print(N);
+      D = 4;
+
+      IdentityMatrix K = I.i()*7 - J.t()/4;
+      N = D.i() * 7 - E / 4 - K; Print(N);
+      K = I * J; N = K - D * E; Print(N);
+      N = I * J; N -= D * E; Print(N);
+      K = 5*I - 3*J;
+      N = K - (5*D - 3*E); Print(N);
+      K = I.i(); N = K - 0.0625 * I; Print(N);
+      K = I.t(); N = K - I; Print(N);
+
+
+      K.ReSize(20); D.ReSize(20); D = 1;
+      D -= K; Print(D);
+
+      I.ReSize(3); J.ReSize(3); K = I * J; N = K - I; Print(N);
+      K << D; N = K - D; Print(N);
+
+
+   }
+
+
+//   cout << "\nEnd of second test\n";
+}
Index: orxonox/trunk/src/util/newmat/tmt3.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmt3.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmt3.cpp	(revision 4565)
@@ -0,0 +1,160 @@
+
+//#define WANT_STREAM
+
+#include "include.h"
+
+#include "newmat.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+
+/**************************** test program ******************************/
+
+void trymat3()
+{
+   Tracer et("Third test of Matrix package");
+   Tracer::PrintTrace();
+
+   {
+      Tracer et1("Stage 1");
+      int i,j;
+      SymmetricMatrix S(7);
+      for (i=1;i<=7;i++) for (j=1;j<=i;j++) S(i,j)=i*i+j;
+		S=-S+2.0;
+
+      DiagonalMatrix D(7);
+      for (i=1;i<=7;i++) D(i,i)=S(i,i);
+
+      Matrix M4(7,7); { M4=D+(D+4.0); M4=M4-D*2.0;  M4=M4-4.0; Print(M4); }
+      SymmetricMatrix S2=D; Matrix M2=S2;  { M2=-D+M2; Print(M2); }
+      UpperTriangularMatrix U2=D; { M2=U2; M2=D-M2; Print(M2); }
+      LowerTriangularMatrix L2=D; { M2=L2; M2=D-M2; Print(M2); }
+      M2=D; M2=M2-D; Print(M2);
+      for (i=1;i<=7;i++) for (j=1;j<=i;j++) L2(i,j)=2.0-i*i-j;
+      U2=L2.t(); D=D.t(); S=S.t();
+      M4=(L2-1.0)+(U2+1.0)-D-S; Print(M4);
+      M4=(-L2+1.0)+(-U2-1.0)+D+S; Print(M4);
+   }
+
+   {
+      Tracer et1("Stage 2");
+      int i,j;
+      DiagonalMatrix D(6);
+      for (i=1;i<=6;i++) D(i,i)=i*3.0+i*i+2.0;
+      UpperTriangularMatrix U2(7); LowerTriangularMatrix L2(7);
+      for (i=1;i<=7;i++) for (j=1;j<=i;j++) L2(i,j)=2.0-i*i+j;
+		{ U2=L2.t(); }
+      DiagonalMatrix D1(7); for (i=1;i<=7;i++) D1(i,i)=(i-2)*(i-4);
+      Matrix M2(6,7);
+      for (i=1;i<=6;i++) for (j=1;j<=7;j++) M2(i,j)=2.0+i*j+i*i+2.0*j*j;
+      Matrix MD=D; SymmetricMatrix MD1(1); MD1=D1;
+      Matrix MX=MD*M2*MD1 - D*(M2*D1); Print(MX);
+      MX=MD*M2*MD1 - (D*M2)*D1; Print(MX);
+      {
+         D.ReSize(7); for (i=1;i<=7;i++) D(i,i)=i*3.0+i*i+2.0;
+         LowerTriangularMatrix LD(1); LD=D;
+         UpperTriangularMatrix UD(1); UD=D;
+         M2=U2; M2=LD*M2*MD1 - D*(U2*D1); Print(M2);
+         M2=U2; M2=UD*M2*MD1 - (D*U2)*D1; Print(M2);
+         M2=L2; M2=LD*M2*MD1 - D*(L2*D1); Print(M2);
+         M2=L2; M2=UD*M2*MD1 - (D*L2)*D1; Print(M2);
+      }
+   }
+
+   {
+      Tracer et1("Stage 3");
+      // test inverse * scalar
+      DiagonalMatrix D(6);
+      for (int i=1;i<=6;i++) D(i)=i*i;
+      DiagonalMatrix E = D.i() * 4.0;
+      DiagonalMatrix I(6); I = 1.0;
+      E=D*E-I*4.0; Print(E);
+      E = D.i() / 0.25; E=D*E-I*4.0; Print(E);
+   }
+   {
+      Tracer et1("Stage 4");
+      Matrix sigma(3,3); Matrix sigmaI(3,3);
+      sigma = 0; sigma(1,1) = 1.0; sigma(2,2) = 1.0; sigma(3,3) = 1.0;
+      sigmaI = sigma.i();
+      sigmaI -= sigma;  Clean(sigmaI, 0.000000001); Print(sigmaI);
+   }
+   {
+      Tracer et1("Stage 5");
+      Matrix X(5,5); DiagonalMatrix DM(5);
+      for (int i=1; i<=5; i++) for (int j=1; j<=5; j++)
+         X(i,j) = (23*i+59*j) % 43;
+      DM << 1 << 8 << -7 << 2 << 3;
+      Matrix Y = X.i() * DM; Y = X * Y - DM;
+      Clean(Y, 0.000000001); Print(Y);
+   }
+   {
+      Tracer et1("Stage 6");          // test reverse function
+      ColumnVector CV(10), RCV(10);
+      CV  <<  2 << 7 << 1  << 6 << -3 <<  1 << 8 << -4 << 0 << 17;
+      RCV << 17 << 0 << -4 << 8 << 1  << -3 << 6 <<  1 << 7 << 2;
+      ColumnVector X = CV - RCV.Reverse(); Print(X);
+      RowVector Y = CV.t() - RCV.t().Reverse(); Print(Y);
+      DiagonalMatrix D = CV.AsDiagonal() - RCV.AsDiagonal().Reverse();
+      Print(D);
+      X = CV & CV.Rows(1,9).Reverse();
+      ColumnVector Z(19);
+      Z.Rows(1,10) = RCV.Reverse(); Z.Rows(11,19) = RCV.Rows(2,10);
+      X -= Z; Print(X); Z -= Z.Reverse(); Print(Z);
+      Matrix A(3,3); A << 1 << 2 << 3 << 4 << 5 << 6 << 7 << 8 << 9;
+      Matrix B(3,3); B << 9 << 8 << 7 << 6 << 5 << 4 << 3 << 2 << 1;
+      Matrix Diff = A - B.Reverse(); Print(Diff);
+      Diff = (-A).Reverse() + B; Print(Diff);
+      UpperTriangularMatrix U;
+      U << A.Reverse(); Diff = U; U << B; Diff -= U; Print(Diff);
+      U << (-A).Reverse(); Diff = U; U << B; Diff += U; Print(Diff);
+   }
+   {
+      Tracer et1("Stage 7");           // test IsSingular function
+      ColumnVector XX(4);
+      Matrix A(3,3);
+      A = 0;
+      CroutMatrix B1 = A;
+      XX(1) = B1.IsSingular() ? 0 : 1;
+      A << 1 << 3 << 6
+        << 7 << 11 << 13
+        << 2 << 4  << 1;
+      CroutMatrix B2(A);
+      XX(2) = B2.IsSingular() ? 1 : 0;
+      BandMatrix C(3,1,1); C.Inject(A);
+      BandLUMatrix B3(C);
+      XX(3) = B3.IsSingular() ? 1 : 0;
+      C = 0;
+      BandLUMatrix B4(C);
+      XX(4) = B4.IsSingular() ? 0 : 1;
+      Print(XX);
+   }
+   {
+      Tracer et1("Stage 8");           // inverse with vector of 0s
+      Matrix A(3,3); Matrix Z(3,3); ColumnVector X(6);
+      A <<  1 <<  3 <<  6
+        <<  7 << 11 << 13
+        <<  2 <<  4 <<  1;
+      Z = 0;
+      Matrix B = (A | Z) & (Z | A);   // 6 * 6 matrix
+      X = 0.0;
+      X = B.i() * X;
+      Print(X);
+      // also check inverse with non-zero Y
+      Matrix Y(3,3);
+      Y << 0.0 << 1.0 << 1.0
+        << 5.0 << 0.0 << 5.0
+        << 3.0 << 3.0 << 0.0;
+      Matrix YY = Y & Y;        // stack Y matrices
+      YY = B.i() * YY;
+      Matrix Y1 = A.i() * Y;
+      YY -= Y1 & Y1; Clean(YY, 0.000000001); Print(YY);
+      Y1 = A * Y1 - Y; Clean(Y1, 0.000000001); Print(Y1);
+   }
+
+
+}
+
Index: orxonox/trunk/src/util/newmat/tmt4.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmt4.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmt4.cpp	(revision 4565)
@@ -0,0 +1,155 @@
+
+//#define WANT_STREAM
+
+
+#include "include.h"
+
+#include "newmat.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+
+/**************************** test program ******************************/
+
+
+void trymat4()
+{
+//   cout << "\nFourth test of Matrix package\n";
+   Tracer et("Fourth test of Matrix package");
+   Tracer::PrintTrace();
+
+   int i,j;
+
+   {
+      Tracer et1("Stage 1");
+      Matrix M(10,10);
+      UpperTriangularMatrix U(10);
+      for (i=1;i<=10;i++) for (j=1;j<=10;j++) M(i,j) = 100*i+j;
+      U << -M;
+      Matrix X1 = M.Rows(2,4);
+      Matrix Y1 = U.t().Rows(2,4);
+      Matrix X = U; { Print(Matrix(X.Columns(2,4).t()-Y1)); }
+      RowVector RV = M.Row(5);
+      {
+         X.ReSize(3,10);
+         X.Row(1) << M.Row(2); X.Row(2) << M.Row(3); X.Row(3) << M.Row(4);
+         Print(Matrix(X-X1));
+      }
+      {
+         UpperTriangularMatrix V = U.SymSubMatrix(3,5);
+         Matrix MV = U.SubMatrix(3,5,3,5); { Print(Matrix(MV-V)); }
+         Matrix X2 = M.t().Columns(2,4); { Print(Matrix(X2-X1.t())); }
+         Matrix Y2 = U.Columns(2,4); { Print(Matrix(Y2-Y1.t())); }
+         ColumnVector CV = M.t().Column(5); { Print(ColumnVector(CV-RV.t())); }
+         X.ReSize(10,3); M = M.t();
+         X.Column(1) << M.Column(2); X.Column(2) << M.Column(3);
+         X.Column(3) << M.Column(4);
+         Print(Matrix(X-X2));
+      }
+   }
+
+   {
+      Tracer et1("Stage 2");
+      Matrix M; Matrix X; M.ReSize(5,8);
+      for (i=1;i<=5;i++) for (j=1;j<=8;j++) M(i,j) = 100*i+j;
+      {
+         X = M.Columns(5,8); M.Columns(5,8) << M.Columns(1,4);
+             M.Columns(1,4) << X;
+         X = M.Columns(3,4); M.Columns(3,4) << M.Columns(1,2);
+             M.Columns(1,2) << X;
+         X = M.Columns(7,8); M.Columns(7,8) << M.Columns(5,6);
+             M.Columns(5,6) << X;
+      }
+      {
+         X = M.Column(2); M.Column(2) = M.Column(1); M.Column(1) = X;
+         X = M.Column(4); M.Column(4) = M.Column(3); M.Column(3) = X;
+         X = M.Column(6); M.Column(6) = M.Column(5); M.Column(5) = X;
+         X = M.Column(8); M.Column(8) = M.Column(7); M.Column(7) = X;
+         X.ReSize(5,8);
+      }
+      for (i=1;i<=5;i++) for (j=1;j<=8;j++) X(i,9-j) = 100*i+j;
+      Print(Matrix(X-M));
+   }
+   {
+      Tracer et1("Stage 3");
+      // try submatrices of zero dimension
+      Matrix A(4,5); Matrix B, C;
+      for (i=1; i<=4; i++) for (j=1; j<=5; j++)
+         A(i,j) = 100+i*10+j;
+      B = A + 100;
+      C = A | B.Columns(4,3); Print(Matrix(A - C));
+      C = A | B.Columns(1,0); Print(Matrix(A - C));
+      C = A | B.Columns(6,5); Print(Matrix(A - C));
+      C = A & B.Rows(2,1); Print(Matrix(A - C));
+   }
+   {
+      Tracer et1("Stage 4");
+      BandMatrix BM(5,3,2);
+      BM(1,1) = 1; BM(1,2) = 2; BM(1,3) = 3;
+      BM(2,1) = 4; BM(2,2) = 5; BM(2,3) = 6; BM(2,4) = 7;
+      BM(3,1) = 8; BM(3,2) = 9; BM(3,3) =10; BM(3,4) =11; BM(3,5) =12;
+      BM(4,1) =13; BM(4,2) =14; BM(4,3) =15; BM(4,4) =16; BM(4,5) =17;
+                   BM(5,2) =18; BM(5,3) =19; BM(5,4) =20; BM(5,5) =21;
+      SymmetricBandMatrix SM(5,3);
+      SM.Inject(BandMatrix(BM + BM.t()));
+      Matrix A = BM + 1;
+      Matrix M = A + A.t() - 2;
+      Matrix C = A.i() * BM;
+      C = A * C - BM; Clean(C, 0.000000001); Print(C);
+      C = A.i() * SM;
+      C = A * C - M; Clean(C, 0.000000001); Print(C);
+
+      // check row-wise load
+      BandMatrix BM1(5,3,2);
+      BM1.Row(1) <<  1 <<  2 <<  3;
+      BM1.Row(2) <<  4 <<  5 <<  6 <<  7;
+      BM1.Row(3) <<  8 <<  9 << 10 << 11 << 12;
+      BM1.Row(4) << 13 << 14 << 15 << 16 << 17;
+      BM1.Row(5)       << 18 << 19 << 20 << 21;
+      Matrix M1 = BM1 - BM; Print(M1);
+   }
+   {
+      Tracer et1("Stage 5");
+      Matrix X(4,4);
+      X << 1 << 2 << 3 << 4
+        << 5 << 6 << 7 << 8
+        << 9 <<10 <<11 <<12
+        <<13 <<14 <<15 <<16;
+      Matrix Y(4,0);
+      Y = X | Y;
+      X -= Y; Print(X);
+
+      DiagonalMatrix D(1);
+      D << 23;                       // matrix input with just one value
+      D(1) -= 23; Print(D);
+
+   }
+   {
+      Tracer et1("Stage 6");
+      Matrix h (2,2);
+      h << 1.0 << 2.0 << 0.0 << 1.0 ;
+      RowVector c(2);
+      c << 0.0 << 1.0;
+      h -= c & c;
+      h -= c.t().Reverse() | c.Reverse().t();
+      Print(h);
+   }
+   {
+      Tracer et1("Stage 7");
+      // Check row-wise input for diagonal matrix
+      DiagonalMatrix D(4);
+      D << 18 << 23 << 31 << 17;
+      DiagonalMatrix D1(4);
+      D1.Row(1) << 18; D1.Row(2) << 23; D1.Row(3) << 31; D1.Row(4) << 17;
+      D1 -= D; Print(D1);
+      D1(1) = 18; D1(2) = 23; D1(3) = 31; D1(4) = 17;
+      D1 -= D; Print(D1);
+   }
+
+//   cout << "\nEnd of fourth test\n";
+}
+
Index: orxonox/trunk/src/util/newmat/tmt5.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmt5.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmt5.cpp	(revision 4565)
@@ -0,0 +1,125 @@
+
+//#define WANT_STREAM
+
+#include "include.h"
+
+#include "newmat.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+
+// **************************** test program ******************************
+
+
+
+ReturnMatrix Returner0(const GenericMatrix& GM)
+{ Matrix M = GM; M.Release(); return M; }
+
+ReturnMatrix Returner1(const GenericMatrix& GM)
+{ Matrix M = GM+1; M.Release(); return M; }
+
+ReturnMatrix Returner2(const GenericMatrix& GM)
+{ UpperBandMatrix M = GM*2; M.Release(); return M; }
+
+ReturnMatrix Returner3(const GenericMatrix& GM)
+{ LowerBandMatrix M = GM*3; M.Release(); return M; }
+
+ReturnMatrix Returner4(const GenericMatrix& GM)
+{ SymmetricMatrix M = GM+4; M.Release(); return M; }
+
+ReturnMatrix Returner5(const GenericMatrix& GM)
+{ SymmetricBandMatrix M = GM*5; M.Release(); return M; }
+
+ReturnMatrix Returner6(const GenericMatrix& GM)
+{ BandMatrix M = GM*6; M.Release(); return M; }
+
+ReturnMatrix Returner7(const GenericMatrix& GM)
+{ DiagonalMatrix M = GM*7; M.Release(); return M; }
+
+void trymat5()
+{
+//   cout << "\nFifth test of Matrix package\n";
+   Tracer et("Fifth test of Matrix package");
+   Tracer::PrintTrace();
+
+   int i,j;
+
+   Matrix A(5,6);
+   for (i=1;i<=5;i++) for (j=1;j<=6;j++) A(i,j)=1+i*j+i*i+j*j;
+   ColumnVector CV(6);
+   for (i=1;i<=6;i++) CV(i)=i*i+3;
+   ColumnVector CV2(5); for (i=1;i<=5;i++) CV2(i)=1.0;
+   ColumnVector CV1=CV;
+
+   {
+      CV=A*CV;
+      RowVector RV=CV.t(); // RowVector RV; RV=CV.t();
+      RV=RV-1.0;
+      CV=(RV*A).t()+A.t()*CV2; CV1=(A.t()*A)*CV1 - CV;
+      Print(CV1);
+   }
+
+   CV1.ReSize(6);
+   CV2.ReSize(6);
+   CV.ReSize(6);
+   for (i=1;i<=6;i++) { CV1(i)=i*3+1; CV2(i)=10-i; CV(i)=11+i*2; }
+   ColumnVector CX=CV2-CV; { CX=CX+CV1; Print(CX); }
+   Print(ColumnVector(CV1+CV2-CV));
+   RowVector RV=CV.t(); RowVector RV1=CV1.t();
+   RowVector R=RV-RV1; Print(RowVector(R-CV2.t()));
+
+// test loading of list
+
+   RV.ReSize(10);
+   for (i=1;i<=10;i++) RV(i) = i*i;
+   RV1.ReSize(10);
+   RV1 << 1 << 4 << 9 << 16 << 25 << 36 << 49 << 64 << 81 << 100; // << 121;
+   Print(RowVector(RV-RV1));
+
+   et.ReName("Fifth test of Matrix package - almost at end");
+
+   Matrix X(2,3);
+   X << 11 << 12 << 13
+     << 21 << 22 << 23;
+
+   Matrix Y = X.t();                 // check simple transpose
+
+   X(1,1) -= 11; X(1,2) -= 12; X(1,3) -= 13;
+   X(2,1) -= 21; X(2,2) -= 22; X(2,3) -= 23;
+   Print(X);
+
+   Y(1,1) -= 11; Y(2,1) -= 12; Y(3,1) -= 13;
+   Y(1,2) -= 21; Y(2,2) -= 22; Y(3,2) -= 23;
+   Print(Y);
+
+   et.ReName("Fifth test of Matrix package - at end");
+
+   RV = Returner1(RV)-1; Print(RowVector(RV-RV1));
+   CV1 = Returner1(RV.t())-1; Print(ColumnVector(RV.t()-CV1));
+#ifndef DONT_DO_NRIC
+   nricMatrix AA = A;
+   X = Returner1(AA)-A-1; Print(X);
+#endif
+   UpperTriangularMatrix UT(31);
+   for (i=1; i<=31; i++) for (j=i; j<=31; j++) UT(i,j) = i+j+(i-j)*(i-2*j);
+   UpperBandMatrix UB(31,5); UB.Inject(UT);
+   LowerTriangularMatrix LT = UT.t();
+   LowerBandMatrix LB(31,5); LB.Inject(LT);
+   A = Returner0(UB)-LB.t(); Print(A);
+   A = Returner2(UB).t()-LB*2; Print(A);
+   A = Returner3(LB).t()-UB*3; Print(A);
+   SymmetricMatrix SM; SM << (UT+LT);
+   A = Returner4(SM)-UT-LT-4; Print(A);
+   SymmetricBandMatrix SB(31,5); SB.Inject(SM);
+   A = Returner5(SB)/5-UB-LB; Print(A);
+   BandMatrix B = UB+LB*LB; A = LB;
+   A = Returner6(B)/6 - UB - A*A; Print(A);
+   DiagonalMatrix D; D << UT;
+   D << (Returner7(D)/7 - UT); Print(D);
+
+//   cout << "\nEnd of fifth test\n";
+}
Index: orxonox/trunk/src/util/newmat/tmt6.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmt6.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmt6.cpp	(revision 4565)
@@ -0,0 +1,155 @@
+
+//#define WANT_STREAM
+#define WANT_MATH
+
+
+#include "include.h"
+
+#include "newmatap.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+
+/**************************** test program ******************************/
+
+
+// slow sort program
+
+static void SimpleSortDescending(Real* first, const int length)
+{
+   int i = length;
+   while (--i)
+   {
+      Real x = *first; Real* f = first; Real* g = f;
+      int j = i;
+      while (j--) if (x < *(++f)) { g = f; x = *g; }
+      *g = *first; *first++ = x;
+   }
+}
+
+static void TestSort(int n)
+{
+   // make some data
+   RowVector X(n);
+   int i;
+   for (i = 1; i <= n; i++)
+      X(i) = sin((Real)i) + 0.3 * cos(i/5.0) - 0.6 * sin(i/7.0) + 0.2 * sin(2.0 * i);
+   RowVector X_Sorted = X; SimpleSortDescending(X_Sorted.Store(), n);
+   RowVector A = X + X.Reverse(); SimpleSortDescending(A.Store(), n);
+
+   // test descending sort
+
+   RowVector Y = X; SortDescending(Y); Y -= X_Sorted; Print(Y);
+   Y = X_Sorted; SortDescending(Y); Y -= X_Sorted; Print(Y);
+   Y = X_Sorted.Reverse(); SortDescending(Y); Y -= X_Sorted; Print(Y);
+   Y = X + X.Reverse(); SortDescending(Y); Y -= A; Print(Y);
+
+   // test ascending sort
+
+   Y = X; SortAscending(Y); Y -= X_Sorted.Reverse(); Print(Y);
+   Y = X_Sorted; SortAscending(Y); Y -= X_Sorted.Reverse(); Print(Y);
+   Y = X_Sorted.Reverse(); SortAscending(Y); Y -= X_Sorted.Reverse(); Print(Y);
+   Y = X + X.Reverse(); SortAscending(Y); Y -= A.Reverse(); Print(Y);
+}
+
+
+void trymat6()
+{
+   Tracer et("Sixth test of Matrix package");
+   Tracer::PrintTrace();
+
+   int i,j;
+
+
+   DiagonalMatrix D(6);
+   UpperTriangularMatrix U(6);
+   for (i=1;i<=6;i++) { for (j=i;j<=6;j++) U(i,j)=i*i*i-50; D(i,i)=i*i+i-10; }
+   LowerTriangularMatrix L=(U*3.0).t();
+   SymmetricMatrix S(6);
+   for (i=1;i<=6;i++) for (j=i;j<=6;j++) S(i,j)=i*i+2.0+j;
+   Matrix MD=D; Matrix ML=L; Matrix MU=U; Matrix MS=S;
+   Matrix M(6,6);
+   for (i=1;i<=6;i++) for (j=1;j<=6;j++) M(i,j)=i*j+i*i-10.0;  
+   {
+      Tracer et1("Stage 1");
+      Print(Matrix(MS+(-MS)));
+      Print(Matrix((S+M)-(MS+M)));
+      Print(Matrix((M+U)-(M+MU)));
+      Print(Matrix((M+L)-(M+ML)));
+   }
+   {
+      Tracer et1("Stage 2");
+      Print(Matrix((M+D)-(M+MD)));
+      Print(Matrix((U+D)-(MU+MD)));
+      Print(Matrix((D+L)-(ML+MD)));
+      Print(Matrix((-U+D)+MU-MD));
+      Print(Matrix((-L+D)+ML-MD));
+   }
+   {
+      Tracer et1("Stage 3 - concatenate");
+      RowVector A(5);
+      A << 1 << 2 << 3 << 4 << 5;
+      RowVector B(5);
+      B << 3 << 1 << 4 << 1 << 5;
+      Matrix C(3,5);
+      C <<  2 <<  3 <<  5 <<  7 << 11
+        << 13 << 17 << 19 << 23 << 29
+        << 31 << 37 << 41 << 43 << 47;
+      Matrix X1 = A & B & C;
+      Matrix X2 = (A.t() | B.t() | C.t()).t();
+      Matrix X3(5,5);
+      X3.Row(1)=A; X3.Row(2)=B; X3.Rows(3,5)=C;
+      Print(Matrix(X1-X2));
+      Print(Matrix(X1-X3));
+      LowerTriangularMatrix LT1; LT1 << (A & B & C);
+      UpperTriangularMatrix UT1; UT1 << (A.t() | B.t() | C.t());
+      Print(LowerTriangularMatrix(LT1-UT1.t()));
+      DiagonalMatrix D1; D1 << (A.t() | B.t() | C.t());
+      ColumnVector At = A.t();
+      ColumnVector Bt = B.t();
+      Matrix Ct = C.t();
+      LowerTriangularMatrix LT2; LT2 << (At | Bt | Ct);
+      UpperTriangularMatrix UT2; UT2 << (At.t() & Bt.t() & Ct.t());
+      Matrix ABt = At | Bt;
+      DiagonalMatrix D2; D2 << (ABt | Ct);
+      Print(LowerTriangularMatrix(LT2-UT2.t()));
+      Print(DiagonalMatrix(D1-D2));
+      Print(Matrix(LT1+UT2-D2-X1));
+      Matrix M1 = LT1 | UT2; Matrix M2 = UT1 & LT2;
+      Print(Matrix(M1-M2.t()));
+      M1 = UT2 | LT1; M2 = LT2 & UT1;
+      Print(Matrix(M1-M2.t()));
+      M1 = (LT1 | UT2) & (UT2 | LT1);
+      M2 = (UT1 & LT2) | (LT2 & UT1);
+      Print(Matrix(M1-M2.t()));
+      SymmetricMatrix SM1; SM1 << (M1 + M1.t());
+      SymmetricMatrix SM2; SM2 << ((SM1 | M1) & (M1.t() | SM1));
+      Matrix M3(20,20);
+      M3.SubMatrix(1,10,1,10) = SM1;
+      M3.SubMatrix(1,10,11,20) = M1;
+      M3.SubMatrix(11,20,1,10) = M2;
+      M3.SubMatrix(11,20,11,20) = SM1;
+      Print(Matrix(M3-SM2));
+
+      SymmetricMatrix SM(15); SM = 0; SM.SymSubMatrix(1,10) = SM1;
+      M3.ReSize(15,15); M3 = 0; M3.SubMatrix(1,10,1,10) = SM1;
+      M3 -= SM; Print(M3);
+      SM = 0; SM.SymSubMatrix(6,15) = SM1;
+      M3.ReSize(15,15); M3 = 0; M3.SubMatrix(6,15,6,15) = SM1;
+      M3 = M3.t() - SM; Print(M3);
+   }
+   {
+      Tracer et1("Stage 4 - sort");
+      TestSort(1); TestSort(2); TestSort(3); TestSort(4);
+      TestSort(15); TestSort(16); TestSort(17); TestSort(18);
+      TestSort(99); TestSort(100); TestSort(101);
+   }
+
+
+//   cout << "\nEnd of sixth test\n";
+}
+
Index: orxonox/trunk/src/util/newmat/tmt7.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmt7.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmt7.cpp	(revision 4565)
@@ -0,0 +1,132 @@
+
+//#define WANT_STREAM
+
+#include "include.h"
+
+#include "newmat.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+
+/**************************** test program ******************************/
+
+
+void trymat7()
+{
+//   cout << "\nSeventh test of Matrix package\n";
+   Tracer et("Seventh test of Matrix package");
+   Tracer::PrintTrace();
+
+   int i,j;
+
+
+   DiagonalMatrix D(6);
+   UpperTriangularMatrix U(6);
+   for (i=1;i<=6;i++) { for (j=i;j<=6;j++) U(i,j)=i*i*j-50; D(i,i)=i*i+i-10; }
+   LowerTriangularMatrix L=(U*3.0).t();
+   SymmetricMatrix S(6);
+   for (i=1;i<=6;i++) for (j=i;j<=6;j++) S(i,j)=i*i+2.0+j;
+   Matrix MD=D; Matrix ML=L; Matrix MU=U;
+   Matrix MS=S;
+   Matrix M(6,6);
+   for (i=1;i<=6;i++) for (j=1;j<=6;j++) M(i,j)=i*j+i*i-10.0;  
+   {
+      Tracer et1("Stage 1");
+      Print(Matrix((S-M)-(MS-M)));
+      Print(Matrix((-M-S)+(MS+M)));
+      Print(Matrix((U-M)-(MU-M)));
+   }
+   {
+      Tracer et1("Stage 2");
+      Print(Matrix((L-M)+(M-ML)));
+      Print(Matrix((D-M)+(M-MD)));
+      Print(Matrix((D-S)+(MS-MD)));
+      Print(Matrix((D-L)+(ML-MD)));
+   }
+
+   { M=MU.t(); }
+   LowerTriangularMatrix LY=D.i()*U.t();
+   {
+      Tracer et1("Stage 3");
+      MS=D*LY-M; Clean(MS,0.00000001); Print(MS);
+      L=U.t();
+      LY=D.i()*L; MS=D*LY-M; Clean(MS,0.00000001); Print(MS);
+   }
+   {
+      Tracer et1("Stage 4");
+      UpperTriangularMatrix UT(11);
+      int i, j;
+      for (i=1;i<=11;i++) for (j=i;j<=11;j++) UT(i,j)=i*i+j*3;
+      GenericMatrix GM; Matrix X;
+      UpperBandMatrix UB(11,3); UB.Inject(UT); UT = UB;
+      UpperBandMatrix UB2 = UB / 8;
+      GM = UB2-UT/8; X = GM; Print(X);
+      SymmetricBandMatrix SB(11,4); SB << (UB + UB.t());
+      X = SB - UT - UT.t(); Print(X);
+      BandMatrix B = UB + UB.t()*2;
+      DiagonalMatrix D; D << B;
+      X.ReSize(1,1); X(1,1) = Trace(B)-Sum(D); Print(X);
+      X = SB + 5; Matrix X1=X; X = SP(UB,X); Matrix X2 =UB;
+      X1 = (X1.AsDiagonal() * X2.AsDiagonal()).AsRow()-X.AsColumn().t();
+      Print(X1);
+      X1=SB.t(); X2 = B.t(); X = SB.i() * B - X1.i() * X2.t();
+      Clean(X,0.00000001); Print(X);
+      X = SB.i(); X = X * B - X1.i() * X2.t();
+      Clean(X,0.00000001); Print(X);
+      D = 1; X = SB.i() * SB - D; Clean(X,0.00000001); Print(X);
+      ColumnVector CV(11);
+      CV << 2 << 6 <<3 << 8 << -4 << 17.5 << 2 << 1 << -2 << 5 << 3.75;
+      D << 2 << 6 <<3 << 8 << -4 << 17.5 << 2 << 1 << -2 << 5 << 3.75;
+      X = CV.AsDiagonal(); X = X-D; Print(X);
+      SymmetricBandMatrix SB1(11,7); SB1 = 5; 
+      SymmetricBandMatrix SB2 = SB1 + D;
+      X.ReSize(11,11); X=0;
+      for (i=1;i<=11;i++) for (j=1;j<=11;j++)
+      {
+         if (abs(i-j)<=7) X(i,j)=5;
+         if (i==j) X(i,j)+=CV(i);
+      }
+      SymmetricMatrix SM; SM.ReSize(11);
+      SM=SB; SB = SB+SB2; X1 = SM+X-SB; Print(X1);
+      SB2=0; X2=SB2; X1=SB; Print(X2);
+      for (i=1;i<=11;i++) SB2.Column(i)<<SB.Column(i);
+      X1=X1-SB2; Print(X1);
+      X = SB; SB2.ReSize(11,4); SB2 = SB*5; SB2 = SB + SB2;
+      X1 = X*6 - SB2; Print(X1);
+      X1 = SP(SB,SB2/3); X1=X1-SP(X,X*2); Print(X1);
+      X1 = SP(SB2/6,X*2); X1=X1-SP(X*2,X); Print(X1);
+   }
+
+   {
+      // test the simple integer array class
+      Tracer et("Stage 5");
+      ColumnVector Test(10); Test = 0.0;
+      int i;
+      SimpleIntArray A(100);
+      for (i = 0; i < 100; i++) A[i] = i*i+1;
+      SimpleIntArray B(100), C(50), D(0);
+      D.ReSize(150);
+      B = A; A.ReSize(50, true); C = A; A.ReSize(150, true); D = A;
+      for (i = 0; i < 100; i++) if (B[i] != i*i+1) Test(1)=1;
+      for (i = 0; i < 50; i++) if (C[i] != i*i+1) Test(2)=1;
+      for (i = 0; i < 50; i++) if (D[i] != i*i+1) Test(3)=1;
+      A.ReSize(75); A = A.Size();
+      for (i = 0; i < 75; i++) if (A[i] != 75) Test(4)=1;
+      A.ReSize(25); A = A.Size();
+      for (i = 0; i < 25; i++) if (A[i] != 25) Test(5)=1;
+      A.ReSize(25); A = 23;
+      for (i = 0; i < 25; i++) if (A[i] != 23) Test(6)=1;
+      A.ReSize(0); A.ReSize(15); A = A.Size();
+      for (i = 0; i < 15; i++) if (A[i] != 15) Test(7)=1;
+      const SimpleIntArray E = B;
+      for (i = 0; i < 100; i++) if (E[i] != i*i+1) Test(8)=1;
+      Print(Test);
+   }
+
+
+//   cout << "\nEnd of seventh test\n";
+}
Index: orxonox/trunk/src/util/newmat/tmt8.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmt8.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmt8.cpp	(revision 4565)
@@ -0,0 +1,268 @@
+
+//#define WANT_STREAM
+
+#include "include.h"
+
+#include "newmatap.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+
+
+// **************************** test program ******************************
+
+
+void Transposer(const GenericMatrix& GM1, GenericMatrix&GM2)
+   { GM2 = GM1.t(); }
+
+// this is a routine in "Numerical Recipes in C" format
+// if R is a row vector, C a column vector and D diagonal
+// make matrix DCR
+
+static void DCR(Real d[], Real c[], int m, Real r[], int n, Real **dcr)
+{
+   int i, j;
+   for (i = 1; i <= m; i++) for (j = 1; j <= n; j++)
+   dcr[i][j] = d[i] * c[i] * r[j];
+}
+
+ReturnMatrix TestReturn(const GeneralMatrix& gm) { return gm; }
+
+void trymat8()
+{
+//   cout << "\nEighth test of Matrix package\n";
+   Tracer et("Eighth test of Matrix package");
+   Tracer::PrintTrace();
+
+   int i;
+
+
+   DiagonalMatrix D(6);
+   for (i=1;i<=6;i++)  D(i,i)=i*i+i-10;
+   DiagonalMatrix D2=D;
+   Matrix MD=D;
+
+   DiagonalMatrix D1(6); for (i=1;i<=6;i++) D1(i,i)=-100+i*i*i;
+   Matrix MD1=D1;
+   Print(Matrix(D*D1-MD*MD1));
+   Print(Matrix((-D)*D1+MD*MD1));
+   Print(Matrix(D*(-D1)+MD*MD1));
+   DiagonalMatrix DX=D;
+   {
+      Tracer et1("Stage 1");
+      DX=(DX+D1)*DX; Print(Matrix(DX-(MD+MD1)*MD));
+      DX=D;
+      DX=-DX*DX+(DX-(-D1))*((-D1)+DX);
+      // Matrix MX = Matrix(MD1);
+      // MD1=DX+(MX.t())*(MX.t()); Print(MD1);
+      MD1=DX+(Matrix(MD1).t())*(Matrix(MD1).t()); Print(MD1);
+      DX=D; DX=DX; DX=D2-DX; Print(DiagonalMatrix(DX));
+      DX=D;
+   }
+   {
+      Tracer et1("Stage 2");
+      D.Release(2);
+      D1=D; D2=D;
+      Print(DiagonalMatrix(D1-DX));
+      Print(DiagonalMatrix(D2-DX));
+      MD1=1.0;
+      Print(Matrix(MD1-1.0));
+   }
+   {
+      Tracer et1("Stage 3");
+      //GenericMatrix
+      LowerTriangularMatrix LT(4);
+      LT << 1 << 2 << 3 << 4 << 5 << 6  << 7 << 8 << 9 << 10;
+      UpperTriangularMatrix UT = LT.t() * 2.0;
+      GenericMatrix GM1 = LT;
+      LowerTriangularMatrix LT1 = GM1-LT; Print(LT1);
+      GenericMatrix GM2 = GM1; LT1 = GM2; LT1 = LT1-LT; Print(LT1);
+      GM2 = GM1; LT1 = GM2; LT1 = LT1-LT; Print(LT1);
+      GM2 = GM1*2; LT1 = GM2; LT1 = LT1-LT*2; Print(LT1);
+      GM1.Release();
+      GM1=GM1; LT1=GM1-LT; Print(LT1); LT1=GM1-LT; Print(LT1);
+      GM1.Release();
+      GM1=GM1*4; LT1=GM1-LT*4; Print(LT1);
+      LT1=GM1-LT*4; Print(LT1); GM1.CleanUp();
+      GM1=LT; GM2=UT; GM1=GM1*GM2; Matrix M=GM1; M=M-LT*UT; Print(M);
+      Transposer(LT,GM2); LT1 = LT - GM2.t(); Print(LT1);
+      GM1=LT; Transposer(GM1,GM2); LT1 = LT - GM2.t(); Print(LT1);
+      GM1 = LT; GM1 = GM1 + GM1; LT1 = LT*2-GM1; Print(LT1);
+      DiagonalMatrix D; D << LT; GM1 = D; LT1 = GM1; LT1 -= D; Print(LT1);
+      UpperTriangularMatrix UT1 = GM1; UT1 -= D; Print(UT1);
+   }
+   {
+      Tracer et1("Stage 4");
+      // Another test of SVD
+      Matrix M(12,12); M = 0;
+      M(1,1) = M(2,2) = M(4,4) = M(6,6) =
+         M(7,7) = M(8,8) = M(10,10) = M(12,12) = -1;
+      M(1,6) = M(1,12) = -5.601594;
+      M(3,6) = M(3,12) = -0.000165;
+      M(7,6) = M(7,12) = -0.008294;
+      DiagonalMatrix D;
+      SVD(M,D);
+      SortDescending(D);
+      // answer given by matlab
+      DiagonalMatrix DX(12);
+      DX(1) = 8.0461;
+      DX(2) = DX(3) = DX(4) = DX(5) = DX(6) = DX(7) = 1;
+      DX(8) = 0.1243;
+      DX(9) = DX(10) = DX(11) = DX(12) = 0;
+      D -= DX; Clean(D,0.0001); Print(D);
+   }
+#ifndef DONT_DO_NRIC
+   {
+      Tracer et1("Stage 5");
+      // test numerical recipes in C interface
+      DiagonalMatrix D(10);
+      D << 1 << 4 << 6 << 2 << 1 << 6 << 4 << 7 << 3 << 1;
+      ColumnVector C(10);
+      C << 3 << 7 << 5 << 1 << 4 << 2 << 3 << 9 << 1 << 3;
+      RowVector R(6);
+      R << 2 << 3 << 5 << 7 << 11 << 13;
+      nricMatrix M(10, 6);
+      DCR( D.nric(), C.nric(), 10, R.nric(), 6, M.nric() );
+      M -= D * C * R;  Print(M);
+
+      D.ReSize(5);
+      D << 1.25 << 4.75 << 9.5 << 1.25 << 3.75;
+      C.ReSize(5);
+      C << 1.5 << 7.5 << 4.25 << 0.0 << 7.25;
+      R.ReSize(9);
+      R << 2.5 << 3.25 << 5.5 << 7 << 11.25 << 13.5 << 0.0 << 1.5 << 3.5;
+      Matrix MX = D * C * R;
+      M.ReSize(MX);
+      DCR( D.nric(), C.nric(), 5, R.nric(), 9, M.nric() );
+      M -= MX;  Print(M);
+   }
+#endif
+   {
+      Tracer et1("Stage 6");
+      // test dotproduct
+      DiagonalMatrix test(5); test = 1;
+      ColumnVector C(10);
+      C << 3 << 7 << 5 << 1 << 4 << 2 << 3 << 9 << 1 << 3;
+      RowVector R(10);
+      R << 2 << 3 << 5 << 7 << 11 << 13 << -3 << -4 << 2 << 4;
+      test(1) = (R * C).AsScalar() - DotProduct(C, R);
+      test(2) = C.SumSquare() - DotProduct(C, C);
+      test(3) = 6.0 * (C.t() * R.t()).AsScalar() - DotProduct(2.0 * C, 3.0 * R);
+      Matrix MC = C.AsMatrix(2,5), MR = R.AsMatrix(5,2);
+      test(4) = DotProduct(MC, MR) - (R * C).AsScalar();
+      UpperTriangularMatrix UT(5);
+      UT << 3 << 5 << 2 << 1 << 7
+              << 1 << 1 << 8 << 2
+                   << 7 << 0 << 1
+                        << 3 << 5
+                             << 6;
+      LowerTriangularMatrix LT(5);
+      LT << 5
+         << 2 << 3
+         << 1 << 0 << 7
+         << 9 << 8 << 1 << 2
+         << 0 << 2 << 1 << 9 << 2;
+      test(5) = DotProduct(UT, LT) - Sum(SP(UT, LT));
+      Print(test);
+      // check row-wise load;
+      LowerTriangularMatrix LT1(5);
+      LT1.Row(1) << 5;
+      LT1.Row(2) << 2   << 3;
+      LT1.Row(3) << 1   << 0   << 7;
+      LT1.Row(4) << 9   << 8   << 1   << 2;
+      LT1.Row(5) << 0   << 2   << 1   << 9   << 2;
+      Matrix M = LT1 - LT; Print(M);
+      // check solution with identity matrix
+      IdentityMatrix IM(5); IM *= 2;
+      LinearEquationSolver LES1(IM);
+      LowerTriangularMatrix LTX = LES1.i() * LT;
+      M = LTX * 2 - LT; Print(M);
+      DiagonalMatrix D = IM;
+      LinearEquationSolver LES2(IM);
+      LTX = LES2.i() * LT;
+      M = LTX * 2 - LT; Print(M);
+      UpperTriangularMatrix UTX = LES1.i() * UT;
+      M = UTX * 2 - UT; Print(M);
+      UTX = LES2.i() * UT;
+      M = UTX * 2 - UT; Print(M);
+   }
+
+   {
+      Tracer et1("Stage 7");
+      // Some more GenericMatrix stuff with *= |= &=
+      // but don't any additional checks
+      BandMatrix BM1(6,2,3);
+      BM1.Row(1) << 3 << 8 << 4 << 1;
+      BM1.Row(2) << 5 << 1 << 9 << 7 << 2;
+      BM1.Row(3) << 1 << 0 << 6 << 3 << 1 << 3;
+      BM1.Row(4)      << 4 << 2 << 5 << 2 << 4;
+      BM1.Row(5)           << 3 << 3 << 9 << 1;
+      BM1.Row(6)                << 4 << 2 << 9;
+      BandMatrix BM2(6,1,1);
+      BM2.Row(1) << 2.5 << 7.5;
+      BM2.Row(2) << 1.5 << 3.0 << 8.5;
+      BM2.Row(3)        << 6.0 << 6.5 << 7.0;
+      BM2.Row(4)               << 2.5 << 2.0 << 8.0;
+      BM2.Row(5)                      << 0.5 << 4.5 << 3.5;
+      BM2.Row(6)                             << 9.5 << 7.5;
+      Matrix RM1 = BM1, RM2 = BM2;
+      Matrix X;
+      GenericMatrix GRM1 = RM1, GBM1 = BM1, GRM2 = RM2, GBM2 = BM2;
+      Matrix Z(6,0); Z = 5; Print(Z);
+      GRM1 |= Z; GBM1 |= Z; GRM2 &= Z.t(); GBM2 &= Z.t();
+      X = GRM1 - BM1; Print(X); X = GBM1 - BM1; Print(X);
+      X = GRM2 - BM2; Print(X); X = GBM2 - BM2; Print(X);
+
+      GRM1 = RM1; GBM1 = BM1; GRM2 = RM2; GBM2 = BM2;
+      GRM1 *= GRM2; GBM1 *= GBM2;
+      X = GRM1 - BM1 * BM2; Print(X);
+      X = RM1 * RM2 - GBM1; Print(X);
+
+      GRM1 = RM1; GBM1 = BM1; GRM2 = RM2; GBM2 = BM2;
+      GRM1 *= GBM2; GBM1 *= GRM2;          // Bs and Rs swapped on LHS
+      X = GRM1 - BM1 * BM2; Print(X);
+      X = RM1 * RM2 - GBM1; Print(X);
+
+      X = BM1.t(); BandMatrix BM1X = BM1.t();
+      GRM1 = RM1; X -= GRM1.t(); Print(X); X = BM1X - BM1.t(); Print(X);
+
+      // check that linear equation solver works with Identity Matrix
+      IdentityMatrix IM(6); IM *= 2;
+      GBM1 = BM1; GBM1 *= 4; GRM1 = RM1; GRM1 *= 4;
+      DiagonalMatrix D = IM;
+      LinearEquationSolver LES1(D);
+      BandMatrix BX;
+      BX = LES1.i() * GBM1; BX -= BM1 * 2; X = BX; Print(X);
+      LinearEquationSolver LES2(IM);
+      BX = LES2.i() * GBM1; BX -= BM1 * 2; X = BX; Print(X);
+      BX = D.i() * GBM1; BX -= BM1 * 2; X = BX; Print(X);
+      BX = IM.i() * GBM1; BX -= BM1 * 2; X = BX; Print(X);
+      BX = IM.i(); BX *= GBM1; BX -= BM1 * 2; X = BX; Print(X);
+
+      // try symmetric band matrices
+      SymmetricBandMatrix SBM; SBM << SP(BM1, BM1.t());
+      SBM << IM.i() * SBM;
+      X = 2 * SBM - SP(RM1, RM1.t()); Print(X);
+
+      // Do this again with more general D
+      D << 2.5 << 7.5 << 2 << 5 << 4.5 << 7.5;
+      BX = D.i() * BM1; X = BX - D.i() * RM1;
+      Clean(X,0.00000001); Print(X);
+      BX = D.i(); BX *= BM1; X = BX - D.i() * RM1;
+      Clean(X,0.00000001); Print(X);
+      SBM << SP(BM1, BM1.t());
+      BX = D.i() * SBM; X = BX - D.i() * SP(RM1, RM1.t());
+      Clean(X,0.00000001); Print(X);
+
+      // test return
+      BX = TestReturn(BM1); X = BX - BM1;
+      if (BX.BandWidth() != BM1.BandWidth()) X = 5;
+      Print(X);
+   }
+
+//   cout << "\nEnd of eighth test\n";
+}
Index: orxonox/trunk/src/util/newmat/tmt9.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmt9.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmt9.cpp	(revision 4565)
@@ -0,0 +1,213 @@
+//#define WANT_STREAM
+
+#include "include.h"
+#include "newmatap.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+
+/**************************** test program ******************************/
+
+
+void trymat9()
+{
+   Tracer et("Ninth test of Matrix package");
+   Tracer::PrintTrace();
+
+
+   int i; int j;
+   Matrix A(7,7); Matrix X(7,3);
+   for (i=1;i<=7;i++) for (j=1;j<=7;j++) A(i,j)=i*i+j+((i==j) ? 1 : 0);
+   for (i=1;i<=7;i++) for (j=1;j<=3;j++) X(i,j)=i-j;
+   Matrix B = A.i(); DiagonalMatrix D(7); D=1.0;
+   {
+      Tracer et1("Stage 1");
+      Matrix Q = B*A-D; Clean(Q, 0.000000001); Print(Q);
+      Q=A; Q = Q.i() * X; Q = A*Q - X; Clean(Q, 0.000000001); Print(Q);
+      Q=X; Q = A.i() * Q; Q = A*Q - X; Clean(Q, 0.000000001); Print(Q);
+   }
+   for (i=1;i<=7;i++) D(i,i)=i*i+1;
+   DiagonalMatrix E(3); for (i=1;i<=3;i++) E(i,i)=i+23;
+   {
+      Tracer et1("Stage 2");
+      Matrix DXE = D.i() * X * E;
+      DXE = E.i() * DXE.t() * D - X.t(); Clean(DXE, 0.00000001); Print(DXE); 
+      E=D; for (i=1;i<=7;i++) E(i,i)=i*3+1;
+   }
+   DiagonalMatrix F=D;
+   {
+      Tracer et1("Stage 3");
+      F=E.i()*F; F=F*E-D; Clean(F,0.00000001); Print(F);
+      F=E.i()*D; F=F*E-D; Clean(F,0.00000001); Print(F);
+   }
+   {
+      Tracer et1("Stage 4");
+      F=E; F=F.i()*D; F=F*E-D; Clean(F,0.00000001); Print(F);
+   }
+   {
+      Tracer et1("Stage 5");
+      // testing equal
+      ColumnVector A(18), B(18);
+      Matrix X(3,3);
+      X << 3 << 5  << 7 << 5 << 8 << 2 << 7 << 2 << 9;
+      SymmetricMatrix S; S << X;
+      B(1) = S == X;         A(1) = true;
+      B(2) = S == (X+1);     A(2) = false;
+      B(3) = (S+2) == (X+2); A(3) = true;
+      Matrix Y = X;
+      B(4) = X == Y;         A(4) = true;
+      B(5) = (X*2) == (Y*2); A(5) = true;
+      Y(3,3) = 10;
+      B(6) = X == Y;         A(6) = false;
+      B(7) = (X*2) == (Y*2); A(7) = false;
+      B(8) = S == Y;         A(8) = false;
+      B(9) = S == S;         A(9) = true;
+      Matrix Z = X.SubMatrix(1,2,2,3);
+      B(10) = X == Z;        A(10) = false;
+      GenericMatrix GS = S;
+      GenericMatrix GX = X;
+      GenericMatrix GY = Y;
+      B(11) = GS == GX;      A(11) = true;
+      B(12) = GS == GY;      A(12) = false;
+      CroutMatrix CS = S;
+      CroutMatrix CX = X;
+      CroutMatrix CY = Y;
+      B(13) = CS == CX;      A(13) = true;
+      B(14) = CS == CY;      A(14) = false;
+      B(15) = X == CX;       A(15) = false;
+      B(16) = X == A;        A(16) = false;
+      B(17) = X == (X | X);  A(17) = false;
+      B(18) = CX == X;       A(18) = false;
+      A = A - B; Print(A);
+   }
+   {
+      Tracer et1("Stage 6");
+      // testing equal
+      ColumnVector A(22), B(22);
+      BandMatrix X(6,2,1);
+      X(1,1)=23; X(1,2)=21;
+      X(2,1)=12; X(2,2)=17; X(2,3)=45;
+      X(3,1)=35; X(3,2)=19; X(3,3)=24; X(3,4)=29;
+                 X(4,2)=17; X(4,3)=11; X(4,4)=19; X(4,5)=35;
+                            X(5,3)=10; X(5,4)=44; X(5,5)=23; X(5,6)=31;
+                                       X(6,4)=49; X(6,5)=41; X(6,6)=17;
+      SymmetricBandMatrix S1(6,2); S1.Inject(X);
+      BandMatrix U(6,2,3); U = 0.0; U.Inject(X);
+      B(1) = U == X;         A(1) = true;
+      B(2) = U == (X*3);     A(2) = false;
+      B(3) = (U*5) == (X*5); A(3) = true;
+      Matrix Y = X;
+      B(4) = X == Y;         A(4) = true;
+      B(5) = (X*2) == (Y*2); A(5) = true;
+      Y(6,6) = 10;
+      B(6) = X == Y;         A(6) = false;
+      B(7) = (X*2) == (Y*2); A(7) = false;
+      B(8) = U == Y;         A(8) = false;
+      B(9) = U == U;         A(9) = true;
+      Matrix Z = X.SubMatrix(1,2,2,3);
+      B(10) = X == Z;        A(10) = false;
+      GenericMatrix GU = U;
+      GenericMatrix GX = X;
+      GenericMatrix GY = Y;
+      B(11) = GU == GX;      A(11) = true;
+      B(12) = GU == GY;      A(12) = false;
+      X = X + X.t(); U = U + U.t();
+      SymmetricBandMatrix S(6,2); S.Inject(X);
+      Matrix D = S-X; Print(D);
+      BandLUMatrix BS = S;
+      BandLUMatrix BX = X;
+      BandLUMatrix BU = U;
+      CroutMatrix CX = X;
+      B(13) = BS == BX;      A(13) = true;
+      B(14) = BX == BU;      A(14) = false;
+      B(15) = X == BX;       A(15) = false;
+      B(16) = X != BX;       A(16) = true;
+      B(17) = BX != BS;      A(17) = false;
+      B(18) = (2*X) != (X*2);A(18) = false;
+      B(19) = (X*2) != (X+2);A(19) = true;
+      B(20) = BX == CX;      A(20) = false;
+      B(21) = CX == BX;      A(21) = false;
+      B(22) = BX == X;       A(22) = false;
+      A = A - B; Print(A);
+      DiagonalMatrix I(6); I=1.0;
+      D = BS.i() * X - I;  Clean(D,0.00000001); Print(D);
+      D = BX.i() * X - I;  Clean(D,0.00000001); Print(D);
+      D = BU.i() * X - I;  Clean(D,0.00000001); Print(D);
+
+      // test row wise load
+      SymmetricBandMatrix X1(6,2);
+      X1.Row(1) << 23;
+      X1.Row(2) << 12 << 17;
+      X1.Row(3) << 35 << 19 << 24;
+      X1.Row(4)       << 17 << 11 << 19;
+      X1.Row(5)             << 10 << 44 << 23;
+      X1.Row(6)                   << 49 << 41 << 17;
+      Matrix M = X1 - S1; Print(M);
+
+      // check out submatrix
+      SymmetricBandMatrix X2(20,3); X2 = 0.0;
+      X2.SubMatrix(2,7,2,7) = X1; X2.SymSubMatrix(11,16) = 2 * X1;
+      Matrix MX1 = X1;
+      Matrix MX2(20,20); MX2 = 0;
+      MX2.SymSubMatrix(2,7) = MX1; MX2.SubMatrix(11,16,11,16) = MX1 * 2;
+      MX2 -= X2; Print(MX2);
+
+      BandMatrix X4(20,3,3); X4 = 0.0;
+      X4.SubMatrix(2,7,3,8) = X1; X4.SubMatrix(11,16,10,15) = 2 * X1;
+      MX1 = X1;
+      Matrix MX4(20,20); MX4 = 0;
+      MX4.SubMatrix(2,7,3,8) = MX1; MX4.SubMatrix(11,16,10,15) = MX1 * 2;
+      MX4 -= X4; Print(MX4);
+
+      MX1 = X1.i() * X1 - IdentityMatrix(6);
+      Clean(MX1,0.00000001); Print(MX1);
+
+   }
+
+   {
+      Tracer et1("Stage 7");
+      // testing equal
+      ColumnVector A(12), B(12);
+      BandMatrix X(6,2,1);
+      X(1,1)=23; X(1,2)=21;
+      X(2,1)=12; X(2,2)=17; X(2,3)=45;
+      X(3,1)=35; X(3,2)=19; X(3,3)=24; X(3,4)=29;
+                 X(4,2)=17; X(4,3)=11; X(4,4)=19; X(4,5)=35;
+                            X(5,3)=10; X(5,4)=44; X(5,5)=23; X(5,6)=31;
+                                       X(6,4)=49; X(6,5)=41; X(6,6)=17;
+      Matrix Y = X;
+      LinearEquationSolver LX = X;
+      LinearEquationSolver LY = Y;
+      CroutMatrix CX = X;
+      CroutMatrix CY = Y;
+      BandLUMatrix BX = X;
+      B(1) = LX == CX;       A(1) = false;
+      B(2) = LY == CY;       A(2) = true;
+      B(3) = X == Y;         A(3) = true;
+      B(4) = BX == LX;       A(4) = true;
+      B(5) = CX == CY;       A(5) = true;
+      B(6) = LX == LY;       A(6) = false;
+      B(7) = BX == BX;       A(7) = true;
+      B(8) = CX == CX;       A(8) = true;
+      B(9) = LX == LX;       A(9) = true;
+      B(10) = LY == LY;      A(10) = true;
+      CroutMatrix CX1 = X.SubMatrix(1,4,1,4);
+      B(11) = CX == CX1;     A(11) = false;
+      BandLUMatrix BX1 = X.SymSubMatrix(1,4);    // error with SubMatrix
+      B(12) = BX == BX1;     A(12) = false;
+      A = A - B; Print(A);
+      DiagonalMatrix I(6); I=1.0; Matrix D;
+      D = LX.i() * X - I;  Clean(D,0.00000001); Print(D);
+      D = LY.i() * X - I;  Clean(D,0.00000001); Print(D);
+      I.ReSize(4); I = 1;
+      D = CX1.i() * X.SymSubMatrix(1,4) - I;  Clean(D,0.00000001); Print(D);
+      D = BX1.i() * X.SubMatrix(1,4,1,4) - I;  Clean(D,0.00000001); Print(D);
+   }
+
+//   cout << "\nEnd of ninth test\n";
+}
+
Index: orxonox/trunk/src/util/newmat/tmta.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmta.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmta.cpp	(revision 4565)
@@ -0,0 +1,126 @@
+
+//#define WANT_STREAM
+
+
+#include "include.h"
+
+#include "newmatap.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+
+/**************************** test program ******************************/
+
+
+static void process(const GeneralMatrix& A,
+   const ColumnVector& X1, const ColumnVector& X2)
+{
+      Matrix B = A;
+      LinearEquationSolver L(A);
+      Matrix Y(4,2);
+      Y.Column(1) << L.i() * X1; Y.Column(2) << L.i() * X2;
+      Matrix Z(4,2); Z.Column(1) << X1; Z.Column(2) << X2;
+      Z = B * Y - Z; Clean(Z,0.00000001); Print(Z);
+}
+
+
+
+void trymata()
+{
+//   cout << "\nTenth test of Matrix package\n";
+   Tracer et("Tenth test of Matrix package");
+   Tracer::PrintTrace();
+   int i; int j;
+   UpperTriangularMatrix U(8);
+   for (i=1;i<=8;i++) for (j=i;j<=8;j++) U(i,j)=i+j*j+5;
+   Matrix X(8,6);
+   for (i=1;i<=8;i++) for (j=1;j<=6;j++) X(i,j)=i*j+1.0;
+   Matrix Y = U.i()*X; Matrix MU=U;
+   Y=Y-MU.i()*X; Clean(Y,0.00000001); Print(Y);
+   Y = U.t().i()*X; Y=Y-MU.t().i()*X; Clean(Y,0.00000001); Print(Y);
+   UpperTriangularMatrix UX(8);
+   for (i=1;i<=8;i++) for (j=i;j<=8;j++) UX(i,j)=i+j+1;
+   UX(4,4)=0; UX(4,5)=0;
+   UpperTriangularMatrix UY = U.i() * UX;
+   { X=UX; MU=U; Y=UY-MU.i()*X; Clean(Y,0.000000001); Print(Y); }
+   LowerTriangularMatrix LY = U.t().i() * UX.t();
+   { Y=LY-MU.i().t()*X.t(); Clean(Y,0.000000001); Print(Y); }
+   DiagonalMatrix D(8); for (i=1;i<=8;i++) D(i,i)=i+1;
+   { X=D.i()*MU; }
+   { UY=U; UY=D.i()*UY; Y=UY-X; Clean(Y,0.00000001); Print(Y); }
+   { UY=D.i()*U; Y=UY-X; Clean(Y,0.00000001); Print(Y); }
+//   X=MU.t();
+//   LY=D.i()*U.t(); Y=D*LY-X; Clean(Y,0.00000001); Print(Y);
+//   LowerTriangularMatrix L=U.t();
+//   LY=D.i()*L; Y=D*LY-X; Clean(Y,0.00000001); Print(Y);
+   U.ReSize(8);
+   for (i=1;i<=8;i++) for (j=i;j<=8;j++) U(i,j)=i+j*j+5;
+   MU = U;
+   MU = U.i() - MU.i(); Clean(MU,0.00000001); Print(MU);
+   MU = U.t().i() - U.i().t(); Clean(MU,0.00000001); Print(MU);
+
+   // test LINEQ
+   {
+      ColumnVector X1(4), X2(4);
+      X1(1)=1; X1(2)=2; X1(3)=3; X1(4)=4;
+      X2(1)=1; X2(2)=10; X2(3)=100; X2(4)=1000;
+
+
+      Matrix A(4,4);
+      A(1,1)=1; A(1,2)=3; A(1,3)=0; A(1,4)=0;
+      A(2,1)=3; A(2,2)=2; A(2,3)=5; A(2,4)=0;
+      A(3,1)=0; A(3,2)=5; A(3,3)=4; A(3,4)=1;
+      A(4,1)=0; A(4,2)=0; A(4,3)=1; A(4,4)=3;
+      process(A,X1,X2);
+
+      BandMatrix B(4,1,1);  B.Inject(A);
+      process(B,X1,X2);
+
+      UpperTriangularMatrix U(4);
+      U(1,1)=1; U(1,2)=2; U(1,3)=3; U(1,4)=4;
+		U(2,2)=8; U(2,3)=7; U(2,4)=6;
+			  U(3,3)=2; U(3,4)=7;
+				    U(4,4)=1;
+      process(U,X1,X2);
+
+      // check rowwise load
+      UpperTriangularMatrix U1(4);
+      U1.Row(1) << 1 << 2 << 3 << 4;
+      U1.Row(2)      << 8 << 7 << 6;
+      U1.Row(3)           << 2 << 7;
+      U1.Row(4)                << 1;
+
+      U1 -= U;
+
+      Print(U1);
+
+      LowerTriangularMatrix L = U.t();
+      process(L,X1,X2);
+   }
+
+   // test inversion of poorly conditioned matrix
+   // a user complained this didn't work under OS9
+   {
+      Matrix M(4,4);
+
+      M <<  8.613057e+00 <<  8.693985e+00 << -2.322050e-01  << 0.000000e+00
+        <<  8.693985e+00 <<  8.793868e+00 << -2.346310e-01  << 0.000000e+00
+        << -2.322050e-01 << -2.346310e-01 <<  6.264000e-03  << 0.000000e+00
+        <<  0.000000e+00 <<  0.000000e+00 <<  0.000000e+00  << 3.282806e+03 ;
+      Matrix MI = M.i();
+      DiagonalMatrix I(4); I = 1;
+      Matrix Diff = MI *  M - I;
+      Clean(Diff,0.00000001); Print(Diff);
+      // Alternatively do Cholesky
+      SymmetricMatrix SM; SM << M;
+      LowerTriangularMatrix LT = Cholesky(SM).i();
+      MI = LT.t() * LT; Diff = MI *  M - I;
+      Clean(Diff,0.00000001); Print(Diff);
+   }
+
+//   cout << "\nEnd of tenth test\n";
+}
Index: orxonox/trunk/src/util/newmat/tmtb.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmtb.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmtb.cpp	(revision 4565)
@@ -0,0 +1,152 @@
+
+//#define WANT_STREAM
+
+#include "include.h"
+
+#include "newmat.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+
+/**************************** test program ******************************/
+
+// make sure matrices work as members of a class
+
+class TestClass
+{
+   Matrix A;
+   Matrix B;
+public:
+   TestClass();
+   ReturnMatrix Sum();
+};
+
+TestClass::TestClass() : A(2,3)
+{
+   B.ReSize(2,3);
+   A << 1 << 4
+     << 4 << 1
+     << 2 << 9;
+   B << 8 << 5
+     << 5 << 8
+     << 7 << 0;
+}
+
+ReturnMatrix TestClass::Sum() { return Matrix(A + B).ForReturn(); }
+
+
+
+void trymatb()
+{
+//   cout << "\nEleventh test of Matrix package\n";
+   Tracer et("Eleventh test of Matrix package");
+   Tracer::PrintTrace();
+   int i; int j;
+   RowVector RV; RV.ReSize(10);
+   {
+      Tracer et1("Stage 1");
+      for (i=1;i<=10;i++) RV(i)=i*i-3;
+      Matrix X(1,1); X(1,1) = .25;
+      Print(RowVector(X.i() * RV - RV / .25));
+//      Print(RowVector(X.i() * Matrix(RV) - RV / .25)); // != zortech, AT&T
+      Print(RowVector(X.i() * RV - RV / .25));
+   }
+   LowerTriangularMatrix L(5); UpperTriangularMatrix U(5);
+   for (i=1; i<=5; i++) for (j=1; j<=i; j++)
+   { L(i,j) = i*i + j -2.0; U(j,i) = i*i*j+3; }
+   DiagonalMatrix D(5);
+   for (i=1; i<=5; i++) D(i,i) = i*i + i + 2;
+   Matrix M1 = -L; Matrix M2 = L-U; Matrix M3 = U*3; Matrix M4 = U-L;
+   Matrix M5 = M1 - D; M1 = D * (-3) - M3;
+   {
+      Tracer et1("Stage 2");
+      Print(Matrix((M2-M4*2)+M5*3-M1));
+      M1 = L.t(); Print(Matrix(M1.t()-L));
+      M1 = U.t(); Print(Matrix(M1.t()-U));
+   }
+   {
+      Tracer et1("Stage 3");
+      SymmetricMatrix S(5);
+      for (i=1; i<=5; i++) for (j=1; j<=i; j++) S(i,j) = i*j+i-j+5;
+      M2 = S.i() * M4; M1 = S; M3=M1*M2-M4; Clean(M3,0.00000001); Print(M3);
+      SymmetricMatrix T(5);
+      for (i=1; i<=5; i++) for (j=1; j<=i; j++) T(i,j) = i*i*j*j+i-j+5-i*j;
+      M1 = S.i() * T; M1 = S * M1; M1 = M1-T; Clean(M1,0.00000001); Print(M1);
+      ColumnVector CV(5); for (i=1; i<=5; i++) CV(i) = i*i*i+10;
+      M1 = CV * RV;
+   }
+   {
+      Tracer et1("Stage 4");
+      M4.ReSize(5,10);
+      for (i=1; i<=5; i++) for (j=1; j<=10; j++) M4(i,j) = (i*i*i+10)*(j*j-3);
+      Print(Matrix(M1-M4));
+      M1 = L.t(); M2 = U.t(); M3 = L+U; Print(Matrix(M1-M3.t()+M2));
+   }
+//   UpperTriangularMatrix U2((const UpperTriangularMatrix&)U); // != zortech
+   UpperTriangularMatrix U2((UpperTriangularMatrix&)U);
+   {
+      Tracer et1("Stage 5");
+      Print(Matrix(U2-U));
+      M2.ReSize(10,10);
+      for (i=1; i<=10; i++) for (j=1; j<=10; j++) M2(i,j) = (i*i*i+10)*(j*j-3);
+      D << M2; L << M2; U << M2;               // check copy into
+      Print( Matrix( (D+M2)-(L+U) ) );
+   }
+   {
+      Tracer et1("Stage 6");
+      M1.ReSize(6,10);
+      for (i=1; i<=6; i++) for (j=1; j<=10; j++)  M1(i,j) = 100*i + j;
+      M2 = M1.SubMatrix(3,5,4,7);  M3.ReSize(3,4);
+      for (i=3; i<=5; i++) for (j=4; j<=7; j++)   M3(i-2,j-3) = 100*i + j;
+      Print(Matrix(M2-M3));
+   }
+   int a1,a2,a3,a4;
+   {
+      Tracer et1("Stage 7");
+      int a1,a2,a3,a4;
+      a1=4; a2=9; a3=4; a4=7;
+      U.ReSize(10);
+      for (i=1; i<=10; i++) for (j=i; j<=10; j++)  U(i,j) = 100*i + j;
+      M2 = U.SubMatrix(a1,a2,a3,a4);
+      M3.ReSize(a2-a1+1,a4-a3+1); M3=0.0;
+      for (i=a1; i<=a2; i++) for (j=(i>a3) ? i : a3; j<=a4; j++)
+         M3(i-a1+1,j-a3+1) = 100*i + j;
+      Print(Matrix(M2-M3));
+   }
+   {
+      Tracer et1("Stage 8");
+      a1=3; a2=9; a3=2; a4=7;
+      U.ReSize(10);
+      for (i=1; i<=10; i++) for (j=i; j<=10; j++)  U(i,j) = 100*i + j;
+      M2 = U.SubMatrix(a1,a2,a3,a4);
+      M3.ReSize(a2-a1+1,a4-a3+1); M3=0.0;
+      for (i=a1; i<=a2; i++) for (j=(i>a3) ? i : a3; j<=a4; j++)
+         M3(i-a1+1,j-a3+1) = 100*i + j;
+      Print(Matrix(M2-M3));
+   }
+   {
+      Tracer et1("Stage 9");
+      a1=4; a2=6; a3=2; a4=5;
+      U.ReSize(10);
+      for (i=1; i<=10; i++) for (j=i; j<=10; j++)  U(i,j) = 100*i + j;
+      M2 = U.SubMatrix(a1,a2,a3,a4);
+      M3.ReSize(a2-a1+1,a4-a3+1); M3=0.0;
+      for (i=a1; i<=a2; i++) for (j=(i>a3) ? i : a3; j<=a4; j++)
+         M3(i-a1+1,j-a3+1) = 100*i + j;
+      Print(Matrix(M2-M3));
+   }
+
+   {
+      Tracer et1("Stage 10");
+      TestClass TC;
+      Matrix M = TC.Sum() - 9;
+      Print(M);
+   }
+
+
+//   cout << "\nEnd of eleventh test\n";
+}
Index: orxonox/trunk/src/util/newmat/tmtc.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmtc.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmtc.cpp	(revision 4565)
@@ -0,0 +1,200 @@
+
+//#define WANT_STREAM
+
+
+#include "include.h"
+#include "newmat.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+
+
+
+void trymatc()
+{
+//   cout << "\nTwelfth test of Matrix package\n";
+   Tracer et("Twelfth test of Matrix package");
+   Tracer::PrintTrace();
+   DiagonalMatrix D(15); D=1.5;
+   Matrix A(15,15);
+   int i,j;
+   for (i=1;i<=15;i++) for (j=1;j<=15;j++) A(i,j)=i*i+j-150;
+   { A = A + D; }
+   ColumnVector B(15);
+   for (i=1;i<=15;i++) B(i)=i+i*i-150.0;
+   {
+      Tracer et1("Stage 1");
+      ColumnVector B1=B;
+      B=(A*2.0).i() * B1;
+      Matrix X = A*B-B1/2.0;
+      Clean(X, 0.000000001); Print(X);
+      A.ReSize(3,5);
+      for (i=1; i<=3; i++) for (j=1; j<=5; j++) A(i,j) = i+100*j;
+
+      B = A.AsColumn()+10000;
+      RowVector R = (A+10000).AsColumn().t();
+      Print( RowVector(R-B.t()) );
+   }
+
+   {
+      Tracer et1("Stage 2");
+      B = A.AsColumn()+10000;
+      Matrix XR = (A+10000).AsMatrix(15,1).t();
+      Print( RowVector(XR-B.t()) );
+   }
+
+   {
+      Tracer et1("Stage 3");
+      B = (A.AsMatrix(15,1)+A.AsColumn())/2.0+10000;
+      Matrix MR = (A+10000).AsColumn().t();
+      Print( RowVector(MR-B.t()) );
+
+      B = (A.AsMatrix(15,1)+A.AsColumn())/2.0;
+      MR = A.AsColumn().t();
+      Print( RowVector(MR-B.t()) );
+   }
+
+   {
+      Tracer et1("Stage 4");
+      B = (A.AsMatrix(15,1)+A.AsColumn())/2.0;
+      RowVector R = A.AsColumn().t();
+      Print( RowVector(R-B.t()) );
+   }
+
+   {
+      Tracer et1("Stage 5");
+      RowVector R = (A.AsColumn()-5000).t();
+      B = ((R.t()+10000) - A.AsColumn())-5000;
+      Print( RowVector(B.t()) );
+   }
+
+   {
+      Tracer et1("Stage 6");
+      B = A.AsColumn(); ColumnVector B1 = (A+10000).AsColumn() - 10000;
+      Print(ColumnVector(B1-B));
+   }
+
+   {
+      Tracer et1("Stage 7");
+      Matrix X = B.AsMatrix(3,5); Print(Matrix(X-A));
+      for (i=1; i<=3; i++) for (j=1; j<=5; j++) B(5*(i-1)+j) -= i+100*j;
+      Print(B);
+   }
+
+   {
+      Tracer et1("Stage 8");
+      A.ReSize(7,7); D.ReSize(7);
+      for (i=1; i<=7; i++) for (j=1; j<=7; j++) A(i,j) = i*j*j;
+      for (i=1; i<=7; i++) D(i,i) = i;
+      UpperTriangularMatrix U; U << A;
+      Matrix X = A; for (i=1; i<=7; i++) X(i,i) = i;
+      A.Inject(D); Print(Matrix(X-A));
+      X = U; U.Inject(D); A = U; for (i=1; i<=7; i++) X(i,i) = i;
+      Print(Matrix(X-A));
+   }
+
+   {
+      Tracer et1("Stage 9");
+      A.ReSize(7,5);
+      for (i=1; i<=7; i++) for (j=1; j<=5; j++) A(i,j) = i+100*j;
+      Matrix Y = A; Y = Y - ((const Matrix&)A); Print(Y);
+      Matrix X = A; // X.Release();
+      Y = A; Y = ((const Matrix&)X) - A; Print(Y); Y = 0.0;
+      Y = ((const Matrix&)X) - ((const Matrix&)A); Print(Y);
+   }
+
+   {
+      Tracer et1("Stage 10");
+      // some tests on submatrices
+      UpperTriangularMatrix U(20);
+      for (i=1; i<=20; i++) for (j=i; j<=20; j++) U(i,j)=100 * i + j;
+      UpperTriangularMatrix V = U.SymSubMatrix(1,5);
+      UpperTriangularMatrix U1 = U;
+      U1.SubMatrix(4,8,5,9) /= 2;
+      U1.SubMatrix(4,8,5,9) += 388 * V;
+      U1.SubMatrix(4,8,5,9) *= 2;
+      U1.SubMatrix(4,8,5,9) += V;
+      U1 -= U; UpperTriangularMatrix U2 = U1;
+      U1 << U1.SubMatrix(4,8,5,9);
+      U2.SubMatrix(4,8,5,9) -= U1; Print(U2);
+      U1 -= (777*V); Print(U1);
+
+      U1 = U; U1.SubMatrix(4,8,5,9) -= U.SymSubMatrix(1,5);
+      U1 -= U;  U2 = U1; U1 << U1.SubMatrix(4,8,5,9);
+      U2.SubMatrix(4,8,5,9) -= U1; Print(U2);
+      U1 += V; Print(U1);
+
+      U1 = U;
+      U1.SubMatrix(3,10,15,19) += 29;
+      U1 -= U;
+      Matrix X = U1.SubMatrix(3,10,15,19); X -= 29; Print(X);
+      U1.SubMatrix(3,10,15,19) *= 0; Print(U1);
+
+      LowerTriangularMatrix L = U.t();
+      LowerTriangularMatrix M = L.SymSubMatrix(1,5);
+      LowerTriangularMatrix L1 = L;
+      L1.SubMatrix(5,9,4,8) /= 2;
+      L1.SubMatrix(5,9,4,8) += 388 * M;
+      L1.SubMatrix(5,9,4,8) *= 2;
+      L1.SubMatrix(5,9,4,8) += M;
+      L1 -= L; LowerTriangularMatrix L2 = L1;
+      L1 << L1.SubMatrix(5,9,4,8);
+      L2.SubMatrix(5,9,4,8) -= L1; Print(L2);
+      L1 -= (777*M); Print(L1);
+
+      L1 = L; L1.SubMatrix(5,9,4,8) -= L.SymSubMatrix(1,5);
+      L1 -= L; L2 =L1; L1 << L1.SubMatrix(5,9,4,8);
+      L2.SubMatrix(5,9,4,8) -= L1; Print(L2);
+      L1 += M; Print(L1);
+
+      L1 = L;
+      L1.SubMatrix(15,19,3,10) -= 29;
+      L1 -= L;
+      X = L1.SubMatrix(15,19,3,10); X += 29; Print(X);
+      L1.SubMatrix(15,19,3,10) *= 0; Print(L1);
+   }
+
+   {
+      Tracer et1("Stage 11");
+      // more tests on submatrices
+      Matrix M(20,30);
+      for (i=1; i<=20; i++) for (j=1; j<=30; j++) M(i,j)=100 * i + j;
+      Matrix M1 = M;
+
+      for (j=1; j<=30; j++)
+         { ColumnVector CV = 3 * M1.Column(j); M.Column(j) += CV; }
+      for (i=1; i<=20; i++)
+         { RowVector RV = 5 * M1.Row(i); M.Row(i) -= RV; }
+
+      M += M1; Print(M);
+ 
+   }
+
+   {
+      Tracer et1("Stage 12");
+      // more tests on Release
+      Matrix M(20,30);
+      for (i=1; i<=20; i++) for (j=1; j<=30; j++) M(i,j)=100 * i + j;
+      Matrix M1 = M;
+      M.Release();
+      Matrix M2 = M;
+      Matrix X = M;   Print(X);
+      X = M1 - M2;    Print(X);
+
+#ifndef DONT_DO_NRIC
+      nricMatrix N = M1;
+      nricMatrix N1 = N;
+      N.Release();
+      nricMatrix N2 = N;
+      nricMatrix Y = N;   Print(Y);
+      Y = N1 - N2;        Print(Y);
+#endif
+
+   }
+
+//   cout << "\nEnd of twelfth test\n";
+}
Index: orxonox/trunk/src/util/newmat/tmtd.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmtd.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmtd.cpp	(revision 4565)
@@ -0,0 +1,196 @@
+
+//#define WANT_STREAM
+
+#include "include.h"
+#include "newmatap.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+ReturnMatrix Inverter(const CroutMatrix& X)
+{
+   Matrix Y = X.i();
+   Y.Release();
+   return Y.ForReturn();
+}
+
+
+void trymatd()
+{
+   Tracer et("Thirteenth test of Matrix package");
+   Tracer::PrintTrace();
+   Matrix X(5,20);
+   int i,j;
+   for (j=1;j<=20;j++) X(1,j) = j+1;
+   for (i=2;i<=5;i++) for (j=1;j<=20; j++) X(i,j) = (long)X(i-1,j) * j % 1001;
+   SymmetricMatrix S; S << X * X.t();
+   Matrix SM = X * X.t() - S;
+   Print(SM);
+   LowerTriangularMatrix L = Cholesky(S);
+   Matrix Diff = L*L.t()-S; Clean(Diff, 0.000000001);
+   Print(Diff);
+   {
+      Tracer et1("Stage 1");
+      LowerTriangularMatrix L1(5);
+      Matrix Xt = X.t(); Matrix Xt2 = Xt;
+      QRZT(X,L1);
+      Diff = L - L1; Clean(Diff,0.000000001); Print(Diff);
+      UpperTriangularMatrix Ut(5);
+      QRZ(Xt,Ut);
+      Diff = L - Ut.t(); Clean(Diff,0.000000001); Print(Diff);
+      Matrix Y(3,20);
+      for (j=1;j<=20;j++) Y(1,j) = 22-j;
+      for (i=2;i<=3;i++) for (j=1;j<=20; j++)
+         Y(i,j) = (long)Y(i-1,j) * j % 101;
+      Matrix Yt = Y.t(); Matrix M,Mt; Matrix Y2=Y;
+      QRZT(X,Y,M); QRZ(Xt,Yt,Mt);
+      Diff = Xt - X.t(); Clean(Diff,0.000000001); Print(Diff);
+      Diff = Yt - Y.t(); Clean(Diff,0.000000001); Print(Diff);
+      Diff = Mt - M.t(); Clean(Diff,0.000000001); Print(Diff);
+      Diff = Y2 * Xt2 * S.i() - M * L.i();
+      Clean(Diff,0.000000001); Print(Diff);
+   }
+
+   ColumnVector C1(5);
+   {
+      Tracer et1("Stage 2");
+      X.ReSize(5,5);
+      for (j=1;j<=5;j++) X(1,j) = j+1;
+      for (i=2;i<=5;i++) for (j=1;j<=5; j++)
+         X(i,j) = (long)X(i-1,j) * j % 1001;
+      for (i=1;i<=5;i++) C1(i) = i*i;
+      CroutMatrix A = X;
+      ColumnVector C2 = A.i() * C1; C1 = X.i()  * C1;
+      X = C1 - C2; Clean(X,0.000000001); Print(X);
+   }
+
+   {
+      Tracer et1("Stage 3");
+      X.ReSize(7,7);
+      for (j=1;j<=7;j++) X(1,j) = j+1;
+      for (i=2;i<=7;i++) for (j=1;j<=7; j++)
+         X(i,j) = (long)X(i-1,j) * j % 1001;
+      C1.ReSize(7);
+      for (i=1;i<=7;i++) C1(i) = i*i;
+      RowVector R1 = C1.t();
+      Diff = R1 * X.i() - ( X.t().i() * R1.t() ).t(); Clean(Diff,0.000000001);
+      Print(Diff);
+   }
+
+   {
+      Tracer et1("Stage 4");
+      X.ReSize(5,5);
+      for (j=1;j<=5;j++) X(1,j) = j+1;
+      for (i=2;i<=5;i++) for (j=1;j<=5; j++)
+         X(i,j) = (long)X(i-1,j) * j % 1001;
+      C1.ReSize(5);
+      for (i=1;i<=5;i++) C1(i) = i*i;
+      CroutMatrix A1 = X*X;
+      ColumnVector C2 = A1.i() * C1; C1 = X.i()  * C1; C1 = X.i()  * C1;
+      X = C1 - C2; Clean(X,0.000000001); Print(X);
+   }
+
+
+   {
+      Tracer et1("Stage 5");
+      int n = 40;
+      SymmetricBandMatrix B(n,2); B = 0.0;
+      for (i=1; i<=n; i++)
+      {
+         B(i,i) = 6;
+         if (i<=n-1) B(i,i+1) = -4;
+         if (i<=n-2) B(i,i+2) = 1;
+      }
+      B(1,1) = 5; B(n,n) = 5;
+      SymmetricMatrix A = B;
+      ColumnVector X(n);
+      X(1) = 429;
+      for (i=2;i<=n;i++) X(i) = (long)X(i-1) * 31 % 1001;
+      X = X / 100000L;
+      // the matrix B is rather ill-conditioned so the difficulty is getting
+      // good agreement (we have chosen X very small) may not be surprising;
+      // maximum element size in B.i() is around 1400
+      ColumnVector Y1 = A.i() * X;
+      LowerTriangularMatrix C1 = Cholesky(A);
+      ColumnVector Y2 = C1.t().i() * (C1.i() * X) - Y1;
+      Clean(Y2, 0.000000001); Print(Y2);
+      UpperTriangularMatrix CU = C1.t().i();
+      LowerTriangularMatrix CL = C1.i();
+      Y2 = CU * (CL * X) - Y1;
+      Clean(Y2, 0.000000001); Print(Y2);
+      Y2 = B.i() * X - Y1; Clean(Y2, 0.000000001); Print(Y2);
+
+      LowerBandMatrix C2 = Cholesky(B);
+      Matrix M = C2 - C1; Clean(M, 0.000000001); Print(M);
+      ColumnVector Y3 = C2.t().i() * (C2.i() * X) - Y1;
+      Clean(Y3, 0.000000001); Print(Y3);
+      CU = C1.t().i();
+      CL = C1.i();
+      Y3 = CU * (CL * X) - Y1;
+      Clean(Y3, 0.000000001); Print(Y3);
+
+      Y3 = B.i() * X - Y1; Clean(Y3, 0.000000001); Print(Y3);
+
+      SymmetricMatrix AI = A.i();
+      Y2 = AI*X - Y1; Clean(Y2, 0.000000001); Print(Y2);
+      SymmetricMatrix BI = B.i();
+      BandMatrix C = B; Matrix CI = C.i();
+      M = A.i() - CI; Clean(M, 0.000000001); Print(M);
+      M = B.i() - CI; Clean(M, 0.000000001); Print(M);
+      M = AI-BI; Clean(M, 0.000000001); Print(M);
+      M = AI-CI; Clean(M, 0.000000001); Print(M);
+
+      M = A; AI << M; M = AI-A; Clean(M, 0.000000001); Print(M);
+      C = B; BI << C; M = BI-B; Clean(M, 0.000000001); Print(M);
+
+
+   }
+
+   {
+      Tracer et1("Stage 5");
+      SymmetricMatrix A(4), B(4);
+      A << 5
+        << 1 << 4
+        << 2 << 1 << 6
+        << 1 << 0 << 1 << 7;
+      B << 8
+        << 1 << 5
+        << 1 << 0 << 9
+        << 2 << 1 << 0 << 6;
+      LowerTriangularMatrix AB = Cholesky(A) * Cholesky(B);
+      Matrix M = Cholesky(A) * B * Cholesky(A).t() - AB*AB.t();
+      Clean(M, 0.000000001); Print(M);
+      M = A * Cholesky(B); M = M * M.t() - A * B * A;
+      Clean(M, 0.000000001); Print(M);
+   }
+   {
+      Tracer et1("Stage 6");
+      int N=49;
+      int i;
+      SymmetricBandMatrix S(N,1);
+      Matrix B(N,N+1); B=0;
+      for (i=1;i<=N;i++) { S(i,i)=1; B(i,i)=1; B(i,i+1)=-1; }
+      for (i=1;i<N; i++) S(i,i+1)=-.5;
+      DiagonalMatrix D(N+1); D = 1;
+      B = B.t()*S.i()*B - (D-1.0/(N+1))*2.0;
+      Clean(B, 0.000000001); Print(B);
+   }
+   {
+      Tracer et1("Stage 7");
+      // See if you can pass a CroutMatrix to a function
+      Matrix A(4,4);
+      A.Row(1) <<  3 <<  2 << -1 <<  4;
+      A.Row(2) << -8 <<  7 <<  2 <<  0;
+      A.Row(3) <<  2 << -2 <<  3 <<  1;
+      A.Row(4) << -1 <<  5 <<  2 <<  2;
+      CroutMatrix B = A;
+      Matrix C = A * Inverter(B) - IdentityMatrix(4);
+      Clean(C, 0.000000001); Print(C);
+   }
+
+
+//   cout << "\nEnd of Thirteenth test\n";
+}
Index: orxonox/trunk/src/util/newmat/tmte.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmte.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmte.cpp	(revision 4565)
@@ -0,0 +1,482 @@
+
+//#define WANT_STREAM
+#define WANT_MATH
+
+#include "include.h"
+
+#include "newmatap.h"
+//#include "newmatio.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+// check D is sorted
+void CheckIsSorted(const DiagonalMatrix& D, bool ascending = false)
+{
+   DiagonalMatrix D1 = D;
+   if (ascending) SortAscending(D1); else SortDescending(D1);
+   D1 -= D; Print(D1);
+}
+
+
+
+void trymate()
+{
+
+
+   Tracer et("Fourteenth test of Matrix package");
+   Tracer::PrintTrace();
+
+   {
+      Tracer et1("Stage 1");
+      Matrix A(8,5);
+      {
+#ifndef ATandT
+         Real   a[] = { 22, 10,  2,  3,  7,
+                        14,  7, 10,  0,  8,
+                        -1, 13, -1,-11,  3,
+		        -3, -2, 13, -2,  4,
+                         9,  8,  1, -2,  4,
+                         9,  1, -7,  5, -1,
+                         2, -6,  6,  5,  1,
+                         4,  5,  0, -2,  2 };
+#else
+         Real a[40];
+         a[ 0]=22; a[ 1]=10; a[ 2]= 2; a[ 3]= 3; a[ 4]= 7;
+         a[ 5]=14; a[ 6]= 7; a[ 7]=10; a[ 8]= 0; a[ 9]= 8;
+         a[10]=-1; a[11]=13; a[12]=-1; a[13]=-11;a[14]= 3;
+         a[15]=-3; a[16]=-2; a[17]=13; a[18]=-2; a[19]= 4;
+         a[20]= 9; a[21]= 8; a[22]= 1; a[23]=-2; a[24]= 4;
+         a[25]= 9; a[26]= 1; a[27]=-7; a[28]= 5; a[29]=-1;
+         a[30]= 2; a[31]=-6; a[32]= 6; a[33]= 5; a[34]= 1;
+         a[35]= 4; a[36]= 5; a[37]= 0; a[38]=-2; a[39]= 2;
+#endif
+         A << a;
+      }
+      DiagonalMatrix D; Matrix U; Matrix V;
+      int anc = A.Ncols(); IdentityMatrix I(anc);
+      SymmetricMatrix S1; S1 << A.t() * A;
+      SymmetricMatrix S2; S2 << A * A.t();
+      Real zero = 0.0; SVD(A+zero,D,U,V); CheckIsSorted(D);
+      DiagonalMatrix D1; SVD(A,D1); CheckIsSorted(D1);
+      Print(DiagonalMatrix(D-D1));
+      Matrix W;
+      SVD(A, D1, W, W, true, false); D1 -= D; W -= U;
+      Clean(W,0.000000001); Print(W); Clean(D1,0.000000001); Print(D1);
+      Matrix WX;
+      SVD(A, D1, WX, W, false, true); D1 -= D; W -= V;
+      Clean(W,0.000000001); Print(W); Clean(D1,0.000000001); Print(D1);
+      Matrix SU = U.t() * U - I; Clean(SU,0.000000001); Print(SU);
+      Matrix SV = V.t() * V - I; Clean(SV,0.000000001); Print(SV);
+      Matrix B = U * D * V.t() - A; Clean(B,0.000000001); Print(B);
+      D1=0.0;  SVD(A,D1,A); CheckIsSorted(D1); Print(Matrix(A-U));
+      D(1) -= sqrt(1248.0); D(2) -= 20; D(3) -= sqrt(384.0);
+      Clean(D,0.000000001); Print(D);
+
+      Jacobi(S1, D, V);  CheckIsSorted(D, true);
+      V = S1 - V * D * V.t(); Clean(V,0.000000001); Print(V);
+      D = D.Reverse(); D(1)-=1248; D(2)-=400; D(3)-=384;
+      Clean(D,0.000000001); Print(D);
+
+      Jacobi(S1, D);  CheckIsSorted(D, true);
+      D = D.Reverse(); D(1)-=1248; D(2)-=400; D(3)-=384;
+      Clean(D,0.000000001); Print(D);
+
+      SymmetricMatrix JW(5);
+      Jacobi(S1, D, JW);  CheckIsSorted(D, true);
+      D = D.Reverse(); D(1)-=1248; D(2)-=400; D(3)-=384;
+      Clean(D,0.000000001); Print(D);
+
+      Jacobi(S2, D, V);  CheckIsSorted(D, true);
+      V = S2 - V * D * V.t(); Clean(V,0.000000001); Print(V);
+      D = D.Reverse(); D(1)-=1248; D(2)-=400; D(3)-=384;
+      Clean(D,0.000000001); Print(D);
+
+      EigenValues(S1, D, V); CheckIsSorted(D, true);
+      V = S1 - V * D * V.t(); Clean(V,0.000000001); Print(V);
+      D(5)-=1248; D(4)-=400; D(3)-=384;
+      Clean(D,0.000000001); Print(D);
+
+      EigenValues(S2, D, V); CheckIsSorted(D, true);
+      V = S2 - V * D * V.t(); Clean(V,0.000000001); Print(V);
+      D(8)-=1248; D(7)-=400; D(6)-=384;
+      Clean(D,0.000000001); Print(D);
+
+      EigenValues(S1, D); CheckIsSorted(D, true);
+      D(5)-=1248; D(4)-=400; D(3)-=384;
+      Clean(D,0.000000001); Print(D);
+
+      SymmetricMatrix EW(S2);
+      EigenValues(S2, D, EW); CheckIsSorted(D, true);
+      D(8)-=1248; D(7)-=400; D(6)-=384;
+      Clean(D,0.000000001); Print(D);
+
+   }
+
+   {
+      Tracer et1("Stage 2");
+      Matrix A(20,21);
+      int i,j;
+      for (i=1; i<=20; i++) for (j=1; j<=21; j++)
+      { if (i>j) A(i,j) = 0; else if (i==j) A(i,j) = 21-i; else A(i,j) = -1; }
+      A = A.t();
+      SymmetricMatrix S1; S1 << A.t() * A;
+      SymmetricMatrix S2; S2 << A * A.t();
+      DiagonalMatrix D; Matrix U; Matrix V;
+#ifdef ATandT
+      int anc = A.Ncols(); DiagonalMatrix I(anc);     // AT&T 2.1 bug
+#else
+      DiagonalMatrix I(A.Ncols());
+#endif
+      I=1.0;
+      SVD(A,D,U,V); CheckIsSorted(D);
+      Matrix SU = U.t() * U - I;    Clean(SU,0.000000001); Print(SU);
+      Matrix SV = V.t() * V - I;    Clean(SV,0.000000001); Print(SV);
+      Matrix B = U * D * V.t() - A; Clean(B,0.000000001);  Print(B);
+      for (i=1; i<=20; i++)  D(i) -= sqrt((22.0-i)*(21.0-i));
+      Clean(D,0.000000001); Print(D);
+      Jacobi(S1, D, V); CheckIsSorted(D, true);
+      V = S1 - V * D * V.t(); Clean(V,0.000000001); Print(V);
+      D = D.Reverse();
+      for (i=1; i<=20; i++)  D(i) -= (22-i)*(21-i);
+      Clean(D,0.000000001); Print(D);
+      Jacobi(S2, D, V); CheckIsSorted(D, true);
+      V = S2 - V * D * V.t(); Clean(V,0.000000001); Print(V);
+      D = D.Reverse();
+      for (i=1; i<=20; i++)  D(i) -= (22-i)*(21-i);
+      Clean(D,0.000000001); Print(D);
+
+      EigenValues(S1, D, V); CheckIsSorted(D, true);
+      V = S1 - V * D * V.t(); Clean(V,0.000000001); Print(V);
+      for (i=1; i<=20; i++)  D(i) -= (i+1)*i;
+      Clean(D,0.000000001); Print(D);
+      EigenValues(S2, D, V); CheckIsSorted(D, true);
+      V = S2 - V * D * V.t(); Clean(V,0.000000001); Print(V);
+      for (i=2; i<=21; i++)  D(i) -= (i-1)*i;
+      Clean(D,0.000000001); Print(D);
+
+      EigenValues(S1, D); CheckIsSorted(D, true);
+      for (i=1; i<=20; i++)  D(i) -= (i+1)*i;
+      Clean(D,0.000000001); Print(D);
+      EigenValues(S2, D); CheckIsSorted(D, true);
+      for (i=2; i<=21; i++)  D(i) -= (i-1)*i;
+      Clean(D,0.000000001); Print(D);
+   }
+
+   {
+      Tracer et1("Stage 3");
+      Matrix A(30,30);
+      int i,j;
+      for (i=1; i<=30; i++) for (j=1; j<=30; j++)
+      { if (i>j) A(i,j) = 0; else if (i==j) A(i,j) = 1; else A(i,j) = -1; }
+      Real d1 = A.LogDeterminant().Value();
+      DiagonalMatrix D; Matrix U; Matrix V;
+#ifdef ATandT
+      int anc = A.Ncols(); DiagonalMatrix I(anc);     // AT&T 2.1 bug
+#else
+      DiagonalMatrix I(A.Ncols());
+#endif
+      I=1.0;
+      SVD(A,D,U,V); CheckIsSorted(D);
+      Matrix SU = U.t() * U - I; Clean(SU,0.000000001); Print(SU);
+      Matrix SV = V.t() * V - I; Clean(SV,0.000000001); Print(SV);
+      Real d2 = D.LogDeterminant().Value();
+      Matrix B = U * D * V.t() - A; Clean(B,0.000000001); Print(B);
+      Real d3 = D.LogDeterminant().Value();
+      ColumnVector Test(3);
+      Test(1) = d1 - 1; Test(2) = d2 - 1; Test(3) = d3 - 1;
+      Clean(Test,0.00000001); Print(Test); // only 8 decimal figures
+      A.ReSize(2,2);
+      Real a = 1.5; Real b = 2; Real c = 2 * (a*a + b*b);
+      A << a << b << a << b;
+      I.ReSize(2); I=1;
+      SVD(A,D,U,V); CheckIsSorted(D);
+      SU = U.t() * U - I; Clean(SU,0.000000001); Print(SU);
+      SV = V.t() * V - I; Clean(SV,0.000000001); Print(SV);
+      B = U * D * V.t() - A; Clean(B,0.000000001); Print(B);
+      D = D*D; SortDescending(D);
+      DiagonalMatrix D50(2); D50 << c << 0; D = D - D50;
+      Clean(D,0.000000001);
+      Print(D);
+      A << a << a << b << b;
+      SVD(A,D,U,V); CheckIsSorted(D);
+      SU = U.t() * U - I; Clean(SU,0.000000001); Print(SU);
+      SV = V.t() * V - I; Clean(SV,0.000000001); Print(SV);
+      B = U * D * V.t() - A; Clean(B,0.000000001); Print(B);
+      D = D*D; SortDescending(D);
+      D = D - D50;
+      Clean(D,0.000000001);
+      Print(D);
+   }
+
+   {
+      Tracer et1("Stage 4");
+
+      // test for bug found by Olof Runborg,
+      // Department of Numerical Analysis and Computer Science (NADA),
+      // KTH, Stockholm
+
+      Matrix A(22,20);
+
+      A = 0;
+
+      int a=1;
+
+      A(a+0,a+2) = 1;     A(a+0,a+18) = -1;
+      A(a+1,a+9) = 1;     A(a+1,a+12) = -1;
+      A(a+2,a+11) = 1;    A(a+2,a+12) = -1;
+      A(a+3,a+10) = 1;    A(a+3,a+19) = -1;
+      A(a+4,a+16) = 1;    A(a+4,a+19) = -1;
+      A(a+5,a+17) = 1;    A(a+5,a+18) = -1;
+      A(a+6,a+10) = 1;    A(a+6,a+4) = -1;
+      A(a+7,a+3) = 1;     A(a+7,a+2) = -1;
+      A(a+8,a+14) = 1;    A(a+8,a+15) = -1;
+      A(a+9,a+13) = 1;    A(a+9,a+16) = -1;
+      A(a+10,a+8) = 1;    A(a+10,a+9) = -1;
+      A(a+11,a+1) = 1;    A(a+11,a+15) = -1;
+      A(a+12,a+16) = 1;   A(a+12,a+4) = -1;
+      A(a+13,a+6) = 1;    A(a+13,a+9) = -1;
+      A(a+14,a+5) = 1;    A(a+14,a+4) = -1;
+      A(a+15,a+0) = 1;    A(a+15,a+1) = -1;
+      A(a+16,a+14) = 1;   A(a+16,a+0) = -1;
+      A(a+17,a+7) = 1;    A(a+17,a+6) = -1;
+      A(a+18,a+13) = 1;   A(a+18,a+5) = -1;
+      A(a+19,a+7) = 1;    A(a+19,a+8) = -1;
+      A(a+20,a+17) = 1;   A(a+20,a+3) = -1;
+      A(a+21,a+6) = 1;    A(a+21,a+11) = -1;
+
+
+      Matrix U, V; DiagonalMatrix S;
+
+      SVD(A, S, U, V, true, true); CheckIsSorted(S);
+
+      DiagonalMatrix D(20); D = 1;
+
+      Matrix tmp = U.t() * U - D;
+      Clean(tmp,0.000000001); Print(tmp);
+
+      tmp = V.t() * V - D;
+      Clean(tmp,0.000000001); Print(tmp);
+
+      tmp = U * S * V.t() - A ;
+      Clean(tmp,0.000000001); Print(tmp);
+
+   }
+
+   {
+      Tracer et1("Stage 5");
+      Matrix A(10,10);
+
+      A.Row(1)  <<  1.00 <<  0.07 <<  0.05 <<  0.00 <<  0.06
+                <<  0.09 <<  0.03 <<  0.02 <<  0.02 << -0.03;
+      A.Row(2)  <<  0.07 <<  1.00 <<  0.05 <<  0.05 << -0.03
+                <<  0.07 <<  0.00 <<  0.07 <<  0.00 <<  0.02;
+      A.Row(3)  <<  0.05 <<  0.05 <<  1.00 <<  0.05 <<  0.02
+                <<  0.01 << -0.05 <<  0.04 <<  0.05 << -0.03;
+      A.Row(4)  <<  0.00 <<  0.05 <<  0.05 <<  1.00 << -0.05
+                <<  0.04 <<  0.01 <<  0.02 << -0.05 <<  0.00;
+      A.Row(5)  <<  0.06 << -0.03 <<  0.02 << -0.05 <<  1.00
+                << -0.03 <<  0.02 << -0.02 <<  0.04 <<  0.00;
+      A.Row(6)  <<  0.09 <<  0.07 <<  0.01 <<  0.04 << -0.03
+                <<  1.00 << -0.06 <<  0.08 << -0.02 << -0.10;
+      A.Row(7)  <<  0.03 <<  0.00 << -0.05 <<  0.01 <<  0.02
+                << -0.06 <<  1.00 <<  0.09 <<  0.12 << -0.03;
+      A.Row(8)  <<  0.02 <<  0.07 <<  0.04 <<  0.02 << -0.02
+                <<  0.08 <<  0.09 <<  1.00 <<  0.00 << -0.02;
+      A.Row(9)  <<  0.02 <<  0.00 <<  0.05 << -0.05 <<  0.04
+                << -0.02 <<  0.12 <<  0.00 <<  1.00 <<  0.02;
+      A.Row(10) << -0.03 <<  0.02 << -0.03 <<  0.00 <<  0.00
+                << -0.10 << -0.03 << -0.02 <<  0.02 <<  1.00;
+
+      SymmetricMatrix AS; AS << A;
+      Matrix V; DiagonalMatrix D, D1;
+      ColumnVector Check(6);
+      EigenValues(AS,D,V); CheckIsSorted(D, true);
+      Check(1) = MaximumAbsoluteValue(A - V * D * V.t());
+      DiagonalMatrix I(10); I = 1;
+      Check(2) = MaximumAbsoluteValue(V * V.t() - I);
+      Check(3) = MaximumAbsoluteValue(V.t() * V - I);
+
+      EigenValues(AS, D1); CheckIsSorted(D1, true);
+      D -= D1;
+      Clean(D,0.000000001); Print(D);
+
+      Jacobi(AS,D,V);
+      Check(4) = MaximumAbsoluteValue(A - V * D * V.t());
+      Check(5) = MaximumAbsoluteValue(V * V.t() - I);
+      Check(6) = MaximumAbsoluteValue(V.t() * V - I);
+
+      SortAscending(D);
+      D -= D1;
+      Clean(D,0.000000001); Print(D);
+
+      Clean(Check,0.000000001); Print(Check);
+
+      // Check loading rows
+
+      SymmetricMatrix B(10);
+
+      B.Row(1)  <<  1.00;
+      B.Row(2)  <<  0.07 <<  1.00;
+      B.Row(3)  <<  0.05 <<  0.05 <<  1.00;
+      B.Row(4)  <<  0.00 <<  0.05 <<  0.05 <<  1.00;
+      B.Row(5)  <<  0.06 << -0.03 <<  0.02 << -0.05 <<  1.00;
+      B.Row(6)  <<  0.09 <<  0.07 <<  0.01 <<  0.04 << -0.03
+                <<  1.00;
+      B.Row(7)  <<  0.03 <<  0.00 << -0.05 <<  0.01 <<  0.02
+                << -0.06 <<  1.00;
+      B.Row(8)  <<  0.02 <<  0.07 <<  0.04 <<  0.02 << -0.02
+                <<  0.08 <<  0.09 <<  1.00;
+      B.Row(9)  <<  0.02 <<  0.00 <<  0.05 << -0.05 <<  0.04
+                << -0.02 <<  0.12 <<  0.00 <<  1.00;
+      B.Row(10) << -0.03 <<  0.02 << -0.03 <<  0.00 <<  0.00
+                << -0.10 << -0.03 << -0.02 <<  0.02 <<  1.00;
+
+      B -= AS; Print(B);
+
+   }
+
+   {
+      Tracer et1("Stage 6");
+      // badly scaled matrix
+      Matrix A(9,9);
+
+      A.Row(1) << 1.13324e+012 << 3.68788e+011 << 3.35163e+009
+               << 3.50193e+011 << 1.25335e+011 << 1.02212e+009
+               << 3.16602e+009 << 1.02418e+009 << 9.42959e+006;
+      A.Row(2) << 3.68788e+011 << 1.67128e+011 << 1.27449e+009
+               << 1.25335e+011 << 6.05413e+010 << 4.34573e+008
+               << 1.02418e+009 << 4.69192e+008 << 3.61098e+006;
+      A.Row(3) << 3.35163e+009 << 1.27449e+009 << 1.25571e+007
+               << 1.02212e+009 << 4.34573e+008 << 3.69769e+006
+               << 9.42959e+006 << 3.61098e+006 << 3.59450e+004;
+      A.Row(4) << 3.50193e+011 << 1.25335e+011 << 1.02212e+009
+               << 1.43514e+011 << 5.42310e+010 << 4.15822e+008
+               << 1.23068e+009 << 4.31545e+008 << 3.58714e+006;
+      A.Row(5) << 1.25335e+011 << 6.05413e+010 << 4.34573e+008
+               << 5.42310e+010 << 2.76601e+010 << 1.89102e+008
+               << 4.31545e+008 << 2.09778e+008 << 1.51083e+006;
+      A.Row(6) << 1.02212e+009 << 4.34573e+008 << 3.69769e+006
+               << 4.15822e+008 << 1.89102e+008 << 1.47143e+006
+               << 3.58714e+006 << 1.51083e+006 << 1.30165e+004;
+      A.Row(7) << 3.16602e+009 << 1.02418e+009 << 9.42959e+006
+               << 1.23068e+009 << 4.31545e+008 << 3.58714e+006
+               << 1.12335e+007 << 3.54778e+006 << 3.34311e+004;
+      A.Row(8) << 1.02418e+009 << 4.69192e+008 << 3.61098e+006
+               << 4.31545e+008 << 2.09778e+008 << 1.51083e+006
+               << 3.54778e+006 << 1.62552e+006 << 1.25885e+004;
+      A.Row(9) << 9.42959e+006 << 3.61098e+006 << 3.59450e+004
+               << 3.58714e+006 << 1.51083e+006 << 1.30165e+004
+               << 3.34311e+004 << 1.25885e+004 << 1.28000e+002;
+
+
+      SymmetricMatrix AS; AS << A;
+      Matrix V; DiagonalMatrix D, D1;
+      ColumnVector Check(6);
+      EigenValues(AS,D,V); CheckIsSorted(D, true);
+      Check(1) = MaximumAbsoluteValue(A - V * D * V.t()) / 100000;
+      DiagonalMatrix I(9); I = 1;
+      Check(2) = MaximumAbsoluteValue(V * V.t() - I);
+      Check(3) = MaximumAbsoluteValue(V.t() * V - I);
+
+      EigenValues(AS, D1);
+      D -= D1;
+      Clean(D,0.001); Print(D);
+
+      Jacobi(AS,D,V);
+      Check(4) = MaximumAbsoluteValue(A - V * D * V.t()) / 100000;
+      Check(5) = MaximumAbsoluteValue(V * V.t() - I);
+      Check(6) = MaximumAbsoluteValue(V.t() * V - I);
+
+      SortAscending(D);
+      D -= D1;
+      Clean(D,0.001); Print(D);
+
+      Clean(Check,0.0000001); Print(Check);
+   }
+
+   {
+      Tracer et1("Stage 7");
+      // matrix with all singular values close to 1
+      Matrix A(8,8);
+      A.Row(1)<<-0.4343<<-0.0445<<-0.4582<<-0.1612<<-0.3191<<-0.6784<<0.1068<<0;
+      A.Row(2)<<0.5791<<0.5517<<0.2575<<-0.1055<<-0.0437<<-0.5282<<0.0442<<0;
+      A.Row(3)<<0.5709<<-0.5179<<-0.3275<<0.2598<<-0.196<<-0.1451<<-0.4143<<0;
+      A.Row(4)<<0.2785<<-0.5258<<0.1251<<-0.4382<<0.0514<<-0.0446<<0.6586<<0;
+      A.Row(5)<<0.2654<<0.3736<<-0.7436<<-0.0122<<0.0376<<0.3465<<0.3397<<0;
+      A.Row(6)<<0.0173<<-0.0056<<-0.1903<<-0.7027<<0.4863<<-0.0199<<-0.4825<<0;
+      A.Row(7)<<0.0434<<0.0966<<0.1083<<-0.4576<<-0.7857<<0.3425<<-0.1818<<0;
+      A.Row(8)<<0.0<<0.0<<0.0<<0.0<<0.0<<0.0<<0.0<<-1.0;
+      Matrix U,V; DiagonalMatrix D;
+      SVD(A,D,U,V); CheckIsSorted(D);
+      Matrix B = U * D * V.t() - A; Clean(B,0.000000001); Print(B);
+      DiagonalMatrix I(8); I = 1; D -= I; Clean(D,0.0001); Print(D);
+      U *= U.t(); U -= I; Clean(U,0.000000001); Print(U);
+      V *= V.t(); V -= I; Clean(V,0.000000001); Print(V);
+
+   }
+
+   {
+      Tracer et1("Stage 8");
+      // check SortSV functions
+
+      Matrix A(15, 10);
+      int i, j;
+      for (i = 1; i <= 15; ++i) for (j = 1; j <= 10; ++j)
+         A(i, j) = i + j / 1000.0;
+      DiagonalMatrix D(10);
+      D << 0.2 << 0.5 << 0.1 << 0.7 << 0.8 << 0.3 << 0.4 << 0.7 << 0.9 << 0.6;
+      Matrix U = A; Matrix V = 10 - 2 * A;
+      Matrix Prod = U * D * V.t();
+
+      DiagonalMatrix D2 = D; SortDescending(D2);
+      DiagonalMatrix D1 = D; SortSV(D1, U, V); Matrix X = D1 - D2; Print(X);
+      X = Prod - U * D1 * V.t(); Clean(X,0.000000001); Print(X);
+      U = A; V = 10 - 2 * A;
+      D1 = D; SortSV(D1, U); X = D1 - D2; Print(X);
+      D1 = D; SortSV(D1, V); X = D1 - D2; Print(X);
+      X = Prod - U * D1 * V.t(); Clean(X,0.000000001); Print(X);
+
+      D2 = D; SortAscending(D2);
+      U = A; V = 10 - 2 * A;
+      D1 = D; SortSV(D1, U, V, true); X = D1 - D2; Print(X);
+      X = Prod - U * D1 * V.t(); Clean(X,0.000000001); Print(X);
+      U = A; V = 10 - 2 * A;
+      D1 = D; SortSV(D1, U, true); X = D1 - D2; Print(X);
+      D1 = D; SortSV(D1, V, true); X = D1 - D2; Print(X);
+      X = Prod - U * D1 * V.t(); Clean(X,0.000000001); Print(X);
+   }
+
+   {
+      Tracer et1("Stage 9");
+      // Tom William's example
+      Matrix A(10,10);
+      Matrix U;
+      Matrix V;
+      DiagonalMatrix Sigma;
+      Real myVals[] =
+      {
+         1,    1,    1,    1,    1,    1,    1,    1,    1,    1,
+         1,    1,    1,    1,    1,    1,    1,    1,    1,    1,
+         1,    1,    1,    1,    1,    1,    1,    1,    1,    1,
+         1,    1,    1,    1,    1,    1,    1,    1,    1,    1,
+         1,    1,    1,    1,    1,    1,    1,    1,    1,    1,
+         1,    1,    1,    1,    1,    1,    1,    1,    1,    0,
+         1,    1,    1,    1,    1,    1,    1,    1,    1,    0,
+         1,    1,    1,    1,    1,    1,    1,    1,    0,    0,
+         1,    1,    1,    1,    1,    1,    1,    0,    0,    0,
+         1,    1,    1,    1,    1,    0,    0,    0,    0,    0,
+      };
+
+      A << myVals;
+      SVD(A, Sigma, U, V); CheckIsSorted(Sigma);
+      A -= U * Sigma * V.t();
+      Clean(A, 0.000000001); Print(A);
+   }
+
+
+
+}
Index: orxonox/trunk/src/util/newmat/tmtf.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmtf.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmtf.cpp	(revision 4565)
@@ -0,0 +1,404 @@
+
+//#define WANT_STREAM
+#define WANT_MATH
+
+#include "include.h"
+
+#include "newmatap.h"
+
+//#include "newmatio.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+
+
+static void SlowFT(const ColumnVector& a, const ColumnVector&b,
+   ColumnVector& x, ColumnVector& y)
+{
+   int n = a.Nrows();
+   x.ReSize(n); y.ReSize(n);
+   Real f = 6.2831853071795864769/n;
+   for (int j=1; j<=n; j++)
+   {
+      Real sumx = 0.0; Real sumy = 0.0;
+      for (int k=1; k<=n; k++)
+      {
+	 Real theta = - (j-1) * (k-1) * f;
+	 Real c = cos(theta); Real s = sin(theta);
+	 sumx += c * a(k) - s * b(k); sumy += s * a(k) + c * b(k);
+      }
+      x(j) = sumx; y(j) = sumy;
+   }
+}
+
+static void SlowDTT_II(const ColumnVector& a, ColumnVector& c, ColumnVector& s)
+{
+   int n = a.Nrows(); c.ReSize(n); s.ReSize(n);
+   Real f = 6.2831853071795864769 / (4*n);
+   int k;
+
+   for (k=1; k<=n; k++)
+   {
+      Real sum = 0.0;
+      const int k1 = k-1;              // otherwise Visual C++ 5 fails
+      for (int j=1; j<=n; j++) sum += cos(k1 * (2*j-1) * f) * a(j);
+      c(k) = sum;
+   }
+
+   for (k=1; k<=n; k++)
+   {
+      Real sum = 0.0;
+      for (int j=1; j<=n; j++) sum += sin(k * (2*j-1) * f) * a(j);
+      s(k) = sum;
+   }
+}
+
+static void SlowDTT(const ColumnVector& a, ColumnVector& c, ColumnVector& s)
+{
+   int n1 = a.Nrows(); int n = n1 - 1;
+   c.ReSize(n1); s.ReSize(n1);
+   Real f = 6.2831853071795864769 / (2*n);
+   int k;
+
+   int sign = 1;
+   for (k=1; k<=n1; k++)
+   {
+      Real sum = 0.0;
+      for (int j=2; j<=n; j++) sum += cos((j-1) * (k-1) * f) * a(j);
+      c(k) = sum + (a(1) + sign * a(n1)) / 2.0;
+      sign = -sign;
+   }
+
+   for (k=2; k<=n; k++)
+   {
+      Real sum = 0.0;
+      for (int j=2; j<=n; j++) sum += sin((j-1) * (k-1) * f) * a(j);
+      s(k) = sum;
+   }
+   s(1) = s(n1) = 0;
+}
+
+static void test(int n)
+{
+   Tracer et("Test FFT");
+
+   ColumnVector A(n), B(n), X, Y;
+   for (int i=0; i<n; i++)
+   {
+      Real f = 6.2831853071795864769*i/n;
+      A.element(i) = fabs(sin(7.0*f) + 0.5 * cos(19.0 * f)) + (Real)i/(Real)n;
+      B.element(i) = fabs(0.25 * cos(31.0 * f)) + (Real)i/(Real)n;
+   }
+   FFT(A, B, X, Y); FFTI(X, Y, X, Y);
+   X = X - A; Y = Y - B;
+   Clean(X,0.000000001); Clean(Y,0.000000001); Print(X); Print(Y);
+}
+
+static void test1(int n)
+{
+   Tracer et("Test RealFFT");
+
+   ColumnVector A(n), B(n), X, Y;
+   for (int i=0; i<n; i++)
+   {
+      Real f = 6.2831853071795864769*i/n;
+      A.element(i) = fabs(sin(7.0*f) + 0.5 * cos(19.0 * f)) + (Real)i/(Real)n;
+   }
+   B = 0.0;
+   FFT(A, B, X, Y);
+   B.CleanUp();                 // release some space
+   int n2 = n/2+1;
+   ColumnVector X1,Y1,X2,Y2;
+   RealFFT(A, X1, Y1);
+   X2 = X1 - X.Rows(1,n2); Y2 = Y1 - Y.Rows(1,n2);
+   Clean(X2,0.000000001); Clean(Y2,0.000000001); Print(X2); Print(Y2);
+   X2.CleanUp(); Y2.CleanUp();  // release some more space
+   RealFFTI(X1,Y1,B);
+   B = A - B;
+   Clean(B,0.000000001); Print(B);
+}
+
+static void test2(int n)
+{
+   Tracer et("cf FFT and slow FT");
+
+   ColumnVector A(n), B(n), X, Y, X1, Y1;
+   for (int i=0; i<n; i++)
+   {
+      Real f = 6.2831853071795864769*i/n;
+      A.element(i) = fabs(sin(7.0*f) - 0.5 * cos(19.0 * f)) + (Real)i/(Real)n;
+      B.element(i) = fabs(0.25 * cos(31.0 * f)) - (Real)i/(Real)n;
+   }
+   FFT(A, B, X, Y);
+   SlowFT(A, B, X1, Y1);
+   X = X - X1; Y = Y - Y1;
+   Clean(X,0.000000001); Clean(Y,0.000000001); Print(X); Print(Y);
+}
+
+static void test3(int n)
+{
+   Tracer et("cf slow and fast DCT_II and DST_II");
+
+   ColumnVector A(n), X, Y, X1, Y1;
+   for (int i=0; i<n; i++)
+   {
+      Real f = 6.2831853071795864769*i/n;
+      A.element(i) = fabs(sin(7.0*f) - 0.55 * cos(19.0 * f)
+		  + .73 * sin(6.0 * f)) + (Real)i/(Real)n;
+   }
+   DCT_II(A, X); DST_II(A, Y);
+   SlowDTT_II(A, X1, Y1);
+   X -= X1; Y -= Y1;
+   Clean(X,0.000000001); Clean(Y,0.000000001); Print(X); Print(Y);
+}
+
+static void test4(int n)
+{
+   Tracer et("Test DCT_II");
+
+   ColumnVector A1(n);
+   for (int i=0; i<n; i++)
+   {
+      Real f = 6.2831853071795864769*i/n;
+      A1.element(i) =
+         fabs(sin(7.0*f) + 0.7588 * cos(19.0 * f) + (Real)i/(Real)n);
+   }
+   // do DCT II by ordinary FFT
+   ColumnVector P(2*n), Q(2*n);
+   P = 0.0; Q = 0.0; P.Rows(1,n) = A1;
+   FFT(P, Q, P, Q);
+   ColumnVector B1(n);
+   for (int k=0; k<n; k++)
+   {
+      Real arg = k * 6.2831853071795864769 / (4 * n);
+      B1(k+1) = P(k+1) * cos(arg) + Q(k+1) * sin(arg);
+   }
+   // use DCT_II routine
+   ColumnVector B2;
+   DCT_II(A1,B2);
+   // test inverse
+   ColumnVector A2;
+   DCT_II_inverse(B2,A2);
+   A1 -= A2; B1 -= B2;
+   Clean(A1,0.000000001); Clean(B1,0.000000001); Print(A1); Print(B1);
+}
+
+static void test5(int n)
+{
+   Tracer et("Test DST_II");
+
+   ColumnVector A1(n);
+   for (int i=0; i<n; i++)
+   {
+      Real f = 6.2831853071795864769*i/n;
+      A1.element(i) =
+         fabs(sin(11.0*f) + 0.7588 * cos(19.0 * f) + (Real)i/(Real)n);
+   }
+   // do DST II by ordinary FFT
+   ColumnVector P(2*n), Q(2*n);
+   P = 0.0; Q = 0.0; P.Rows(1,n) = A1;
+   FFT(P, Q, P, Q);
+   ColumnVector B1(n);
+   for (int k=1; k<=n; k++)
+   {
+      Real arg = k * 6.2831853071795864769 / (4 * n);
+      B1(k) = P(k+1) * sin(arg) - Q(k+1) * cos(arg);
+   }
+   // use DST_II routine
+   ColumnVector B2;
+   DST_II(A1,B2);
+   // test inverse
+   ColumnVector A2;
+   DST_II_inverse(B2,A2);
+   A1 -= A2; B1 -= B2;
+   Clean(A1,0.000000001); Clean(B1,0.000000001); Print(A1); Print(B1);
+}
+
+static void test6(int n)
+{
+   Tracer et("Test DST");
+
+   ColumnVector A1(n+1);
+   A1(1) = A1(n+1) = 0;
+   for (int i=1; i<n; i++)
+   {
+      Real f = 6.2831853071795864769*i/n;
+      A1.element(i) =
+         fabs(sin(11.0*f) + 0.7588 * cos(19.0 * f) + (Real)i/(Real)n);
+   }
+   // do DST by ordinary FFT
+   ColumnVector P(2*n), Q(2*n); P = 0.0; Q = 0.0; P.Rows(1,n+1) = A1;
+   FFT(P, Q, P, Q);
+   ColumnVector B1 = -Q.Rows(1,n+1);
+   // use DST routine
+   ColumnVector B2;
+   DST(A1,B2);
+   // test inverse
+   ColumnVector A2;
+   DST_inverse(B2,A2);
+   A1 -= A2; B1 -= B2;
+   Clean(A1,0.000000001); Clean(B1,0.000000001); Print(A1); Print(B1);
+}
+
+
+
+static void test7(int n)
+{
+   Tracer et("Test DCT");
+
+   ColumnVector A1(n+1);
+   for (int i=0; i<=n; i++)
+   {
+      Real f = 6.2831853071795864769*i/n;
+      A1.element(i) =
+         fabs(sin(17.0*f) + 0.6399 * cos(23.0 * f) + 1.32*(Real)i/(Real)n);
+   }
+   // do DCT by ordinary FFT
+   ColumnVector P(2*n), Q(2*n); P = 0.0; Q = 0.0; P.Rows(1,n+1) = A1;
+   P(1) /= 2.0; P(n+1) /= 2.0;
+   FFT(P, Q, P, Q);
+   ColumnVector B1 = P.Rows(1,n+1);
+   // use DCT routine
+   ColumnVector B2;
+   DCT(A1,B2);
+   // test inverse
+   ColumnVector A2;
+   DCT_inverse(B2,A2);
+   A1 -= A2; B1 -= B2;
+   Clean(A1,0.000000001); Clean(B1,0.000000001); Print(A1); Print(B1);
+}
+
+static void test8(int n)
+{
+   Tracer et("cf slow and fast DCT and DST");
+
+   ColumnVector A(n+1), X, Y, X1, Y1;
+   for (int i=0; i<=n; i++)
+   {
+      Real f = 6.2831853071795864769*i/n;
+      A.element(i) = fabs(sin(7.0*f) - 0.5 * cos(19.0 * f) +
+         0.3 * (Real)i/(Real)n);
+   }
+   DCT(A, X); DST(A, Y);
+   SlowDTT(A, X1, Y1);
+   X -= X1; Y -= Y1;
+   Clean(X,0.000000001); Clean(Y,0.000000001); Print(X); Print(Y);
+}
+
+
+
+
+void trymatf()
+{
+   Tracer et("Fifteenth test of Matrix package");
+   Tracer::PrintTrace();
+
+   int i;
+   ColumnVector A(12), B(12);
+   for (i = 1; i <=12; i++)
+   {
+      Real i1 = i - 1;
+      A(i) = .7
+	   + .2 * cos(6.2831853071795864769 * 4.0 * i1 / 12)
+	   + .3 * sin(6.2831853071795864769 * 3.0 * i1 / 12);
+      B(i) = .9
+	   + .5 * sin(6.2831853071795864769 * 2.0 * i1 / 12)
+	   + .4 * cos(6.2831853071795864769 * 1.0 * i1 / 12);
+   }
+   FFT(A, B, A, B);
+   A(1) -= 8.4; A(3) -= 3.0; A(5) -= 1.2; A(9) -= 1.2; A(11) += 3.0;
+   B(1) -= 10.8; B(2) -= 2.4; B(4) += 1.8; B(10) -= 1.8; B(12) -= 2.4;
+   Clean(A,0.000000001); Clean(B,0.000000001); Print(A); Print(B);
+
+
+   // test FFT
+   test(2048); test(2000); test(27*81); test(2310); test(49*49);
+   test(13*13*13); test(43*47);
+   test(16*16*3); test(16*16*5); test(16*16*7);
+   test(8*8*5);
+
+   // test realFFT
+   test1(2); test1(98); test1(22); test1(100);
+   test1(2048); test1(2000); test1(35*35*2);
+
+   // compare FFT and slowFFT
+   test2(1); test2(13); test2(12); test2(9); test2(16); test2(30); test2(42);
+   test2(24); test2(8); test2(40); test2(48); test2(4); test2(3); test2(5);
+   test2(32); test2(2);
+
+   // compare DCT_II, DST_II and slow versions
+   test3(2); test3(26); test3(32); test3(18);
+
+   // test DCT_II and DST_II
+   test4(2); test5(2);
+   test4(202); test5(202);
+   test4(1000); test5(1000);
+
+   // test DST and DCT
+   test6(2); test7(2);
+   test6(274); test7(274);
+   test6(1000); test7(1000);
+
+   // compare DCT, DST and slow versions
+   test8(2); test8(26); test8(32); test8(18);
+
+
+   // now do the same thing all over again forcing use of old FFT
+   FFT_Controller::OnlyOldFFT = true;
+
+   for (i = 1; i <=12; i++)
+   {
+      Real i1 = i - 1;
+      A(i) = .7
+	   + .2 * cos(6.2831853071795864769 * 4.0 * i1 / 12)
+	   + .3 * sin(6.2831853071795864769 * 3.0 * i1 / 12);
+      B(i) = .9
+	   + .5 * sin(6.2831853071795864769 * 2.0 * i1 / 12)
+	   + .4 * cos(6.2831853071795864769 * 1.0 * i1 / 12);
+   }
+   FFT(A, B, A, B);
+   A(1) -= 8.4; A(3) -= 3.0; A(5) -= 1.2; A(9) -= 1.2; A(11) += 3.0;
+   B(1) -= 10.8; B(2) -= 2.4; B(4) += 1.8; B(10) -= 1.8; B(12) -= 2.4;
+   Clean(A,0.000000001); Clean(B,0.000000001); Print(A); Print(B);
+
+
+   // test FFT
+   test(2048); test(2000); test(27*81); test(2310); test(49*49);
+   test(13*13*13); test(43*47);
+   test(16*16*3); test(16*16*5); test(16*16*7);
+   test(8*8*5);
+
+   // test realFFT
+   test1(2); test1(98); test1(22); test1(100);
+   test1(2048); test1(2000); test1(35*35*2);
+
+   // compare FFT and slowFFT
+   test2(1); test2(13); test2(12); test2(9); test2(16); test2(30); test2(42);
+   test2(24); test2(8); test2(40); test2(48); test2(4); test2(3); test2(5);
+   test2(32); test2(2);
+
+   // compare DCT_II, DST_II and slow versions
+   test3(2); test3(26); test3(32); test3(18);
+
+   // test DCT_II and DST_II
+   test4(2); test5(2);
+   test4(202); test5(202);
+   test4(1000); test5(1000);
+
+   // test DST and DCT
+   test6(2); test7(2);
+   test6(274); test7(274);
+   test6(1000); test7(1000);
+
+   // compare DCT, DST and slow versions
+   test8(2); test8(26); test8(32); test8(18);
+
+   FFT_Controller::OnlyOldFFT = false;
+
+
+
+}
Index: orxonox/trunk/src/util/newmat/tmtg.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmtg.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmtg.cpp	(revision 4565)
@@ -0,0 +1,154 @@
+
+//#define WANT_STREAM
+
+#define WANT_MATH                   // for sqrt
+
+#include "include.h"
+
+#include "newmatap.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+
+
+void trymatg()
+{
+//   cout << "\nSixteenth test of Matrix package\n";
+//   cout << "\n";
+   Tracer et("Sixteenth test of Matrix package");
+   Tracer::PrintTrace();
+
+   int i,j;
+   Matrix M(4,7);
+   for (i=1; i<=4; i++) for (j=1; j<=7; j++) M(i,j) = 100 * i + j;
+   ColumnVector CV = M.AsColumn();
+   {
+      Tracer et1("Stage 1");
+      RowVector test(7);
+      test(1) = SumSquare(M);
+      test(2) = SumSquare(CV);
+      test(3) = SumSquare(CV.t());
+      test(4) = SumSquare(CV.AsDiagonal());
+      test(5) = SumSquare(M.AsColumn());
+      test(6) = Matrix(CV.t()*CV)(1,1);
+      test(7) = (CV.t()*CV).AsScalar();
+      test = test - 2156560.0; Print(test);
+   }
+
+   UpperTriangularMatrix U(6);
+   for (i=1; i<=6; i++) for (j=i; j<=6; j++) U(i,j) = i + (i-j) * (i-j);
+   M = U; DiagonalMatrix D; D << U;
+   LowerTriangularMatrix L = U.t(); SymmetricMatrix S; S << (L+U)/2.0;
+   {
+      Tracer et1("Stage 2");
+      RowVector test(6);
+      test(1) = U.Trace();
+      test(2) = L.Trace();
+      test(3) = D.Trace();
+      test(4) = S.Trace();
+      test(5) = M.Trace();
+      test(6) = ((L+U)/2.0).Trace();
+      test = test - 21; Print(test);
+      test(1) = LogDeterminant(U).Value();
+      test(2) = LogDeterminant(L).Value();
+      test(3) = LogDeterminant(D).Value();
+      test(4) = LogDeterminant(D).Value();
+      test(5) = LogDeterminant((L+D)/2.0).Value();
+      test(6) = Determinant((L+D)/2.0);
+      test = test - 720; Clean(test,0.000000001); Print(test);
+   }
+
+   {
+      Tracer et1("Stage 3");
+      S << L*U; M = S;
+      RowVector test(8);
+      test(1) = LogDeterminant(S).Value();
+      test(2) = LogDeterminant(M).Value();
+      test(3) = LogDeterminant(L*U).Value();
+      test(4) = LogDeterminant(Matrix(L*L)).Value();
+      test(5) = Determinant(S);
+      test(6) = Determinant(M);
+      test(7) = Determinant(L*U);
+      test(8) = Determinant(Matrix(L*L));
+      test = test - 720.0 * 720.0; Clean(test,0.00000001); Print(test);
+   }
+
+   {
+      Tracer et1("Stage 4");
+      M = S * S;
+      Matrix SX = S;
+      RowVector test(3);
+      test(1) = SumSquare(S);
+      test(2) = SumSquare(SX);
+      test(3) = Trace(M);
+		test = test - 3925961.0; Print(test);
+   }
+   {
+      Tracer et1("Stage 5");
+      SymmetricMatrix SM(10), SN(10);
+      Real S = 0.0;
+      for (i=1; i<=10; i++) for (j=i; j<=10; j++)
+      {
+         SM(i,j) = 1.5 * i - j; SN(i,j) = SM(i,j) * SM(i,j);
+         if (SM(i,j) < 0.0)   SN(i,j) = - SN(i,j);
+         S += SN(i,j) * ((i==j) ? 1.0 : 2.0);
+      }
+      Matrix M = SM, N = SN; RowVector test(4);
+      test(1) = SumAbsoluteValue(SN);
+      test(2) = SumAbsoluteValue(-SN);
+      test(3) = SumAbsoluteValue(N);
+      test(4) = SumAbsoluteValue(-N);
+      test = test - 1168.75; Print(test);
+      test(1) = Sum(SN);
+      test(2) = -Sum(-SN);
+      test(3) = Sum(N);
+      test(4) = -Sum(-N);
+      test = test - S; Print(test);
+      test(1) = MaximumAbsoluteValue(SM);
+      test(2) = MaximumAbsoluteValue(-SM);
+      test(3) = MaximumAbsoluteValue(M);
+      test(4) = MaximumAbsoluteValue(-M);
+      test = test - 8.5; Print(test);
+   }
+   {
+      Tracer et1("Stage 6");
+      Matrix M(15,20); Real value = 0.0;
+      for (i=1; i<=15; i++) { for (j=1; j<=20; j++) M(i,j) = 1.5 * i - j; }
+      for (i=1; i<=20; i++)
+      { Real v = SumAbsoluteValue(M.Column(i)); if (value<v) value = v; }
+      RowVector test(3);
+      test(1) = value;
+      test(2) = Norm1(M);
+      test(3) = NormInfinity(M.t());
+      test = test - 165; Print(test);
+      test.ReSize(5);
+      ColumnVector CV = M.AsColumn(); M = CV.t();
+      test(1) = Norm1(CV.t());
+      test(2) = MaximumAbsoluteValue(M);
+      test(3) = NormInfinity(CV);
+      test(4) = Norm1(M);
+      test(5) = NormInfinity(M.t());
+      test = test - 21.5; Print(test);
+   }
+   {
+      Tracer et1("Stage 7");
+      Matrix M(15,20);
+      for (i=1; i<=15; i++) { for (j=1; j<=20; j++) M(i,j) = 2.5 * i - j; }
+      SymmetricMatrix SM; SM << M * M.t();
+      ColumnVector test(6);
+      test(1) = sqrt(SumSquare(M)) - NormFrobenius(M);
+      Real a = sqrt(SumSquare(SM));
+      test(2) = NormFrobenius(M * M.t()) - a;
+      test(3) = SM.NormFrobenius() - a;
+      test(4) = (M * M.t()).NormFrobenius() - a;
+      test(5) = NormFrobenius(SM) - a;
+      test(6) = Trace(SM) - M.SumSquare();
+      Clean(test, 0.00000001); Print(test);
+  }
+
+//   cout << "\nEnd of Sixteenth test\n";
+}
Index: orxonox/trunk/src/util/newmat/tmth.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmth.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmth.cpp	(revision 4565)
@@ -0,0 +1,784 @@
+
+//#define WANT_STREAM
+
+#include "include.h"
+
+#include "newmatap.h"
+//#include "newmatio.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+static int my_max(int p, int q) { return (p > q) ? p : q; }
+
+static int my_min(int p, int q) { return (p < q) ? p : q; }
+
+
+void BandFunctions(int l1, int u1, int l2, int u2)
+{
+   int i, j;
+   BandMatrix BM1(20, l1, u1); BM1 = 999999.0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= 20; ++j)
+      if (i - j <= l1 && i - j >= -u1) BM1(i, j) = 100 * i + j;
+
+   BandMatrix BM2 = BM1; Matrix M = BM2 - BM1; Print(M);
+
+   BM2.ReSize(20, l2, u2); BM2 = 777777.0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= 20; ++j)
+      if (i - j <= l2 && i - j >= -u2) BM2(i, j) = (100 * i + j) * 11;
+
+   BandMatrix BMA = BM1 + BM2, BMS = BM1 - BM2, BMSP = SP(BM1, BM2),
+      BMM = BM1 * BM2, BMN = -BM1;
+
+   Matrix M1(20,20); M1 = 0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= 20; ++j)
+      if (i - j <= l1 && i - j >= -u1) M1(i, j) = 100 * i + j;
+
+   Matrix M2(20,20); M2 = 0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= 20; ++j)
+      if (i - j <= l2 && i - j >= -u2) M2(i, j) = (100 * i + j) * 11;
+
+   Matrix MA = M1 + M2, MS = M1 - M2, MSP = SP(M1, M2), MM = M1 * M2, MN = -M1;
+   MA -= BMA; MS -= BMS; MSP -= BMSP; MM -= BMM; MN -= BMN;
+   Print(MA); Print(MS); Print(MSP); Print(MM); Print(MN);
+
+   Matrix Test(7, 2);
+   Test(1,1) = BM1.BandWidth().Lower() - l1;
+   Test(1,2) = BM1.BandWidth().Upper() - u1;
+   Test(2,1) = BM2.BandWidth().Lower() - l2;
+   Test(2,2) = BM2.BandWidth().Upper() - u2;
+   Test(3,1) = BMA.BandWidth().Lower() - my_max(l1,l2);
+   Test(3,2) = BMA.BandWidth().Upper() - my_max(u1,u2);
+   Test(4,1) = BMS.BandWidth().Lower() - my_max(l1,l2);
+   Test(4,2) = BMS.BandWidth().Upper() - my_max(u1,u2);
+   Test(5,1) = BMSP.BandWidth().Lower() - my_min(l1,l2);
+   Test(5,2) = BMSP.BandWidth().Upper() - my_min(u1,u2);
+   Test(6,1) = BMM.BandWidth().Lower() - (l1 + l2);
+   Test(6,2) = BMM.BandWidth().Upper() - (u1 + u2);
+   Test(7,1) = BMN.BandWidth().Lower() - l1;
+   Test(7,2) = BMN.BandWidth().Upper() - u1;
+   Print(Test);
+
+   // test element function
+   BandMatrix BM1E(20, l1, u1); BM1E = 999999.0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= 20; ++j)
+      if (i - j <= l1 && i - j >= -u1) BM1E.element(i-1, j-1) = 100 * i + j;
+   BandMatrix BM2E = BM1E; BM2E.ReSize(BM2); BM2E = 777777.0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= 20; ++j)
+      if (i - j <= l2 && i - j >= -u2)
+         BM2E.element(i-1, j-1) = (100 * i + j) * 11;
+   M1 = BM1E - BM1; Print(M1);
+   M2 = BM2E - BM2; Print(M2);
+
+   // test element function with constant
+   BM1E = 444444.04; BM2E = 555555.0;
+   const BandMatrix BM1C = BM1, BM2C = BM2;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= 20; ++j)
+      if (i - j <= l1 && i - j >= -u1)
+         BM1E.element(i-1, j-1) = BM1C.element(i-1, j-1);
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= 20; ++j)
+      if (i - j <= l2 && i - j >= -u2)
+         BM2E.element(i-1, j-1) = BM2C.element(i-1, j-1);
+   M1 = BM1E - BM1; Print(M1);
+   M2 = BM2E - BM2; Print(M2);
+
+   // test subscript function with constant
+   BM1E = 444444.04; BM2E = 555555.0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= 20; ++j)
+      if (i - j <= l1 && i - j >= -u1) BM1E(i, j) = BM1C(i, j);
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= 20; ++j)
+      if (i - j <= l2 && i - j >= -u2) BM2E(i, j) = BM2C(i, j);
+   M1 = BM1E - BM1; Print(M1);
+   M2 = BM2E - BM2; Print(M2);
+}
+
+void LowerBandFunctions(int l1, int l2)
+{
+   int i, j;
+   LowerBandMatrix BM1(20, l1); BM1 = 999999.0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l1) BM1(i, j) = 100 * i + j;
+
+   LowerBandMatrix BM2 = BM1; Matrix M = BM2 - BM1; Print(M);
+
+   BM2.ReSize(20, l2); BM2 = 777777.0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l2) BM2(i, j) = (100 * i + j) * 11;
+
+   LowerBandMatrix BMA = BM1 + BM2, BMS = BM1 - BM2, BMSP = SP(BM1, BM2),
+      BMM = BM1 * BM2, BMN = -BM1;
+
+   Matrix M1(20,20); M1 = 0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l1) M1(i, j) = 100 * i + j;
+
+   Matrix M2(20,20); M2 = 0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l2) M2(i, j) = (100 * i + j) * 11;
+
+   Matrix MA = M1 + M2, MS = M1 - M2, MSP = SP(M1, M2), MM = M1 * M2, MN = -M1;
+   MA -= BMA; MS -= BMS; MSP -= BMSP; MM -= BMM; MN -= BMN;
+   Print(MA); Print(MS); Print(MSP); Print(MM); Print(MN);
+
+   Matrix Test(7, 2);
+   Test(1,1) = BM1.BandWidth().Lower() - l1;
+   Test(1,2) = BM1.BandWidth().Upper();
+   Test(2,1) = BM2.BandWidth().Lower() - l2;
+   Test(2,2) = BM2.BandWidth().Upper();
+   Test(3,1) = BMA.BandWidth().Lower() - my_max(l1,l2);
+   Test(3,2) = BMA.BandWidth().Upper();
+   Test(4,1) = BMS.BandWidth().Lower() - my_max(l1,l2);
+   Test(4,2) = BMS.BandWidth().Upper();
+   Test(5,1) = BMSP.BandWidth().Lower() - my_min(l1,l2);
+   Test(5,2) = BMSP.BandWidth().Upper();
+   Test(6,1) = BMM.BandWidth().Lower() - (l1 + l2);
+   Test(6,2) = BMM.BandWidth().Upper();
+   Test(7,1) = BMN.BandWidth().Lower() - l1;
+   Test(7,2) = BMN.BandWidth().Upper();
+   Print(Test);
+
+   // test element function
+   LowerBandMatrix BM1E(20, l1); BM1E = 999999.0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l1) BM1E.element(i-1, j-1) = 100 * i + j;
+   LowerBandMatrix BM2E = BM1E; BM2E.ReSize(BM2); BM2E = 777777.0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l2) BM2E.element(i-1, j-1) = (100 * i + j) * 11;
+   M1 = BM1E - BM1; Print(M1);
+   M2 = BM2E - BM2; Print(M2);
+
+   // test element function with constant
+   BM1E = 444444.04; BM2E = 555555.0;
+   const LowerBandMatrix BM1C = BM1, BM2C = BM2;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l1) BM1E.element(i-1, j-1) = BM1C.element(i-1, j-1);
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l2) BM2E.element(i-1, j-1) = BM2C.element(i-1, j-1);
+   M1 = BM1E - BM1; Print(M1);
+   M2 = BM2E - BM2; Print(M2);
+
+   // test subscript function with constant
+   BM1E = 444444.04; BM2E = 555555.0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l1) BM1E(i, j) = BM1C(i, j);
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l2) BM2E(i, j) = BM2C(i, j);
+   M1 = BM1E - BM1; Print(M1);
+   M2 = BM2E - BM2; Print(M2);
+}
+
+void UpperBandFunctions(int u1, int u2)
+{
+   int i, j;
+   UpperBandMatrix BM1(20, u1); BM1 = 999999.0;
+   for (i = 1; i <= 20; ++i) for (j = i; j <= 20; ++j)
+      if (i - j >= -u1) BM1(i, j) = 100 * i + j;
+
+   UpperBandMatrix BM2 = BM1; Matrix M = BM2 - BM1; Print(M);
+
+   BM2.ReSize(20, u2); BM2 = 777777.0;
+   for (i = 1; i <= 20; ++i) for (j = i; j <= 20; ++j)
+      if (i - j >= -u2) BM2(i, j) = (100 * i + j) * 11;
+
+   UpperBandMatrix BMA = BM1 + BM2, BMS = BM1 - BM2, BMSP = SP(BM1, BM2),
+      BMM = BM1 * BM2, BMN = -BM1;
+
+   Matrix M1(20,20); M1 = 0;
+   for (i = 1; i <= 20; ++i) for (j = i; j <= 20; ++j)
+      if (i - j >= -u1) M1(i, j) = 100 * i + j;
+
+   Matrix M2(20,20); M2 = 0;
+   for (i = 1; i <= 20; ++i) for (j = i; j <= 20; ++j)
+      if (i - j >= -u2) M2(i, j) = (100 * i + j) * 11;
+
+   Matrix MA = M1 + M2, MS = M1 - M2, MSP = SP(M1, M2), MM = M1 * M2, MN = -M1;
+   MA -= BMA; MS -= BMS; MSP -= BMSP; MM -= BMM; MN -= BMN;
+   Print(MA); Print(MS); Print(MSP); Print(MM); Print(MN);
+
+   Matrix Test(7, 2);
+   Test(1,1) = BM1.BandWidth().Lower();
+   Test(1,2) = BM1.BandWidth().Upper() - u1;
+   Test(2,1) = BM2.BandWidth().Lower();
+   Test(2,2) = BM2.BandWidth().Upper() - u2;
+   Test(3,1) = BMA.BandWidth().Lower();
+   Test(3,2) = BMA.BandWidth().Upper() - my_max(u1,u2);
+   Test(4,1) = BMS.BandWidth().Lower();
+   Test(4,2) = BMS.BandWidth().Upper() - my_max(u1,u2);
+   Test(5,1) = BMSP.BandWidth().Lower();
+   Test(5,2) = BMSP.BandWidth().Upper() - my_min(u1,u2);
+   Test(6,1) = BMM.BandWidth().Lower();
+   Test(6,2) = BMM.BandWidth().Upper() - (u1 + u2);
+   Test(7,1) = BMN.BandWidth().Lower();
+   Test(7,2) = BMN.BandWidth().Upper() - u1;
+   Print(Test);
+
+   // test element function
+   UpperBandMatrix BM1E(20, u1); BM1E = 999999.0;
+   for (i = 1; i <= 20; ++i) for (j = i; j <= 20; ++j)
+      if (i - j >= -u1) BM1E.element(i-1, j-1) = 100 * i + j;
+   UpperBandMatrix BM2E = BM1E; BM2E.ReSize(BM2); BM2E = 777777.0;
+   for (i = 1; i <= 20; ++i) for (j = i; j <= 20; ++j)
+      if (i - j >= -u2) BM2E.element(i-1, j-1) = (100 * i + j) * 11;
+   M1 = BM1E - BM1; Print(M1);
+   M2 = BM2E - BM2; Print(M2);
+
+   // test element function with constant
+   BM1E = 444444.04; BM2E = 555555.0;
+   const UpperBandMatrix BM1C = BM1, BM2C = BM2;
+   for (i = 1; i <= 20; ++i) for (j = i; j <= 20; ++j)
+      if (i - j >= -u1) BM1E.element(i-1, j-1) = BM1C.element(i-1, j-1);
+   for (i = 1; i <= 20; ++i) for (j = i; j <= 20; ++j)
+      if (i - j >= -u2) BM2E.element(i-1, j-1) = BM2C.element(i-1, j-1);
+   M1 = BM1E - BM1; Print(M1);
+   M2 = BM2E - BM2; Print(M2);
+
+   // test subscript function with constant
+   BM1E = 444444.04; BM2E = 555555.0;
+   for (i = 1; i <= 20; ++i) for (j = i; j <= 20; ++j)
+      if (i - j >= -u1) BM1E(i, j) = BM1C(i, j);
+   for (i = 1; i <= 20; ++i) for (j = i; j <= 20; ++j)
+      if (i - j >= -u2) BM2E(i, j) = BM2C(i, j);
+   M1 = BM1E - BM1; Print(M1);
+   M2 = BM2E - BM2; Print(M2);
+}
+
+void SymmetricBandFunctions(int l1, int l2)
+{
+   int i, j;
+   SymmetricBandMatrix BM1(20, l1); BM1 = 999999.0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l1) BM1(i, j) = 100 * i + j;
+
+   SymmetricBandMatrix BM2 = BM1; Matrix M = BM2 - BM1; Print(M);
+
+   BM2.ReSize(20, l2); BM2 = 777777.0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l2) BM2(i, j) = (100 * i + j) * 11;
+
+   SymmetricBandMatrix BMA = BM1 + BM2, BMS = BM1 - BM2, BMSP = SP(BM1, BM2),
+      BMN = -BM1;
+   BandMatrix BMM = BM1 * BM2;
+
+   SymmetricMatrix M1(20); M1 = 0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l1) M1(i, j) = 100 * i + j;
+
+   SymmetricMatrix M2(20); M2 = 0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l2) M2(i, j) = (100 * i + j) * 11;
+
+   SymmetricMatrix MA = M1 + M2, MS = M1 - M2, MSP = SP(M1, M2), MN = -M1;
+   Matrix MM = M1 * M2;
+   MA -= BMA; MS -= BMS; MSP -= BMSP; MM -= BMM; MN -= BMN;
+   Print(MA); Print(MS); Print(MSP); Print(MM); Print(MN);
+
+   Matrix Test(7, 2);
+   Test(1,1) = BM1.BandWidth().Lower() - l1;
+   Test(1,2) = BM1.BandWidth().Upper() - l1;
+   Test(2,1) = BM2.BandWidth().Lower() - l2;
+   Test(2,2) = BM2.BandWidth().Upper() - l2;
+   Test(3,1) = BMA.BandWidth().Lower() - my_max(l1,l2);
+   Test(3,2) = BMA.BandWidth().Upper() - my_max(l1,l2);
+   Test(4,1) = BMS.BandWidth().Lower() - my_max(l1,l2);
+   Test(4,2) = BMS.BandWidth().Upper() - my_max(l1,l2);
+   Test(5,1) = BMSP.BandWidth().Lower() - my_min(l1,l2);
+   Test(5,2) = BMSP.BandWidth().Upper() - my_min(l1,l2);
+   Test(6,1) = BMM.BandWidth().Lower() - (l1 + l2);
+   Test(6,2) = BMM.BandWidth().Upper() - (l1 + l2);
+   Test(7,1) = BMN.BandWidth().Lower() - l1;
+   Test(7,2) = BMN.BandWidth().Upper() - l1;
+   Print(Test);
+
+   // test element function
+   SymmetricBandMatrix BM1E(20, l1); BM1E = 999999.0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l1) BM1E.element(i-1, j-1) = 100 * i + j;
+   SymmetricBandMatrix BM2E = BM1E; BM2E.ReSize(BM2); BM2E = 777777.0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l2) BM2E.element(i-1, j-1) = (100 * i + j) * 11;
+   M1 = BM1E - BM1; Print(M1);
+   M2 = BM2E - BM2; Print(M2);
+
+   // reverse subscripts
+   BM1E = 999999.0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l1) BM1E.element(j-1, i-1) = 100 * i + j;
+   BM2E = 777777.0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l2) BM2E.element(j-1, i-1) = (100 * i + j) * 11;
+   M1 = BM1E - BM1; Print(M1);
+   M2 = BM2E - BM2; Print(M2);
+
+   // test element function with constant
+   BM1E = 444444.04; BM2E = 555555.0;
+   const SymmetricBandMatrix BM1C = BM1, BM2C = BM2;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l1) BM1E.element(i-1, j-1) = BM1C.element(i-1, j-1);
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l2) BM2E.element(i-1, j-1) = BM2C.element(i-1, j-1);
+   M1 = BM1E - BM1; Print(M1);
+   M2 = BM2E - BM2; Print(M2);
+
+   // reverse subscripts
+   BM1E = 444444.0; BM2E = 555555.0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l1) BM1E.element(j-1, i-1) = BM1C.element(j-1, i-1);
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l2) BM2E.element(j-1, i-1) = BM2C.element(j-1, i-1);
+   M1 = BM1E - BM1; Print(M1);
+   M2 = BM2E - BM2; Print(M2);
+
+   // test subscript function with constant
+   BM1E = 444444.0; BM2E = 555555.0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l1) BM1E(i, j) = BM1C(i, j);
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l2) BM2E(i, j) = BM2C(i, j);
+   M1 = BM1E - BM1; Print(M1);
+   M2 = BM2E - BM2; Print(M2);
+
+   // reverse subscripts
+   BM1E = 444444.0; BM2E = 555555.0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l1) BM1E(j, i) = BM1C(j, i);
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l2) BM2E(j, i) = BM2C(j, i);
+   M1 = BM1E - BM1; Print(M1);
+   M2 = BM2E - BM2; Print(M2);
+
+   // partly reverse subscripts
+   BM1E = 444444.0; BM2E = 555555.0;
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l1) BM1E(j, i) = BM1C(i, j);
+   for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      if (i - j <= l2) BM2E(j, i) = BM2C(i, j);
+   M1 = BM1E - BM1; Print(M1);
+   M2 = BM2E - BM2; Print(M2);
+
+}
+
+
+
+void trymath()
+{
+//   cout << "\nSeventeenth test of Matrix package\n";
+//   cout << "\n";
+   Tracer et("Seventeenth test of Matrix package");
+   Tracer::PrintTrace();
+
+
+   {
+      Tracer et1("Stage 1");
+      int i, j;
+      BandMatrix B(8,3,1);
+      for (i=1; i<=8; i++) for (j=-3; j<=1; j++)
+         { int k = i+j; if (k>0 && k<=8) B(i,k) = i + k/64.0; }
+
+      IdentityMatrix I(8); BandMatrix B1; B1 = I;
+      UpperTriangularMatrix UT = I; Print(Matrix(B1-UT));
+      Print(Matrix(B * B - B * 2 + I - (B - I) * (B - I)));
+      Matrix A = B; BandMatrix C; C = B;
+      Print(Matrix(B * A - C * 2 + I - (A - I) * (B - I)));
+
+      C.ReSize(8,2,2); C = 0.0; C.Inject(B);
+      Matrix X = A.t();
+      B1.ReSize(8,2,2); B1.Inject(X); Print(Matrix(C.t()-B1));
+
+      Matrix BI = B.i(); A = A.i()-BI; Clean(A,0.000000001); Print(A);
+      BandLUMatrix BLU = B.t();
+      BI = BLU.i(); A = X.i()-BI; Clean(A,0.000000001); Print(A);
+      X.ReSize(1,1);
+      X(1,1) = BLU.LogDeterminant().Value()-B.LogDeterminant().Value();
+      Clean(X,0.000000001); Print(X);
+
+      UpperBandMatrix U; U << B; LowerBandMatrix L; L << B;
+      DiagonalMatrix D; D << B;
+      Print( Matrix(L + (U - D - B)) );
+
+
+
+      for (i=1; i<=8; i++)  A.Column(i) << B.Column(i);
+      Print(Matrix(A-B));
+   }
+   {
+      Tracer et1("Stage 2");
+      BandMatrix A(7,2,2);
+      int i,j;
+      for (i=1; i<=7; i++) for (j=1; j<=7; j++)
+      {
+         int k=i-j; if (k<0) k = -k;
+         if (k==0) A(i,j)=6;
+         else if (k==1) A(i,j) = -4;
+         else if (k==2) A(i,j) = 1;
+         A(1,1) = A(7,7) = 5;
+      }
+      DiagonalMatrix D(7); D = 0.0; A = A - D;
+      BandLUMatrix B(A); Matrix M = A;
+      ColumnVector V(6);
+      V(1) = LogDeterminant(B).Value();
+      V(2) = LogDeterminant(A).Value();
+      V(3) = LogDeterminant(M).Value();
+      V(4) = Determinant(B);
+      V(5) = Determinant(A);
+      V(6) = Determinant(M);
+      V = V / 64 - 1; Clean(V,0.000000001); Print(V);
+      ColumnVector X(7);
+
+#ifdef ATandT
+      Real a[7];
+      // the previous statement causes a core dump in tmti.cpp
+      // on the HP9000 - seems very strange. Possibly the exception
+      // mechanism is failing to track the stack correctly. If you get
+      // this problem replace by the following statement.
+//      Real* a = new Real [7]; if (!a) exit(1);
+      a[0]=1; a[1]=2; a[2]=3; a[3]=4; a[4]=5; a[5]=6; a[6]=7;
+#else
+      Real a[] = {1,2,3,4,5,6,7};
+#endif
+      X << a;
+// include these if you are using the previous dynamic definition of a
+//#ifdef ATandT
+//      delete [] a;
+//#endif
+      M = (M.i()*X).t() - (B.i()*X).t() * 2.0 + (A.i()*X).t();
+      Clean(M,0.000000001); Print(M);
+
+
+      BandMatrix P(80,2,5); ColumnVector CX(80);
+      for (i=1; i<=80; i++) for (j=1; j<=80; j++)
+      { int d = i-j; if (d<=2 && d>=-5) P(i,j) = i + j/100.0; }
+      for (i=1; i<=80; i++)  CX(i) = i*100.0;
+      Matrix MP = P;
+      ColumnVector V1 = P.i() * CX; ColumnVector V2 = MP.i() * CX;
+      V = V1 - V2; Clean(V,0.000000001); Print(V);
+
+      V1 = P * V1; V2 = MP * V2; V = V1 - V2; Clean(V,0.000000001); Print(V);
+      RowVector XX(1);
+      XX = LogDeterminant(P).Value() / LogDeterminant(MP).Value() - 1.0;
+      Clean(XX,0.000000001); Print(XX);
+
+      LowerBandMatrix LP(80,5);
+      for (i=1; i<=80; i++) for (j=1; j<=80; j++)
+      { int d = i-j; if (d<=5 && d>=0) LP(i,j) = i + j/100.0; }
+      MP = LP;
+      XX.ReSize(4);
+      XX(1) = LogDeterminant(LP).Value();
+      XX(2) = LogDeterminant(MP).Value();
+      V1 = LP.i() * CX; V2 = MP.i() * CX;
+      V = V1 - V2; Clean(V,0.000000001); Print(V);
+
+      UpperBandMatrix UP(80,4);
+      for (i=1; i<=80; i++) for (j=1; j<=80; j++)
+      { int d = i-j; if (d<=0 && d>=-4) UP(i,j) = i + j/100.0; }
+      MP = UP;
+      XX(3) = LogDeterminant(UP).Value();
+      XX(4) = LogDeterminant(MP).Value();
+      V1 = UP.i() * CX; V2 = MP.i() * CX;
+      V = V1 - V2; Clean(V,0.000000001); Print(V);
+      XX = XX / SumAbsoluteValue(XX) - .25; Clean(XX,0.000000001); Print(XX);
+   }
+
+   {
+      Tracer et1("Stage 3");
+      SymmetricBandMatrix SA(8,5);
+      int i,j;
+      for (i=1; i<=8; i++) for (j=1; j<=8; j++)
+         if (i-j<=5 && 0<=i-j) SA(i,j) =i + j/128.0;
+      DiagonalMatrix D(8); D = 10; SA = SA + D;
+
+      Matrix MA1(8,8); Matrix MA2(8,8);
+      for (i=1; i<=8; i++)
+         { MA1.Column(i) << SA.Column(i); MA2.Row(i) << SA.Row(i); }
+      Print(Matrix(MA1-MA2));
+
+      D = 10; SA = SA.t() + D; MA1 = MA1 + D;
+      Print(Matrix(MA1-SA));
+
+      UpperBandMatrix UB(8,3); LowerBandMatrix LB(8,4);
+      D << SA; UB << SA; LB << SA;
+      SA = SA * 5.0; D = D * 5.0; LB = LB * 5.0; UB = UB * 5.0;
+      BandMatrix B = LB - D + UB - SA; Print(Matrix(B));
+
+      SymmetricBandMatrix A(7,2); A = 100.0;
+      for (i=1; i<=7; i++) for (j=1; j<=7; j++)
+      {
+         int k=i-j;
+         if (k==0) A(i,j)=6;
+         else if (k==1) A(i,j) = -4;
+         else if (k==2) A(i,j) = 1;
+         A(1,1) = A(7,7) = 5;
+      }
+      BandLUMatrix C(A); Matrix M = A;
+      ColumnVector X(8);
+      X(1) = LogDeterminant(C).Value() - 64;
+      X(2) = LogDeterminant(A).Value() - 64;
+      X(3) = LogDeterminant(M).Value() - 64;
+      X(4) = SumSquare(M) - SumSquare(A);
+      X(5) = SumAbsoluteValue(M) - SumAbsoluteValue(A);
+      X(6) = MaximumAbsoluteValue(M) - MaximumAbsoluteValue(A);
+      X(7) = Trace(M) - Trace(A);
+      X(8) = Sum(M) - Sum(A);
+      Clean(X,0.000000001); Print(X);
+
+#ifdef ATandT
+      Real a[7]; a[0]=1; a[1]=2; a[2]=3; a[3]=4; a[4]=5; a[5]=6; a[6]=7;
+#else
+      Real a[] = {1,2,3,4,5,6,7};
+#endif
+      X.ReSize(7);
+      X << a;
+      X = M.i()*X - C.i()*X * 2 + A.i()*X;
+      Clean(X,0.000000001); Print(X);
+
+
+      LB << A; UB << A; D << A;
+      BandMatrix XA = LB + (UB - D);
+      Print(Matrix(XA - A));
+
+      for (i=1; i<=7; i++) for (j=1; j<=7; j++)
+      {
+         int k=i-j;
+         if (k==0) A(i,j)=6;
+         else if (k==1) A(i,j) = -4;
+         else if (k==2) A(i,j) = 1;
+         A(1,1) = A(7,7) = 5;
+      }
+      D = 1;
+
+      M = LB.i() * LB - D; Clean(M,0.000000001); Print(M);
+      M = UB.i() * UB - D; Clean(M,0.000000001); Print(M);
+      M = XA.i() * XA - D; Clean(M,0.000000001); Print(M);
+      Matrix MUB = UB; Matrix MLB = LB;
+      M = LB.i() * UB - LB.i() * MUB; Clean(M,0.000000001); Print(M);
+      M = UB.i() * LB - UB.i() * MLB; Clean(M,0.000000001); Print(M);
+      M = LB.i() * UB - LB.i() * Matrix(UB); Clean(M,0.000000001); Print(M);
+      M = UB.i() * LB - UB.i() * Matrix(LB); Clean(M,0.000000001); Print(M);
+   }
+
+   {
+      // some tests about adding and subtracting band matrices of different
+      // sizes - check bandwidth of results
+      Tracer et1("Stage 4");
+
+      BandFunctions(9, 3, 9, 3);   // equal
+      BandFunctions(4, 7, 4, 7);   // equal
+      BandFunctions(9, 3, 5, 8);   // neither < or >
+      BandFunctions(5, 8, 9, 3);   // neither < or >
+      BandFunctions(9, 8, 5, 3);   // >
+      BandFunctions(3, 5, 8, 9);   // <
+
+      LowerBandFunctions(9, 9);    // equal
+      LowerBandFunctions(4, 4);    // equal
+      LowerBandFunctions(9, 5);    // >
+      LowerBandFunctions(3, 8);    // <
+
+      UpperBandFunctions(3, 3);    // equal
+      UpperBandFunctions(7, 7);    // equal
+      UpperBandFunctions(8, 3);    // >
+      UpperBandFunctions(5, 9);    // <
+
+      SymmetricBandFunctions(9, 9);   // equal
+      SymmetricBandFunctions(4, 4);   // equal
+      SymmetricBandFunctions(9, 5);   // >
+      SymmetricBandFunctions(3, 8);   // <
+
+      DiagonalMatrix D(6); D << 2 << 3 << 4.5 << 1.25 << 9.5 << -5;
+      BandMatrix BD = D;
+      UpperBandMatrix UBD; UBD = D;
+      LowerBandMatrix LBD; LBD = D;
+      SymmetricBandMatrix SBD = D;
+      Matrix X = BD - D; Print(X); X = UBD - D; Print(X);
+      X = LBD - D; Print(X); X = SBD - D; Print(X);
+      Matrix Test(9,2);
+      Test(1,1) =  BD.BandWidth().Lower(); Test(1,2) =  BD.BandWidth().Upper();
+      Test(2,1) = UBD.BandWidth().Lower(); Test(2,2) = UBD.BandWidth().Upper();
+      Test(3,1) = LBD.BandWidth().Lower(); Test(3,2) = LBD.BandWidth().Upper();
+      Test(4,1) = SBD.BandWidth().Lower(); Test(4,2) = SBD.BandWidth().Upper();
+
+      IdentityMatrix I(10); I *= 5;
+      BD = I; UBD = I; LBD = I; SBD = I;
+      X = BD - I; Print(X); X = UBD - I; Print(X);
+      X = LBD - I; Print(X); X = SBD - I; Print(X);
+      Test(5,1) =  BD.BandWidth().Lower(); Test(5,2) =  BD.BandWidth().Upper();
+      Test(6,1) = UBD.BandWidth().Lower(); Test(6,2) = UBD.BandWidth().Upper();
+      Test(7,1) = LBD.BandWidth().Lower(); Test(7,2) = LBD.BandWidth().Upper();
+      Test(8,1) = SBD.BandWidth().Lower(); Test(8,2) = SBD.BandWidth().Upper();
+
+      RowVector RV = D.AsRow(); I.ReSize(6); BandMatrix BI = I; I = 1;
+      BD =  RV.AsDiagonal() +  BI; X =  BD - D - I; Print(X);
+      Test(9,1) =  BD.BandWidth().Lower(); Test(9,2) =  BD.BandWidth().Upper();
+
+      Print(Test);
+   }
+
+   {
+      // various element functions
+      Tracer et1("Stage 5");
+
+      int i, j;
+      Matrix Count(1, 1); Count = 0;  // for counting errors
+      Matrix M(20,30);
+      for (i = 1; i <= 20; ++i) for (j = 1; j <= 30; ++j)
+         M(i, j) = 100 * i + j;
+      const Matrix CM = M;
+      for (i = 1; i <= 20; ++i) for (j = 1; j <= 30; ++j)
+      {
+         if (M(i, j) != CM(i, j)) ++Count(1,1);
+         if (M(i, j) != CM.element(i-1, j-1)) ++Count(1,1);
+         if (M(i, j) != M.element(i-1, j-1)) ++Count(1,1);
+      }
+
+      UpperTriangularMatrix U(20);
+      for (i = 1; i <= 20; ++i) for (j = i; j <= 20; ++j)
+         U(i, j) = 100 * i + j;
+      const UpperTriangularMatrix CU = U;
+      for (i = 1; i <= 20; ++i) for (j = i; j <= 20; ++j)
+      {
+         if (U(i, j) != CU(i, j)) ++Count(1,1);
+         if (U(i, j) != CU.element(i-1, j-1)) ++Count(1,1);
+         if (U(i, j) != U.element(i-1, j-1)) ++Count(1,1);
+      }
+
+      LowerTriangularMatrix L(20);
+      for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+         L(i, j) = 100 * i + j;
+      const LowerTriangularMatrix CL = L;
+      for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+      {
+         if (L(i, j) != CL(i, j)) ++Count(1,1);
+         if (L(i, j) != CL.element(i-1, j-1)) ++Count(1,1);
+         if (L(i, j) != L.element(i-1, j-1)) ++Count(1,1);
+      }
+
+      SymmetricMatrix S(20);
+      for (i = 1; i <= 20; ++i) for (j = 1; j <= i; ++j)
+         S(i, j) = 100 * i + j;
+      const SymmetricMatrix CS = S;
+      for (i = 1; i <= 20; ++i) for (j = 1; j <= 20; ++j)
+      {
+         if (S(i, j) != CS(i, j)) ++Count(1,1);
+         if (S(i, j) != CS.element(i-1, j-1)) ++Count(1,1);
+         if (S(i, j) != S.element(i-1, j-1)) ++Count(1,1);
+         if (S(i, j) != S(j, i)) ++Count(1,1);
+         if (S(i, j) != CS(i, j)) ++Count(1,1);
+         if (S(i, j) != CS.element(i-1, j-1)) ++Count(1,1);
+         if (S(i, j) != S.element(i-1, j-1)) ++Count(1,1);
+      }
+
+      DiagonalMatrix D(20);
+      for (i = 1; i <= 20; ++i) D(i) = 100 * i + i * i;
+      const DiagonalMatrix CD = D;
+      for (i = 1; i <= 20; ++i)
+      {
+         if (D(i, i) != CD(i, i)) ++Count(1,1);
+         if (D(i, i) != CD.element(i-1, i-1)) ++Count(1,1);
+         if (D(i, i) != D.element(i-1, i-1)) ++Count(1,1);
+         if (D(i, i) != D(i)) ++Count(1,1);
+         if (D(i) != CD(i)) ++Count(1,1);
+         if (D(i) != CD.element(i-1)) ++Count(1,1);
+         if (D(i) != D.element(i-1)) ++Count(1,1);
+      }
+
+      RowVector R(20);
+      for (i = 1; i <= 20; ++i) R(i) = 100 * i + i * i;
+      const RowVector CR = R;
+      for (i = 1; i <= 20; ++i)
+      {
+         if (R(i) != CR(i)) ++Count(1,1);
+         if (R(i) != CR.element(i-1)) ++Count(1,1);
+         if (R(i) != R.element(i-1)) ++Count(1,1);
+      }
+
+      ColumnVector C(20);
+      for (i = 1; i <= 20; ++i) C(i) = 100 * i + i * i;
+      const ColumnVector CC = C;
+      for (i = 1; i <= 20; ++i)
+      {
+         if (C(i) != CC(i)) ++Count(1,1);
+         if (C(i) != CC.element(i-1)) ++Count(1,1);
+         if (C(i) != C.element(i-1)) ++Count(1,1);
+      }
+
+      Print(Count);
+
+   }
+
+   {
+      // resize to another matrix size
+      Tracer et1("Stage 6");
+
+      Matrix A(20, 30); A = 3;
+      Matrix B(3, 4);
+      B.ReSize(A); B = 6; B -= 2 * A; Print(B);
+
+      A.ReSize(25,25); A = 12;
+
+      UpperTriangularMatrix U(5);
+      U.ReSize(A); U = 12; U << (U - A); Print(U);
+
+      LowerTriangularMatrix L(5);
+      L.ReSize(U); L = 12; L << (L - A); Print(L);
+
+      DiagonalMatrix D(5);
+      D.ReSize(U); D = 12; D << (D - A); Print(D);
+
+      SymmetricMatrix S(5);
+      S.ReSize(U); S = 12; S << (S - A); Print(S);
+
+      IdentityMatrix I(5);
+      I.ReSize(U); I = 12; D << (I - A); Print(D);
+
+      A.ReSize(10, 1); A = 17;
+      ColumnVector C(5); C.ReSize(A); C = 17; C -= A; Print(C);
+
+      A.ReSize(1, 10); A = 15;
+      RowVector R(5); R.ReSize(A); R = 15; R -= A; Print(R);
+
+   }
+
+   {
+      // generic matrix and identity matrix
+      Tracer et1("Stage 7");
+      IdentityMatrix I(5);
+      I *= 4;
+      GenericMatrix GM = I;
+      GM /= 2;
+      DiagonalMatrix D = GM;
+      Matrix A = GM + 10;
+      A -= 10;
+      A -= D;
+      Print(A);
+   }
+
+   {
+      // SP and upper and lower triangular matrices
+      Tracer et1("Stage 8");
+      UpperTriangularMatrix UT(4);
+      UT << 3 << 7 << 3 << 9
+              << 5 << 2 << 6
+                   << 8 << 0
+                        << 4;
+      LowerTriangularMatrix LT; LT.ReSize(UT);
+      LT << 2
+         << 7 << 9
+         << 2 << 8 << 6
+         << 1 << 0 << 3 << 5;
+
+      DiagonalMatrix D = SP(UT, LT);
+      DiagonalMatrix D1(4);
+      D1 << 6 << 45 << 48 << 20;
+      D -= D1; Print(D);
+      BandMatrix BM = SP(UT, LT);
+      Matrix X = BM - D1; Print(X);
+      RowVector RV(2);
+      RV(1) = BM.BandWidth().Lower();
+      RV(2) = BM.BandWidth().Upper();
+      Print(RV);
+   }
+
+//   cout << "\nEnd of Seventeenth test\n";
+}
+
Index: orxonox/trunk/src/util/newmat/tmti.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmti.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmti.cpp	(revision 4565)
@@ -0,0 +1,209 @@
+
+//#define WANT_STREAM
+
+#include "include.h"
+
+#include "newmatap.h"
+//#include "newmatio.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+
+void WillNotConverge()
+{
+   Matrix A(10,10);
+   Throw(ConvergenceException(A));
+}
+
+void ReSizeMatrix(Matrix& A)
+// for seeing if we can redimension a vector as a matrix
+{ A.ReSize(4,5); }
+
+void trymati()
+{
+#ifndef DisableExceptions
+   Tracer et("Eighteenth test of Matrix package");
+   Matrix RUStillThere(10,20); RUStillThere = 1553;
+   Tracer::PrintTrace();
+
+   ColumnVector checks(23); checks = 1.0; checks(1) = 0.0;
+
+   Try { WillNotConverge(); }
+   Catch(ConvergenceException) { checks(2) = 0; }
+   CatchAll { checks(1) = 1; }
+
+
+   Try { Matrix M(10,10); SymmetricMatrix S = M; }
+   Catch(ConvergenceException) { checks(1) = 1; }
+   Catch(InternalException) { checks(1) = 1; }
+   Catch(ProgramException) { checks(3) = 0; }
+   CatchAll { checks(1) = 1; }
+
+
+   Try { Matrix M(10,10); M(10,11) = 2.0; }
+   Catch(ConvergenceException) { checks(1) = 1; }
+   Catch(InternalException) { checks(1) = 1; }
+   Catch(IndexException) { checks(4) = 0; }
+   CatchAll { checks(1) = 1; }
+
+   Try { Matrix M(10,10); M = 0.0; M = M.i(); }
+   Catch(ConvergenceException) { checks(1) = 1; }
+   Catch(InternalException) { checks(1) = 1; }
+   Catch(ProgramException) { checks(1) = 1; }
+   Catch(SingularException) { checks(5) = 0; }
+   Catch(Bad_alloc) { checks(1) = 1; }
+   CatchAndThrow;
+
+   Try { ColumnVector A(30), B(50);  A = 5; B = 3; FFT(A,B,A,B); }
+   Catch(ConvergenceException) { checks(1) = 1; }
+   Catch(InternalException) { checks(1) = 1; }
+   Catch(ProgramException) { checks(6) = 0; }
+   CatchAll { checks(1) = 1; }
+
+   Try
+   {
+      ColumnVector A(30); A = 5; Matrix At = A.t();
+      DiagonalMatrix D;
+      SVD(At,D);
+   }
+   Catch(ConvergenceException) { checks(1) = 1; }
+   Catch(InternalException) { checks(1) = 1; }
+   Catch(Logic_error) { checks(6) = 0; }
+   Catch(Bad_alloc) { checks(1) = 1; }
+   CatchAndThrow;
+
+   Try { BandMatrix X(10,3,4); X(1,10) = 4.0; }
+   Catch(ConvergenceException) { checks(1) = 1; }
+   Catch(InternalException) { checks(1) = 1; }
+   Catch(IndexException) { checks(7) = 0; }
+   CatchAll { checks(1) = 1; }
+
+   Try
+   {
+      SymmetricMatrix S(10); S = 5;
+      LowerTriangularMatrix L = Cholesky(S);
+   }
+   Catch(ConvergenceException) { checks(1) = 1; }
+   Catch(InternalException) { checks(1) = 1; }
+   Catch(ProgramException) { checks(1) = 1; }
+   Catch(NPDException) { checks(8) = 0; }
+   Catch(Bad_alloc) { checks(1) = 1; }
+   CatchAndThrow;
+
+   Try { BandMatrix M(10,3,5); M = 0.0; Matrix XM = M.i(); }
+   Catch(ConvergenceException) { checks(1) = 1; }
+   Catch(InternalException) { checks(1) = 1; }
+   Catch(ProgramException) { checks(1) = 1; }
+   Catch(SingularException) { checks(9) = 0; }
+   Catch(Bad_alloc) { checks(1) = 1; }
+   CatchAndThrow;
+
+   Try { ColumnVector X(10); ColumnVector Y; X = 5; X = X - Y; }
+   Catch(ConvergenceException) { checks(1) = 1; }
+   Catch(InternalException) { checks(1) = 1; }
+   Catch(IncompatibleDimensionsException) { checks(10) = 0; }
+   Catch(Bad_alloc) { checks(1) = 1; }
+   CatchAndThrow;
+
+   Try
+   {
+      UpperTriangularMatrix U(3); RowVector RV(3); RV = 10;
+      U.Row(2) = RV;
+   }
+   Catch(ConvergenceException) { checks(1) = 1; }
+   Catch(InternalException) { checks(1) = 1; }
+   Catch(ProgramException) { checks(11) = 0; }
+   Catch(Bad_alloc) { checks(1) = 1; }
+   CatchAndThrow;
+
+   Try { DiagonalMatrix D(3); D << 12 << 13 << 14 << 15; }
+   Catch(ConvergenceException) { checks(1) = 1; }
+   Catch(InternalException) { checks(1) = 1; }
+   Catch(ProgramException) { checks(12) = 0; }
+   CatchAndThrow;
+
+   Try { ColumnVector D(3); D << 12 << 13; D << 1 << 2 << 3; }
+   Catch(ConvergenceException) { checks(1) = 1; }
+   Catch(InternalException) { checks(1) = 1; }
+   Catch(ProgramException) { checks(13) = 0; }
+   CatchAndThrow;
+
+
+   Try {  { ColumnVector D(3); D << 12 << 13; }  }
+   Catch(ConvergenceException) { checks(1) = 1; }
+   Catch(InternalException) { checks(1) = 1; }
+   Catch(ProgramException) { checks(14) = 0; }
+   CatchAndThrow;
+
+   Try { ColumnVector CV; ReSizeMatrix(CV); }
+   Catch(ConvergenceException) { checks(1) = 1; }
+   Catch(InternalException) { checks(1) = 1; }
+   Catch(VectorException) { checks(15) = 0; }
+   CatchAndThrow;
+
+   Try { RowVector RV(20); ReSizeMatrix(RV); }
+   Catch(ConvergenceException) { checks(1) = 1; }
+   Catch(InternalException) { checks(1) = 1; }
+   Catch(VectorException) { checks(16) = 0; }
+   CatchAndThrow;
+
+   Try
+   {
+      UpperTriangularMatrix U(10); U = 5;
+      DiagonalMatrix D(10); D = 2;
+      D += U;                 // illegal conversion
+   }
+   Catch(ConvergenceException) { checks(1) = 1; }
+   Catch(InternalException) { checks(1) = 1; }
+   Catch(ProgramException) { checks(17) = 0; }
+   CatchAndThrow;
+
+   Try { Matrix A(2,3), B(2,3); if (A < B) A = B; }
+   Catch(ConvergenceException) { checks(1) = 1; }
+   Catch(InternalException) { checks(1) = 1; }
+   Catch(NotDefinedException) { checks(18) = 0; }
+   CatchAndThrow;
+
+   Try { SymmetricBandMatrix A(3,1); A = 1; A = A.Reverse(); }
+   Catch(ConvergenceException) { checks(1) = 1; }
+   Catch(InternalException) { checks(1) = 1; }
+   Catch(NotDefinedException) { checks(19) = 0; }
+   CatchAndThrow;
+
+   Try
+   {
+      Matrix A(5,5); A = 1.0;
+      UpperTriangularMatrix B(10);
+      B.SubMatrix(3,7,3,7) = A;
+   }
+   Catch(ProgramException) { checks(20) = 0; }
+   CatchAndThrow;
+
+   Try {  { RowVector D(1); D << 12 << 13; }  }
+   Catch(InternalException) { checks(1) = 1; }
+   Catch(ProgramException) { checks(21) = 0; }
+   CatchAndThrow;
+
+   Try {  { RowVector D(0); D << 12; }  }
+   Catch(InternalException) { checks(1) = 1; }
+   Catch(ProgramException) { checks(22) = 0; }
+   CatchAndThrow;
+
+   Try { Matrix M(10,10); Matrix XM(3,3); M = 0.0; XM = M.i(); }
+   Catch(SingularException) { checks(23) = 0; }
+   CatchAll { checks(1) = 1; }
+
+   Print(checks);
+   Matrix RUStillThere1(10,20); RUStillThere1 = 1553;
+   RUStillThere = RUStillThere - RUStillThere1;
+   Print(RUStillThere);
+#endif
+
+}
+
+
+
Index: orxonox/trunk/src/util/newmat/tmtj.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmtj.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmtj.cpp	(revision 4565)
@@ -0,0 +1,152 @@
+
+//#define WANT_STREAM
+
+#include "include.h"
+
+#include "newmatap.h"
+//#include "newmatio.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+
+void trymatj()
+{
+   Tracer et("Nineteenth test of Matrix package");
+   Tracer::PrintTrace();
+   // testing elementwise (SP) products
+
+   {
+      Tracer et1("Stage 1");
+      Matrix A(13,7), B(13,7), C(13,7);
+      int i,j;
+      for (i=1;i<=13;i++) for (j=1; j<=7; j++)
+      {
+          Real a = (i+j*j)/2, b = (i*j-i/4);
+          A(i,j)=a; B(i,j)=b; C(i,j)=a*b;
+      }
+      // Where complete matrix routine can be used
+      Matrix X = SP(A,B)-C; Print(X);
+      X = SP(A,B+1.0)-A-C; Print(X);
+      X = SP(A-1,B)+B-C; Print(X);
+      X = SP(A-1,B+1)+B-A-C+1; Print(X);
+      // Where row-wise routine will be used
+      A = A.Rows(7,13); B = B.Rows(7,13); C = C.Rows(7,13);
+      LowerTriangularMatrix LTA; LTA << A;
+      UpperTriangularMatrix UTB; UTB << B;
+      DiagonalMatrix DC; DC << C;
+      X = SP(LTA,UTB) - DC; Print(X);
+      X = SP(LTA*2,UTB) - DC*2; Print(X);
+      X = SP(LTA, UTB /2) - DC/2; Print(X);
+      X = SP(LTA/2, UTB*2) - DC; Print(X);
+      DiagonalMatrix DX;
+      DX << SP(A,B); DX << (DX-C); Print(DX);
+      DX << SP(A*4,B); DX << (DX-C*4); Print(DX);
+      DX << SP(A,B*2); DX << (DX-C*2); Print(DX);
+      DX << SP(A/4,B/4); DX << (DX-C/16); Print(DX);
+      LowerTriangularMatrix LX;
+      LX = SP(LTA,B); LX << (LX-C); Print(LX);
+      LX = SP(LTA*3,B); LX << (LX-C*3); Print(LX);
+      LX = SP(LTA,B*5); LX << (LX-C*5); Print(LX);
+      LX = SP(-LTA,-B); LX << (LX-C); Print(LX);
+   }
+   {
+      // Symmetric Matrices
+      Tracer et1("Stage 2");
+      SymmetricMatrix A(25), B(25), C(25);
+      int i,j;
+      for (i=1;i<=25;i++) for (j=i;j<=25;j++)
+      {
+         Real a = i*j +i - j + 3;
+         Real b = i * i + j;
+         A(i,j)=a; B(i,j)=b; C(i,j)=a*b;
+      }
+      UpperTriangularMatrix UT;
+      UT << SP(A,B); UT << (UT - C); Print(UT);
+      Matrix MA = A, X;
+      X = SP(MA,B)-C; Print(X);
+      X = SP(A,B)-C; Print(X);
+      SymmetricBandMatrix BA(25,5), BB(25,5), BC(25,5);
+      BA.Inject(A); BB.Inject(B); BC.Inject(C);
+      X = SP(BA,BB)-BC; Print(X);
+      X = SP(BA*7,BB)-BC*7; Print(X);
+      X = SP(BA,BB/8)-BC/8; Print(X);
+      X = SP(BA*16,BB/16)-BC; Print(X);
+      X = SP(BA,BB); X=X-BC; Print(X);
+      X = SP(BA*2, BB/2)-BC; Print(X);
+      X = SP(BA, BB/2)-BC/2; Print(X);
+      X = SP(BA*2, BB)-BC*2; Print(X);
+   }
+   {
+      // Band matrices
+      Tracer et1("Stage 3");
+      Matrix A(19,19), B(19,19), C(19,19);
+      int i,j;
+      for (i=1;i<=19;i++) for (j=1;j<=19;j++)
+      {
+         Real a = i*j +i - 1.5*j + 3;
+         Real b = i * i + j;
+         A(i,j)=a; B(i,j)=b; C(i,j)=a*b;
+      }
+      BandMatrix BA(19,10,7), BB(19,8,15), BC(19,8,7);
+      BA.Inject(A); BB.Inject(B); BC.Inject(C);
+      Matrix X; BandMatrix BX; ColumnVector BW(2);
+      X = SP(BA,BB); X=X-BC; Print(X);
+      X = SP(BA/8,BB); X=X-BC/8; Print(X);
+      X = SP(BA,BB*17); X=X-BC*17; Print(X);
+      X = SP(BA/4,BB*7); X=X-BC*7/4; Print(X);
+      X = SP(BA,BB)-BC; Print(X);
+      X = SP(BA/8,BB)-BC/8; Print(X);
+      X = SP(BA,BB*17)-BC*17; Print(X);
+      X = SP(BA/4,BB*7)-BC*7/4; Print(X);
+      BX = SP(BA,BB);
+      BW(1)=BX.upper-7; BW(2)=BX.lower-8; Print(BW);
+
+      BA.ReSize(19,7,10); BB.ReSize(19,15,8);
+      BC.ReSize(19,7,8);
+      BA.Inject(A); BB.Inject(B); BC.Inject(C);
+
+      X = SP(BA,BB); X=X-BC; Print(X);
+      X = SP(BA/8,BB); X=X-BC/8; Print(X);
+      X = SP(BA,BB*17); X=X-BC*17; Print(X);
+      X = SP(BA/4,BB*7); X=X-BC*7/4; Print(X);
+      X = SP(BA,BB)-BC; Print(X);
+      X = SP(BA/8,BB)-BC/8; Print(X);
+      X = SP(BA,BB*17)-BC*17; Print(X);
+      X = SP(BA/4,BB*7)-BC*7/4; Print(X);
+      BX = SP(BA,BB);
+      BW(1)=BX.upper-8; BW(2)=BX.lower-7; Print(BW);
+   }
+   {
+      // SymmetricBandMatrices
+      Tracer et1("Stage 4");
+      Matrix A(7,7), B(7,7);
+      int i,j;
+      for (i=1;i<=7;i++) for (j=1;j<=7;j++)
+      {
+         Real a = i*j +i - 1.5*j + 3;
+         Real b = i * i + j;
+         A(i,j)=a; B(i,j)=b;
+      }
+      BandMatrix BA(7,2,4), BB(7,3,1), BC(7,2,1);
+      BA.Inject(A);
+      SymmetricBandMatrix SB(7,3);
+      SymmetricMatrix S; S << (B+B.t());
+      SB.Inject(S); A = BA; S = SB;
+      Matrix X;  
+      X = SP(BA,SB); X=X-SP(A,S); Print(X);
+      X = SP(BA*2,SB); X=X-SP(A,S*2); Print(X);
+      X = SP(BA,SB/4); X=X-SP(A/4,S); Print(X);
+      X = SP(BA*4,SB/4); X=X-SP(A,S); Print(X);
+      X = SP(BA,SB)-SP(A,S); Print(X);
+      X = SP(BA*2,SB)-SP(A,S*2); Print(X);
+      X = SP(BA,SB/4)-SP(A/4,S); Print(X);
+      X = SP(BA*4,SB/4)-SP(A,S); Print(X);
+   }
+
+}
+
+
Index: orxonox/trunk/src/util/newmat/tmtk.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmtk.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmtk.cpp	(revision 4565)
@@ -0,0 +1,188 @@
+
+#define WANT_STREAM
+
+#include "include.h"
+
+#include "newmatap.h"
+#include "newmatio.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+
+static inline int my_min(int x, int y) { return x < y ? x : y; }
+static inline int my_max(int x, int y) { return x > y ? x : y; }
+
+
+#ifdef SETUP_C_SUBSCRIPTS
+
+void trymatk()
+{
+   Tracer et("Twentieth test of Matrix package");
+   Tracer::PrintTrace();
+   // test C subscript package
+
+
+   int i,j; Matrix X, Y;
+
+   cout << "Matrix\n";
+   Matrix A(15,35), B(15, 35);
+   for (i=0; i<15; i++) for (j=0; j<35; j++)
+      { A[i][j] = i+100*j; B(i+1,j+1) = i+100*j; }
+   X = A - B; Print(X); Y = X;
+   for (i=0; i<15; i++) for (j=0; j<35; j++)
+   {
+      X.element(i,j) = A.element(i,j) - B[i][j];
+      Y.element(i,j) = ((const Matrix&)A)[i][j] - B[i][j];
+   }
+   Print(X); Print(Y);
+   A.CleanUp(); B.CleanUp();
+
+   cout << "UpperTriangularMatrix\n";
+   UpperTriangularMatrix A1(15), B1(15);
+   for (i=0; i<15; i++) for (j=i; j<15; j++)
+      { A1[i][j] = i+100*j; B1(i+1,j+1) = i+100*j; }
+   X = A1 - B1; Print(X); Y = X;
+   for (i=0; i<15; i++) for (j=i; j<15; j++)
+   {
+      X.element(i,j) = A1.element(i,j) - B1[i][j];
+      Y.element(i,j) = ((const UpperTriangularMatrix&)A1)[i][j] - B1[i][j];
+   }
+   Print(X); Print(Y);
+   A1.CleanUp(); B1.CleanUp();
+
+   cout << "LowerTriangularMatrix\n";
+   LowerTriangularMatrix A2(35), B2(35); 
+   for (i=0; i<35; i++) for (j=0; j<=i; j++)
+      { A2[i][j] = i+100*j; B2(i+1,j+1) = i+100*j; }
+   X = A2 - B2; Print(X); Y = X;
+   for (i=0; i<35; i++) for (j=0; j<=i; j++)
+   {
+      X.element(i,j) = A2.element(i,j) - B2[i][j];
+      Y.element(i,j) = ((const LowerTriangularMatrix&)A2)[i][j] - B2[i][j];
+   }
+   Print(X); Print(Y);
+   A2.CleanUp(); B2.CleanUp();
+
+   cout << "SymmetricMatrix\n";
+   SymmetricMatrix A3(10), B3(10);
+   for (i=0; i<10; i++) for (j=0; j<=i; j++)
+      { A3[i][j] = i+100*j; B3(i+1,j+1) = i+100*j; }
+   X = A3 - B3; Print(X); Y = X;
+   for (i=0; i<10; i++) for (j=0; j<=i; j++)
+   {
+      X.element(i,j) = A3.element(i,j) - B3[i][j];
+      Y.element(i,j) = ((const SymmetricMatrix&)A3)[i][j] - B3[i][j];
+   }
+   Print(X); Print(Y);
+   A3.CleanUp(); B3.CleanUp();
+
+   cout << "DiagonalMatrix\n";
+   DiagonalMatrix A4(10), B4(10);
+   for (i=0; i<10; i++)
+      { A4[i] = i+100; B4(i+1) = i+100; }
+   X = A4 - B4; Print(X); Y = X;
+   for (i=0; i<10; i++)
+   {
+      X.element(i,i) = A4.element(i) - B4[i];
+      Y.element(i,i) = ((const DiagonalMatrix&)A4)[i] - B4[i];
+   }
+   Print(X); Print(Y);
+   A4.CleanUp(); B4.CleanUp();
+
+   cout << "RowVector\n";
+   RowVector A5(10), B5(10);
+   for (i=0; i<10; i++)
+      { A5[i] = i+100; B5(i+1) = i+100; }
+   X = A5 - B5; Print(X); Y = X;
+   for (i=0; i<10; i++)
+   {
+      X.element(0,i) = A5.element(i) - B5[i];
+      Y.element(0,i) = ((const RowVector&)A5)[i] - B5[i];
+   }
+   Print(X); Print(Y);
+   A5.CleanUp(); B5.CleanUp();
+
+   cout << "ColumnVector\n";
+   ColumnVector A6(10), B6(10);
+   for (i=0; i<10; i++)
+      { A6[i] = i+100; B6(i+1) = i+100; }
+   X = A6 - B6; Print(X); Y = X;
+   for (i=0; i<10; i++)
+   {
+      X.element(i,0) = A6.element(i) - B6[i];
+      Y.element(i,0) = ((const ColumnVector&)A6)[i] - B6[i];
+   }
+   Print(X); Print(Y);
+   A6.CleanUp(); B6.CleanUp();
+
+   cout << "BandMatrix\n";
+   BandMatrix A7(55,10, 5), B7(55, 10, 5);
+   for (i=0; i<55; i++) for (j=my_max(0,i-10); j<=my_min(54,i+5); j++)
+      { A7[i][j] = i+100*j; B7(i+1,j+1) = i+100*j; }
+   X = A7 - B7; Print(X); Y = X;
+   for (i=0; i<55; i++) for (j=my_max(0,i-10); j<=my_min(54,i+5); j++)
+   {
+      X.element(i,j) = A7.element(i,j) - B7[i][j];
+      Y.element(i,j) = ((const BandMatrix&)A7)[i][j] - B7[i][j];
+   }
+   Print(X); Print(Y);
+   A7.CleanUp(); B7.CleanUp();
+
+   cout << "UpperBandMatrix\n";
+   UpperBandMatrix A8(80,15), B8(80,15);
+   for (i=0; i<80; i++) for (j=i; j<=my_min(79,i+15); j++)
+      { A8[i][j] = i+100*j; B8(i+1,j+1) = i+100*j; }
+   X = A8 - B8; Print(X); Y = X;
+   for (i=0; i<80; i++) for (j=i; j<=my_min(79,i+15); j++)
+   {
+      X.element(i,j) = A8.element(i,j) - B8[i][j];
+      Y.element(i,j) = ((const UpperBandMatrix&)A8)[i][j] - B8[i][j];
+   }
+   Print(X); Print(Y);
+   A8.CleanUp(); B8.CleanUp();
+
+   cout << "LowerBandMatrix\n";
+   LowerBandMatrix A9(75,27), B9(75,27);
+   for (i=0; i<75; i++) for (j=my_max(0,i-27); j<=i; j++)
+      { A9[i][j] = i+100*j; B9(i+1,j+1) = i+100*j; }
+   X = A9 - B9; Print(X); Y = X;
+   for (i=0; i<75; i++) for (j=my_max(0,i-27); j<=i; j++)
+   {
+      X.element(i,j) = A9.element(i,j) - B9[i][j];
+      Y.element(i,j) = ((const LowerBandMatrix&)A9)[i][j] - B9[i][j];
+   }
+   Print(X); Print(Y);
+   A9.CleanUp(); B9.CleanUp();
+
+   cout << "SymmetricBandMatrix\n";
+   SymmetricBandMatrix Aa(69,15), Ba(69,15);
+   for (i=0; i<69; i++) for (j=my_max(0,i-15); j<=i; j++)
+      { Aa[i][j] = i+100*j; Ba(i+1,j+1) = i+100*j; }
+   X = Aa - Ba; Print(X); Y = X;
+   for (i=0; i<69; i++) for (j=my_max(0,i-15); j<=i; j++)
+   {
+      X.element(i,j) = Aa.element(i,j) - Ba[i][j];
+      Y.element(i,j) = ((const SymmetricBandMatrix&)Aa)[i][j] - Ba[i][j];
+   }
+   Print(X); Print(Y);
+   Aa.CleanUp(); Ba.CleanUp();
+
+
+}
+
+
+#else
+
+void trymatk()
+{
+   Tracer et("Twentieth test of Matrix package");
+   Tracer::PrintTrace();
+   // test C subscript package
+   cout << "C subscripts not enabled, not tested\n\n";
+}
+
+#endif
Index: orxonox/trunk/src/util/newmat/tmtl.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmtl.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmtl.cpp	(revision 4565)
@@ -0,0 +1,198 @@
+
+#define WANT_STREAM
+
+#define WANT_MATH
+
+#include "newmat.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+
+
+// test maxima and minima
+
+Real TestMax(const GenericMatrix& GM)
+{
+   Matrix M = GM;
+   ColumnVector CV = GM.AsColumn();
+
+   int c, i, j, k; int n = CV.Nrows(), nr = M.Nrows(), nc = M.Ncols();
+   Real x1, x2, x3;
+
+   MatrixBandWidth mbw = GM.BandWidth();
+   int u = mbw.Upper(); int l = mbw.Lower();
+   bool IsBandMatrix = u > 0 && l > 0 && !(u == 0 && l == 0);
+
+   x1 = GM.MaximumAbsoluteValue();
+   x2 = GM.MaximumAbsoluteValue1(i);
+   if (fabs(CV(i)) != x2) return 1.1;
+   x3 = GM.MaximumAbsoluteValue2(i,j);
+   if (fabs(M(i,j)) != x3) return 1.2;
+   if (x3 != x1) return 1.3;
+   if (x2 != x1) return 1.4;
+   c = 0;
+   for (k = 1; k <= n; k++)
+   {
+      Real cvk = fabs(CV(k));
+      if (x1 <= cvk) { if (x1 < cvk) return 1.5; else c++; }
+   }
+   if (c == 0) return 1.6;
+
+
+   x1 = GM.MinimumAbsoluteValue();
+   x2 = GM.MinimumAbsoluteValue1(i);
+   if (fabs(CV(i)) != x2) return 2.1;
+   x3 = GM.MinimumAbsoluteValue2(i,j);
+   if (fabs(M(i,j)) != x3) return 2.2;
+   if (x3 != x1) return 2.3;
+   if (x2 != CV.MinimumAbsoluteValue()) return 2.4;
+   c = 0;
+   if (IsBandMatrix)
+   {
+      for (i = 1; i <= nr; i++) for (j = 1; j <= nc; j++)
+         if (i - j <= l && j - i <= u)
+      {
+         Real mij = fabs(M(i,j));
+         if (x1 >= mij) { if (x1 > mij) return 2.51; else c++; }
+      }
+   }
+   else
+   {
+      for (k = 1; k <= n; k++)
+      {
+         Real cvk = fabs(CV(k));
+         if (x1 >= cvk) { if (x1 > cvk) return 2.52; else c++; }
+      }
+   }
+   if (c == 0) return 2.6;
+
+   x1 = GM.Maximum();
+   x2 = GM.Maximum1(i);
+   if (CV(i) != x2) return 3.1;
+   x3 = GM.Maximum2(i,j);
+   if (M(i,j) != x3) return 3.2;
+   if (x3 != x1) return 3.3;
+   if (x2 != CV.Maximum()) return 3.4;
+   c = 0;
+   if (IsBandMatrix)
+   {
+      for (i = 1; i <= nr; i++) for (j = 1; j <= nc; j++)
+         if (i - j <= l && j - i <= u)
+      {
+         Real mij = M(i,j);
+         if (x1 <= mij) { if (x1 < mij) return 3.51; else c++; }
+      }
+   }
+   else
+   {
+      for (k = 1; k <= n; k++)
+      {
+         Real cvk = CV(k);
+         if (x1 <= cvk) { if (x1 < cvk) return 3.52; else c++; }
+      }
+   }
+   if (c == 0) return 3.6;
+
+   x1 = GM.Minimum();
+   x2 = GM.Minimum1(i);
+   if (CV(i) != x2) return 4.1;
+   x3 = GM.Minimum2(i,j);
+   if (M(i,j) != x3) return 4.2;
+   if (x3 != x1) return 4.3;
+   if (x2 != CV.Minimum()) return 4.4;
+   c = 0;
+   if (IsBandMatrix)
+   {
+      for (i = 1; i <= nr; i++) for (j = 1; j <= nc; j++)
+         if (i - j <= l && j - i <= u)
+      {
+         Real mij = M(i,j);
+         if (x1 >= mij) { if (x1 > mij) return 4.51; else c++; }
+      }
+   }
+   else
+   {
+      for (k = 1; k <= n; k++)
+      {
+         Real cvk = CV(k);
+         if (x1 >= cvk) { if (x1 > cvk) return 4.52; else c++; }
+      }
+   }
+   if (c == 0) return 4.6;
+
+   return 0;
+
+}
+
+
+void trymatl()
+{
+   Tracer et("Twenty first test of Matrix package");
+   Tracer::PrintTrace();
+
+   Matrix M(4, 4);
+   M <<  1.5 <<  1.0 << -4.0 <<  4.5
+     <<  3.5 <<  7.0 <<  2.0 << -1.0
+     << -1.5 <<  7.5 << -6.0 <<  5.0
+     <<  0.5 << -8.0 <<  5.5 <<  4.0;
+   UpperTriangularMatrix UM;  UM << M;
+   LowerTriangularMatrix LM;  LM << -M;
+   SymmetricMatrix SM; SM << (UM + UM.t());
+   BandMatrix BM(4, 1, 2);  BM.Inject(M);
+   DiagonalMatrix DM; DM << M;
+   SymmetricBandMatrix SBM(4,1); SBM.Inject(M);
+   RowVector Test(28); int k = 0;
+
+   // tests for different shapes
+   Test(++k) = TestMax(GenericMatrix(M));
+   Test(++k) = TestMax(GenericMatrix(UM));
+   Test(++k) = TestMax(GenericMatrix(LM));
+   Test(++k) = TestMax(GenericMatrix(SM));
+   Test(++k) = TestMax(GenericMatrix(DM));
+   Test(++k) = TestMax(GenericMatrix(BM));
+   Test(++k) = TestMax(GenericMatrix(SBM));
+
+   // tests for constant matrices - don't put non-zeros in corners of BM
+   Matrix MX(4,4);
+   MX = 1; Test(++k) = TestMax(GenericMatrix(MX));
+   BM.Inject(MX); Test(++k) = TestMax(GenericMatrix(BM));
+   MX = 0; Test(++k) = TestMax(GenericMatrix(MX));
+   BM.Inject(MX); Test(++k) = TestMax(GenericMatrix(BM));
+   MX = -1; Test(++k) = TestMax(GenericMatrix(MX));
+   BM.Inject(MX); Test(++k) = TestMax(GenericMatrix(BM));
+
+   // test for non-square
+   MX = M | (2 * M).t(); Test(++k) = TestMax(GenericMatrix(MX));
+
+   // test on row and column vector
+   RowVector RV(6);
+   RV << 1 << 3 << -5 << 2 << -4 << 3;
+   Test(++k) = TestMax(GenericMatrix(RV));
+   Test(++k) = TestMax(GenericMatrix(RV.t()));
+
+   // test for function form
+   Test(++k) = (MaximumAbsoluteValue(RV) - 5);
+   Test(++k) = (MinimumAbsoluteValue(RV) - 1);
+   Test(++k) = (Maximum(RV) - 3);
+   Test(++k) = (Minimum(RV) + 5);
+   Test(++k) = (MaximumAbsoluteValue(-RV) - 5);
+   Test(++k) = (MinimumAbsoluteValue(-RV) - 1);
+   Test(++k) = (Maximum(-RV) - 5);
+   Test(++k) = (Minimum(-RV) + 3);
+
+   // test formulae
+   RowVector RV2(6);
+   RV2 << 2 << 8 << 1 << 9 << 3 << -1;
+   Test(++k) = (MaximumAbsoluteValue(RV+RV2) - 11);
+   Test(++k) = (MinimumAbsoluteValue(RV+RV2) - 1);
+   Test(++k) = (Maximum(RV+RV2) - 11);
+   Test(++k) = (Minimum(RV+RV2) + 4);
+
+
+   Print(Test);
+}
+
Index: orxonox/trunk/src/util/newmat/tmtm.cpp
===================================================================
--- orxonox/trunk/src/util/newmat/tmtm.cpp	(revision 4565)
+++ orxonox/trunk/src/util/newmat/tmtm.cpp	(revision 4565)
@@ -0,0 +1,222 @@
+
+#define WANT_STREAM
+
+#define WANT_MATH
+
+#include "newmat.h"
+
+#include "tmt.h"
+
+#ifdef use_namespace
+using namespace NEWMAT;
+#endif
+
+
+
+// test Kronecker Product
+
+
+void trymatm()
+{
+   Tracer et("Twenty second test of Matrix package");
+   Tracer::PrintTrace();
+
+   {
+      Tracer et1("Stage 1");
+
+
+      Matrix A(2,3);
+      A << 3 << 5 << 2
+        << 4 << 1 << 6;
+
+      Matrix B(4,3);
+      B <<  7 <<  2 <<  9
+        <<  1 <<  3 <<  6
+        <<  4 << 10 <<  5
+        << 11 <<  8 << 12;
+
+      Matrix C(8, 9);
+
+      C.Row(1) << 21 <<  6 << 27  << 35 << 10 << 45  << 14 <<  4 << 18;
+      C.Row(2) <<  3 <<  9 << 18  <<  5 << 15 << 30  <<  2 <<  6 << 12;
+      C.Row(3) << 12 << 30 << 15  << 20 << 50 << 25  <<  8 << 20 << 10;
+      C.Row(4) << 33 << 24 << 36  << 55 << 40 << 60  << 22 << 16 << 24;
+
+      C.Row(5) << 28 <<  8 << 36  <<  7 <<  2 <<  9  << 42 << 12 << 54;
+      C.Row(6) <<  4 << 12 << 24  <<  1 <<  3 <<  6  <<  6 << 18 << 36;
+      C.Row(7) << 16 << 40 << 20  <<  4 << 10 <<  5  << 24 << 60 << 30;
+      C.Row(8) << 44 << 32 << 48  << 11 <<  8 << 12  << 66 << 48 << 72;
+
+      Matrix AB = KP(A,B) - C; Print(AB);
+
+      IdentityMatrix I1(10); IdentityMatrix I2(15); I2 *= 2;
+      DiagonalMatrix D = KP(I1, I2) - IdentityMatrix(150) * 2;
+      Print(D);
+   }
+
+   {
+      Tracer et1("Stage 2");
+
+      UpperTriangularMatrix A(3);
+      A << 3 << 8 << 5
+             << 7 << 2
+                  << 4;
+      UpperTriangularMatrix B(4);
+      B << 4 << 1 << 7 << 2
+             << 3 << 9 << 8
+                  << 1 << 5
+                       << 6;
+
+      UpperTriangularMatrix C(12);
+
+      C.Row(1) <<12<< 3<<21<< 6 <<32<< 8<<56<<16 <<20<< 5<<35<<10;
+      C.Row(2)     << 9<<27<<24 << 0<<24<<72<<64 << 0<<15<<45<<40;
+      C.Row(3)         << 3<<15 << 0<< 0<< 8<<40 << 0<< 0<< 5<<25;
+      C.Row(4)             <<18 << 0<< 0<< 0<<48 << 0<< 0<< 0<<30;
+
+      C.Row(5)                  <<28<< 7<<49<<14 << 8<< 2<<14<< 4;
+      C.Row(6)                      <<21<<63<<56 << 0<< 6<<18<<16;
+      C.Row(7)                          << 7<<35 << 0<< 0<< 2<<10;
+      C.Row(8)                              <<42 << 0<< 0<< 0<<12;
+
+      C.Row(9)                                   <<16<< 4<<28<< 8;
+      C.Row(10)                                      <<12<<36<<32;
+      C.Row(11)                                          << 4<<20;
+      C.Row(12)                                              <<24;
+
+
+      UpperTriangularMatrix AB = KP(A,B) - C; Print(AB);
+
+      LowerTriangularMatrix BT = B.t(); Matrix N(12,12);
+
+      N.Row(1) <<12 << 0<< 0<< 0 <<32<< 0<< 0<< 0 <<20<< 0<< 0<< 0;
+      N.Row(2) << 3 << 9<< 0<< 0 << 8<<24<< 0<< 0 << 5<<15<< 0<< 0;
+      N.Row(3) <<21 <<27<< 3<< 0 <<56<<72<< 8<< 0 <<35<<45<< 5<< 0;
+      N.Row(4) << 6 <<24<<15<<18 <<16<<64<<40<<48 <<10<<40<<25<<30;
+
+      N.Row(5) << 0 << 0<< 0<< 0 <<28<< 0<< 0<< 0 << 8<< 0<< 0<< 0;
+      N.Row(6) << 0 << 0<< 0<< 0 << 7<<21<< 0<< 0 << 2<< 6<< 0<< 0;
+      N.Row(7) << 0 << 0<< 0<< 0 <<49<<63<< 7<< 0 <<14<<18<< 2<< 0;
+      N.Row(8) << 0 << 0<< 0<< 0 <<14<<56<<35<<42 << 4<<16<<10<<12;
+
+      N.Row(9) << 0 << 0<< 0<< 0 << 0<< 0<< 0<< 0 <<16<< 0<< 0<< 0;
+      N.Row(10)<< 0 << 0<< 0<< 0 << 0<< 0<< 0<< 0 << 4<<12<< 0<< 0;
+      N.Row(11)<< 0 << 0<< 0<< 0 << 0<< 0<< 0<< 0 <<28<<36<< 4<< 0;
+      N.Row(12)<< 0 << 0<< 0<< 0 << 0<< 0<< 0<< 0 << 8<<32<<20<<24;
+
+      Matrix N1 = KP(A, BT); N1 -= N; Print(N1);
+      AB << KP(A, BT); AB << (AB - N); Print(AB);
+      BT << KP(A, BT); BT << (BT - N); Print(BT);
+
+      LowerTriangularMatrix AT = A.t();
+      N1 = KP(AT, B); N1 -= N.t(); Print(N1);
+      AB << KP(AT, B); AB << (AB - N.t()); Print(AB);
+      BT << KP(AT, B); BT << (BT - N.t()); Print(BT);
+   }
+
+   {
+      Tracer et1("Stage 3");
+
+      BandMatrix BMA(6,2,3);
+      BMA.Row(1) << 5.25 << 4.75 << 2.25 << 1.75;
+      BMA.Row(2) << 1.25 << 9.75 << 4.50 << 0.25 << 1.50;
+      BMA.Row(3) << 7.75 << 1.50 << 3.00 << 4.25 << 0.50 << 5.50;
+      BMA.Row(4) << 2.75 << 9.00 << 8.00 << 3.25 << 3.50;
+      BMA.Row(5) << 8.75 << 6.25 << 5.00 << 5.75;
+      BMA.Row(6) << 3.75 << 6.75 << 6.00;
+
+      Matrix A = BMA;
+
+      BandMatrix BMB(4,2,1);
+      BMB.Row(1) << 4.5 << 9.5;
+      BMB.Row(2) << 1.5 << 6.0 << 2.0;
+      BMB.Row(3) << 0.5 << 2.5 << 8.5 << 7.5;
+      BMB.Row(4) << 3.0 << 4.0 << 6.5;
+
+      Matrix B = BMB;
+
+      BandMatrix BMC = KP(BMA, BMB);
+      BandMatrix BMC1(24,11,15);
+      BMC1.Inject(Matrix(KP(BMA, B)));  // not directly Band Matrix
+      Matrix C2 = KP(A, BMB);
+      Matrix C = KP(A, B);
+
+      Matrix M = C - BMC; Print(M);
+      M = C - BMC1; Print(M);
+      M = C - C2; Print(M);
+
+      RowVector X(4);
+      X(1) = BMC.BandWidth().Lower() - 10;
+      X(2) = BMC.BandWidth().Upper() - 13;
+      X(3) = BMC1.BandWidth().Lower() - 11;
+      X(4) = BMC1.BandWidth().Upper() - 15;
+      Print(X);
+
+      UpperTriangularMatrix UT;  UT << KP(BMA, BMB);
+      UpperTriangularMatrix UT1; UT1 << (C - UT); Print(UT1);
+      LowerTriangularMatrix LT;  LT << KP(BMA, BMB);
+      LowerTriangularMatrix LT1; LT1 << (C - LT); Print(LT1);
+   }
+
+   {
+      Tracer et1("Stage 4");
+
+      SymmetricMatrix SM1(4);
+      SM1.Row(1) << 2;
+      SM1.Row(2) << 4 << 5;
+      SM1.Row(3) << 9 << 2 << 1;
+      SM1.Row(4) << 3 << 6 << 8 << 2;
+
+      SymmetricMatrix SM2(3);
+      SM2.Row(1) <<  3;
+      SM2.Row(2) << -7 << -6;
+      SM2.Row(3) <<  4 << -2 << -1;
+
+      SymmetricMatrix SM = KP(SM1, SM2);
+      Matrix M1 = SM1; Matrix M2 = SM2;
+      Matrix M = KP(SM1, SM2); M -= SM; Print(M);
+      M = KP(SM1, SM2) - SM; Print(M);
+      M = KP(M1, SM2) - SM; Print(M);
+      M = KP(SM1, M2) - SM; Print(M);
+      M = KP(M1, M2); M -= SM; Print(M);
+   }
+
+   {
+      Tracer et1("Stage 5");
+
+      Matrix A(2,3);
+      A << 3 << 5 << 2
+        << 4 << 1 << 6;
+
+      Matrix B(3,4);
+      B <<  7 <<  2 <<  9 << 11
+        <<  1 <<  3 <<  6 <<  8
+        <<  4 << 10 <<  5 << 12;
+
+      RowVector C(2); C << 3 << 7;
+      ColumnVector D(4); D << 0 << 5 << 13 << 11;
+
+      Matrix M = KP(C * A, B * D) - KP(C, B) * KP(A, D); Print(M);
+   }
+
+   {
+      Tracer et1("Stage 6");
+
+      RowVector A(3), B(5), C(15);
+      A << 5 << 2 << 4;
+      B << 3 << 2 << 0 << 1 << 6;
+      C << 15 << 10 << 0 << 5 << 30
+        <<  6 <<  4 << 0 << 2 << 12
+        << 12 <<  8 << 0 << 4 << 24;
+      Matrix N = KP(A, B) - C;    Print(N);
+      N = KP(A.t(), B.t()) - C.t();    Print(N);
+      N = KP(A.AsDiagonal(), B.AsDiagonal()) - C.AsDiagonal();    Print(N);
+   }
+
+}
+
+
+
+
+
+
