diff --git a/miscmaths.cc b/miscmaths.cc
index ccc650d75179f6920e96e24974bbbb6a3b0c8fdd..267554bf6487142f4e9f3cae41b1841fe6a79ed5 100644
--- a/miscmaths.cc
+++ b/miscmaths.cc
@@ -83,9 +83,9 @@ namespace MISCMATHS {
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;
+ if (fs.eof()) { fs.clear(); } // should only be executed if the file had no valid line terminators
+ fs.seekg(curpos);
+ return cline;
}
}
return "";
@@ -130,15 +130,15 @@ namespace MISCMATHS {
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;
+ sline >> newnum;
+ if ( sline.eof() ) {
+ throw Exception("Could not find enough numbers in matrix file");
+ }
+ mat(r,c) = newnum;
}
if ( r!=nrows ) {
- getline(fs,ss); // this is processed now, so move the stream past it
- ss = skip_alpha(fs);
+ getline(fs,ss); // this is processed now, so move the stream past it
+ ss = skip_alpha(fs);
}
}
mat.Release();
@@ -175,8 +175,8 @@ namespace MISCMATHS {
istringstream ss(currentLine.c_str());
string dummyToken="";
while (!ss.eof()) {
- nColumns++;
- ss >> dummyToken;
+ nColumns++;
+ ss >> dummyToken;
}
}
nColumns--;
@@ -240,8 +240,8 @@ namespace MISCMATHS {
swapbytes = true;
Swap_Nbytes(1,sizeof(testval),&testval);
if (testval!=BINFLAG) {
- cerr << "Unrecognised binary matrix file format" << endl;
- return 2;
+ cerr << "Unrecognised binary matrix file format" << endl;
+ return 2;
}
}
@@ -263,9 +263,9 @@ namespace MISCMATHS {
}
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;
+ fs.read((char*)&val,sizeof(val));
+ if (swapbytes) Swap_Nbytes(1,sizeof(val),&val);
+ mres(x,y)=val;
}
}
@@ -277,13 +277,13 @@ namespace MISCMATHS {
int write_ascii_matrix(const string& filename, const Matrix& mat,
- int precision)
+ int precision)
{
return write_ascii_matrix(mat, filename, precision);
}
int write_ascii_matrix(const Matrix& mat, const string& filename,
- int precision)
+ int precision)
{
Tracer tr("write_ascii_matrix");
if ( (filename.size()<1) ) return -1;
@@ -298,7 +298,7 @@ namespace MISCMATHS {
}
int write_ascii_matrix(ofstream& fs, const Matrix& mat,
- int precision)
+ int precision)
{
return write_ascii_matrix(mat, fs, precision);
}
@@ -316,9 +316,9 @@ namespace MISCMATHS {
#endif
for (int i=1; i<=mat.Nrows(); i++) {
for (int j=1; j<=mat.Ncols(); j++) {
- fs << mat(i,j) << " ";
+ fs << mat(i,j) << " ";
#ifdef PPC64
- if ((n++ % 50) == 0) fs.flush();
+ if ((n++ % 50) == 0) fs.flush();
#endif
}
fs << endl;
@@ -327,7 +327,7 @@ namespace MISCMATHS {
}
int write_vest(string p_fname, const Matrix& x, int precision)
- { return write_vest(x,p_fname,precision); }
+ { return write_vest(x,p_fname,precision); }
int write_vest(const Matrix& x, string p_fname, int precision)
{
@@ -335,10 +335,10 @@ namespace MISCMATHS {
out.open(p_fname.c_str(), ios::out);
if(!out)
- {
- cerr << "Unable to open " << p_fname << endl;
- return -1;
- }
+ {
+ cerr << "Unable to open " << p_fname << endl;
+ return -1;
+ }
out << "! VEST-Waveform File" << endl;
out << "/NumWaves\t" << x.Ncols() << endl;
@@ -391,10 +391,10 @@ namespace MISCMATHS {
#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));
+ val = mat(x,y);
+ fs.write((char*)&val,sizeof(val));
#ifdef PPC64
- if ((n++ % 50) == 0) fs.flush();
+ if ((n++ % 50) == 0) fs.flush();
#endif
}
}
@@ -408,120 +408,120 @@ namespace MISCMATHS {
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));
- }
+ {
+ 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));
- }
+ 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);
- }
- }
+ {
+ 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;
- }
+ {
+ 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 double *a, const double *b)
- {
- Tracer tr("cross");
- ColumnVector a1(3), b1(3);
- a1 << a;
- b1 << b;
- return cross(a1,b1);
- }
+ {
+ 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());
- }
+ {
+ 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;
- }
+ {
+ 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");
+ {
+ Tracer tr("diag");
- m.ReSize(diagvals.Nrows());
- m=0.0;
- for (int j=1; j<=diagvals.Nrows(); j++)
- m(j)=diagvals(j);
- return 0;
- }
+ 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");
+ {
+ 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;
- }
+ 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;
+ {
+ 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& input) {
Tracer tr("pinv");
@@ -538,92 +538,92 @@ namespace MISCMATHS {
}
int rank(const Matrix& X)
- {
- // calculates the rank of matrix X
- Tracer tr("rank");
+ {
+ // calculates the rank of matrix X
+ Tracer tr("rank");
- DiagonalMatrix eigenvals;
- SVD(X,eigenvals);
+ DiagonalMatrix eigenvals;
+ SVD(X,eigenvals);
- double tolerance = Max(X.Nrows(),X.Ncols()) * eigenvals.Maximum() * 1e-16;
+ double tolerance = Max(X.Nrows(),X.Ncols()) * eigenvals.Maximum() * 1e-16;
- int therank=0;
+ int therank=0;
- for(int i=0; i<eigenvals.Nrows(); i++)
- if (eigenvals(i+1)>tolerance)
- therank++;
+ for(int i=0; i<eigenvals.Nrows(); i++)
+ if (eigenvals(i+1)>tolerance)
+ therank++;
- // cout << "tolerance = " << tolerance << "\n" << "eigenvalues = " << eigenvals << "\n" << "rank = " << therank << endl;
+ // cout << "tolerance = " << tolerance << "\n" << "eigenvalues = " << eigenvals << "\n" << "rank = " << therank << endl;
- return therank;
- }
+ 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;
- }
+ {
+ 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; }
@@ -640,13 +640,13 @@ namespace MISCMATHS {
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;
+ // 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;
+ // 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;
+ cerr << "ERROR:: unknown mode in get_sortidx = " << mode << endl;
}
}
return idx;
@@ -662,11 +662,11 @@ namespace MISCMATHS {
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);
+ 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);
+ res.SubMatrix(n+1,n+1,1,ncols)=vals.SubMatrix(row,row,1,ncols);
} else {
- cerr << "ERROR:: unknown mode in apply_sortidx = " << mode << endl;
+ cerr << "ERROR:: unknown mode in apply_sortidx = " << mode << endl;
}
}
return res;
@@ -679,41 +679,41 @@ namespace MISCMATHS {
// 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;
- }
- }
+ {
+ 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");
+ {
+ Tracer tr("reshape");
- Matrix r;
+ Matrix r;
- reshape(r,m,nrows,ncols);
+ reshape(r,m,nrows,ncols);
- r.Release();
- return r;
- }
+ r.Release();
+ return r;
+ }
int addrow(Matrix& m, int ncols)
{
@@ -737,195 +737,195 @@ namespace MISCMATHS {
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;
- }
+ 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);
- }
+ {
+ 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;
+ 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);
- }
+ {
+ 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;
+ 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)
+ {
+ 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;
+ 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;
+ {
+ // 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)
@@ -939,9 +939,9 @@ namespace MISCMATHS {
double x, y, z, w, d;
for (int i = 0; i < 3; i++) {
- for (int j = 0; j < 3; j++) {
- rot.m[i][j] = rotmat(i + 1, j + 1);
- }}
+ for (int j = 0; j < 3; j++) {
+ rot.m[i][j] = rotmat(i + 1, j + 1);
+ }}
legacy::nifti_mat44_to_quatern(rot, x, y, z, d, d, d, d, d, d, w);
@@ -959,212 +959,212 @@ namespace MISCMATHS {
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)
+ 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,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;
- }
+ 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,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 (*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=IdentityMatrix(4);
- 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=IdentityMatrix(4);
- 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;
+ 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
- aff = aff * skew * scale;
+ (*params2rotmat)(params,n,aff,centre);
- return 0;
+ if (n<=6) return 0;
+
+ Matrix scale=IdentityMatrix(4);
+ 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=IdentityMatrix(4);
+ 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), a1(4,4), a2(4,4);
+ float rms_deviation(const Matrix& affmat1, const Matrix& affmat2,
+ const ColumnVector& centre, const float rmax)
+ {
+ Tracer trcr("rms_deviation");
+ Matrix isodiff(4,4), a1(4,4), a2(4,4);
- if ((affmat1.Nrows()==4) && (affmat1.Ncols()==4)) { a1=affmat1; }
- else if ((affmat1.Nrows()==3) && (affmat1.Ncols()==3)) { a1=IdentityMatrix(4); a1.SubMatrix(1,3,1,3)=affmat1; }
- else { cerr << "ERROR:: Can only calculate RMS deviation for 4x4 or 3x3 matrices" << endl; exit(-5); }
+ if ((affmat1.Nrows()==4) && (affmat1.Ncols()==4)) { a1=affmat1; }
+ else if ((affmat1.Nrows()==3) && (affmat1.Ncols()==3)) { a1=IdentityMatrix(4); a1.SubMatrix(1,3,1,3)=affmat1; }
+ else { cerr << "ERROR:: Can only calculate RMS deviation for 4x4 or 3x3 matrices" << endl; exit(-5); }
- if ((affmat2.Nrows()==4) && (affmat2.Ncols()==4)) { a2=affmat2; }
- else if ((affmat2.Nrows()==3) && (affmat2.Ncols()==3)) { a2=IdentityMatrix(4); a2.SubMatrix(1,3,1,3)=affmat2; }
- else { cerr << "ERROR:: Can only calculate RMS deviation for 4x4 or 3x3 matrices" << endl; exit(-5); }
+ if ((affmat2.Nrows()==4) && (affmat2.Ncols()==4)) { a2=affmat2; }
+ else if ((affmat2.Nrows()==3) && (affmat2.Ncols()==3)) { a2=IdentityMatrix(4); a2.SubMatrix(1,3,1,3)=affmat2; }
+ else { cerr << "ERROR:: Can only calculate RMS deviation for 4x4 or 3x3 matrices" << endl; exit(-5); }
- try {
- isodiff = a1*a2.i() - IdentityMatrix(4);
- } catch(...) {
- cerr << "RMS_DEVIATION ERROR:: Could not invert matrix" << endl;
- exit(-5);
+ try {
+ isodiff = a1*a2.i() - IdentityMatrix(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;
}
- 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);
- centre = 0;
- return rms_deviation(affmat1,affmat2,centre,rmax);
-}
+ float rms_deviation(const Matrix& affmat1, const Matrix& affmat2,
+ const float rmax)
+ {
+ ColumnVector centre(3);
+ centre = 0;
+ return rms_deviation(affmat1,affmat2,centre,rmax);
+ }
// helper function - calls nifti, but with FSL default case
-void get_axis_orientations(const Matrix& sform_mat, int sform_code,
- const Matrix& qform_mat, int qform_code,
- int& icode, int& jcode, int& kcode)
-{
- Matrix vox2mm(4,4);
- if (sform_code!=NIFTI_XFORM_UNKNOWN) {
- vox2mm = sform_mat;
- } else if (qform_code!=NIFTI_XFORM_UNKNOWN) {
- vox2mm = qform_mat;
- } else {
- // ideally should be sampling_mat(), but for orientation it doesn't matter
- vox2mm = IdentityMatrix(4);
- vox2mm(1,1) = -vox2mm(1,1);
- }
- NiftiIO::mat44 v2mm;
- for (int ii=0; ii<4; ii++)
- for (int jj=0; jj<4; jj++)
- v2mm.m[ii][jj] = vox2mm(ii+1,jj+1);
- NiftiIO::legacy::nifti_mat44_to_orientation(v2mm,&icode,&jcode,&kcode);
-}
+ void get_axis_orientations(const Matrix& sform_mat, int sform_code,
+ const Matrix& qform_mat, int qform_code,
+ int& icode, int& jcode, int& kcode)
+ {
+ Matrix vox2mm(4,4);
+ if (sform_code!=NIFTI_XFORM_UNKNOWN) {
+ vox2mm = sform_mat;
+ } else if (qform_code!=NIFTI_XFORM_UNKNOWN) {
+ vox2mm = qform_mat;
+ } else {
+ // ideally should be sampling_mat(), but for orientation it doesn't matter
+ vox2mm = IdentityMatrix(4);
+ vox2mm(1,1) = -vox2mm(1,1);
+ }
+ NiftiIO::mat44 v2mm;
+ for (int ii=0; ii<4; ii++)
+ for (int jj=0; jj<4; jj++)
+ v2mm.m[ii][jj] = vox2mm(ii+1,jj+1);
+ NiftiIO::legacy::nifti_mat44_to_orientation(v2mm,&icode,&jcode,&kcode);
+ }
-// Matlab style functions for percentiles, quantiles and median
-// AUG 06 CB
+ // Matlab style functions for percentiles, quantiles and median
+ // AUG 06 CB
-ColumnVector seq(const int size)
-{
- ColumnVector outputVector(size);
- for(int i=1; i<=size; i++)
- outputVector(i) = i;
- return outputVector;
-}
+ ColumnVector seq(const int size)
+ {
+ ColumnVector outputVector(size);
+ for(int i=1; i<=size; i++)
+ outputVector(i) = i;
+ return outputVector;
+ }
-float interp1(const ColumnVector& x, const ColumnVector& y, float xi)
-// Look-up function for data table defined by x, y
-// Returns the values yi at xi using linear interpolation
-// Assumes that x is sorted in ascending order
-{
-
- float ans;
- if(xi >= x.Maximum())
- ans = y(x.Nrows());
- else
- if(xi <= x.Minimum())
- ans = y(1);
- else{
- int ind=2;
- while(xi >= x(ind)) { ind++; }
- float xa = x(ind-1), xb = x(ind), ya = y(ind-1), yb = y(ind);
- ans = ya + (xi - xa)/(xb - xa) * (yb - ya);
- }
- return ans;
-}
+ float interp1(const ColumnVector& x, const ColumnVector& y, float xi)
+ // Look-up function for data table defined by x, y
+ // Returns the values yi at xi using linear interpolation
+ // Assumes that x is sorted in ascending order
+ {
+
+ float ans;
+ if(xi >= x.Maximum())
+ ans = y(x.Nrows());
+ else
+ if(xi <= x.Minimum())
+ ans = y(1);
+ else{
+ int ind=2;
+ while(xi >= x(ind)) { ind++; }
+ float xa = x(ind-1), xb = x(ind), ya = y(ind-1), yb = y(ind);
+ ans = ya + (xi - xa)/(xb - xa) * (yb - ya);
+ }
+ return ans;
+ }
-float quantile(const ColumnVector& in, int which)
-{
- float p;
- switch (which)
+ float quantile(const ColumnVector& in, int which)
+ {
+ float p;
+ switch (which)
{
case 0 : p = 0.0; break;
case 1 : p = 25.0; break;
@@ -1174,934 +1174,934 @@ float quantile(const ColumnVector& in, int which)
default: p = 0.0;
}
- return percentile(in,p);
-}
+ return percentile(in,p);
+ }
-float percentile(const ColumnVector& in, float p)
-{
- ColumnVector y = in;
- SortAscending(y);
- int num = y.Nrows();
+ float percentile(const ColumnVector& in, float p)
+ {
+ ColumnVector y = in;
+ SortAscending(y);
+ int num = y.Nrows();
- ColumnVector xx,yy,sequence,a(1),b(1),c(1),d(1);
- sequence = 100*(seq(num)-0.5)/num; a << y(1); b << y(num); c = 0; d = 100;
- xx = (c & sequence & d);
- yy = (a & y & b);
+ ColumnVector xx,yy,sequence,a(1),b(1),c(1),d(1);
+ sequence = 100*(seq(num)-0.5)/num; a << y(1); b << y(num); c = 0; d = 100;
+ xx = (c & sequence & d);
+ yy = (a & y & b);
- return interp1(xx,yy,p);
-}
+ return interp1(xx,yy,p);
+ }
-ReturnMatrix quantile(const Matrix& in, int which)
-{
- int num = in.Ncols();
- Matrix res(1,num);
- for (int ctr=1; ctr<=num; ctr++){
- ColumnVector tmp = in.Column(ctr);
- res(1,ctr) = quantile(tmp,which);
+ ReturnMatrix quantile(const Matrix& in, int which)
+ {
+ int num = in.Ncols();
+ Matrix res(1,num);
+ for (int ctr=1; ctr<=num; ctr++){
+ ColumnVector tmp = in.Column(ctr);
+ res(1,ctr) = quantile(tmp,which);
+ }
+ res.Release();
+ return res;
}
- res.Release();
- return res;
-}
-ReturnMatrix percentile(const Matrix& in, float p)
-{
- int num = in.Ncols();
- Matrix res(1,num);
- for (int ctr=1; ctr<=num; ctr++){
- ColumnVector tmp = in.Column(ctr);
- res(1,ctr) = percentile(tmp,p);
+ ReturnMatrix percentile(const Matrix& in, float p)
+ {
+ int num = in.Ncols();
+ Matrix res(1,num);
+ for (int ctr=1; ctr<=num; ctr++){
+ ColumnVector tmp = in.Column(ctr);
+ res(1,ctr) = percentile(tmp,p);
+ }
+ res.Release();
+ return res;
}
- res.Release();
- return res;
-}
-void cart2sph(const ColumnVector& dir, float& th, float& ph)
-{
- float mag=sqrt(dir(1)*dir(1)+dir(2)*dir(2)+dir(3)*dir(3));
- if(mag==0){
- ph=M_PI/2;
- th=M_PI/2;
- }
- else{
+ void cart2sph(const ColumnVector& dir, float& th, float& ph)
+ {
+ float mag=sqrt(dir(1)*dir(1)+dir(2)*dir(2)+dir(3)*dir(3));
+ if(mag==0){
+ ph=M_PI/2;
+ th=M_PI/2;
+ }
+ else{
- if(dir(1)==0 && dir(2)>=0) ph=M_PI/2;
- else if(dir(1)==0 && dir(2)<0) ph=-M_PI/2;
- else if(dir(1)>0) ph=atan(dir(2)/dir(1));
- else if(dir(2)>0) ph=atan(dir(2)/dir(1))+M_PI;
- else ph=atan(dir(2)/dir(1))-M_PI;
+ if(dir(1)==0 && dir(2)>=0) ph=M_PI/2;
+ else if(dir(1)==0 && dir(2)<0) ph=-M_PI/2;
+ else if(dir(1)>0) ph=atan(dir(2)/dir(1));
+ else if(dir(2)>0) ph=atan(dir(2)/dir(1))+M_PI;
+ else ph=atan(dir(2)/dir(1))-M_PI;
- if(dir(3)==0) th=M_PI/2;
- else if(dir(3)>0) th=atan(sqrt(dir(1)*dir(1)+dir(2)*dir(2))/dir(3));
- else th=atan(sqrt(dir(1)*dir(1)+dir(2)*dir(2))/dir(3))+M_PI;
+ if(dir(3)==0) th=M_PI/2;
+ else if(dir(3)>0) th=atan(sqrt(dir(1)*dir(1)+dir(2)*dir(2))/dir(3));
+ else th=atan(sqrt(dir(1)*dir(1)+dir(2)*dir(2))/dir(3))+M_PI;
+ }
}
-}
-
-void cart2sph(const Matrix& dir,ColumnVector& th,ColumnVector& ph)
-{
- if(th.Nrows()!=dir.Ncols()){
- th.ReSize(dir.Ncols());
- }
- if(ph.Nrows()!=dir.Ncols()){
- ph.ReSize(dir.Ncols());
- }
+ void cart2sph(const Matrix& dir,ColumnVector& th,ColumnVector& ph)
+ {
+ if(th.Nrows()!=dir.Ncols()){
+ th.ReSize(dir.Ncols());
+ }
- for (int i=1;i<=dir.Ncols();i++) {
- float mag=sqrt(dir(1,i)*dir(1,i)+dir(2,i)*dir(2,i)+dir(3,i)*dir(3,i));
- if(mag==0){
- ph(i)=M_PI/2;
- th(i)=M_PI/2;
+ if(ph.Nrows()!=dir.Ncols()){
+ ph.ReSize(dir.Ncols());
}
- else{
- if(dir(1,i)==0 && dir(2,i)>=0) ph(i)=M_PI/2;
- else if(dir(1,i)==0 && dir(2,i)<0) ph(i)=-M_PI/2;
- else if(dir(1,i)>0) ph(i)=atan(dir(2,i)/dir(1,i));
- else if(dir(2,i)>0) ph(i)=atan(dir(2,i)/dir(1,i))+M_PI;
- else ph(i)=atan(dir(2,i)/dir(1,i))-M_PI;
- if(dir(3,i)==0) th(i)=M_PI/2;
- else if(dir(3,i)>0) th(i)=atan(sqrt(dir(1,i)*dir(1,i)+dir(2,i)*dir(2,i))/dir(3,i));
- else th(i)=atan(sqrt(dir(1,i)*dir(1,i)+dir(2,i)*dir(2,i))/dir(3,i))+M_PI;
+ for (int i=1;i<=dir.Ncols();i++) {
+ float mag=sqrt(dir(1,i)*dir(1,i)+dir(2,i)*dir(2,i)+dir(3,i)*dir(3,i));
+ if(mag==0){
+ ph(i)=M_PI/2;
+ th(i)=M_PI/2;
+ }
+ else{
+ if(dir(1,i)==0 && dir(2,i)>=0) ph(i)=M_PI/2;
+ else if(dir(1,i)==0 && dir(2,i)<0) ph(i)=-M_PI/2;
+ else if(dir(1,i)>0) ph(i)=atan(dir(2,i)/dir(1,i));
+ else if(dir(2,i)>0) ph(i)=atan(dir(2,i)/dir(1,i))+M_PI;
+ else ph(i)=atan(dir(2,i)/dir(1,i))-M_PI;
+
+ if(dir(3,i)==0) th(i)=M_PI/2;
+ else if(dir(3,i)>0) th(i)=atan(sqrt(dir(1,i)*dir(1,i)+dir(2,i)*dir(2,i))/dir(3,i));
+ else th(i)=atan(sqrt(dir(1,i)*dir(1,i)+dir(2,i)*dir(2,i))/dir(3,i))+M_PI;
+ }
}
}
-}
-
-// added by SJ
-void cart2sph(const vector<ColumnVector>& dir,ColumnVector& th,ColumnVector& ph)
-{
- if(th.Nrows()!=(int)dir.size()){
- th.ReSize(dir.size());
- }
- if(ph.Nrows()!=(int)dir.size()){
- ph.ReSize(dir.size());
- }
- //double _2pi=2*M_PI;
- double _pi2=M_PI/2;
+ // added by SJ
+ void cart2sph(const vector<ColumnVector>& dir,ColumnVector& th,ColumnVector& ph)
+ {
+ if(th.Nrows()!=(int)dir.size()){
+ th.ReSize(dir.size());
+ }
- int j=1;
- for (unsigned int i=0;i<dir.size();i++) {
- float mag=std::sqrt(dir[i](1)*dir[i](1)+dir[i](2)*dir[i](2)+dir[i](3)*dir[i](3));
- if(mag==0){
- ph(j)=_pi2;
- th(j)=_pi2;
+ if(ph.Nrows()!=(int)dir.size()){
+ ph.ReSize(dir.size());
}
- else{
- if(dir[i](1)==0 && dir[i](2)>=0) ph(j)=_pi2;
- else if(dir[i](1)==0 && dir[i](2)<0) ph(j)=-_pi2;
- else if(dir[i](1)>0) ph(j)=std::atan(dir[i](2)/dir[i](1));
- else if(dir[i](2)>0) ph(j)=std::atan(dir[i](2)/dir[i](1))+M_PI;
- else ph(j)=std::atan(dir[i](2)/dir[i](1))-M_PI;
+ //double _2pi=2*M_PI;
+ double _pi2=M_PI/2;
+
+ int j=1;
+ for (unsigned int i=0;i<dir.size();i++) {
+ float mag=std::sqrt(dir[i](1)*dir[i](1)+dir[i](2)*dir[i](2)+dir[i](3)*dir[i](3));
+ if(mag==0){
+ ph(j)=_pi2;
+ th(j)=_pi2;
+ }
+ else{
+ if(dir[i](1)==0 && dir[i](2)>=0) ph(j)=_pi2;
+ else if(dir[i](1)==0 && dir[i](2)<0) ph(j)=-_pi2;
+ else if(dir[i](1)>0) ph(j)=std::atan(dir[i](2)/dir[i](1));
+ else if(dir[i](2)>0) ph(j)=std::atan(dir[i](2)/dir[i](1))+M_PI;
+ else ph(j)=std::atan(dir[i](2)/dir[i](1))-M_PI;
- //ph(j)=fmod(ph(j),_2pi);
+ //ph(j)=fmod(ph(j),_2pi);
- if(dir[i](3)==0) th(j)=_pi2;
- else if(dir[i](3)>0) th(j)=std::atan(std::sqrt(dir[i](1)*dir[i](1)+dir[i](2)*dir[i](2))/dir[i](3));
- else th(j)=std::atan(std::sqrt(dir[i](1)*dir[i](1)+dir[i](2)*dir[i](2))/dir[i](3))+M_PI;
+ if(dir[i](3)==0) th(j)=_pi2;
+ else if(dir[i](3)>0) th(j)=std::atan(std::sqrt(dir[i](1)*dir[i](1)+dir[i](2)*dir[i](2))/dir[i](3));
+ else th(j)=std::atan(std::sqrt(dir[i](1)*dir[i](1)+dir[i](2)*dir[i](2))/dir[i](3))+M_PI;
- //th(j)=fmod(th(j),M_PI);
+ //th(j)=fmod(th(j),M_PI);
+ }
+ j++;
}
- j++;
}
-}
-// Added by CFB --- Matlab style Matrix functions
+ // Added by CFB --- Matlab style Matrix functions
-ReturnMatrix ones(const int dim1, const int dim2)
-{
- int tdim = dim2;
- if(tdim<0){tdim=dim1;}
- Matrix res(dim1,tdim); res = 1.0;
- res.Release();
- return res;
-}
+ ReturnMatrix ones(const int dim1, const int dim2)
+ {
+ int tdim = dim2;
+ if(tdim<0){tdim=dim1;}
+ Matrix res(dim1,tdim); res = 1.0;
+ res.Release();
+ return res;
+ }
-ReturnMatrix zeros(const int dim1, const int dim2)
-{
- int tdim = dim2;
- if(tdim<0){tdim=dim1;}
- Matrix res(dim1,tdim); res = 0.0;
- res.Release();
- return res;
-}
+ ReturnMatrix zeros(const int dim1, const int dim2)
+ {
+ int tdim = dim2;
+ if(tdim<0){tdim=dim1;}
+ Matrix res(dim1,tdim); res = 0.0;
+ res.Release();
+ return res;
+ }
-ReturnMatrix repmat(const Matrix &mat, const int rows, const int cols)
-{
- Matrix res = mat;
- for(int ctr = 1; ctr < cols; ctr++){res |= mat;}
- Matrix tmpres = res;
- for(int ctr = 1; ctr < rows; ctr++){res &= tmpres;}
- res.Release();
- return res;
-}
+ ReturnMatrix repmat(const Matrix &mat, const int rows, const int cols)
+ {
+ Matrix res = mat;
+ for(int ctr = 1; ctr < cols; ctr++){res |= mat;}
+ Matrix tmpres = res;
+ for(int ctr = 1; ctr < rows; ctr++){res &= tmpres;}
+ res.Release();
+ return res;
+ }
-ReturnMatrix dist2(const Matrix &mat1, const Matrix &mat2)
-{
- Matrix res(mat1.Ncols(),mat2.Ncols());
- for(int ctr1 = 1; ctr1 <= mat1.Ncols(); ctr1++)
- for(int ctr2 =1; ctr2 <= mat2.Ncols(); ctr2++)
+ ReturnMatrix dist2(const Matrix &mat1, const Matrix &mat2)
+ {
+ Matrix res(mat1.Ncols(),mat2.Ncols());
+ for(int ctr1 = 1; ctr1 <= mat1.Ncols(); ctr1++)
+ for(int ctr2 =1; ctr2 <= mat2.Ncols(); ctr2++)
{
- ColumnVector tmp;
- tmp=mat1.Column(ctr1)-mat2.Column(ctr2);
- res(ctr1,ctr2) = std::sqrt(tmp.SumSquare());
+ ColumnVector tmp;
+ tmp=mat1.Column(ctr1)-mat2.Column(ctr2);
+ res(ctr1,ctr2) = std::sqrt(tmp.SumSquare());
}
- res.Release();
- return res;
-}
+ res.Release();
+ return res;
+ }
-ReturnMatrix abs(const Matrix& mat)
-{
- Matrix res = mat;
- for (int mc=1; mc<=mat.Ncols(); mc++) {
- for (int mr=1; mr<=mat.Nrows(); mr++) {
- res(mr,mc)=std::abs(res(mr,mc));
+ ReturnMatrix abs(const Matrix& mat)
+ {
+ Matrix res = mat;
+ for (int mc=1; mc<=mat.Ncols(); mc++) {
+ for (int mr=1; mr<=mat.Nrows(); mr++) {
+ res(mr,mc)=std::abs(res(mr,mc));
+ }
}
+ res.Release();
+ return res;
}
- res.Release();
- return res;
-}
-void abs_econ(Matrix& mat)
-{
+ void abs_econ(Matrix& mat)
+ {
for (int mc=1; mc<=mat.Ncols(); mc++) {
- for (int mr=1; mr<=mat.Nrows(); mr++) {
- mat(mr,mc)=std::abs(mat(mr,mc));
- }
+ for (int mr=1; mr<=mat.Nrows(); mr++) {
+ mat(mr,mc)=std::abs(mat(mr,mc));
+ }
}
-}
+ }
-ReturnMatrix sqrt(const Matrix& mat)
-{
- Matrix res = mat;
- bool neg_flag = false;
- for (int mc=1; mc<=mat.Ncols(); mc++) {
- for (int mr=1; mr<=mat.Nrows(); mr++) {
- if(res(mr,mc)<0){ neg_flag = true; }
- res(mr,mc)=std::sqrt(std::abs(res(mr,mc)));
+ ReturnMatrix sqrt(const Matrix& mat)
+ {
+ Matrix res = mat;
+ bool neg_flag = false;
+ for (int mc=1; mc<=mat.Ncols(); mc++) {
+ for (int mr=1; mr<=mat.Nrows(); mr++) {
+ if(res(mr,mc)<0){ neg_flag = true; }
+ res(mr,mc)=std::sqrt(std::abs(res(mr,mc)));
+ }
}
+ if(neg_flag){
+ //cerr << " Matrix contained negative elements" << endl;
+ //cerr << " return sqrt(abs(X)) instead" << endl;
+ }
+ res.Release();
+ return res;
}
- if(neg_flag){
- //cerr << " Matrix contained negative elements" << endl;
- //cerr << " return sqrt(abs(X)) instead" << endl;
- }
- res.Release();
- return res;
-}
-void sqrt_econ(Matrix& mat)
-{
- bool neg_flag = false;
- for (int mc=1; mc<=mat.Ncols(); mc++) {
- for (int mr=1; mr<=mat.Nrows(); mr++) {
- if(mat(mr,mc)<0){ neg_flag = true; }
- mat(mr,mc)=std::sqrt(std::abs(mat(mr,mc)));
+ void sqrt_econ(Matrix& mat)
+ {
+ bool neg_flag = false;
+ for (int mc=1; mc<=mat.Ncols(); mc++) {
+ for (int mr=1; mr<=mat.Nrows(); mr++) {
+ if(mat(mr,mc)<0){ neg_flag = true; }
+ mat(mr,mc)=std::sqrt(std::abs(mat(mr,mc)));
+ }
+ }
+ if(neg_flag){
+ //cerr << " Matrix contained negative elements" << endl;
+ //cerr << " return sqrt(abs(X)) instead" << endl;
}
}
- if(neg_flag){
- //cerr << " Matrix contained negative elements" << endl;
- //cerr << " return sqrt(abs(X)) instead" << endl;
- }
-}
-ReturnMatrix sqrtm(const Matrix& mat)
-{
+ ReturnMatrix sqrtm(const Matrix& mat)
+ {
Matrix res, tmpU, tmpV;
DiagonalMatrix tmpD;
SVD(mat, tmpD, tmpU, tmpV);
res = tmpU*sqrt(tmpD)*tmpV.t();
res.Release();
return res;
-}
-
-ReturnMatrix log(const Matrix& mat)
-{
- Matrix res = mat;
- bool neg_flag = false;
- for (int mc=1; mc<=mat.Ncols(); mc++) {
- for (int mr=1; mr<=mat.Nrows(); mr++) {
- if(res(mr,mc)<0){ neg_flag = true; }
- res(mr,mc)=std::log(std::abs(res(mr,mc)));
- }
}
- if(neg_flag){
- // cerr << " Matrix contained negative elements" << endl;
- // cerr << " return log(abs(X)) instead" << endl;
- }
- res.Release();
- return res;
-}
-void log_econ(Matrix& mat)
-{
- bool neg_flag = false;
- for (int mc=1; mc<=mat.Ncols(); mc++) {
- for (int mr=1; mr<=mat.Nrows(); mr++) {
- if(mat(mr,mc)<0){ neg_flag = true; }
- mat(mr,mc)=std::log(std::abs(mat(mr,mc)));
+ ReturnMatrix log(const Matrix& mat)
+ {
+ Matrix res = mat;
+ bool neg_flag = false;
+ for (int mc=1; mc<=mat.Ncols(); mc++) {
+ for (int mr=1; mr<=mat.Nrows(); mr++) {
+ if(res(mr,mc)<0){ neg_flag = true; }
+ res(mr,mc)=std::log(std::abs(res(mr,mc)));
+ }
+ }
+ if(neg_flag){
+ // cerr << " Matrix contained negative elements" << endl;
+ // cerr << " return log(abs(X)) instead" << endl;
}
+ res.Release();
+ return res;
}
- if(neg_flag){
- // cerr << " Matrix contained negative elements" << endl;
- // cerr << " return log(abs(X)) instead" << endl;
+
+ void log_econ(Matrix& mat)
+ {
+ bool neg_flag = false;
+ for (int mc=1; mc<=mat.Ncols(); mc++) {
+ for (int mr=1; mr<=mat.Nrows(); mr++) {
+ if(mat(mr,mc)<0){ neg_flag = true; }
+ mat(mr,mc)=std::log(std::abs(mat(mr,mc)));
+ }
+ }
+ if(neg_flag){
+ // cerr << " Matrix contained negative elements" << endl;
+ // cerr << " return log(abs(X)) instead" << endl;
+ }
}
-}
-ReturnMatrix exp(const Matrix& mat)
-{
- Matrix res = mat;
- for (int mc=1; mc<=mat.Ncols(); mc++) {
- for (int mr=1; mr<=mat.Nrows(); mr++) {
- res(mr,mc)=std::exp(res(mr,mc));
+ ReturnMatrix exp(const Matrix& mat)
+ {
+ Matrix res = mat;
+ for (int mc=1; mc<=mat.Ncols(); mc++) {
+ for (int mr=1; mr<=mat.Nrows(); mr++) {
+ res(mr,mc)=std::exp(res(mr,mc));
+ }
}
+ res.Release();
+ return res;
}
- res.Release();
- return res;
-}
-void exp_econ(Matrix& mat)
-{
- for (int mc=1; mc<=mat.Ncols(); mc++) {
- for (int mr=1; mr<=mat.Nrows(); mr++) {
- mat(mr,mc)=std::exp(mat(mr,mc));
+ void exp_econ(Matrix& mat)
+ {
+ for (int mc=1; mc<=mat.Ncols(); mc++) {
+ for (int mr=1; mr<=mat.Nrows(); mr++) {
+ mat(mr,mc)=std::exp(mat(mr,mc));
+ }
}
}
-}
// optimised code for calculating matrix exponential
-ReturnMatrix expm(const Matrix& mat){
- float nmat = sum(mat).Maximum();
- int nc=mat.Ncols(),nr=mat.Nrows();
- Matrix res(nr,nc);
- IdentityMatrix id(nr);
- Matrix U(nr,nc),V(nr,nc);
-
- if(nmat <= 1.495585217958292e-002){ // m=3
- Matrix mat2(nr,nc);
- mat2=mat*mat;
- U = mat*(mat2+60.0*id);
- V = 12.0*mat2+120.0*id;
- res = (-U+V).i()*(U+V);
- }
- else if(nmat <= 2.539398330063230e-001){ // m=5
- Matrix mat2(nr,nc),mat4(nr,nc);
- mat2=mat*mat;mat4=mat2*mat2;
- U = mat*(mat4+420.0*mat2+15120.0*id);
- V = 30.0*mat4+3360.0*mat2+30240.0*id;
- res = (-U+V).i()*(U+V);
- }
- else if(nmat <= 9.504178996162932e-001){ // m=7
- Matrix mat2(nr,nc),mat4(nr,nc),mat6(nr,nc);
- mat2=mat*mat;mat4=mat2*mat2,mat6=mat4*mat2;
- U = mat*(mat6+1512.0*mat4+277200.0*mat2+8648640.0*id);
- V = 56.0*mat6+25200.0*mat4+1995840.0*mat2+17297280.0*id;
- res = (-U+V).i()*(U+V);
- }
- else if(nmat <= 2.097847961257068e+000){
- Matrix mat2(nr,nc),mat4(nr,nc),mat6(nr,nc),mat8(nr,nc);
- mat2=mat*mat;mat4=mat2*mat2,mat6=mat4*mat2,mat8=mat6*mat2;
- U = mat*(mat8+3960.0*mat6+2162160.0*mat4+302702400.0*mat2+8821612800.0*id);
- V = 90.0*mat8+110880.0*mat6+30270240.0*mat4+2075673600.0*mat2+17643225600.0*id;
- res = (-U+V).i()*(U+V);
- }
- else if(nmat <= 5.371920351148152e+000){
- Matrix mat2(nr,nc),mat4(nr,nc),mat6(nr,nc);
- mat2=mat*mat;mat4=mat2*mat2,mat6=mat4*mat2;
- U = mat*(mat6*(mat6+16380.0*mat4+40840800.0*mat2)+
- +33522128640.0*mat6+10559470521600.0*mat4+1187353796428800.0*mat2+32382376266240000.0*id);
- V = mat6*(182.0*mat6+960960.0*mat4+1323241920.0*mat2)
- + 670442572800.0*mat6+129060195264000.0*mat4+7771770303897600.0*mat2+64764752532480000.0*id;
- res = (-U+V).i()*(U+V);
- }
- else{
- double t;int s;
- t = frexp(nmat/5.371920351148152,&s);
- if(t==0.5) s--;
- t = std::pow(2.0,s);
- res = (mat/t);
- Matrix mat2(nr,nc),mat4(nr,nc),mat6(nr,nc);
- mat2=res*res;mat4=mat2*mat2,mat6=mat4*mat2;
- U = res*(mat6*(mat6+16380*mat4+40840800*mat2)+
- +33522128640.0*mat6+10559470521600.0*mat4+1187353796428800.0*mat2+32382376266240000.0*id);
- V = mat6*(182.0*mat6+960960.0*mat4+1323241920.0*mat2)
- + 670442572800.0*mat6+129060195264000.0*mat4+7771770303897600.0*mat2+64764752532480000.0*id;
- res = (-U+V).i()*(U+V);
- for(int i=1;i<=s;i++)
- res = res*res;
- }
-
- res.Release();
- return res;
-}
+ ReturnMatrix expm(const Matrix& mat){
+ float nmat = sum(mat).Maximum();
+ int nc=mat.Ncols(),nr=mat.Nrows();
+ Matrix res(nr,nc);
+ IdentityMatrix id(nr);
+ Matrix U(nr,nc),V(nr,nc);
+
+ if(nmat <= 1.495585217958292e-002){ // m=3
+ Matrix mat2(nr,nc);
+ mat2=mat*mat;
+ U = mat*(mat2+60.0*id);
+ V = 12.0*mat2+120.0*id;
+ res = (-U+V).i()*(U+V);
+ }
+ else if(nmat <= 2.539398330063230e-001){ // m=5
+ Matrix mat2(nr,nc),mat4(nr,nc);
+ mat2=mat*mat;mat4=mat2*mat2;
+ U = mat*(mat4+420.0*mat2+15120.0*id);
+ V = 30.0*mat4+3360.0*mat2+30240.0*id;
+ res = (-U+V).i()*(U+V);
+ }
+ else if(nmat <= 9.504178996162932e-001){ // m=7
+ Matrix mat2(nr,nc),mat4(nr,nc),mat6(nr,nc);
+ mat2=mat*mat;mat4=mat2*mat2,mat6=mat4*mat2;
+ U = mat*(mat6+1512.0*mat4+277200.0*mat2+8648640.0*id);
+ V = 56.0*mat6+25200.0*mat4+1995840.0*mat2+17297280.0*id;
+ res = (-U+V).i()*(U+V);
+ }
+ else if(nmat <= 2.097847961257068e+000){
+ Matrix mat2(nr,nc),mat4(nr,nc),mat6(nr,nc),mat8(nr,nc);
+ mat2=mat*mat;mat4=mat2*mat2,mat6=mat4*mat2,mat8=mat6*mat2;
+ U = mat*(mat8+3960.0*mat6+2162160.0*mat4+302702400.0*mat2+8821612800.0*id);
+ V = 90.0*mat8+110880.0*mat6+30270240.0*mat4+2075673600.0*mat2+17643225600.0*id;
+ res = (-U+V).i()*(U+V);
+ }
+ else if(nmat <= 5.371920351148152e+000){
+ Matrix mat2(nr,nc),mat4(nr,nc),mat6(nr,nc);
+ mat2=mat*mat;mat4=mat2*mat2,mat6=mat4*mat2;
+ U = mat*(mat6*(mat6+16380.0*mat4+40840800.0*mat2)+
+ +33522128640.0*mat6+10559470521600.0*mat4+1187353796428800.0*mat2+32382376266240000.0*id);
+ V = mat6*(182.0*mat6+960960.0*mat4+1323241920.0*mat2)
+ + 670442572800.0*mat6+129060195264000.0*mat4+7771770303897600.0*mat2+64764752532480000.0*id;
+ res = (-U+V).i()*(U+V);
+ }
+ else{
+ double t;int s;
+ t = frexp(nmat/5.371920351148152,&s);
+ if(t==0.5) s--;
+ t = std::pow(2.0,s);
+ res = (mat/t);
+ Matrix mat2(nr,nc),mat4(nr,nc),mat6(nr,nc);
+ mat2=res*res;mat4=mat2*mat2,mat6=mat4*mat2;
+ U = res*(mat6*(mat6+16380*mat4+40840800*mat2)+
+ +33522128640.0*mat6+10559470521600.0*mat4+1187353796428800.0*mat2+32382376266240000.0*id);
+ V = mat6*(182.0*mat6+960960.0*mat4+1323241920.0*mat2)
+ + 670442572800.0*mat6+129060195264000.0*mat4+7771770303897600.0*mat2+64764752532480000.0*id;
+ res = (-U+V).i()*(U+V);
+ for(int i=1;i<=s;i++)
+ res = res*res;
+ }
+
+ res.Release();
+ return res;
+ }
-ReturnMatrix tanh(const Matrix& mat)
-{
- Matrix res = mat;
- for (int mc=1; mc<=mat.Ncols(); mc++) {
- for (int mr=1; mr<=mat.Nrows(); mr++) {
- res(mr,mc)=std::tanh(res(mr,mc));
+ ReturnMatrix tanh(const Matrix& mat)
+ {
+ Matrix res = mat;
+ for (int mc=1; mc<=mat.Ncols(); mc++) {
+ for (int mr=1; mr<=mat.Nrows(); mr++) {
+ res(mr,mc)=std::tanh(res(mr,mc));
+ }
}
+ res.Release();
+ return res;
}
- res.Release();
- return res;
-}
-void tanh_econ(Matrix& mat)
-{
- for (int mc=1; mc<=mat.Ncols(); mc++) {
- for (int mr=1; mr<=mat.Nrows(); mr++) {
- mat(mr,mc)=std::tanh(mat(mr,mc));
+ void tanh_econ(Matrix& mat)
+ {
+ for (int mc=1; mc<=mat.Ncols(); mc++) {
+ for (int mr=1; mr<=mat.Nrows(); mr++) {
+ mat(mr,mc)=std::tanh(mat(mr,mc));
+ }
}
}
-}
-ReturnMatrix pow(const Matrix& mat, const double exp)
-{
- Matrix res = mat;
- for (int mc=1; mc<=mat.Ncols(); mc++) {
- for (int mr=1; mr<=mat.Nrows(); mr++) {
- res(mr,mc)=std::pow(res(mr,mc),exp);
+ ReturnMatrix pow(const Matrix& mat, const double exp)
+ {
+ Matrix res = mat;
+ for (int mc=1; mc<=mat.Ncols(); mc++) {
+ for (int mr=1; mr<=mat.Nrows(); mr++) {
+ res(mr,mc)=std::pow(res(mr,mc),exp);
+ }
}
+ res.Release();
+ return res;
}
- res.Release();
- return res;
-}
-void pow_econ(Matrix& mat, const double exp)
-{
- for (int mc=1; mc<=mat.Ncols(); mc++) {
- for (int mr=1; mr<=mat.Nrows(); mr++) {
- mat(mr,mc)=std::pow(mat(mr,mc),exp);
+ void pow_econ(Matrix& mat, const double exp)
+ {
+ for (int mc=1; mc<=mat.Ncols(); mc++) {
+ for (int mr=1; mr<=mat.Nrows(); mr++) {
+ mat(mr,mc)=std::pow(mat(mr,mc),exp);
+ }
}
}
-}
-ReturnMatrix max(const Matrix& mat)
-{
- Matrix res;
- if(mat.Nrows()>1){
- res=zeros(1,mat.Ncols());
- res=mat.Row(1);
- for(int mc=1; mc<=mat.Ncols();mc++){
- for(int mr=2; mr<=mat.Nrows();mr++){
- if(mat(mr,mc)>res(1,mc)){res(1,mc)=mat(mr,mc);}
+ ReturnMatrix max(const Matrix& mat)
+ {
+ Matrix res;
+ if(mat.Nrows()>1){
+ res=zeros(1,mat.Ncols());
+ res=mat.Row(1);
+ for(int mc=1; mc<=mat.Ncols();mc++){
+ for(int mr=2; mr<=mat.Nrows();mr++){
+ if(mat(mr,mc)>res(1,mc)){res(1,mc)=mat(mr,mc);}
+ }
}
}
- }
- else{
- res=zeros(1);
- res=mat(1,1);
- for(int mc=2; mc<=mat.Ncols(); mc++){
- if(mat(1,mc)>res(1,1)){res(1,1)=mat(1,mc);}
+ else{
+ res=zeros(1);
+ res=mat(1,1);
+ for(int mc=2; mc<=mat.Ncols(); mc++){
+ if(mat(1,mc)>res(1,1)){res(1,1)=mat(1,mc);}
+ }
}
+ res.Release();
+ return res;
}
- res.Release();
- return res;
-}
-ReturnMatrix max(const Matrix& mat,ColumnVector& index)
-{
- index.ReSize(mat.Nrows());
- index=1;
- Matrix res;
- if(mat.Nrows()>1){
- res=zeros(1,mat.Ncols());
- res=mat.Row(1);
- for(int mc=1; mc<=mat.Ncols();mc++){
- for(int mr=2; mr<=mat.Nrows();mr++){
- if(mat(mr,mc)>res(1,mc))
- {
- res(1,mc)=mat(mr,mc);
- index(mr)=mc;
- }
+ ReturnMatrix max(const Matrix& mat,ColumnVector& index)
+ {
+ index.ReSize(mat.Nrows());
+ index=1;
+ Matrix res;
+ if(mat.Nrows()>1){
+ res=zeros(1,mat.Ncols());
+ res=mat.Row(1);
+ for(int mc=1; mc<=mat.Ncols();mc++){
+ for(int mr=2; mr<=mat.Nrows();mr++){
+ if(mat(mr,mc)>res(1,mc))
+ {
+ res(1,mc)=mat(mr,mc);
+ index(mr)=mc;
+ }
+ }
}
}
- }
- else{
- res=zeros(1);
- res=mat(1,1);
- for(int mc=2; mc<=mat.Ncols(); mc++){
- if(mat(1,mc)>res(1,1))
- {
- res(1,1)=mat(1,mc);
- index(1)=mc;
- }
+ else{
+ res=zeros(1);
+ res=mat(1,1);
+ for(int mc=2; mc<=mat.Ncols(); mc++){
+ if(mat(1,mc)>res(1,1))
+ {
+ res(1,1)=mat(1,mc);
+ index(1)=mc;
+ }
+ }
}
+ res.Release();
+ return res;
}
- res.Release();
- return res;
-}
-ReturnMatrix min(const Matrix& mat)
-{
- Matrix res;
- if(mat.Nrows()>1){
- res=zeros(1,mat.Ncols());
- res=mat.Row(1);
- for(int mc=1; mc<=mat.Ncols();mc++){
- for(int mr=2; mr<=mat.Nrows();mr++){
- if(mat(mr,mc)<res(1,mc)){res(1,mc)=mat(mr,mc);}
+ ReturnMatrix min(const Matrix& mat)
+ {
+ Matrix res;
+ if(mat.Nrows()>1){
+ res=zeros(1,mat.Ncols());
+ res=mat.Row(1);
+ for(int mc=1; mc<=mat.Ncols();mc++){
+ for(int mr=2; mr<=mat.Nrows();mr++){
+ if(mat(mr,mc)<res(1,mc)){res(1,mc)=mat(mr,mc);}
+ }
}
}
- }
- else{
- res=zeros(1);
- res=mat(1,1);
- for(int mc=2; mc<=mat.Ncols(); mc++){
- if(mat(1,mc)<res(1,1)){res(1,1)=mat(1,mc);}
+ else{
+ res=zeros(1);
+ res=mat(1,1);
+ for(int mc=2; mc<=mat.Ncols(); mc++){
+ if(mat(1,mc)<res(1,1)){res(1,1)=mat(1,mc);}
+ }
}
+ res.Release();
+ return res;
}
- res.Release();
- return res;
-}
-ReturnMatrix sum(const Matrix& mat, const int dim)
-{
- Matrix res;
-
- if (dim == 1){
- res = zeros(1,mat.Ncols());
- for (int mc=1; mc<=mat.Ncols(); mc++) {
- for (int mr=1; mr<=mat.Nrows(); mr++) {
- res(1,mc) += mat(mr,mc);
- }
- }
+ ReturnMatrix sum(const Matrix& mat, const int dim)
+ {
+ Matrix res;
+
+ if (dim == 1){
+ res = zeros(1,mat.Ncols());
+ for (int mc=1; mc<=mat.Ncols(); mc++) {
+ for (int mr=1; mr<=mat.Nrows(); mr++) {
+ res(1,mc) += mat(mr,mc);
+ }
+ }
}
else{
- res = zeros(mat.Nrows(),1);
- for (int mr=1; mr<=mat.Nrows(); mr++) {
- for (int mc=1; mc<=mat.Ncols(); mc++) {
- res(mr,1) += mat(mr,mc);
- }
- }
+ res = zeros(mat.Nrows(),1);
+ for (int mr=1; mr<=mat.Nrows(); mr++) {
+ for (int mc=1; mc<=mat.Ncols(); mc++) {
+ res(mr,1) += mat(mr,mc);
+ }
+ }
}
- res.Release();
- return res;
-}
+ res.Release();
+ return res;
+ }
-ReturnMatrix mean(const Matrix& mat, const RowVector& weights, const int dim) //weights are considered to be in the "direction" of dim and normalised to sum 1
-{
+ ReturnMatrix mean(const Matrix& mat, const RowVector& weights, const int dim) //weights are considered to be in the "direction" of dim and normalised to sum 1
+ {
Matrix res;
if (dim == 1){
- res = zeros(1,mat.Ncols());
- for (int mc=1; mc<=mat.Ncols(); mc++) {
- for (int mr=1; mr<=mat.Nrows(); mr++) {
- res(1,mc) += weights(mr)*mat(mr,mc);
- }
- }
+ res = zeros(1,mat.Ncols());
+ for (int mc=1; mc<=mat.Ncols(); mc++) {
+ for (int mr=1; mr<=mat.Nrows(); mr++) {
+ res(1,mc) += weights(mr)*mat(mr,mc);
+ }
+ }
}
else{
- res = zeros(mat.Nrows(),1);
- for (int mr=1; mr<=mat.Nrows(); mr++) {
- for (int mc=1; mc<=mat.Ncols(); mc++) {
- res(mr,1) += weights(mc)*mat(mr,mc);
- }
- }
+ res = zeros(mat.Nrows(),1);
+ for (int mr=1; mr<=mat.Nrows(); mr++) {
+ for (int mc=1; mc<=mat.Ncols(); mc++) {
+ res(mr,1) += weights(mc)*mat(mr,mc);
+ }
+ }
}
res.Release();
return res;
-}
+ }
-ReturnMatrix mean(const Matrix& mat, const int dim)
-{
+ ReturnMatrix mean(const Matrix& mat, const int dim)
+ {
Matrix res;
int N;
if (dim == 1){
- res = zeros(1,mat.Ncols());
- N = mat.Nrows();
- for (int mc=1; mc<=mat.Ncols(); mc++) {
- for (int mr=1; mr<=mat.Nrows(); mr++) {
- res(1,mc) += mat(mr,mc)/N;
- }
- }
+ res = zeros(1,mat.Ncols());
+ N = mat.Nrows();
+ for (int mc=1; mc<=mat.Ncols(); mc++) {
+ for (int mr=1; mr<=mat.Nrows(); mr++) {
+ res(1,mc) += mat(mr,mc)/N;
+ }
+ }
}
else{
- res = zeros(mat.Nrows(),1);
- N = mat.Ncols();
- for (int mr=1; mr<=mat.Nrows(); mr++) {
- for (int mc=1; mc<=mat.Ncols(); mc++) {
- res(mr,1) += mat(mr,mc)/N;
- }
- }
+ res = zeros(mat.Nrows(),1);
+ N = mat.Ncols();
+ for (int mr=1; mr<=mat.Nrows(); mr++) {
+ for (int mc=1; mc<=mat.Ncols(); mc++) {
+ res(mr,1) += mat(mr,mc)/N;
+ }
+ }
}
res.Release();
return res;
-}
+ }
-ReturnMatrix var(const Matrix& mat, const int dim)
-{
+ ReturnMatrix var(const Matrix& mat, const int dim)
+ {
Matrix res, matmean;
matmean = mean(mat,dim);
int N;
if (dim == 1){
- res = zeros(1,mat.Ncols());
- N = mat.Nrows();
- if(N>1){
- for (int mc=1; mc<=mat.Ncols(); mc++) {
- for (int mr=1; mr<=mat.Nrows(); mr++) {
- res(1,mc) += (mat(mr,mc) - matmean(1,mc)) * (mat(mr,mc) - matmean(1,mc))/(N-1);
- }
- }
- }
+ res = zeros(1,mat.Ncols());
+ N = mat.Nrows();
+ if(N>1){
+ for (int mc=1; mc<=mat.Ncols(); mc++) {
+ for (int mr=1; mr<=mat.Nrows(); mr++) {
+ res(1,mc) += (mat(mr,mc) - matmean(1,mc)) * (mat(mr,mc) - matmean(1,mc))/(N-1);
+ }
+ }
+ }
}
else{
- res = zeros(mat.Nrows(),1);
- N = mat.Ncols();
- if(N>1){
- for (int mr=1; mr<=mat.Nrows(); mr++) {
- for (int mc=1; mc<=mat.Ncols(); mc++) {
- res(mr,1) += (mat(mr,mc) -matmean(mr,1))* (mat(mr,mc)-matmean(mr,1))/(N-1);
- }
- }
- }
+ res = zeros(mat.Nrows(),1);
+ N = mat.Ncols();
+ if(N>1){
+ for (int mr=1; mr<=mat.Nrows(); mr++) {
+ for (int mc=1; mc<=mat.Ncols(); mc++) {
+ res(mr,1) += (mat(mr,mc) -matmean(mr,1))* (mat(mr,mc)-matmean(mr,1))/(N-1);
+ }
+ }
+ }
}
- res.Release();
- return res;
-}
+ res.Release();
+ return res;
+ }
-ReturnMatrix stdev(const Matrix& mat, const int dim)
-{
- return sqrt(var(mat,dim));
-}
+ ReturnMatrix stdev(const Matrix& mat, const int dim)
+ {
+ return sqrt(var(mat,dim));
+ }
+
+ ReturnMatrix gt(const Matrix& mat1,const Matrix& mat2)
+ {
+ int ctrcol = std::min(mat1.Ncols(),mat2.Ncols());
+ int ctrrow = std::min(mat1.Nrows(),mat2.Nrows());
+ Matrix res(ctrrow,ctrcol);
+ res=0.0;
-ReturnMatrix gt(const Matrix& mat1,const Matrix& mat2)
-{
- int ctrcol = std::min(mat1.Ncols(),mat2.Ncols());
- int ctrrow = std::min(mat1.Nrows(),mat2.Nrows());
- Matrix res(ctrrow,ctrcol);
- res=0.0;
-
- for (int ctr1 = 1; ctr1 <= ctrrow; ctr1++) {
- for (int ctr2 =1; ctr2 <= ctrcol; ctr2++) {
- if( mat1(ctr1,ctr2) > mat2(ctr1,ctr2)){
- res(ctr1,ctr2) = 1.0;
+ for (int ctr1 = 1; ctr1 <= ctrrow; ctr1++) {
+ for (int ctr2 =1; ctr2 <= ctrcol; ctr2++) {
+ if( mat1(ctr1,ctr2) > mat2(ctr1,ctr2)){
+ res(ctr1,ctr2) = 1.0;
+ }
}
}
- }
- res.Release();
- return res;
-}
+ res.Release();
+ return res;
+ }
-ReturnMatrix lt(const Matrix& mat1,const Matrix& mat2)
-{
- int ctrcol = std::min(mat1.Ncols(),mat2.Ncols());
- int ctrrow = std::min(mat1.Nrows(),mat2.Nrows());
- Matrix res(ctrrow,ctrcol);
- res=0.0;
+ ReturnMatrix lt(const Matrix& mat1,const Matrix& mat2)
+ {
+ int ctrcol = std::min(mat1.Ncols(),mat2.Ncols());
+ int ctrrow = std::min(mat1.Nrows(),mat2.Nrows());
+ Matrix res(ctrrow,ctrcol);
+ res=0.0;
- for (int ctr1 = 1; ctr1 <= ctrrow; ctr1++) {
- for (int ctr2 =1; ctr2 <= ctrcol; ctr2++) {
- if( mat1(ctr1,ctr2) < mat2(ctr1,ctr2)){
- res(ctr1,ctr2) = 1.0;
+ for (int ctr1 = 1; ctr1 <= ctrrow; ctr1++) {
+ for (int ctr2 =1; ctr2 <= ctrcol; ctr2++) {
+ if( mat1(ctr1,ctr2) < mat2(ctr1,ctr2)){
+ res(ctr1,ctr2) = 1.0;
+ }
}
}
- }
- res.Release();
- return res;
-}
+ res.Release();
+ return res;
+ }
-ReturnMatrix geqt(const Matrix& mat1,const Matrix& mat2)
-{
- int ctrcol = std::min(mat1.Ncols(),mat2.Ncols());
- int ctrrow = std::min(mat1.Nrows(),mat2.Nrows());
- Matrix res(ctrrow,ctrcol);
- res=0.0;
+ ReturnMatrix geqt(const Matrix& mat1,const Matrix& mat2)
+ {
+ int ctrcol = std::min(mat1.Ncols(),mat2.Ncols());
+ int ctrrow = std::min(mat1.Nrows(),mat2.Nrows());
+ Matrix res(ctrrow,ctrcol);
+ res=0.0;
- for (int ctr1 = 1; ctr1 <= ctrrow; ctr1++) {
- for (int ctr2 =1; ctr2 <= ctrcol; ctr2++) {
- if( mat1(ctr1,ctr2) >= mat2(ctr1,ctr2)){
- res(ctr1,ctr2) = 1.0;
+ for (int ctr1 = 1; ctr1 <= ctrrow; ctr1++) {
+ for (int ctr2 =1; ctr2 <= ctrcol; ctr2++) {
+ if( mat1(ctr1,ctr2) >= mat2(ctr1,ctr2)){
+ res(ctr1,ctr2) = 1.0;
+ }
}
}
+
+ res.Release();
+ return res;
}
+ ReturnMatrix geqt(const Matrix& mat,const float a)
+ {
+ int ncols = mat.Ncols();
+ int nrows = mat.Nrows();
+ Matrix res(nrows,ncols);
+ res=0.0;
- res.Release();
- return res;
-}
-ReturnMatrix geqt(const Matrix& mat,const float a)
-{
- int ncols = mat.Ncols();
- int nrows = mat.Nrows();
- Matrix res(nrows,ncols);
- res=0.0;
-
- for (int ctr1 = 1; ctr1 <= nrows; ctr1++) {
- for (int ctr2 =1; ctr2 <= ncols; ctr2++) {
- if( mat(ctr1,ctr2) >= a){
- res(ctr1,ctr2) = 1.0;
+ for (int ctr1 = 1; ctr1 <= nrows; ctr1++) {
+ for (int ctr2 =1; ctr2 <= ncols; ctr2++) {
+ if( mat(ctr1,ctr2) >= a){
+ res(ctr1,ctr2) = 1.0;
+ }
}
}
+
+ res.Release();
+ return res;
}
- res.Release();
- return res;
-}
+ ReturnMatrix leqt(const Matrix& mat1,const Matrix& mat2)
+ {
+ int ctrcol = std::min(mat1.Ncols(),mat2.Ncols());
+ int ctrrow = std::min(mat1.Nrows(),mat2.Nrows());
+ Matrix res(ctrrow,ctrcol);
+ res=0.0;
-ReturnMatrix leqt(const Matrix& mat1,const Matrix& mat2)
-{
- int ctrcol = std::min(mat1.Ncols(),mat2.Ncols());
- int ctrrow = std::min(mat1.Nrows(),mat2.Nrows());
- Matrix res(ctrrow,ctrcol);
- res=0.0;
-
- for (int ctr1 = 1; ctr1 <= ctrrow; ctr1++) {
- for (int ctr2 =1; ctr2 <= ctrcol; ctr2++) {
- if( mat1(ctr1,ctr2) <= mat2(ctr1,ctr2)){
- res(ctr1,ctr2) = 1.0;
+ for (int ctr1 = 1; ctr1 <= ctrrow; ctr1++) {
+ for (int ctr2 =1; ctr2 <= ctrcol; ctr2++) {
+ if( mat1(ctr1,ctr2) <= mat2(ctr1,ctr2)){
+ res(ctr1,ctr2) = 1.0;
+ }
}
}
- }
- res.Release();
- return res;
-}
+ res.Release();
+ return res;
+ }
-ReturnMatrix eq(const Matrix& mat1,const Matrix& mat2)
-{
- int ctrcol = std::min(mat1.Ncols(),mat2.Ncols());
- int ctrrow = std::min(mat1.Nrows(),mat2.Nrows());
- Matrix res(ctrrow,ctrcol);
- res=0.0;
+ ReturnMatrix eq(const Matrix& mat1,const Matrix& mat2)
+ {
+ int ctrcol = std::min(mat1.Ncols(),mat2.Ncols());
+ int ctrrow = std::min(mat1.Nrows(),mat2.Nrows());
+ Matrix res(ctrrow,ctrcol);
+ res=0.0;
- for (int ctr1 = 1; ctr1 <= ctrrow; ctr1++) {
- for (int ctr2 =1; ctr2 <= ctrcol; ctr2++) {
- if( mat1(ctr1,ctr2) == mat2(ctr1,ctr2)){
- res(ctr1,ctr2) = 1.0;
+ for (int ctr1 = 1; ctr1 <= ctrrow; ctr1++) {
+ for (int ctr2 =1; ctr2 <= ctrcol; ctr2++) {
+ if( mat1(ctr1,ctr2) == mat2(ctr1,ctr2)){
+ res(ctr1,ctr2) = 1.0;
+ }
}
}
- }
- res.Release();
- return res;
-}
+ res.Release();
+ return res;
+ }
-ReturnMatrix neq(const Matrix& mat1,const Matrix& mat2)
-{
- int ctrcol = std::min(mat1.Ncols(),mat2.Ncols());
- int ctrrow = std::min(mat1.Nrows(),mat2.Nrows());
- Matrix res(ctrrow,ctrcol);
- res=0.0;
+ ReturnMatrix neq(const Matrix& mat1,const Matrix& mat2)
+ {
+ int ctrcol = std::min(mat1.Ncols(),mat2.Ncols());
+ int ctrrow = std::min(mat1.Nrows(),mat2.Nrows());
+ Matrix res(ctrrow,ctrcol);
+ res=0.0;
- for (int ctr1 = 1; ctr1 <= ctrrow; ctr1++) {
- for (int ctr2 =1; ctr2 <= ctrcol; ctr2++) {
- if( mat1(ctr1,ctr2) != mat2(ctr1,ctr2)){
- res(ctr1,ctr2) = 1.0;
+ for (int ctr1 = 1; ctr1 <= ctrrow; ctr1++) {
+ for (int ctr2 =1; ctr2 <= ctrcol; ctr2++) {
+ if( mat1(ctr1,ctr2) != mat2(ctr1,ctr2)){
+ res(ctr1,ctr2) = 1.0;
+ }
}
}
- }
- res.Release();
- return res;
-}
-
-ReturnMatrix SD(const Matrix& mat1,const Matrix& mat2)
-{
- if((mat1.Nrows() != mat2.Nrows()) ||
- (mat1.Ncols() != mat2.Ncols()) ){
- cerr <<"MISCMATHS::SD - matrices are of different dimensions"<<endl;
- exit(-1);
- }
- Matrix ret(mat1.Nrows(),mat1.Ncols());
- for (int r = 1; r <= mat1.Nrows(); r++) {
- for (int c =1; c <= mat1.Ncols(); c++) {
- if( mat2(r,c)==0)
- ret(r,c)=0;
- else
- ret(r,c) = mat1(r,c)/mat2(r,c);
- }
+ res.Release();
+ return res;
}
- ret.Release();
- return ret;
-}
+ ReturnMatrix SD(const Matrix& mat1,const Matrix& mat2)
+ {
+ if((mat1.Nrows() != mat2.Nrows()) ||
+ (mat1.Ncols() != mat2.Ncols()) ){
+ cerr <<"MISCMATHS::SD - matrices are of different dimensions"<<endl;
+ exit(-1);
+ }
+ Matrix ret(mat1.Nrows(),mat1.Ncols());
+ for (int r = 1; r <= mat1.Nrows(); r++) {
+ for (int c =1; c <= mat1.Ncols(); c++) {
+ if( mat2(r,c)==0)
+ ret(r,c)=0;
+ else
+ ret(r,c) = mat1(r,c)/mat2(r,c);
+ }
+ }
-void SD_econ(Matrix& mat1,const Matrix& mat2)
-{
- if((mat1.Nrows() != mat2.Nrows()) ||
- (mat1.Ncols() != mat2.Ncols()) ){
- cerr <<"MISCMATHS::SD - matrices are of different dimensions"<<endl;
- exit(-1);
+ ret.Release();
+ return ret;
}
- for (int r = 1; r <= mat1.Nrows(); r++) {
- for (int c =1; c <= mat1.Ncols(); c++) {
- if( mat2(r,c)==0)
- mat1(r,c)=0;
- else
- mat1(r,c) = mat1(r,c)/mat2(r,c);
+
+ void SD_econ(Matrix& mat1,const Matrix& mat2)
+ {
+ if((mat1.Nrows() != mat2.Nrows()) ||
+ (mat1.Ncols() != mat2.Ncols()) ){
+ cerr <<"MISCMATHS::SD - matrices are of different dimensions"<<endl;
+ exit(-1);
+ }
+ for (int r = 1; r <= mat1.Nrows(); r++) {
+ for (int c =1; c <= mat1.Ncols(); c++) {
+ if( mat2(r,c)==0)
+ mat1(r,c)=0;
+ else
+ mat1(r,c) = mat1(r,c)/mat2(r,c);
+ }
}
}
-}
-void SP_econ(Matrix& mat1,const Matrix& mat2)
-{
- if((mat1.Nrows() != mat2.Nrows()) ||
- (mat1.Ncols() != mat2.Ncols()) ){
- cerr <<"MISCMATHS::SD - matrices are of different dimensions"<<endl;
- exit(-1);
- }
- for (int r = 1; r <= mat1.Nrows(); r++) {
- for (int c =1; c <= mat1.Ncols(); c++) {
+ void SP_econ(Matrix& mat1,const Matrix& mat2)
+ {
+ if((mat1.Nrows() != mat2.Nrows()) ||
+ (mat1.Ncols() != mat2.Ncols()) ){
+ cerr <<"MISCMATHS::SD - matrices are of different dimensions"<<endl;
+ exit(-1);
+ }
+ for (int r = 1; r <= mat1.Nrows(); r++) {
+ for (int c =1; c <= mat1.Ncols(); c++) {
mat1(r,c) = mat1(r,c) * mat2(r,c);
+ }
}
}
-}
-//Deprecate?
-ReturnMatrix vox_to_vox(const ColumnVector& xyz1,const ColumnVector& dims1,const ColumnVector& dims2,const Matrix& xfm){
- ColumnVector xyz1_mm(4),xyz2_mm,xyz2(3);
- xyz1_mm<<xyz1(1)*dims1(1)<<xyz1(2)*dims1(2)<<xyz1(3)*dims1(3)<<1;
- xyz2_mm=xfm*xyz1_mm;
- xyz2_mm=xyz2_mm/xyz2_mm(4);
- xyz2<<xyz2_mm(1)/dims2(1)<<xyz2_mm(2)/dims2(2)<<xyz2_mm(3)/dims2(3);
- xyz2.Release();
- return xyz2;
-}
+ //Deprecate?
+ ReturnMatrix vox_to_vox(const ColumnVector& xyz1,const ColumnVector& dims1,const ColumnVector& dims2,const Matrix& xfm){
+ ColumnVector xyz1_mm(4),xyz2_mm,xyz2(3);
+ xyz1_mm<<xyz1(1)*dims1(1)<<xyz1(2)*dims1(2)<<xyz1(3)*dims1(3)<<1;
+ xyz2_mm=xfm*xyz1_mm;
+ xyz2_mm=xyz2_mm/xyz2_mm(4);
+ xyz2<<xyz2_mm(1)/dims2(1)<<xyz2_mm(2)/dims2(2)<<xyz2_mm(3)/dims2(3);
+ xyz2.Release();
+ return xyz2;
+ }
-//Deprecate?
-ReturnMatrix mni_to_imgvox(const ColumnVector& mni,const ColumnVector& mni_origin,const Matrix& mni2img, const ColumnVector& img_dims){
- ColumnVector mni_new_origin(4),img_mm;//homogeneous
- ColumnVector img_vox(3);
- mni_new_origin<<mni(1)+mni_origin(1)<<mni(2)+mni_origin(2)<<mni(3)+mni_origin(3)<<1;
- img_mm=mni2img*mni_new_origin;
- img_vox<<img_mm(1)/img_dims(1)<<img_mm(2)/img_dims(2)<<img_mm(3)/img_dims(3);
- img_vox.Release();
- return img_vox;
-}
+ //Deprecate?
+ ReturnMatrix mni_to_imgvox(const ColumnVector& mni,const ColumnVector& mni_origin,const Matrix& mni2img, const ColumnVector& img_dims){
+ ColumnVector mni_new_origin(4),img_mm;//homogeneous
+ ColumnVector img_vox(3);
+ mni_new_origin<<mni(1)+mni_origin(1)<<mni(2)+mni_origin(2)<<mni(3)+mni_origin(3)<<1;
+ img_mm=mni2img*mni_new_origin;
+ img_vox<<img_mm(1)/img_dims(1)<<img_mm(2)/img_dims(2)<<img_mm(3)/img_dims(3);
+ img_vox.Release();
+ return img_vox;
+ }
-void remmean_econ(Matrix& mat, const int dim)
-{
+ void remmean_econ(Matrix& mat, const int dim)
+ {
Matrix matmean;
remmean(mat, matmean, dim);
-}
+ }
-void remmean(Matrix& mat, Matrix& matmean, const int dim)
-{
+ void remmean(Matrix& mat, Matrix& matmean, const int dim)
+ {
matmean = mean(mat,dim);
if (dim == 1){
- for (int mr=1; mr<=mat.Nrows(); mr++)
- mat.Row(mr) -= matmean.AsRow();
+ for (int mr=1; mr<=mat.Nrows(); mr++)
+ mat.Row(mr) -= matmean.AsRow();
}
else{
- for (int mc=1; mc<=mat.Ncols(); mc++)
- mat.Column(mc) -= matmean.AsColumn();
+ for (int mc=1; mc<=mat.Ncols(); mc++)
+ mat.Column(mc) -= matmean.AsColumn();
}
-}
+ }
-void remmean(const Matrix& mat, Matrix& demeanedmat, Matrix& Mean, const int dim)
-{
+ void remmean(const Matrix& mat, Matrix& demeanedmat, Matrix& Mean, const int dim)
+ {
demeanedmat = mat;
remmean(demeanedmat,Mean, dim);
-}
+ }
-ReturnMatrix remmean(const Matrix& mat, const int dim)
-{
- Matrix res = mat;
- remmean_econ(res,dim);
- res.Release();
- return res;
-}
+ ReturnMatrix remmean(const Matrix& mat, const int dim)
+ {
+ Matrix res = mat;
+ remmean_econ(res,dim);
+ res.Release();
+ return res;
+ }
-ReturnMatrix oldcov(const Matrix& mat, const int norm)
-{
- SymmetricMatrix res;
- Matrix tmp;
- int N;
- tmp=remmean(mat);
- if (norm == 1) {N = mat.Nrows();}
- else {N = mat.Nrows()-1;}
- res << tmp.t()*tmp;
- res = res/N;
-
- res.Release();
- return res;
-}
+ ReturnMatrix oldcov(const Matrix& mat, const int norm)
+ {
+ SymmetricMatrix res;
+ Matrix tmp;
+ int N;
+ tmp=remmean(mat);
+ if (norm == 1) {N = mat.Nrows();}
+ else {N = mat.Nrows()-1;}
+ res << tmp.t()*tmp;
+ res = res/N;
+
+ res.Release();
+ return res;
+ }
-ReturnMatrix cov(const Matrix& data, const bool sampleCovariance, int econ)
-{
- //This assumes vectors are stored using column order in data
- SymmetricMatrix res;
- res << zeros(data.Ncols(),data.Ncols());
- Matrix meanM(mean(data));
- int N=data.Nrows();
- if (sampleCovariance && N>1)
- N--;
- if ( econ < 1 )
- econ=data.Nrows();
- for(int startRow=1; startRow <= data.Nrows(); startRow+=econ) {
- Matrix suba=data.SubMatrix(startRow,Min(startRow+econ-1,data.Nrows()),1,data.Ncols());
- for (int row=1; row <= suba.Nrows(); row++)
- suba.Row(row)-=meanM;
- res << res + suba.t()*suba/N;
- }
- res.Release();
- return res;
-}
+ ReturnMatrix cov(const Matrix& data, const bool sampleCovariance, int econ)
+ {
+ //This assumes vectors are stored using column order in data
+ SymmetricMatrix res;
+ res << zeros(data.Ncols(),data.Ncols());
+ Matrix meanM(mean(data));
+ int N=data.Nrows();
+ if (sampleCovariance && N>1)
+ N--;
+ if ( econ < 1 )
+ econ=data.Nrows();
+ for(int startRow=1; startRow <= data.Nrows(); startRow+=econ) {
+ Matrix suba=data.SubMatrix(startRow,Min(startRow+econ-1,data.Nrows()),1,data.Ncols());
+ for (int row=1; row <= suba.Nrows(); row++)
+ suba.Row(row)-=meanM;
+ res << res + suba.t()*suba/N;
+ }
+ res.Release();
+ return res;
+ }
-ReturnMatrix cov_r(const Matrix& data, const bool sampleCovariance, int econ)
-{
- //This assumes vectors are stored using row order in data
- SymmetricMatrix res;
- res << zeros(data.Nrows(),data.Nrows());
- Matrix meanM(mean(data,2));
- int N=data.Ncols();
- if (sampleCovariance && N>1)
- N--;
- if ( econ < 1 )
- econ=data.Ncols();
- for(int startCol=1; startCol <= data.Ncols(); startCol+=econ) {
- Matrix suba=data.SubMatrix(1,data.Nrows(),startCol,Min(startCol+econ-1,data.Ncols()));
- for (int col=1; col <= suba.Ncols(); col++)
- suba.Column(col)-=meanM;
- res << res + suba*suba.t()/N;
- }
- res.Release();
- return res;
-}
+ ReturnMatrix cov_r(const Matrix& data, const bool sampleCovariance, int econ)
+ {
+ //This assumes vectors are stored using row order in data
+ SymmetricMatrix res;
+ res << zeros(data.Nrows(),data.Nrows());
+ Matrix meanM(mean(data,2));
+ int N=data.Ncols();
+ if (sampleCovariance && N>1)
+ N--;
+ if ( econ < 1 )
+ econ=data.Ncols();
+ for(int startCol=1; startCol <= data.Ncols(); startCol+=econ) {
+ Matrix suba=data.SubMatrix(1,data.Nrows(),startCol,Min(startCol+econ-1,data.Ncols()));
+ for (int col=1; col <= suba.Ncols(); col++)
+ suba.Column(col)-=meanM;
+ res << res + suba*suba.t()/N;
+ }
+ res.Release();
+ return res;
+ }
-ReturnMatrix cov_r(const Matrix& data, const Matrix& weights2, int econ)
-{
- //This assumes vectors are stored using row order in data, weights are a single "row". No bool flag as biased vs unbiased isn't relevant here
- RowVector weights=((weights2/weights2.Sum()).AsRow());
- SymmetricMatrix res;
- res << zeros(data.Nrows(),data.Nrows());
- Matrix meanM(mean(data,weights,2));
- double N=(1-weights.SumSquare());//As weights.Sum() is equal to 1
- if ( econ < 1 )
- econ=data.Ncols();
- for(int startCol=1; startCol <= data.Ncols(); startCol+=econ) {
- Matrix suba=data.SubMatrix(1,data.Nrows(),startCol,Min(startCol+econ-1,data.Ncols()));
- for (int col=1; col <= suba.Ncols(); col++) {
- suba.Column(col)-=meanM;
- suba.Column(col)*=sqrt(weights(startCol+col-1));
- }
- res << res + suba*suba.t()/N;
- }
- res.Release();
- return res;
-}
+ ReturnMatrix cov_r(const Matrix& data, const Matrix& weights2, int econ)
+ {
+ //This assumes vectors are stored using row order in data, weights are a single "row". No bool flag as biased vs unbiased isn't relevant here
+ RowVector weights=((weights2/weights2.Sum()).AsRow());
+ SymmetricMatrix res;
+ res << zeros(data.Nrows(),data.Nrows());
+ Matrix meanM(mean(data,weights,2));
+ double N=(1-weights.SumSquare());//As weights.Sum() is equal to 1
+ if ( econ < 1 )
+ econ=data.Ncols();
+ for(int startCol=1; startCol <= data.Ncols(); startCol+=econ) {
+ Matrix suba=data.SubMatrix(1,data.Nrows(),startCol,Min(startCol+econ-1,data.Ncols()));
+ for (int col=1; col <= suba.Ncols(); col++) {
+ suba.Column(col)-=meanM;
+ suba.Column(col)*=sqrt(weights(startCol+col-1));
+ }
+ res << res + suba*suba.t()/N;
+ }
+ res.Release();
+ return res;
+ }
-ReturnMatrix corrcoef(const Matrix& mat, const bool norm)
-{
+ ReturnMatrix corrcoef(const Matrix& mat, const bool norm)
+ {
SymmetricMatrix res;
res = cov(mat,norm);
Matrix D;
@@ -2110,45 +2110,45 @@ ReturnMatrix corrcoef(const Matrix& mat, const bool norm)
res << SD(res,D);
res.Release();
return res;
-}
+ }
-ReturnMatrix flipud(const Matrix& mat)
-{
- Matrix rmat(mat.Nrows(),mat.Ncols());
- for(int j=1;j<=mat.Ncols();j++)
- for(int i=1;i<=mat.Nrows();i++)
- rmat(i,j)=mat(mat.Nrows()-i+1,j);
- rmat.Release();
- return rmat;
-}
+ ReturnMatrix flipud(const Matrix& mat)
+ {
+ Matrix rmat(mat.Nrows(),mat.Ncols());
+ for(int j=1;j<=mat.Ncols();j++)
+ for(int i=1;i<=mat.Nrows();i++)
+ rmat(i,j)=mat(mat.Nrows()-i+1,j);
+ rmat.Release();
+ return rmat;
+ }
-ReturnMatrix fliplr(const Matrix& mat)
-{
- Matrix rmat(mat.Nrows(),mat.Ncols());
- for(int j=1;j<=mat.Ncols();j++)
- for(int i=1;i<=mat.Nrows();i++)
- rmat(i,j)=mat(i,mat.Ncols()-j+1);
- rmat.Release();
- return rmat;
-}
+ ReturnMatrix fliplr(const Matrix& mat)
+ {
+ Matrix rmat(mat.Nrows(),mat.Ncols());
+ for(int j=1;j<=mat.Ncols();j++)
+ for(int i=1;i<=mat.Nrows();i++)
+ rmat(i,j)=mat(i,mat.Ncols()-j+1);
+ rmat.Release();
+ return rmat;
+ }
-void symm_orth(Matrix &Mat)
-{
- SymmetricMatrix Metric;
- Metric << Mat.t()*Mat;
- Metric = Metric.i();
- Matrix tmpE;
- DiagonalMatrix tmpD;
- EigenValues(Metric,tmpD,tmpE);
- Mat = Mat * tmpE * sqrt(abs(tmpD)) * tmpE.t();
-}
+ void symm_orth(Matrix &Mat)
+ {
+ SymmetricMatrix Metric;
+ Metric << Mat.t()*Mat;
+ Metric = Metric.i();
+ Matrix tmpE;
+ DiagonalMatrix tmpD;
+ EigenValues(Metric,tmpD,tmpE);
+ Mat = Mat * tmpE * sqrt(abs(tmpD)) * tmpE.t();
+ }
-void powerspectrum(const Matrix &Mat1, Matrix &Result, bool useLog)
+ void powerspectrum(const Matrix &Mat1, Matrix &Result, bool useLog)
//calculates the powerspectrum for every column of Mat1
-{
- Matrix res;
- for(int ctr=1; ctr <= Mat1.Ncols(); ctr++)
+ {
+ Matrix res;
+ for(int ctr=1; ctr <= Mat1.Ncols(); ctr++)
{
ColumnVector tmpCol;
tmpCol=Mat1.Column(ctr);
@@ -2166,67 +2166,67 @@ void powerspectrum(const Matrix &Mat1, Matrix &Result, bool useLog)
if(res.Storage()==0){res= tmpPow;}else{res|=tmpPow;}
}
Result=res;
-}
+ }
-void element_mod_n(Matrix& Mat,double n)
-{
- //represent each element in modulo n (useful for wrapping phases (n=2*M_PI))
+ void element_mod_n(Matrix& Mat,double n)
+ {
+ //represent each element in modulo n (useful for wrapping phases (n=2*M_PI))
+
+ double tmp;
+ for( int j=1;j<=Mat.Ncols();j++){
+ for( int i=1;i<=Mat.Nrows();i++){
- double tmp;
- for( int j=1;j<=Mat.Ncols();j++){
- for( int i=1;i<=Mat.Nrows();i++){
+ while( !( (Mat(i,j)>0) && (Mat(i,j)<n) ) ){
+ tmp = ( Mat(i,j) - rounddouble(Mat(i,j)/n)*n );
+ Mat(i,j)= tmp > 0 ? tmp : tmp + n;
+ }
- while( !( (Mat(i,j)>0) && (Mat(i,j)<n) ) ){
- tmp = ( Mat(i,j) - rounddouble(Mat(i,j)/n)*n );
- Mat(i,j)= tmp > 0 ? tmp : tmp + n;
}
}
}
-}
-
-int nextpow2(int n)
-{
- return (int)pow(2,ceil(log(float(n))/log(float(2))));
-}
+ int nextpow2(int n)
+ {
+ return (int)pow(2,ceil(log(float(n))/log(float(2))));
+ }
-void xcorr(const Matrix& p_ts, Matrix& ret, int lag, int p_zeropad)
-{
- Tracer tr("MISCMATHS::xcorr");
-
- int sizeTS = p_ts.Nrows();
- int numTS = p_ts.Ncols();
-
- if(p_zeropad == 0)
- p_zeropad = sizeTS;
- if(lag == 0)
- lag = sizeTS;
-
- ColumnVector x(p_zeropad);
- x = 0;
- ColumnVector fft_real;
- ColumnVector fft_im;
- ColumnVector dummy(p_zeropad);
- ColumnVector dummy2;
- dummy = 0;
- ColumnVector realifft(p_zeropad);
- ret.ReSize(lag,numTS);
- ret = 0;
-
- for(int i = 1; i <= numTS; i++)
+ void xcorr(const Matrix& p_ts, Matrix& ret, int lag, int p_zeropad)
+ {
+ Tracer tr("MISCMATHS::xcorr");
+
+ int sizeTS = p_ts.Nrows();
+ int numTS = p_ts.Ncols();
+
+ if(p_zeropad == 0)
+ p_zeropad = sizeTS;
+ if(lag == 0)
+ lag = sizeTS;
+
+ ColumnVector x(p_zeropad);
+ x = 0;
+ ColumnVector fft_real;
+ ColumnVector fft_im;
+ ColumnVector dummy(p_zeropad);
+ ColumnVector dummy2;
+ dummy = 0;
+ ColumnVector realifft(p_zeropad);
+ ret.ReSize(lag,numTS);
+ ret = 0;
+
+ for(int i = 1; i <= numTS; i++)
{
x.Rows(1,sizeTS) = p_ts.Column(i);
FFT(x, dummy, fft_real, fft_im);
for(int j = 1; j <= p_zeropad; j++)
- {
- // (x+iy)(x-iy) = x^2 + y^2
- fft_real(j) = fft_real(j)*fft_real(j) + fft_im(j)*fft_im(j);
- fft_im(j) = 0;
- }
+ {
+ // (x+iy)(x-iy) = x^2 + y^2
+ fft_real(j) = fft_real(j)*fft_real(j) + fft_im(j)*fft_im(j);
+ fft_im(j) = 0;
+ }
FFTI(fft_real, fft_im, realifft, dummy2);
@@ -2234,194 +2234,194 @@ void xcorr(const Matrix& p_ts, Matrix& ret, int lag, int p_zeropad)
ret.Column(i) = realifft.Rows(1,lag);
for(int j = 1; j <= lag-1; j++)
- {
- // Correction to make autocorr unbiased and normalised
- ret(j,i) = ret(j,i)/((sizeTS-j)*varx);
- }
+ {
+ // Correction to make autocorr unbiased and normalised
+ ret(j,i) = ret(j,i)/((sizeTS-j)*varx);
+ }
}
-}
+ }
-ReturnMatrix xcorr(const Matrix& p_ts, int lag, int p_zeropad )
-{
- Matrix r;
+ ReturnMatrix xcorr(const Matrix& p_ts, int lag, int p_zeropad )
+ {
+ Matrix r;
- xcorr(p_ts,r,lag,p_zeropad);
- r.Release();
- return r;
-}
+ xcorr(p_ts,r,lag,p_zeropad);
+ r.Release();
+ return r;
+ }
-void detrend(Matrix& p_ts, int p_level)
-{
- Tracer trace("MISCMATHS::detrend");
+ void detrend(Matrix& p_ts, int p_level)
+ {
+ Tracer trace("MISCMATHS::detrend");
- int sizeTS = p_ts.Nrows();
+ int sizeTS = p_ts.Nrows();
- // p_ts = b*a + e (OLS regression)
- // e is detrended data
- Matrix a(sizeTS, p_level+1);
+ // p_ts = b*a + e (OLS regression)
+ // e is detrended data
+ Matrix a(sizeTS, p_level+1);
- // Create a
- for(int t = 1; t <= sizeTS; t++)
+ // Create a
+ for(int t = 1; t <= sizeTS; t++)
{
for(int l = 0; l <= p_level; l++)
- a(t,l+1) = pow((float)t/sizeTS,l);
+ a(t,l+1) = pow((float)t/sizeTS,l);
}
- // Form residual forming matrix R:
- Matrix R = IdentityMatrix(sizeTS)-a*pinv(a);
+ // Form residual forming matrix R:
+ Matrix R = IdentityMatrix(sizeTS)-a*pinv(a);
- for(int t = 1; t <= sizeTS; t++)
+ for(int t = 1; t <= sizeTS; t++)
{
p_ts.Column(t) = R*p_ts.Column(t);
}
-}
+ }
-ReturnMatrix read_vest(string p_fname)
-{
- ifstream in;
- in.open(p_fname.c_str(), ios::in);
+ ReturnMatrix read_vest(string p_fname)
+ {
+ ifstream in;
+ in.open(p_fname.c_str(), ios::in);
- if(!in) throw Exception(string("Unable to open "+p_fname).c_str());
+ if(!in) throw Exception(string("Unable to open "+p_fname).c_str());
- int numWaves = 0;
- int numPoints = 0;
+ int numWaves = 0;
+ int numPoints = 0;
- string str;
+ string str;
- while(true)
+ while(true)
{
if(!in.good()) throw Exception(string(p_fname+" is not a valid vest file").c_str());
in >> str;
if(str == "/Matrix")
- break;
+ break;
else if(str == "/NumWaves")
- {
- in >> numWaves;
- }
+ {
+ in >> numWaves;
+ }
else if(str == "/NumPoints" || str == "/NumContrasts")
- {
- in >> numPoints;
- }
+ {
+ in >> numPoints;
+ }
}
- Matrix p_mat(numPoints, numWaves);
+ Matrix p_mat(numPoints, numWaves);
- for(int i = 1; i <= numPoints; i++)
+ for(int i = 1; i <= numPoints; i++)
{
for(int j = 1; j <= numWaves; j++)
- {
- if (!in.eof()) in >> ws >> p_mat(i,j) >> ws;
- else throw Exception(string(p_fname+" has insufficient data points").c_str());
- }
+ {
+ if (!in.eof()) in >> ws >> p_mat(i,j) >> ws;
+ else throw Exception(string(p_fname+" has insufficient data points").c_str());
+ }
}
- in.close();
+ in.close();
- p_mat.Release();
- return p_mat;
-}
+ p_mat.Release();
+ return p_mat;
+ }
-void ols(const Matrix& data,const Matrix& des,const Matrix& tc, Matrix& cope,Matrix& varcope){
- // ols
- // data is t x v
- // des is t x ev (design matrix)
- // tc is cons x ev (contrast matrix)
- // cope and varcope will be cons x v
- // but will be resized if they are wrong
- // hence may be passed in uninitialised
- // TB 2004
- if(data.Nrows() != des.Nrows()){
- cerr <<"MISCMATHS::ols - data and design have different number of time points"<<endl;
- exit(-1);
- }
- if(des.Ncols() != tc.Ncols()){
- cerr <<"MISCMATHS::ols - design and contrast matrix have different number of EVs"<<endl;
- exit(-1);
- }
- Matrix pdes = pinv(des);
- Matrix prevar=diag(tc*pdes*pdes.t()*tc.t());
- Matrix R=IdentityMatrix(des.Nrows())-des*pdes;
- float tR=R.Trace();
- Matrix pe=pdes*data;
- cope=tc*pe;
- Matrix res=data-des*pe;
- Matrix sigsq=sum(SP(res,res))/tR;
- varcope=prevar*sigsq;
+ void ols(const Matrix& data,const Matrix& des,const Matrix& tc, Matrix& cope,Matrix& varcope){
+ // ols
+ // data is t x v
+ // des is t x ev (design matrix)
+ // tc is cons x ev (contrast matrix)
+ // cope and varcope will be cons x v
+ // but will be resized if they are wrong
+ // hence may be passed in uninitialised
+ // TB 2004
+ if(data.Nrows() != des.Nrows()){
+ cerr <<"MISCMATHS::ols - data and design have different number of time points"<<endl;
+ exit(-1);
+ }
+ if(des.Ncols() != tc.Ncols()){
+ cerr <<"MISCMATHS::ols - design and contrast matrix have different number of EVs"<<endl;
+ exit(-1);
+ }
+ Matrix pdes = pinv(des);
+ Matrix prevar=diag(tc*pdes*pdes.t()*tc.t());
+ Matrix R=IdentityMatrix(des.Nrows())-des*pdes;
+ float tR=R.Trace();
+ Matrix pe=pdes*data;
+ cope=tc*pe;
+ Matrix res=data-des*pe;
+ Matrix sigsq=sum(SP(res,res))/tR;
+ varcope=prevar*sigsq;
-}
+ }
-float ols_dof(const Matrix& des){
- if ( des.Nrows() > 4000 ) //Use the simple version as huge designs require too much RAM in the full calculation
+ float ols_dof(const Matrix& des){
+ if ( des.Nrows() > 4000 ) //Use the simple version as huge designs require too much RAM in the full calculation
+ return des.Nrows() - des.Ncols();
+ try {
+ Matrix pdes = pinv(des);
+ Matrix R=IdentityMatrix(des.Nrows())-des*pdes;
+ return R.Trace();}
+ catch (...) {
+ cerr << "ols_dof: Error in determining the trace, resorting to basic calculation" << endl;
+ }
return des.Nrows() - des.Ncols();
- try {
- Matrix pdes = pinv(des);
- Matrix R=IdentityMatrix(des.Nrows())-des*pdes;
- return R.Trace();}
- catch (...) {
- cerr << "ols_dof: Error in determining the trace, resorting to basic calculation" << endl;
- }
- return des.Nrows() - des.Ncols();
-}
+ }
-int conjgrad(ColumnVector& x, const Matrix& A, const ColumnVector& b, int maxit,
- float reltol)
-{
- // solves: A * x = b (for x)
- // implementation of algorithm in Golub and Van Loan (3rd ed, page 527)
- ColumnVector rk1, rk2, pk, apk;
- double betak, alphak, rk1rk1=0, rk2rk2, r00=0;
- int k=0;
- rk1 = b - A*x; // a *big* calculation
- for (int n=1; n<=maxit; n++) {
- k++;
- if (k==1) {
- pk = rk1;
- rk1rk1 = (rk1.t() * rk1).AsScalar();
- r00=rk1rk1;
- } else {
- rk2rk2 = rk1rk1; // from before
- rk1rk1 = (rk1.t() * rk1).AsScalar();
- if (rk2rk2<1e-10*rk1rk1) {
- cerr << "WARNING:: Conj Grad - low demoninator (rk2rk2)" << endl;
- if (rk2rk2<=0) {
- cerr << "Aborting conj grad ..." << endl;
- return 1;
- }
+ int conjgrad(ColumnVector& x, const Matrix& A, const ColumnVector& b, int maxit,
+ float reltol)
+ {
+ // solves: A * x = b (for x)
+ // implementation of algorithm in Golub and Van Loan (3rd ed, page 527)
+ ColumnVector rk1, rk2, pk, apk;
+ double betak, alphak, rk1rk1=0, rk2rk2, r00=0;
+ int k=0;
+ rk1 = b - A*x; // a *big* calculation
+ for (int n=1; n<=maxit; n++) {
+ k++;
+ if (k==1) {
+ pk = rk1;
+ rk1rk1 = (rk1.t() * rk1).AsScalar();
+ r00=rk1rk1;
+ } else {
+ rk2rk2 = rk1rk1; // from before
+ rk1rk1 = (rk1.t() * rk1).AsScalar();
+ if (rk2rk2<1e-10*rk1rk1) {
+ cerr << "WARNING:: Conj Grad - low demoninator (rk2rk2)" << endl;
+ if (rk2rk2<=0) {
+ cerr << "Aborting conj grad ..." << endl;
+ return 1;
+ }
+ }
+ betak = rk1rk1 / rk2rk2;
+ pk = rk1 + betak * pk; // note RHS pk is p(k-1) in algorithm
}
- betak = rk1rk1 / rk2rk2;
- pk = rk1 + betak * pk; // note RHS pk is p(k-1) in algorithm
- }
- // stop if sufficient accuracy is achieved
- if (rk1rk1<reltol*reltol*r00) return 0;
-
- apk = A * pk; // the *big* calculation in this algorithm
-
- ColumnVector pap = pk.t() * apk;
- if (pap.AsScalar()<0) {
- cerr << "ERROR:: Conj Grad - negative eigenvector found (matrix must be symmetric and positive-definite)\nAborting ... " << endl;
- return 2;
- } else if (pap.AsScalar()<1e-10) {
- cerr << "WARNING:: Conj Grad - nearly null eigenvector found (terminating early)" << endl;
- return 1;
- } else {
- alphak = rk1rk1 / pap.AsScalar();
+ // stop if sufficient accuracy is achieved
+ if (rk1rk1<reltol*reltol*r00) return 0;
+
+ apk = A * pk; // the *big* calculation in this algorithm
+
+ ColumnVector pap = pk.t() * apk;
+ if (pap.AsScalar()<0) {
+ cerr << "ERROR:: Conj Grad - negative eigenvector found (matrix must be symmetric and positive-definite)\nAborting ... " << endl;
+ return 2;
+ } else if (pap.AsScalar()<1e-10) {
+ cerr << "WARNING:: Conj Grad - nearly null eigenvector found (terminating early)" << endl;
+ return 1;
+ } else {
+ alphak = rk1rk1 / pap.AsScalar();
+ }
+ x = x + alphak * pk; // note LHS is x(k) and RHS is x(k-1) in algorithm
+ rk2 = rk1; // update prior to the next step
+ rk1 = rk1 - alphak * apk; // note LHS is r(k) in algorithm
}
- x = x + alphak * pk; // note LHS is x(k) and RHS is x(k-1) in algorithm
- rk2 = rk1; // update prior to the next step
- rk1 = rk1 - alphak * apk; // note LHS is r(k) in algorithm
+ return 0;
}
- return 0;
-}
-int conjgrad(ColumnVector& x, const Matrix& A, const ColumnVector& b, int maxit)
-{
- return conjgrad(x,A,b,maxit,1e-10);
-}
+ int conjgrad(ColumnVector& x, const Matrix& A, const ColumnVector& b, int maxit)
+ {
+ return conjgrad(x,A,b,maxit,1e-10);
+ }
-float csevl(const float x, const ColumnVector& cs, const int n)
+ float csevl(const float x, const ColumnVector& cs, const int n)
{
float b0 = 0;
@@ -2430,11 +2430,11 @@ float csevl(const float x, const ColumnVector& cs, const int n)
const float twox=2*x;
for(int i=1; i<=n; i++)
- {
- b2=b1;
- b1=b0;
- b0=twox*b1-b2+cs(n+1-i);
- }
+ {
+ b2=b1;
+ b1=b0;
+ b0=twox*b1-b2+cs(n+1-i);
+ }
return 0.5*(b0-b2);
}
@@ -2491,24 +2491,24 @@ float csevl(const float x, const ColumnVector& cs, const int n)
float psi;
if(y<2.0)
- {
- // do we need to deal with the following case?
- // c psi(x) for -2. .lt. x .lt. 2.
-
- int n = int(floor(x));
- y = x - n;
- n = n - 1;
- psi = csevl(2*y-1, psics, ntpsi);
- if(n==-1)
+ {
+ // do we need to deal with the following case?
+ // c psi(x) for -2. .lt. x .lt. 2.
+
+ int n = int(floor(x));
+ y = x - n;
+ n = n - 1;
+ psi = csevl(2*y-1, psics, ntpsi);
+ if(n==-1)
{
psi = psi - 1.0/x;
}
- }
+ }
else
- {
- const float aux = csevl(8/(Sqr(y))-1, apsics, ntapsi);
- psi = log(x) - 0.5/x + aux;
- }
+ {
+ const float aux = csevl(8/(Sqr(y))-1, apsics, ntapsi);
+ psi = log(x) - 0.5/x + aux;
+ }
return psi;
}
@@ -2537,34 +2537,34 @@ float csevl(const float x, const ColumnVector& cs, const int n)
ColumnVector lambdaB(nevs);
if(nevs<ntpts-10)
- {
- // initialise with OLS
- B=pinv(X)*Y;
- ColumnVector res=Y-X*B;
- gam_y=(ntpts-nevs)/(res.t()*res).AsScalar();
-
- ilambda_B << (X.t()*X*gam_y).i();
- lambdaB=0;
- for(int l=1; l <= nevs; l++)
+ {
+ // initialise with OLS
+ B=pinv(X)*Y;
+ ColumnVector res=Y-X*B;
+ gam_y=(ntpts-nevs)/(res.t()*res).AsScalar();
+
+ ilambda_B << (X.t()*X*gam_y).i();
+ lambdaB=0;
+ for(int l=1; l <= nevs; l++)
{
lambdaB(l)=ilambda_B(l,l);
}
- }
+ }
else
- {
- OUT("no ols");
- B.ReSize(nevs);
- B=0;
- lambdaB=1;
+ {
+ OUT("no ols");
+ B.ReSize(nevs);
+ B=0;
+ lambdaB=1;
-// ColumnVector res=Y-X*B;
-// gam_y=ntpts/(res.t()*res).AsScalar();
+ // ColumnVector res=Y-X*B;
+ // gam_y=ntpts/(res.t()*res).AsScalar();
- gam_y=10;
- }
+ gam_y=10;
+ }
-// OUT(B(1));
-// OUT(lambdaB(1));
+ // OUT(B(1));
+ // OUT(lambdaB(1));
float trace_ilambdaZZ=1;
@@ -2583,11 +2583,11 @@ float csevl(const float x, const ColumnVector& cs, const int n)
int i = 1;;
for(; i<=niters; i++)
- {
- cout<<i<<",";
- ////////////////////
- // update phim
- for(int l=1; l <= nevs; l++)
+ {
+ cout<<i<<",";
+ ////////////////////
+ // update phim
+ for(int l=1; l <= nevs; l++)
{
float b_m0 = 1e10;
float c_m0 = 2;
@@ -2597,167 +2597,167 @@ float csevl(const float x, const ColumnVector& cs, const int n)
gam_m(l) = b_m*c_m;
}
-// OUT(gam_m(1));
+ // OUT(gam_m(1));
- ////////////////////
- // update B
- ColumnVector beta(nevs);
- beta = 0;
- SymmetricMatrix lambda_B(nevs);
- lambda_B = 0;
+ ////////////////////
+ // update B
+ ColumnVector beta(nevs);
+ beta = 0;
+ SymmetricMatrix lambda_B(nevs);
+ lambda_B = 0;
- for(int l=1; l <= nevs; l++)
- lambda_B(l,l)=gam_m(l);
+ for(int l=1; l <= nevs; l++)
+ lambda_B(l,l)=gam_m(l);
- SymmetricMatrix tmp = lambda_B + gam_y*ZZ;
- lambda_B << tmp;
+ SymmetricMatrix tmp = lambda_B + gam_y*ZZ;
+ lambda_B << tmp;
- beta += gam_y*ZY;
+ beta += gam_y*ZY;
- ilambda_B << lambda_B.i();
- B=ilambda_B*beta;
+ ilambda_B << lambda_B.i();
+ B=ilambda_B*beta;
- lambdaB.ReSize(nevs);
- lambdaB=0;
- for(int l=1; l <= nevs; l++)
+ lambdaB.ReSize(nevs);
+ lambdaB=0;
+ for(int l=1; l <= nevs; l++)
{
lambdaB(l)=ilambda_B(l,l);
}
- ////////////////////
- // compute trace for noise precision phiy update
+ ////////////////////
+ // compute trace for noise precision phiy update
- SymmetricMatrix tmp3;
- tmp3 << ilambda_B;
+ SymmetricMatrix tmp3;
+ tmp3 << ilambda_B;
- SymmetricMatrix tmp2;
- tmp2 << tmp3*ZZ;
+ SymmetricMatrix tmp2;
+ tmp2 << tmp3*ZZ;
- trace_ilambdaZZ=tmp2.Trace();
-// OUT(trace_ilambdaZZ);
+ trace_ilambdaZZ=tmp2.Trace();
+ // OUT(trace_ilambdaZZ);
- /////////////////////
- // update phiy
- float b_y0 = 1e10;
- float c_y0 = 1;
+ /////////////////////
+ // update phiy
+ float b_y0 = 1e10;
+ float c_y0 = 1;
- float c_y = (ntpts-1)/2.0 + c_y0;
+ float c_y = (ntpts-1)/2.0 + c_y0;
- float sum = YY + (B.t()*ZZ*B).AsScalar() - 2*(B.t()*ZY).AsScalar();
+ float sum = YY + (B.t()*ZZ*B).AsScalar() - 2*(B.t()*ZY).AsScalar();
- float b_y = 1.0/(0.5*(sum + trace_ilambdaZZ)+1/b_y0);
+ float b_y = 1.0/(0.5*(sum + trace_ilambdaZZ)+1/b_y0);
- gam_y = b_y*c_y;
+ gam_y = b_y*c_y;
-// OUT(gam_y);
+ // OUT(gam_y);
- }
+ }
cout << endl;
}
-vector<float> ColumnVector2vector(const ColumnVector& col)
-{
- vector<float> vec(col.Nrows());
+ vector<float> ColumnVector2vector(const ColumnVector& col)
+ {
+ vector<float> vec(col.Nrows());
- for(int c = 0; c < col.Nrows(); c++)
- vec[c] = col(c+1);
+ for(int c = 0; c < col.Nrows(); c++)
+ vec[c] = col(c+1);
- return vec;
-}
+ return vec;
+ }
-/////////////////////////////////////////////////////////////////////////////////////////////////////
+ /////////////////////////////////////////////////////////////////////////////////////////////////////
-// Uninteresting byte swapping functions
+ // Uninteresting byte swapping functions
-typedef struct { unsigned char a,b ; } TWObytes ;
+ typedef struct { unsigned char a,b ; } TWObytes ;
-void Swap_2bytes( int n , void *ar ) /* 2 bytes at a time */
-{
- int ii ;
- TWObytes *tb = (TWObytes *)ar ;
- unsigned char tt ;
+ void Swap_2bytes( int n , void *ar ) /* 2 bytes at a time */
+ {
+ int ii ;
+ TWObytes *tb = (TWObytes *)ar ;
+ unsigned char tt ;
- for( ii=0 ; ii < n ; ii++ ){
- tt = tb[ii].a ; tb[ii].a = tb[ii].b ; tb[ii].b = tt ;
- }
- return ;
-}
+ for( ii=0 ; ii < n ; ii++ ){
+ tt = tb[ii].a ; tb[ii].a = tb[ii].b ; tb[ii].b = tt ;
+ }
+ return ;
+ }
-/*---------------------------------------------------------------------------*/
+ /*---------------------------------------------------------------------------*/
-typedef struct { unsigned char a,b,c,d ; } FOURbytes ;
+ typedef struct { unsigned char a,b,c,d ; } FOURbytes ;
-void Swap_4bytes( int n , void *ar ) /* 4 bytes at a time */
-{
- int ii ;
- FOURbytes *tb = (FOURbytes *)ar ;
- unsigned char tt ;
+ void Swap_4bytes( int n , void *ar ) /* 4 bytes at a time */
+ {
+ int ii ;
+ FOURbytes *tb = (FOURbytes *)ar ;
+ unsigned char tt ;
- for( ii=0 ; ii < n ; ii++ ){
- tt = tb[ii].a ; tb[ii].a = tb[ii].d ; tb[ii].d = tt ;
- tt = tb[ii].b ; tb[ii].b = tb[ii].c ; tb[ii].c = tt ;
- }
- return ;
-}
+ for( ii=0 ; ii < n ; ii++ ){
+ tt = tb[ii].a ; tb[ii].a = tb[ii].d ; tb[ii].d = tt ;
+ tt = tb[ii].b ; tb[ii].b = tb[ii].c ; tb[ii].c = tt ;
+ }
+ return ;
+ }
-/*---------------------------------------------------------------------------*/
+ /*---------------------------------------------------------------------------*/
-typedef struct { unsigned char a,b,c,d , D,C,B,A ; } EIGHTbytes ;
+ typedef struct { unsigned char a,b,c,d , D,C,B,A ; } EIGHTbytes ;
-void Swap_8bytes( int n , void *ar ) /* 8 bytes at a time */
-{
- int ii ;
- EIGHTbytes *tb = (EIGHTbytes *)ar ;
- unsigned char tt ;
+ void Swap_8bytes( int n , void *ar ) /* 8 bytes at a time */
+ {
+ int ii ;
+ EIGHTbytes *tb = (EIGHTbytes *)ar ;
+ unsigned char tt ;
- for( ii=0 ; ii < n ; ii++ ){
- tt = tb[ii].a ; tb[ii].a = tb[ii].A ; tb[ii].A = tt ;
- tt = tb[ii].b ; tb[ii].b = tb[ii].B ; tb[ii].B = tt ;
- tt = tb[ii].c ; tb[ii].c = tb[ii].C ; tb[ii].C = tt ;
- tt = tb[ii].d ; tb[ii].d = tb[ii].D ; tb[ii].D = tt ;
- }
- return ;
-}
+ for( ii=0 ; ii < n ; ii++ ){
+ tt = tb[ii].a ; tb[ii].a = tb[ii].A ; tb[ii].A = tt ;
+ tt = tb[ii].b ; tb[ii].b = tb[ii].B ; tb[ii].B = tt ;
+ tt = tb[ii].c ; tb[ii].c = tb[ii].C ; tb[ii].C = tt ;
+ tt = tb[ii].d ; tb[ii].d = tb[ii].D ; tb[ii].D = tt ;
+ }
+ return ;
+ }
-/*---------------------------------------------------------------------------*/
-
-typedef struct { unsigned char a,b,c,d,e,f,g,h ,
- H,G,F,E,D,C,B,A ; } SIXTEENbytes ;
-
-void Swap_16bytes( int n , void *ar ) /* 16 bytes at a time */
-{
- int ii ;
- SIXTEENbytes *tb = (SIXTEENbytes *)ar ;
- unsigned char tt ;
-
- for( ii=0 ; ii < n ; ii++ ){
- tt = tb[ii].a ; tb[ii].a = tb[ii].A ; tb[ii].A = tt ;
- tt = tb[ii].b ; tb[ii].b = tb[ii].B ; tb[ii].B = tt ;
- tt = tb[ii].c ; tb[ii].c = tb[ii].C ; tb[ii].C = tt ;
- tt = tb[ii].d ; tb[ii].d = tb[ii].D ; tb[ii].D = tt ;
-
- tt = tb[ii].e ; tb[ii].e = tb[ii].E ; tb[ii].E = tt ;
- tt = tb[ii].f ; tb[ii].f = tb[ii].F ; tb[ii].F = tt ;
- tt = tb[ii].g ; tb[ii].g = tb[ii].G ; tb[ii].G = tt ;
- tt = tb[ii].h ; tb[ii].h = tb[ii].H ; tb[ii].H = tt ;
- }
- return ;
-}
+ /*---------------------------------------------------------------------------*/
-/*---------------------------------------------------------------------------*/
-
-void Swap_Nbytes( int n , int siz , void *ar ) /* subsuming case */
-{
- switch( siz ){
- case 2: Swap_2bytes ( n , ar ) ; break ;
- case 4: Swap_4bytes ( n , ar ) ; break ;
- case 8: Swap_8bytes ( n , ar ) ; break ;
- case 16: Swap_16bytes( n , ar ) ; break ;
- }
- return ;
-}
+ typedef struct { unsigned char a,b,c,d,e,f,g,h ,
+ H,G,F,E,D,C,B,A ; } SIXTEENbytes ;
+
+ void Swap_16bytes( int n , void *ar ) /* 16 bytes at a time */
+ {
+ int ii ;
+ SIXTEENbytes *tb = (SIXTEENbytes *)ar ;
+ unsigned char tt ;
+
+ for( ii=0 ; ii < n ; ii++ ){
+ tt = tb[ii].a ; tb[ii].a = tb[ii].A ; tb[ii].A = tt ;
+ tt = tb[ii].b ; tb[ii].b = tb[ii].B ; tb[ii].B = tt ;
+ tt = tb[ii].c ; tb[ii].c = tb[ii].C ; tb[ii].C = tt ;
+ tt = tb[ii].d ; tb[ii].d = tb[ii].D ; tb[ii].D = tt ;
+
+ tt = tb[ii].e ; tb[ii].e = tb[ii].E ; tb[ii].E = tt ;
+ tt = tb[ii].f ; tb[ii].f = tb[ii].F ; tb[ii].F = tt ;
+ tt = tb[ii].g ; tb[ii].g = tb[ii].G ; tb[ii].G = tt ;
+ tt = tb[ii].h ; tb[ii].h = tb[ii].H ; tb[ii].H = tt ;
+ }
+ return ;
+ }
+
+ /*---------------------------------------------------------------------------*/
+
+ void Swap_Nbytes( int n , int siz , void *ar ) /* subsuming case */
+ {
+ switch( siz ){
+ case 2: Swap_2bytes ( n , ar ) ; break ;
+ case 4: Swap_4bytes ( n , ar ) ; break ;
+ case 8: Swap_8bytes ( n , ar ) ; break ;
+ case 16: Swap_16bytes( n , ar ) ; break ;
+ }
+ return ;
+ }
-// end namespace MISCMATHS
+ // end namespace MISCMATHS
}
diff --git a/sparsefn.cc b/sparsefn.cc
index 338e60f82f42ecf4b88691bc984540e5a6f62712..432a12b9b9296eb043a76390944d72d74735b288 100644
--- a/sparsefn.cc
+++ b/sparsefn.cc
@@ -37,14 +37,14 @@ namespace MISCMATHS {
float sum = 0;
for(int j = 1; j<=m.Nrows(); j++)
- {
- // do diagonal
- sum += C(j,j)*m(j)*m(j);
+ {
+ // do diagonal
+ sum += C(j,j)*m(j)*m(j);
- // do off-diagonal
- const SparseMatrix::Row& row = C.row(j);
+ // do off-diagonal
+ const SparseMatrix::Row& row = C.row(j);
- for(SparseMatrix::Row::const_iterator it=row.begin();it!=row.end();it++)
+ for(SparseMatrix::Row::const_iterator it=row.begin();it!=row.end();it++)
{
int c = (*it).first+1;
@@ -53,7 +53,7 @@ namespace MISCMATHS {
double val = (*it).second;
sum += 2*val*m(j)*m(c);
}
- }
+ }
return sum;
}
@@ -61,23 +61,23 @@ namespace MISCMATHS {
{
ret.ReSize(n,n);
for(int j = 1; j<=n; j++)
- {
- ret.insert(j,j,1);
- }
+ {
+ ret.insert(j,j,1);
+ }
}
void addto(SparseMatrix::Row& A, const SparseMatrix::Row& B, float S)
{
// computes A = A+B*S
if(S!=0)
- {
- for(SparseMatrix::Row::const_iterator it=B.begin();it!=B.end();it++)
+ {
+ for(SparseMatrix::Row::const_iterator it=B.begin();it!=B.end();it++)
{
int c = (*it).first;
double val = (*it).second;
A[c] += val*S;
}
- }
+ }
}
void addto(SparseMatrix& A, const SparseMatrix& B, float S)
@@ -85,18 +85,18 @@ namespace MISCMATHS {
Tracer_Plus trace("sparsefns::addto");
// computes A+B*S
if(S!=0)
- {
- for(int j = 1; j<=B.Nrows(); j++)
+ {
+ for(int j = 1; j<=B.Nrows(); j++)
{
const SparseMatrix::Row& row = B.row(j);
for(SparseMatrix::Row::const_iterator it=row.begin();it!=row.end();it++)
- {
- int c = (*it).first+1;
- double val = (*it).second*S;
- A.addto(j,c,val);
- }
+ {
+ int c = (*it).first+1;
+ double val = (*it).second*S;
+ A.addto(j,c,val);
+ }
}
- }
+ }
}
void symmetric_addto(SparseMatrix& A, const SparseMatrix& B, float S)
@@ -104,23 +104,23 @@ namespace MISCMATHS {
Tracer_Plus trace("sparsefns::symmetric_addto");
// computes A+B*S
if(S!=0)
- {
- for(int j = 1; j<=B.Nrows(); j++)
+ {
+ for(int j = 1; j<=B.Nrows(); j++)
{
const SparseMatrix::Row& row = B.row(j);
A.addto(j,j,B(j,j)*S);
for(SparseMatrix::Row::const_iterator it=row.lower_bound(j);it!=row.end();it++)
- {
- int c = (*it).first+1;
- double val = (*it).second*S;
+ {
+ int c = (*it).first+1;
+ double val = (*it).second*S;
- A.addto(j,c,val);
- A.addto(c,j,val);
- }
+ A.addto(j,c,val);
+ A.addto(c,j,val);
+ }
}
- }
+ }
}
void addto(SparseMatrix& A, const Matrix& B)
@@ -128,10 +128,10 @@ namespace MISCMATHS {
Tracer_Plus trace("sparsefns::addto2");
for(int r=1; r <= B.Nrows(); r++)
for(int c=1; c <= B.Ncols(); c++)
- {
- if(B(r,c)!=0)
- A.addto(r,c,B(r,c));
- }
+ {
+ if(B(r,c)!=0)
+ A.addto(r,c,B(r,c));
+ }
}
@@ -143,38 +143,38 @@ namespace MISCMATHS {
U.ReSize(length,length);
for(int j = 1; j<=length; j++)
- {
+ {
- const SparseMatrix::Row& rowAj = A.row(j);
- SparseMatrix::Row& rowUj = U.row(j);
+ const SparseMatrix::Row& rowAj = A.row(j);
+ SparseMatrix::Row& rowUj = U.row(j);
- for(SparseMatrix::Row::const_iterator it=rowAj.lower_bound(j-1);it!=rowAj.end();it++)
+ for(SparseMatrix::Row::const_iterator it=rowAj.lower_bound(j-1);it!=rowAj.end();it++)
{
int c = (*it).first;
double val = (*it).second;
rowUj[c] = val;
}
- for(int k = 1; k<=j-1; k++)
+ for(int k = 1; k<=j-1; k++)
{
SparseMatrix::Row& rowk = U.row(k);
double Ukj = U(k,j);
if(Ukj!=0)
for(SparseMatrix::Row::iterator it=rowk.lower_bound(j-1);it!=rowk.end();it++)
- {
- int c = (*it).first+1;
- double val = (*it).second*Ukj;
- U.addto(j,c,-val);
- }
+ {
+ int c = (*it).first+1;
+ double val = (*it).second*Ukj;
+ U.addto(j,c,-val);
+ }
}
- double sqrtUjj = std::sqrt(Max(U(j,j),1e-6));
+ double sqrtUjj = std::sqrt(Max(U(j,j),1e-6));
- for(SparseMatrix::Row::iterator it=rowUj.lower_bound(j-1);it!=rowUj.end();it++)
+ for(SparseMatrix::Row::iterator it=rowUj.lower_bound(j-1);it!=rowUj.end();it++)
{
(*it).second /= sqrtUjj;
}
- }
+ }
U.transpose(L);
}
@@ -183,7 +183,7 @@ namespace MISCMATHS {
{
Tracer_Plus trace("sparsefns::inv");
- // assumes A=LU is symmetric
+ // assumes A=LU is symmetric
int length = U.Nrows();
@@ -193,24 +193,24 @@ namespace MISCMATHS {
speye(length,b);
for(int bi=1;bi<=b.Ncols();bi++)
- {
- // solve for y (L*y=b)
- ColumnVector y(length);
- y = 0;
- y(1) = b(1,bi)/L(1,1);
+ {
+ // solve for y (L*y=b)
+ ColumnVector y(length);
+ y = 0;
+ y(1) = b(1,bi)/L(1,1);
- bool compute = false;
- if(b(1,bi)!=0) compute = true;
+ bool compute = false;
+ if(b(1,bi)!=0) compute = true;
- for(int r = 2; r<=length; r++)
+ for(int r = 2; r<=length; r++)
{
if(!compute && b(r,bi)!=0) compute = true;
if(compute)
- {
- float sum = 0.0;
- const SparseMatrix::Row& row = L.row(r);
- for(SparseMatrix::Row::const_iterator it=row.begin();it!=row.end();it++)
+ {
+ float sum = 0.0;
+ const SparseMatrix::Row& row = L.row(r);
+ for(SparseMatrix::Row::const_iterator it=row.begin();it!=row.end();it++)
{
int c = (*it).first+1;
@@ -220,38 +220,38 @@ namespace MISCMATHS {
sum += val*y(c);
}
- y(r) = (b(r,bi)-sum)/L(r,r);
- }
+ y(r) = (b(r,bi)-sum)/L(r,r);
+ }
}
- // solve for x(bi) (U*x=y)
+ // solve for x(bi) (U*x=y)
- ret.set(length,bi,y(length)/U(length,length));
- compute = false;
- if(y(length)!=0) compute = true;
+ ret.set(length,bi,y(length)/U(length,length));
+ compute = false;
+ if(y(length)!=0) compute = true;
- // do not do rows which we already have from symmetry
- // therefore end at r=bi and not r=1
- for(int r = length; r>=bi; r--)
+ // do not do rows which we already have from symmetry
+ // therefore end at r=bi and not r=1
+ for(int r = length; r>=bi; r--)
{
if(!compute && y(r)!=0) compute = true;
if(compute)
- {
- float sum = 0.0;
- const SparseMatrix::Row& row = U.row(r);
- for(SparseMatrix::Row::const_iterator it=row.lower_bound(r);it!=row.end();it++)
+ {
+ float sum = 0.0;
+ const SparseMatrix::Row& row = U.row(r);
+ for(SparseMatrix::Row::const_iterator it=row.lower_bound(r);it!=row.end();it++)
{
int c = (*it).first+1;
double val = (*it).second;
sum += val*ret(c,bi);
}
- ret.set(r,bi,(y(r)-sum)/U(r,r));
- ret.set(bi,r,(y(r)-sum)/U(r,r));
- }
+ ret.set(r,bi,(y(r)-sum)/U(r,r));
+ ret.set(bi,r,(y(r)-sum)/U(r,r));
+ }
}
- }
+ }
}
void solvefortracex(const SparseMatrix& U, const SparseMatrix& L, const SparseMatrix& b1, const SparseMatrix& b2, float& tr1, float& tr2)
@@ -264,32 +264,32 @@ namespace MISCMATHS {
tr2 = 0.0;
for(int bi=1;bi<=b1.Ncols();bi++)
- {
- // solve for y (L*y=b)
- ColumnVector y1(length);
- ColumnVector y2(length);
- y1 = 0;
- y2 = 0;
- y1(1) = b1(1,bi)/L(1,1);
- y2(1) = b2(1,bi)/L(1,1);
-
- bool compute1 = false;
- if(b1(1,bi)!=0) compute1 = true;
-
- bool compute2 = false;
- if(b2(1,bi)!=0) compute2 = true;
-
- for(int r = 2; r<=length; r++)
+ {
+ // solve for y (L*y=b)
+ ColumnVector y1(length);
+ ColumnVector y2(length);
+ y1 = 0;
+ y2 = 0;
+ y1(1) = b1(1,bi)/L(1,1);
+ y2(1) = b2(1,bi)/L(1,1);
+
+ bool compute1 = false;
+ if(b1(1,bi)!=0) compute1 = true;
+
+ bool compute2 = false;
+ if(b2(1,bi)!=0) compute2 = true;
+
+ for(int r = 2; r<=length; r++)
{
if(!compute1 && b1(r,bi)!=0) compute1 = true;
if(!compute2 && b2(r,bi)!=0) compute2 = true;
if(compute1 || compute2)
- {
- float sum1 = 0.0;
- float sum2 = 0.0;
- const SparseMatrix::Row& row = L.row(r);
- for(SparseMatrix::Row::const_iterator it=row.begin();it!=row.end();it++)
+ {
+ float sum1 = 0.0;
+ float sum2 = 0.0;
+ const SparseMatrix::Row& row = L.row(r);
+ for(SparseMatrix::Row::const_iterator it=row.begin();it!=row.end();it++)
{
int c = (*it).first+1;
@@ -300,36 +300,36 @@ namespace MISCMATHS {
if(compute2) sum2 += val*y2(c);
}
- if(compute1) y1(r) = (b1(r,bi)-sum1)/L(r,r);
- if(compute2) y2(r) = (b2(r,bi)-sum2)/L(r,r);
- }
+ if(compute1) y1(r) = (b1(r,bi)-sum1)/L(r,r);
+ if(compute2) y2(r) = (b2(r,bi)-sum2)/L(r,r);
+ }
}
- // solve for x(bi) (U*x=y)
- ColumnVector x1(length);
- ColumnVector x2(length);
- x1 = 0;
- x2 = 0;
+ // solve for x(bi) (U*x=y)
+ ColumnVector x1(length);
+ ColumnVector x2(length);
+ x1 = 0;
+ x2 = 0;
- x1(length) = y1(length)/U(length,length);
- x2(length) = y2(length)/U(length,length);
+ x1(length) = y1(length)/U(length,length);
+ x2(length) = y2(length)/U(length,length);
- compute1 = false;
- if(y1(length)!=0) compute1 = true;
- compute2 = false;
- if(y2(length)!=0) compute2 = true;
+ compute1 = false;
+ if(y1(length)!=0) compute1 = true;
+ compute2 = false;
+ if(y2(length)!=0) compute2 = true;
- for(int r = length; r>=bi; r--)
+ for(int r = length; r>=bi; r--)
{
if(!compute1 && y1(r)!=0) compute1 = true;
if(!compute2 && y2(r)!=0) compute2 = true;
if(compute1 || compute2)
- {
- float sum1 = 0.0;
- float sum2 = 0.0;
- const SparseMatrix::Row& row = U.row(r);
- for(SparseMatrix::Row::const_iterator it=row.lower_bound(r);it!=row.end();it++)
+ {
+ float sum1 = 0.0;
+ float sum2 = 0.0;
+ const SparseMatrix::Row& row = U.row(r);
+ for(SparseMatrix::Row::const_iterator it=row.lower_bound(r);it!=row.end();it++)
{
int c = (*it).first+1;
@@ -338,14 +338,14 @@ namespace MISCMATHS {
if(compute2) sum2 += val*x2(c);
}
- if(compute1) x1(r) = (y1(r)-sum1)/U(r,r);
- if(compute2) x2(r) = (y2(r)-sum2)/U(r,r);
- }
+ if(compute1) x1(r) = (y1(r)-sum1)/U(r,r);
+ if(compute2) x2(r) = (y2(r)-sum2)/U(r,r);
+ }
}
- tr1 += x1(bi);
- tr2 += x2(bi);
- }
+ tr1 += x1(bi);
+ tr2 += x2(bi);
+ }
}
@@ -361,25 +361,25 @@ namespace MISCMATHS {
// assumes symmetric A and b
for(int r = every; r<=A.Ncols(); r+=every)
- {
-// cout << float(r)/A.Ncols() << "\r";
-// cout.flush();
+ {
+ // cout << float(r)/A.Ncols() << "\r";
+ // cout.flush();
- ColumnVector br = b.RowAsColumn(r);
- ColumnVector xr = x.RowAsColumn(r);
+ ColumnVector br = b.RowAsColumn(r);
+ ColumnVector xr = x.RowAsColumn(r);
- solveforx(A,br,xr,tol);
+ solveforx(A,br,xr,tol);
- for(int c = 1; c<=b.Ncols(); c++)
+ for(int c = 1; c<=b.Ncols(); c++)
{
if(xr(c)!=0)
- {
- x.set(r,c,xr(c));
- }
+ {
+ x.set(r,c,xr(c));
+ }
}
- tr += xr(r);
- }
+ tr += xr(r);
+ }
cout << endl;
@@ -394,23 +394,23 @@ namespace MISCMATHS {
// assumes symmetric A and b
for(int r = 1; r<=A.Ncols(); r++)
- {
- cout << float(r)/A.Ncols() << "\r";
- cout.flush();
+ {
+ cout << float(r)/A.Ncols() << "\r";
+ cout.flush();
- ColumnVector br = b.RowAsColumn(r);
- ColumnVector xr = x.RowAsColumn(r);
+ ColumnVector br = b.RowAsColumn(r);
+ ColumnVector xr = x.RowAsColumn(r);
- solveforx(A,br,xr);
+ solveforx(A,br,xr);
- for(int c = 1; c<=b.Ncols(); c++)
+ for(int c = 1; c<=b.Ncols(); c++)
{
if(xr(c)!=0)
- {
- x.set(r,c,xr(c));
- }
+ {
+ x.set(r,c,xr(c));
+ }
}
- }
+ }
cout << endl;
}
@@ -422,24 +422,24 @@ namespace MISCMATHS {
Tracer_Plus trace("sparsefns::solveforx");
if(norm2(b)==0)
- {
- x = 0;
- }
+ {
+ x = 0;
+ }
else
- {
- int k = 2;
- kmax = Max(b.Nrows(),kmax);
+ {
+ int k = 2;
+ kmax = Max(b.Nrows(),kmax);
- ColumnVector tmp;
- multiply(A,x,tmp);
+ ColumnVector tmp;
+ multiply(A,x,tmp);
- ColumnVector r = b-tmp;
- ColumnVector rho(kmax);
- rho = Sqr(norm2(r));
- ColumnVector w;
- ColumnVector p = r;
+ ColumnVector r = b-tmp;
+ ColumnVector rho(kmax);
+ rho = Sqr(norm2(r));
+ ColumnVector w;
+ ColumnVector p = r;
- while(std::sqrt(rho(k))>tol*norm2(b) && k < kmax)
+ while(std::sqrt(rho(k))>tol*norm2(b) && k < kmax)
{
k++;
//if(k>2)
@@ -465,7 +465,7 @@ namespace MISCMATHS {
}
- if(k>kmax/2.0)
+ if(k>kmax/2.0)
{
OUT(std::sqrt(rho(k-1)));
OUT(norm2(b));
@@ -473,7 +473,7 @@ namespace MISCMATHS {
cout.flush();
}
- }
+ }
// write_ascii_matrix("rho",rho);
@@ -495,27 +495,27 @@ namespace MISCMATHS {
if(b(1)!=0) compute = true;
for(int r = 2; r<=length; r++)
- {
- if(!compute && b(r)!=0) compute = true;
+ {
+ if(!compute && b(r)!=0) compute = true;
- if(compute)
+ if(compute)
{
float sum = 0.0;
const SparseMatrix::Row& row = L.row(r);
for(SparseMatrix::Row::const_iterator it=row.begin();it!=row.end();it++)
- {
- int c = (*it).first+1;
+ {
+ int c = (*it).first+1;
- if(c > r-1) break;
+ if(c > r-1) break;
- double val = (*it).second;
- sum += val*y(c);
+ double val = (*it).second;
+ sum += val*y(c);
- }
+ }
y(r) = (b(r)-sum)/L(r,r);
}
- }
+ }
// solve for x (U*x=y)
x(length) = y(length)/U(length,length);
@@ -523,24 +523,24 @@ namespace MISCMATHS {
if(y(length)!=0) compute = true;
for(int r = length; r>=1; r--)
- {
- if(!compute && y(r)!=0) compute = true;
+ {
+ if(!compute && y(r)!=0) compute = true;
- if(compute)
+ if(compute)
{
float sum = 0.0;
const SparseMatrix::Row& row = U.row(r);
for(SparseMatrix::Row::const_iterator it=row.lower_bound(r);it!=row.end();it++)
- {
- int c = (*it).first+1;
+ {
+ int c = (*it).first+1;
- double val = (*it).second;
- sum += val*x(c);
- }
+ double val = (*it).second;
+ sum += val*x(c);
+ }
x(r) = (y(r)-sum)/U(r,r);
}
- }
+ }
}
@@ -557,23 +557,23 @@ namespace MISCMATHS {
x.ReSize(length,b.Ncols());
for(int bi=1;bi<=b.Ncols();bi++)
- {
- // solve for y (L*y=b)
- ColumnVector y(length);
- y = 0;
- y(1) = b(1,bi)/L(1,1);
- bool compute = false;
- if(b(1,bi)!=0) compute = true;
-
- for(int r = 2; r<=length; r++)
+ {
+ // solve for y (L*y=b)
+ ColumnVector y(length);
+ y = 0;
+ y(1) = b(1,bi)/L(1,1);
+ bool compute = false;
+ if(b(1,bi)!=0) compute = true;
+
+ for(int r = 2; r<=length; r++)
{
if(!compute && b(r,bi)!=0) compute = true;
if(compute)
- {
- float sum = 0.0;
- SparseMatrix::Row& row = L.row(r);
- for(SparseMatrix::Row::iterator it=row.begin();it!=row.end();it++)
+ {
+ float sum = 0.0;
+ SparseMatrix::Row& row = L.row(r);
+ for(SparseMatrix::Row::iterator it=row.begin();it!=row.end();it++)
{
int c = (*it).first+1;
@@ -584,24 +584,24 @@ namespace MISCMATHS {
}
- y(r) = (b(r,bi)-sum)/L(r,r);
- }
+ y(r) = (b(r,bi)-sum)/L(r,r);
+ }
}
- // solve for x (U*x=y)
- x.set(length,bi,y(length)/U(length,length));
- compute = false;
- if(y(length)!=0) compute = true;
+ // solve for x (U*x=y)
+ x.set(length,bi,y(length)/U(length,length));
+ compute = false;
+ if(y(length)!=0) compute = true;
- for(int r = length; r>=1; r--)
+ for(int r = length; r>=1; r--)
{
if(!compute && y(r)!=0) compute = true;
if(compute)
- {
- float sum = 0.0;
- SparseMatrix::Row& row = U.row(r);
- for(SparseMatrix::Row::iterator it=row.lower_bound(r);it!=row.end();it++)
+ {
+ float sum = 0.0;
+ SparseMatrix::Row& row = U.row(r);
+ for(SparseMatrix::Row::iterator it=row.lower_bound(r);it!=row.end();it++)
{
int c = (*it).first+1;
@@ -609,10 +609,10 @@ namespace MISCMATHS {
sum += val*x(c,bi);
}
- x.set(r,bi,(y(r)-sum)/U(r,r));
- }
+ x.set(r,bi,(y(r)-sum)/U(r,r));
+ }
}
- }
+ }
}
@@ -623,22 +623,22 @@ namespace MISCMATHS {
ret.ReSize(A.Nrows(),A.Nrows());
for(int r=1; r <= A.Nrows(); r++)
- {
- // diagonal
- if(A(r) != 0)
+ {
+ // diagonal
+ if(A(r) != 0)
{
ret.set(r,r,Sqr(A(r)));
// off-diagonal
for(int c=r+1; c <= A.Nrows(); c++)
- {
- if(A(c) != 0)
+ {
+ if(A(c) != 0)
{
ret.set(r,c,A(r)*A(c));
ret.set(c,r,A(r)*A(c));
}
- }
+ }
}
- }
+ }
}
}