miscmaths.cc

/*  miscmaths.cc

    Mark Jenkinson & Mark Woolrich & Christian Beckmann & Tim Behrens, FMRIB Image Analysis Group

    Copyright (C) 1999-2000 University of Oxford  */

/*  CCOPYRIGHT  */

// Miscellaneous maths functions

#include "miscmaths.h"
#include "stdlib.h"
#include "newmatio.h"

using namespace std;

namespace MISCMATHS {

  // The following lines are ignored by the current SGI compiler
  //  (version egcs-2.91.57)
  // A temporary fix of including the std:: in front of all abs() etc
  //  has been done for now
  using std::abs;
  using std::sqrt;
  using std::exp;
  using std::log;
  //  using std::pow;
  using std::atan2;


  string size(const Matrix& mat)
  {
    string str = num2str(mat.Nrows())+"*"+num2str(mat.Ncols());
        
    return str;
  }



  float Sinc(const float x) {
    if (fabs(x)<1e-9) {
      return 1-x*x*M_PI*M_PI/6.0;
    } else {
      return sin(M_PI*x)/(M_PI*x);
    }
  }

  double Sinc(const double x) {
    if (fabs(x)<1e-9) {
      return 1-x*x*M_PI*M_PI/6.0;
    } else {
      return sin(M_PI*x)/(M_PI*x);
    }
  }


  // General string/IO functions

  bool isnum(const string& str)
  {
    // assumes that initial whitespace has been removed
    if (isdigit(str[0])) return true;
    if ( (str[0]=='-') || (str[0]=='+') || (str[0]=='.') )  return true;
    return false;
  }
    
  string skip_alpha(ifstream& fs) 
  {
    string cline;
    while (!fs.eof()) {
      getline(fs,cline);
      cline += " "; // force extra entry in parsing
      istrstream ss(cline.c_str());
      string cc="";
      ss >> cc;
      if (isnum(cc)) {
	fs.seekg(-((int)cline.size()),ios::cur);
	return cline;
      }
    }
    return "";
  }

  ReturnMatrix read_ascii_matrix(int nrows, int ncols, const string& filename)
  {
    return read_ascii_matrix(filename,nrows,ncols);
  }


  ReturnMatrix read_ascii_matrix(const string& filename, int nrows, int ncols)
  {
    Matrix mat(nrows,ncols);
    mat = 0.0;
  
    if ( filename.size()<1 ) return mat;
    ifstream fs(filename.c_str());
    if (!fs) { 
      cerr << "Could not open matrix file " << filename << endl;
      return mat;
    }
    mat = read_ascii_matrix(fs,nrows,ncols);
    fs.close();
    mat.Release();
    return mat;
  }

  ReturnMatrix read_ascii_matrix(int nrows, int ncols, ifstream& fs)
  {
    return read_ascii_matrix(fs, nrows, ncols);
  }

  ReturnMatrix read_ascii_matrix(ifstream& fs, int nrows, int ncols)
  {
    Matrix mat(nrows,ncols);
    mat = 0.0;
    string ss="";
  
    ss = skip_alpha(fs);
    for (int r=1; r<=nrows; r++) {
      for (int c=1; c<=ncols; c++) {
	if (!fs.eof()) {
	  fs >> ss;
	  while ( !isnum(ss) && !fs.eof() ) {
	    fs >> ss;
	  }
	  mat(r,c) = atof(ss.c_str());
	}
      }
    }
    mat.Release();
    return mat;
  }

  
  ReturnMatrix read_ascii_matrix(const string& filename)
  {
    Matrix mat;
    if ( filename.size()<1 ) return mat;
    ifstream fs(filename.c_str());
    if (!fs) { 
      cerr << "Could not open matrix file " << filename << endl;
      mat.Release();
      return mat;
    }
    mat = read_ascii_matrix(fs);
    fs.close();
    mat.Release();
    return mat;
  }


