src/common/sm_matrix.cc

#include <math.h>
#include "sm_matrix.h"


void  smZero(vec3 A) {
        A[0] = A[1] = A[2] = 0; 
}

void smCopy(mat3 a, mat3 b) {
        b[0][0]=a[0][0];
        b[0][1]=a[0][1];
        b[0][2]=a[0][2];
        b[1][0]=a[1][0];
        b[1][1]=a[1][1];
        b[1][2]=a[1][2];
        b[2][0]=a[2][0];
        b[2][1]=a[2][1];
        b[2][2]=a[2][2];
}

void smCopy(vec3 a, vec3 b) {
        b[0]=a[0];
        b[1]=a[1];
        b[2]=a[2];
}

void smCopy(vec3 *a, vec3 *b, int len) {
        for ( int i = 0; i < len; i++) {        
                b[i][0]=a[i][0];
                b[i][1]=a[i][1];
                b[i][2]=a[i][2];
        }
}



void smMultMat(mat3 a, mat3 b, mat3 c) {
        int i, j, k;
        for (i=0; i<3; i++) {
                for (j=0; j<3; j++) {
                        c[i][j] = 0.0;
                        for (k=0; k<3; k++) {
                                c[i][j] += a[i][k] * b[k][j];
                        }
                }
        }
}


void smAdd(vec3 a, vec3 b) {
        b[0] += a[0];
        b[1] += a[1];
        b[2] += a[2];
}

void smAdd(vec3 a, vec3 b, vec3 c) {
        c[0] = a[0] + b[0];
        c[1] = a[1] + b[1];
        c[2] = a[2] + b[2];
}

void smAdd(vec3 a, vec3 *in_vec, int count) {
        for (int i = 0; i < count; i++) {
                in_vec[i][0] += a[0];
                in_vec[i][1] += a[1];
                in_vec[i][2] += a[2];
        }
}

void smAdd(vec3 a, vec3 *in_vec, vec3 *out_vec, int count) {
        for (int i = 0; i < count; i++) {
                out_vec[i][0] = in_vec[i][0] + a[0];
                out_vec[i][1] = in_vec[i][1] + a[1];
                out_vec[i][2] = in_vec[i][2] + a[2];
        }       
}



void smSub(vec3 a, vec3 b, vec3 c) {
        c[0] = a[0] - b[0];
        c[1] = a[1] - b[1];
        c[2] = a[2] - b[2];
}


void smSub(vec3 *in_vec, vec3 a, vec3 *out_vec, int count) {
        for (int i = 0; i < count; i++) {
                out_vec[i][0] = in_vec[i][0] - a[0];
                out_vec[i][1] = in_vec[i][1] - a[1];
                out_vec[i][2] = in_vec[i][2] - a[2];
        }       
        
}


void smSub( vec3 a,  vec3 *in_vec, vec3 *out_vec, int count) {
        for (int i = 0; i < count; i++) {
                out_vec[i][0] = a[0] - in_vec[i][0];
                out_vec[i][1] = a[1] - in_vec[i][1];
                out_vec[i][2] = a[2] - in_vec[i][2];
        }       
        
}

void smSub(vec3 *in_vec, vec3 a, int count) {
        for (int i = 0; i < count; i++) {
                in_vec[i][0] -= a[0];
                in_vec[i][1] -= a[1];
                in_vec[i][2] -= a[2];
        }
}

void smMultVec(vec3 a, mat3 b) {
        vec3 c;
        c[0] = a[0]*b[0][0] + a[1]*b[1][0] + a[2]*b[2][0];
        c[1] = a[0]*b[0][1] + a[1]*b[1][1] + a[2]*b[2][1];
        c[2] = a[0]*b[0][2] + a[1]*b[1][2] + a[2]*b[2][2];
        smCopy(c, a);
}

void smMultVec(vec3 a, mat3 b, vec3 a_out) {
        a_out[0] = a[0]*b[0][0] + a[1]*b[1][0] + a[2]*b[2][0];
        a_out[1] = a[0]*b[0][1] + a[1]*b[1][1] + a[2]*b[2][1];
        a_out[2] = a[0]*b[0][2] + a[1]*b[1][2] + a[2]*b[2][2];
}


void smMultVec(vec3 *a, mat3 b, int len) {
        for (int i = 0; i < len; i++) {
                smMultVec( a[i], b );
        }
}

