int svdcmp (float **A, int m, int n, float *s, float **V)
{
float *afort, *vfort; /*Vector format work spaces */
int i;
int info=0;
#ifndef SUNPERF
int one=1;
float *swork;
int ldwork;
#endif
/*Alloc work spaces and copy from numerical recipes matrix
form to the form sunperf wants */
afort = (float *) calloc(m*MAX(m,n),sizeof(float));
vfort = (float *) calloc(n*n,sizeof(float));
if( (afort == NULL) || (vfort == NULL))
die(0,"svdcmp could not alloc work arrays\n");
#ifdef SUNPERF
for(i=0;i<m;++i)
scopy(n,A[i],1,(afort+i),m);
#else
for(i=0;i<m;++i)
scopy_(&n,A[i],&one,(afort+i),&m);
#endif
/* old rule: always initialize things like this */
for(i=0;i<n*n;++i) vfort[i] = 0.0;
#ifdef SUNPERF
sgesvd('o','s',m,n,afort,m,s,NULL,m,vfort,n,&info);
#else
ldwork = 12+2*n; /* This is slightly larger than the minimum
required for safety. Space required is never
large by modern standards */
swork = calloc(ldwork,sizeof(float));
if(n == NULL) die(0,"Cannot alloc work space for SVD routine\n");
sgesvd_("o","s",&m,&n,afort,&m,s,NULL,&m,vfort,&n,
swork, &ldwork,
&info);
free(swork);
#endif
if(info!=0)
{
if(info > 0)
elog_notify(0,"Convergence failure in svd routine\n");
else
elog_notify(0,"Illegal value for argument %d passed to sgesvd\nNo solution possible\n",
-info);
free(afort);
free(vfort);
return(info);
}
#ifdef SUNPERF
/* note we do return something even when convergence failed.
Must handle negative return as junk is returned then. */
for(i=0;i<m;++i)
scopy(n,(afort+i),m,A[i],1);
/* Note this copies V transpose in vfort to V in the matrix V.
Tricky BLAS code I know*/
for(i=0;i<n;++i)
scopy(n,(vfort+i*n),1,V[i],1);
#else
for(i=0;i<m;++i)
scopy_(&n,(afort+i),&m,A[i],&one);
for(i=0;i<n;++i)
scopy_(&n,(vfort+i*n),&one,V[i],&one);
#endif
free(afort);
free(vfort);
return(0);
}
/* Kabsch alignment */
void kabsch_alignment( std::vector<float> ref, std::vector<float> tar, t_tiltdata &data, gmx_bool bVerbose)
{
if (ref.size() != tar.size())
{
std::cerr << "\nError! Sizes of reference coordinate matrix and simulated structure coordinate matrices do not match!" << std::endl;
std::exit(1);
}
int ncoords = ref.size();
int natoms = ncoords/3;
// Center the two selections
std::vector<float> stsel1(ncoords,0), stsel2(ncoords,0), stsel2T(ncoords,0);
std::vector<float> ref_com(3,0), tar_com(3,0);
average_coordinate(ref, ref_com);
average_coordinate(tar, tar_com);
for (int i=0; i<natoms; i++)
{
for (int j=0; j<3; j++)
{
stsel1[i+j*natoms] = ref[i+j*natoms] - ref_com[j];
stsel2[i+j*natoms] = tar[i+j*natoms] - tar_com[j];
}
}
// Initial residual
float E0 = sdot(ncoords,&stsel1[0],1,&stsel1[0],1)+sdot(ncoords,&stsel2[0],1,&stsel2[0],1) ;
// dot(target_transpose,reference)
std::vector<float> T1_dot_2(3*natoms,0);
sgemm('T','N',3,natoms,natoms,1,&stsel2[0],natoms,&stsel1[0],natoms,1,&T1_dot_2[0],3);
// SVD of the dot product
std::vector<float> U(9,0), S(3,0), V(9,0), work(5*9,0);
int info;
sgesvd('A','A',3,3,&T1_dot_2[0],3,&S[0],&U[0],3,&V[0],3,&work[0],9*5,info);
/*std::cout << "\n S: ";
for (int i=0;i<3;i++)
{
std::cout << S[i] << " ";
}
std::cout << "\n U: ";
for (int i=0;i<9;i++)
{
std::cout << U[i] << " ";
}*/
float reflect = det3x3(&U[0]) * det3x3(&V[0]);
if ( 1 - reflect > 1e-5)
{
S[2] = -S[2];
U[6] = -U[6];
U[7] = -U[7];
U[8] = -U[8];
}
float rmsd = sqrt(fabs(
E0
- (2.0 *
(S[0]+S[1]+S[2])
)
)
/natoms);
// Rotation matrix is dot(U,V)
std::vector<float> M(9,0);
sgemm('N','N',3,3,3,1,&U[0],3,&V[0],3,1,&M[0],3);
/*
M = [ 0 3 6 ] = [ 00 01 02 ]
[ 1 4 7 ] [ 10 11 12 ]
[ 2 5 8 ] [ 20 21 22 ]
*/
float trace = M[0]+M[4]+M[8];
float angle = acos((trace-1)/2)*RAD2DEG;
float rx,ry,rz,ux,uy,uz;
rx = atan2(M[5],M[8])*RAD2DEG;
ry = atan2(-M[2],sqrt(M[5]*M[5]+M[8]*M[8]))*RAD2DEG;
rz = atan2(M[1],M[0])*RAD2DEG;
float zeta = sqrt(
(M[5]-M[7])*(M[5]-M[7])
+ (M[6]-M[2])*(M[6]-M[2])
+ (M[3]-M[1])*(M[3]-M[1])
);
//std::cout << "\n" << M[5] << " - " << M[7] << " = " << M[5]-M[7];
//std::cout << "\n" << M[6] << " - " << M[2] << " = " << M[6]-M[2];
//std::cout << "\n" << M[3] << " - " << M[1] << " = " << M[3]-M[1] << std::endl;
ux = (M[5]-M[7])/zeta;
uy = (M[6]-M[2])/zeta;
uz = (M[3]-M[1])/zeta;
//std::cout << zeta << " { " << ux << " " << uy << " " << uz << " }" << sqrt(ux*ux+uy*uy+uz*uz) << std:: endl;
if (bVerbose)
{
fprintf(stdout,"%12s%12s%12s%12s%12s%12s%12s%12s\n","Angle(deg)","rmsd(nm)","x(deg)","y(deg)","z(deg)","ux(nm)","uy(nm)","uz(nm)");
fprintf(stdout,"%12.3f%12.6f%12.4f%12.4f%12.4f%12.4f%12.4f%12.4f\n",angle,rmsd,rx,ry,rz,ux,uy,uz);
}
data.rotation.push_back(angle);
data.rmsd.push_back(rmsd);
data.x_rotation.push_back(rx);
data.y_rotation.push_back(ry);
data.z_rotation.push_back(rz);
data.x_rotation_axis.push_back(ux);
data.y_rotation_axis.push_back(uy);
data.z_rotation_axis.push_back(uz);
return;
}
