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