00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00043
00065
00067
00068
00069 #define pi 3.141592653589793238462643383279502884197e0
00070
00071 #define two_pi 2.0e0*pi
00072 #define deg2rad pi/180.0e0
00073 #define rad2deg 180.0e0/pi
00074 #define max(a,b) ((a) > (b))? (a) : (b)
00075 #define min(a,b) ((a) < (b))? (a) : (b)
00076
00077 int max_soln = 16;
00078
00079 int deg_pol = 16;
00080
00081 int print_level = 1;
00082
00083
00084 double len0[6], b_ang0[7], t_ang0[2];
00085
00086 double aa13_min_sqr, aa13_max_sqr;
00087
00088 double delta[4], xi[3], eta[3], alpha[3], theta[3];
00089
00090 double cos_alpha[3], sin_alpha[3], cos_theta[3], sin_theta[3];
00091
00092 double cos_delta[4], sin_delta[4];
00093
00094 double cos_xi[3], cos_eta[3], sin_xi[3], sin_eta[3];
00095
00096 double r_a1a3[3], r_a1n1[3], r_a3c3[3];
00097
00098 double b_a1a3[3], b_a1n1[3], b_a3c3[3];
00099
00100 double len_na[3], len_ac[3], len_aa[3];
00101
00102
00103 double C0[3][3], C1[3][3], C2[3][3];
00104
00105 double Q[5][17], R[3][17];
00106
00107
00109 double dot_product(double va[], double vb[]);
00110 void matmul(double ma[3][3], double mb[3], double mc[3]);
00111 double sign(double a, double b);
00112 void solve_3pep_poly(double r_n1[3], double r_a1[3], double r_a3[3], double r_c3[3], double r_soln_n[16][3][3], double r_soln_a[16][3][3], double r_soln_c[16][3][3], int *n_soln);
00113 void initialize_loop_closure(double b_len[], double b_ang[], double t_ang[]);
00114 void get_input_angles(int *n_soln, double r_n1[], double r_a1[], double r_a3[], double r_c3[]);
00115 void test_two_cone_existence_soln(double tt, double kx, double et, double ap, int *n_soln, char cone_type[]);
00116 void get_poly_coeff(double poly_coeff[]);
00117 void poly_mul_sub2(double u1[5][5], double u2[5][5], double u3[5][5], double u4[5][5], int p1[2], int p2[2], int p3[2], int p4[2], double u5[5][5], int p5[2]);
00118 void poly_mul2(double u1[5][5], double u2[5][5], int p1[2], int p2[2], double u3[5][5], int p3[2]);
00119 void poly_sub2(double u1[5][5], double u2[5][5], int p1[2], int p2[2], double u3[5][5], int p3[2]);
00120 void poly_mul_sub1(double u1[17], double u2[17], double u3[17], double u4[17], int p1, int p2, int p3, int p4, double u5[17], int *p5);
00121 void poly_mul1(double u1[], double u2[], int p1, int p2, double u3[], int *p3);
00122 void poly_sub1(double u1[], double u2[], int p1, int p2, double u3[], int *p3);
00123 void coord_from_poly_roots(int *n_soln, double roots[16], double r_n1[3], double r_a1[3], double r_a3[3], double r_c3[3], double r_soln_n[16][3][3], double r_soln_a[16][3][3], double r_soln_c[16][3][3]);
00124 double calc_t2(double t0);
00125 double calc_t1(double t0, double t2);
00126 void calc_dih_ang(double r1[], double r2[], double r3[], double *angle);
00127 void calc_bnd_ang(double r1[], double r2[], double *angle);
00128 void cross(double p[], double q[], double s[]);
00129 void quaternion(double axis[], double quarter_ang, double p[]);
00130 void rotation_matrix(double q[4], double U[3][3]);
00132
00133 double dot_product(double va[3], double vb[3])
00134 {
00135 return va[0]*vb[0] + va[1]*vb[1] + va[2]*vb[2];
00136 }
00137
00138 void matmul(double ma[3][3], double mb[3], double mc[3])
00139 {
00140 int i, j;
00141
00142 for(i=0;i<3;i++)
00143 {
00144 mc[i] = 0.;
00145 for(j=0;j<3;j++)
00146 mc[i] += ma[i][j]*mb[j];
00147 }
00148 return;
00149 }
00150
00151 double sign(double a, double b)
00152 {
00153 if (b>=0.)
00154 return fabs(a);
00155 else
00156 return -fabs(a);
00157 }
00159
00160
00161 void solve_3pep_poly(double r_n1[3], double r_a1[3], double r_a3[3], double r_c3[3], double r_soln_n[16][3][3], double r_soln_a[16][3][3], double r_soln_c[16][3][3], int *n_soln)
00162 {
00163
00164
00165
00166
00167
00168 double poly_coeff[deg_pol+1], roots[max_soln];
00169
00170
00171 get_input_angles(n_soln, r_n1, r_a1, r_a3, r_c3);
00172
00173
00174
00175
00176 if (*n_soln == 0)
00177 return;
00178
00179
00180 get_poly_coeff(poly_coeff);
00181
00182
00183 solve_sturm(°_pol, n_soln, poly_coeff, roots);
00184
00185
00187
00188
00189
00190 if (*n_soln == 0)
00191 {
00192 return;
00193
00194 }
00195
00196
00197 coord_from_poly_roots(n_soln, roots, r_n1, r_a1, r_a3, r_c3, r_soln_n, r_soln_a, r_soln_c);
00198
00199 return;
00200
00201 }
00203
00204 void initialize_loop_closure(double b_len[6], double b_ang[7], double t_ang[2])
00205 {
00209
00210
00211
00212 double len1, len2, a_min, a_max;
00213
00214 double axis[3], rr_a1[3], rr_c1[3], rr_n2[3], rr_a2[3], rr_n2a2_ref[3], rr_c1a1[3];
00215
00216 double rr_a1a2[3], dr[3], bb_c1a1[3], bb_a1a2[3], bb_a2n2[3];
00217
00218
00219
00220 double p[4], Us[3][3];
00221 double mulpro[3];
00222 double tmp_val[3];
00223
00224 double tol_secant = 1.0e-15;
00225
00226 int max_iter_sturm = 100;
00227 int max_iter_secant = 20;
00228
00229 int i, j;
00230
00231
00232 initialize_sturm(&tol_secant, &max_iter_sturm, &max_iter_secant);
00233
00234
00235 for(i=0;i<6;i++)
00236 len0[i] = b_len[i];
00237
00238 for(i=0;i<7;i++)
00239 b_ang0[i] = b_ang[i];
00240
00241 for(i=0;i<2;i++)
00242 t_ang0[i] = t_ang[i];
00243
00244
00245 for(i=0;i<3;i++)
00246 rr_c1[i] = 0.;
00247
00248 axis[0] = 1.;
00249 axis[1] = 0.;
00250 axis[2] = 0.;
00251
00252
00253 for(i=0;i<2;i++)
00254 {
00255
00256 rr_a1[0] = cos(b_ang0[3*i+1])*len0[3*i];
00257 rr_a1[1] = sin(b_ang0[3*i+1])*len0[3*i];
00258 rr_a1[2] = 0.0e0;
00259
00260 rr_n2[0] = len0[3*i+1];
00261 rr_n2[1] = 0.0e0;
00262 rr_n2[2] = 0.0e0;
00263
00264 for(j=0;j<3;j++)
00265 rr_c1a1[j] = rr_a1[j] - rr_c1[j];
00266
00267 rr_n2a2_ref[0] = -cos(b_ang0[3*i+2])*len0[3*i+2];
00268 rr_n2a2_ref[1] = sin(b_ang0[3*i+2])*len0[3*i+2];
00269 rr_n2a2_ref[2] = 0.0e0;
00270
00271 quaternion(axis, t_ang0[i]*0.25e0, p);
00272
00273 rotation_matrix(p, Us);
00274
00275
00276
00277 matmul(Us, rr_n2a2_ref, mulpro);
00278 for(j=0;j<3;j++)
00279 {
00280 rr_a2[j] = mulpro[j] + rr_n2[j];
00281 rr_a1a2[j] = rr_a2[j] - rr_a1[j];
00282 dr[j] = rr_a1a2[j];
00283 }
00284
00285 len2 = dot_product(dr, dr);
00286
00287 len1 = sqrt(len2);
00288
00289
00290 len_aa[i+1] = len1;
00291
00292
00293
00294 for(j=0;j<3;j++)
00295 {
00296 bb_c1a1[j] = rr_c1a1[j]/len0[3*i];
00297 bb_a1a2[j] = rr_a1a2[j]/len1;
00298 bb_a2n2[j] = (rr_n2[j] - rr_a2[j])/len0[3*i+2];
00299 }
00300
00301
00302 for(j=0;j<3;j++)
00303 tmp_val[j] = -bb_a1a2[j];
00304 calc_bnd_ang(tmp_val, bb_a2n2, &xi[i+1]);
00305
00306
00307 for(j=0;j<3;j++)
00308 tmp_val[j] = -bb_c1a1[j];
00309 calc_bnd_ang(bb_a1a2, tmp_val, &eta[i]);
00310
00311
00312 calc_dih_ang(bb_c1a1, bb_a1a2, bb_a2n2, &delta[i+1]);
00313
00314 delta[i+1] = pi - delta[i+1];
00315 }
00316
00317
00318
00319 a_min = b_ang[3] - (xi[1] + eta[1]);
00320
00321 a_max = min((b_ang[3] + (xi[1] + eta[1])), pi);
00322
00323
00325
00327
00328
00329 aa13_min_sqr = pow(len_aa[1],2) + pow(len_aa[2],2) - 2.0e0*len_aa[1]*len_aa[2]*cos(a_min);
00330
00331 aa13_max_sqr = pow(len_aa[1],2) + pow(len_aa[2],2) - 2.0e0*len_aa[1]*len_aa[2]*cos(a_max);
00333
00334
00335
00336 }
00338
00339 void get_input_angles(int *n_soln, double r_n1[3], double r_a1[3], double r_a3[3], double r_c3[3])
00340 {
00344
00345
00346
00347
00348 double dr_sqr;
00349 double tmp_val[3];
00350
00351 int i;
00352
00353 char cone_type[2];
00355
00356
00357 *n_soln = max_soln;
00358
00359
00360
00361 for(i=0;i<3;i++)
00362 r_a1a3[i] = r_a3[i] - r_a1[i];
00363
00364 dr_sqr = dot_product(r_a1a3,r_a1a3);
00365
00366 len_aa[0] = sqrt(dr_sqr);
00367
00368
00369
00372
00373
00374 if ((dr_sqr < aa13_min_sqr) || (dr_sqr > aa13_max_sqr))
00375 {
00376 *n_soln = 0;
00377 return;
00378 }
00379
00380
00381
00382 for(i=0;i<3;i++)
00383 r_a1n1[i] = r_n1[i] - r_a1[i];
00384
00385 len_na[0] = sqrt(dot_product(r_a1n1,r_a1n1));
00386
00387 len_na[1] = len0[2];
00388
00389 len_na[2] = len0[5];
00390
00391 for(i=0;i<3;i++)
00392 r_a3c3[i] = r_c3[i] - r_a3[i];
00393
00394 len_ac[0] = len0[0];
00395
00396 len_ac[1] = len0[3];
00397
00398 len_ac[2] = sqrt(dot_product(r_a3c3,r_a3c3));
00399
00400
00401
00402
00403
00404 for(i=0;i<3;i++)
00405 {
00406 b_a1n1[i] = r_a1n1[i]/len_na[0];
00407 b_a3c3[i] = r_a3c3[i]/len_ac[2];
00408 b_a1a3[i] = r_a1a3[i]/len_aa[0];
00409 }
00410
00411
00412
00413 for(i=0;i<3;i++)
00414 tmp_val[i] = -b_a1n1[i];
00415 calc_dih_ang(tmp_val, b_a1a3, b_a3c3, &delta[3]);
00416
00417 delta[0] = delta[3];
00418
00419
00420
00421 for(i=0;i<3;i++)
00422 tmp_val[i] = -b_a1a3[i];
00423 calc_bnd_ang(tmp_val, b_a1n1, &xi[0]);
00424
00425
00426
00427 calc_bnd_ang(b_a1a3, b_a3c3, &eta[2]);
00428
00429
00430 for(i=0;i<3;i++)
00431 {
00432
00433 cos_delta[i+1] = cos(delta[i+1]);
00434
00435 sin_delta[i+1] = sin(delta[i+1]);
00436
00437
00438 cos_xi[i] = cos(xi[i]);
00439
00440 sin_xi[i] = sin(xi[i]);
00441
00442 sin_xi[i] = sin(xi[i]);
00443
00444 cos_eta[i] = cos(eta[i]);
00445
00446 cos_eta[i] = cos(eta[i]);
00447
00448 sin_eta[i] = sin(eta[i]);
00449
00450 sin_eta[i] = sin(eta[i]);
00451
00452 }
00453
00454 cos_delta[0] = cos_delta[3];
00455
00456 sin_delta[0] = sin_delta[3];
00457
00458
00459
00460 theta[0] = b_ang0[0];
00461
00462 theta[1] = b_ang0[3];
00463
00464 theta[2] = b_ang0[6];
00465
00466
00467
00468 for(i=0;i<3;i++)
00469 cos_theta[i] = cos(theta[i]);
00470
00471
00472
00473 cos_alpha[0] = -(pow(len_aa[0],2) + pow(len_aa[1],2) - pow(len_aa[2],2))/(2.0e0*len_aa[0]*len_aa[1]);
00474
00475 alpha[0] = acos(cos_alpha[0]);
00476
00477 sin_alpha[0] = sin(alpha[0]);
00478
00479 cos_alpha[1] = (pow(len_aa[1],2) + pow(len_aa[2],2) - pow(len_aa[0],2))/(2.0e0*len_aa[1]*len_aa[2]);
00480
00481 alpha[1] = acos(cos_alpha[1]);
00482
00483 sin_alpha[1] = sin(alpha[1]);
00484
00485 alpha[2] = pi - alpha[0] + alpha[1];
00486
00487 cos_alpha[2] = cos(alpha[2]);
00488
00489 sin_alpha[2] = sin(alpha[2]);
00490
00491
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501
00502
00503
00504
00505
00506
00507
00508
00509 for(i=0;i<3;i++)
00510 {
00511
00512
00513 test_two_cone_existence_soln(theta[i], xi[i], eta[i], alpha[i], n_soln, cone_type);
00514
00515
00516
00517
00518 if (*n_soln == 0)
00519 return;
00520
00521 }
00522
00523 return;
00524
00525 }
00527
00528 void test_two_cone_existence_soln(double tt, double kx, double et, double ap, int *n_soln, char cone_type[2])
00529 {
00530
00531
00532
00533
00534
00535
00536 double at, ex, abs_at, ap1, kx1, et1;
00537
00538 double cos_tx1, cos_tx2, cos_te1, cos_te2, cos_ea1, cos_ea2, cos_xa1, cos_xa2;
00539
00540 int s1, s2, t1, t2;
00541 int complicated = 0;
00542
00543
00544
00545 *n_soln = max_soln;
00546
00547
00548 ap1 = ap;
00549
00550 kx1 = kx;
00551
00552 et1 = et;
00553
00554
00555 at = ap1 - tt;
00556
00557 ex = kx1 + et1;
00558
00559 abs_at = fabs(at);
00560
00561
00562
00563
00564
00565
00566 if (abs_at > ex)
00567 {
00568 *n_soln = 0;
00569 return;
00570 }
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588 if (complicated)
00589 {
00590
00591 cos_tx1 = cos(tt+kx1);
00592
00593 cos_tx2 = cos(tt-kx1);
00594
00595 cos_te1 = cos(tt+et1);
00596
00597 cos_te2 = cos(tt-et1);
00598
00599 cos_ea1 = cos(et1+ap1);
00600
00601 cos_ea2 = cos(et1-ap1);
00602
00603 cos_xa1 = cos(kx1+ap1);
00604
00605 cos_xa2 = cos(kx1-ap1);
00606
00607 s1 = 0;
00608 s2 = 0;
00609 t1 = 0;
00610 t2 = 0;
00611
00612 if ((cos_te1-cos_xa2)*(cos_te1-cos_xa1) <= 0.0e0)
00613 s1 = 0;
00614
00615 if ((cos_te2-cos_xa2)*(cos_te2-cos_xa1) <= 0.0e0)
00616 s2 = 0;
00617
00618 if ((cos_tx1-cos_ea2)*(cos_tx1-cos_ea1) <= 0.0e0)
00619 t1 = 0;
00620
00621 if ((cos_tx2-cos_ea2)*(cos_tx2-cos_ea1) <= 0.0e0)
00622 t2 = 0;
00623 }
00624
00625 return;
00626
00627 }
00629
00630 void get_poly_coeff(double poly_coeff[16+1])
00631 {
00632
00633
00634
00635 int i, j;
00636
00637 double A0, A1, A2, A3, A4, A21, A22, A31, A32, A41, A42;
00638
00639 double B0[3], B1[3], B2[3], B3[3], B4[3], B5[3], B6[3], B7[3], B8[3];
00640
00641 double u11[5][5], u12[5][5], u13[5][5], u31[5][5], u32[5][5], u33[5][5];
00642
00643 double um1[5][5], um2[5][5], um3[5][5], um4[5][5], um5[5][5], um6[5][5], q_tmp[5][5];
00644
00645 int p1 [2], p3[2], p_um1[2], p_um2[2], p_um3[2], p_um4[2], p_um5[2], p_um6[2], p_Q[2];
00646
00647
00648
00649 int p2, p4, p_f1, p_f2, p_f3, p_f4, p_f5, p_f6, p_f7, p_f8, p_f9;
00650 int p_f10, p_f11, p_f12, p_f13, p_f14, p_f15, p_f16, p_f17, p_f18;
00651 int p_f19, p_f20, p_f21, p_f22, p_f23, p_f24, p_f25, p_f26;
00652
00653 int p_final;
00654
00655
00656 double f1[17], f2[17], f3[17], f4[17], f5[17], f6[17], f7[17], f8[17], f9[17];
00657 double f10[17], f11[17], f12[17], f13[17], f14[17], f15[17], f16[17], f17[17], f18[17];
00658 double f19[17], f20[17], f21[17], f22[17], f23[17], f24[17], f25[17], f26[17];
00659
00660
00661
00662 for(i=0;i<3;i++)
00663 {
00664
00665 A0 = cos_alpha[i]*cos_xi[i]*cos_eta[i] - cos_theta[i];
00666
00667 A1 = -sin_alpha[i]*cos_xi[i]*sin_eta[i];
00668
00669 A2 = sin_alpha[i]*sin_xi[i]*cos_eta[i];
00670
00671 A3 = sin_xi[i]*sin_eta[i];
00672
00673 A4 = A3*cos_alpha[i];
00674
00675 j = i;
00676
00677 A21 = A2*cos_delta[j];
00678
00679 A22 = A2*sin_delta[j];
00680
00681 A31 = A3*cos_delta[j];
00682
00683 A32 = A3*sin_delta[j];
00684
00685 A41 = A4*cos_delta[j];
00686
00687 A42 = A4*sin_delta[j];
00688
00689 B0[i] = A0 + A22 + A31;
00690
00691 B1[i] = 2.0e0*(A1 + A42);
00692
00693 B2[i] = 2.0e0*(A32 - A21);
00694
00695 B3[i] = -4.0e0*A41;
00696
00697 B4[i] = A0 + A22 - A31;
00698
00699 B5[i] = A0 - A22 - A31;
00700
00701 B6[i] = -2.0e0*(A21 + A32);
00702
00703 B7[i] = 2.0e0*(A1 - A42);
00704
00705 B8[i] = A0 - A22 + A31;
00706
00707 }
00708
00709
00710
00711 i = 0;
00712
00713 C0[i][0] = B0[i];
00714 C0[i][1] = B2[i];
00715 C0[i][2] = B5[i];
00716
00717 C1[i][0] = B1[i];
00718 C1[i][1] = B3[i];
00719 C1[i][2] = B7[i];
00720
00721 C2[i][0] = B4[i];
00722 C2[i][1] = B6[i];
00723 C2[i][2] = B8[i];
00724
00725 for(i=1;i<3;i++)
00726 {
00727
00728 C0[i][0] = B0[i];
00729 C0[i][1] = B1[i];
00730 C0[i][2] = B4[i];
00731
00732 C1[i][0] = B2[i];
00733 C1[i][1] = B3[i];
00734 C1[i][2] = B6[i];
00735
00736 C2[i][0] = B5[i];
00737 C2[i][1] = B7[i];
00738 C2[i][2] = B8[i];
00739
00740 }
00741
00742
00743
00744 for(i=0;i<3;i++)
00745 {
00746
00747 u11[0][i] = C0[0][i];
00748
00749 u12[0][i] = C1[0][i];
00750
00751 u13[0][i] = C2[0][i];
00752
00753 u31[i][0] = C0[1][i];
00754
00755 u32[i][0] = C1[1][i];
00756
00757 u33[i][0] = C2[1][i];
00758
00759 }
00760
00761
00762 p1[0] = 2;
00763 p1[1] = 0;
00764
00765 p3[0] = 0;
00766 p3[1] = 2;
00767
00768
00769 poly_mul_sub2(u32, u32, u31, u33, p3, p3, p3, p3, um1, p_um1);
00770
00771 poly_mul_sub2(u12, u32, u11, u33, p1, p3, p1, p3, um2, p_um2);
00772
00773 poly_mul_sub2(u12, u33, u13, u32, p1, p3, p1, p3, um3, p_um3);
00774
00775 poly_mul_sub2(u11, u33, u31, u13, p1, p3, p3, p1, um4, p_um4);
00776
00777 poly_mul_sub2(u13, um1, u33, um2, p1, p_um1, p3, p_um2, um5, p_um5);
00778
00779 poly_mul_sub2(u13, um4, u12, um3, p1, p_um4, p1, p_um3, um6, p_um6);
00780
00781 poly_mul_sub2(u11, um5, u31, um6, p1, p_um5, p3, p_um6, q_tmp, p_Q);
00782
00783
00784 for(i=0;i<5;i++)
00785 for(j=0;j<5;j++)
00786 Q[i][j] = q_tmp[i][j];
00787
00788
00789
00790 for(i=0;i<3;i++)
00791 for(j=0;j<17;j++)
00792 R[i][j] = 0.;
00793
00794
00795
00796 for(i=0;i<3;i++)
00797 {
00798 R[0][i] = C0[2][i];
00799 R[1][i] = C1[2][i];
00800 R[2][i] = C2[2][i];
00801 }
00802
00803 p2 = 2;
00804
00805 p4 = 4;
00806
00807
00808 poly_mul_sub1(R[1], R[1], R[0], R[2], p2, p2, p2, p2, f1, &p_f1);
00809
00810 poly_mul1(R[1], R[2], p2, p2, f2, &p_f2);
00811
00812 poly_mul_sub1(R[1], f1, R[0], f2, p2, p_f1, p2, p_f2, f3, &p_f3);
00813
00814 poly_mul1(R[2], f1, p2, p_f1, f4, &p_f4);
00815
00816 poly_mul_sub1(R[1], f3, R[0], f4, p2, p_f3, p2, p_f4, f5, &p_f5);
00817
00818
00819 poly_mul_sub1(Q[1], R[1], Q[0], R[2], p4, p2, p4, p2, f6, &p_f6);
00820
00821 poly_mul_sub1(Q[2], f1, R[2], f6, p4, p_f1, p2, p_f6, f7, &p_f7);
00822
00823 poly_mul_sub1(Q[3], f3, R[2], f7, p4, p_f3, p2, p_f7, f8, &p_f8);
00824
00825 poly_mul_sub1(Q[4], f5, R[2], f8, p4, p_f5, p2, p_f8, f9, &p_f9);
00826
00827
00828 poly_mul_sub1(Q[3], R[1], Q[4], R[0], p4, p2, p4, p2, f10, &p_f10);
00829
00830 poly_mul_sub1(Q[2], f1, R[0], f10, p4, p_f1, p2, p_f10, f11, &p_f11);
00831
00832 poly_mul_sub1(Q[1], f3, R[0], f11, p4, p_f3, p2, p_f11, f12, &p_f12);
00833
00834
00835 poly_mul_sub1(Q[2], R[1], Q[1], R[2], p4, p2, p4, p2, f13, &p_f13);
00836
00837 poly_mul_sub1(Q[3], f1, R[2], f13, p4, p_f1, p2, p_f13, f14, &p_f14);
00838
00839 poly_mul_sub1(Q[3], R[1], Q[2], R[2], p4, p2, p4, p2, f15, &p_f15);
00840
00841 poly_mul_sub1(Q[4], f1, R[2], f15, p4, p_f1, p2, p_f15, f16, &p_f16);
00842
00843 poly_mul_sub1(Q[1], f14, Q[0], f16, p4, p_f14, p4, p_f16, f17, &p_f17);
00844
00845
00846 poly_mul_sub1(Q[2], R[2], Q[3], R[1], p4, p2, p4, p2, f18, &p_f18);
00847
00848 poly_mul_sub1(Q[1], R[2], Q[3], R[0], p4, p2, p4, p2, f19, &p_f19);
00849
00850 poly_mul_sub1(Q[3], f19, Q[2], f18, p4, p_f19, p4, p_f18, f20, &p_f20);
00851
00852 poly_mul_sub1(Q[1], R[1], Q[2], R[0], p4, p2, p4, p2, f21, &p_f21);
00853
00854 poly_mul1(Q[4], f21, p4, p_f21, f22, &p_f22);
00855
00856 poly_sub1(f20, f22, p_f20, p_f22, f23, &p_f23);
00857
00858 poly_mul1(R[0], f23, p2, p_f23, f24, &p_f24);
00859
00860 poly_sub1(f17, f24, p_f17, p_f24, f25, &p_f25);
00861
00862 poly_mul_sub1(Q[4], f12, R[2], f25, p4, p_f12, p2, p_f25, f26, &p_f26);
00863
00864 poly_mul_sub1(Q[0], f9, R[0], f26, p4, p_f9, p2, p_f26, poly_coeff, &p_final);
00865
00866
00867
00868
00869
00870 if (p_final != 16)
00871 {
00872 printf("Error. Degree of polynomial is not 16!\n");
00873 exit(1);
00874 }
00875
00876
00877
00878
00879 if (poly_coeff[16] < 0.0e0)
00880 for(i=0;i<17;i++)
00881 poly_coeff[i] *= -1.0;
00882
00883
00884
00885
00886
00887
00888
00889
00890
00891
00892
00893
00894
00895
00896
00897 return;
00898
00899 }
00901
00902 void poly_mul_sub2(double u1[5][5], double u2[5][5], double u3[5][5], double u4[5][5], int p1[2], int p2[2], int p3[2], int p4[2], double u5[5][5], int p5[2])
00903 {
00904
00905
00906
00907
00908
00909
00910 double d1[5][5], d2[5][5];
00911
00912 int pd1[2], pd2[2];
00913
00914
00915 poly_mul2(u1, u2, p1, p2, d1, pd1);
00916
00917 poly_mul2(u3, u4, p3, p4, d2, pd2);
00918
00919 poly_sub2(d1, d2, pd1, pd2, u5, p5);
00920
00921
00922 }
00924
00925 void poly_mul2(double u1[5][5], double u2[5][5], int p1[2], int p2[2], double u3[5][5], int p3[2])
00926 {
00927
00928
00929
00930
00931
00932
00933 int i1, j1, i2, j2, i3, j3, p11, p12, p21, p22;
00934 int i, j;
00935
00936 double u1ij;
00937
00938
00939 for(i=0;i<2;i++)
00940 p3[i] = p1[i] + p2[i];
00941 for(i=0;i<5;i++)
00942 for(j=0;j<5;j++)
00943 u3[i][j] = 0.0e0;
00944
00945
00946 p11 = p1[0];
00947
00948 p12 = p1[1];
00949
00950 p21 = p2[0];
00951
00952 p22 = p2[1];
00953
00954
00955 for(i1=0;i1<=p12;i1++)
00956 {
00957
00958 for(j1=0;j1<=p11;j1++)
00959 {
00960
00961 u1ij = u1[i1][j1];
00962
00963 for(i2=0;i2<=p22;i2++)
00964 {
00965
00966 i3 = i1 + i2;
00967
00968 for (j2=0;j2<=p21;j2++)
00969 {
00970
00971 j3 = j1 + j2;
00972
00973 u3[i3][j3] = u3[i3][j3] + u1ij*u2[i2][j2];
00974
00975 }
00976
00977 }
00978
00979 }
00980
00981 }
00982
00983
00984 }
00986
00987 void poly_sub2(double u1[5][5], double u2[5][5], int p1[2], int p2[2], double u3[5][5], int p3[2])
00988 {
00989
00990
00991
00992
00993
00994
00995 int i, j, p11, p12, p21, p22;
00996
00997 int i1_ok, i2_ok;
00998
00999
01000 p11 = p1[0];
01001
01002 p12 = p1[1];
01003
01004 p21 = p2[0];
01005
01006 p22 = p2[1];
01007
01008 p3[0] = max(p11,p21);
01009
01010 p3[1] = max(p12,p22);
01011
01012
01013 for(i=0;i<=p3[1];i++)
01014 {
01015
01016 i1_ok = (i > p12);
01017
01018 i2_ok = (i > p22);
01019
01020 for(j=0;j<=p3[0];j++)
01021 {
01022
01023
01024 if (i2_ok || (j > p21))
01025 u3[i][j] = u1[i][j];
01026
01027
01028 else if (i1_ok || (j > p11))
01029 u3[i][j] = -u2[i][j];
01030
01031
01032 else
01033 u3[i][j] = u1[i][j] - u2[i][j];
01034
01035
01036 }
01037
01038 }
01039
01040 return;
01041
01042 }
01044
01045 void poly_mul_sub1(double u1[17], double u2[17], double u3[17], double u4[17], int p1, int p2, int p3, int p4, double u5[17], int *p5)
01046 {
01047
01048
01049
01050
01051
01052
01053 double d1[17], d2[17];
01054
01055 int pd1, pd2;
01056
01057
01058 poly_mul1(u1, u2, p1, p2, d1, &pd1);
01059
01060 poly_mul1(u3, u4, p3, p4, d2, &pd2);
01061
01062 poly_sub1(d1, d2, pd1, pd2, u5, p5);
01063
01064 return;
01065
01066 }
01068
01069 void poly_mul1(double u1[17], double u2[17], int p1, int p2, double u3[17], int *p3)
01070 {
01071
01072
01073
01074
01075
01076
01077 int i, i1, i2, i3;
01078
01079 double u1i;
01080
01081
01082 *p3 = p1 + p2;
01083
01084 for(i=0;i<17;i++)
01085 u3[i] = 0.;
01086
01087
01088 for(i1=0;i1<=p1;i1++)
01089 {
01090
01091 u1i = u1[i1];
01092
01093 for(i2=0;i2<=p2;i2++)
01094 {
01095
01096 i3 = i1 + i2;
01097
01098 u3[i3] = u3[i3] + u1i*u2[i2];
01099
01100 }
01101
01102 }
01103
01104 return;
01105
01106 }
01108
01109 void poly_sub1(double u1[17], double u2[17], int p1, int p2, double u3[17], int *p3)
01110 {
01111
01112
01113
01114
01115
01116
01117 int i;
01118
01119
01120 *p3 = max(p1, p2);
01121
01122
01123 for(i=0;i<=*p3;i++)
01124 {
01125
01126
01127 if (i > p2)
01128 u3[i] = u1[i];
01129
01130
01131 else if (i > p1)
01132 u3[i] = -u2[i];
01133
01134
01135 else
01136 u3[i] = u1[i] - u2[i];
01137
01138
01139 }
01140
01141 return;
01142
01143 }
01145
01146 void coord_from_poly_roots(int *n_soln, double roots[16], double r_n1[3], double r_a1[3], double r_a3[3], double r_c3[3], double r_soln_n[16][3][3], double r_soln_a[16][3][3], double r_soln_c[16][3][3])
01147 {
01148
01149
01150
01151
01152
01153 double ex[3], ey[3], ez[3], b_a1a2[3], b_a3a2[3], r_tmp[3];
01154
01155 double p_s[3][3], s1[3][3], s2[3][3], p_t[3][3], t1[3][3], t2[3][3];
01156
01157 double p_s_c[3][3], s1_s[3][3], s2_s[3][3], p_t_c[3][3], t1_s[3][3], t2_s[3][3];
01158
01159 double angle, sig1_init, half_tan[3];
01160
01161 double cos_tau[4], sin_tau[4], cos_sig[3], sin_sig[3], ht, tmp, sig1;
01162
01163 double r_s[3], r_t[3], r0[3], r_n[3][3], r_a[3][3], r_c[3][3], p[4], Us[3][3];
01164
01165 int i_soln, i, j;
01166
01167 double a1c1, c1n2, n2a2, a2c2, c2n3, n3a3, a1a2, a2a3;
01168
01169 double rr_a1c1[3], rr_c1n2[3], rr_n2a2[3], rr_a2c2[3], rr_c2n3[3], rr_n3a3[3], rr_a1a2[3], rr_a2a3[3];
01170
01171 double a3a1a2, a2a3a1, n1a1c1, n2a2c2, n3a3c3, a1c1n2a2, a2c2n3a3;
01172 double tmp_value, ex_tmp[3];
01173 double tmp_array[3], tmp_array1[3], tmp_array2[3], tmp_array3[3];
01174 double mat1[3], mat2[3], mat3[3], mat4[3], mat5[3];
01175 double mat11[3], mat22[3], mat33[3], mat44[3], mat55[3];
01176
01177
01178 if (*n_soln == 0)
01179 return;
01180
01181
01182
01183 for(i=0;i<3;i++)
01184 ex[i] = b_a1a3[i];
01185
01186 cross(r_a1n1, ex, ez);
01187
01188 tmp_value = sqrt(dot_product(ez,ez));
01189 for(i=0;i<3;i++)
01190 ez[i] = ez[i]/tmp_value;
01191
01192 cross(ez, ex, ey);
01193
01194
01195
01196 for(i=0;i<3;i++)
01197 {
01198 b_a1a2[i] = -cos_alpha[0]*ex[i] + sin_alpha[0]*ey[i];
01199 b_a3a2[i] = cos_alpha[2]*ex[i] + sin_alpha[2]*ey[i];
01200 }
01201
01202
01203
01204
01205
01206
01207
01208
01209
01210 for(i=0;i<3;i++)
01211 {
01212 p_s[0][i] = -ex[i];
01213 s1[0][i] = ez[i];
01214 s2[0][i] = ey[i];
01215 p_t[0][i] = b_a1a2[i];
01216 t1[0][i] = ez[i];
01217 t2[0][i] = sin_alpha[0]*ex[i] + cos_alpha[0]*ey[i];
01218 }
01219
01220
01221
01222
01223
01224
01225
01226 for(i=0;i<3;i++)
01227 {
01228 p_s[1][i] = -b_a1a2[i];
01229 s1[1][i] = -ez[i];
01230 s2[1][i] = t2[0][i];
01231 p_t[1][i] = -b_a3a2[i];
01232 t1[1][i] = -ez[i];
01233 t2[1][i] = sin_alpha[2]*ex[i] - cos_alpha[2]*ey[i];
01234 }
01235
01236
01237
01238
01239
01240
01241
01242 for(i=0;i<3;i++)
01243 {
01244 p_s[2][i] = b_a3a2[i];
01245 s2[2][i] = t2[1][i];
01246 s1[2][i] = ez[i];
01247 p_t[2][i] = ex[i];
01248 t1[2][i] = ez[i];
01249 t2[2][i] = -ey[i];
01250 }
01251
01252
01253
01254
01255
01256
01257
01258
01259
01260 for(i=0;i<3;i++)
01261 for(j=0;j<3;j++)
01262 {
01263 p_s_c[i][j] = p_s[i][j]*cos_xi[i];
01264 s1_s[i][j] = s1[i][j]*sin_xi[i];
01265 s2_s[i][j] = s2[i][j]*sin_xi[i];
01266 p_t_c[i][j] = p_t[i][j]*cos_eta[i];
01267 t1_s[i][j] = t1[i][j]*sin_eta[i];
01268 t2_s[i][j] = t2[i][j]*sin_eta[i];
01269 }
01270
01271
01272
01273 for(i=0;i<3;i++)
01274 r_tmp[i] = (r_a1n1[i]/len_na[0] - p_s_c[0][i])/sin_xi[0];
01275
01276 calc_bnd_ang(s1[0], r_tmp, &angle);
01277
01278 sig1_init = sign(angle, dot_product(r_tmp,s2[0]));
01279
01280
01281
01282
01283
01284
01285 for(i=0;i<3;i++)
01286 {
01287 r_a[0][i] = r_a1[i];
01288 r_a[1][i] = r_a1[i] + len_aa[1]*b_a1a2[i];
01289 r_a[2][i] = r_a3[i];
01290 r0[i] = r_a1[i];
01291 }
01292
01293
01294 for(i_soln=0;i_soln<*n_soln;i_soln++)
01295 {
01296
01297 half_tan[2] = roots[i_soln];
01298
01299 half_tan[1] = calc_t2(half_tan[2]);
01300
01301 half_tan[0] = calc_t1(half_tan[2], half_tan[1]);
01302
01303
01304
01305
01306
01307
01308 for(i=1;i<=3;i++)
01309 {
01310 ht = half_tan[i-1];
01311 tmp = 1.0e0 + ht*ht;
01312 cos_tau[i] = (1.0e0 - ht*ht)/tmp;
01313 sin_tau[i] = 2.0e0*ht/tmp;
01314 }
01315
01316 cos_tau[0] = cos_tau[3];
01317
01318 sin_tau[0] = sin_tau[3];
01319
01320
01321
01322
01323
01324
01325
01326 for(i=0;i<3;i++)
01327 {
01328 cos_sig[i] = cos_delta[i]*cos_tau[i] + sin_delta[i]*sin_tau[i];
01329 sin_sig[i] = sin_delta[i]*cos_tau[i] - cos_delta[i]*sin_tau[i];
01330 }
01331
01332
01333
01334
01335
01336
01337 for(i=0;i<3;i++)
01338 for(j=0;j<3;j++)
01339 {
01340 r_s[j] = p_s_c[i][j] + cos_sig[i]*s1_s[i][j] + sin_sig[i]*s2_s[i][j];
01341 r_t[j] = p_t_c[i][j] + cos_tau[i+1]*t1_s[i][j] + sin_tau[i+1]*t2_s[i][j];
01342 r_n[i][j] = r_s[j]*len_na[i] + r_a[i][j];
01343 r_c[i][j] = r_t[j]*len_ac[i] + r_a[i][j];
01344 }
01345
01346
01347
01348 sig1 = atan2(sin_sig[0], cos_sig[0]);
01349
01350 ex_tmp[0] = -ex[0];
01351 ex_tmp[1] = -ex[1];
01352 ex_tmp[2] = -ex[2];
01353 tmp_value = -(sig1-sig1_init)*0.25;
01354 quaternion(ex_tmp, tmp_value, p);
01355
01356 rotation_matrix(p, Us);
01357
01358
01359
01360
01361
01362
01363
01364
01365
01366
01367 for(i=0;i<3;i++)
01368 {
01369 mat11[i] = r_c[0][i]-r0[i];
01370 mat22[i] = r_n[1][i]-r0[i];
01371 mat33[i] = r_a[1][i]-r0[i];
01372 mat44[i] = r_c[1][i]-r0[i];
01373 mat55[i] = r_n[2][i]-r0[i];
01374 }
01375 matmul(Us,mat11,mat1);
01376 matmul(Us,mat22,mat2);
01377 matmul(Us,mat33,mat3);
01378 matmul(Us,mat44,mat4);
01379 matmul(Us,mat55,mat5);
01380 for(i=0;i<3;i++)
01381 {
01382 r_soln_n[i_soln][0][i] = r_n1[i];
01383 r_soln_a[i_soln][0][i] = r_a1[i];
01384 r_soln_c[i_soln][0][i] = mat1[i] + r0[i];
01385 r_soln_n[i_soln][1][i] = mat2[i] + r0[i];
01386 r_soln_a[i_soln][1][i] = mat3[i] + r0[i];
01387 r_soln_c[i_soln][1][i] = mat4[i] + r0[i];
01388 r_soln_n[i_soln][2][i] = mat5[i] + r0[i];
01389 r_soln_a[i_soln][2][i] = r_a3[i];
01390 r_soln_c[i_soln][2][i] = r_c3[i];
01391 }
01392
01393
01394
01395 if (print_level > 0)
01396 {
01397
01398
01399
01400
01401
01402
01403
01404
01405
01406
01407
01408
01409
01410 for(i=0;i<3;i++)
01411 {
01412 rr_a1c1[i] = r_soln_c[i_soln][0][i] - r_soln_a[i_soln][0][i];
01413 rr_c1n2[i] = r_soln_n[i_soln][1][i] - r_soln_c[i_soln][0][i];
01414 rr_n2a2[i] = r_soln_a[i_soln][1][i] - r_soln_n[i_soln][1][i];
01415 rr_a2c2[i] = r_soln_c[i_soln][1][i] - r_soln_a[i_soln][1][i];
01416 rr_c2n3[i] = r_soln_n[i_soln][2][i] - r_soln_c[i_soln][1][i];
01417 rr_n3a3[i] = r_soln_a[i_soln][2][i] - r_soln_n[i_soln][2][i];
01418 rr_a1a2[i] = r_soln_a[i_soln][1][i] - r_soln_a[i_soln][0][i];
01419 rr_a2a3[i] = r_soln_a[i_soln][2][i] - r_soln_a[i_soln][1][i];
01420 }
01421
01422
01423
01424
01425
01426
01427
01428
01429
01430 a1c1 = sqrt(dot_product(rr_a1c1, rr_a1c1));
01431 c1n2 = sqrt(dot_product(rr_c1n2, rr_c1n2));
01432 n2a2 = sqrt(dot_product(rr_n2a2, rr_n2a2));
01433 a2c2 = sqrt(dot_product(rr_a2c2, rr_a2c2));
01434 c2n3 = sqrt(dot_product(rr_c2n3, rr_c2n3));
01435 n3a3 = sqrt(dot_product(rr_n3a3, rr_n3a3));
01436 a1a2 = sqrt(dot_product(rr_a1a2, rr_a1a2));
01437 a2a3 = sqrt(dot_product(rr_a2a3, rr_a2a3));
01438
01439
01440
01441
01442
01443
01444
01445
01446
01447
01448
01449
01450 for(i=0;i<3;i++)
01451 tmp_array[i] = rr_a1a2[i]/a1a2;
01452 calc_bnd_ang(b_a1a3, tmp_array, &a3a1a2);
01453
01454 for(i=0;i<3;i++)
01455 tmp_array[i] = rr_a2a3[i]/a2a3;
01456 calc_bnd_ang(tmp_array, b_a1a3, &a2a3a1);
01457
01458
01459
01460 for(i=0;i<3;i++)
01461 tmp_array[i] = rr_a1c1[i]/a1c1;
01462 calc_bnd_ang(b_a1n1, tmp_array, &n1a1c1);
01463
01464 for(i=0;i<3;i++)
01465 tmp_array[i] = -rr_n3a3[i]/n3a3;
01466 calc_bnd_ang(b_a3c3, tmp_array, &n3a3c3);
01467
01468 for(i=0;i<3;i++)
01469 {
01470 tmp_array1[i] = rr_a2c2[i]/a2c2;
01471 tmp_array2[i] = -rr_n2a2[i]/n2a2;
01472 }
01473 calc_bnd_ang(tmp_array1, tmp_array2, &n2a2c2);
01474
01475
01476
01477
01478
01479
01480
01481
01482
01483 for(i=0;i<3;i++)
01484 {
01485 tmp_array1[i] = rr_a1c1[i]/a1c1;
01486 tmp_array2[i] = rr_c1n2[i]/c1n2;
01487 tmp_array3[i] = rr_n2a2[i]/n2a2;
01488 }
01489 calc_dih_ang(tmp_array1, tmp_array2, tmp_array3, &a1c1n2a2);
01490
01491 for(i=0;i<3;i++)
01492 {
01493 tmp_array1[i] = rr_a2c2[i]/a2c2;
01494 tmp_array2[i] = rr_c2n3[i]/c2n3;
01495 tmp_array3[i] = rr_n3a3[i]/n3a3;
01496 }
01497 calc_dih_ang(tmp_array1, tmp_array2, tmp_array3, &a2c2n3a3);
01498
01499
01500
01501
01502
01503
01504
01505 }
01506
01507
01508 }
01509
01510 return;
01511
01512 }
01514
01515 double calc_t2(double t0)
01516 {
01517
01518
01519
01520 double tmp_value;
01521
01522 double B0, B1, B2, A0, A1, A2, A3, A4, B2_2, B2_3;
01523
01524 double K0, K1, K2, K3, t0_2, t0_3, t0_4;
01525
01526
01527 t0_2 = t0*t0;
01528
01529 t0_3 = t0_2*t0;
01530
01531 t0_4 = t0_3*t0;
01532
01533
01534 A0 = Q[0][0] + Q[0][1]*t0 + Q[0][2]*t0_2 + Q[0][3]*t0_3 + Q[0][4]*t0_4;
01535
01536 A1 = Q[1][0] + Q[1][1]*t0 + Q[1][2]*t0_2 + Q[1][3]*t0_3 + Q[1][4]*t0_4;
01537
01538 A2 = Q[2][0] + Q[2][1]*t0 + Q[2][2]*t0_2 + Q[2][3]*t0_3 + Q[2][4]*t0_4;
01539
01540 A3 = Q[3][0] + Q[3][1]*t0 + Q[3][2]*t0_2 + Q[3][3]*t0_3 + Q[3][4]*t0_4;
01541
01542 A4 = Q[4][0] + Q[4][1]*t0 + Q[4][2]*t0_2 + Q[4][3]*t0_3 + Q[4][4]*t0_4;
01543
01544
01545 B0 = R[0][0] + R[0][1]*t0 + R[0][2]*t0_2;
01546
01547 B1 = R[1][0] + R[1][1]*t0 + R[1][2]*t0_2;
01548
01549 B2 = R[2][0] + R[2][1]*t0 + R[2][2]*t0_2;
01550
01551
01552 B2_2 = B2*B2;
01553
01554 B2_3 = B2_2*B2;
01555
01556
01557 K0 = A2*B2 - A4*B0;
01558
01559 K1 = A3*B2 - A4*B1;
01560
01561 K2 = A1*B2_2 - K1*B0;
01562
01563 K3 = K0*B2 - K1*B1;
01564
01565
01566 tmp_value = (K3*B0 - A0*B2_3)/(K2*B2 - K3*B1);
01567
01568 return tmp_value;
01569
01570 }
01572
01573 double calc_t1(double t0, double t2)
01574 {
01575
01576
01577
01578 double tmp_value;
01579
01580 double U11, U12, U13, U31, U32, U33;
01581
01582 double t0_2, t2_2;
01583
01584
01585 t0_2 = t0*t0;
01586
01587 t2_2 = t2*t2;
01588
01589
01590 U11 = C0[0][0] + C0[0][1]*t0 + C0[0][2]*t0_2;
01591
01592 U12 = C1[0][0] + C1[0][1]*t0 + C1[0][2]*t0_2;
01593
01594 U13 = C2[0][0] + C2[0][1]*t0 + C2[0][2]*t0_2;
01595
01596 U31 = C0[1][0] + C0[1][1]*t2 + C0[1][2]*t2_2;
01597
01598 U32 = C1[1][0] + C1[1][1]*t2 + C1[1][2]*t2_2;
01599
01600 U33 = C2[1][0] + C2[1][1]*t2 + C2[1][2]*t2_2;
01601
01602
01603 tmp_value = (U31*U13-U11*U33)/(U12*U33-U13*U32);
01604
01605 return tmp_value;
01606
01607 }
01609
01610 void calc_dih_ang(double r1[3], double r2[3], double r3[3], double *angle)
01611 {
01615
01616
01617
01618
01619 double p[3], q[3], s[3];
01620
01621 double arg;
01622
01623
01624 cross(r1, r2, p);
01625
01626 cross(r2, r3, q);
01627
01628 cross(r3, r1, s);
01629
01630 arg = dot_product(p,q)/sqrt(dot_product(p,p)*dot_product(q,q));
01631
01632 arg = sign(min(fabs(arg),1.0e0),arg);
01633
01634 *angle = sign(acos(arg), dot_product(s,r2));
01635
01636 return;
01637
01638 }
01640
01641 void calc_bnd_ang(double r1[3], double r2[3], double *angle)
01642 {
01647
01648
01649
01650
01651 double arg;
01652
01653
01654 arg = dot_product(r1, r2);
01655
01656 arg = sign(min(fabs(arg),1.0e0),arg);
01657
01658 *angle = acos(arg);
01659
01660 return;
01661
01662 }
01664
01665 void cross(double p[3], double q[3], double s[3])
01666 {
01668
01669
01670
01671
01672
01673 s[0] = p[1]*q[2] - p[2]*q[1];
01674
01675 s[1] = p[2]*q[0] - p[0]*q[2];
01676
01677 s[2] = p[0]*q[1] - p[1]*q[0];
01678
01679 return;
01680
01681 }
01683
01684 void quaternion(double axis[3], double quarter_ang, double p[4])
01685 {
01689
01690
01691
01692
01693
01694 double tan_w, tan_sqr, tan1, cosine, sine;
01695
01696
01697 tan_w = tan(quarter_ang);
01698
01699 tan_sqr = tan_w * tan_w;
01700
01701 tan1 = 1.0e0 + tan_sqr;
01702
01703 cosine = (1.0e0 - tan_sqr)/tan1;
01704
01705 sine = 2.0e0*tan_w/tan1;
01706
01707 p[0] = cosine;
01708
01709 p[1] = axis[0] * sine;
01710 p[2] = axis[1] * sine;
01711 p[3] = axis[2] * sine;
01712
01713 return;
01714
01715 }
01717
01718 void rotation_matrix(double q[4], double U[3][3])
01719 {
01723
01724
01725
01726
01727 double q0,q1,q2,q3,b0,b1,b2,b3,q00,q01,q02,q03,q11,q12,q13,q22,q23,q33;
01728
01729
01730 q0 = q[0];
01731 q1 = q[1];
01732 q2 = q[2];
01733 q3 = q[3];
01734
01735 b0 = 2.0e0*q0;
01736 b1 = 2.0e0*q1;
01737
01738 q00 = b0*q0-1.0e0;
01739 q02 = b0*q2;
01740 q03 = b0*q3;
01741
01742 q11 = b1*q1;
01743 q12 = b1*q2;
01744 q13 = b1*q3;
01745
01746 b2 = 2.0e0*q2;
01747 b3 = 2.0e0*q3;
01748
01749 q01 = b0*q1;
01750 q22 = b2*q2;
01751 q23 = b2*q3;
01752 q33 = b3*q3;
01753
01754 U[0][0] = q00+q11;
01755 U[0][1] = q12-q03;
01756 U[0][2] = q13+q02;
01757
01758 U[1][0] = q12+q03;
01759 U[1][1] = q00+q22;
01760 U[1][2] = q23-q01;
01761
01762 U[2][0] = q13-q02;
01763 U[2][1] = q23+q01;
01764 U[2][2] = q00+q33;
01765
01766 return;
01767
01768 }
01770