// I had to move it here because it depends on AtomicPair class which is defined after LaueSymmetry vector<AtomicPair> LaueSymmetry::multiply_pairs_by_matrix(vector<AtomicPair> pairs,mat3<double> transformation_matrix) { vector<AtomicPair>::iterator pair; for(pair=pairs.begin(); pair!=pairs.end(); pair++) { for(int average=0; average<2; average++) { pair->r(average)=transformation_matrix*pair->r(average); pair->U(average)=trusted_mat_to_sym_mat(transformation_matrix*pair->U(average)*transformation_matrix.transpose()); } } return pairs; }
vec3 operator*(const vec3 &v, const mat3 &a) { return a.transpose() * v; }
vec2 operator * (const vec2& v, mat3& a) { return a.transpose() * v; }
void SVD::dec(mat3& Cov, mat3& U, mat3& Vt, vec3& S) { S = vec3(0.0); mat3 B = Cov; double e[3]; double work[3]; int i = 0, j = 0, k = 0; for( k = 0; k < 2; k ++) { for( i = k; i < 3; i ++) S[k] = hypot( S[k], B[i][k]); if(S[k] != 0) { if(B[k][k] < 0) S[k] = -S[k]; for(i = k; i < 3; i ++) B[i][k] /= S[k]; B[k][k] += 1; } S[k] =-S[k]; for( j = k+1; j < 3; j++) { if(S[k] != 0) { double t = 0; for( i = k; i < 3; i ++) t += B[i][k] * B[i][j]; t = -t / B[k][k]; for( i = k; i < 3; i ++) B[i][j] += t * B[i][k]; } e[j] = B[k][j]; } for( i = k; i < 3; ++i ) U[i][k] = B[i][k]; if( k < 1) { e[k] = 0; for( i = k+1; i < 3; ++ i ) e[k] = hypot( e[k], e[i] ); if( e[k] != 0 ) { if( e[k+1] < 0 ) e[k] = -e[k]; for( i=k+1; i<3; ++i ) e[i] /= e[k]; e[k+1] += 1; } e[k] = -e[k]; if( e[k] != 0 ) { for( i = k+1; i < 3; ++ i ) work[i] = 0; for( j = k+1; j < 3; ++ j ) for( i = k+1; i < 3; ++ i ) work[i] += e[j] *B[i][j]; for( j = k+1; j < 3; ++ j ){ double t = -e[j] / e[k+1]; for( i = k+1; i < 3; ++ i ) B[i][j] += t * work[i]; } } for( i = k+1; i < 3; ++ i ) Vt[i][k] = e[i]; } } S[2] = B[2][2]; e[1] = B[1][2]; e[2] = 0; for( i = 0; i < 3; ++ i ) U[i][2] = 0; U[2][2] = 1; for( k = 1; k >= 0; -- k ){ if( S[k] != 0 ) { for( j = k+1; j < 3; ++ j ){ double t = 0; for( i = k; i < 3; ++ i ) t += U[i][k] * U[i][j]; t = -t / U[k][k]; for( i = k; i < 3; ++ i ) U[i][j] += t * U[i][k]; } for( i = k; i < 3; ++ i ) U[i][k] = -U[i][k]; U[k][k] += 1 ; for( i = 0; i < k-1; ++ i ) U[i][k] = 0; } else { for( i = 0; i < 3; ++ i ) U[i][k] = 0; U[k][k] = 1; } } for( k = 2; k >= 0; -- k ){ if( (k < 1) && ( e[k] != 0 ) ) for( j = k+1; j < 3; ++ j ) { double t = 0; for( i = k+1; i < 3; ++ i ) t += Vt[i][k] * Vt[i][j]; t = -t / Vt[k+1][k]; for( i = k+1; i < 3; ++ i ) Vt[i][j] += t * Vt[i][k]; } for( i = 0; i < 3; ++ i ) Vt[i][k]=0; Vt[k][k] = 1; } int p = 3; int pp = p-1; int iter = 0; double eps = pow( 2.0, -52.0 ); while( p > 0 ) { k = 0; int kase = 0; for( k = p-2; k >= -1; -- k ) { if( k == -1 )break; if( abs(e[k]) <= eps*( abs(S[k]) + abs(S[k+1]) ) ) { e[k] = 0; break; } } if( k == p-2 ) kase = 4; else { int ks; for( ks = p-1; ks >= k; -- ks ) { if( ks == k )break; double t = ( (ks != p) ? abs(e[ks]) : 0 ) + ( (ks != k+1) ? abs(e[ks-1]) : 0 ); if( abs(S[ks]) <= eps*t ) { S[ks] = 0; break; } } if( ks == k ) kase = 3; else if( ks == p-1 ) kase = 1; else { kase = 2; k = ks; } } k++; switch( kase ) { case 1: { double f = e[p-2]; e[p-2] = 0; for( j = p-2; j >= k; -- j ) { double t = hypot( S[j], f ); double cs = S[j] / t; double sn = f / t; S[j] = t; if( j != k ) { f = -sn * e[j-1]; e[j-1] = cs * e[j-1]; } for( i = 0; i < 3; ++ i ) { t = cs*Vt[i][j] + sn*Vt[i][p-1]; Vt[i][p-1] = -sn*Vt[i][j] + cs*Vt[i][p-1]; Vt[i][j] = t; } } } break; case 2: { double f = e[k-1]; e[k-1] = 0; for( j=k; j<p; ++j ) { double t = hypot( S[j], f ); double cs = S[j] / t; double sn = f / t; S[j] = t; f = -sn * e[j]; e[j] = cs * e[j]; for( i=0; i<3; ++i ) { t = cs*U[i][j]+ sn*U[i][k-1]; U[i][k-1] = -sn*U[i][j] + cs*U[i][k-1]; U[i][j] = t; } } } break; case 3: { double scale = max( max( max( max(abs(S[p-1]), abs(S[p-2]) ), abs(e[p-2]) ),abs(S[k,k]) ), abs(e[k]) ); double sp = S[p-1] / scale; double spm1 = S[p-2] / scale; double epm1 = e[p-2] / scale; double sk = S[k] / scale; double ek = e[k] / scale; double b = ( (spm1+sp)*(spm1-sp) + epm1*epm1 ) / 2.0; double c = (sp*epm1) * (sp*epm1); double shift = 0; if( ( b != 0 ) || ( c != 0 ) ) { shift = sqrt( b*b+c ); if( b < 0 ) shift = -shift; shift = c / ( b+shift ); } double f = (sk+sp)*(sk-sp) + shift; double g = sk * ek; // chase zeros for( j=k; j<p-1; ++j ) { double t = hypot( f, g ); double cs = f / t; double sn = g / t; if( j != k )e[j-1] = t; f = cs*S[j] + sn*e[j]; e[j] = cs*e[j] - sn*S[j]; g = sn * S[j+1]; S[j+1] = cs * S[j+1]; for( i=0; i<3; ++i ) { t = cs*Vt[i][j]+ sn*Vt[i][j+1]; Vt[i][j+1] = -sn*Vt[i][j] + cs*Vt[i][j+1]; Vt[i][j]= t; } t = hypot( f, g ); cs = f / t; sn = g / t; S[j] = t; f = cs*e[j] + sn*S[j+1]; S[j+1] = -sn*e[j] + cs*S[j+1]; g = sn * e[j+1]; e[j+1] = cs * e[j+1]; for( i = 0; i < 3; ++ i ) { t = cs*U[i][j] + sn*U[i][j+1] ; U[i][j+1]= -sn*U[i][j] + cs*U[i][j+1]; U[i][j] = t; } } e[p-2] = f; iter = iter + 1; } break; // convergence case 4: { if( S[k] <= 0 ) { S[k] = ( S[k] < 0 ) ? -S[k] : 0; for( i = 0; i <= pp; ++ i ) Vt[i][k]= -Vt[i][k]; } /*while( k < pp ) { if( S[k] >= S[k+1] )break; double t = S[k]; S[k] = S[k+1]; S[k+1] = t; if(k < 2 ) for( i = 0; i < 3; ++ i ) swap( Vt[i][k], Vt[i][k+1]); if( k < 2 ) for( i = 0; i < 3; ++ i ) swap( U[i][k], U[i][k+1] ); k ++; }*/ iter = 0; p --; } break; } } Vt = Vt.transpose(); }
void PolarDecomposer::PolarDecompose(const mat3& A, mat3& Q, mat3& S, double& det, double tolerance) { mat3 At = A.transpose(); mat3 Aadj; mat3 Ek; double A_one = norm_one(At); double A_inf = norm_inf(At); double Aadj_one, Aadj_inf, E_one, gamma, g1, g2; do { Aadj = mat3(At[1].Cross(At[2]), At[2].Cross(At[0]), At[0].Cross(At[1])); det = At[0][0] * Aadj[0][0] + At[0][1] * Aadj[0][1] + At[0][2] * Aadj[0][2]; if(det == 0.0) { //TODO: handle this case printf("Warning: zero determinant encountered.\n"); break; } Aadj_one = norm_one(Aadj); Aadj_inf = norm_inf(Aadj); gamma = sqrt(sqrt((Aadj_one*Aadj_inf)/(A_one*A_inf))/fabs(det)); g1 = gamma * 0.5; g2 = 0.5/(gamma*det); for(unsigned int i = 0; i < 3; i++) for(unsigned int j = 0; j < 3; j++) { Ek[i][j] = At[i][j]; At[i][j] = g1 * At[i][j] + g2 * Aadj[i][j]; Ek[i][j] -= At[i][j]; } E_one = norm_one(Ek); A_one = norm_one(At); A_inf = norm_inf(At); } while( E_one > A_one * tolerance); if(fabs(det) < EPSILON)//edit by Xing Q = mat3::Identity(); else Q = At.transpose(); //TODO: if S is to be used. uncomment this part for(unsigned int i = 0; i < 3; i++) for(unsigned int j = 0; j < 3; j++) { S[i][j] = 0; for(unsigned int k = 0; k < 3; k++) S[i][j] += At[i][k] * A[k][j]; } for(unsigned int i = 0; i < 3; i++) for(unsigned int j = i; j < 3; j++) { S[i][j] = S[j][i] = 0.5*(S[i][j] + S[j][i]); } }