  ReturnMatrix read_ascii_matrix(ifstream& fs)
  {
    int rcount=0, cmax=0;
    string cline;
    // skip initial non-numeric lines
    //  and count the number of columns in the first numeric line
    cline = skip_alpha(fs);
    cline += " ";
    {
      istrstream ss(cline.c_str());
      string cc="";
      while (!ss.eof()) {
	cmax++;
	ss >> cc;
      }
    }
    cmax--;

    do {
      getline(fs,cline);
      cline += " "; // force extra entry in parsing
      istrstream ss(cline.c_str());
      string cc="";
      ss >> cc;
      if (!isnum(cc)) break;  // stop processing when non-numeric line found
      rcount++;  // add new row to matrix
    } while (!fs.eof());
    
    // now know the size of matrix
    fs.clear();
    fs.seekg(0,ios::beg);    
    return read_ascii_matrix(fs,rcount,cmax);

  }

#define BINFLAG 42

  ReturnMatrix read_binary_matrix(const string& filename)
  {
    Matrix mat;
    if ( filename.size()<1 ) return mat;
    ifstream fs(filename.c_str(), ios::in | ios::binary);
    if (!fs) { 
      cerr << "Could not open matrix file " << filename << endl;
      return mat;
    }
    mat = read_binary_matrix(fs);
    fs.close();
    mat.Release();
    return mat;
  }


  ReturnMatrix read_binary_matrix(ifstream& fs)
  {
    bool swapbytes = false;
    unsigned int testval;
    // test for byte swapping
    fs.read((char*)&testval,sizeof(testval));
    if (testval!=BINFLAG) {
      swapbytes = true;
      Swap_Nbytes(1,sizeof(testval),&testval);
      if (testval!=BINFLAG) { 
	cerr << "Unrecognised binary matrix file format" << endl;
	Matrix mres;
	mres.Release();
	return mres;
      }
    }

    // read matrix dimensions (num rows x num cols)
    unsigned int ival,nx,ny;
    fs.read((char*)&ival,sizeof(ival));
    // ignore the padding (reserved for future use)
    fs.read((char*)&ival,sizeof(ival));
    if (swapbytes) Swap_Nbytes(1,sizeof(ival),&ival);
    nx = ival;
    fs.read((char*)&ival,sizeof(ival));
    if (swapbytes) Swap_Nbytes(1,sizeof(ival),&ival);
    ny = ival;

    // set up and read matrix (rows fast, cols slow)
    double val;
    Matrix mres(nx,ny);
    for (unsigned int y=1; y<=ny; y++) {
      for (unsigned int x=1; x<=nx; x++) {
	fs.read((char*)&val,sizeof(val));
	if (swapbytes) Swap_Nbytes(1,sizeof(val),&val);
	mres(x,y)=val;
      }
    }
    
    mres.Release();
    return mres;
  }


  // WRITE FUNCTIONS //


  int write_ascii_matrix(const string& filename, const Matrix& mat, 
			 int precision)
  {
    return write_ascii_matrix(mat, filename, precision);
  }

  int write_ascii_matrix(const Matrix& mat, const string& filename, 
			 int precision)
  {
    Tracer tr("write_ascii_matrix");
    if ( (filename.size()<1) ) return -1;
    ofstream fs(filename.c_str());
    if (!fs) { 
      cerr << "Could not open file " << filename << " for writing" << endl;
      return -1;
    }
    int retval = write_ascii_matrix(mat,fs,precision);
    fs.close();
    return retval;
  }

  int write_ascii_matrix(ofstream& fs, const Matrix& mat, 
			 int precision)
  {
    return write_ascii_matrix(mat, fs, precision);
  }
  
  int write_ascii_matrix(const Matrix& mat, ofstream& fs, int precision)
  {
    for (int i=1; i<=mat.Nrows(); i++) {
      for (int j=1; j<=mat.Ncols(); j++) {
	if (precision>0)  { 
	  fs.setf(ios::scientific | ios::showpos); 
	  fs.precision(precision+7);  
	  fs.precision(precision); } 
	fs << mat(i,j) << "  ";
      }
      fs << endl;
    }
    return 0;
  }
  
  int write_vest(string p_fname, const Matrix& x, int precision)
     { return write_vest(x,p_fname,precision); } 

