1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72 package ffx.potential.constraint;
73
74 import static ffx.numerics.math.DoubleMath.dot;
75 import static ffx.numerics.math.DoubleMath.normalize;
76 import static ffx.numerics.math.DoubleMath.sub;
77 import static java.lang.System.arraycopy;
78 import static org.apache.commons.math3.util.FastMath.abs;
79 import static org.apache.commons.math3.util.FastMath.cos;
80 import static org.apache.commons.math3.util.FastMath.sqrt;
81 import static org.apache.commons.math3.util.FastMath.toRadians;
82
83 import ffx.numerics.Constraint;
84 import ffx.potential.bonded.Angle;
85 import ffx.potential.bonded.Atom;
86 import ffx.potential.bonded.Bond;
87 import java.util.logging.Logger;
88
89
90
91
92
93
94
95
96
97
98
99 public class SettleConstraint implements Constraint {
100
101 private static final Logger logger = Logger.getLogger(SettleConstraint.class.getName());
102
103 private final int index0;
104 private final int index1;
105 private final int index2;
106
107 private final double distance1;
108
109 private final double distance2;
110
111
112
113
114
115
116
117 private SettleConstraint(Angle a012) {
118 Atom center = a012.getCentralAtom();
119 index0 = center.getXyzIndex() - 1;
120
121 Bond b01 = a012.getBond(0);
122 Bond b02 = a012.getBond(1);
123
124 assert b01.bondType.distance == b02.bondType.distance;
125
126 distance1 = b01.bondType.distance;
127
128 Atom a1 = b01.get1_2(center);
129 Atom a2 = b02.get1_2(center);
130 index1 = a1.getXyzIndex() - 1;
131 index2 = a2.getXyzIndex() - 1;
132
133 double angVal = a012.angleType.angle[a012.nh];
134 distance2 = lawOfCosines(distance1, distance1, angVal);
135 }
136
137
138
139
140
141
142
143
144 public static SettleConstraint settleFactory(Angle a012) {
145 SettleConstraint newC = new SettleConstraint(a012);
146 a012.setConstraint(newC);
147 return newC;
148 }
149
150
151
152
153
154
155
156
157
158
159 static double lawOfCosines(double distAB, double distBC, double angABC) {
160 double val = distAB * distAB;
161 val += distBC * distBC;
162 val -= (2.0 * distAB * distBC * cos(toRadians(angABC)));
163 val = sqrt(val);
164 return val;
165 }
166
167 @Override
168 public void applyConstraintToStep(
169 final double[] xPrior, double[] xNew, final double[] masses, double tol) {
170
171
172
173
174 int xi0 = 3 * index0;
175 int xi1 = 3 * index1;
176 int xi2 = 3 * index2;
177
178
179 double[] apos0 = new double[3];
180 double[] apos1 = new double[3];
181 double[] apos2 = new double[3];
182
183
184 double[] xp0 = new double[3];
185 double[] xp1 = new double[3];
186 double[] xp2 = new double[3];
187
188 for (int i = 0; i < 3; i++) {
189 apos0[i] = xPrior[xi0 + i];
190 xp0[i] = xNew[xi0 + i] - apos0[i];
191 apos1[i] = xPrior[xi1 + i];
192 xp1[i] = xNew[xi1 + i] - apos1[i];
193 apos2[i] = xPrior[xi2 + i];
194 xp2[i] = xNew[xi2 + i] - apos2[i];
195 }
196
197 double m0 = masses[xi0];
198 double m1 = masses[xi1];
199 double m2 = masses[xi2];
200
201
202
203 double xb0 = apos1[0] - apos0[0];
204 double yb0 = apos1[1] - apos0[1];
205 double zb0 = apos1[2] - apos0[2];
206 double xc0 = apos2[0] - apos0[0];
207 double yc0 = apos2[1] - apos0[1];
208 double zc0 = apos2[2] - apos0[2];
209
210 double invTotalMass = 1.0 / (m0 + m1 + m2);
211 double xcom = (xp0[0] * m0 + (xb0 + xp1[0]) * m1 + (xc0 + xp2[0]) * m2) * invTotalMass;
212 double ycom = (xp0[1] * m0 + (yb0 + xp1[1]) * m1 + (yc0 + xp2[1]) * m2) * invTotalMass;
213 double zcom = (xp0[2] * m0 + (zb0 + xp1[2]) * m1 + (zc0 + xp2[2]) * m2) * invTotalMass;
214
215 double xa1 = xp0[0] - xcom;
216 double ya1 = xp0[1] - ycom;
217 double za1 = xp0[2] - zcom;
218 double xb1 = xb0 + xp1[0] - xcom;
219 double yb1 = yb0 + xp1[1] - ycom;
220 double zb1 = zb0 + xp1[2] - zcom;
221 double xc1 = xc0 + xp2[0] - xcom;
222 double yc1 = yc0 + xp2[1] - ycom;
223 double zc1 = zc0 + xp2[2] - zcom;
224
225 double xaksZd = yb0 * zc0 - zb0 * yc0;
226 double yaksZd = zb0 * xc0 - xb0 * zc0;
227 double zaksZd = xb0 * yc0 - yb0 * xc0;
228 double xaksXd = ya1 * zaksZd - za1 * yaksZd;
229 double yaksXd = za1 * xaksZd - xa1 * zaksZd;
230 double zaksXd = xa1 * yaksZd - ya1 * xaksZd;
231 double xaksYd = yaksZd * zaksXd - zaksZd * yaksXd;
232 double yaksYd = zaksZd * xaksXd - xaksZd * zaksXd;
233 double zaksYd = xaksZd * yaksXd - yaksZd * xaksXd;
234
235 double axlng = sqrt(xaksXd * xaksXd + yaksXd * yaksXd + zaksXd * zaksXd);
236 double aylng = sqrt(xaksYd * xaksYd + yaksYd * yaksYd + zaksYd * zaksYd);
237 double azlng = sqrt(xaksZd * xaksZd + yaksZd * yaksZd + zaksZd * zaksZd);
238 double trns11 = xaksXd / axlng;
239 double trns21 = yaksXd / axlng;
240 double trns31 = zaksXd / axlng;
241 double trns12 = xaksYd / aylng;
242 double trns22 = yaksYd / aylng;
243 double trns32 = zaksYd / aylng;
244 double trns13 = xaksZd / azlng;
245 double trns23 = yaksZd / azlng;
246 double trns33 = zaksZd / azlng;
247
248 double xb0d = trns11 * xb0 + trns21 * yb0 + trns31 * zb0;
249 double yb0d = trns12 * xb0 + trns22 * yb0 + trns32 * zb0;
250 double xc0d = trns11 * xc0 + trns21 * yc0 + trns31 * zc0;
251 double yc0d = trns12 * xc0 + trns22 * yc0 + trns32 * zc0;
252 double za1d = trns13 * xa1 + trns23 * ya1 + trns33 * za1;
253 double xb1d = trns11 * xb1 + trns21 * yb1 + trns31 * zb1;
254 double yb1d = trns12 * xb1 + trns22 * yb1 + trns32 * zb1;
255 double zb1d = trns13 * xb1 + trns23 * yb1 + trns33 * zb1;
256 double xc1d = trns11 * xc1 + trns21 * yc1 + trns31 * zc1;
257 double yc1d = trns12 * xc1 + trns22 * yc1 + trns32 * zc1;
258 double zc1d = trns13 * xc1 + trns23 * yc1 + trns33 * zc1;
259
260
261
262 double rc = 0.5 * distance2;
263 double rb = sqrt(distance1 * distance1 - rc * rc);
264 double ra = rb * (m1 + m2) * invTotalMass;
265 rb -= ra;
266 double sinphi = za1d / ra;
267 double cosphi = sqrt(1 - sinphi * sinphi);
268 double sinpsi = (zb1d - zc1d) / (2 * rc * cosphi);
269 double cospsi = sqrt(1 - sinpsi * sinpsi);
270
271 double ya2d = ra * cosphi;
272 double xb2d = -rc * cospsi;
273 double yb2d = -rb * cosphi - rc * sinpsi * sinphi;
274 double yc2d = -rb * cosphi + rc * sinpsi * sinphi;
275 double xb2d2 = xb2d * xb2d;
276 double hh2 = 4.0f * xb2d2 + (yb2d - yc2d) * (yb2d - yc2d) + (zb1d - zc1d) * (zb1d - zc1d);
277 double deltx = 2.0f * xb2d + sqrt(4.0f * xb2d2 - hh2 + distance2 * distance2);
278 xb2d -= deltx * 0.5;
279
280
281
282 double alpha = (xb2d * (xb0d - xc0d) + yb0d * yb2d + yc0d * yc2d);
283 double beta = (xb2d * (yc0d - yb0d) + xb0d * yb2d + xc0d * yc2d);
284 double gamma = xb0d * yb1d - xb1d * yb0d + xc0d * yc1d - xc1d * yc0d;
285
286 double al2be2 = alpha * alpha + beta * beta;
287 double sintheta = (alpha * gamma - beta * sqrt(al2be2 - gamma * gamma)) / al2be2;
288
289
290
291 double costheta = sqrt(1 - sintheta * sintheta);
292 double xa3d = -ya2d * sintheta;
293 double ya3d = ya2d * costheta;
294 double za3d = za1d;
295 double xb3d = xb2d * costheta - yb2d * sintheta;
296 double yb3d = xb2d * sintheta + yb2d * costheta;
297 double zb3d = zb1d;
298 double xc3d = -xb2d * costheta - yc2d * sintheta;
299 double yc3d = -xb2d * sintheta + yc2d * costheta;
300 double zc3d = zc1d;
301
302
303
304 double xa3 = trns11 * xa3d + trns12 * ya3d + trns13 * za3d;
305 double ya3 = trns21 * xa3d + trns22 * ya3d + trns23 * za3d;
306 double za3 = trns31 * xa3d + trns32 * ya3d + trns33 * za3d;
307 double xb3 = trns11 * xb3d + trns12 * yb3d + trns13 * zb3d;
308 double yb3 = trns21 * xb3d + trns22 * yb3d + trns23 * zb3d;
309 double zb3 = trns31 * xb3d + trns32 * yb3d + trns33 * zb3d;
310 double xc3 = trns11 * xc3d + trns12 * yc3d + trns13 * zc3d;
311 double yc3 = trns21 * xc3d + trns22 * yc3d + trns23 * zc3d;
312 double zc3 = trns31 * xc3d + trns32 * yc3d + trns33 * zc3d;
313
314 xp0[0] = xcom + xa3;
315 xp0[1] = ycom + ya3;
316 xp0[2] = zcom + za3;
317 xp1[0] = xcom + xb3 - xb0;
318 xp1[1] = ycom + yb3 - yb0;
319 xp1[2] = zcom + zb3 - zb0;
320 xp2[0] = xcom + xc3 - xc0;
321 xp2[1] = ycom + yc3 - yc0;
322 xp2[2] = zcom + zc3 - zc0;
323
324 for (int i = 0; i < 3; i++) {
325 xNew[xi0 + i] = xp0[i] + apos0[i];
326 xNew[xi1 + i] = xp1[i] + apos1[i];
327 xNew[xi2 + i] = xp2[i] + apos2[i];
328 }
329 }
330
331 @Override
332 public void applyConstraintToVelocities(
333 final double[] x, double[] v, final double[] masses, double tol) {
334
335
336
337
338 int xi0 = 3 * index0;
339 int xi1 = 3 * index1;
340 int xi2 = 3 * index2;
341
342
343 double[] apos0 = new double[3];
344 double[] apos1 = new double[3];
345 double[] apos2 = new double[3];
346
347
348 double[] v0 = new double[3];
349 double[] v1 = new double[3];
350 double[] v2 = new double[3];
351
352 for (int i = 0; i < 3; i++) {
353 apos0[i] = x[xi0 + i];
354 apos1[i] = x[xi1 + i];
355 apos2[i] = x[xi2 + i];
356 }
357
358 for (int i = 0; i < 3; i++) {
359 v0[i] = v[xi0 + i];
360 v1[i] = v[xi1 + i];
361 v2[i] = v[xi2 + i];
362 }
363
364 double mA = masses[xi0];
365 double mB = masses[xi1];
366 double mC = masses[xi2];
367
368 double[] eAB = new double[3];
369 double[] eBC = new double[3];
370 double[] eCA = new double[3];
371 for (int i = 0; i < 3; i++) {
372 eAB[i] = apos1[i] - apos0[i];
373 eBC[i] = apos2[i] - apos1[i];
374 eCA[i] = apos0[i] - apos2[i];
375 }
376 normalize(eAB, eAB);
377 normalize(eBC, eBC);
378 normalize(eCA, eCA);
379
380
381
382 double vAB = (v1[0] - v0[0]) * eAB[0] + (v1[1] - v0[1]) * eAB[1] + (v1[2] - v0[2]) * eAB[2];
383 double vBC = (v2[0] - v1[0]) * eBC[0] + (v2[1] - v1[1]) * eBC[1] + (v2[2] - v1[2]) * eBC[2];
384 double vCA = (v0[0] - v2[0]) * eCA[0] + (v0[1] - v2[1]) * eCA[1] + (v0[2] - v2[2]) * eCA[2];
385 double cA = -(eAB[0] * eCA[0] + eAB[1] * eCA[1] + eAB[2] * eCA[2]);
386 double cB = -(eAB[0] * eBC[0] + eAB[1] * eBC[1] + eAB[2] * eBC[2]);
387 double cC = -(eBC[0] * eCA[0] + eBC[1] * eCA[1] + eBC[2] * eCA[2]);
388 double s2A = 1 - cA * cA;
389 double s2B = 1 - cB * cB;
390 double s2C = 1 - cC * cC;
391
392
393
394
395
396
397
398 double mABCinv = 1 / (mA * mB * mC);
399 double denom =
400 (((s2A * mB + s2B * mA) * mC
401 + (s2A * mB * mB + 2 * (cA * cB * cC + 1) * mA * mB + s2B * mA * mA))
402 * mC
403 + s2C * mA * mB * (mA + mB))
404 * mABCinv;
405 double tab =
406 ((cB * cC * mA - cA * mB - cA * mC) * vCA
407 + (cA * cC * mB - cB * mC - cB * mA) * vBC
408 + (s2C * mA * mA * mB * mB * mABCinv + (mA + mB + mC)) * vAB)
409 / denom;
410 double tbc =
411 ((cA * cB * mC - cC * mB - cC * mA) * vCA
412 + (s2A * mB * mB * mC * mC * mABCinv + (mA + mB + mC)) * vBC
413 + (cA * cC * mB - cB * mA - cB * mC) * vAB)
414 / denom;
415 double tca =
416 ((s2B * mA * mA * mC * mC * mABCinv + (mA + mB + mC)) * vCA
417 + (cA * cB * mC - cC * mB - cC * mA) * vBC
418 + (cB * cC * mA - cA * mB - cA * mC) * vAB)
419 / denom;
420
421 double invMA = 1.0 / mA;
422 double invMB = 1.0 / mB;
423 double invMC = 1.0 / mC;
424 for (int i = 0; i < 3; i++) {
425 v0[i] += (eAB[i] * tab - eCA[i] * tca) * invMA;
426 v1[i] += (eBC[i] * tbc - eAB[i] * tab) * invMB;
427 v2[i] += (eCA[i] * tca - eBC[i] * tbc) * invMC;
428 }
429
430
431
432
433 arraycopy(v0, 0, v, xi0, 3);
434 arraycopy(v1, 0, v, xi1, 3);
435 arraycopy(v2, 0, v, xi2, 3);
436
437
438
439 }
440
441 @Override
442 public int[] constrainedAtomIndices() {
443 return new int[] {index0, index1, index2};
444 }
445
446 @Override
447 public boolean constraintSatisfied(double[] x, double tol) {
448 return constraintSatisfied(x, null, tol, 0.0);
449 }
450
451 @Override
452 public boolean constraintSatisfied(double[] x, double[] v, double xTol, double vTol) {
453 int xi0 = 3 * index0;
454 int xi1 = 3 * index1;
455 int xi2 = 3 * index2;
456
457
458 double dist01 = 0;
459 double dist02 = 0;
460
461 double dist12 = 0;
462
463 double[] x0 = new double[3];
464 double[] x1 = new double[3];
465 double[] x2 = new double[3];
466 arraycopy(x, xi0, x0, 0, 3);
467 arraycopy(x, xi1, x1, 0, 3);
468 arraycopy(x, xi2, x2, 0, 3);
469
470 for (int i = 0; i < 3; i++) {
471
472 double dx = x0[i] - x1[i];
473 dx *= dx;
474 dist01 += dx;
475
476
477 dx = x0[i] - x2[i];
478 dx *= dx;
479 dist02 += dx;
480
481
482 dx = x1[i] - x2[i];
483 dx *= dx;
484 dist12 += dx;
485 }
486 dist01 = sqrt(dist01);
487 dist02 = sqrt(dist02);
488 dist12 = sqrt(dist12);
489
490
491 double deltaIdeal = Math.abs((dist01 - distance1) / distance1);
492 if (deltaIdeal > xTol) {
493 logger.finer(" delId 01: " + deltaIdeal);
494 return false;
495 }
496 deltaIdeal = Math.abs((dist02 - distance1) / distance1);
497 if (deltaIdeal > xTol) {
498 logger.finer(" delId 02: " + deltaIdeal);
499 return false;
500 }
501 deltaIdeal = Math.abs((dist12 - distance2) / distance2);
502 if (deltaIdeal > xTol) {
503 logger.finer(" delId 12: " + deltaIdeal);
504 return false;
505 }
506
507 if (v != null && vTol > 0) {
508 double[] v0 = new double[3];
509 double[] v1 = new double[3];
510 double[] v2 = new double[3];
511 arraycopy(v, xi0, v0, 0, 3);
512 arraycopy(v, xi1, v1, 0, 3);
513 arraycopy(v, xi2, v2, 0, 3);
514
515
516 double[] v01 = new double[3];
517 double[] v02 = new double[3];
518 double[] v12 = new double[3];
519 sub(v1, v0, v01);
520 sub(v2, v0, v02);
521 sub(v2, v1, v12);
522
523
524 double[] x01 = new double[3];
525 double[] x02 = new double[3];
526 double[] x12 = new double[3];
527 sub(x1, x0, x01);
528 sub(x2, x0, x02);
529 sub(x2, x1, x12);
530
531 double xv01 = dot(v01, x01);
532 double xv02 = dot(v02, x02);
533 double xv12 = dot(v12, x12);
534
535 if (abs(xv01) > vTol) {
536 return false;
537 }
538 if (abs(xv02) > vTol) {
539 return false;
540 }
541 if (abs(xv12) > vTol) {
542 return false;
543 }
544 }
545 return true;
546 }
547
548 @Override
549 public int getNumDegreesFrozen() {
550 return 3;
551
552 }
553 }