| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | #include "na64util/numerical/biplaneIntersection.h" | ||
| 2 | |||
| 3 | #include <gsl/gsl_linalg.h> | ||
| 4 | #include <gsl/gsl_matrix_float.h> | ||
| 5 | #include <assert.h> | ||
| 6 | |||
| 7 | int | ||
| 8 | 7 | na64sw_projected_lines_intersection( const na64sw_Float_t * k1, const na64sw_Float_t * k2, const na64sw_Float_t * k3 | |
| 9 | , const na64sw_Float_t * r01, const na64sw_Float_t * r02 | ||
| 10 | , na64sw_Float_t * x1, na64sw_Float_t * x2 | ||
| 11 | ) { | ||
| 12 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 7 times.
|
7 | assert(k1); |
| 13 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 7 times.
|
7 | assert(k2); |
| 14 | 7 | double m_[9] = { k1[0], k2[0], 0 | |
| 15 | 7 | , k1[1], k2[1], 0 | |
| 16 | 7 | , k1[2], k2[2], 0 }; | |
| 17 |
2/2✓ Branch 0 taken 5 times.
✓ Branch 1 taken 2 times.
|
7 | if(!k3) { |
| 18 | 5 | m_[2] = k1[1]*k2[2] - k1[2]*k2[1]; // a_y*b_z - a_z*b_y | |
| 19 | 5 | m_[5] = k1[2]*k2[0] - k1[0]*k2[2]; // a_z*b_x - a_x*b_z | |
| 20 | 5 | m_[8] = k1[0]*k2[1] - k1[1]*k2[0]; // a_x*b_y - a_y*b_x | |
| 21 | } else { | ||
| 22 | 2 | m_[2] = k3[0]; | |
| 23 | 2 | m_[5] = k3[1]; | |
| 24 | 2 | m_[8] = k3[2]; | |
| 25 | } | ||
| 26 | 7 | gsl_matrix_view m = gsl_matrix_view_array(m_, 3, 3); | |
| 27 | #if 1 /* SVD-based solution; may be less efficient, but | ||
| 28 | detects singular matrices */ | ||
| 29 | 7 | gsl_vector * w = gsl_vector_alloc(3); | |
| 30 | 7 | gsl_matrix * V = gsl_matrix_alloc(3, 3); | |
| 31 | 7 | gsl_vector * S = gsl_vector_alloc(3); | |
| 32 | 7 | gsl_linalg_SV_decomp( &m.matrix, V, S, w); | |
| 33 | |||
| 34 | /* test if matrix is singular */ | ||
| 35 | 7 | double sMax = gsl_vector_get(S, 0) | |
| 36 | 7 | , sMin = gsl_vector_get(S, 0); | |
| 37 |
2/2✓ Branch 0 taken 14 times.
✓ Branch 1 taken 7 times.
|
21 | for(int i = 0; i < 2; ++i) { |
| 38 | 14 | const double c = gsl_vector_get(S, i); | |
| 39 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 14 times.
|
14 | if(sMax < c) sMax = c; |
| 40 |
2/2✓ Branch 0 taken 5 times.
✓ Branch 1 taken 9 times.
|
14 | else if(sMin > c) sMin = c; |
| 41 | } | ||
| 42 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 6 times.
|
7 | if(sMin/sMax < 1e-12) return 1; |
| 43 | |||
| 44 | 6 | gsl_vector * r = gsl_vector_alloc(3); | |
| 45 | 6 | double cv_[] = { r02[0] - r01[0], r02[1] - r01[1], r02[2] - r01[2] }; | |
| 46 | 6 | gsl_vector_view cv = gsl_vector_view_array(cv_, 3); | |
| 47 | 6 | gsl_linalg_SV_solve( &m.matrix, V, S, &cv.vector, r); | |
| 48 | #else /* LU decomp version */ | ||
| 49 | /* invert matrix */ | ||
| 50 | gsl_permutation * p = gsl_permutation_alloc(3); | ||
| 51 | int s; | ||
| 52 | gsl_linalg_LU_decomp(&m.matrix, p, &s); | ||
| 53 | gsl_matrix * inv = gsl_matrix_alloc(3, 3); | ||
| 54 | gsl_linalg_LU_invert(&m.matrix, p, inv); | ||
| 55 | gsl_permutation_free(p); | ||
| 56 | /* multiply matrix on diff vector to get {u1, -u2, d} */ | ||
| 57 | double cv_[] = { r02[0] - r01[0], r02[1] - r01[1], r02[2] - r01[2] }; | ||
| 58 | gsl_vector_view cv = gsl_vector_view_array(cv_, 3); | ||
| 59 | |||
| 60 | gsl_vector * r = gsl_vector_alloc(3); | ||
| 61 | gsl_vector_set_zero(r); | ||
| 62 | gsl_blas_dgemv( CblasNoTrans, 1. | ||
| 63 | , inv, &cv.vector | ||
| 64 | , 1., r); | ||
| 65 | #endif | ||
| 66 | 6 | const double u1 = gsl_vector_get(r, 0) | |
| 67 | 6 | , u2 = -gsl_vector_get(r, 1) | |
| 68 | //, d = gsl_vector_get(r, 2) // distance is not needed | ||
| 69 | ; | ||
| 70 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 4 times.
|
6 | if(x2) { |
| 71 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 2 times.
|
8 | for(int i = 0; i < 3; ++i) { |
| 72 | 6 | x1[i] = r01[i] + u1*k1[i]; | |
| 73 | 6 | x2[i] = r02[i] + u2*k2[i]; | |
| 74 | } | ||
| 75 | } else { | ||
| 76 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 4 times.
|
16 | for(int i = 0; i < 3; ++i) { |
| 77 | 12 | x1[i] = (r02[i] + u2*k2[i] + r01[i] + u1*k1[i])/2; | |
| 78 | } | ||
| 79 | } | ||
| 80 | /* This produces gnuplot script that can be used to roughly visualize | ||
| 81 | * results in form of parametric planes and arrows. I decided to keep it | ||
| 82 | * in case we should make a pic for docs. */ | ||
| 83 | #if 0 | ||
| 84 | //printf( "# u1 = %e, u2 = %e, d=%e\n", u1, u2, d ); | ||
| 85 | printf( "set arrow 1 from %e,%e,%e to %e,%e,%e\n" | ||
| 86 | , r01[0], r01[1], r01[2] | ||
| 87 | , r01[0] + k1[0], r01[1] + k1[1], r01[2] + k1[2] | ||
| 88 | ); | ||
| 89 | printf( "set arrow 2 from %e,%e,%e to %e,%e,%e\n" | ||
| 90 | , r02[0], r02[1], r02[2] | ||
| 91 | , r02[0] + k2[0], r02[1] + k2[1], r02[2] + k2[2] | ||
| 92 | ); | ||
| 93 | printf( "set arrow 3 from %e,%e,%e to %e,%e,%e\n" | ||
| 94 | , x1[0], x1[1], x1[2] | ||
| 95 | , x2[0], x2[1], x2[2] | ||
| 96 | ); | ||
| 97 | printf( "splot %.3e*u + %.3e*v + %.3e, %.3e*u + %.3e*v + %.3e, %.3e*u + %.3e*v + %.3e," | ||
| 98 | " %.3e*u + %.3e*v + %.3e, %.3e*u + %.3e*v + %.3e, %.3e*u + %.3e*v + %.3e\n" | ||
| 99 | , k1[0], k3[0], r01[0] | ||
| 100 | , k1[1], k3[1], r01[1] | ||
| 101 | , k1[2], k3[2], r01[2] | ||
| 102 | // | ||
| 103 | , k2[0], k3[0], r02[0] | ||
| 104 | , k2[1], k3[1], r02[1] | ||
| 105 | , k2[2], k3[2], r02[2] | ||
| 106 | ); | ||
| 107 | #endif | ||
| 108 | |||
| 109 | 6 | return 0; | |
| 110 | } | ||
| 111 | |||
| 112 | #if 0 | ||
| 113 | int | ||
| 114 | main(int argc, char * argv[]) { | ||
| 115 | float k1[] = { 1, 0, 1} | ||
| 116 | , k2[] = { 0, 1, -1} | ||
| 117 | , k3[] = { 0, 0, -1} | ||
| 118 | ; | ||
| 119 | float r1[] = { .25, 0, -.25} | ||
| 120 | , r2[] = { 0, 1, 1} | ||
| 121 | ; | ||
| 122 | float res[6]; | ||
| 123 | plane_intersection(k1, k2, k3, r1, r2, res, res + 3); | ||
| 124 | |||
| 125 | return 0; | ||
| 126 | } | ||
| 127 | #endif | ||
| 128 | |||
| 129 |