  int write_vest(const Matrix& x, string p_fname, int precision)
  {
    ofstream out;
    out.open(p_fname.c_str(), ios::out);
    
    if(!out)
      {
	cerr << "Unable to open " << p_fname << endl;
	return -1;
      }

    out << "! VEST-Waveform File" << endl;
    out << "/NumWaves\t" << x.Ncols() << endl;
    out << "/NumPoints\t" << x.Nrows() << endl;
    out << "/Skip" << endl;
    out << endl << "/Matrix" << endl;

    int retval = write_ascii_matrix(x, out, precision);

    out.close();

    return retval;
  }


  int write_binary_matrix(const Matrix& mat, const string& filename)
  {
    Tracer tr("write_binary_matrix");
    if ( (filename.size()<1) ) return -1;
    ofstream fs(filename.c_str(), ios::out | ios::binary);
    if (!fs) { 
      cerr << "Could not open file " << filename << " for writing" << endl;
      return -1;
    }
    int retval = write_binary_matrix(mat,fs);
    fs.close();
    return retval;
  }


  int write_binary_matrix(const Matrix& mat, ofstream& fs)
  {
    unsigned int ival, nx, ny;

    ival = BINFLAG;
    fs.write((char*)&ival,sizeof(ival));
    ival = 0;  // padding (reserved for future use)
    fs.write((char*)&ival,sizeof(ival));
    ival = mat.Nrows();
    fs.write((char*)&ival,sizeof(ival));
    ival = mat.Ncols();
    fs.write((char*)&ival,sizeof(ival));

    nx = mat.Nrows();
    ny = mat.Ncols();

    double val;
    for (unsigned int y=1; y<=ny; y++) {
      for (unsigned int x=1; x<=nx; x++) {
	val = mat(x,y);
	fs.write((char*)&val,sizeof(val));
      }
    }

    return 0;
  }


  // General mathematical functions

  int round(int x) { return x; }

  int round(float x) 
    { 
      if (x>0.0) return ((int) (x+0.5));
      else       return ((int) (x-0.5));
    }

   int round(double x) 
   { 
     if (x>0.0) return ((int) (x+0.5));
     else       return ((int) (x-0.5));
   }  
  
  double rounddouble(double x){
    return ( floor(x+0.5));
  }
  int periodicclamp(int x, int x1, int x2)
   {
     if (x2<x1) return periodicclamp(x,x2,x1);
     int d = x2-x1+1;
     int xp = x-x1;
     if (xp>=0) {
       return (xp % d) + x1;
     } else {
       xp = xp + d + std::abs(xp/d)*d;
       assert(xp>0);
       return periodicclamp(xp + d + std::abs(xp/d)*d,x1,x2);
     }
   }

  ColumnVector cross(const ColumnVector& a, const ColumnVector& b)
    {
      Tracer tr("cross");
      ColumnVector ans(3);
      ans(1) = a(2)*b(3) - a(3)*b(2);
      ans(2) = a(3)*b(1) - a(1)*b(3);
      ans(3) = a(1)*b(2) - a(2)*b(1);
      return ans;
    }


  ColumnVector cross(const Real *a, const Real *b)
    {
      Tracer tr("cross");
      ColumnVector a1(3), b1(3);
      a1 << a;
      b1 << b;
      return cross(a1,b1);
    }


  double norm2(const ColumnVector& x)
    {
      return std::sqrt(x.SumSquare());
    }

  double norm2(double a, double b, double c)
    {
	return a*a + b*b + c*c;
    }

  float norm2(float a, float b, float c)
    {
	return a*a + b*b + c*c;
    }

  int Identity(Matrix& m)
    {
      Tracer tr("Identity");
      m=0.0;
      for (int j=1; j<=m.Nrows(); j++)
	m(j,j)=1.0;
      return 0;
    }

  ReturnMatrix Identity(int num)
    {
      Tracer tr("Identity");
      Matrix eye(num,num);
      Identity(eye);
      eye.Release();
      return eye;
    }


  int diag(Matrix& m, const float diagvals[])
    {
      Tracer tr("diag");
      m=0.0;
      for (int j=1; j<=m.Nrows(); j++)
	m(j,j)=diagvals[j-1];
      return 0;
    }


