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 package ffx.potential.utils;
39
40 import static ffx.numerics.math.DoubleMath.*;
41 import static java.lang.String.format;
42 import static java.lang.System.arraycopy;
43 import static org.apache.commons.math3.util.FastMath.PI;
44 import static org.apache.commons.math3.util.FastMath.abs;
45 import static org.apache.commons.math3.util.FastMath.acos;
46 import static org.apache.commons.math3.util.FastMath.atan2;
47 import static org.apache.commons.math3.util.FastMath.cos;
48 import static org.apache.commons.math3.util.FastMath.max;
49 import static org.apache.commons.math3.util.FastMath.min;
50 import static org.apache.commons.math3.util.FastMath.pow;
51 import static org.apache.commons.math3.util.FastMath.sin;
52 import static org.apache.commons.math3.util.FastMath.sqrt;
53 import static org.apache.commons.math3.util.FastMath.tan;
54 import static org.apache.commons.math3.util.FastMath.toDegrees;
55
56 import ffx.numerics.math.DoubleMath;
57 import org.apache.commons.math3.linear.RealMatrix;
58 import org.apache.commons.math3.linear.RealVector;
59 import org.apache.commons.math3.linear.Array2DRowRealMatrix;
60 import org.apache.commons.math3.linear.ArrayRealVector;
61
62 import ffx.potential.bonded.SturmMethod;
63
64 import java.util.logging.Level;
65 import java.util.logging.Logger;
66
67 import org.apache.commons.math3.util.FastMath;
68
69
70
71
72
73
74
75
76
77
78 public class LoopClosure {
79
80 private static final Logger logger = Logger.getLogger(LoopClosure.class.getName());
81
82 private static final int MAX_SOLUTIONS = 16;
83
84 private static final double[] BOND_LENS;
85 private static final double[] BOND_ANGLES;
86 private static final double AA_LEN;
87 private static final double AA13_MIN_SQR;
88 private static final double AA13_MAX_SQR;
89 private static final double[] XI = new double[3];
90 private static final double[] DELTA = new double[4];
91 private static final double[] ETA = new double[3];
92
93 private final double[] alpha = new double[3];
94 private final double[] theta = new double[3];
95 private final double[] cos_alpha = new double[3];
96 private final double[] sin_alpha = new double[3];
97 private final double[] cos_theta = new double[3];
98 private final double[] cos_delta = new double[4];
99 private final double[] sin_delta = new double[4];
100 private final double[] cos_xi = new double[3];
101 private final double[] cos_eta = new double[3];
102 private final double[] sin_xi = new double[3];
103 private final double[] sin_eta = new double[3];
104 private final double[] r_a1a3 = new double[3];
105 private final double[] r_a1n1 = new double[3];
106 private final double[] r_a3c3 = new double[3];
107 private final double[] b_a1a3 = new double[3];
108 private final double[] b_a1n1 = new double[3];
109 private final double[] b_a3c3 = new double[3];
110 private final double[] len_na = new double[3];
111 private final double[] len_ac = new double[3];
112 private final double[][] C0 = new double[3][3];
113 private final double[][] C1 = new double[3][3];
114 private final double[][] C2 = new double[3][3];
115 private final double[][] Q = new double[5][17];
116 private final double[][] R = new double[3][17];
117
118 static {
119
120 BOND_LENS = new double[3];
121 BOND_LENS[0] = 1.52;
122 BOND_LENS[1] = 1.33;
123 BOND_LENS[2] = 1.45;
124 BOND_ANGLES = new double[3];
125 BOND_ANGLES[0] = Math.toRadians(111.6);
126 BOND_ANGLES[1] = Math.toRadians(117.5);
127 BOND_ANGLES[2] = Math.toRadians(120.0);
128
129
130 var rotMatrix = rotationMatrix(new double[]{1.0, 0.0, 0.0}, Math.PI * 0.25e0);
131
132
133
134 var relCarboxyl = new double[3];
135 relCarboxyl[0] = cos(BOND_ANGLES[1]) * BOND_LENS[0];
136 relCarboxyl[1] = sin(BOND_ANGLES[1]) * BOND_LENS[0];
137 relCarboxyl[2] = 0.0e0;
138 var relNitrogen = new double[3];
139 relNitrogen[0] = BOND_LENS[1];
140 relNitrogen[1] = 0.0e0;
141 relNitrogen[2] = 0.0e0;
142 var relAlpha2 = new double[3];
143 relAlpha2[0] = -cos(BOND_ANGLES[2]) * BOND_LENS[2];
144 relAlpha2[1] = sin(BOND_ANGLES[2]) * BOND_LENS[2];
145 relAlpha2[2] = 0.0e0;
146
147
148 relAlpha2 = matrixMultiplication(rotMatrix, relAlpha2);
149 var alpha1Alpha2 = new double[3];
150 var alpha2N = new double[3];
151 for (int i = 0; i < 3; i++) {
152 alpha1Alpha2[i] = relAlpha2[i] + relNitrogen[i] - relCarboxyl[i];
153 alpha2N[i] = -relAlpha2[i];
154 }
155
156
157 AA_LEN = sqrt(dot(alpha1Alpha2, alpha1Alpha2));
158 double[] tmp_val = new double[3];
159 for (int i = 0; i < 2; i++) {
160
161 for (int j = 0; j < 3; j++) {
162 tmp_val[j] = -alpha1Alpha2[j];
163 }
164
165 XI[i + 1] = DoubleMath.angle(tmp_val, alpha2N);
166 for (int j = 0; j < 3; j++) {
167 tmp_val[j] = -relCarboxyl[j];
168 }
169
170 ETA[i] = DoubleMath.angle(alpha1Alpha2, tmp_val);
171 DELTA[i + 1] = PI - dihedralAngle(relCarboxyl, alpha1Alpha2, alpha2N);
172 }
173
174 double a_min = BOND_ANGLES[0] - (XI[1] + ETA[1]);
175 double a_max = min((BOND_ANGLES[0] + (XI[1] + ETA[1])), PI);
176 AA13_MIN_SQR = pow(AA_LEN, 2) + pow(AA_LEN, 2) - 2.0 * AA_LEN * AA_LEN * cos(a_min);
177 AA13_MAX_SQR = pow(AA_LEN, 2) + pow(AA_LEN, 2) - 2.0 * AA_LEN * AA_LEN * cos(a_max);
178 }
179
180
181
182
183
184
185
186
187 public double calcT1(double t0, double t2) {
188 double t0_2 = t0 * t0;
189 double t2_2 = t2 * t2;
190 double U11 = C0[0][0] + C0[0][1] * t0 + C0[0][2] * t0_2;
191 double U12 = C1[0][0] + C1[0][1] * t0 + C1[0][2] * t0_2;
192 double U13 = C2[0][0] + C2[0][1] * t0 + C2[0][2] * t0_2;
193 double U31 = C0[1][0] + C0[1][1] * t2 + C0[1][2] * t2_2;
194 double U32 = C1[1][0] + C1[1][1] * t2 + C1[1][2] * t2_2;
195 double U33 = C2[1][0] + C2[1][1] * t2 + C2[1][2] * t2_2;
196
197 return (U31 * U13 - U11 * U33) / (U12 * U33 - U13 * U32);
198 }
199
200
201
202
203
204
205
206 public double calcT2(double t0) {
207 double t0_2 = t0 * t0;
208 double t0_3 = t0_2 * t0;
209 double t0_4 = t0_3 * t0;
210
211 double A0 = Q[0][0] + Q[0][1] * t0 + Q[0][2] * t0_2 + Q[0][3] * t0_3 + Q[0][4] * t0_4;
212 double A1 = Q[1][0] + Q[1][1] * t0 + Q[1][2] * t0_2 + Q[1][3] * t0_3 + Q[1][4] * t0_4;
213 double A2 = Q[2][0] + Q[2][1] * t0 + Q[2][2] * t0_2 + Q[2][3] * t0_3 + Q[2][4] * t0_4;
214 double A3 = Q[3][0] + Q[3][1] * t0 + Q[3][2] * t0_2 + Q[3][3] * t0_3 + Q[3][4] * t0_4;
215 double A4 = Q[4][0] + Q[4][1] * t0 + Q[4][2] * t0_2 + Q[4][3] * t0_3 + Q[4][4] * t0_4;
216
217 double B0 = R[0][0] + R[0][1] * t0 + R[0][2] * t0_2;
218 double B1 = R[1][0] + R[1][1] * t0 + R[1][2] * t0_2;
219 double B2 = R[2][0] + R[2][1] * t0 + R[2][2] * t0_2;
220
221 double B2_2 = B2 * B2;
222 double B2_3 = B2_2 * B2;
223
224 double K0 = A2 * B2 - A4 * B0;
225 double K1 = A3 * B2 - A4 * B1;
226 double K2 = A1 * B2_2 - K1 * B0;
227 double K3 = K0 * B2 - K1 * B1;
228
229 return (K3 * B0 - A0 * B2_3) / (K2 * B2 - K3 * B1);
230 }
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245 private void getCoordsFromPolyRoots(int numSolutions, double[] roots, double[] r_n1, double[] r_a1,
246 double[] r_a3, double[] r_c3, double[][][] r_soln_n, double[][][] r_soln_a,
247 double[][][] r_soln_c) {
248 double[] ex = new double[3];
249 double[] ey = new double[3];
250 double[] ez = new double[3];
251 double[] b_a1a2 = new double[3];
252 double[] b_a3a2 = new double[3];
253 double[] r_tmp = new double[3];
254 double[][] p_s = new double[3][3];
255 double[][] s1 = new double[3][3];
256 double[][] s2 = new double[3][3];
257 double[][] p_t = new double[3][3];
258 double[][] t1 = new double[3][3];
259 double[][] t2 = new double[3][3];
260 double[][] p_s_c = new double[3][3];
261 double[][] s1_s = new double[3][3];
262 double[][] s2_s = new double[3][3];
263 double[][] p_t_c = new double[3][3];
264 double[][] t1_s = new double[3][3];
265 double[][] t2_s = new double[3][3];
266 double[] half_tan = new double[3];
267 double[] cos_tau = new double[4];
268 double[] sin_tau = new double[4];
269 double[] cos_sig = new double[3];
270 double[] sin_sig = new double[3];
271 double[] r_s = new double[3];
272 double[] r_t = new double[3];
273 double[] r0 = new double[3];
274 double[][] r_n = new double[3][3];
275 double[][] r_a = new double[3][3];
276 double[][] r_c = new double[3][3];
277 double[] rr_a1c1 = new double[3];
278 double[] rr_c1n2 = new double[3];
279 double[] rr_n2a2 = new double[3];
280 double[] rr_a2c2 = new double[3];
281 double[] rr_c2n3 = new double[3];
282 double[] rr_n3a3 = new double[3];
283 double[] rr_a1a2 = new double[3];
284 double[] rr_a2a3 = new double[3];
285 double[] ex_tmp = new double[3];
286 double[] tmp_array = new double[3];
287 double[] tmp_array1 = new double[3];
288 double[] tmp_array2 = new double[3];
289 double[] tmp_array3 = new double[3];
290 double[] mat11 = new double[3];
291 double[] mat22 = new double[3];
292 double[] mat33 = new double[3];
293 double[] mat44 = new double[3];
294 double[] mat55 = new double[3];
295
296 arraycopy(b_a1a3, 0, ex, 0, 3);
297 X(r_a1n1, ex, ez);
298 double tmp_value = sqrt(dot(ez, ez));
299 for (int i = 0; i < 3; i++) {
300 ez[i] = ez[i] / tmp_value;
301 }
302 X(ez, ex, ey);
303
304 for (int i = 0; i < 3; i++) {
305 b_a1a2[i] = -cos_alpha[0] * ex[i] + sin_alpha[0] * ey[i];
306 b_a3a2[i] = cos_alpha[2] * ex[i] + sin_alpha[2] * ey[i];
307 }
308
309 for (int i = 0; i < 3; i++) {
310 p_s[0][i] = -ex[i];
311 s1[0][i] = ez[i];
312 s2[0][i] = ey[i];
313 p_t[0][i] = b_a1a2[i];
314 t1[0][i] = ez[i];
315 t2[0][i] = sin_alpha[0] * ex[i] + cos_alpha[0] * ey[i];
316 }
317
318 for (int i = 0; i < 3; i++) {
319 p_s[1][i] = -b_a1a2[i];
320 s1[1][i] = -ez[i];
321 s2[1][i] = t2[0][i];
322 p_t[1][i] = -b_a3a2[i];
323 t1[1][i] = -ez[i];
324 t2[1][i] = sin_alpha[2] * ex[i] - cos_alpha[2] * ey[i];
325 }
326
327 for (int i = 0; i < 3; i++) {
328 p_s[2][i] = b_a3a2[i];
329 s2[2][i] = t2[1][i];
330 s1[2][i] = ez[i];
331 p_t[2][i] = ex[i];
332 t1[2][i] = ez[i];
333 t2[2][i] = -ey[i];
334 }
335
336 for (int i = 0; i < 3; i++) {
337 for (int j = 0; j < 3; j++) {
338 p_s_c[i][j] = p_s[i][j] * cos_xi[i];
339 s1_s[i][j] = s1[i][j] * sin_xi[i];
340 s2_s[i][j] = s2[i][j] * sin_xi[i];
341 p_t_c[i][j] = p_t[i][j] * cos_eta[i];
342 t1_s[i][j] = t1[i][j] * sin_eta[i];
343 t2_s[i][j] = t2[i][j] * sin_eta[i];
344 }
345 }
346
347 for (int i = 0; i < 3; i++) {
348 r_tmp[i] = (r_a1n1[i] / len_na[0] - p_s_c[0][i]) / sin_xi[0];
349 }
350
351
352 double sig1Init = DoubleMath.angle(s1[0], r_tmp);
353 if (dot(r_tmp, s2[0]) >= 0.0) {
354 sig1Init = FastMath.abs(sig1Init);
355 } else {
356 sig1Init = -FastMath.abs(sig1Init);
357 }
358
359 for (int i = 0; i < 3; i++) {
360 r_a[0][i] = r_a1[i];
361 r_a[1][i] = r_a1[i] + AA_LEN * b_a1a2[i];
362 r_a[2][i] = r_a3[i];
363 r0[i] = r_a1[i];
364 }
365
366 for (int i_soln = 0; i_soln < numSolutions; i_soln++) {
367 half_tan[2] = roots[i_soln];
368 half_tan[1] = calcT2(half_tan[2]);
369 half_tan[0] = calcT1(half_tan[2], half_tan[1]);
370
371 for (int i = 1; i <= 3; i++) {
372 double ht = half_tan[i - 1];
373 double tmp = 1.0 + ht * ht;
374 cos_tau[i] = (1.0 - ht * ht) / tmp;
375 sin_tau[i] = 2.0 * ht / tmp;
376 }
377
378 cos_tau[0] = cos_tau[3];
379 sin_tau[0] = sin_tau[3];
380
381 for (int i = 0; i < 3; i++) {
382 cos_sig[i] = cos_delta[i] * cos_tau[i] + sin_delta[i] * sin_tau[i];
383 sin_sig[i] = sin_delta[i] * cos_tau[i] - cos_delta[i] * sin_tau[i];
384 }
385
386 for (int i = 0; i < 3; i++) {
387 for (int j = 0; j < 3; j++) {
388 r_s[j] = p_s_c[i][j] + cos_sig[i] * s1_s[i][j] + sin_sig[i] * s2_s[i][j];
389 r_t[j] = p_t_c[i][j] + cos_tau[i + 1] * t1_s[i][j] + sin_tau[i + 1] * t2_s[i][j];
390 r_n[i][j] = r_s[j] * len_na[i] + r_a[i][j];
391 r_c[i][j] = r_t[j] * len_ac[i] + r_a[i][j];
392 }
393 }
394
395 double sig1 = atan2(sin_sig[0], cos_sig[0]);
396 ex_tmp[0] = -ex[0];
397 ex_tmp[1] = -ex[1];
398 ex_tmp[2] = -ex[2];
399 tmp_value = -(sig1 - sig1Init) * 0.25;
400 var rotMatrix = rotationMatrix(ex_tmp, tmp_value);
401
402 for (int i = 0; i < 3; i++) {
403 mat11[i] = r_c[0][i] - r0[i];
404 mat22[i] = r_n[1][i] - r0[i];
405 mat33[i] = r_a[1][i] - r0[i];
406 mat44[i] = r_c[1][i] - r0[i];
407 mat55[i] = r_n[2][i] - r0[i];
408 }
409
410 var mat1 = matrixMultiplication(rotMatrix, mat11);
411 var mat2 = matrixMultiplication(rotMatrix, mat22);
412 var mat3 = matrixMultiplication(rotMatrix, mat33);
413 var mat4 = matrixMultiplication(rotMatrix, mat44);
414 var mat5 = matrixMultiplication(rotMatrix, mat55);
415
416 for (int i = 0; i < 3; i++) {
417 r_soln_n[i_soln][0][i] = r_n1[i];
418 r_soln_a[i_soln][0][i] = r_a1[i];
419 r_soln_c[i_soln][0][i] = mat1[i] + r0[i];
420 r_soln_n[i_soln][1][i] = mat2[i] + r0[i];
421 r_soln_a[i_soln][1][i] = mat3[i] + r0[i];
422 r_soln_c[i_soln][1][i] = mat4[i] + r0[i];
423 r_soln_n[i_soln][2][i] = mat5[i] + r0[i];
424 r_soln_a[i_soln][2][i] = r_a3[i];
425 r_soln_c[i_soln][2][i] = r_c3[i];
426 }
427
428 if (logger.isLoggable(Level.FINE)) {
429 logger.fine(format("roots: t0\t\t t2\t\t t1\t\t %d\n%15.6f %15.6f %15.6f\n",
430 i_soln, half_tan[2], half_tan[1], half_tan[0]));
431 }
432
433 for (int i = 0; i < 3; i++) {
434 rr_a1c1[i] = r_soln_c[i_soln][0][i] - r_soln_a[i_soln][0][i];
435 rr_c1n2[i] = r_soln_n[i_soln][1][i] - r_soln_c[i_soln][0][i];
436 rr_n2a2[i] = r_soln_a[i_soln][1][i] - r_soln_n[i_soln][1][i];
437 rr_a2c2[i] = r_soln_c[i_soln][1][i] - r_soln_a[i_soln][1][i];
438 rr_c2n3[i] = r_soln_n[i_soln][2][i] - r_soln_c[i_soln][1][i];
439 rr_n3a3[i] = r_soln_a[i_soln][2][i] - r_soln_n[i_soln][2][i];
440 rr_a1a2[i] = r_soln_a[i_soln][1][i] - r_soln_a[i_soln][0][i];
441 rr_a2a3[i] = r_soln_a[i_soln][2][i] - r_soln_a[i_soln][1][i];
442 }
443
444 if (logger.isLoggable(Level.FINE)) {
445 double a1c1 = sqrt(dot(rr_a1c1, rr_a1c1));
446 double c1n2 = sqrt(dot(rr_c1n2, rr_c1n2));
447 double n2a2 = sqrt(dot(rr_n2a2, rr_n2a2));
448 double a2c2 = sqrt(dot(rr_a2c2, rr_a2c2));
449 double c2n3 = sqrt(dot(rr_c2n3, rr_c2n3));
450 double n3a3 = sqrt(dot(rr_n3a3, rr_n3a3));
451 double a1a2 = sqrt(dot(rr_a1a2, rr_a1a2));
452 double a2a3 = sqrt(dot(rr_a2a3, rr_a2a3));
453
454 StringBuilder sb = new StringBuilder();
455 sb.append(format("na: n2a2, n3a3 = %9.3f%9.3f%9.3f\n", BOND_LENS[2], n2a2, n3a3));
456 sb.append(format("ac: a1c1, a2c2 = %9.3f%9.3f%9.3f\n", BOND_LENS[0], a1c1, a2c2));
457 sb.append(format("cn: c1n2, c2n3 = %9.3f%9.3f%9.3f\n", BOND_LENS[1], c1n2, c2n3));
458 sb.append(
459 format("aa: a1a2, a2a3 = %9.3f%9.3f%9.3f%9.3f\n", AA_LEN, a1a2, AA_LEN, a2a3));
460 logger.fine(sb.toString());
461 sb.setLength(0);
462
463 for (int i = 0; i < 3; i++) {
464 tmp_array[i] = rr_a1a2[i] / a1a2;
465 }
466
467 DoubleMath.angle(b_a1a3, tmp_array);
468
469 for (int i = 0; i < 3; i++) {
470 tmp_array[i] = rr_a2a3[i] / a2a3;
471 }
472
473 DoubleMath.angle(tmp_array, b_a1a3);
474
475 for (int i = 0; i < 3; i++) {
476 tmp_array[i] = rr_a1c1[i] / a1c1;
477 }
478
479 DoubleMath.angle(b_a1n1, tmp_array);
480
481 for (int i = 0; i < 3; i++) {
482 tmp_array[i] = -rr_n3a3[i] / n3a3;
483 }
484
485 DoubleMath.angle(b_a3c3, tmp_array);
486
487 for (int i = 0; i < 3; i++) {
488 tmp_array1[i] = rr_a2c2[i] / a2c2;
489 tmp_array2[i] = -rr_n2a2[i] / n2a2;
490 }
491
492 DoubleMath.angle(tmp_array1, tmp_array2);
493
494 for (int i = 0; i < 3; i++) {
495 tmp_array1[i] = rr_a1c1[i] / a1c1;
496 tmp_array2[i] = rr_c1n2[i] / c1n2;
497 tmp_array3[i] = rr_n2a2[i] / n2a2;
498 }
499
500 var a1c1n2a2 = dihedralAngle(tmp_array1, tmp_array2, tmp_array3);
501
502 for (int i = 0; i < 3; i++) {
503 tmp_array1[i] = rr_a2c2[i] / a2c2;
504 tmp_array2[i] = rr_c2n3[i] / c2n3;
505 tmp_array3[i] = rr_n3a3[i] / n3a3;
506 }
507
508 var a2c2n3a3 = dihedralAngle(tmp_array1, tmp_array2, tmp_array3);
509 sb.append(format("t_ang1 = %9.3f\n", toDegrees(a1c1n2a2)));
510 sb.append(format("t_ang2 = %9.3f\n", toDegrees(a2c2n3a3)));
511 logger.fine(sb.toString());
512 }
513 }
514 }
515
516
517
518
519
520
521
522
523
524 public boolean getInputAngles(double[] r_n1, double[] r_a1, double[] r_a3, double[] r_c3) {
525 double[] tmp_val = new double[3];
526 for (int i = 0; i < 3; i++) {
527 r_a1a3[i] = r_a3[i] - r_a1[i];
528 }
529
530 var dr_sqr = dot(r_a1a3, r_a1a3);
531 if ((dr_sqr < AA13_MIN_SQR) || (dr_sqr > AA13_MAX_SQR)) {
532 return false;
533 }
534
535 var a1A3Len = sqrt(dr_sqr);
536 for (int i = 0; i < 3; i++) {
537 r_a1n1[i] = r_n1[i] - r_a1[i];
538 }
539
540 len_na[0] = sqrt(dot(r_a1n1, r_a1n1));
541 len_na[1] = BOND_LENS[2];
542 len_na[2] = BOND_LENS[2];
543
544 for (int i = 0; i < 3; i++) {
545 r_a3c3[i] = r_c3[i] - r_a3[i];
546 }
547
548 len_ac[0] = BOND_LENS[0];
549 len_ac[1] = BOND_LENS[0];
550 len_ac[2] = sqrt(dot(r_a3c3, r_a3c3));
551
552 for (int i = 0; i < 3; i++) {
553 b_a1n1[i] = r_a1n1[i] / len_na[0];
554 b_a3c3[i] = r_a3c3[i] / len_ac[2];
555 b_a1a3[i] = r_a1a3[i] / a1A3Len;
556 }
557
558 for (int i = 0; i < 3; i++) {
559 tmp_val[i] = -b_a1n1[i];
560 }
561
562 DELTA[3] = dihedralAngle(r_n1, r_a1, r_a3, r_c3);
563 DELTA[0] = DELTA[3];
564
565 for (int i = 0; i < 3; i++) {
566 tmp_val[i] = -b_a1a3[i];
567 }
568
569 XI[0] = DoubleMath.angle(tmp_val, b_a1n1);
570 ETA[2] = DoubleMath.angle(b_a1a3, b_a3c3);
571
572 for (int i = 0; i < 3; i++) {
573 cos_delta[i + 1] = cos(DELTA[i + 1]);
574 sin_delta[i + 1] = sin(DELTA[i + 1]);
575 cos_xi[i] = cos(XI[i]);
576 sin_xi[i] = sin(XI[i]);
577 sin_xi[i] = sin(XI[i]);
578 cos_eta[i] = cos(ETA[i]);
579 sin_eta[i] = sin(ETA[i]);
580 }
581
582 cos_delta[0] = cos_delta[3];
583 sin_delta[0] = sin_delta[3];
584 theta[0] = BOND_ANGLES[0];
585 theta[1] = BOND_ANGLES[0];
586 theta[2] = BOND_ANGLES[0];
587
588 for (int i = 0; i < 3; i++) {
589 cos_theta[i] = cos(theta[i]);
590 }
591
592 cos_alpha[0] =
593 -((a1A3Len * a1A3Len) + (AA_LEN * AA_LEN) - (AA_LEN * AA_LEN)) / (2.0
594 * a1A3Len * AA_LEN);
595 alpha[0] = acos(cos_alpha[0]);
596 sin_alpha[0] = sin(alpha[0]);
597 cos_alpha[1] =
598 ((AA_LEN * AA_LEN) + (AA_LEN * AA_LEN) - (a1A3Len * a1A3Len)) / (2.0
599 * AA_LEN * AA_LEN);
600 alpha[1] = acos(cos_alpha[1]);
601 sin_alpha[1] = sin(alpha[1]);
602 alpha[2] = PI - alpha[0] + alpha[1];
603 cos_alpha[2] = cos(alpha[2]);
604 sin_alpha[2] = sin(alpha[2]);
605
606 if (logger.isLoggable(Level.FINE)) {
607 StringBuilder sb = new StringBuilder();
608 sb.append(format(" xi = %9.4f%9.4f%9.4f\n", toDegrees(XI[0]), toDegrees(XI[1]),
609 toDegrees(XI[2])));
610 sb.append(format(" eta = %9.4f%9.4f%9.4f\n", toDegrees(ETA[0]), toDegrees(ETA[1]),
611 toDegrees(ETA[2])));
612 sb.append(format(" delta = %9.4f%9.4f%9.4f\n", toDegrees(DELTA[1]), toDegrees(DELTA[2]),
613 toDegrees(DELTA[3])));
614 sb.append(format(" theta = %9.4f%9.4f%9.4f\n", toDegrees(theta[0]), toDegrees(theta[1]),
615 toDegrees(theta[2])));
616 sb.append(format(" alpha = %9.4f%9.4f%9.4f\n", toDegrees(alpha[0]), toDegrees(alpha[1]),
617 toDegrees(alpha[2])));
618 logger.fine(sb.toString());
619 }
620
621 for (int i = 0; i < 3; i++) {
622 if (!testTwoConeExistenceSoln(theta[i], XI[i], ETA[i], alpha[i])) {
623 return false;
624 }
625 }
626
627
628 return true;
629 }
630
631
632
633
634
635
636 public void getPolyCoeff(double[] polyCoeff) {
637 double[] B0 = new double[3];
638 double[] B1 = new double[3];
639 double[] B2 = new double[3];
640 double[] B3 = new double[3];
641 double[] B4 = new double[3];
642 double[] B5 = new double[3];
643 double[] B6 = new double[3];
644 double[] B7 = new double[3];
645 double[] B8 = new double[3];
646 double[][] u11 = new double[5][5];
647 double[][] u12 = new double[5][5];
648 double[][] u13 = new double[5][5];
649 double[][] u31 = new double[5][5];
650 double[][] u32 = new double[5][5];
651 double[][] u33 = new double[5][5];
652 double[][] um1 = new double[5][5];
653 double[][] um2 = new double[5][5];
654 double[][] um3 = new double[5][5];
655 double[][] um4 = new double[5][5];
656 double[][] um5 = new double[5][5];
657 double[][] um6 = new double[5][5];
658 double[][] q_tmp = new double[5][5];
659 int[] p1 = new int[2];
660 int[] p3 = new int[2];
661 int[] p_um1 = new int[2];
662 int[] p_um2 = new int[2];
663 int[] p_um3 = new int[2];
664 int[] p_um4 = new int[2];
665 int[] p_um5 = new int[2];
666 int[] p_um6 = new int[2];
667 int[] p_Q = new int[2];
668 int[] p_f1 = new int[1];
669 int[] p_f2 = new int[1];
670 int[] p_f3 = new int[1];
671 int[] p_f4 = new int[1];
672 int[] p_f5 = new int[1];
673 int[] p_f6 = new int[1];
674 int[] p_f7 = new int[1];
675 int[] p_f8 = new int[1];
676 int[] p_f9 = new int[1];
677 int[] p_f10 = new int[1];
678 int[] p_f11 = new int[1];
679 int[] p_f12 = new int[1];
680 int[] p_f13 = new int[1];
681 int[] p_f14 = new int[1];
682 int[] p_f15 = new int[1];
683 int[] p_f16 = new int[1];
684 int[] p_f17 = new int[1];
685 int[] p_f18 = new int[1];
686 int[] p_f19 = new int[1];
687 int[] p_f20 = new int[1];
688 int[] p_f21 = new int[1];
689 int[] p_f22 = new int[1];
690 int[] p_f23 = new int[1];
691 int[] p_f24 = new int[1];
692 int[] p_f25 = new int[1];
693 int[] p_f26 = new int[1];
694 int[] p_final = new int[1];
695 double[] f1 = new double[17];
696 double[] f2 = new double[17];
697 double[] f3 = new double[17];
698 double[] f4 = new double[17];
699 double[] f5 = new double[17];
700 double[] f6 = new double[17];
701 double[] f7 = new double[17];
702 double[] f8 = new double[17];
703 double[] f9 = new double[17];
704 double[] f10 = new double[17];
705 double[] f11 = new double[17];
706 double[] f12 = new double[17];
707 double[] f13 = new double[17];
708 double[] f14 = new double[17];
709 double[] f15 = new double[17];
710 double[] f16 = new double[17];
711 double[] f17 = new double[17];
712 double[] f18 = new double[17];
713 double[] f19 = new double[17];
714 double[] f20 = new double[17];
715 double[] f21 = new double[17];
716 double[] f22 = new double[17];
717 double[] f23 = new double[17];
718 double[] f24 = new double[17];
719 double[] f25 = new double[17];
720 double[] f26 = new double[17];
721
722 for (int i = 0; i < 3; i++) {
723 double A0 = cos_alpha[i] * cos_xi[i] * cos_eta[i] - cos_theta[i];
724 double A1 = -sin_alpha[i] * cos_xi[i] * sin_eta[i];
725 double A2 = sin_alpha[i] * sin_xi[i] * cos_eta[i];
726 double A3 = sin_xi[i] * sin_eta[i];
727 double A4 = A3 * cos_alpha[i];
728 double A21 = A2 * cos_delta[i];
729 double A22 = A2 * sin_delta[i];
730 double A31 = A3 * cos_delta[i];
731 double A32 = A3 * sin_delta[i];
732 double A41 = A4 * cos_delta[i];
733 double A42 = A4 * sin_delta[i];
734 B0[i] = A0 + A22 + A31;
735 B1[i] = 2.0 * (A1 + A42);
736 B2[i] = 2.0 * (A32 - A21);
737 B3[i] = -4.0 * A41;
738 B4[i] = A0 + A22 - A31;
739 B5[i] = A0 - A22 - A31;
740 B6[i] = -2.0 * (A21 + A32);
741 B7[i] = 2.0 * (A1 - A42);
742 B8[i] = A0 - A22 + A31;
743 }
744
745 C0[0][0] = B0[0];
746 C0[0][1] = B2[0];
747 C0[0][2] = B5[0];
748 C1[0][0] = B1[0];
749 C1[0][1] = B3[0];
750 C1[0][2] = B7[0];
751 C2[0][0] = B4[0];
752 C2[0][1] = B6[0];
753 C2[0][2] = B8[0];
754
755 for (int i = 1; i < 3; i++) {
756 C0[i][0] = B0[i];
757 C0[i][1] = B1[i];
758 C0[i][2] = B4[i];
759 C1[i][0] = B2[i];
760 C1[i][1] = B3[i];
761 C1[i][2] = B6[i];
762 C2[i][0] = B5[i];
763 C2[i][1] = B7[i];
764 C2[i][2] = B8[i];
765 }
766
767 for (int i = 0; i < 3; i++) {
768 u11[0][i] = C0[0][i];
769 u12[0][i] = C1[0][i];
770 u13[0][i] = C2[0][i];
771 u31[i][0] = C0[1][i];
772 u32[i][0] = C1[1][i];
773 u33[i][0] = C2[1][i];
774 }
775
776 p1[0] = 2;
777 p1[1] = 0;
778 p3[0] = 0;
779 p3[1] = 2;
780
781 polyMulSub2(u32, u32, u31, u33, p3, p3, p3, p3, um1, p_um1);
782 polyMulSub2(u12, u32, u11, u33, p1, p3, p1, p3, um2, p_um2);
783 polyMulSub2(u12, u33, u13, u32, p1, p3, p1, p3, um3, p_um3);
784 polyMulSub2(u11, u33, u31, u13, p1, p3, p3, p1, um4, p_um4);
785 polyMulSub2(u13, um1, u33, um2, p1, p_um1, p3, p_um2, um5, p_um5);
786 polyMulSub2(u13, um4, u12, um3, p1, p_um4, p1, p_um3, um6, p_um6);
787 polyMulSub2(u11, um5, u31, um6, p1, p_um5, p3, p_um6, q_tmp, p_Q);
788
789 for (int i = 0; i < 5; i++) {
790 arraycopy(q_tmp[i], 0, Q[i], 0, 5);
791 }
792
793 for (int i = 0; i < 3; i++) {
794 for (int j = 0; j < 17; j++) {
795 R[i][j] = 0.0;
796 }
797 }
798
799 for (int i = 0; i < 3; i++) {
800 R[0][i] = C0[2][i];
801 R[1][i] = C1[2][i];
802 R[2][i] = C2[2][i];
803 }
804
805 int p2 = 2;
806 int p4 = 4;
807
808 polyMulSub1(R[1], R[1], R[0], R[2], p2, p2, p2, p2, f1, p_f1);
809 polyMul1(R[1], R[2], p2, p2, f2, p_f2);
810 polyMulSub1(R[1], f1, R[0], f2, p2, p_f1[0], p2, p_f2[0], f3, p_f3);
811 polyMul1(R[2], f1, p2, p_f1[0], f4, p_f4);
812 polyMulSub1(R[1], f3, R[0], f4, p2, p_f3[0], p2, p_f4[0], f5, p_f5);
813 polyMulSub1(Q[1], R[1], Q[0], R[2], p4, p2, p4, p2, f6, p_f6);
814 polyMulSub1(Q[2], f1, R[2], f6, p4, p_f1[0], p2, p_f6[0], f7, p_f7);
815 polyMulSub1(Q[3], f3, R[2], f7, p4, p_f3[0], p2, p_f7[0], f8, p_f8);
816 polyMulSub1(Q[4], f5, R[2], f8, p4, p_f5[0], p2, p_f8[0], f9, p_f9);
817 polyMulSub1(Q[3], R[1], Q[4], R[0], p4, p2, p4, p2, f10, p_f10);
818 polyMulSub1(Q[2], f1, R[0], f10, p4, p_f1[0], p2, p_f10[0], f11, p_f11);
819 polyMulSub1(Q[1], f3, R[0], f11, p4, p_f3[0], p2, p_f11[0], f12, p_f12);
820 polyMulSub1(Q[2], R[1], Q[1], R[2], p4, p2, p4, p2, f13, p_f13);
821 polyMulSub1(Q[3], f1, R[2], f13, p4, p_f1[0], p2, p_f13[0], f14, p_f14);
822 polyMulSub1(Q[3], R[1], Q[2], R[2], p4, p2, p4, p2, f15, p_f15);
823 polyMulSub1(Q[4], f1, R[2], f15, p4, p_f1[0], p2, p_f15[0], f16, p_f16);
824 polyMulSub1(Q[1], f14, Q[0], f16, p4, p_f14[0], p4, p_f16[0], f17, p_f17);
825 polyMulSub1(Q[2], R[2], Q[3], R[1], p4, p2, p4, p2, f18, p_f18);
826 polyMulSub1(Q[1], R[2], Q[3], R[0], p4, p2, p4, p2, f19, p_f19);
827 polyMulSub1(Q[3], f19, Q[2], f18, p4, p_f19[0], p4, p_f18[0], f20, p_f20);
828 polyMulSub1(Q[1], R[1], Q[2], R[0], p4, p2, p4, p2, f21, p_f21);
829 polyMul1(Q[4], f21, p4, p_f21[0], f22, p_f22);
830 polySub1(f20, f22, p_f20, p_f22, f23, p_f23);
831 polyMul1(R[0], f23, p2, p_f23[0], f24, p_f24);
832 polySub1(f17, f24, p_f17, p_f24, f25, p_f25);
833 polyMulSub1(Q[4], f12, R[2], f25, p4, p_f12[0], p2, p_f25[0], f26, p_f26);
834 polyMulSub1(Q[0], f9, R[0], f26, p4, p_f9[0], p2, p_f26[0], polyCoeff, p_final);
835
836 if (p_final[0] != 16) {
837 logger.info("Error. Degree of polynomial is not 16!");
838 return;
839 }
840
841 if (polyCoeff[16] < 0.0e0) {
842 for (int i = 0; i < 17; i++) {
843 polyCoeff[i] *= -1.0;
844 }
845 }
846
847 if (logger.isLoggable(Level.FINE)) {
848 StringBuilder string = new StringBuilder();
849 string.append(" Polynomial Coefficients\n");
850 for (int i = 0; i < 17; i++) {
851 string.append(format(" %5d %15.6f\n", i, polyCoeff[i]));
852 }
853 string.append("\n");
854 logger.fine(string.toString());
855 }
856 }
857
858
859
860
861
862
863
864
865 private static double[] matrixMultiplication(double[][] ma, double[] mb) {
866 RealMatrix matrix = new Array2DRowRealMatrix(ma);
867 matrix.multiply(matrix);
868 RealVector vector = new ArrayRealVector(mb);
869 RealVector result = matrix.operate(vector);
870 return result.toArray();
871 }
872
873
874
875
876
877
878
879
880
881
882
883 public void polyMul1(double[] u1, double[] u2, int p1, int p2, double[] u3, int[] p3) {
884 p3[0] = p1 + p2;
885 for (int i = 0; i < 17; i++) {
886 u3[i] = 0.0;
887 }
888 for (int i1 = 0; i1 <= p1; i1++) {
889 double u1i = u1[i1];
890 for (int i2 = 0; i2 <= p2; i2++) {
891 int i3 = i1 + i2;
892 u3[i3] = u3[i3] + u1i * u2[i2];
893 }
894 }
895 }
896
897
898
899
900
901
902
903
904
905
906
907 public void polyMul2(double[][] u1, double[][] u2, int[] p1, int[] p2, double[][] u3, int[] p3) {
908 for (int i = 0; i < 2; i++) {
909 p3[i] = p1[i] + p2[i];
910 }
911 for (int i = 0; i < 5; i++) {
912 for (int j = 0; j < 5; j++) {
913 u3[i][j] = 0.0;
914 }
915 }
916 int p11 = p1[0];
917 int p12 = p1[1];
918 int p21 = p2[0];
919 int p22 = p2[1];
920 for (int i1 = 0; i1 <= p12; i1++) {
921 for (int j1 = 0; j1 <= p11; j1++) {
922 double u1ij = u1[i1][j1];
923 for (int i2 = 0; i2 <= p22; i2++) {
924 int i3 = i1 + i2;
925 for (int j2 = 0; j2 <= p21; j2++) {
926 int j3 = j1 + j2;
927 u3[i3][j3] = u3[i3][j3] + u1ij * u2[i2][j2];
928 }
929 }
930 }
931 }
932 }
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948 public void polyMulSub1(double[] u1, double[] u2, double[] u3, double[] u4, int p1, int p2, int p3,
949 int p4, double[] u5, int[] p5) {
950
951 double[] d1 = new double[17];
952 double[] d2 = new double[17];
953 int[] pd1 = new int[1];
954 int[] pd2 = new int[1];
955
956 polyMul1(u1, u2, p1, p2, d1, pd1);
957 polyMul1(u3, u4, p3, p4, d2, pd2);
958 polySub1(d1, d2, pd1, pd2, u5, p5);
959 }
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975 public void polyMulSub2(double[][] u1, double[][] u2, double[][] u3, double[][] u4, int[] p1,
976 int[] p2, int[] p3, int[] p4, double[][] u5, int[] p5) {
977 double[][] d1 = new double[5][5];
978 double[][] d2 = new double[5][5];
979 int[] pd1 = new int[2];
980 int[] pd2 = new int[2];
981
982 polyMul2(u1, u2, p1, p2, d1, pd1);
983 polyMul2(u3, u4, p3, p4, d2, pd2);
984 polySub2(d1, d2, pd1, pd2, u5, p5);
985 }
986
987
988
989
990
991
992
993
994
995
996
997 public void polySub1(double[] u1, double[] u2, int[] p1, int[] p2, double[] u3, int[] p3) {
998 p3[0] = max(p1[0], p2[0]);
999 for (int i = 0; i <= p3[0]; i++) {
1000 if (i > p2[0]) {
1001 u3[i] = u1[i];
1002 } else if (i > p1[0]) {
1003 u3[i] = -u2[i];
1004 } else {
1005 u3[i] = u1[i] - u2[i];
1006 }
1007 }
1008 }
1009
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020 public void polySub2(double[][] u1, double[][] u2, int[] p1, int[] p2, double[][] u3, int[] p3) {
1021 int p11 = p1[0];
1022 int p12 = p1[1];
1023 int p21 = p2[0];
1024 int p22 = p2[1];
1025 p3[0] = max(p11, p21);
1026 p3[1] = max(p12, p22);
1027 for (int i = 0; i <= p3[1]; i++) {
1028 boolean i1_ok = (i > p12);
1029 boolean i2_ok = (i > p22);
1030 for (int j = 0; j <= p3[0]; j++) {
1031 if (i2_ok || (j > p21)) {
1032 u3[i][j] = u1[i][j];
1033 } else if (i1_ok || (j > p11)) {
1034 u3[i][j] = -u2[i][j];
1035 } else {
1036 u3[i][j] = u1[i][j] - u2[i][j];
1037 }
1038 }
1039 }
1040 }
1041
1042
1043
1044
1045
1046
1047
1048
1049 private static double[][] rotationMatrix(double[] axis, double angle) {
1050
1051 var tan_w = tan(angle);
1052 var tan_sqr = tan_w * tan_w;
1053 var tan1 = 1.0 + tan_sqr;
1054 var cosine = (1.0 - tan_sqr) / tan1;
1055 var sine = 2.0 * tan_w / tan1;
1056 var quaternion = new double[]{cosine, axis[0] * sine, axis[1] * sine, axis[2] * sine};
1057
1058
1059 var b0 = 2.0 * quaternion[0];
1060 var b1 = 2.0 * quaternion[1];
1061 var q00 = b0 * quaternion[0] - 1.0;
1062 var q02 = b0 * quaternion[2];
1063 var q03 = b0 * quaternion[3];
1064 var q11 = b1 * quaternion[1];
1065 var q12 = b1 * quaternion[2];
1066 var q13 = b1 * quaternion[3];
1067 var b2 = 2.0 * quaternion[2];
1068 var b3 = 2.0 * quaternion[3];
1069 var q01 = b0 * quaternion[1];
1070 var q22 = b2 * quaternion[2];
1071 var q23 = b2 * quaternion[3];
1072 var q33 = b3 * quaternion[3];
1073
1074
1075 var result = new double[3][3];
1076 result[0][0] = q00 + q11;
1077 result[0][1] = q12 - q03;
1078 result[0][2] = q13 + q02;
1079 result[1][0] = q12 + q03;
1080 result[1][1] = q00 + q22;
1081 result[1][2] = q23 - q01;
1082 result[2][0] = q13 - q02;
1083 result[2][1] = q23 + q01;
1084 result[2][2] = q00 + q33;
1085
1086 return result;
1087 }
1088
1089
1090
1091
1092
1093
1094
1095
1096
1097
1098
1099
1100
1101 public int solve3PepPoly(double[] r_n1, double[] r_a1, double[] r_a3, double[] r_c3,
1102 double[][][] r_soln_n, double[][][] r_soln_a, double[][][] r_soln_c) {
1103 var polyCoeff = new double[MAX_SOLUTIONS + 1];
1104 var roots = new double[MAX_SOLUTIONS];
1105
1106 if (!getInputAngles(r_n1, r_a1, r_a3, r_c3)) {
1107
1108 return 0;
1109 }
1110
1111 getPolyCoeff(polyCoeff);
1112
1113 var sturmMethod = new SturmMethod();
1114 var solutions = sturmMethod.solveSturm(MAX_SOLUTIONS, polyCoeff, roots);
1115 if (solutions > 0) {
1116 getCoordsFromPolyRoots(solutions, roots, r_n1, r_a1, r_a3, r_c3, r_soln_n, r_soln_a, r_soln_c);
1117 } else {
1118 logger.info("Could not find alternative loop solutions using KIC.");
1119 }
1120
1121 return solutions;
1122 }
1123
1124
1125
1126
1127
1128
1129
1130
1131
1132 private boolean testTwoConeExistenceSoln(double tt, double kx, double et, double ap) {
1133 double at = ap - tt;
1134 double ex = kx + et;
1135 double abs_at = abs(at);
1136 return (abs_at <= ex);
1137 }
1138 }