void smMultVec(vec3 *a, mat3 b, vec3 *a_out, int len) {
        for (int i = 0; i < len; i++) {
                smMultVec( a[i], b, a_out[i] );
        }
}

float smDotProduct(vec3 a, vec3 b) {
        return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}


void smMultMat( mat3 u, vec3 in_vec) {
        vec3 tmp_vec;
        tmp_vec[0] = 0;
        tmp_vec[1] = 0;
        tmp_vec[2] = 0;
        for (int i = 0; i < 3; i++) {
                for (int j = 0; j < 3; j++) {
                        tmp_vec[i] += u[i][j] * in_vec[j]; 
                }
        }
        in_vec[0] = tmp_vec[0];
        in_vec[1] = tmp_vec[1];
        in_vec[2] = tmp_vec[2];
}

void smMultMat( mat3 u, vec3 *in_vec, vec3 *out_vec, int len) {
        vec3 tmp_vec;
        long n,i,j;     
        for (n = 0; n < len; n++) {
                tmp_vec[0] = 0;
                tmp_vec[1] = 0;
                tmp_vec[2] = 0;
                for (i = 0; i < 3; i++) {
                        for (j = 0; j < 3; j++) {
                                tmp_vec[i] += u[i][j] * in_vec[n][j]; 
                        }
                }
                out_vec[n][0] = tmp_vec[0];
                out_vec[n][1] = tmp_vec[1];
                out_vec[n][2] = tmp_vec[2];
        }
}

void smMultMat( mat3 u, vec3 *in_vec, int len) {
        vec3 tmp_vec;
        long n,i,j;     
        for (n = 0; n < len; n++) {
                tmp_vec[0] = 0;
                tmp_vec[1] = 0;
                tmp_vec[2] = 0;
                for (i = 0; i < 3; i++) {
                        for (j = 0; j < 3; j++) {
                                tmp_vec[i] += u[i][j] * in_vec[n][j]; 
                        }
                }
                in_vec[n][0] = tmp_vec[0];
                in_vec[n][1] = tmp_vec[1];
                in_vec[n][2] = tmp_vec[2];
        }
}

float smDistance(vec3 a, vec3 b) {
        vec3 tmp;
        tmp[0] = a[0] - b[0];
        tmp[1] = a[1] - b[1];
        tmp[2] = a[2] - b[2];
        return sqrt( tmp[0]*tmp[0] + tmp[1]*tmp[1] + tmp[2]*tmp[2] );
}

int smInverseMatrix(float original_matrix[3][3], float inverse_matrix[3][3]) {
#define N 3
        float a[N*N];
        int is[N], js[N];
        int i, j, k, l, u, v;
        float d, p;
        
        for (i=0; i<N; i++) {
                for (j=0; j<N; j++) {
                        a[i*N+j] = original_matrix[i][j];
                }
        }
        for (k=0; k<N; k++) {
                d = 0.0;
                for (i=k; i<N; i++) {
                        for (j=k; j<N; j++) {
                                l = i*N+j;
                                p = fabs(a[l]);
                                if (p>d) {
                                        d = p;
                                        is[k]=i;
                                        js[k] = j;
                                }
                        }
                }
                if (d+1.0 == 1.0) {
                        //      printf( "Error: Inverse Matrix failed...\n");
                        //      fflush(stdout);
                        //      exit(0);
                        return(1);
                }
                if (is[k] != k) {
                        for (j=0; j<N; j++) {
                                u = k*N+j;
                                v = is[k]*N+j;
                                p = a[u];
                                a[u] = a[v];
                                a[v] = p;
                        }
                }
                if (js[k] != k) {
                        for (i=0; i<N; i++) {
                                u = i*N+k;
                                v = i*N+js[k];
                                p = a[u];
                                a[u] = a[v];
                                a[v] = p;
                        }
                }
                l = k*N+k;
                a[l] = 1.0/a[l];
                for (j=0; j<N; j++) {
                        if (j!=k) {
                                u = k*N+j;
                                a[u] = a[u]*a[l];
                        }
                }
                for (i=0; i<N; i++) {
                        if (i!=k) {
                                for (j=0; j<N; j++) {
                                        if (j!= k) {
                                                u = i*N+j;
                                                a[u] = a[u] - a[i*N+k]*a[k*N+j];
                                        }
                                }
                        }
                }
                for (i=0; i<N; i++) {
                        if (i!=k) {
                                u = i*N+k;
                                a[u] = -a[u]*a[l];
                        }
                }
        }
        for (k=N-1; k>=0; k--) {
                if (js[k] != k) {
                        for (j=0; j<N; j++) {
                                u = k*N+j;
                                v = js[k]*N+j;
                                p = a[u];
                                a[u] = a[v];
                                a[v] = p;
                        }
                }
                if (is[k] != k) {
                        for (i=0; i<N; i++) {
                                u = i*N+k;
                                v = i*N+is[k];
                                p = a[u];
                                a[u] = a[v];
                                a[v] = p;
                        }
                }
        }
        for (i=0; i<N; i++)
                for (j=0; j<N; j++)
                        inverse_matrix[i][j] = a[i*N+j];
        return(0);
#undef N
}