  int diag(DiagonalMatrix& m, const ColumnVector& diagvals)
    {
      Tracer tr("diag");

      m.ReSize(diagvals.Nrows());
      m=0.0;
      for (int j=1; j<=diagvals.Nrows(); j++)
	m(j)=diagvals(j);
      return 0;
    }

  int diag(Matrix& m, const ColumnVector& diagvals)
    {
      Tracer tr("diag");
      
      m.ReSize(diagvals.Nrows(),diagvals.Nrows());
      m=0.0;
      for (int j=1; j<=diagvals.Nrows(); j++)
	m(j,j)=diagvals(j);
      return 0;
    }

  ReturnMatrix diag(const Matrix& Mat)
    {
      Tracer tr("diag");
      if(Mat.Ncols()==1){
	Matrix retmat(Mat.Nrows(),Mat.Nrows());
	diag(retmat,Mat);
	retmat.Release();
	return retmat;}
      else{
	int mindim = Min(Mat.Ncols(),Mat.Nrows());
	Matrix retmat(mindim,1);
	for(int ctr=1; ctr<=mindim;ctr++){
	  retmat(ctr,1)=Mat(ctr,ctr);
	}
	retmat.Release();
	return retmat;
      }
    }

  ReturnMatrix pinv(const Matrix& mat)
    {
      // calculates the psuedo-inverse using SVD
      Tracer tr("pinv");
      DiagonalMatrix D;
      Matrix U, V;
      SVD(mat,D,U,V);
      float tol;
      tol = MaximumAbsoluteValue(D) * Max(mat.Nrows(),mat.Ncols()) * 1e-16;
      for (int n=1; n<=D.Nrows(); n++) {
	if (fabs(D(n,n))>tol)  D(n,n) = 1.0/D(n,n);
      }
      Matrix pinv = V * D * U.t();
      pinv.Release();
      return pinv;
    }


  ReturnMatrix sqrtaff(const Matrix& mat)
    {
      Tracer tr("sqrtaff");
      Matrix matnew(4,4), rot(4,4), id4(4,4);
      Identity(rot);
      Identity(id4);
      ColumnVector params(12), centre(3), trans(4);
      centre = 0.0;
      // Quaternion decomposition -> params(1..3) = sin(theta/2)*(unit_axis_vec)
      // Want a new quaternion : q = sin(theta/4)*(unit_axis_vec)
      // Therefore factor of conversion is: factor = sin(theta/4)/sin(theta/2)
      //      = 1/(2  * cos(theta/4))   which is calculated below
      //  NB: t = theta/2
      decompose_aff(params,mat,centre,rotmat2quat);
      double sint;
      sint = std::sqrt(params(1)*params(1) + params(2)*params(2) + 
		  params(3)*params(3));
      double t = asin(sint);
      double factor = 1.0/(2.0*cos(0.5*t));
      params(1) = factor * params(1);
      params(2) = factor * params(2);
      params(3) = factor * params(3);
      params(7) = std::sqrt(params(7));
      params(8) = std::sqrt(params(8));
      params(9) = std::sqrt(params(9));
      params(10) = 0.5*params(10);
      params(11) = 0.5*params(11);
      params(12) = 0.5*params(12);

      construct_rotmat_quat(params,3,rot,centre);
      rot(1,4) = 0.0;
      rot(2,4) = 0.0;
      rot(3,4) = 0.0;
      
      Matrix scale(4,4);
      Identity(scale);
      scale(1,1)=params(7);
      scale(2,2)=params(8);
      scale(3,3)=params(9);

      Matrix skew(4,4);
      Identity(skew);
      skew(1,2)=params(10);
      skew(1,3)=params(11);
      skew(2,3)=params(12);

      trans(1) = params(4);
      trans(2) = params(5);
      trans(3) = params(6);
      trans(4) = 1.0;

      // The translation, being independent of the 3x3 submatrix, is
      //  calculated so that it will be equal for each of the two 
      //  halves of the approximate square root 
      //  (i.e. matnew and mat*matnew.i() have exactly the same translation)
      ColumnVector th(4);
      th = (mat*scale.i()*skew.i()*rot.i() + id4).SubMatrix(1,3,1,3).i() 
	* trans.SubMatrix(1,3,1,1);

      matnew = rot*skew*scale;
      matnew(1,4) = th(1);
      matnew(2,4) = th(2);
      matnew(3,4) = th(3);

      matnew.Release();
      return matnew;
    }


