void _computeForceRaw(Real3D& force1,
Real3D& force2,
Real3D& force3,
Real3D& force4,
const Real3D& r21,
const Real3D& r32,
const Real3D& r43, const real _phi0) const {
Real3D retF[4];
Real3D rijjk = r21.cross(r32); // [r21 x r32]
Real3D rjkkn = r32.cross(r43); // [r32 x r43]
real rijjk_sqr = rijjk.sqr();
real rjkkn_sqr = rjkkn.sqr();
real rijjk_abs = sqrt(rijjk_sqr);
real rjkkn_abs = sqrt(rjkkn_sqr);
real inv_rijjk = 1.0 / rijjk_abs;
real inv_rjkkn = 1.0 / rjkkn_abs;
// cosine between planes
real cos_phi = (rijjk * rjkkn) * (inv_rijjk * inv_rjkkn);
if (cos_phi > 1.0) cos_phi = 1.0;
else if (cos_phi < -1.0) cos_phi = -1.0;
real coef1 = (-2.0) * K * (cos_phi - cos(_phi0));
real A1 = inv_rijjk * inv_rjkkn;
real A2 = inv_rijjk * inv_rijjk;
real A3 = inv_rjkkn * inv_rjkkn;
Real3D p3232 ( prod(r32,r32) );
Real3D p3243 ( prod(r32,r43) );
Real3D p2132 ( prod(r21,r32) );
Real3D p2143 ( prod(r21,r43) );
Real3D p2121 ( prod(r21,r21) );
Real3D p4343 ( prod(r43,r43) );
// we have 4 particles 1,2,3,4
for(int l=0; l<4; l++){
Real3D B1, B2, B3;
for(int i=0; i<3; i++){
B1[i]= r21[i] * ( p3232[i] * (d(l,2)-d(l,3)) + p3243[i] * (d(l,2)-d(l,1)) ) +
r32[i] * ( p2132[i] * (d(l,3)-d(l,2)) + p3243[i] * (d(l,1)-d(l,0)) ) +
r43[i] * ( p2132[i] * (d(l,2)-d(l,1)) + p3232[i] * (d(l,0)-d(l,1)) ) +
2.0 * r32[i] * p2143[i] * (d(l,1)-d(l,2));
B2[i]= 2.0 * r21[i] * ( p3232[i] * (d(l,1)-d(l,0)) + p2132[i] * (d(l,1)-d(l,2)) ) +
2.0 * r32[i] * ( p2121[i] * (d(l,2)-d(l,1)) + p2132[i] * (d(l,0)-d(l,1)));
B3[i]= 2.0 * r43[i] * ( p3232[i] * (d(l,3)-d(l,2)) + p3243[i] * (d(l,1)-d(l,2)) ) +
2.0 * r32[i] * ( p4343[i] * (d(l,2)-d(l,1)) + p3243[i] * (d(l,2)-d(l,3)));
}
retF[l] = coef1 * ( A1*B1 - 0.5*cos_phi*(A2*B2 + A3*B3) );
}
force1 = retF[0];
force2 = retF[1];
force3 = retF[2];
force4 = retF[3];
}
// Quaternion product, p*q = [p_real*q_real - p_unreal * q_unreal, p_real * q_unreal + q_real * p_unreal + p_unreal x q_unreal]
inline Quaternion Quaternion::operator* (const Quaternion& v) const
{ return Quaternion( real_part*v.real_part - (unreal_part * v.unreal_part),
real_part * v.unreal_part + v.real_part * unreal_part + unreal_part.cross(v.unreal_part) ); }
void _computeForceRaw(Real3D& force1,
Real3D& force2,
Real3D& force3,
Real3D& force4,
const Real3D& dist21,
const Real3D& dist32,
const Real3D& dist43) const {
/*
real rxji, ryji, rzji;
real rxkj, rykj, rzkj;
real rxnk, rynk, rznk;
real cpjikjx, cpjikjy, cpjikjz;
real cpkjnkx, cpkjnky, cpkjnkz;
real cpjikjn, cpkjnkn, dp, arg;
real thijkn, costh, sinth, r1;
real fix, fiy, fiz, fix2, fiy2, fiz2, fxi, fyi, fzi;
real fjx, fjy, fjz, fjx1, fjy1, fjz1, fjx2, fjy2, fjz2;
real fjx3, fjy3, fjz3, fjx4, fjy4, fjz4, fjx5, fjy5, fjz5, fxj, fyj, fzj;
real fkx, fky, fkz, fkx2, fky2, fkz2, fkx3, fky3, fkz3;
real fkx4, fky4, fkz4, fkx5, fky5, fkz5, fkx6, fky6, fkz6, fxk, fyk, fzk;
real fnx, fny, fnz, fnx2, fny2, fnz2, fxn, fyn, fzn;
rxji = dist21[0];
ryji = dist21[1];
rzji = dist21[2];
rxkj = dist32[0];
rykj = dist32[1];
rzkj = dist32[2];
rxnk = dist43[0];
rynk = dist43[1];
rznk = dist43[2];
cpjikjx = rxji * rykj - ryji * rxkj;
cpjikjy = rxji * rzkj - rzji * rxkj;
cpjikjz = ryji * rzkj - rzji * rykj;
cpkjnkx = rxkj * rynk - rykj * rxnk;
cpkjnky = rxkj * rznk - rzkj * rxnk;
cpkjnkz = rykj * rznk - rzkj * rynk;
cpjikjn = sqrt(pow(cpjikjx, 2) + pow(cpjikjy, 2) + pow(cpjikjz, 2));
cpkjnkn = sqrt(pow(cpkjnkx, 2) + pow(cpkjnky, 2) + pow(cpkjnkz, 2));
dp = cpjikjx * cpkjnkx + cpjikjy * cpkjnky + cpjikjz * cpkjnkz;
arg = dp / cpjikjn / cpkjnkn;
if(arg < -1.0) arg = -1.0;
if(arg > 1.0) arg = 1.0;
thijkn = acos(arg);
costh = cos(thijkn);
sinth = sin(thijkn);
r1 = -K1 +
2.0 * K2 * sin(2.0 * thijkn) / sinth -
3.0 * K3 * sin(3.0 * thijkn) / sinth;
// particle i
fix = rxkj * (-rykj * rynk - rzkj * rznk) + rxnk * (rykj * rykj + rzkj * rzkj);
fiy = rykj * (-rxkj * rxnk - rzkj * rznk) + rynk * (rxkj * rxkj + rzkj * rzkj);
fiz = rzkj * (-rxkj * rxnk - rykj * rynk) + rznk * (rxkj * rxkj + rykj * rykj);
fix = fix / cpjikjn / cpkjnkn;
fiy = fiy / cpjikjn / cpkjnkn;
fiz = fiz / cpjikjn / cpkjnkn;
fix2 = 2.0 * rxji * (-rykj * rykj - rzkj * rzkj) + 2.0 * rxkj * (ryji * rykj + rzji * rzkj);
fiy2 = 2.0 * ryji * (-rxkj * rxkj - rzkj * rzkj) + 2.0 * rykj * (rxji * rxkj + rzji * rzkj);
fiz2 = 2.0 * rzji * (-rxkj * rxkj - rykj * rykj) + 2.0 * rzkj * (rxji * rxkj + ryji * rykj);
fix2 = fix2 / pow(cpjikjn, 2);
fiy2 = fiy2 / pow(cpjikjn, 2);
fiz2 = fiz2 / pow(cpjikjn, 2);
fxi = r1 * (fix - 0.5 * costh * fix2);
fyi = r1 * (fiy - 0.5 * costh * fiy2);
fzi = r1 * (fiz - 0.5 * costh * fiz2);
// particle j
fjx = rxji * (-rykj * rynk - rzkj * rznk) + rxkj * (rykj * rynk + rzkj * rznk);
fjy = ryji * (-rxkj * rxnk - rzkj * rznk) + rykj * (rxkj * rxnk + rzkj * rznk);
fjz = rzji * (-rxkj * rxnk - rykj * rynk) + rzkj * (rxkj * rxnk + rykj * rynk);
fjx1 = rxnk * (-ryji * rykj - rzji * rzkj - rykj * rykj - rzkj * rzkj);
fjy1 = rynk * (-rxji * rxkj - rzji * rzkj - rxkj * rxkj - rzkj * rzkj);
fjz1 = rznk * (-rxji * rxkj - ryji * rykj - rxkj * rxkj - rykj * rykj);
fjx2 = 2.0 * rxkj * (ryji * rynk + rzji * rznk);
fjy2 = 2.0 * rykj * (rxji * rxnk + rzji * rznk);
fjz2 = 2.0 * rzkj * (rxji * rxnk + ryji * rynk);
fjx = (fjx + fjx1 + fjx2) / cpjikjn / cpkjnkn;
fjy = (fjy + fjy1 + fjy2) / cpjikjn / cpkjnkn;
fjz = (fjz + fjz1 + fjz2) / cpjikjn / cpkjnkn;
fjx3 = 2.0 * rxji * (rykj * rykj + rzkj * rzkj + ryji * rykj + rzji * rzkj);
fjy3 = 2.0 * ryji * (rxkj * rxkj + rzkj * rzkj + rxji * rxkj + rzji * rzkj);
fjz3 = 2.0 * rzji * (rxkj * rxkj + rykj * rykj + rxji * rxkj + ryji * rykj);
fjx4 = 2.0 * rxkj * (-ryji * ryji - rzji * rzji - ryji * rykj - rzji * rzkj);
fjy4 = 2.0 * rykj * (-rxji * rxji - rzji * rzji - rxji * rxkj - rzji * rzkj);
fjz4 = 2.0 * rzkj * (-rxji * rxji - ryji * ryji - rxji * rxkj - ryji * rykj);
fjx3 = (fjx3 + fjx4) / pow(cpjikjn, 2);
fjy3 = (fjy3 + fjy4) / pow(cpjikjn, 2);
fjz3 = (fjz3 + fjz4) / pow(cpjikjn, 2);
fjx5 = 2.0 * rxnk * (rykj * rynk + rzkj * rznk) + 2.0 * rxkj * (-rynk * rynk - rznk * rznk);
fjy5 = 2.0 * rynk * (rxkj * rxnk + rzkj * rznk) + 2.0 * rykj * (-rxnk * rxnk - rznk * rznk);
fjz5 = 2.0 * rznk * (rxkj * rxnk + rykj * rynk) + 2.0 * rzkj * (-rxnk * rxnk - rynk * rynk);
fjx5 = fjx5 / pow(cpkjnkn, 2);
fjy5 = fjy5 / pow(cpkjnkn, 2);
fjz5 = fjz5 / pow(cpkjnkn, 2);
fxj = r1 * (fjx - 0.5 * costh * (fjx3 + fjx5));
fyj = r1 * (fjy - 0.5 * costh * (fjy3 + fjy5));
fzj = r1 * (fjz - 0.5 * costh * (fjz3 + fjz5));
// particle k
fkx = rxji * (rykj * rykj + rzkj * rzkj + rykj * rynk + rzkj * rznk);
fky = ryji * (rxkj * rxkj + rzkj * rzkj + rxkj * rxnk + rzkj * rznk);
fkz = rzji * (rxkj * rxkj + rykj * rykj + rxkj * rxnk + rykj * rynk);
fkx2 = rxkj * (-ryji * rykj - rzji * rzkj);
fky2 = rykj * (-rxji * rxkj - rzji * rzkj);
fkz2 = rzkj * (-rxji * rxkj - ryji * rykj);
fkx3 = rxnk * (ryji * rykj + rzji * rzkj) + 2.0 * rxkj * (-ryji * rynk - rzji * rznk);
fky3 = rynk * (rxji * rxkj + rzji * rzkj) + 2.0 * rykj * (-rxji * rxnk - rzji * rznk);
fkz3 = rznk * (rxji * rxkj + ryji * rykj) + 2.0 * rzkj * (-rxji * rxnk - ryji * rynk);
fkx = (fkx + fkx2 + fkx3) / cpjikjn / cpkjnkn;
fky = (fky + fky2 + fky3) / cpjikjn / cpkjnkn;
fkz = (fkz + fkz2 + fkz3) / cpjikjn / cpkjnkn;
fkx4 = 2.0 * rxji * (-ryji * rykj - rzji * rzkj) + 2.0 * rxkj * (ryji * ryji + rzji * rzji);
fky4 = 2.0 * ryji * (-rxji * rxkj - rzji * rzkj) + 2.0 * rykj * (rxji * rxji + rzji * rzji);
fkz4 = 2.0 * rzji * (-rxji * rxkj - ryji * rykj) + 2.0 * rzkj * (rxji * rxji + ryji * ryji);
fkx4 = fkx4 / pow(cpjikjn, 2);
fky4 = fky4 / pow(cpjikjn, 2);
fkz4 = fkz4 / pow(cpjikjn, 2);
fkx5 = 2.0 * rxnk * (-rykj * rykj - rzkj * rzkj - rykj * rynk - rzkj * rznk);
fky5 = 2.0 * rynk * (-rxkj * rxkj - rzkj * rzkj - rxkj * rxnk - rzkj * rznk);
fkz5 = 2.0 * rznk * (-rxkj * rxkj - rykj * rykj - rxkj * rxnk - rykj * rynk);
fkx6 = 2.0 * rxkj * (rynk * rynk + rznk * rznk + rykj * rynk + rzkj * rznk);
fky6 = 2.0 * rykj * (rxnk * rxnk + rznk * rznk + rxkj * rxnk + rzkj * rznk);
fkz6 = 2.0 * rzkj * (rxnk * rxnk + rynk * rynk + rxkj * rxnk + rykj * rynk);
fkx5 = (fkx5 + fkx6) / pow(cpkjnkn, 2);
fky5 = (fky5 + fky6) / pow(cpkjnkn, 2);
fkz5 = (fkz5 + fkz6) / pow(cpkjnkn, 2);
fxk = r1 * (fkx - 0.5 * costh * (fkx4 + fkx5));
fyk = r1 * (fky - 0.5 * costh * (fky4 + fky5));
fzk = r1 * (fkz - 0.5 * costh * (fkz4 + fkz5));
// particle n
fnx = rxji * (-rykj * rykj - rzkj * rzkj) + rxkj * (ryji * rykj + rzji * rzkj);
fny = ryji * (-rxkj * rxkj - rzkj * rzkj) + rykj * (rxji * rxkj + rzji * rzkj);
fnz = rzji * (-rxkj * rxkj - rykj * rykj) + rzkj * (rxji * rxkj + ryji * rykj);
fnx = fnx / cpjikjn / cpkjnkn;
fny = fny / cpjikjn / cpkjnkn;
fnz = fnz / cpjikjn / cpkjnkn;
fnx2 = 2.0 * rxnk * (rykj * rykj + rzkj * rzkj) + 2.0 * rxkj * (-rykj * rynk - rzkj * rznk);
fny2 = 2.0 * rynk * (rxkj * rxkj + rzkj * rzkj) + 2.0 * rykj * (-rxkj * rxnk - rzkj * rznk);
fnz2 = 2.0 * rznk * (rxkj * rxkj + rykj * rykj) + 2.0 * rzkj * (-rxkj * rxnk - rykj * rynk);
fnx2 = fnx2 / pow(cpkjnkn, 2);
fny2 = fny2 / pow(cpkjnkn, 2);
fnz2 = fnz2 / pow(cpkjnkn, 2);
fxn = r1 * (fnx - 0.5 * costh * fnx2);
fyn = r1 * (fny - 0.5 * costh * fny2);
fzn = r1 * (fnz - 0.5 * costh * fnz2);
// forces i, j, k, n
force1[0] = fxi;
force1[1] = fyi;
force1[2] = fzi;
force2[0] = fxj;
force2[1] = fyj;
force2[2] = fzj;
force3[0] = fxk;
force3[1] = fyk;
force3[2] = fzk;
force4[0] = fxn;
force4[1] = fxn;
force4[2] = fxn;
*/
// compute phi
real dist21_sqr = dist21 * dist21;
real dist32_sqr = dist32 * dist32;
real dist43_sqr = dist43 * dist43;
real dist21_magn = sqrt(dist21_sqr);
real dist32_magn = sqrt(dist32_sqr);
real dist43_magn = sqrt(dist43_sqr);
// cos0
real sb1 = 1.0 / dist21_sqr;
real sb2 = 1.0 / dist32_sqr;
real sb3 = 1.0 / dist43_sqr;
real rb1 = sqrt(sb1);
real rb3 = sqrt(sb3);
real c0 = dist21 * dist43 * rb1 * rb3;
// 1st and 2nd angle
real ctmp = dist21 * dist32;
real r12c1 = 1.0 / (dist21_magn * dist32_magn);
real c1mag = ctmp * r12c1;
ctmp = (-1.0 * dist32) * dist43;
real r12c2 = 1.0 / (dist32_magn * dist43_magn);
real c2mag = ctmp * r12c2;
//cos and sin of 2 angles and final cos
real sin2 = 1.0 - c1mag * c1mag;
if (sin2 < 0) sin2 = 0.0;
real sc1 = sqrt(sin2);
sc1 = 1.0 / sc1;
sin2 = 1.0 - c2mag * c2mag;
if (sin2 < 0) sin2 = 0.0;
real sc2 = sqrt(sin2);
sc2 = 1.0 / sc2;
real s1 = sc1 * sc1;
real s2 = sc2 * sc2;
real s12 = sc1 * sc2;
real c = (c0 + c1mag * c2mag) * s12;
Real3D cc = dist21.cross(dist32);
real cmag = sqrt(cc * cc);
real dx = cc * dist43 / cmag / dist43_magn;
if (c > 1.0) c = 1.0;
else if (c < -1.0) c = -1.0;
// phi
real phi = acos(c);
if (dx < 0.0) phi *= -1.0;
//phi = 1.0; //testing
real si = sin(phi);
real siinv = 1.0 / si;
// force calculation
real a = K1 -
K2 * 2.0 * sin(2.0 * phi) * siinv +
K3 * 3.0 * sin(3.0 * phi) * siinv -
K4 * 4.0 * sin(4.0 * phi) * siinv;
c = c * a;
s12 = s12 * a;
real a11 = c * sb1 * s1;
real a22 = -sb2 * (2.0 * c0 * s12 - c * (s1 + s2));
real a33 = c * sb3 * s2;
real a12 = -r12c1 * (c1mag * c * s1 + c2mag * s12);
real a13 = -rb1 * rb3 * s12;
real a23 = r12c2 * (c2mag * c * s2 + c1mag * s12);
Real3D sf2 = a12 * dist21 + a22 * dist32 + a23 * dist43;
force1 = a11 * dist21 + a12 * dist32 + a13 * dist43;
force2 = (-1.0 * sf2) - force1;
force4 = a13 * dist21 + a23 * dist32 + a33 * dist43;
force3 = sf2 - force4;
}