void smMinus(vec3 a) {
        a[0] *= -1.0;
        a[1] *= -1.0;
        a[2] *= -1.0;
}

float smNorm(vec3 a) {  
        return sqrt(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]);
}


void smNormalize(vec3 a) {
        float norm;
        norm = smNorm(a);
        a[0] /= norm;
        a[1] /= norm;
        a[2] /= norm;
        return;
}


void smCrossProduct(vec3 a, vec3 b, vec3 c) {
        c[0] = a[1]*b[2] - a[2]*b[1];
        c[1] = a[2]*b[0] - a[0]*b[2];
        c[2] = a[0]*b[1] - a[1]*b[0];
}



void smComputeConvertMatrix(vec3 a, vec3 b, vec3 c, float convert_matrix[3][3]) {       
        int i;
        vec3 t_vector;
        vec3 origin_point, base_vector[3];
        
        /* get future origin point coordinates */
        smCopy(a, origin_point);
        
        /* determine base vectors of new coordinate system */
        smSub(b, a, base_vector[0] );
        smNormalize( base_vector[0] );
        
        smSub(c, a, t_vector );
        smCrossProduct(base_vector[0], t_vector, base_vector[2] );
        smNormalize( base_vector[2] );
        
        smCrossProduct(base_vector[2], base_vector[0], base_vector[1] );
        smNormalize( base_vector[1] );
        
        /* Make Convert Matrix */
        for (i=0; i<3; i++) {
                convert_matrix[i][0] = base_vector[i][0];
                convert_matrix[i][1] = base_vector[i][1];
                convert_matrix[i][2] = base_vector[i][2];
        }
}


void smMakeRotationMatrix(float angle, vec3 C, float rot_mat[3][3]) {
        float u, w, v, f;
        float t;
        t = tan(angle/2.0);
        u = t*C[0]; 
        v = t*C[1]; 
        w = t*C[2]; 
        f = 1 / (1 + t*t);
        rot_mat[0][0] = f * (1 + u*u - v*v - w*w);
        rot_mat[0][1] = f * 2 * (u*v - w);
        rot_mat[0][2] = f * 2 * (u*w + v);
        rot_mat[1][0] = f * 2 * (u*v + w);
        rot_mat[1][1] = f * (1 - u*u + v*v - w*w);
        rot_mat[1][2] = f * 2 * (v*w - u);
        rot_mat[2][0] = f * 2 * (u*w - v);
        rot_mat[2][1] = f * 2 * (v*w + u);
        rot_mat[2][2] = f * (1 - u*u - v*v + w*w);
}


float smVectorAngle(vec3 point1, vec3 point2, vec3 center) {
        vec3 n1, n2;    
        smSub( point1, center, n1 );
        smSub( point2, center, n2 );    
        smNormalize( n1 );
        smNormalize( n2 );
        return acos( smDotProduct( n1, n2 ) );
}



float smVectorAngle(vec3 point1, vec3 point2) {
        vec3 n1, n2;    
        smCopy( point1, n1 );
        smCopy( point2, n2 );   
        smNormalize( n1 );
        smNormalize( n2 );
        return acos( smDotProduct( n1, n2 ) );
}