  //------------------------------------------------------------------------//

  // Handy MATLAB-like functions
  
  void reshape(Matrix& r, const Matrix& m, int nrows, int ncols)
    {
      Tracer tr("reshape");
      if (nrows*ncols != m.Nrows() * m.Ncols() ) {
	cerr << "WARNING: cannot reshape " << m.Nrows() << "x"
	     << m.Ncols() << " matrix into " << nrows << "x" 
	     << ncols << endl;
	cerr << " Returning original matrix instead" << endl;
	r = m;
	return;
      }
      r.ReSize(nrows,ncols);
      int rr = 1, rc = 1;
      for (int mc=1; mc<=m.Ncols(); mc++) {
	for (int mr=1; mr<=m.Nrows(); mr++) {
	  r(rr,rc) = m(mr,mc);
	  rr++;
	  if (rr>nrows) {
	    rc++;
	    rr=1;
	  }
	}
      }
    }

  ReturnMatrix reshape(const Matrix& m, int nrows, int ncols)
    {
      Tracer tr("reshape");
     
      Matrix r; 
      
      reshape(r,m,nrows,ncols);

      r.Release();
      return r;
    }

  //------------------------------------------------------------------------//


  // Spatial transformation functions (rotations and affine transforms)


  int construct_rotmat_euler(const ColumnVector& params, int n, Matrix& aff,
			     const ColumnVector& centre)
    {
      Tracer tr("construct_rotmat_euler");
      ColumnVector angl(3);
      Matrix newaff(4,4);
      Identity(aff);

      if (n<=0) return 0;
      // order of parameters is 3 rotation + 3 translation
      // angles are in radians 
      //  order of parameters is (Rx,Ry,Rz) and R = Rx.Ry.Rz
      angl=0.0;
      angl(1)=params(1);
      make_rot(angl,centre,newaff);
      aff = aff * newaff;
      if (n==1) return 0;

      angl=0.0;
      angl(2)=params(2);
      make_rot(angl,centre,newaff);
      aff = aff * newaff; 
      if (n==2) return 0;

      angl=0.0;
      angl(3)=params(3);
      make_rot(angl,centre,newaff);
      aff = aff * newaff;
      if (n==3) return 0;

      aff(1,4)+=params(4);
      if (n==4) return 0;
      aff(2,4)+=params(5);
      if (n==5) return 0;
      aff(3,4)+=params(6);
      if (n==6) return 0;

      return 1;
    }  

  int construct_rotmat_euler(const ColumnVector& params, int n, Matrix& aff)
    {
      Tracer tr("construct_rotmat_euler");
      ColumnVector centre(3);
      centre = 0.0;
      return construct_rotmat_euler(params,n,aff,centre);
    }


  int construct_rotmat_quat(const ColumnVector& params, int n, Matrix& aff,
			    const ColumnVector& centre)
    {
      Tracer tr("construct_rotmat_quat");
      Identity(aff);

      if (n<=0) return 0;
      // order of parameters is 3 rotation (last 3 quaternion components) 
      //  + 3 translation

      if ((n>=1) && (n<3)) { cerr<<"Can only do 3 or more, not "<< n <<endl; }
      float w, w2 = 1.0 - Sqr(params(1)) - Sqr(params(2)) - Sqr(params(3));
      if (w2 < 0.0) {
	cerr << "Parameters do not form a valid axis - greater than unity\n";
	return -1;
      }
      w = std::sqrt(w2);
      float x=params(1), y=params(2), z=params(3);
      aff(1,1) = 1 - 2*y*y - 2*z*z;
      aff(2,2) = 1 - 2*x*x - 2*z*z;
      aff(3,3) = 1 - 2*x*x - 2*y*y;
      aff(1,2) = 2*x*y - 2*w*z;
      aff(2,1) = 2*x*y + 2*w*z;
      aff(1,3) = 2*x*z + 2*w*y;
      aff(3,1) = 2*x*z - 2*w*y;
      aff(2,3) = 2*y*z - 2*w*x;
      aff(3,2) = 2*y*z + 2*w*x;

      // Given Rotation matrix R:  x' = Rx + (I-R)*centre
      ColumnVector trans(3);
      trans = aff.SubMatrix(1,3,1,3)*centre;
      aff.SubMatrix(1,3,4,4) = centre - trans;

      aff(1,4) += params(4);
      if (n==4) return 0;
      aff(2,4) += params(5);
      if (n==5) return 0;
      aff(3,4) += params(6);
      if (n==6) return 0;

      return 1;
    }  

