in layer0/Matrix.cpp [1033:1500]
float MatrixFitRMSTTTf(PyMOLGlobals * G, int n, const float *v1, const float *v2, const float *wt,
float *ttt)
{
/*
Subroutine to do the actual RMS fitting of two sets of vector coordinates
This routine does not rotate the actual coordinates, but instead returns
the RMS fitting value, along with the center-of-mass translation vectors
T1 and T2 and the rotation matrix M, which rotates the translated
coordinates of molecule 2 onto the translated coordinates of molecule 1.
*/
double m[3][3], aa[3][3];
double sumwt, tol;
int a, b, c, maxiter;
double t1[3], t2[3];
/* special case: just one atom pair */
if(n == 1) {
if(ttt) {
for(a = 1; a < 11; a++)
ttt[a] = 0.0F;
for(a = 0; a < 12; a += 5)
ttt[a] = 1.0F;
for(a = 0; a < 3; a++)
ttt[a + 12] = v2[a] - v1[a];
}
return 0.0F;
}
/* Initialize arrays. */
for(a = 0; a < 3; a++) {
for(b = 0; b < 3; b++) {
aa[a][b] = 0.0;
}
t1[a] = 0.0;
t2[a] = 0.0;
}
sumwt = 0.0F;
tol = SettingGetGlobal_f(G, cSetting_fit_tolerance);
maxiter = SettingGetGlobal_i(G, cSetting_fit_iterations);
/* Calculate center-of-mass vectors */
{
const float *vv1 = v1, *vv2 = v2;
if(wt) {
for(c = 0; c < n; c++) {
for(a = 0; a < 3; a++) {
t1[a] += wt[c] * vv1[a];
t2[a] += wt[c] * vv2[a];
}
if(wt[c] != 0.0F) {
sumwt = sumwt + wt[c];
} else {
sumwt = sumwt + 1.0F; /* WHAT IS THIS? */
}
vv1 += 3;
vv2 += 3;
}
} else {
for(c = 0; c < n; c++) {
for(a = 0; a < 3; a++) {
t1[a] += vv1[a];
t2[a] += vv2[a];
}
sumwt += 1.0F;
vv1 += 3;
vv2 += 3;
}
}
if(sumwt == 0.0F)
sumwt = 1.0F;
for(a = 0; a < 3; a++) {
t1[a] /= sumwt;
t2[a] /= sumwt;
}
}
{
/* Calculate correlation matrix */
double x[3], xx[3];
const float *vv1 = v1, *vv2 = v2;
for(c = 0; c < n; c++) {
if(wt) {
for(a = 0; a < 3; a++) {
x[a] = wt[c] * (vv1[a] - t1[a]);
xx[a] = wt[c] * (vv2[a] - t2[a]);
}
} else {
for(a = 0; a < 3; a++) {
x[a] = vv1[a] - t1[a];
xx[a] = vv2[a] - t2[a];
}
}
for(a = 0; a < 3; a++)
for(b = 0; b < 3; b++)
aa[a][b] = aa[a][b] + xx[a] * x[b];
vv1 += 3;
vv2 += 3;
}
}
if(n > 1) {
int got_kabsch = false;
int fit_kabsch = SettingGetGlobal_i(G, cSetting_fit_kabsch);
if(fit_kabsch) {
/* WARNING: Kabsch isn't numerically stable */
/* Kabsch as per
http://en.wikipedia.org/wiki/Kabsch_algorithm
minimal RMS matrix is (AtA)^(1/2) * A_inverse, where
Aij = Pki Qkj
assuming Pki and Qkj are centered about the same origin.
*/
/* NOTE: This Kabsch implementation only works with 4 or more atoms */
double At[3][3], AtA[3][3];
/* compute At and At * A */
transpose33d33d((double *) (void *) aa, (double *) (void *) At);
multiply33d33d((double *) (void *) At, (double *) (void *) aa,
(double *) (void *) AtA);
/* solve A*At (a real symmetric matrix) */
{
double e_vec[3][3], e_val[3];
xx_word n_rot;
if(xx_matrix_jacobi_solve((xx_float64 *) (void *) e_vec,
(xx_float64 *) (void *) e_val,
&n_rot, (xx_float64 *) (void *) AtA, 3)) {
double V[3][3], Vt[3][3];
/* Kabsch requires non-negative eigenvalues (since sqrt is taken) */
if((e_val[0] >= 0.0) && (e_val[1] >= 0.0) && (e_val[2] >= 0.0)) {
switch (fit_kabsch) {
case 1:
{
/* original Kabsch performs an unnecessary matrix inversion */
/* rot matrix = (AtA)^(1/2) * A^(-1) */
double rootD[3][3], sqrtAtA[3][3], Ai[3][3];
for(a = 0; a < 3; a++)
for(b = 0; b < 3; b++) {
if(a == b) {
rootD[a][b] = sqrt1d(e_val[a]);
} else {
rootD[a][b] = 0.0;
}
V[a][b] = e_vec[a][b];
Vt[a][b] = e_vec[b][a];
}
multiply33d33d((double *) (void *) rootD, (double *) (void *) Vt,
(double *) (void *) sqrtAtA);
multiply33d33d((double *) (void *) V, (double *) (void *) sqrtAtA,
(double *) (void *) sqrtAtA);
/* compute Ai */
if(xx_matrix_invert((xx_float64 *) (void *) Ai,
(xx_float64 *) (void *) aa, 3)) {
/* now compute the rotation matrix = (AtA)^(1/2) * Ai */
multiply33d33d((double *) (void *) sqrtAtA,
(double *) (void *) Ai, (double *) (void *) m);
got_kabsch = true;
{ /* is the rotation matrix left-handed? Then swap the
recomposition so as to avoid the reflection */
double cp[3], dp;
cross_product3d((double *) (void *) m, (double *) (void *) m + 3, cp);
dp = dot_product3d((double *) (void *) m + 6, cp);
if((1.0 - fabs(dp)) > R_SMALL4) { /* not orthonormal? */
got_kabsch = false;
PRINTFB(G, FB_Matrix, FB_Warnings)
"Matrix-Warning: Kabsch matrix not orthonormal: falling back on iteration.\n"
ENDFB(G);
} else if(dp < 0.0F) {
multiply33d33d((double *) (void *) rootD, (double *) (void *) V,
(double *) (void *) sqrtAtA);
multiply33d33d((double *) (void *) Vt, (double *) (void *) sqrtAtA,
(double *) (void *) sqrtAtA);
multiply33d33d((double *) (void *) sqrtAtA, (double *) (void *) Ai,
(double *) (void *) m);
}
}
} else {
PRINTFB(G, FB_Matrix, FB_Warnings)
"Matrix-Warning: Kabsch matrix inversion failed: falling back on iteration.\n"
ENDFB(G);
}
}
break;
case 2:
{
/* improved Kabsch skips the matrix inversion */
if((e_val[0] > R_SMALL8) &&
(e_val[1] > R_SMALL8) && (e_val[2] > R_SMALL8)) {
/* inv rot matrix = U Vt = A V D^(-1/2) Vt
* where U from SVD of A = U S Vt
* and where U is known to be = A V D^(-1/2)
* where D are eigenvalues of AtA */
double invRootD[3][3], Mi[3][3];
for(a = 0; a < 3; a++)
for(b = 0; b < 3; b++) {
if(a == b) {
invRootD[a][b] = 1.0 / sqrt1d(e_val[a]);
} else {
invRootD[a][b] = 0.0;
}
V[a][b] = e_vec[a][b];
Vt[a][b] = e_vec[b][a];
}
/* now compute the rotation matrix directly */
multiply33d33d((double *) (void *) invRootD, (double *) (void *) Vt,
(double *) (void *) Mi);
multiply33d33d((double *) (void *) V, (double *) (void *) Mi,
(double *) (void *) Mi);
multiply33d33d((double *) (void *) aa, (double *) (void *) Mi,
(double *) (void *) Mi);
got_kabsch = true;
{ /* cis the rotation matrix left-handed? Then swap the
recomposition so as to avoid the reflection */
double cp[3], dp;
cross_product3d((double *) (void *) Mi, (double *) (void *) Mi + 3,
cp);
dp = dot_product3d((double *) (void *) Mi + 6, cp);
if((1.0 - fabs(dp)) > R_SMALL4) { /* not orthonormal? */
got_kabsch = false;
PRINTFB(G, FB_Matrix, FB_Warnings)
"Matrix-Warning: Kabsch matrix not orthonormal: falling back on iteration.\n"
ENDFB(G);
} else if(dp < 0.0F) {
multiply33d33d((double *) (void *) invRootD, (double *) (void *) V,
(double *) (void *) Mi);
multiply33d33d((double *) (void *) Vt, (double *) (void *) Mi,
(double *) (void *) Mi);
multiply33d33d((double *) (void *) aa, (double *) (void *) Mi,
(double *) (void *) Mi);
}
}
if(got_kabsch) { /* transpose to get the inverse */
transpose33d33d((double *) (void *) Mi, (double *) (void *) m);
}
} else {
PRINTFB(G, FB_Matrix, FB_Warnings)
"Matrix-Warning: Kabsch matrix degenerate: falling back on iteration.\n"
ENDFB(G);
}
}
break;
}
/* validate the result */
if(got_kabsch) {
if((fabs(length3d(m[0]) - 1.0) < R_SMALL4) &&
(fabs(length3d(m[1]) - 1.0) < R_SMALL4) &&
(fabs(length3d(m[2]) - 1.0) < R_SMALL4)) {
recondition33d((double *) (void *) m);
} else {
got_kabsch = false;
PRINTFB(G, FB_Matrix, FB_Warnings)
"Matrix-Warning: Kabsch matrix not normal: falling back on iteration.\n"
ENDFB(G);
}
}
} else {
PRINTFB(G, FB_Matrix, FB_Warnings)
"Matrix-Warning: Kabsch eigenvalue(s) negative: falling back on iteration.\n"
ENDFB(G);
}
} else {
PRINTFB(G, FB_Matrix, FB_Warnings)
"Matrix-Warning: Kabsch decomposition failed: falling back on iteration.\n"
ENDFB(G);
}
}
}
if(!got_kabsch) {
/* use PyMOL's original iteration algorithm if Kabsch is
disabled or fails (quite common). */
/* Primary iteration scheme to determine rotation matrix for molecule 2 */
double sg, bb, cc;
int iters, iy, iz, unchanged = 0;
double sig, gam;
double tmp;
double save[3][3];
int perturbed = false;
for(a = 0; a < 3; a++) {
for(b = 0; b < 3; b++) {
m[a][b] = 0.0;
save[a][b] = aa[a][b];
}
m[a][a] = 1.0;
}
iters = 0;
while(1) {
/* IX, IY, and IZ rotate 1-2-3, 2-3-1, 3-1-2, etc. */
iz = (iters + 1) % 3;
iy = (iz + 1) % 3;
sig = aa[iz][iy] - aa[iy][iz];
gam = aa[iy][iy] + aa[iz][iz];
if(iters >= maxiter) {
PRINTFB(G, FB_Matrix, FB_Details)
" Matrix: Warning: no convergence (%1.8f<%1.8f after %d iterations).\n",
(float) tol, (float) gam, iters ENDFB(G);
break;
}
/* Determine size of off-diagonal element. If off-diagonals exceed the
diagonal elements tolerance, perform Jacobi rotation. */
tmp = sig * sig + gam * gam;
sg = sqrt1d(tmp);
if((sg != 0.0F) && (fabs(sig) > (tol * fabs(gam)))) {
unchanged = 0;
sg = 1.0F / sg;
for(a = 0; a < 3; a++) {
bb = gam * aa[iy][a] + sig * aa[iz][a];
cc = gam * aa[iz][a] - sig * aa[iy][a];
aa[iy][a] = bb * sg;
aa[iz][a] = cc * sg;
bb = gam * m[iy][a] + sig * m[iz][a];
cc = gam * m[iz][a] - sig * m[iy][a];
m[iy][a] = bb * sg;
m[iz][a] = cc * sg;
}
} else {
unchanged++;
if(unchanged == 3) {
double residual = 0.0;
for(a = 0; a < 3; a++) {
for(b = 0; b < 3; b++) {
residual += fabs(aa[a][b] - save[a][b]);
}
}
if(residual > R_SMALL4) {
/* matrix has changed significantly, so we assume that
we found the minimum */
break;
} else if(perturbed) {
/* we ended up back where we started even after perturbing */
break;
} else { /* hmm...no change from start... so displace 90
degrees just to make sure we didn't start out
trapped in precisely the opposite direction */
for(a = 0; a < 3; a++) {
bb = aa[iz][a];
cc = -aa[iy][a];
aa[iy][a] = bb;
aa[iz][a] = cc;
bb = m[iz][a];
cc = -m[iy][a];
m[iy][a] = bb;
m[iz][a] = cc;
}
perturbed = true;
unchanged = 0;
}
/* only give up if we've iterated through all three planes */
}
}
iters++;
}
recondition33d((double *) (void *) m);
}
}
/* At this point, we should have a converged rotation matrix (M). Calculate
the weighted RMS error. */
{
const float *vv1 = v1, *vv2 = v2;
double etmp, tmp;
double err = 0.0;
for(c = 0; c < n; c++) {
etmp = 0.0;
for(a = 0; a < 3; a++) {
tmp = m[a][0] * (vv2[0] - t2[0])
+ m[a][1] * (vv2[1] - t2[1])
+ m[a][2] * (vv2[2] - t2[2]);
tmp = (vv1[a] - t1[a]) - tmp;
etmp += tmp * tmp;
}
if(wt)
err += wt[c] * etmp;
else
err += etmp;
vv1 += 3;
vv2 += 3;
}
err = err / sumwt;
err = sqrt1d(err);
/* NOTE: TTT's are now row-major (to be more like homogenous matrices) */
if(ttt) {
ttt[0] = (float) m[0][0];
ttt[1] = (float) m[1][0];
ttt[2] = (float) m[2][0];
ttt[3] = (float) t2[0];
ttt[4] = (float) m[0][1];
ttt[5] = (float) m[1][1];
ttt[6] = (float) m[2][1];
ttt[7] = (float) t2[1];
ttt[8] = (float) m[0][2];
ttt[9] = (float) m[1][2];
ttt[10] = (float) m[2][2];
ttt[11] = (float) t2[2];
ttt[12] = (float) -t1[0];
ttt[13] = (float) -t1[1];
ttt[14] = (float) -t1[2];
}
/* for compatibility with normal 4x4 matrices */
if(fabs(err) < R_SMALL4)
err = 0.0F;
return ((float) err);
}
}