char smCalcMinRmsdRotation( vec3 *vec_a, vec3 *vec_b, int len, float matrix[3][3]) {
        double R[3][3], RT[3][3], RTR[3][3], eigen_val[3],
    eigen_vec[3][3] = { {0,0,0}, {0,0,0}, {0,0,0} }, b_mat[3][3];
        long n,i,j,k;
        double a, b, c, q, p, U1, U2, U3, U4, max_val, h,
                i_val, j_val, k_val, L, M, N, P, tmp;
        double sum;
        
        for (i = 0; i < 3; i++) {
                for (j = 0; j < 3; j++) {
                        R[i][j] = 0;
                        for (n = 0; n < len; n++) {
                                R[i][j] += vec_b[n][i] * vec_a[n][j];
                        }
                }
        }
        
        for (i = 0; i < 3; i++) {
                for (j = 0; j < 3; j++) {
                        RT[j][i] = R[i][j]; 
                }       
        }
        
        max_val = 0;
        for (i = 0; i < 3; i++) {
                for (j = 0; j < 3; j++) {
                        RTR[i][j] = 0;
                        for (k = 0; k < 3; k++) {
                                RTR[i][j] += RT[i][k] * R[k][j];
                                if ( fabs(RTR[i][j]) > max_val ) 
                                        max_val = fabs(RTR[i][j]);
                        }
                }
        }
        for (i = 0; i < 3; i++) {
                for (j = 0; j < 3; j++) {
                        RTR[i][j] /= max_val;
                }
        }
        
        //find the co-effiecents for the eigen value cubic equation
        a = -(
                  RTR[0][0] + 
                  RTR[1][1] + 
                  RTR[2][2]
                  );
        b = -(
                  -(RTR[0][0]*RTR[2][2]) - 
                  (RTR[1][1]*RTR[2][2]) - 
                  (RTR[0][0]*RTR[1][1]) + 
                  (RTR[0][2]*RTR[2][0]) +
                  (RTR[1][0]*RTR[0][1]) +
                  (RTR[1][2]*RTR[2][1])
                  );
        
        c = -(
                  (RTR[0][0]*RTR[1][1]*RTR[2][2]) +
                  (RTR[2][0]*RTR[0][1]*RTR[1][2]) +
                  (RTR[1][0]*RTR[0][2]*RTR[2][1]) -
                  (RTR[0][2]*RTR[1][1]*RTR[2][0]) -
                  (RTR[0][1]*RTR[1][0]*RTR[2][2]) -
                  (RTR[0][0]*RTR[1][2]*RTR[2][1])
                  );
        
        
        //apply cubic equation to find the actual eigen values
        
        p = b - (pow(a,2.0)/ 3.0);
        q = c + (2.0*pow(a,3.0) - 9.0*a*b) / 27.0;
        
        h = (pow(q,2.0)/4.0) + (pow(p,3.0)/27.0);
        
        i_val = sqrt( (pow(q,2.0) / 4.0) - h );
        j_val = cbrt( i_val );
        
        k_val = acos( -(q/ (2.0*i_val)) );
        
        L = j_val * -1; 
        M = cos(k_val/3.0);     
        N = sqrt(3.0) * sin( k_val / 3.0 );     
        P = (a/3.0 ) * -1;
        
        eigen_val[0] = 2 * j_val * cos(k_val/3.0) - (a/(3.0));
        eigen_val[1] = L * (M+N)+P;
        eigen_val[2] = L * (M-N)+P;
        
        double swap;
        if ( eigen_val[0] < eigen_val[1] ) {
                swap = eigen_val[1];
                eigen_val[1] = eigen_val[0];
                eigen_val[0] = swap;
        }
        if ( eigen_val[1] < eigen_val[2] ) {
                swap = eigen_val[2];
                eigen_val[2] = eigen_val[1];
                eigen_val[1] = swap;
                if ( eigen_val[0] < eigen_val[1] ) {
                        swap = eigen_val[1];
                        eigen_val[1] = eigen_val[0];
                        eigen_val[0] = swap;
                }
        }
        //for each of the eigen values, determine the eigen vector
        for (i = 0; i < 2; i++) {
                U1 = (RTR[1][1] - eigen_val[i]) - 
                (RTR[0][1] * RTR[1][0])
                /(RTR[0][0]-eigen_val[i]);
                U2 = (RTR[2][1]) - 
                        (RTR[0][1] * RTR[2][0])
                        /(RTR[0][0]-eigen_val[i]);
                U3 = (RTR[1][2]) - 
                        (RTR[0][2] * RTR[1][0])
                        /(RTR[0][0]-eigen_val[i]);
                U4 =  (RTR[2][2] - eigen_val[i]) - 
                        (RTR[0][2] * RTR[2][0])
                        /(RTR[0][0]-eigen_val[i]);
                
                if ( fabs(U4 - (U2*U3)/U1) < 0.000001 ) {
                        //this eigen vector is on a plane
                        //because any value of Z is acceptable                  
                        eigen_vec[i][2] = 1;
                        eigen_vec[i][1] = -( eigen_vec[i][2] * U4/U3);
                        eigen_vec[i][0] = (-RTR[1][0] * eigen_vec[i][1] - RTR[2][0]*eigen_vec[i][2]) / (RTR[0][0]-eigen_val[i]);                
                        
                        //normalize the vector
                        tmp = 1/sqrt( eigen_vec[i][0]*eigen_vec[i][0] + eigen_vec[i][1]*eigen_vec[i][1] + eigen_vec[i][2]*eigen_vec[i][2]);
                        eigen_vec[i][0] *= tmp;
                        eigen_vec[i][1] *= tmp;
                        eigen_vec[i][2] *= tmp;
                } else {
                        //fprintf(stderr, "Weird error\n");
                        return 1;
                }
        }       
        //eigen_vec[2] is the cross product of eigen_vec[0] eigen_vec[1]
        eigen_vec[2][0] = (eigen_vec[0][1]*eigen_vec[1][2] - eigen_vec[0][2]*eigen_vec[1][1]);
        eigen_vec[2][1] = (eigen_vec[0][2]*eigen_vec[1][0] - eigen_vec[0][0]*eigen_vec[1][2]);
        eigen_vec[2][2] = (eigen_vec[0][0]*eigen_vec[1][1] - eigen_vec[0][1]*eigen_vec[1][0]);
        
        double sigma[3] = {1,1,1};      
        double Rak[3][3];       
        for (j = 0; j < 3; j++) {
                for (i = 0; i < 3; i++) {
                        Rak[j][i] = 0;
                        for (k = 0; k < 3; k++) {
                                Rak[j][i] += R[j][k] * eigen_vec[i][k]; 
                        }
                }
        }
        for (i=0;i<3;i++)
                eigen_val[i]=sqrt(max_val*eigen_val[i]);        
        
        for (i=0;i<3;i++) {
                for (j=0;j<3;j++) {
                        Rak[i][j]=0;
                        for (k=0;k<3;k++)
                                Rak[i][j]+=(eigen_vec[k][i]*eigen_vec[k][j]*sigma[k]/eigen_val[k]);
                }
        }
        for (i=0;i<3;i++){
                for (j=0;j<3;j++) {
                        matrix[i][j]=0;
                        for (k=0;k<3;k++)
                                matrix[i][j]+=(R[i][k]*Rak[k][j]);
                }
        }               
        return 0;
}