  int construct_rotmat_quat(const ColumnVector& params, int n, Matrix& aff)
    {
      Tracer tr("construct_rotmat_quat");
      ColumnVector centre(3);
      centre = 0.0;
      return construct_rotmat_quat(params,n,aff,centre);
    }

  int make_rot(const ColumnVector& angl, const ColumnVector& centre, 
	       Matrix& rot)
    {
      // Matrix rot must be 4x4; angl and orig must be length 3
      Tracer tr("make_rot");
      Identity(rot);  // default return value
      float theta;
      theta = norm2(angl);
      if (theta<1e-8) {  // avoid round-off errors and return Identity
	return 0;
      }
      ColumnVector axis = angl/theta;
      ColumnVector x1(3), x2(3), x3(3);
      x1 = axis;
      x2(1) = -axis(2);  x2(2) = axis(1);  x2(3) = 0.0;
      if (norm2(x2)<=0.0) {
	x2(1) = 1.0;  x2(2) = 0.0;  x2(3) = 0.0;
      }
      x2 = x2/norm2(x2);
      x3 = cross(x1,x2);
      x3 = x3/norm2(x3);

      Matrix basischange(3,3);
      basischange.SubMatrix(1,3,1,1) = x2;
      basischange.SubMatrix(1,3,2,2) = x3;
      basischange.SubMatrix(1,3,3,3) = x1;

      Matrix rotcore(3,3);
      Identity(rotcore);
      rotcore(1,1)=cos(theta);
      rotcore(2,2)=cos(theta);
      rotcore(1,2)=sin(theta);
      rotcore(2,1)=-sin(theta);

      rot.SubMatrix(1,3,1,3) = basischange * rotcore * basischange.t();
  
      Matrix ident3(3,3);
      Identity(ident3);
      ColumnVector trans(3);
      trans = (ident3 - rot.SubMatrix(1,3,1,3))*centre;
      rot.SubMatrix(1,3,4,4)=trans;
      return 0;
    }


  int getrotaxis(ColumnVector& axis, const Matrix& rotmat)
    {
      Tracer tr("getrotaxis");
      Matrix residuals(3,3);
      residuals = rotmat*rotmat.t() - Identity(3);
      if (residuals.SumSquare() > 1e-4)
	{ cerr << "Failed orthogonality check!" << endl;  return -1; }
      Matrix u(3,3), v(3,3);
      DiagonalMatrix d(3);
      SVD(rotmat-Identity(3),d,u,v);
      // return column of V corresponding to minimum value of |S|
      for (int i=1; i<=3; i++) {
	if (fabs(d(i))<1e-4)  axis = v.SubMatrix(1,3,i,i);
      }
      return 0;
    }  
  

  int rotmat2euler(ColumnVector& angles, const Matrix& rotmat)
    {
      // uses the convention that R = Rx.Ry.Rz
      Tracer tr("rotmat2euler");
      float cz, sz, cy, sy, cx, sx;
      cy = std::sqrt(Sqr(rotmat(1,1)) + Sqr(rotmat(1,2)));
      if (cy < 1e-4) {
	//cerr << "Cos y is too small - Gimbal lock condition..." << endl;
	cx = rotmat(2,2);
	sx = -rotmat(3,2);
	sy = -rotmat(1,3);
	angles(1) = atan2(sx,cx);
	angles(2) = atan2(sy,(float)0.0);
	angles(3) = 0.0;
      } else {
	// choose by convention that cy > 0
	// get the same rotation if: sy stays same & all other values swap sign
	cz = rotmat(1,1)/cy;
	sz = rotmat(1,2)/cy;
	cx = rotmat(3,3)/cy;
	sx = rotmat(2,3)/cy;
	sy = -rotmat(1,3);
	//atan2(sin,cos)  (defined as atan2(y,x))
	angles(1) = atan2(sx,cx);
	angles(2) = atan2(sy,cy);
	angles(3) = atan2(sz,cz);
      }
      return 0;
    }


