/* energy gradient (helper function) */
void addEgradt(double *dy, double *yt, double *rhot, int L, int R, int dim, int CSP, int CSD, double *scales2, double *scaleweight2, double energyweight)
{
int CSPL = CSP*L;
int CSDL = 1;
int i, l, sl, si;
if (dim == 2) {
#pragma omp parallel for schedule(static) shared(dy,yt,rhot,L,R,dim,CSP,CSD,CSPL,CSDL,scales2,scaleweight2,energyweight) private(i,l,sl,si)
for (i=0; i<L; i++) { /* particle */
double xi[2] = {rhot[INDRHOX(i,0)],rhot[INDRHOX(i,1)]};
/* debug */
/*mexPrintf("xi %f %f\n",xi[0],xi[1]);*/
for (l=0; l<L; l++) { /* particle */
double xl[2] = {rhot[INDRHOX(l,0)],rhot[INDRHOX(l,1)]};
/* debug */
/*mexPrintf("xl %f %f\n",xl[0],xl[1]);*/
double ximxl[2]; VECMINUS(ximxl,xi,xl);
double v = VECDOT2(ximxl,ximxl);
for (sl=0; sl<R; sl++) { /* scale */
double alsl[2] = {rhot[INDRHOP(l,0,sl)],rhot[INDRHOP(l,1,sl)]};
double aisl[2] = {rhot[INDRHOP(i,0,sl)],rhot[INDRHOP(i,1,sl)]};
double rsl2 = scales2[sl];
double swsl2 = scaleweight2[sl];
double kbasesl = exp(-v/rsl2);
double ksl = Ks(kbasesl,swsl2);
double d1ksl = D1Ks(kbasesl,rsl2,swsl2);
double d1kslXximxl[2]; VECSCALAR(d1kslXximxl,d1ksl,ximxl);
double d2ksl = D2Ks(kbasesl,rsl2,swsl2);
double aislTXalsl; VECDOT(aislTXalsl,aisl,alsl);
/* dx */
double vt1[2]; VECSCALAR(vt1,energyweight*4*d1ksl*aislTXalsl,ximxl);
dy[INDYX(i,0,0,0,0)] += vt1[0];
dy[INDYX(i,1,0,0,0)] += vt1[1];
/* da */
VECSCALAR(vt1,energyweight*2*ksl,alsl);
dy[INDYP(i,0,sl,0,0,0)] += vt1[0];
dy[INDYP(i,1,sl,0,0,0)] += vt1[1];
}
}
}
} else {
#pragma omp parallel for schedule(static) shared(dy,yt,rhot,L,R,dim,CSP,CSD,CSPL,CSDL,scales2,scaleweight2,energyweight) private(i,l,sl,si)
for (i=0; i<L; i++) { /* particle */
double xi[3] = {rhot[INDRHOX(i,0)],rhot[INDRHOX(i,1)],rhot[INDRHOX(i,2)]};
for (l=0; l<L; l++) { /* particle */
double xl[3] = {rhot[INDRHOX(l,0)],rhot[INDRHOX(l,1)],rhot[INDRHOX(l,2)]};
double ximxl[3]; _3VECMINUS(ximxl,xi,xl);
double v = _3VECDOT2(ximxl,ximxl);
for (sl=0; sl<R; sl++) { /* scale */
double alsl[3] = {rhot[INDRHOP(l,0,sl)],rhot[INDRHOP(l,1,sl)],rhot[INDRHOP(l,2,sl)]};
double aisl[3] = {rhot[INDRHOP(i,0,sl)],rhot[INDRHOP(i,1,sl)],rhot[INDRHOP(i,2,sl)]};
double rsl2 = scales2[sl];
double swsl2 = scaleweight2[sl];
double kbasesl = exp(-v/rsl2);
double ksl = Ks(kbasesl,swsl2);
double d1ksl = D1Ks(kbasesl,rsl2,swsl2);
double d1kslXximxl[3]; _3VECSCALAR(d1kslXximxl,d1ksl,ximxl);
double d2ksl = D2Ks(kbasesl,rsl2,swsl2);
double aislTXalsl; _3VECDOT(aislTXalsl,aisl,alsl);
/* dx */
double vt1[3]; _3VECSCALAR(vt1,energyweight*4*d1ksl*aislTXalsl,ximxl);
dy[INDYX(i,0,0,0,0)] += vt1[0];
dy[INDYX(i,1,0,0,0)] += vt1[1];
dy[INDYX(i,2,0,0,0)] += vt1[2];
/* da */
_3VECSCALAR(vt1,energyweight*2*ksl,alsl);
dy[INDYP(i,0,sl,0,0,0)] += vt1[0];
dy[INDYP(i,1,sl,0,0,0)] += vt1[1];
dy[INDYP(i,2,sl,0,0,0)] += vt1[2];
}
}
}
}
}
int Fbonded_eval_impr_term(Fbonded *p,
const ImprPrm *prm,
const dvec *pos_i,
const dvec *pos_j,
const dvec *pos_k,
const dvec *pos_l,
dvec *f_i,
dvec *f_j,
dvec *f_k,
dvec *f_l,
dreal *u,
dreal virial[NELEMS_VIRIAL]) {
dvec r12, r23, r34;
dvec A, B, C;
dreal rA, rB, rC;
dreal cos_phi, sin_phi, phi;
dreal K, K1;
dvec f1, f2, f3;
dreal k = prm->k_impr;
dreal delta = prm->psi0;
dreal diff;
Domain_shortest_vec(p->domain, &r12, pos_i, pos_j);
Domain_shortest_vec(p->domain, &r23, pos_j, pos_k);
Domain_shortest_vec(p->domain, &r34, pos_k, pos_l);
VECCROSS(A, r12, r23);
VECCROSS(B, r23, r34);
VECCROSS(C, r23, A);
rA = 1 / sqrt(VECLEN2(A));
rB = 1 / sqrt(VECLEN2(B));
rC = 1 / sqrt(VECLEN2(C));
VECMUL(B, rB, B); /* normalize B */
cos_phi = VECDOT(A, B) * rA;
sin_phi = VECDOT(C, B) * rC;
phi = -atan2(sin_phi, cos_phi);
diff = phi - delta;
if (diff < -M_PI) diff += 2.0 * M_PI;
else if (diff > M_PI) diff -= 2.0 * M_PI;
K = k * diff * diff;
K1 = 2.0 * k * diff;
if (fabs(sin_phi) > 0.1) {
dvec dcosdA, dcosdB;
dvec tv1, tv2;
/* use sine version to avoid 1/cos terms */
VECMUL(A, rA, A); /* normalize A */
VECMSUB(dcosdA, cos_phi, A, B);
VECMUL(dcosdA, rA, dcosdA);
VECMSUB(dcosdB, cos_phi, B, A);
VECMUL(dcosdB, rB, dcosdB);
K1 /= sin_phi;
VECCROSS(f1, r23, dcosdA);
VECMUL(f1, K1, f1);
VECCROSS(f3, dcosdB, r23);
VECMUL(f3, K1, f3);
VECCROSS(tv1, dcosdA, r12);
VECCROSS(tv2, r34, dcosdB);
VECADD(f2, tv1, tv2);
VECMUL(f2, K1, f2);
}
else {
dvec dsindB, dsindC;
/* phi is too close to 0 or pi, use cos version to avoid 1/sin */
VECMUL(C, rC, C); /* normalize C */
VECMSUB(dsindC, sin_phi, C, B);
VECMUL(dsindC, rC, dsindC);
VECMSUB(dsindB, sin_phi, B, C);
VECMUL(dsindB, rB, dsindB);
K1 /= -cos_phi;
f1.x = K1 * ((r23.y * r23.y + r23.z * r23.z) * dsindC.x
- r23.x * r23.y * dsindC.y
- r23.x * r23.z * dsindC.z);
f1.y = K1 * ((r23.z * r23.z + r23.x * r23.x) * dsindC.y
- r23.y * r23.z * dsindC.z
- r23.y * r23.x * dsindC.x);
f1.z = K1 * ((r23.x * r23.x + r23.y * r23.y) * dsindC.z
- r23.z * r23.x * dsindC.x
- r23.z * r23.y * dsindC.y);
VECCROSS(f3, dsindB, r23);
VECMUL(f3, K1, f3);
f2.x = K1 * (-(r23.y * r12.y + r23.z * r12.z) * dsindC.x
+ (2.0 * r23.x * r12.y - r12.x * r23.y) * dsindC.y
+ (2.0 * r23.x * r12.z - r12.x * r23.z) * dsindC.z
+ dsindB.z * r34.y - dsindB.y * r34.z);
f2.y = K1 * (-(r23.z * r12.z + r23.x * r12.x) * dsindC.y
+ (2.0 * r23.y * r12.z - r12.y * r23.z) * dsindC.z
+ (2.0 * r23.y * r12.x - r12.y * r23.x) * dsindC.x
+ dsindB.x * r34.z - dsindB.z * r34.x);
f2.z = K1 * (-(r23.x * r12.x + r23.y * r12.y) * dsindC.z
+ (2.0 * r23.z * r12.x - r12.z * r23.x) * dsindC.x
+ (2.0 * r23.z * r12.y - r12.z * r23.y) * dsindC.y
+ dsindB.y * r34.x - dsindB.x * r34.y);
}
*u += K;
VECADD(*f_i, *f_i, f1);
f_j->x += f2.x - f1.x;
f_j->y += f2.y - f1.y;
f_j->z += f2.z - f1.z;
f_k->x += f3.x - f2.x;
f_k->y += f3.y - f2.y;
f_k->z += f3.z - f2.z;
VECSUB(*f_l, *f_l, f3);
virial[VIRIAL_XX] += (f1.x * r12.x + f2.x * r23.x + f3.x * r34.x);
virial[VIRIAL_XY] += (f1.x * r12.y + f2.x * r23.y + f3.x * r34.y);
virial[VIRIAL_XZ] += (f1.x * r12.z + f2.x * r23.z + f3.x * r34.z);
virial[VIRIAL_YY] += (f1.y * r12.y + f2.y * r23.y + f3.y * r34.y);
virial[VIRIAL_YZ] += (f1.y * r12.z + f2.y * r23.z + f3.y * r34.z);
virial[VIRIAL_ZZ] += (f1.z * r12.z + f2.z * r23.z + f3.z * r34.z);
return OK;
}
/* The computational routine */
void gradOrder1ScaleC(double *dy, double *yt, double *rhot, int L, int R, int dim, int CSP, int CSD, double *scales2, double *scaleweight2, double energyweight)
{
/* dy already initialized to zeros */
int CSPL = CSP*L;
int CSDL = 1;
double invR = 1.0/R;
int i, l, sl, si;
if (dim == 2) {
#pragma omp parallel for schedule(static) shared(dy,yt,rhot,L,R,dim,CSP,CSD,CSPL,CSDL,scales2,scaleweight2) private(i,l,sl,si)
for (i=0; i<L; i++) { /* particle */
double xi[2] = {rhot[INDRHOX(i,0)],rhot[INDRHOX(i,1)]};
double dxi[2] = {yt[INDYX(i,0,0,0,0)],yt[INDYX(i,1,0,0,0)]};
for (l=0; l<L; l++) { /* particle */
double xl[2] = {rhot[INDRHOX(l,0)],rhot[INDRHOX(l,1)]};
double dxl[2] = {yt[INDYX(l,0,0,0,0)],yt[INDYX(l,1,0,0,0)]};
double ximxl[2]; VECMINUS(ximxl,xi,xl);
double v = VECDOT2(ximxl,ximxl);
for (sl=0; sl<R; sl++) { /* scale */
double alsl[2] = {rhot[INDRHOP(l,0,sl)],rhot[INDRHOP(l,1,sl)]};
double aisl[2] = {rhot[INDRHOP(i,0,sl)],rhot[INDRHOP(i,1,sl)]};
double rsl2 = scales2[sl];
double swsl2 = scaleweight2[sl];
double kbasesl = exp(-v/rsl2);
double d1ksl = D1Ks(kbasesl,rsl2,swsl2);
double d1kslXximxl[2]; VECSCALAR(d1kslXximxl,d1ksl,ximxl);
double dalsl[2] = {yt[INDYP(l,0,sl,0,0,0)],yt[INDYP(l,1,sl,0,0,0)]};
/* dx */
double rt1; VECDOT(rt1,aisl,dxl);
double rt2; VECDOT(rt2,alsl,dxi);
double vt1[2]; VECSCALAR(vt1,2*(rt1+rt2),d1kslXximxl);
dy[INDYX(i,0,0,0,0)] += vt1[0];
dy[INDYX(i,1,0,0,0)] += vt1[1];
for (si=0; si<R; si++) { /* scale */
double alsi[2] = {rhot[INDRHOP(l,0,si)],rhot[INDRHOP(l,1,si)]};
double aisi[2] = {rhot[INDRHOP(i,0,si)],rhot[INDRHOP(i,1,si)]};
double daisi[2] = {yt[INDYP(i,0,si,0,0,0)],yt[INDYP(i,1,si,0,0,0)]};
double daisl[2] = {yt[INDYP(i,0,sl,0,0,0)],yt[INDYP(i,1,sl,0,0,0)]};
double rsi2 = scales2[si];
double swsi2 = scaleweight2[si];
double kbasesi = exp(-v/rsi2);
double ksi = Ks(kbasesi,swsi2);
double d1ksi = D1Ks(kbasesi,rsi2,swsi2);
double d2ksi = D2Ks(kbasesi,rsi2,swsi2);
double aislTXalsi; VECDOT(aislTXalsi,aisl,alsi);
double aisiTXalsl; VECDOT(aisiTXalsl,aisi,alsl);
/* dx */
VECSCALAR(vt1,aislTXalsi,daisl);
double vt2[2]; VECSCALAR(vt2,aisiTXalsl,dalsl);
double vt3[2]; VECMINUS(vt3,vt1,vt2);
double rt; VECDOT(rt,ximxl,vt3);
VECSCALAR(vt1,-2*d1ksi,vt3);
VECSCALAR(vt2,-4*d2ksi*rt,ximxl);
VECADD(vt3,vt1,vt2);
dy[INDYX(i,0,0,0,0)] += vt3[0];
dy[INDYX(i,1,0,0,0)] += vt3[1];
/* da */
VECSCALAR(vt1,d1ksl,daisi);
VECSCALAR(vt2,d1ksi,dalsl);
VECMINUS(vt3,vt1,vt2);
VECDOT(rt,ximxl,vt3);
VECSCALAR(vt1,-2*rt,alsl);
VECSCALAR(vt2,invR*ksi,dxl);
VECADD(vt3,vt1,vt2);
dy[INDYP(i,0,si,0,0,0)] += vt3[0];
dy[INDYP(i,1,si,0,0,0)] += vt3[1];
}
}
}
}
} else {
/* #pragma omp parallel for schedule(static) shared(dy,yt,rhot,L,R,dim,CSP,CSD,CSPL,CSDL,scales2,scaleweight2) private(i,l,sl,si)*/
for (i=0; i<L; i++) { /* particle */
double xi[3] = {rhot[INDRHOX(i,0)],rhot[INDRHOX(i,1)],rhot[INDRHOX(i,2)]};
double dxi[3] = {yt[INDYX(i,0,0,0,0)],yt[INDYX(i,1,0,0,0)],yt[INDYX(i,2,0,0,0)]};
for (l=0; l<L; l++) { /* particle */
double xl[3] = {rhot[INDRHOX(l,0)],rhot[INDRHOX(l,1)],rhot[INDRHOX(l,2)]};
double dxl[3] = {yt[INDYX(l,0,0,0,0)],yt[INDYX(l,1,0,0,0)],yt[INDYX(l,2,0,0,0)]};
double ximxl[3]; _3VECMINUS(ximxl,xi,xl);
double v = _3VECDOT2(ximxl,ximxl);
for (sl=0; sl<R; sl++) { /* scale */
double alsl[3] = {rhot[INDRHOP(l,0,sl)],rhot[INDRHOP(l,1,sl)],rhot[INDRHOP(l,2,sl)]};
double aisl[3] = {rhot[INDRHOP(i,0,sl)],rhot[INDRHOP(i,1,sl)],rhot[INDRHOP(i,2,sl)]};
double rsl2 = scales2[sl];
double swsl2 = scaleweight2[sl];
double kbasesl = exp(-v/rsl2);
double d1ksl = D1Ks(kbasesl,rsl2,swsl2);
double d1kslXximxl[3]; _3VECSCALAR(d1kslXximxl,d1ksl,ximxl);
double dalsl[3] = {yt[INDYP(l,0,sl,0,0,0)],yt[INDYP(l,1,sl,0,0,0)],yt[INDYP(l,2,sl,0,0,0)]};
/* dx */
double rt1; _3VECDOT(rt1,aisl,dxl);
double rt2; _3VECDOT(rt2,alsl,dxi);
double vt1[3]; _3VECSCALAR(vt1,2*(rt1+rt2),d1kslXximxl);
dy[INDYX(i,0,0,0,0)] += vt1[0];
dy[INDYX(i,1,0,0,0)] += vt1[1];
dy[INDYX(i,2,0,0,0)] += vt1[2];
for (si=0; si<R; si++) { /* scale */
double alsi[3] = {rhot[INDRHOP(l,0,si)],rhot[INDRHOP(l,1,si)],rhot[INDRHOP(l,2,si)]};
double aisi[3] = {rhot[INDRHOP(i,0,si)],rhot[INDRHOP(i,1,si)],rhot[INDRHOP(i,2,si)]};
double daisi[3] = {yt[INDYP(i,0,si,0,0,0)],yt[INDYP(i,1,si,0,0,0)],yt[INDYP(i,2,si,0,0,0)]};
double daisl[3] = {yt[INDYP(i,0,sl,0,0,0)],yt[INDYP(i,1,sl,0,0,0)],yt[INDYP(i,2,sl,0,0,0)]};
double rsi2 = scales2[si];
double swsi2 = scaleweight2[si];
double kbasesi = exp(-v/rsi2);
double ksi = Ks(kbasesi,swsi2);
double d1ksi = D1Ks(kbasesi,rsi2,swsi2);
double d2ksi = D2Ks(kbasesi,rsi2,swsi2);
double aislTXalsi; _3VECDOT(aislTXalsi,aisl,alsi);
double aisiTXalsl; _3VECDOT(aisiTXalsl,aisi,alsl);
/* dx */
_3VECSCALAR(vt1,aislTXalsi,daisl);
double vt2[3]; _3VECSCALAR(vt2,aisiTXalsl,dalsl);
double vt3[3]; _3VECMINUS(vt3,vt1,vt2);
double rt; _3VECDOT(rt,ximxl,vt3);
_3VECSCALAR(vt1,-2*d1ksi,vt3);
_3VECSCALAR(vt2,-4*d2ksi*rt,ximxl);
_3VECADD(vt3,vt1,vt2);
dy[INDYX(i,0,0,0,0)] += vt3[0];
dy[INDYX(i,1,0,0,0)] += vt3[1];
dy[INDYX(i,2,0,0,0)] += vt3[2];
/* da */
_3VECSCALAR(vt1,d1ksl,daisi);
_3VECSCALAR(vt2,d1ksi,dalsl);
_3VECMINUS(vt3,vt1,vt2);
_3VECDOT(rt,ximxl,vt3);
_3VECSCALAR(vt1,-2*rt,alsl);
_3VECSCALAR(vt2,invR*ksi,dxl);
_3VECADD(vt3,vt1,vt2);
dy[INDYP(i,0,si,0,0,0)] += vt3[0];
dy[INDYP(i,1,si,0,0,0)] += vt3[1];
dy[INDYP(i,2,si,0,0,0)] += vt3[2];
}
}
}
}
}
addEgradt(dy,yt,rhot,L,R,dim,CSP,CSD,scales2,scaleweight2,energyweight);
}
/* The computational routine */
double energyScaleC(double *Et, double *rhot, int L, int R, int dim, int CSP, double *scales2, double *scaleweight2)
{
/* Et already initialized to zeros */
int CSPL = CSP*L;
int i, l, sl;
double Ett = 0;
if (dim == 2) {
#pragma omp parallel for schedule(static) shared(rhot,L,R,dim,CSP,CSPL,scales2,scaleweight2) private(i,l,sl) reduction(+:Ett)
for (i=0; i<L; i++) { /* particle */
double xi[2] = {rhot[INDRHOX(i,0)],rhot[INDRHOX(i,1)]};
for (l=0; l<L; l++) { /* particle */
double xl[2] = {rhot[INDRHOX(l,0)],rhot[INDRHOX(l,1)]};
double ximxl[2]; VECMINUS(ximxl,xi,xl);
double v = VECDOT2(ximxl,ximxl);
for (sl=0; sl<R; sl++) { /* scale */
double alsl[2] = {rhot[INDRHOP(l,0,sl)],rhot[INDRHOP(l,1,sl)]};
double aisl[2] = {rhot[INDRHOP(i,0,sl)],rhot[INDRHOP(i,1,sl)]};
double rsl2 = scales2[sl];
double swsl2 = scaleweight2[sl];
double kbasesl = exp(-v/rsl2);
double ksl = Ks(kbasesl,swsl2);
double aislTXalsi; VECDOT(aislTXalsi,aisl,alsl);
Ett = Ett+ksl*aislTXalsi;
}
}
}
} else {
#pragma omp parallel for schedule(static) shared(rhot,L,R,dim,CSP,CSPL,scales2,scaleweight2) private(i,l,sl) reduction(+:Ett)
for (i=0; i<L; i++) { /* particle */
double xi[3] = {rhot[INDRHOX(i,0)],rhot[INDRHOX(i,1)],rhot[INDRHOX(i,2)]};
for (l=0; l<L; l++) { /* particle */
double xl[3] = {rhot[INDRHOX(l,0)],rhot[INDRHOX(l,1)],rhot[INDRHOX(l,2)]};
double ximxl[3]; _3VECMINUS(ximxl,xi,xl);
double v = _3VECDOT2(ximxl,ximxl);
for (sl=0; sl<R; sl++) { /* scale */
double alsl[3] = {rhot[INDRHOP(l,0,sl)],rhot[INDRHOP(l,1,sl)],rhot[INDRHOP(l,2,sl)]};
double aisl[3] = {rhot[INDRHOP(i,0,sl)],rhot[INDRHOP(i,1,sl)],rhot[INDRHOP(i,2,sl)]};
double rsl2 = scales2[sl];
double swsl2 = scaleweight2[sl];
double kbasesl = exp(-v/rsl2);
double ksl = Ks(kbasesl,swsl2);
double aislTXalsi; _3VECDOT(aislTXalsi,aisl,alsl);
Ett = Ett+ksl*aislTXalsi;
}
}
}
}
Et[0] = Ett;
}