char smCalcMinRmsdRotationTri( vec3 *vec_a, vec3 *vec_b, float matrix[3][3]) {
        double R[3][3], RT[3][3], RTR[3][3], eigen_val[3],
    eigen_vec[3][3] = { {0,0,0}, {0,0,0}, {0,0,0} }, b_mat[3][3];
        long n,i,j,k;
        double a, b, c, q, p, U1, U2, U3, U4, max_val, h,
                i_val, j_val, k_val, L, M, N, P, tmp;
        
        //get the dot product from vec_a & vec_b
        
        vec3 cross_a, cross_b;
        
        smCrossProduct(vec_a[0], vec_a[1], cross_a);
        smCrossProduct(vec_b[0], vec_b[1], cross_b);

        
        for (i = 0; i < 3; i++) {
                for (j = 0; j < 3; j++) {
                        R[i][j] = (vec_a[0][i] * vec_b[0][j]) + (vec_a[1][i] * vec_b[1][j]) + 
                        (vec_a[2][i] * vec_b[2][j]) + (cross_a[i] * cross_b[j]);
                        /*
                        R[i][j] = 0;
                        for (n = 0; n < len; n++) {
                                R[i][j] += vec_a[n][i] * vec_b[n][j];
                        }
                         */
                }
        }
        for (i = 0; i < 3; i++) {
                for (j = 0; j < 3; j++) {
                        RT[j][i] = R[i][j]; 
                }       
        }
        
        //max_val = 0;
        for (i = 0; i < 3; i++) {
                for (j = 0; j < 3; j++) {
                        RTR[i][j] = 0;
                        for (k = 0; k < 3; k++) {
                                RTR[i][j] += RT[k][i] * R[j][k];
                                //if ( fabs(RTR[i][j]) > max_val ) 
                                //      max_val = fabs(RTR[i][j]);
                        }
                }
        }
        
        //for (i = 0; i < 3; i++) {
        //      for (j = 0; j < 3; j++) {
        //              RTR[i][j] /= max_val;
        //      }
        //}
        
        
        
        //find the co-effiecents for the eigen value cubic equation
        a = -(
                  RTR[0][0] + 
                  RTR[1][1] + 
                  RTR[2][2]
                  );
        b = -(
                  -(RTR[0][0]*RTR[2][2]) - 
                  (RTR[1][1]*RTR[2][2]) - 
                  (RTR[0][0]*RTR[1][1]) + 
                  (RTR[0][2]*RTR[2][0]) +
                  (RTR[1][0]*RTR[0][1]) +
                  (RTR[1][2]*RTR[2][1])
                  );
        
        c = -(
                  (RTR[0][0]*RTR[1][1]*RTR[2][2]) +
                  (RTR[2][0]*RTR[0][1]*RTR[1][2]) +
                  (RTR[1][0]*RTR[0][2]*RTR[2][1]) -
                  (RTR[0][2]*RTR[1][1]*RTR[2][0]) -
                  (RTR[0][1]*RTR[1][0]*RTR[2][2]) -
                  (RTR[0][0]*RTR[1][2]*RTR[2][1])
                  );
        
        //apply cubic equation to find the actual eigen values
        
        p = b - (pow(a,2.0)/ 3.0);
        q = c + (2.0*pow(a,3.0) - 9.0*a*b) / 27.0;
        
        h = (pow(q,2.0)/4.0) + (pow(p,3.0)/27.0);
        
        i_val = sqrt( (pow(q,2.0) / 4.0) - h );
        j_val = cbrt( i_val );
        
        k_val = acos( -(q/ (2.0*i_val)) );
        
        L = j_val * -1;
        
        M = cos(k_val/3.0);
        
        N = sqrt(3.0) * sin( k_val / 3.0 );
        
        
        P = (a/3.0 ) * -1;
        
        eigen_val[0] = 2 * j_val * cos(k_val/3.0) - (a/(3.0));
        eigen_val[1] = L * (M+N)+P;
        eigen_val[2] = L * (M-N)+P;
        
        
        //for each of the eigen values, determine the eigen vector
        
        for (i = 0; i < 3; i++) {
                U1 = (RTR[1][1] - eigen_val[i]) - 
                (RTR[0][1] * RTR[1][0])
                /(RTR[0][0]-eigen_val[i]);
                U2 = (RTR[2][1]) - 
                        (RTR[0][1] * RTR[2][0])
                        /(RTR[0][0]-eigen_val[i]);
                U3 = (RTR[1][2]) - 
                        (RTR[0][2] * RTR[1][0])
                        /(RTR[0][0]-eigen_val[i]);
                U4 =  (RTR[2][2] - eigen_val[i]) - 
                        (RTR[0][2] * RTR[2][0])
                        /(RTR[0][0]-eigen_val[i]);
                
                if ( fabs(U4 - (U2*U3)/U1) < 0.001 ) {
                        //this eigen vector is on a plane
                        //because any value of Z is acceptable
                        
                        eigen_vec[i][2] = 1;
                        eigen_vec[i][1] = -( eigen_vec[i][2] * U4/U3);
                        eigen_vec[i][0] = (-RTR[1][0] * eigen_vec[i][1] - RTR[2][0]*eigen_vec[i][2]) / (RTR[0][0]-eigen_val[i]);
                        
                        
                        //normalize the vector
                        tmp = 1/sqrt( eigen_vec[i][0]*eigen_vec[i][0] + eigen_vec[i][1]*eigen_vec[i][1] + eigen_vec[i][2]*eigen_vec[i][2]);
                        eigen_vec[i][0] *= tmp;
                        eigen_vec[i][1] *= tmp;
                        eigen_vec[i][2] *= tmp;
                } else {
                        //fprintf(stderr, "Weird error\n");
                        return 1;
                }
        }
        
        for (k = 0; k < 3; k++) {
                for (i = 0; i < 3; i++) {
                        b_mat[k][i] = 0;
                        for (j = 0; j < 3; j++) {
                                b_mat[k][i] += R[j][i] * eigen_vec[k][j];       
                        }
                        b_mat[k][i] /= sqrt( eigen_val[k]);
                }
        }
        
        
        for (i = 0; i < 3; i++) {
                for (j = 0; j < 3; j++) {
                        matrix[i][j] = 0;
                        for (k = 0; k < 3; k++) {
                                matrix[i][j] += b_mat[k][i] * eigen_vec[k][j];          
                        }
                }
        }
        
        for (i = 0; i < 3; i++) {
                for (j = 0; j < 3; j++) {
                        //matrix[i][j] /= sqrt(max_val);                
                        //assert(!isnan(matrix[i][j]));
                        if ( isnan(matrix[i][j]) ) {
                                return 1;
                        }
                }
        } 
        return 0;
}



