void
efp_charge_quadrupole_grad(double q1, const double *quad2, const vec_t *dr,
vec_t *force, vec_t *add1, vec_t *add2)
{
double r = vec_len(dr);
double r2 = r * r;
double r5 = r2 * r2 * r;
double r7 = r5 * r2;
double t1x = q1 / r5 * -2.0 * (dr->x * quad2[quad_idx(0, 0)] +
dr->y * quad2[quad_idx(0, 1)] +
dr->z * quad2[quad_idx(0, 2)]);
double t1y = q1 / r5 * -2.0 * (dr->x * quad2[quad_idx(1, 0)] +
dr->y * quad2[quad_idx(1, 1)] +
dr->z * quad2[quad_idx(1, 2)]);
double t1z = q1 / r5 * -2.0 * (dr->x * quad2[quad_idx(2, 0)] +
dr->y * quad2[quad_idx(2, 1)] +
dr->z * quad2[quad_idx(2, 2)]);
double g = 5.0 * q1 / r7 * quadrupole_sum(quad2, dr);
force->x = g * dr->x + t1x;
force->y = g * dr->y + t1y;
force->z = g * dr->z + t1z;
add1->x = 0.0;
add1->y = 0.0;
add1->z = 0.0;
add2->x = t1z * dr->y - t1y * dr->z;
add2->y = t1x * dr->z - t1z * dr->x;
add2->z = t1y * dr->x - t1x * dr->y;
}
double
efp_quadrupole_quadrupole_energy(const double *quad1, const double *quad2, const vec_t *dr)
{
double r = vec_len(dr);
double r2 = r * r;
double r5 = r2 * r2 * r;
double r7 = r5 * r2;
double r9 = r7 * r2;
double q1dr = quadrupole_sum(quad1, dr);
double q2dr = quadrupole_sum(quad2, dr);
double q1q2 = 0.0;
double q1q2dr = 0.0;
for (size_t a = 0; a < 3; a++) {
double t1 = 0.0;
double t2 = 0.0;
for (size_t b = 0; b < 3; b++) {
size_t idx = quad_idx(a, b);
t1 += quad1[idx] * vec_get(dr, b);
t2 += quad2[idx] * vec_get(dr, b);
q1q2 += quad1[idx] * quad2[idx];
}
q1q2dr += t1 * t2;
}
return (2.0 / r5 * q1q2 - 20.0 / r7 * q1q2dr + 35.0 / r9 * q1dr * q2dr) / 3.0;
}
static vec_t
get_multipole_field(const vec_t *xyz, const struct multipole_pt *mult_pt,
const struct swf *swf)
{
vec_t field = vec_zero;
vec_t dr = {
xyz->x - mult_pt->x - swf->cell.x,
xyz->y - mult_pt->y - swf->cell.y,
xyz->z - mult_pt->z - swf->cell.z
};
double t1, t2;
double r = vec_len(&dr);
double r3 = r * r * r;
double r5 = r3 * r * r;
double r7 = r5 * r * r;
/* charge */
field.x += swf->swf * mult_pt->monopole * dr.x / r3;
field.y += swf->swf * mult_pt->monopole * dr.y / r3;
field.z += swf->swf * mult_pt->monopole * dr.z / r3;
/* dipole */
t1 = vec_dot(&mult_pt->dipole, &dr);
field.x += swf->swf * (3.0 / r5 * t1 * dr.x - mult_pt->dipole.x / r3);
field.y += swf->swf * (3.0 / r5 * t1 * dr.y - mult_pt->dipole.y / r3);
field.z += swf->swf * (3.0 / r5 * t1 * dr.z - mult_pt->dipole.z / r3);
/* quadrupole */
t1 = quadrupole_sum(mult_pt->quadrupole, &dr);
t2 = mult_pt->quadrupole[quad_idx(0, 0)] * dr.x +
mult_pt->quadrupole[quad_idx(1, 0)] * dr.y +
mult_pt->quadrupole[quad_idx(2, 0)] * dr.z;
field.x += swf->swf * (-2.0 / r5 * t2 + 5.0 / r7 * t1 * dr.x);
t2 = mult_pt->quadrupole[quad_idx(0, 1)] * dr.x +
mult_pt->quadrupole[quad_idx(1, 1)] * dr.y +
mult_pt->quadrupole[quad_idx(2, 1)] * dr.z;
field.y += swf->swf * (-2.0 / r5 * t2 + 5.0 / r7 * t1 * dr.y);
t2 = mult_pt->quadrupole[quad_idx(0, 2)] * dr.x +
mult_pt->quadrupole[quad_idx(1, 2)] * dr.y +
mult_pt->quadrupole[quad_idx(2, 2)] * dr.z;
field.z += swf->swf * (-2.0 / r5 * t2 + 5.0 / r7 * t1 * dr.z);
/* octupole-polarizability interactions are ignored */
return (field);
}
double
efp_dipole_quadrupole_energy(const vec_t *d1, const double *quad2, const vec_t *dr)
{
double r = vec_len(dr);
double r2 = r * r;
double r5 = r2 * r2 * r;
double r7 = r5 * r2;
double d1dr = vec_dot(d1, dr);
double q2dr = quadrupole_sum(quad2, dr);
double d1q2dr = 0.0;
for (size_t a = 0; a < 3; a++)
for (size_t b = 0; b < 3; b++) {
size_t idx = quad_idx(a, b);
d1q2dr += quad2[idx] * vec_get(d1, a) * vec_get(dr, b);
}
return 5.0 / r7 * q2dr * d1dr - 2.0 / r5 * d1q2dr;
}
void DeformToFit::registerAndDeformNodes(Structure::Node * snode, Structure::Node * tnode)
{
auto scenter = snode->position(Eigen::Vector4d(0.5, 0.5, 0, 0));
auto tcenter = tnode->position(Eigen::Vector4d(0.5, 0.5, 0, 0));
auto translation = Vector3(tcenter - scenter);
if (snode->type() == tnode->type())
{
if (snode->type() == Structure::CURVE)
{
auto scpts = snode->controlPoints();
auto tcpts = tnode->controlPoints();
for (auto & p : tcpts) p -= translation;
// Register
Vector3 sfront = scpts.front();
Vector3 tfront = tcpts.front();
Vector3 tback = tcpts.back();
bool isReverse = (sfront - tfront).norm() > (sfront - tback).norm() ? true : false;
if (isReverse) std::reverse(tcpts.begin(), tcpts.end());
// Encode target curve as deltas from center
std::map < double, Vector3 > deltas;
for (size_t i = 0; i < tcpts.size(); i++){
double t = double(i) / (tcpts.size() - 1);
deltas[t] = tcpts[i] - scenter;
}
// Deform source curve from deltas
for (size_t i = 0; i < scpts.size(); i++){
double t = double(i) / (scpts.size() - 1);
scpts[i] = (scenter + translation) + linear_interpolate<Vector3>(t, deltas);
}
// Apply deformed control points
snode->setControlPoints(scpts);
}
if (snode->type() == Structure::SHEET)
{
Structure::Sheet * ssheet = (Structure::Sheet*) snode;
Structure::Sheet * tsheet = (Structure::Sheet*) tnode;
auto ssurface = ssheet->surface;
auto tsurface = tsheet->surface;
// Remove translation
tsurface.translate(-translation);
// Minimize rotation
Vector3 tpos, tu, tv, tnormal;
tsurface.GetFrame(0.5, 0.5, tpos, tu, tv, tnormal);
Vector3 spos, su, sv, snormal;
ssurface.GetFrame(0.5, 0.5, spos, su, sv, snormal);
if (snormal.dot(tnormal) < 0)
tnormal *= -1;
Eigen::Quaterniond q = Eigen::Quaterniond::FromTwoVectors(snormal, tnormal);
for (auto & row : tsurface.mCtrlPoint) for (auto & p : row) p = (q.inverse() * (p - scenter)) + scenter;
QMap < double, QVector<double> > dists;
for (double u = 0; u <= 1.0; u += 1.0){
for (double v = 0; v <= 1.0; v += 1.0){
for (double i = 0; i <= 1.0; i += 1.0){
for (double j = 0; j <= 1.0; j += 1.0){
dists[(ssurface.P(u, v) - tsurface.P(i, j)).norm()] = (QVector<double>() << u << v << i << j);
}
}
}
}
auto bestChoice = dists.values().front();
bool isReverseU = bestChoice[0] != bestChoice[2], isReverseV = bestChoice[1] != bestChoice[3];
// Reverse if needed
if ( isReverseV ){
for (int i = 0; i < (int)tsurface.mCtrlPoint.size(); i++){
std::reverse(tsurface.mCtrlPoint[i].begin(), tsurface.mCtrlPoint[i].end());
std::reverse(tsurface.mCtrlWeight[i].begin(), tsurface.mCtrlWeight[i].end());
}
}
if( isReverseU ){
std::reverse(tsurface.mCtrlPoint.begin(), tsurface.mCtrlPoint.end());
std::reverse(tsurface.mCtrlWeight.begin(), tsurface.mCtrlWeight.end());
}
std::map < double, size_t > mapU, mapV;
for (size_t i = 0; i < tsurface.mNumUCtrlPoints; i++) mapU[double(i) / (tsurface.mNumUCtrlPoints - 1)] = i;
for (size_t j = 0; j < tsurface.mNumVCtrlPoints; j++) mapV[double(j) / (tsurface.mNumVCtrlPoints - 1)] = j;
auto getQuad = [&](size_t u, size_t v, Array2D_Vector3& cpts){
return QVector<Vector3>() << cpts[u][v] << cpts[u+1][v] << cpts[u][v+1] << cpts[u+1][v+1];
};
for (size_t i = 0; i < ssurface.mNumUCtrlPoints; i++){
for (size_t j = 0; j < ssurface.mNumVCtrlPoints; j++){
double u = double(i) / (ssurface.mNumUCtrlPoints-1);
double v = double(j) / (ssurface.mNumVCtrlPoints-1);
auto weight_u = linear_interpolate_weight<size_t>(u, mapU);
auto weight_v = linear_interpolate_weight<size_t>(v, mapV);
std::pair <size_t, size_t> quad_idx(std::get<0>(weight_u), std::get<0>(weight_v));
std::pair <double, double> quad_uv(std::get<2>(weight_u), std::get<2>(weight_v));
auto quad = getQuad(quad_idx.first, quad_idx.second, tsurface.mCtrlPoint);
auto interp = quad_interpolate(quad[0], quad[1], quad[2], quad[3], quad_uv.first, quad_uv.second);
ssurface.mCtrlPoint[i][j] = interp;
}
}
// Apply rotation
for (auto & row : ssurface.mCtrlPoint) for (auto & p : row) p = (q * (p - scenter)) + tcenter;
ssheet->surface.mCtrlPoint = ssurface.mCtrlPoint;
ssheet->surface.quads.clear();
}
}
else
{
Structure::Curve curve((snode->type() == Structure::CURVE) ? (*(Structure::Curve*)snode) : (*(Structure::Curve*)tnode));
Structure::Sheet sheet((snode->type() == Structure::SHEET) ? (*(Structure::Sheet*)snode) : (*(Structure::Sheet*)tnode));
double minU = std::min((sheet.surface.P(0, 0) - sheet.surface.P(1, 0)).norm(), (sheet.surface.P(0, 1) - sheet.surface.P(1, 1)).norm());
double minV = std::min((sheet.surface.P(0, 0) - sheet.surface.P(0, 1)).norm(), (sheet.surface.P(1, 0) - sheet.surface.P(1, 1)).norm());
bool isProjectAlongU = minU < minV;
// Roll up sheet
int idx = (isProjectAlongU) ? (sheet.surface.mNumUCtrlPoints - 1) * 0.5 : (sheet.surface.mNumVCtrlPoints - 1) * 0.5;
Array1D_Vector3 projection = (isProjectAlongU) ? sheet.surface.GetControlPointsV(idx) : sheet.surface.GetControlPointsU(idx);
Array2D_Vector3 ctrlPnts(sheet.surface.mNumUCtrlPoints);
if (isProjectAlongU){
ctrlPnts = Array2D_Vector3(sheet.surface.mNumUCtrlPoints, projection);
}else{
for (size_t i = 0; i < sheet.surface.mNumUCtrlPoints; i++)
ctrlPnts[i] = Array1D_Vector3(sheet.surface.mNumVCtrlPoints, projection[i]);
}
sheet.surface.mCtrlPoint = ctrlPnts;
Structure::Curve curveFromSheet(NURBS::NURBSCurved::createCurveFromPoints(isProjectAlongU ?
sheet.surface.GetControlPointsV(0) : sheet.surface.GetControlPointsU(0)), "temp");
// Sheet to curve case:
if (snode->type() == Structure::SHEET)
{
DeformToFit::registerAndDeformNodes(&curveFromSheet, &curve);
if (isProjectAlongU)
sheet.surface.mCtrlPoint = Array2D_Vector3(sheet.surface.mNumUCtrlPoints, curveFromSheet.curve.mCtrlPoint);
else{
for (size_t i = 0; i < sheet.surface.mNumUCtrlPoints; i++)
sheet.surface.mCtrlPoint[i] = Array1D_Vector3(sheet.surface.mNumVCtrlPoints, curveFromSheet.curve.mCtrlPoint[i]);
}
((Structure::Sheet*)snode)->surface.mCtrlPoint = sheet.surface.mCtrlPoint;
}
// Curve to sheet case:
if (snode->type() == Structure::CURVE)
{
DeformToFit::registerAndDeformNodes(snode, &curveFromSheet);
}
}
}
void
efp_quadrupole_quadrupole_grad(const double *quad1, const double *quad2,
const vec_t *dr, vec_t *force, vec_t *add1,
vec_t *add2)
{
double r = vec_len(dr);
double r2 = r * r;
double r5 = r2 * r2 * r;
double r7 = r5 * r2;
double r9 = r7 * r2;
double r11 = r9 * r2;
double q1ss = quadrupole_sum(quad1, dr);
double q2ss = quadrupole_sum(quad2, dr);
double q1s[3] = { 0.0, 0.0, 0.0 };
double q2s[3] = { 0.0, 0.0, 0.0 };
double q1sq2s = 0.0;
for (size_t a = 0; a < 3; a++) {
for (size_t b = 0; b < 3; b++) {
q1s[a] += quad1[quad_idx(a, b)] * vec_get(dr, b);
q2s[a] += quad2[quad_idx(a, b)] * vec_get(dr, b);
}
q1sq2s += q1s[a] * q2s[a];
}
double q1q2 = 0.0;
for (size_t a = 0; a < 3; a++)
for (size_t b = 0; b < 3; b++)
q1q2 += quad1[quad_idx(a, b)] * quad2[quad_idx(a, b)];
double g = 30.0 / r7 * q1q2 - 420.0 / r9 * q1sq2s +
945.0 / r11 * q1ss * q2ss;
double t1x = 0.0, t1y = 0.0, t1z = 0.0;
for (size_t a = 0; a < 3; a++)
for (size_t b = 0; b < 3; b++) {
size_t ab = quad_idx(a, b);
double dra = vec_get(dr, a);
t1x += (quad1[quad_idx(0, b)] * quad2[ab] +
quad1[ab] * quad2[quad_idx(0, b)]) * dra;
t1y += (quad1[quad_idx(1, b)] * quad2[ab] +
quad1[ab] * quad2[quad_idx(1, b)]) * dra;
t1z += (quad1[quad_idx(2, b)] * quad2[ab] +
quad1[ab] * quad2[quad_idx(2, b)]) * dra;
}
force->x = (g * dr->x + 60.0 / r7 * t1x -
210.0 / r9 * (q1s[0] * q2ss + q2s[0] * q1ss)) / 9.0;
force->y = (g * dr->y + 60.0 / r7 * t1y -
210.0 / r9 * (q1s[1] * q2ss + q2s[1] * q1ss)) / 9.0;
force->z = (g * dr->z + 60.0 / r7 * t1z -
210.0 / r9 * (q1s[2] * q2ss + q2s[2] * q1ss)) / 9.0;
double q1q2tt[3][3];
memset(q1q2tt, 0, 9 * sizeof(double));
for (size_t a = 0; a < 3; a++)
for (size_t b = 0; b < 3; b++)
for (size_t c = 0; c < 3; c++) {
double dra = vec_get(dr, a);
double drc = vec_get(dr, c);
q1q2tt[b][c] += quad1[quad_idx(a, b)] *
(-10.0 / r7 * (drc * q2s[a] + dra * q2s[c]) +
35.0 / r9 * dra * drc * q2ss +
2.0 / r5 * quad2[quad_idx(a, c)]);
}
add1->x = 2.0 / 3.0 * (q1q2tt[1][2] - q1q2tt[2][1]);
add1->y = 2.0 / 3.0 * (q1q2tt[2][0] - q1q2tt[0][2]);
add1->z = 2.0 / 3.0 * (q1q2tt[0][1] - q1q2tt[1][0]);
double q2q1tt[3][3];
memset(q2q1tt, 0, 9 * sizeof(double));
for (size_t a = 0; a < 3; a++)
for (size_t b = 0; b < 3; b++)
for (size_t c = 0; c < 3; c++) {
double dra = vec_get(dr, a);
double drc = vec_get(dr, c);
q2q1tt[b][c] += quad2[quad_idx(a, b)] *
(-10.0 / r7 * (drc * q1s[a] + dra * q1s[c]) +
35.0 / r9 * dra * drc * q1ss +
2.0 / r5 * quad1[quad_idx(a, c)]);
}
add2->x = 2.0 / 3.0 * (q2q1tt[1][2] - q2q1tt[2][1]);
add2->y = 2.0 / 3.0 * (q2q1tt[2][0] - q2q1tt[0][2]);
add2->z = 2.0 / 3.0 * (q2q1tt[0][1] - q2q1tt[1][0]);
}
void
efp_dipole_quadrupole_grad(const vec_t *d1, const double *quad2,
const vec_t *dr, vec_t *force, vec_t *add1,
vec_t *add2)
{
double r = vec_len(dr);
double r2 = r * r;
double r3 = r2 * r;
double r5 = r3 * r2;
double r7 = r5 * r2;
double r9 = r7 * r2;
double q2sx = 0.0;
double q2sy = 0.0;
double q2sz = 0.0;
for (size_t a = 0; a < 3; a++) {
q2sx += quad2[quad_idx(0, a)] * vec_get(dr, a);
q2sy += quad2[quad_idx(1, a)] * vec_get(dr, a);
q2sz += quad2[quad_idx(2, a)] * vec_get(dr, a);
}
double d1dr = vec_dot(d1, dr);
double q2s = quadrupole_sum(quad2, dr);
double t1 = d1->x * q2sx + d1->y * q2sy + d1->z * q2sz;
double t2 = -10.0 / r7 * t1 + 35.0 / r9 * q2s * d1dr;
double d1q2x = d1->x * quad2[quad_idx(0, 0)] +
d1->y * quad2[quad_idx(0, 1)] +
d1->z * quad2[quad_idx(0, 2)];
double d1q2y = d1->x * quad2[quad_idx(1, 0)] +
d1->y * quad2[quad_idx(1, 1)] +
d1->z * quad2[quad_idx(1, 2)];
double d1q2z = d1->x * quad2[quad_idx(2, 0)] +
d1->y * quad2[quad_idx(2, 1)] +
d1->z * quad2[quad_idx(2, 2)];
double q2xdr = dr->x * quad2[quad_idx(0, 0)] +
dr->y * quad2[quad_idx(0, 1)] +
dr->z * quad2[quad_idx(0, 2)];
double q2ydr = dr->x * quad2[quad_idx(1, 0)] +
dr->y * quad2[quad_idx(1, 1)] +
dr->z * quad2[quad_idx(1, 2)];
double q2zdr = dr->x * quad2[quad_idx(2, 0)] +
dr->y * quad2[quad_idx(2, 1)] +
dr->z * quad2[quad_idx(2, 2)];
force->x = t2 * dr->x + 2.0 / r5 * d1q2x -
5.0 / r7 * (q2s * d1->x + 2.0 * q2xdr * d1dr);
force->y = t2 * dr->y + 2.0 / r5 * d1q2y -
5.0 / r7 * (q2s * d1->y + 2.0 * q2ydr * d1dr);
force->z = t2 * dr->z + 2.0 / r5 * d1q2z -
5.0 / r7 * (q2s * d1->z + 2.0 * q2zdr * d1dr);
add1->x = 2.0 / r5 * (d1->z * q2ydr - d1->y * q2zdr) +
5.0 / r7 * q2s * (dr->z * d1->y - dr->y * d1->z);
add1->y = 2.0 / r5 * (d1->x * q2zdr - d1->z * q2xdr) +
5.0 / r7 * q2s * (dr->x * d1->z - dr->z * d1->x);
add1->z = 2.0 / r5 * (d1->y * q2xdr - d1->x * q2ydr) +
5.0 / r7 * q2s * (dr->y * d1->x - dr->x * d1->y);
add2->x = -10.0 / r7 * d1dr * (q2ydr * dr->z - q2zdr * dr->y) -
2.0 / r5 * ((q2zdr * d1->y + dr->y * d1q2z) -
(q2ydr * d1->z + dr->z * d1q2y));
add2->y = -10.0 / r7 * d1dr * (q2zdr * dr->x - q2xdr * dr->z) -
2.0 / r5 * ((q2xdr * d1->z + dr->z * d1q2x) -
(q2zdr * d1->x + dr->x * d1q2z));
add2->z = -10.0 / r7 * d1dr * (q2xdr * dr->y - q2ydr * dr->x) -
2.0 / r5 * ((q2ydr * d1->x + dr->x * d1q2y) -
(q2xdr * d1->y + dr->y * d1q2x));
}