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; }