  int rotmat2quat(ColumnVector& quaternion, const Matrix& rotmat)
    {
      Tracer tr("rotmat2quat");

      float trace = rotmat.SubMatrix(1,3,1,3).Trace();
	
      if (trace > 0) {
	float w = std::sqrt((trace + 1.0)/4.0);
	quaternion(1) = (rotmat(3,2) - rotmat(2,3))/(4.0*w);
	quaternion(2) = (rotmat(1,3) - rotmat(3,1))/(4.0*w);
	quaternion(3) = (rotmat(2,1) - rotmat(1,2))/(4.0*w);
      } else if ((rotmat(1,1) > rotmat(2,2)) && (rotmat(1,1) > rotmat(3,3))) {
	// first col case
	float s = std::sqrt(1.0 + rotmat(1,1) - rotmat(2,2) - rotmat(3,3)) * 2.0;
	quaternion(1) = 0.5 / s;
	quaternion(2) = (-rotmat(1,2) - rotmat(1,2)) / s;
	quaternion(3) = (-rotmat(1,3) - rotmat(3,1)) / s;
      } else if ((rotmat(2,2) > rotmat(1,1)) && (rotmat(2,2) > rotmat(3,3))) {
	// 2nd col case
	float s = std::sqrt(1.0 + rotmat(2,2) - rotmat(1,1) - rotmat(3,3)) * 2.0;
	quaternion(1) = (-rotmat(1,2) - rotmat(2,1)) / s;
	quaternion(2) = 0.5 / s;
	quaternion(3) = (-rotmat(2,3) - rotmat(3,2)) / s;
      } else if ((rotmat(3,3) > rotmat(1,1)) && (rotmat(3,3) > rotmat(2,2))) {
	// 3rd col case
	float s = std::sqrt(1.0 + rotmat(3,3) - rotmat(1,1) - rotmat(2,2)) * 2.0;
	quaternion(1) = (-rotmat(1,3) - rotmat(3,1)) / s;
	quaternion(2) = (-rotmat(2,3) - rotmat(3,2)) / s;
	quaternion(3) = 0.5 / s;
      }
      return 0;
    }


  int decompose_aff(ColumnVector& params, const Matrix& affmat, 
		    const ColumnVector& centre,
		    int (*rotmat2params)(ColumnVector& , const Matrix& ))
    {
      // decomposes using the convention: mat = rotmat * skew * scale
      // order of parameters is 3 rotation + 3 translation + 3 scales + 3 skews
      // angles are in radians
      Tracer tr("decompose_aff");
      if (params. Nrows() < 12)
	params.ReSize(12);
      if (rotmat2params==0)  
	{ 
	  cerr << "No rotmat2params function specified" << endl;  
	  return -1; 
	}
      ColumnVector x(3), y(3), z(3);
      Matrix aff3(3,3);
      aff3 = affmat.SubMatrix(1,3,1,3);
      x = affmat.SubMatrix(1,3,1,1);
      y = affmat.SubMatrix(1,3,2,2);
      z = affmat.SubMatrix(1,3,3,3);
      float sx, sy, sz, a, b, c;
      sx = norm2(x);
      sy = std::sqrt( dot(y,y) - (Sqr(dot(x,y)) / Sqr(sx)) );
      a = dot(x,y)/(sx*sy);
      ColumnVector x0(3), y0(3);
      x0 = x/sx;
      y0 = y/sy - a*x0;
      sz = std::sqrt(dot(z,z) - Sqr(dot(x0,z)) - Sqr(dot(y0,z)));
      b = dot(x0,z)/sz;
      c = dot(y0,z)/sz;
      params(7) = sx;  params(8) = sy;  params(9) = sz;
      Matrix scales(3,3);
      float diagvals[] = {sx,sy,sz};
      diag(scales,diagvals);
      Real skewvals[] = {1,a,b,0 , 0,1,c,0 , 0,0,1,0 , 0,0,0,1}; 
      Matrix skew(4,4);
      skew  << skewvals;
      params(10) = a;  params(11) = b;  params(12) = c;
      Matrix rotmat(3,3);
      rotmat = aff3 * scales.i() * (skew.SubMatrix(1,3,1,3)).i();
      ColumnVector transl(3);
      //transl = affmat.SubMatrix(1,3,4,4);
      //transl = transl - (Identity(3) - rotmat)*centre;
      transl = affmat.SubMatrix(1,3,1,3)*centre + affmat.SubMatrix(1,3,4,4)
	         - centre;
      for (int i=1; i<=3; i++)  { params(i+3) = transl(i); }
      ColumnVector rotparams(3);
      (*rotmat2params)(rotparams,rotmat);
      for (int i=1; i<=3; i++)  { params(i) = rotparams(i); }
      return 0;
    }

