static void get_minv(tensor A,tensor B)
{
int m,n;
double fac,rfac;
tensor tmp;
tmp[XX][XX] = A[YY][YY] + A[ZZ][ZZ];
tmp[YY][XX] = -A[XX][YY];
tmp[ZZ][XX] = -A[XX][ZZ];
tmp[XX][YY] = -A[XX][YY];
tmp[YY][YY] = A[XX][XX] + A[ZZ][ZZ];
tmp[ZZ][YY] = -A[YY][ZZ];
tmp[XX][ZZ] = -A[XX][ZZ];
tmp[YY][ZZ] = -A[YY][ZZ];
tmp[ZZ][ZZ] = A[XX][XX] + A[YY][YY];
/* This is a hack to prevent very large determinants */
rfac = (tmp[XX][XX]+tmp[YY][YY]+tmp[ZZ][ZZ])/3;
if (rfac == 0.0)
gmx_fatal(FARGS,"Can not stop center of mass: maybe 2dimensional system");
fac = 1.0/rfac;
for(m=0; (m<DIM); m++)
for(n=0; (n<DIM); n++)
tmp[m][n] *= fac;
m_inv(tmp,B);
for(m=0; (m<DIM); m++)
for(n=0; (n<DIM); n++)
B[m][n] *= fac;
}
static void init_proj_matrix(settleparam_t *p,
real invmO, real invmH, real dOH, real dHH)
{
real imOn, imHn;
matrix mat;
p->imO = invmO;
p->imH = invmH;
/* We normalize the inverse masses with imO for the matrix inversion.
* so we can keep using masses of almost zero for frozen particles,
* without running out of the float range in m_inv.
*/
imOn = 1;
imHn = p->imH/p->imO;
/* Construct the constraint coupling matrix */
mat[0][0] = imOn + imHn;
mat[0][1] = imOn*(1 - 0.5*dHH*dHH/(dOH*dOH));
mat[0][2] = imHn*0.5*dHH/dOH;
mat[1][1] = mat[0][0];
mat[1][2] = mat[0][2];
mat[2][2] = imHn + imHn;
mat[1][0] = mat[0][1];
mat[2][0] = mat[0][2];
mat[2][1] = mat[1][2];
m_inv(mat, p->invmat);
msmul(p->invmat, 1/p->imO, p->invmat);
p->invdOH = 1/dOH;
p->invdHH = 1/dHH;
}
static void kalman_gain(
const covariance_t * predicted_covariance,
const matrix2d_t * measurement_noise_cov,
kalman_gain_t * dest)
{
matrix2d_t residual;
m_add(&(predicted_covariance->_cov_a), measurement_noise_cov, &residual);
uint8_t inverse_success = m_inv(&residual, &residual);
if(inverse_success); // TODO error handling
m_mult(&(predicted_covariance->_cov_a), &residual, &(dest->_k1));
m_mult(&(predicted_covariance->_cov_c), &residual, &(dest->_k2));
}
void cscipher_dec(void* buffer, const cscipher_ctx_t* ctx){
uint8_t i=7,j,k;
uint8_t tmp[8];
memxor(buffer, ctx->keys[8], 8);
do{
for(j=0; j<3; ++j){
for(k=0; k<4; ++k){
tmp[2*k] = ((uint8_t*)buffer)[k];
tmp[2*k+1] = ((uint8_t*)buffer)[4+k];
}
for(k=0; k<4; ++k){
((uint16_t*)buffer)[k] = m_inv(((uint16_t*)tmp)[k]);
}
if(j==2){
memxor(buffer, ctx->keys[i], 8);
}else{
memxor(buffer, round_const+((j==1)?0:8), 8);
}
}
}while(i--);
}
m_complex m_div(m_complex a, m_complex b)
{
return m_mul(a, m_inv(b));
}
void settle_proj(FILE *fp,int nsettle, t_iatom iatoms[],rvec x[],
real dOH,real dHH,real invmO,real invmH,
rvec *der,rvec *derp,
bool bCalcVir,tensor rmdder)
{
/* Settle for projection out constraint components
* of derivatives of the coordinates.
* Berk Hess 2008-1-10
*/
/* Initialized data */
static bool bFirst=TRUE;
static real imO,imH,invdOH,invdHH;
static matrix invmat;
real imOn,imHn;
matrix mat;
int i,m,m2,ow1,hw2,hw3;
rvec roh2,roh3,rhh,dc,fc;
if (bFirst) {
if (fp)
fprintf(fp,"Going to use settle for derivatives (%d waters)\n",nsettle);
imO = invmO;
imH = invmH;
/* We normalize the inverse masses with imO for the matrix inversion.
* so we can keep using masses of almost zero for frozen particles,
* without running out of the float range in m_inv.
*/
imOn = 1;
imHn = imH/imO;
/* Construct the constraint coupling matrix */
mat[0][0] = imOn + imHn;
mat[0][1] = imOn*(1 - 0.5*dHH*dHH/(dOH*dOH));
mat[0][2] = imHn*0.5*dHH/dOH;
mat[1][1] = mat[0][0];
mat[1][2] = mat[0][2];
mat[2][2] = imHn + imHn;
mat[1][0] = mat[0][1];
mat[2][0] = mat[0][2];
mat[2][1] = mat[1][2];
m_inv(mat,invmat);
msmul(invmat,1/imO,invmat);
invdOH = 1/dOH;
invdHH = 1/dHH;
bFirst = FALSE;
}
#ifdef PRAGMAS
#pragma ivdep
#endif
for (i=0; i<nsettle; i++) {
ow1 = iatoms[i*2+1];
hw2 = ow1 + 1;
hw3 = ow1 + 2;
for(m=0; m<DIM; m++)
roh2[m] = (x[ow1][m] - x[hw2][m])*invdOH;
for(m=0; m<DIM; m++)
roh3[m] = (x[ow1][m] - x[hw3][m])*invdOH;
for(m=0; m<DIM; m++)
rhh [m] = (x[hw2][m] - x[hw3][m])*invdHH;
/* 18 flops */
/* Determine the projections of der on the bonds */
clear_rvec(dc);
for(m=0; m<DIM; m++)
dc[0] += (der[ow1][m] - der[hw2][m])*roh2[m];
for(m=0; m<DIM; m++)
dc[1] += (der[ow1][m] - der[hw3][m])*roh3[m];
for(m=0; m<DIM; m++)
dc[2] += (der[hw2][m] - der[hw3][m])*rhh [m];
/* 27 flops */
/* Determine the correction for the three bonds */
mvmul(invmat,dc,fc);
/* 15 flops */
/* Subtract the corrections from derp */
for(m=0; m<DIM; m++) {
derp[ow1][m] -= imO*( fc[0]*roh2[m] + fc[1]*roh3[m]);
derp[hw2][m] -= imH*(-fc[0]*roh2[m] + fc[2]*rhh [m]);
derp[hw3][m] -= imH*(-fc[1]*roh3[m] - fc[2]*rhh [m]);
}
/* 45 flops */
if (bCalcVir) {
/* Determining r x m der is easy,
* since fc contains the mass weighted corrections for der.
*/
for(m=0; m<DIM; m++) {
for(m2=0; m2<DIM; m2++) {
rmdder[m][m2] +=
dOH*roh2[m]*roh2[m2]*fc[0] +
dOH*roh3[m]*roh3[m2]*fc[1] +
dHH*rhh [m]*rhh [m2]*fc[2];
}
}
}
}
}
int nsc_dclm_pbc(const rvec *coords, real *radius, int nat,
int densit, int mode,
real *value_of_area, real **at_area,
real *value_of_vol,
real **lidots, int *nu_dots,
atom_id index[], int ePBC, matrix box)
{
int iat, i, ii, iii, ix, iy, iz, ixe, ixs, iye, iys, ize, izs, i_ac;
int jat, j, jj, jjj, jx, jy, jz;
int distribution;
int l;
int maxnei, nnei, last, maxdots = 0;
int *wkdot = NULL, *wkbox = NULL, *wkat1 = NULL, *wkatm = NULL;
Neighb *wknb, *ctnb;
int iii1, iii2, iiat, lfnr = 0, i_at, j_at;
real dx, dy, dz, dd, ai, aisq, ajsq, aj, as, a;
real xi, yi, zi, xs = 0., ys = 0., zs = 0.;
real dotarea, area, vol = 0.;
real *xus, *dots = NULL, *atom_area = NULL;
int nxbox, nybox, nzbox, nxy, nxyz;
real xmin = 0, ymin = 0, zmin = 0, xmax, ymax, zmax, ra2max, d;
const real *pco;
/* Added DvdS 2006-07-19 */
t_pbc pbc;
rvec ddx, *x = NULL;
int iat_xx, jat_xx;
distribution = unsp_type(densit);
if (distribution != -last_unsp || last_cubus != 4 ||
(densit != last_densit && densit != last_n_dot))
{
if (make_unsp(densit, (-distribution), &n_dot, 4))
{
return 1;
}
}
xus = xpunsp;
dotarea = FOURPI/(real) n_dot;
area = 0.;
if (debug)
{
fprintf(debug, "nsc_dclm: n_dot=%5d %9.3f\n", n_dot, dotarea);
}
/* start with neighbour list */
/* calculate neighbour list with the box algorithm */
if (nat == 0)
{
WARNING("nsc_dclm: no surface atoms selected");
return 1;
}
if (mode & FLAG_VOLUME)
{
vol = 0.;
}
if (mode & FLAG_DOTS)
{
maxdots = (3*n_dot*nat)/10;
/* should be set to NULL on first user call */
if (dots == NULL)
{
snew(dots, maxdots);
}
else
{
srenew(dots, maxdots);
}
lfnr = 0;
}
if (mode & FLAG_ATOM_AREA)
{
/* should be set to NULL on first user call */
if (atom_area == NULL)
{
snew(atom_area, nat);
}
else
{
srenew(atom_area, nat);
}
}
/* Compute minimum size for grid cells */
ra2max = radius[index[0]];
for (iat_xx = 1; (iat_xx < nat); iat_xx++)
{
iat = index[iat_xx];
ra2max = max(ra2max, radius[iat]);
}
ra2max = 2*ra2max;
/* Added DvdS 2006-07-19 */
/* Updated 2008-10-09 */
if (box)
{
set_pbc(&pbc, ePBC, box);
snew(x, nat);
for (i = 0; (i < nat); i++)
{
iat = index[0];
copy_rvec(coords[iat], x[i]);
}
put_atoms_in_triclinic_unitcell(ecenterTRIC, box, nat, x);
nxbox = max(1, floor(norm(box[XX])/ra2max));
nybox = max(1, floor(norm(box[YY])/ra2max));
nzbox = max(1, floor(norm(box[ZZ])/ra2max));
if (debug)
{
fprintf(debug, "nbox = %d, %d, %d\n", nxbox, nybox, nzbox);
}
}
else
{
/* dimensions of atomic set, cell edge is 2*ra_max */
iat = index[0];
xmin = coords[iat][XX]; xmax = xmin; xs = xmin;
ymin = coords[iat][YY]; ymax = ymin; ys = ymin;
zmin = coords[iat][ZZ]; zmax = zmin; zs = zmin;
for (iat_xx = 1; (iat_xx < nat); iat_xx++)
{
iat = index[iat_xx];
pco = coords[iat];
xmin = min(xmin, *pco); xmax = max(xmax, *pco);
ymin = min(ymin, *(pco+1)); ymax = max(ymax, *(pco+1));
zmin = min(zmin, *(pco+2)); zmax = max(zmax, *(pco+2));
xs = xs+ *pco; ys = ys+ *(pco+1); zs = zs+ *(pco+2);
}
xs = xs/ (real) nat;
ys = ys/ (real) nat;
zs = zs/ (real) nat;
if (debug)
{
fprintf(debug, "nsc_dclm: n_dot=%5d ra2max=%9.3f %9.3f\n", n_dot, ra2max, dotarea);
}
d = xmax-xmin; nxbox = (int) max(ceil(d/ra2max), 1.);
d = (((real)nxbox)*ra2max-d)/2.;
xmin = xmin-d; xmax = xmax+d;
d = ymax-ymin; nybox = (int) max(ceil(d/ra2max), 1.);
d = (((real)nybox)*ra2max-d)/2.;
ymin = ymin-d; ymax = ymax+d;
d = zmax-zmin; nzbox = (int) max(ceil(d/ra2max), 1.);
d = (((real)nzbox)*ra2max-d)/2.;
zmin = zmin-d; zmax = zmax+d;
}
/* Help variables */
nxy = nxbox*nybox;
nxyz = nxy*nzbox;
/* box number of atoms */
snew(wkatm, nat);
snew(wkat1, nat);
snew(wkdot, n_dot);
snew(wkbox, nxyz+1);
if (box)
{
matrix box_1;
rvec x_1;
int ix, iy, iz, m;
m_inv(box, box_1);
for (i = 0; (i < nat); i++)
{
mvmul(box_1, x[i], x_1);
ix = ((int)floor(x_1[XX]*nxbox) + 2*nxbox) % nxbox;
iy = ((int)floor(x_1[YY]*nybox) + 2*nybox) % nybox;
iz = ((int)floor(x_1[ZZ]*nzbox) + 2*nzbox) % nzbox;
j = ix + iy*nxbox + iz*nxbox*nybox;
if (debug)
{
fprintf(debug, "Atom %d cell index %d. x = (%8.3f,%8.3f,%8.3f) fc = (%8.3f,%8.3f,%8.3f)\n",
i, j, x[i][XX], x[i][YY], x[i][ZZ], x_1[XX], x_1[YY], x_1[ZZ]);
}
range_check(j, 0, nxyz);
wkat1[i] = j;
wkbox[j]++;
}
}
else
{
/* Put the atoms in their boxes */
for (iat_xx = 0; (iat_xx < nat); iat_xx++)
{
iat = index[iat_xx];
pco = coords[iat];
i = (int) max(floor((pco[XX]-xmin)/ra2max), 0);
i = min(i, nxbox-1);
j = (int) max(floor((pco[YY]-ymin)/ra2max), 0);
j = min(j, nybox-1);
l = (int) max(floor((pco[ZZ]-zmin)/ra2max), 0);
l = min(l, nzbox-1);
i = i+j*nxbox+l*nxy;
wkat1[iat_xx] = i;
wkbox[i]++;
}
}
/* sorting of atoms in accordance with box numbers */
j = wkbox[0];
for (i = 1; i < nxyz; i++)
{
j = max(wkbox[i], j);
}
for (i = 1; i <= nxyz; i++)
{
wkbox[i] += wkbox[i-1];
}
/* maxnei = (int) floor(ra2max*ra2max*ra2max*0.5); */
maxnei = min(nat, 27*j);
snew(wknb, maxnei);
for (iat_xx = 0; iat_xx < nat; iat_xx++)
{
iat = index[iat_xx];
range_check(wkat1[iat_xx], 0, nxyz);
wkatm[--wkbox[wkat1[iat_xx]]] = iat_xx;
if (debug)
{
fprintf(debug, "atom %5d on place %5d\n", iat, wkbox[wkat1[iat_xx]]);
}
}
if (debug)
{
fprintf(debug, "nsc_dclm: n_dot=%5d ra2max=%9.3f %9.3f\n",
n_dot, ra2max, dotarea);
fprintf(debug, "neighbour list calculated/box(xyz):%d %d %d\n",
nxbox, nybox, nzbox);
for (i = 0; i < nxyz; i++)
{
fprintf(debug, "box %6d : atoms %4d-%4d %5d\n",
i, wkbox[i], wkbox[i+1]-1, wkbox[i+1]-wkbox[i]);
}
for (i = 0; i < nat; i++)
{
fprintf(debug, "list place %5d by atom %7d\n", i, index[wkatm[i]]);
}
}
/* calculate surface for all atoms, step cube-wise */
for (iz = 0; iz < nzbox; iz++)
{
iii = iz*nxy;
if (box)
{
izs = iz-1;
ize = min(iz+2, izs+nzbox);
}
else
{
izs = max(iz-1, 0);
ize = min(iz+2, nzbox);
}
for (iy = 0; iy < nybox; iy++)
{
ii = iy*nxbox+iii;
if (box)
{
iys = iy-1;
iye = min(iy+2, iys+nybox);
}
else
{
iys = max(iy-1, 0);
iye = min(iy+2, nybox);
}
for (ix = 0; ix < nxbox; ix++)
{
i = ii+ix;
iii1 = wkbox[i];
iii2 = wkbox[i+1];
if (iii1 >= iii2)
{
continue;
}
if (box)
{
ixs = ix-1;
ixe = min(ix+2, ixs+nxbox);
}
else
{
ixs = max(ix-1, 0);
ixe = min(ix+2, nxbox);
}
iiat = 0;
/* make intermediate atom list */
for (jz = izs; jz < ize; jz++)
{
jjj = ((jz+nzbox) % nzbox)*nxy;
for (jy = iys; jy < iye; jy++)
{
jj = ((jy+nybox) % nybox)*nxbox+jjj;
for (jx = ixs; jx < ixe; jx++)
{
j = jj+((jx+nxbox) % nxbox);
for (jat = wkbox[j]; jat < wkbox[j+1]; jat++)
{
range_check(wkatm[jat], 0, nat);
range_check(iiat, 0, nat);
wkat1[iiat] = wkatm[jat];
iiat++;
} /* end of cycle "jat" */
} /* end of cycle "jx" */
} /* end of cycle "jy" */
} /* end of cycle "jz" */
for (iat = iii1; iat < iii2; iat++)
{
i_at = index[wkatm[iat]];
ai = radius[i_at];
aisq = ai*ai;
pco = coords[i_at];
xi = pco[XX]; yi = pco[YY]; zi = pco[ZZ];
for (i = 0; i < n_dot; i++)
{
wkdot[i] = 0;
}
ctnb = wknb; nnei = 0;
for (j = 0; j < iiat; j++)
{
j_at = index[wkat1[j]];
if (j_at == i_at)
{
continue;
}
aj = radius[j_at];
ajsq = aj*aj;
pco = coords[j_at];
/* Added DvdS 2006-07-19 */
if (box)
{
/*rvec xxi;
xxi[XX] = xi;
xxi[YY] = yi;
xxi[ZZ] = zi;
pbc_dx(&pbc,pco,xxi,ddx);*/
pbc_dx(&pbc, coords[j_at], coords[i_at], ddx);
dx = ddx[XX];
dy = ddx[YY];
dz = ddx[ZZ];
}
else
{
dx = pco[XX]-xi;
dy = pco[YY]-yi;
dz = pco[ZZ]-zi;
}
dd = dx*dx+dy*dy+dz*dz;
as = ai+aj;
if (dd > as*as)
{
continue;
}
nnei++;
ctnb->x = dx;
ctnb->y = dy;
ctnb->z = dz;
ctnb->dot = (dd+aisq-ajsq)/(2.*ai); /* reference dot product */
ctnb++;
}
/* check points on accessibility */
if (nnei)
{
last = 0; i_ac = 0;
for (l = 0; l < n_dot; l++)
{
if (xus[3*l]*(wknb+last)->x+
xus[1+3*l]*(wknb+last)->y+
xus[2+3*l]*(wknb+last)->z <= (wknb+last)->dot)
{
for (j = 0; j < nnei; j++)
{
if (xus[3*l]*(wknb+j)->x+xus[1+3*l]*(wknb+j)->y+
xus[2+3*l]*(wknb+j)->z > (wknb+j)->dot)
{
last = j;
break;
}
}
if (j >= nnei)
{
i_ac++;
wkdot[l] = 1;
}
} /* end of cycle j */
} /* end of cycle l */
}
else
{
i_ac = n_dot;
for (l = 0; l < n_dot; l++)
{
wkdot[l] = 1;
}
}
if (debug)
{
fprintf(debug, "i_ac=%d, dotarea=%8.3f, aisq=%8.3f\n",
i_ac, dotarea, aisq);
}
a = aisq*dotarea* (real) i_ac;
area = area + a;
if (mode & FLAG_ATOM_AREA)
{
range_check(wkatm[iat], 0, nat);
atom_area[wkatm[iat]] = a;
}
if (mode & FLAG_DOTS)
{
for (l = 0; l < n_dot; l++)
{
if (wkdot[l])
{
lfnr++;
if (maxdots <= 3*lfnr+1)
{
maxdots = maxdots+n_dot*3;
srenew(dots, maxdots);
}
dots[3*lfnr-3] = ai*xus[3*l]+xi;
dots[3*lfnr-2] = ai*xus[1+3*l]+yi;
dots[3*lfnr-1] = ai*xus[2+3*l]+zi;
}
}
}
if (mode & FLAG_VOLUME)
{
dx = 0.; dy = 0.; dz = 0.;
for (l = 0; l < n_dot; l++)
{
if (wkdot[l])
{
dx = dx+xus[3*l];
dy = dy+xus[1+3*l];
dz = dz+xus[2+3*l];
}
}
vol = vol+aisq*(dx*(xi-xs)+dy*(yi-ys)+dz*(zi-zs)+ai* (real) i_ac);
}
} /* end of cycle "iat" */
} /* end of cycle "ix" */
} /* end of cycle "iy" */
} /* end of cycle "iz" */
sfree(wkatm);
sfree(wkat1);
sfree(wkdot);
sfree(wkbox);
sfree(wknb);
if (box)
{
sfree(x);
}
if (mode & FLAG_VOLUME)
{
vol = vol*FOURPI/(3.* (real) n_dot);
*value_of_vol = vol;
}
if (mode & FLAG_DOTS)
{
*nu_dots = lfnr;
*lidots = dots;
}
if (mode & FLAG_ATOM_AREA)
{
*at_area = atom_area;
}
*value_of_area = area;
if (debug)
{
fprintf(debug, "area=%8.3f\n", area);
}
return 0;
}