void smMoveMeanOrigin( vec3 *vec_a, int len_a ) {       
        vec3 mean = {0,0,0};
    for (int i = 0; i < len_a; i++) {
                for (int j = 0; j < 3; j++) {
                        mean[j] += vec_a[i][j];
                }       
    }
    for (int j = 0; j < 3; j++) {
                mean[j] /= (float)len_a;
    }
    for (int i = 0; i < len_a; i++) {
                for (int j = 0; j < 3; j++) {
                        vec_a[i][j] -= mean[j];
                }
    }           
}

void smMoveMeanOrigin( vec3 *vec_a, int len_a, vec3 *vec_b ) {
        vec3 mean = {0,0,0};
    for (int i = 0; i < len_a; i++) {
                for (int j = 0; j < 3; j++) {
                        mean[j] += vec_a[i][j];
                }       
    }
    for (int j = 0; j < 3; j++) {
                mean[j] /= (float)len_a;
    }
        for (int i = 0; i < len_a; i++) {
                for (int j = 0; j < 3; j++) {
                        vec_b[i][j] = vec_a[i][j] - mean[j];
                }
    }   
}


void smCalcMeanOrigin( vec3 *vec_a, int len_a, vec3 trans ) {   
        vec3 mean = {0,0,0};
    for (int i = 0; i < len_a; i++) {
                for (int j = 0; j < 3; j++) {
                        mean[j] += vec_a[i][j];
                }       
    }
    for (int j = 0; j < 3; j++) {
                mean[j] /= (float)len_a;
    }
        trans[0] = -mean[0];
        trans[1] = -mean[1];
        trans[2] = -mean[2];    
}