  int decompose_aff(ColumnVector& params, const Matrix& affmat, 
		    int (*rotmat2params)(ColumnVector& , const Matrix& ))
    {
      Tracer tr("decompose_aff");
      ColumnVector centre(3);
      centre = 0.0;
      return decompose_aff(params,affmat,centre,rotmat2params);
    }



  int compose_aff(const ColumnVector& params, int n, const ColumnVector& centre,
		  Matrix& aff, 
		  int (*params2rotmat)(const ColumnVector& , int , Matrix& ,
			     const ColumnVector& ) )
    {
      Tracer tr("compose_aff");
      if (n<=0) return 0;
      // order of parameters is 3 rotation + 3 translation + 3 scales + 3 skews
      // angles are in radians

      (*params2rotmat)(params,n,aff,centre);
  
      if (n<=6)  return 0;
  
      Matrix scale(4,4);
      Identity(scale);
      if (n>=7) {
	scale(1,1)=params(7);
	if (n>=8) scale(2,2)=params(8);
	else      scale(2,2)=params(7);
	if (n>=9) scale(3,3)=params(9);
	else      scale(3,3)=params(7);
      }
      // fix the translation so that the centre is not moved
      ColumnVector strans(3);
      strans = centre - scale.SubMatrix(1,3,1,3)*centre;
      scale.SubMatrix(1,3,4,4) = strans;

      Matrix skew(4,4);
      Identity(skew);
      if (n>=10) {
	if (n>=10) skew(1,2)=params(10);
	if (n>=11) skew(1,3)=params(11);
	if (n>=12) skew(2,3)=params(12);
      }
      // fix the translation so that the centre is not moved
      ColumnVector ktrans(3);
      ktrans = centre - skew.SubMatrix(1,3,1,3)*centre;
      skew.SubMatrix(1,3,4,4) = ktrans;

      aff = aff * skew * scale;

      return 0;
    }


float rms_deviation(const Matrix& affmat1, const Matrix& affmat2, 
		    const ColumnVector& centre, const float rmax) 
{
  Tracer trcr("rms_deviation");
  Matrix isodiff(4,4);
  try {
    isodiff = affmat1*affmat2.i() - Identity(4);
  } catch(...) {
    cerr << "RMS_DEVIATION ERROR:: Could not invert matrix" << endl;  
    exit(-5); 
  }
  Matrix adiff(3,3);
  adiff = isodiff.SubMatrix(1,3,1,3);
  ColumnVector tr(3);
  tr = isodiff.SubMatrix(1,3,4,4) + adiff*centre;
  float rms = std::sqrt( (tr.t() * tr).AsScalar() + 
		    (rmax*rmax/5.0)*Trace(adiff.t()*adiff) );
  return rms;
}


float rms_deviation(const Matrix& affmat1, const Matrix& affmat2, 
		    const float rmax) 
{
  ColumnVector centre(3);