bool MDTRA_Compact_PDB_File :: jacobi( float *matrix, float *d, float *v )
{
float tresh, theta, tau, t, sm, s, h, g, c;
float b[3];
float z[3];
memset( v, 0, sizeof(*v) * 12 );
memset( z, 0, sizeof(z) );
for( int ip = 0; ip < 3; ip++ ) {
v[ip*4+ip] = 1.0f;
b[ip] = d[ip] = matrix[ip*4+ip];
}
for ( int i = 1; i <= JACOBI_MAXSWEEP; i++ ) {
sm = 0.0;
for ( int ip = 0; ip < 2; ip++ ) {
for ( int iq = ip+1; iq < 3; iq++ )
sm += fabsf( matrix[iq*4+ip] );
}
if ( sm == 0.0f )
return true;
tresh = (i < 4) ? (0.2f * sm/9.0f) : 0.0f;
for ( int ip = 0; ip < 2; ip++ ) {
for( int iq = ip+1; iq < 3; iq++ ) {
g = 100.0f * fabs(matrix[iq*4+ip]);
if ((i > 4) && fabsf(d[ip]+g) == fabsf(d[ip])
&& fabsf(d[iq]+g) == fabsf(d[iq]))
{
matrix[iq*4+ip] = 0.0f;
}
else if (fabs(matrix[iq*4+ip]) > tresh) {
h = d[iq] - d[ip];
if ((fabsf(h)+g)==fabsf(h)) {
t = matrix[iq*4+ip]/h;
} else {
theta = 0.5f*h/matrix[iq*4+ip];
t = 1.0f/(fabsf(theta)+sqrt(1.0f+theta*theta));
if (theta<0.0f) t = -t;
}
c = 1.0f/sqrtf(1.0f+t*t);
s = t*c;
tau = s/(1.0f + c);
h = t*matrix[iq*4+ip];
z[ip] -= h;
z[iq] += h;
d[ip] -= h;
d[iq] += h;
matrix[iq*4+ip] = 0.0f;
for ( int j = 0; j <= ip-1; j++ ) JACOBI_ROTATE(matrix,j,ip,j,iq);
for ( int j = ip+1; j <= iq-1; j++ )JACOBI_ROTATE(matrix,ip,j,j,iq);
for ( int j = iq+1; j < 3; j++ ) JACOBI_ROTATE(matrix,ip,j,iq,j);
for ( int j = 0; j < 3; j++ ) JACOBI_ROTATE(v,j,ip,j,iq);
}
}
}
for ( int ip = 0; ip < 3; ip++ ) {
b[ip] += z[ip];
d[ip] = b[ip];
z[ip] = 0.0f;
}
}
printf("ERROR: jacobi: Too many iterations\n");
return false;
}
static void Jacobi4(double **a, double *w, double **v)
{
int i, j, k, l;
double b[4], z[4];
// initialize
for (i = 0; i < 4; i++)
{
for (j = 0; j < 4; j++)
v[i][j] = 0.0;
v[i][i] = 1.0;
z[i] = 0.0;
b[i] = w[i] = a[i][i];
}
// begin rotation sequence
for (i = 0; i < JACOBI_MAX_ROTATIONS; i++)
{
double sum = 0.0;
for (j = 0; j < 3; j++)
{
for (k = j + 1; k < 4; k++)
sum += fabs(a[j][k]);
}
if (sum == 0.0)
break;
const double tresh = (i < 3) ? 0.2 * sum / 16 : 0;
for (j = 0; j < 3; j++)
{
for (k = j + 1; k < 4; k++)
{
double g = 100.0 * fabs(a[j][k]);
double theta, t;
// after for runs
if (i > 3 && (fabs(w[j]) + g) == fabs(w[j]) && (fabs(w[k]) + g) == fabs(w[k]))
{
a[j][k] = 0.0;
}
else if (fabs(a[j][k]) > tresh)
{
double h = w[k] - w[j];
if ((fabs(h) + g) == fabs(h))
{
t = (a[j][k]) / h;
}
else
{
theta = 0.5 * h / a[j][k];
t = 1.0 / (fabs(theta) + sqrt(1.0 + theta * theta));
if (theta < 0.0)
t = -t;
}
double c = 1.0 / sqrt(1.0 + t * t);
double s = t * c;
double tau = s / (1.0 + c);
h = t * a[j][k];
z[j] -= h;
z[k] += h;
w[j] -= h;
w[k] += h;
a[j][k] = 0.0;
// j already shifted left by 1 unit
for (l = 0; l < j; l++)
{
JACOBI_ROTATE(a, l, j, l, k);
}
// j and k already shifted left by 1 unit
for (l = j + 1; l < k; l++)
{
JACOBI_ROTATE(a, j, l, l, k);
}
// k already shifted left by 1 unit
for (l = k + 1; l < 4; l++)
{
JACOBI_ROTATE(a, j, l, k, l);
}
for (l = 0; l < 4; l++)
{
JACOBI_ROTATE(v, l, j, l, k);
}
}
}
}
for (j = 0; j < 4; j++)
{
b[j] += z[j];
w[j] = b[j];
z[j] = 0.0;
}
}
// sort eigenfunctions
for (j = 0; j < 3; j++)
{
int k = j;
double tmp = w[k];
for (i = j + 1; i < 4; i++)
{
if (w[i] > tmp)
{
k = i;
tmp = w[k];
}
}
if (k != j)
{
w[k] = w[j];
w[j] = tmp;
for (i = 0; i < 4; i++)
{
tmp = v[i][j];
v[i][j] = v[i][k];
v[i][k] = tmp;
}
}
}
// Ensure eigenvector consistency (Jacobi can compute vectors that
// are negative of one another). Compute most positive eigenvector.
const int nCeilHalf = (4 >> 1) + (4 & 1);
for (j = 0; j < 4; j++)
{
double sum = 0;
for (i = 0; i < 4; i++)
{
if (v[i][j] >= 0.0)
sum++;
}
if (sum < nCeilHalf)
{
for(i = 0; i < 4; i++)
v[i][j] *= -1.0;
}
}
}
