miscmaths.cc

/*  miscmaths.cc

    Mark Jenkinson, Mark Woolrich, Christian Beckmann, Tim Behrens and Matthew Webster, FMRIB Image Analysis Group

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

/*  CCOPYRIGHT  */

// Miscellaneous maths functions
#define NOMINMAX
#include <cstdlib>
#include <cmath>
#include <algorithm>
#include "miscmaths.h"
#include "miscprob.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 isNumber( const string& input)
  {
    if (input.size()==0) return false;
    char *pend;
    strtod(input.c_str(),&pend);
    if (*pend!='\0') return false;
    return true; 
  } 

  // Use this to skip all lines that contain non-numeric entries, and return the first line starting with a number
  //  and the file pointer is reset to the beginning of the first line that starts with a number
  string skip_alpha(ifstream& fs) 
  {
    string cline;
    while (!fs.eof()) {
      streampos curpos = fs.tellg();
      // read a line, then turn it into a stream in order to read out the first token
      getline(fs,cline);
      cline += " "; // force extra entry in parsing (ensure at least one token is read)
      istringstream ss(cline.c_str());
      string firstToken="";
      ss >> firstToken; // Put first non-whitespace sequence into firstToken
      if (isNumber(firstToken)) {
	if (fs.eof()) { fs.clear(); }  // should only be executed if the file had no valid line terminators
	fs.seekg(curpos); 
	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="";
    double newnum;

    ss = skip_alpha(fs);
    for (int r=1; r<=nrows; r++) {
      istringstream sline(ss.c_str());
      for (int c=1; c<=ncols; c++) {
	sline >> newnum;
	if ( sline.eof() ) {
	  throw Exception("Could not find enough numbers in matrix file");
	}
	mat(r,c) = newnum;
      }						
      getline(fs,ss);  // this is processed now, so move the stream past it
      ss = skip_alpha(fs);
    }
    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 nRows(0), nColumns(0);
    string currentLine;
    // skip initial non-numeric lines
    //  and count the number of columns in the first numeric line
    currentLine = skip_alpha(fs);
    currentLine += " ";
    {
      istringstream ss(currentLine.c_str());
      string dummyToken="";
      while (!ss.eof()) {
	nColumns++;
	ss >> dummyToken;
      }
    }
    nColumns--;

    // count the number of lines that start with a number (don't worry if they don't have enough numbers at this stage)
    do {
      getline(fs,currentLine);
      currentLine += " "; // force extra entry in parsing
      istringstream ss(currentLine.c_str()); 
      string firstToken("");
      ss >> firstToken; //Put first non-whitespace sequence into cc
      if (isNumber(firstToken)) nRows++;  // 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,nRows,nColumns);

  }

#define BINFLAG 42

  ReturnMatrix read_binary_matrix(const string& filename)
  {
    Matrix mres;
    read_binary_matrix(mres,filename);
    mres.Release();
    return mres;
  }


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

  ReturnMatrix read_binary_matrix(ifstream& fs)
  {
    Matrix mres;
    read_binary_matrix(mres,fs);
    mres.Release();
    return mres;
  }

  int read_binary_matrix(Matrix& mres, 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;
	return 2;
      }
    }

    // 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;
    if ( (((unsigned int) mres.Ncols())<ny) || (((unsigned int) mres.Nrows())<nx) ) {
      mres.ReSize(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;
      }
    }
    
    return 0;
  }


  // 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)
  {
    fs.setf(ios::floatfield);  // use fixed or scientific notation as appropriate
    if (precision>0)  { 
      fs.precision(precision); 
    } else {
      fs.precision(10);    // default precision
    }
#ifdef PPC64	
    int n=0;
#endif
    for (int i=1; i<=mat.Nrows(); i++) {
      for (int j=1; j<=mat.Ncols(); j++) {
	fs << mat(i,j) << "  ";
#ifdef PPC64	
	if ((n++ % 50) == 0) fs.flush();
#endif
      }
      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;
#ifdef PPC64	
    int n=0;
#endif
    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));
#ifdef PPC64	
	if ((n++ % 50) == 0) fs.flush();
#endif
      }
    }

    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 norm2sq(double a, double b, double c)
    {
	return a*a + b*b + c*c;
    }

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

  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& mat2)
    {
      // calculates the psuedo-inverse using SVD
      // note that the right-pinv(x') = pinv(x).t()
      Matrix mat(mat2);
      if ( mat2.Ncols() > mat2.Nrows() )
	mat=mat.t();
      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);
	else D(n,n) = 0.0; // reduce the number of columns because too close to singular
      }
      Matrix pinv = V * D * U.t();
      if ( mat2.Ncols() > mat2.Nrows() )
	pinv=pinv.t();
      pinv.Release();
      return pinv;
    }

  int rank(const Matrix& X)
    {
      // calculates the rank of matrix X
      Tracer tr("rank");

      DiagonalMatrix eigenvals;
      SVD(X,eigenvals);

      double tolerance = Max(X.Nrows(),X.Ncols()) * eigenvals.Maximum() * 1e-16;

      int therank=0;

      for(int i=0; i<eigenvals.Nrows(); i++)
	if (eigenvals(i+1)>tolerance)
	  therank++;

      // cout << "tolerance = " << tolerance << "\n" << "eigenvalues = " << eigenvals << "\n" << "rank = " << therank << endl;

      return therank;
    }


  ReturnMatrix sqrtaff(const Matrix& mat)
    {
      Tracer tr("sqrtaff");
      Matrix matnew(4,4), rot, id4;
      rot=IdentityMatrix(4);
      id4=IdentityMatrix(4);
      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=IdentityMatrix(4);
      scale(1,1)=params(7);
      scale(2,2)=params(8);
      scale(3,3)=params(9);

      Matrix skew=IdentityMatrix(4);
      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;
    }

  bool strict_less_than(pair<double, int> x, pair<double, int> y) { return x.first < y.first; }

  vector<int> get_sortindex(const Matrix& vals, const string& mode, int col)
  {
    // mode is either "new2old" or "old2new"
    // return the mapping of old and new indices in the *ascending* sort of vals (from column=col)
    int length=vals.Nrows();
    vector<pair<double, int> > sortlist(length);
    for (int n=0; n<length; n++) {
      sortlist[n] = pair<double, int>((double) vals(n+1,col),n+1);
    }
    sort(sortlist.begin(),sortlist.end(),strict_less_than);  // O(N.log(N))
    vector<int> idx(length);
    for (int n=0; n<length; n++) {
      if (mode=="old2new") {
	//  here idx[n] is the where in the ordered list the old n'th row is mapped to (i.e. idx[n] = rank)
	idx[sortlist[n].second-1] = n+1;
      } else if (mode=="new2old") {
	// here idx[n] is the the old row number of the n'th ordered item (i.e. idx[n] is old row number with rank = n)
	idx[n] = sortlist[n].second;  
      } else {
	cerr << "ERROR:: unknown mode in get_sortidx = " << mode << endl; 
      }
    }
    return idx;
  }

  Matrix apply_sortindex(const Matrix& vals, vector<int> sidx, const string& mode)
  {
    // mode is either "new2old" or "old2new"
    // apply the index mapping from get_sortindex to the whole matrix (swapping rows)
    Matrix res(vals);
    res=0.0;
    int ncols=vals.Ncols();
    for (unsigned int n=0; n<sidx.size(); n++) {
      int row = sidx[n];
      if (mode=="old2new") {
	res.SubMatrix(row,row,1,ncols)=vals.SubMatrix(n+1,n+1,1,ncols);
      } else if (mode=="new2old") {
	res.SubMatrix(n+1,n+1,1,ncols)=vals.SubMatrix(row,row,1,ncols);
      } else {
	cerr << "ERROR:: unknown mode in apply_sortidx = " << mode << endl; 
      }
    }
    return res;
  }



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

  // 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;
    }

  int addrow(Matrix& m, int ncols)
  {
    if (m.Nrows()==0) {
      Matrix mm(1,ncols);
      mm=0;
      m = mm;
    } else {
      Matrix mm(m.Nrows()+1,ncols);
      mm = 0;
      mm.SubMatrix(1,m.Nrows(),1,ncols) = m;
      m = mm;
    }
    return 0;
  }
  
  //------------------------------------------------------------------------//


  // 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);
      aff=IdentityMatrix(4);

      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");
      aff=IdentityMatrix(4);

      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");
      rot=IdentityMatrix(4);  // 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=IdentityMatrix(3);
      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=IdentityMatrix(3);
      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() - IdentityMatrix(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-IdentityMatrix(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;