void eigen_decomposition(const double A[n][n], double V[n][n], double d[n]) { double e[n]; memcpy((double*)V, (const double*)A, n*n*sizeof(double)); tred2(V, d, e); tql2(V, d, e); }
int maxentmc_symmeig(size_t const n, maxentmc_float_t * const A, size_t const tda, maxentmc_float_t * const eigval) { if(n==0 || tda==0){ MAXENTMC_MESSAGE(stderr,"error: zero matrix size"); return -1; } MAXENTMC_CHECK_NULL(A); MAXENTMC_CHECK_NULL(eigval); maxentmc_float_t temp[n]; tred2(n, A, tda, eigval, temp); int status = tqli(n, A, tda, eigval, temp); if(status){ MAXENTMC_MESSAGE(stderr,"error: too many iterations in tqli"); } else eigsrt(n, A, tda, eigval); return status; }
int eigen_sym_core(double *mat, int n, double *eval) { double **a; int i, j; double *off_diag; a = (double **) malloc(n * sizeof(double*)); for (i = 0; i < n; i++) a[i] = (double *) calloc(n, sizeof(double)); off_diag = (double *) malloc(n * sizeof(double*)); for (i = 0; i < n; i++) for (j = 0; j < n; j++) a[i][j] = mat[i*n+j]; /* make this matrix tridiagonal */ tred2(a, n, eval, off_diag); /* compute eigenvalues and eigenvectors */ tqli(eval, off_diag, n, a); free(off_diag); for (i = n-1; i >= 0; i--) free(a[i]); free(a); return 0; }
//--------------------------------------------------------------------------- // DiagonalizeMatrix // // Diagonalize a real, symmetric matrix of dimension [1...inDim] by // computing the eigenvalues and eigenvectors. // // Input matrix is destroyed by this routine. // // Eigenvalues are sorted in descending order, and stored in output // array; corresponding Eigenvectors are stored in an output matrix. // Output arrays must be allocated to the proper dimension prior to // calling this routine, arrays are addressed using NR convention // [1...N]. // // This procedure uses either Jacobi rotation or QL reduction to // compute the eigenvalues/eigenvectors. Comment out the appropriate // section of code for the un-favored algorithm. // // plk 3/10/2005 //--------------------------------------------------------------------------- void DiagonalizeFMatrix(float **inMatrix, \ int inDim, \ float *outEigenValue, \ float **outEigenVector) { int nrot; float *theE; //----------------------------------------------------------- // Jacobi rotation algorithm // //jacobi(inMatrix,inDim,outEigenValue,outEigenVector,&nrot); //eigsrt(outEigenValue,outEigenVector,inDim); //----------------------------------------------------------- //----------------------------------------------------------- // QL reduction algorithm // theE = vector(1,inDim); tred2(inMatrix, inDim, outEigenValue, theE); tqli(outEigenValue,theE,inDim,inMatrix); CopyFMatrix(inMatrix,outEigenVector,\ 1,inDim,\ 1,inDim); //---------------------------------------------------------- }
int principal_stresses(StressTensor* stress, double d[3], double V[3][3]) { int i,j; double e[3]; void tred2(int n, double d[3], double e[3], double V[3][3]); void tql2 (int n, double d[3], double e[3], double V[3][3]); for (i = 0; i < 3; i++) { for (j = 0; j < 3; j++) { V[i][j] = (*stress)[i][j]; } } // Tridiagonalize. tred2(3, d, e, V); // Diagonalize. tql2(3, d, e, V); /* fprintf(stderr,"eig 1: %e eigV: %e %e %e\n",d[0],V[0][0],V[1][0],V[2][0]); */ /* fprintf(stderr,"eig 2: %e eigV: %e %e %e\n",d[1],V[0][1],V[1][1],V[2][1]); */ /* fprintf(stderr,"eig 3: %e eigV: %e %e %e\n",d[2],V[0][2],V[1][2],V[2][2]); */ return(1); }
/* \brief Finds the largest eigenvector of a symmetric matrix * * Uses Householder reduction (code taken from Numerical Recipes in C) * * \params matrix * Matrix pointer (content is modified !) * \params n * Matrix dimension * \params e * Largest eigenvector */ void largestEigenvector(double *matrix, int n, double *eigenvector) { /* <!> *$€%+§&@ convention of Numerical Recipes to begin index by 1 */ /* Copy matrix into n*n table */ int i; double **a, *iM; double d[n+1]; /* diagonal elements */ double e[n+1]; /* off-diagonal elements */ double max; /* maximum eigenvalue */ int iMax; /* index of maximum eigenvector */ /* Initialize a and z */ a = malloc((n+1)*sizeof(double *)); iM = matrix; for (i = 1 ; i <= n ; i++) { a[i] = malloc((n+1)*sizeof(double)); memcpy(a[i]+1, iM, n*sizeof(double)); iM += n; } /* Householder reduction */ tred2(a, n, d, e); /* QL algorithm */ tqli(d, e, n, a); /* Search eigenvector of maximum eigenvalue */ max = 0; iMax = 1; for (i = 1 ; i <= n ; i++) { if (d[i] > max) { max = d[i]; iMax = i; } } /* Return maximum eigenvector */ max = 0; for (i = 0 ; i < n ; i++) { eigenvector[i] = *(a[i+1] + iMax); max += SQR(eigenvector[i]); } max = sqrt(max); for (i = 0 ; i < n ; i++) eigenvector[i] /= sqrt(max); /* release space */ for (i = 1 ; i <= n ; i++) { free(a[i]); } free(a); }
int symm_eigen(double *vals, double **A, const int n) { ////////////////////////////////////////////////////////////////// // it is users' responsibility to make sure that A is symmetric //////////////////////////////////////////////////////////////////// Vector<double> e(n); tred2(A,n,vals,e.begin()); tqli(vals,e.begin(),n,A); return 0; }
void eigen_decomposition(float A[n][n], float V[n][n], float d[n]) { float e[n]; for (int i = 0; i < n; i++) { for (int j = 0; j < n; j++) { V[i][j] = A[i][j]; } } tred2(V, d, e); tql2(V, d, e); }
void eigen_decomposition(double A[n][n], double V[n][n], double d[n]) { double e[n]; for (int i = 0; i < n; i++) { for (int j = 0; j < n; j++) { V[i][j] = A[i][j]; } } tred2(V, d, e); tql2(V, d, e); }
int main(void) { int i,j,k,l,m; float *d,*e,**a,**f; static float c[NP][NP]={ 5.0, 4.3, 3.0, 2.0, 1.0, 0.0,-1.0,-2.0,-3.0,-4.0, 4.3, 5.1, 4.0, 3.0, 2.0, 1.0, 0.0,-1.0,-2.0,-3.0, 3.0, 4.0, 5.0, 4.0, 3.0, 2.0, 1.0, 0.0,-1.0,-2.0, 2.0, 3.0, 4.0, 5.0, 4.0, 3.0, 2.0, 1.0, 0.0,-1.0, 1.0, 2.0, 3.0, 4.0, 5.0, 4.0, 3.0, 2.0, 1.0, 0.0, 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 4.0, 3.0, 2.0, 1.0, -1.0, 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 4.0, 3.0, 2.0, -2.0,-1.0, 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 4.0, 3.0, -3.0,-2.0,-1.0, 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 4.0, -4.0,-3.0,-2.0,-1.0, 0.0, 1.0, 2.0, 3.0, 4.0, 5.0}; d=vector(1,NP); e=vector(1,NP); a=matrix(1,NP,1,NP); f=matrix(1,NP,1,NP); for (i=1;i<=NP;i++) for (j=1;j<=NP;j++) a[i][j]=c[i-1][j-1]; tred2(a,NP,d,e); printf("diagonal elements\n"); for (i=1;i<=NP;i++) { printf("%12.6f",d[i]); if ((i % 5) == 0) printf("\n"); } printf("off-diagonal elements\n"); for (i=2;i<=NP;i++) { printf("%12.6f",e[i]); if ((i % 5) == 0) printf("\n"); } /* Check transformation matrix */ for (j=1;j<=NP;j++) { for (k=1;k<=NP;k++) { f[j][k]=0.0; for (l=1;l<=NP;l++) { for (m=1;m<=NP;m++) f[j][k] += a[l][j]*c[l-1][m-1]*a[m][k]; } } } /* How does it look? */ printf("tridiagonal matrix\n"); for (i=1;i<=NP;i++) { for (j=1;j<=NP;j++) printf("%7.2f",f[i][j]); printf("\n"); } free_matrix(f,1,NP,1,NP); free_matrix(a,1,NP,1,NP); free_vector(e,1,NP); free_vector(d,1,NP); return 0; }
/* * Argument a contains pointers to the rows of a symmetrix matrix. The * in each row is the row number + 1. These rows are stored in * contiguous memory starting with 0. Evecs also contains pointers to * contiguous memory. N is the dimension. */ void cmat_diag(double**a, double*evals, double**evecs, int n, int matz, double tol) { int i,j; int diagonal=1; double*fv1; /* I'm having problems with diagonalizing matrices which are already * diagonal. So let's first check to see if _a_ is diagonal, and if it * is, then just return the diagonal elements in evals and a unit matrix * in evecs */ for (i=1; i < n; i++) { for (j=0; j < i; j++) { if (fabs(a[i][j]) > tol) diagonal=0; } } if (diagonal) { for(i=0; i < n; i++) { evals[i] = a[i][i]; evecs[i][i] = 1.0; for(j=0; j < i; j++) { evecs[i][j] = evecs[j][i] = 0.0; } } eigsort(n,evals,evecs); return; } fv1 = (double*) malloc(sizeof(double)*n); if (!fv1) { fprintf(stderr,"cmat_diag: malloc fv1 failed\n"); abort(); } for(i=0; i < n; i++) { for(j=0; j <= i; j++) { evecs[i][j] = evecs[j][i] = a[i][j]; } } tred2(n,evecs,evals,fv1,1); cmat_transpose_square_matrix(evecs,n); tqli(n,evals,evecs,fv1,1,tol); cmat_transpose_square_matrix(evecs,n); eigsort(n,evals,evecs); free(fv1); }
void acados_eigen_decomposition(int dim, double *A, double *V, double *d, double *e) { int i, j; for (i=0; i<dim; i++) for (j=0; j<dim; j++) V[i*dim+j] = A[i*dim+j]; tred2(dim, V, d, e); tql2(dim, V, d, e); return; }
int main(void) { int i,j,k; float *d,*e,*f,**a; static float c[NP][NP]={ 5.0, 4.3, 3.0, 2.0, 1.0, 0.0,-1.0,-2.0,-3.0,-4.0, 4.3, 5.1, 4.0, 3.0, 2.0, 1.0, 0.0,-1.0,-2.0,-3.0, 3.0, 4.0, 5.0, 4.0, 3.0, 2.0, 1.0, 0.0,-1.0,-2.0, 2.0, 3.0, 4.0, 5.0, 4.0, 3.0, 2.0, 1.0, 0.0,-1.0, 1.0, 2.0, 3.0, 4.0, 5.0, 4.0, 3.0, 2.0, 1.0, 0.0, 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 4.0, 3.0, 2.0, 1.0, -1.0, 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 4.0, 3.0, 2.0, -2.0,-1.0, 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 4.0, 3.0, -3.0,-2.0,-1.0, 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 4.0, -4.0,-3.0,-2.0,-1.0, 0.0, 1.0, 2.0, 3.0, 4.0, 5.0}; d=vector(1,NP); e=vector(1,NP); f=vector(1,NP); a=matrix(1,NP,1,NP); for (i=1;i<=NP;i++) for (j=1;j<=NP;j++) a[i][j]=c[i-1][j-1]; tred2(a,NP,d,e); tqli(d,e,NP,a); printf("\nEigenvectors for a real symmetric matrix\n"); for (i=1;i<=NP;i++) { for (j=1;j<=NP;j++) { f[j]=0.0; for (k=1;k<=NP;k++) f[j] += (c[j-1][k-1]*a[k][i]); } printf("%s %3d %s %10.6f\n","eigenvalue",i," =",d[i]); printf("%11s %14s %9s\n","vector","mtrx*vect.","ratio"); for (j=1;j<=NP;j++) { if (fabs(a[j][i]) < TINY) printf("%12.6f %12.6f %12s\n", a[j][i],f[j],"div. by 0"); else printf("%12.6f %12.6f %12.6f\n", a[j][i],f[j],f[j]/a[j][i]); } printf("Press ENTER to continue...\n"); (void) getchar(); } free_matrix(a,1,NP,1,NP); free_vector(f,1,NP); free_vector(e,1,NP); free_vector(d,1,NP); return 0; }
static int eigenvectors(double (*m)[3], double (*evecs)[3], int dim) { double d[3], e[3]; int i, j, rc; for (i=0; i<3; i++){ for (j=0; j<3; j++){ evecs[i][j] = m[i][j]; } } tred2(evecs, dim, d, e); rc = tqli(d, dim, e, evecs); return rc; }
void eigen_decomposition(int dim, double *A, double *V, double *d) { int i, j; for (i=0; i<dim; i++) for (j=0; j<dim; j++) V[i*dim+j] = A[i*dim+j]; double *e = (double *) calloc(dim, sizeof(double)); tred2(dim, V, d, e); tql2(dim, V, d, e); free(e); return; }
void HOE(int n, double **o, long *iseed) /* RANDOM ORTOGONAL matrix - from O(N), */ /* by eigenvectors of a symmetric real matrix-GOE */ /* that is in turn obtained by P(H)\propto\exp{-Tr(H^2)/2}. */ { int i,j,t1,t2; double *d,*e,tmp; d=(double *)malloc(sizeof(double)*n); e=(double *)malloc(sizeof(double)*n); /* get GOE into o[][] */ GOE(n,o,1,iseed); /* shift indices of o[][] */ for(i=0;i<n;i++) o[i]--; /* eigenvalues of real symmetric matrix in a */ tred2(o-1,n,d-1,e-1,1); tqli(d-1,e-1,n,o-1,1); /* eigenvalues are in d[], eigenvec. in a[][] */ /* free memory - we don't need eigenvalues */ free(e); free(d); /* shift o[] back */ for(i=0;i<n;i++) o[i]++; return; /* finaly, just for sure, permute eigenvectors - eig. is in column */ /* n/2 of transpositions */ for(i=0;i<n/2;i++) { /* two random indices */ t1=rand() % n; t2=rand() % n; /* switch */ for(j=0;j<n;j++) { tmp=o[j][t1]; o[j][t1]=o[j][t2]; o[j][t2]=tmp; } } }
bool EigenvalueDecomposition::decompose(const MatrixFloat &a){ n = a.getNumCols(); eigenvectors.resize(n,n); realEigenvalues.resize(n); complexEigenvalues.resize(n); issymmetric = true; for(int j = 0; (j < n) & issymmetric; j++) { for(int i = 0; (i < n) & issymmetric; i++) { issymmetric = (a[i][j] == a[j][i]); } } if (issymmetric) { for(int i = 0; i < n; i++) { for(int j = 0; j < n; j++) { eigenvectors[i][j] = a[i][j]; } } // Tridiagonalize. tred2(); // Diagonalize. tql2(); } else { h.resize(n,n); ort.resize(n); for(int j = 0; j < n; j++) { for(int i = 0; i < n; i++) { h[i][j] = a[i][j]; } } // Reduce to Hessenberg form. orthes(); // Reduce Hessenberg to real Schur form. hqr2(); } return true; }
C_toolbox_eigen_sym::C_toolbox_eigen_sym(/**const*/ double* a, bool yesvec):C_toolbox() { n = 3; z = new double*[n]; z[0] = new double[n]; z[1] = new double[n]; z[2] = new double[n]; z[0][0] = a[0]; z[0][1] = a[1]; z[0][2] = a[2]; z[1][0] = a[1]; z[1][1] = a[3]; z[1][2] = a[4]; z[2][0] = a[2]; z[2][1] = a[4]; z[2][2] = a[5]; d = new double[n]; e = new double[n]; yesvecs = yesvec; tred2(); tqli(); sortEig(); }
C_toolbox_eigen_sym::C_toolbox_eigen_sym(/**const*/ double** a, const int nb_rows, bool yesvec):C_toolbox() { n = nb_rows; z = new double*[n]; for(int i=0; i<n ; i++) { z[i] = new double[n]; for(int j=0 ; j<n ; j++) { z[i][j] = a[i][j]; } } d = new double[n]; e = new double[n]; yesvecs = yesvec; tred2(); tqli(); sortEig(); }
/////////////////////////////////////////////////////////////////////////////// // // Computes m_eigenValues and eigenvectors of a real, symmetric // matrix. // // A matrix can be decomposed as A = U*D*U', where the eigenvalue matrix D is // diagonal and the eigenvector matrix U is orthogonal. That is, // the diagonal values of D are the m_eigenValues, and // U*U' = I, where I is the identity matrix. The columns of U // represent the eigenvectors in the sense that A*U = V*U. // // (Adapted from JAMA, a Java Matrix Library, developed by jointly // by the Mathworks and NIST; see http://math.nist.gov/javanumerics/jama). // // Modified by: Donovan Parks, August 2007 ([email protected]) /////////////////////////////////////////////////////////////////////////////// EigDecomp::EigDecomp(double* A, double* U, double* D, double* E, int rows) { m_rows = rows; m_U = U; m_D = D; m_E = E; for(int j = 0; j < m_rows; ++j) { for(int i = 0; i < m_rows; ++i) { m_U[i+j*m_rows] = A[i+j*m_rows]; } } // Tridiagonalize. tred2(); // Diagonalize. tql2(); }
void eigen_decomposition(double A[n][n], double V[n][n], double d[n]) { double e[n]; double da[3]; double dt, dat; double vet[3]; int i, j; for (i = 0; i < n; i++) { for (j = 0; j < n; j++) { V[i][j] = A[i][j]; } } tred2(V, d, e); tql2(V, d, e); /* Sort the eigen values and vectors by abs eigen value */ da[0]=absd(d[0]); da[1]=absd(d[1]); da[2]=absd(d[2]); if((da[0]>=da[1])&&(da[0]>da[2])) { dt=d[2]; dat=da[2]; vet[0]=V[0][2]; vet[1]=V[1][2]; vet[2]=V[2][2]; d[2]=d[0]; da[2]=da[0]; V[0][2] = V[0][0]; V[1][2] = V[1][0]; V[2][2] = V[2][0]; d[0]=dt; da[0]=dat; V[0][0] = vet[0]; V[1][0] = vet[1]; V[2][0] = vet[2]; } else if((da[1]>=da[0])&&(da[1]>da[2])) { dt=d[2]; dat=da[2]; vet[0]=V[0][2]; vet[1]=V[1][2]; vet[2]=V[2][2]; d[2]=d[1]; da[2]=da[1]; V[0][2] = V[0][1]; V[1][2] = V[1][1]; V[2][2] = V[2][1]; d[1]=dt; da[1]=dat; V[0][1] = vet[0]; V[1][1] = vet[1]; V[2][1] = vet[2]; } if(da[0]>da[1]) { dt=d[1]; dat=da[1]; vet[0]=V[0][1]; vet[1]=V[1][1]; vet[2]=V[2][1]; d[1]=d[0]; da[1]=da[0]; V[0][1] = V[0][0]; V[1][1] = V[1][0]; V[2][1] = V[2][0]; d[0]=dt; da[0]=dat; V[0][0] = vet[0]; V[1][0] = vet[1]; V[2][0] = vet[2]; } }
int eigen_sym(DMat20 H_mat, DVec20 Pi_vec, int num_state, DVec20 eval, DMat20 evec, DMat20 inv_evec) { DVec20 forg, new_forg, forg_sqrt, off_diag, eval_new; DMat20 b; int i, j, k, error, new_num, inew, jnew; double zero; double **a; a = (double **) malloc(num_state * sizeof(double*)); for (i = 0; i < num_state; i++) a[i] = (double*) (calloc(num_state, sizeof(double))); /* copy a to b */ for (i = 0; i < num_state; i++) for (j = 0; j < num_state; j++) { a[i][j] = H_mat[i][j] / Pi_vec[i]; b[i][j] = a[i][j]; } for (i = 0; i < num_state; i++) forg[i] = Pi_vec[i]; eliminateZero(b, forg, num_state, a, new_forg, &new_num); transformHMatrix(a, new_forg, forg_sqrt, new_num); /* make this matrix tridiagonal */ tred2(a, new_num, eval_new, off_diag); /* compute eigenvalues and eigenvectors */ tqli(eval_new, off_diag, new_num, a); /* now get back eigen */ for (i = num_state-1,inew = new_num-1; i >= 0; i--) eval[i] = (forg[i] > ZERO) ? eval_new[inew--] : 0; /* calculate the actual eigenvectors of H and its inverse matrix */ for (i = num_state-1,inew = new_num-1; i >= 0; i--) if (forg[i] > ZERO) { for (j = num_state-1, jnew = new_num-1; j >= 0; j--) if (forg[j] > ZERO) { evec[i][j] = a[inew][jnew] / forg_sqrt[inew]; inv_evec[i][j] = a[jnew][inew] * forg_sqrt[jnew]; jnew--; } else { evec[i][j] = (i == j); inv_evec[i][j] = (i == j); } inew--; } else for (j=0; j < num_state; j++) { evec[i][j] = (i==j); inv_evec[i][j] = (i==j); } /* printf("eigen_sym \n"); for (i = 0; i < num_state; i++) printf("%f ", eval[i]); printf("\n");*/ /* check eigenvalue equation */ error = 0; for (j = 0; j < num_state; j++) { for (i = 0, zero = 0.0; i < num_state; i++) { for (k = 0; k < num_state; k++) zero += b[i][k] * evec[k][j]; zero -= eval[j] * evec[i][j]; if (fabs(zero) > 1.0e-5) { error = 1; printf("zero = %f\n", zero); } } for (i = 0, zero = 0.0; i < num_state; i++) { for (k = 0; k < num_state; k++) zero += evec[i][k] * inv_evec[k][j]; if (i == j) zero -= 1.0; if (fabs(zero) > 1.0e-5) { error = 1; printf("zero = %f\n", zero); } } } for (i = num_state-1; i >= 0; i--) free(a[i]); free(a); if (error) { printf("\nWARNING: Eigensystem doesn't satisfy eigenvalue equation!\n"); return 1; } return 0; }
bool Plane::FitToPoints(const Vector<Vec4f> &Points, Vec3f &Basis1, Vec3f &Basis2, float &NormalEigenvalue, float &ResidualError) { Vec3f Centroid, Normal; float ScatterMatrix[3][3]; int Order[3]; float DiagonalMatrix[3]; float OffDiagonalMatrix[3]; // Find centroid Centroid = Vec3f::Origin; float TotalWeight = 0.0f; for(UINT i = 0; i < Points.Length(); i++) { TotalWeight += Points[i].w; Centroid += Vec3f(Points[i].x, Points[i].y, Points[i].z) * Points[i].w; } Centroid /= TotalWeight; // Compute scatter matrix Find_ScatterMatrix(Points, Centroid, ScatterMatrix, Order); tred2(ScatterMatrix,DiagonalMatrix,OffDiagonalMatrix); tqli(DiagonalMatrix,OffDiagonalMatrix,ScatterMatrix); /* ** Find the smallest eigenvalue first. */ float Min = DiagonalMatrix[0]; float Max = DiagonalMatrix[0]; UINT MinIndex = 0; UINT MiddleIndex = 0; UINT MaxIndex = 0; for(UINT i = 1; i < 3; i++) { if(DiagonalMatrix[i] < Min) { Min = DiagonalMatrix[i]; MinIndex = i; } if(DiagonalMatrix[i] > Max) { Max = DiagonalMatrix[i]; MaxIndex = i; } } for(UINT i = 0; i < 3; i++) { if(MinIndex != i && MaxIndex != i) { MiddleIndex = i; } } /* ** The normal of the plane is the smallest eigenvector. */ for(UINT i = 0; i < 3; i++) { Normal[Order[i]] = ScatterMatrix[i][MinIndex]; Basis1[Order[i]] = ScatterMatrix[i][MiddleIndex]; Basis2[Order[i]] = ScatterMatrix[i][MaxIndex]; } NormalEigenvalue = Math::Abs(DiagonalMatrix[MinIndex]); Basis1.SetLength(DiagonalMatrix[MiddleIndex]); Basis2.SetLength(DiagonalMatrix[MaxIndex]); if(!Basis1.Valid() || !Basis2.Valid() || !Normal.Valid()) { *this = ConstructFromPointNormal(Centroid, Vec3f::eX); Basis1 = Vec3f::eY; Basis2 = Vec3f::eZ; } else { *this = ConstructFromPointNormal(Centroid, Normal); } ResidualError = 0.0f; for(UINT i = 0; i < Points.Length(); i++) { ResidualError += UnsignedDistance(Vec3f(Points[i].x, Points[i].y, Points[i].z)); } ResidualError /= Points.Length(); return true; }
void EigenValues(const SymmetricMatrix& A, DiagonalMatrix& D, Matrix& Z) { DiagonalMatrix E; tred2(A, D, E, Z); tql2(D, E, Z); }
bool ClipPlane::fitPointsToPlane( const std::vector<cc::Vec4f>& points, cc::Vec3f& basis1, cc::Vec3f& basis2, float& normalEigenValue, float& residualError, ClipPlane& outPlane ) { // find centroid cc::Vec3f centroid(0.0f, 0.0f, 0.0f); float totalWeight = 0.0f; for( const auto& p : points ) { totalWeight += p.w; centroid += cc::Vec3f(p.x, p.y, p.z) * p.w; } centroid /= totalWeight; // compute scatter matrix float scatterMatrix[3][3]; int order[3]; findScatterMatrix(points, centroid, scatterMatrix, order); float diagonalMatrix[3]; float offDiagonalMatrix[3]; tred2(scatterMatrix, diagonalMatrix, offDiagonalMatrix); tqli(diagonalMatrix, offDiagonalMatrix, scatterMatrix); // find smallest eigenvalue first float min = diagonalMatrix[0]; float max = diagonalMatrix[0]; unsigned int minIndex = 0; unsigned int middleIndex = 0; unsigned int maxIndex = 0; for( unsigned int i = 1; i < 3; i++ ) { if( diagonalMatrix[i] < min ) { min = diagonalMatrix[i]; minIndex = i; } if( diagonalMatrix[i] > max ) { max = diagonalMatrix[i]; maxIndex = i; } } for( unsigned int i = 0; i < 3; i++ ) { if( minIndex != i && maxIndex != i ) { middleIndex = i; } } // the normal of the plane is the smallest eigenvector. cc::Vec3f normal; for( unsigned int i = 0; i < 3; i++ ) { normal[order[i]] = scatterMatrix[i][minIndex]; basis1[order[i]] = scatterMatrix[i][middleIndex]; basis2[order[i]] = scatterMatrix[i][maxIndex]; } normalEigenValue = fabsf(diagonalMatrix[minIndex]); setVec3Length(basis1, diagonalMatrix[middleIndex]); setVec3Length(basis2, diagonalMatrix[maxIndex]); if( !isVec3Valid(basis1) || !isVec3Valid(basis2) || !isVec3Valid(normal) ) { outPlane = ClipPlane::constructFromPointNormal(centroid, cc::Vec3f(1.0f, 0.0f, 0.0f)); basis1 = cc::Vec3f(0.0f, 1.0f, 0.0f); basis2 = cc::Vec3f(0.0f, 0.0f, 1.0f); } else { outPlane = ClipPlane::constructFromPointNormal(centroid, normal); } residualError = 0.0f; for( unsigned int i = 0; i < points.size(); i++ ) { residualError += outPlane.unsignedDistance(cc::Vec3f(points[i].x, points[i].y, points[i].z)); } residualError /= points.size(); return true; }
/* In place projection onto basis vectors */ void pca_project(double** data, int n, int m, int ncomponents) { int i, j, k, k2; double **symmat, /* **symmat2, */ *evals, *interm; //TODO: assert ncomponents < m symmat = (double**) malloc(m*sizeof(double*)); for (i = 0; i < m; i++) symmat[i] = (double*) malloc(m*sizeof(double)); covcol(data, n, m, symmat); /********************************************************************* Eigen-reduction **********************************************************************/ /* Allocate storage for dummy and new vectors. */ evals = (double*) malloc(m*sizeof(double)); /* Storage alloc. for vector of eigenvalues */ interm = (double*) malloc(m*sizeof(double)); /* Storage alloc. for 'intermediate' vector */ //MALLOC_ARRAY(symmat2,m,m,double); //for (i = 0; i < m; i++) { // for (j = 0; j < m; j++) { // symmat2[i][j] = symmat[i][j]; /* Needed below for col. projections */ // } //} tred2(symmat, m, evals, interm); /* Triangular decomposition */ tqli(evals, interm, m, symmat); /* Reduction of sym. trid. matrix */ /* evals now contains the eigenvalues, columns of symmat now contain the associated eigenvectors. */ /* printf("\nEigenvalues:\n"); for (j = m-1; j >= 0; j--) { printf("%18.5f\n", evals[j]); } printf("\n(Eigenvalues should be strictly positive; limited\n"); printf("precision machine arithmetic may affect this.\n"); printf("Eigenvalues are often expressed as cumulative\n"); printf("percentages, representing the 'percentage variance\n"); printf("explained' by the associated axis or principal component.)\n"); printf("\nEigenvectors:\n"); printf("(First three; their definition in terms of original vbes.)\n"); for (j = 0; j < m; j++) { for (i = 1; i <= 3; i++) { printf("%12.4f", symmat[j][m-i]); } printf("\n"); } */ /* Form projections of row-points on prin. components. */ /* Store in 'data', overwriting original data. */ for (i = 0; i < n; i++) { for (j = 0; j < m; j++) { interm[j] = data[i][j]; } /* data[i][j] will be overwritten */ for (k = 0; k < ncomponents; k++) { data[i][k] = 0.0; for (k2 = 0; k2 < m; k2++) { data[i][k] += interm[k2] * symmat[k2][m-k-1]; } } } /* printf("\nProjections of row-points on first 3 prin. comps.:\n"); for (i = 0; i < n; i++) { for (j = 0; j < 3; j++) { printf("%12.4f", data[i][j]); } printf("\n"); } */ /* Form projections of col.-points on first three prin. components. */ /* Store in 'symmat2', overwriting what was stored in this. */ //for (j = 0; j < m; j++) { // for (k = 0; k < m; k++) { // interm[k] = symmat2[j][k]; } /*symmat2[j][k] will be overwritten*/ // for (i = 0; i < 3; i++) { // symmat2[j][i] = 0.0; // for (k2 = 0; k2 < m; k2++) { // symmat2[j][i] += interm[k2] * symmat[k2][m-i-1]; } // if (evals[m-i-1] > 0.0005) /* Guard against zero eigenvalue */ // symmat2[j][i] /= sqrt(evals[m-i-1]); /* Rescale */ // else // symmat2[j][i] = 0.0; /* Standard kludge */ // } // } /* printf("\nProjections of column-points on first 3 prin. comps.:\n"); for (j = 0; j < m; j++) { for (k = 0; k < 3; k++) { printf("%12.4f", symmat2[j][k]); } printf("\n"); } */ for (i = 0; i < m; i++) free(symmat[i]); free(symmat); //FREE_ARRAY(symmat2,m); free(evals); free(interm); }
gint cmds (array_d *D, array_d *X) { gint N = D->nrows; /* D_ij^2 = ||X_i - x_j||^2 */ gdouble C = 0; /* C = 2/N sum_ij (D_ij^2) */ array_d c; gint i, j, ncols; gdouble sum; /* c_ij = <x_i, x_j> */ gdouble *d = g_malloc (N * sizeof (gdouble)); gdouble *e = g_malloc (N * sizeof (gdouble)); arrayd_init_null (&c); arrayd_alloc (&c, N, N); /* diagonal and off-diagonal elements of tridiagonalisation of c (e[0]=0) */ /* compute constant C */ for (i = 0; i < N; i++) { for (j = 0; j < N; j++) C += sqr(D->vals[i][j]); } C /= (gdouble) (N*N); /* recover scalar products from distances */ /* diagonal elements */ for (i = 0; i < N; i++) { sum = 0; for (j = 0; j < N; j++) sum += sqr(D->vals[i][j]); c.vals[i][i] = sum/N - .5*C; } /* upper triangle */ for (i = 0; i < N; i++) { for (j = i+1; j < N; j++) { c.vals[j][i] = c.vals[i][j] = -.5*(sqr(D->vals[i][j]) - c.vals[i][i] - c.vals[j][j]); } } /* printf("matrix of scalar products:"); printX(c,N,1); printf("\ntrace of c: %.2f\n",tr(c,N)); */ /* calculate eigenvectors and eigenvalues */ /* tridiagonalize c into d (diagonal) and e (off-diagonal) */ tred2(c.vals,N,d,e); g_printerr ("through tred2\n"); /* printf("diagonal:\n"); for (i = 0; i < N; i++) { printf("%.2f ",d[i]); } printf("\noff-diagonal:"); for (i = 1; i < N; i++) { printf("%.2f ",e[i]); } printf("\n"); */ /* * Tridiagonal QL Implicit: calculate eigenvectors and -values from * a tridiagonal matrix */ tqli (d, e, N, c.vals); g_printerr ("through tqli\n"); eigensort (c.vals, d, N); g_printerr ("through eigensort\n"); sum = 0; for (i = 0; i < N; i++) { sum += d[i]; } g_printerr ("trace of c after decomposition: %.2f\n",sum); /* printf("\nX:\n"); recoverX (c.vals, d, N); printX(c,N,1); */ ncols = MIN (N, X->ncols); for (i = 0; i < N; i++) { for (j = 0; j < ncols; j++) { if (d[j] > 0) X->vals[i][j] = c.vals[i][j] * sqrt(d[j]); else X->vals[i][j] = 0; } } arrayd_free (&c, 0, 0); g_free (d); g_free (e); return 0; }
VOID eispack_rs P9C(int, nm, int, n, double *, a, double *, w, int, matz, double *, z, double *, fv1, double *, fv2, int *, ierr) { /* this subroutine calls the recommended sequence of */ /* subroutines from the eigensystem subroutine package (eispack) */ /* to find the eigenvalues and eigenvectors (if desired) */ /* of a real symmetric matrix. */ /* on input */ /* nm must be set to the row dimension of the two-dimensional */ /* array parameters as declared in the calling program */ /* dimension statement. */ /* n is the order of the matrix a. */ /* a contains the real symmetric matrix. */ /* matz is an integer variable set equal to zero if */ /* only eigenvalues are desired. otherwise it is set to */ /* any non-zero integer for both eigenvalues and eigenvectors. */ /* on output */ /* w contains the eigenvalues in ascending order. */ /* z contains the eigenvectors if matz is not zero. */ /* ierr is an integer output variable set equal to an error */ /* completion code described in the documentation for tqlrat */ /* and tql2. the normal completion code is zero. */ /* fv1 and fv2 are temporary storage arrays. */ /* questions and comments should be directed to burton s. garbow, */ /* mathematics and computer science div, argonne national laboratory */ /* this version dated august 1983. */ /* ------------------------------------------------------------------ */ if (n <= nm) { goto L10; } *ierr = n * 10; goto L50; L10: if (matz != 0) { goto L20; } /* .......... find eigenvalues only .......... */ tred1(nm, n, a, w, fv1, fv2); /* tqlrat encounters catastrophic underflow on the Vax */ /* call tqlrat(n,w,fv2,ierr) */ tql1(n, w, fv1, ierr); goto L50; /* .......... find both eigenvalues and eigenvectors .......... */ L20: tred2(nm, n, a, w, fv1, z); tql2(nm, n, w, fv1, z, ierr); L50: return; }