char smMoveMinRmsd( vec3 *vec_a, vec3 *vec_b, int len ) {
  mat3 matrix;
  if ( smCalcMinRmsdRotation( vec_a, vec_b, len, matrix ) ) {
    return 1;
  }
  smMultVec( vec_a, matrix, len );
  return 0;
}


float smCalcRmsd( vec3 *vec_a, vec3 *vec_b, int len ) {
        double total = 0;
        for (int i = 0; i < len; i++) {
                for (int j = 0; j < 3; j++) {
                        total += pow(vec_a[i][j] - vec_b[i][j], 2);
                }       
        }
        return sqrt( total / (double) len );    
}

float smCalcInterRmsd( vec3 *vec_a, vec3 *vec_b, int len ) {
        double total = 0, count = 0;
        for (int i = 0; i < len; i++) {
                for ( int j = i+1; j < len; j++ ) {
                        float dist_a = smDistance( vec_a[i], vec_a[j] );
                        float dist_b = smDistance( vec_b[i], vec_b[j] );
                        total += pow(dist_a - dist_b, 2);
                        count++;
                }
        }
        return sqrt( total / count );   
}


float smCalcFrobeniusNorm( float matrix_a[3][3], float matrix_b[3][3] ) {
        long  i,j;
        float  total = 0, tmp_matrix[3][3];
        
        for (i = 0; i < 3; i++) {
                for (j = 0; j < 3; j++) {
                        tmp_matrix[i][j] = matrix_a[i][j] - matrix_b[i][j];     
                }
        }
        for (i = 0; i < 3; i++) {
                for (j = 0; j < 3; j++) {
                        total += tmp_matrix[i][j] * tmp_matrix[i][j];           
                }
        }
        return sqrt(total);
        
}

---