Xmipp  v3.23.11-Nereus
geometry.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  *
3  * Authors: Carlos Oscar S. Sorzano (coss@cnb.csic.es)
4  *
5  * Unidad de Bioinformatica of Centro Nacional de Biotecnologia , CSIC
6  *
7  * This program is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2 of the License, or
10  * (at your option) any later version.
11  *
12  * This program is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with this program; if not, write to the Free Software
19  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
20  * 02111-1307 USA
21  *
22  * All comments concerning this program package may be sent to the
23  * e-mail address 'xmipp@cnb.csic.es'
24  ***************************************************************************/
25 
26 #include <iostream>
27 #include <math.h>
28 
29 #include "geometry.h"
30 #include "xmipp_funcs.h"
31 #include "transformations.h"
32 #include "bilib/kernel.h"
33 
34 /* ######################################################################### */
35 /* Geometrical Operations */
36 /* ######################################################################### */
37 
38 /* Project a point to a plane ---------------------------------------------- */
39 void Uproject_to_plane(const Matrix1D<double> &point,
40  const Matrix1D<double> &direction, double distance,
41  Matrix1D<double> &result)
42 {
43 
44  if (result.size() != 3)
45  result.resize(3);
46  double xx = distance - (XX(point) * XX(direction) + YY(point) * YY(direction) +
47  ZZ(point) * ZZ(direction));
48  XX(result) = XX(point) + xx * XX(direction);
49  YY(result) = YY(point) + xx * YY(direction);
50  ZZ(result) = ZZ(point) + xx * ZZ(direction);
51 }
52 
53 /* Project a point to a plane ---------------------------------------------- */
54 void Uproject_to_plane(const Matrix1D<double> &r,
55  double rot, double tilt, double psi, Matrix1D<double> &result)
56 {
57  Matrix2D<double> euler(3, 3);
58  Euler_angles2matrix(rot, tilt, psi, euler);
59  Uproject_to_plane(r, euler, result);
60 }
61 
62 /* Project a point to a plane ---------------------------------------------- */
63 void Uproject_to_plane(const Matrix1D<double> &r,
64  const Matrix2D<double> &euler, Matrix1D<double> &result)
65 {
66  SPEED_UP_temps012;
67  if (VEC_XSIZE(result) != 3)
68  result.resize(3);
69  M3x3_BY_V3x1(result, euler, r);
70 }
71 
72 /* Spherical distance ------------------------------------------------------ */
73 double spherical_distance(const Matrix1D<double> &r1, const Matrix1D<double> &r2)
74 {
75  double r1r2 = XX(r1) * XX(r2) + YY(r1) * YY(r2) + ZZ(r1) * ZZ(r2);
76  double R1 = sqrt(XX(r1) * XX(r1) + YY(r1) * YY(r1) + ZZ(r1) * ZZ(r1));
77  double R2 = sqrt(XX(r2) * XX(r2) + YY(r2) * YY(r2) + ZZ(r2) * ZZ(r2));
78  double argument = r1r2 / (R1 * R2);
79  double ang = acos(CLIP(argument,-1.,1.));
80  double retVal = ang*R1;
81  return retVal;
82 }
83 
84 /* Point to line distance -------------------------------------------------- */
85 double point_line_distance_3D(const Matrix1D<double> &p,
86  const Matrix1D<double> &a,
87  const Matrix1D<double> &v)
88 
89 {
90  Matrix1D<double> p_a(3);
91 
92  V3_MINUS_V3(p_a, p, a);
93  return (vectorProduct(p_a, v).module() / v.module());
94 }
95 
96 /* Least-squares-fit a plane to an arbitrary number of (x,y,z) points
97  PLane described as Ax + By + C = z
98  where D = -1
99  Returns -1 if A2+B2+C2 <<1
100 */
101 void least_squares_plane_fit(FitPoint *IN_points,
102  int Npoints,
103  double &plane_a,
104  double &plane_b,
105  double &plane_c)
106 {
107  double D = 0;
108  double E = 0;
109  double F = 0;
110  double G = 0;
111  double H = 0;
112  double I = 0;
113  double J = 0;
114  double K = 0;
115  double L = 0;
116  double W2 = 0;
117  double denom = 0;
118  const FitPoint * point;
119 
120  for (int i = 0; i < Npoints; i++)
121  {
122  point = &(IN_points[i]);//Can I copy just the address?
123  W2 = point->w * point->w;
124  D += point->x * point->x * W2 ;
125  E += point->x * point->y * W2 ;
126  F += point->x * W2 ;
127  G += point->y * point->y * W2 ;
128  H += point->y * W2 ;
129  I += 1 * W2 ;
130  J += point->x * point->z * W2 ;
131  K += point->y * point->z * W2 ;
132  L += point->z * W2 ;
133  }
134 
135  denom = F * F * G - 2 * E * F * H + D * H * H + E * E * I - D * G * I;
136 
137  // X axis slope
138  plane_a = (H * H * J - G * I * J + E * I * K + F * G * L - H * (F * K + E * L)) / denom;
139  // Y axis slope
140  plane_b = (E * I * J + F * F * K - D * I * K + D * H * L - F * (H * J + E * L)) / denom;
141  // Z axis intercept
142  plane_c = (F * G * J - E * H * J - E * F * K + D * H * K + E * E * L - D * G * L) / denom;
143 }
144 
145 
146 /* Least-squares-fit a plane to an image.
147  *
148  * Performs the same computation as least_squares_plane_fit function but it has been
149  * optimized removing redundant computations or moving computations to
150  * outer loop.
151  *
152  * Check least_squares_plane_fit function for a clearer code.
153 */
154 void least_squares_plane_fit_All_Points(const MultidimArray<double> &Image,
155  double& plane_a,
156  double& plane_b,
157  double& plane_c)
158 {
159  double D = 0;
160  double E = 0;
161  double F = 0;
162  double G = 0;
163  double H = 0;
164  double I = 0;
165  double J = 0;
166  double K = 0;
167  double L = 0;
168  double denom = 0;
169  int nIterations=0;
170  double sumjValues=0.0;
171  for (int j=STARTINGX(Image); j<=FINISHINGX(Image); ++j)
172  {
173  D += j*j;
174  sumjValues += j;
175  I += 1;
176  }
177  F = sumjValues;
178 
179  double *ref;
180  double sumElements;
181  for (int i=STARTINGY(Image); i<=FINISHINGY(Image); ++i)
182  {
183  nIterations++;
184  ref = &A2D_ELEM(Image, i, STARTINGX(Image));
185  sumElements = 0.0;
186  for (int j=STARTINGX(Image); j<=FINISHINGX(Image); ++j)
187  {
188  J += j*(*ref);
189  sumElements += (*ref);
190  ref++;
191  }
192  K += i*sumElements;
193  L += sumElements;
194  E += sumjValues*i;
195  G += i*i*I;
196  H += i*I;
197  }
198  D = D*nIterations;
199  F = F*nIterations;
200  I = I*nIterations;
201 
202  denom = F * F * G - 2 * E * F * H + D * H * H + E * E * I - D * G * I;
203 
204  // X axis slope
205  plane_a = (H * H * J - G * I * J + E * I * K + F * G * L - H * (F * K + E * L)) / denom;
206  // Y axis slope
207  plane_b = (E * I * J + F * F * K - D * I * K + D * H * L - F * (H * J + E * L)) / denom;
208  // Z axis intercept
209  plane_c = (F * G * J - E * H * J - E * F * K + D * H * K + E * E * L - D * G * L) / denom;
210 }
211 
212 void least_squares_line_fit(const std::vector<fit_point2D> & IN_points,
213  double &line_a,
214  double &line_b)
215 {
216 
217  double sumx = 0.;
218  double sumy = 0.;
219  double sumxy = 0.;
220  double sumxx = 0.;
221  double sumw = 0.;
222  double W2;
223  const fit_point2D * point;
224 
225  int n = IN_points.size();
226 
227  for (int i = 0; i < n; i++)
228  {
229  point = &(IN_points[i]);
230  W2 = point->w * point->w;
231  sumx += point->x * point->w ;
232  sumy += point->y * point->w ;
233  sumxx += point->x * point->x * W2 ;
234  sumxy += point->x * point->y * W2 ;
235  sumw += point->w ;
236  }
237  line_a = (sumx*sumy - sumw*sumxy) / (sumx*sumx - sumw*sumxx) ;
238  line_b = (sumy - line_a*sumx) / sumw ;
239 }
240 
241 /* Bspline fitting --------------------------------------------------------- */
242 /* See http://en.wikipedia.org/wiki/Weighted_least_squares */
243 void Bspline_model_fitting(const std::vector<FitPoint> &IN_points,
244  int SplineDegree, int l0, int lF, int m0, int mF,
245  double h_x, double h_y, double x0, double y0,
246  Bspline_model &result)
247 {
248  // Initialize model
249  result.l0 = l0;
250  result.lF = lF;
251  result.m0 = m0;
252  result.mF = mF;
253  result.x0 = x0;
254  result.y0 = y0;
255  result.SplineDegree = SplineDegree;
256  result.h_x = h_x;
257  result.h_y = h_y;
258  result.c_ml.initZeros(mF - m0 + 1, lF - l0 + 1);
259  STARTINGY(result.c_ml) = m0;
260  STARTINGX(result.c_ml) = l0;
261 
262  // Modify the list of points to include the weight
263  int Npoints = IN_points.size();
264  std::vector<FitPoint> AUX_points = IN_points;
265  for (int i = 0; i < Npoints; ++i)
266  {
267  double sqrt_w = sqrt(AUX_points[i].w);
268  AUX_points[i].x *= sqrt_w;
269  AUX_points[i].y *= sqrt_w;
270  AUX_points[i].z *= sqrt_w;
271  }
272 
273  // Now solve the normal linear regression problem
274  // Ax=B
275  // A=system matrix
276  // x=B-spline coefficients
277  // B=vector of measured values
278  int Ncoeff = YSIZE(result.c_ml) * XSIZE(result.c_ml);
279  Matrix2D<double> A(Npoints, Ncoeff);
280  Matrix1D<double> B(Npoints);
281  for (int i = 0; i < Npoints; ++i)
282  {
283  B(i) = AUX_points[i].z;
284  double xarg = (AUX_points[i].x - x0) / h_x;
285  double yarg = (AUX_points[i].y - y0) / h_y;
286  for (int m = m0; m <= mF; ++m)
287  for (int l = l0; l <= lF; ++l)
288  {
289  double coeff=0.;
290  switch (SplineDegree)
291  {
292  case 2:
293  coeff = Bspline02(xarg - l) * Bspline02(yarg - m);
294  break;
295  case 3:
296  coeff = Bspline03(xarg - l) * Bspline03(yarg - m);
297  break;
298  case 4:
299  coeff = Bspline04(xarg - l) * Bspline04(yarg - m);
300  break;
301  case 5:
302  coeff = Bspline05(xarg - l) * Bspline05(yarg - m);
303  break;
304  case 6:
305  coeff = Bspline06(xarg - l) * Bspline06(yarg - m);
306  break;
307  case 7:
308  coeff = Bspline07(xarg - l) * Bspline07(yarg - m);
309  break;
310  case 8:
311  coeff = Bspline08(xarg - l) * Bspline08(yarg - m);
312  break;
313  case 9:
314  coeff = Bspline09(xarg - l) * Bspline09(yarg - m);
315  break;
316  }
317  A(i, (m - m0)*XSIZE(result.c_ml) + l - l0) = coeff;
318  }
319  }
320 
321  Matrix1D<double> x = (A.transpose() * A).inv() * (A.transpose() * B);
322  for (int m = m0; m <= mF; ++m)
323  for (int l = l0; l <= lF; ++l)
324  result.c_ml(m, l) = x((m - m0) * XSIZE(result.c_ml) + l - l0);
325 }
326 
327 /* Rectangle enclosing ----------------------------------------------------- */
328 void rectangle_enclosing(const Matrix1D<double> &v0, const Matrix1D<double> &vF,
329  const Matrix2D<double> &V, Matrix1D<double> &corner1,
330  Matrix1D<double> &corner2)
331 {
332  SPEED_UP_temps01;
333  Matrix1D<double> v(2);
334  corner1.resize(2);
335  corner2.resize(2);
336 
337  // Store values for reusing input as output vectors
338  double XX_v0 = XX(v0);
339  double YY_v0 = YY(v0);
340  double XX_vF = XX(vF);
341  double YY_vF = YY(vF);
342 
343  VECTOR_R2(v, XX_v0, YY_v0);
344  M2x2_BY_V2x1(v, V, v);
345  XX(corner1) = XX(v);
346  XX(corner2) = XX(v);
347  YY(corner1) = YY(v);
348  YY(corner2) = YY(v);
349 
350 #define DEFORM_AND_CHOOSE_CORNERS2D \
351  M2x2_BY_V2x1(v,V,v); \
352  XX(corner1)=XMIPP_MIN(XX(corner1),XX(v)); \
353  XX(corner2)=XMIPP_MAX(XX(corner2),XX(v)); \
354  YY(corner1)=XMIPP_MIN(YY(corner1),YY(v)); \
355  YY(corner2)=XMIPP_MAX(YY(corner2),YY(v));
356 
357  VECTOR_R2(v, XX_vF, YY_v0);
358  DEFORM_AND_CHOOSE_CORNERS2D;
359  VECTOR_R2(v, XX_v0, YY_vF);
360  DEFORM_AND_CHOOSE_CORNERS2D;
361  VECTOR_R2(v, XX_vF, YY_vF);
362  DEFORM_AND_CHOOSE_CORNERS2D;
363 }
364 
365 /* Rectangle enclosing ----------------------------------------------------- */
366 void box_enclosing(const Matrix1D<double> &v0, const Matrix1D<double> &vF,
367  const Matrix2D<double> &V, Matrix1D<double> &corner1,
368  Matrix1D<double> &corner2)
369 {
370  SPEED_UP_temps012;
371  Matrix1D<double> v(3);
372  corner1.resize(3);
373  corner2.resize(3);
374 
375  // Store values for reusing input as output vectors
376  double XX_v0 = XX(v0);
377  double YY_v0 = YY(v0);
378  double ZZ_v0 = ZZ(v0);
379  double XX_vF = XX(vF);
380  double YY_vF = YY(vF);
381  double ZZ_vF = ZZ(vF);
382 
383  VECTOR_R3(v, XX_v0, YY_v0, ZZ_v0);
384  M3x3_BY_V3x1(v, V, v);
385  XX(corner1) = XX(v);
386  XX(corner2) = XX(v);
387  YY(corner1) = YY(v);
388  YY(corner2) = YY(v);
389  ZZ(corner1) = ZZ(v);
390  ZZ(corner2) = ZZ(v);
391 
392 #define DEFORM_AND_CHOOSE_CORNERS3D \
393  M3x3_BY_V3x1(v,V,v); \
394  XX(corner1)=XMIPP_MIN(XX(corner1),XX(v)); \
395  XX(corner2)=XMIPP_MAX(XX(corner2),XX(v)); \
396  YY(corner1)=XMIPP_MIN(YY(corner1),YY(v)); \
397  YY(corner2)=XMIPP_MAX(YY(corner2),YY(v)); \
398  ZZ(corner1)=XMIPP_MIN(ZZ(corner1),ZZ(v)); \
399  ZZ(corner2)=XMIPP_MAX(ZZ(corner2),ZZ(v));
400 
401  VECTOR_R3(v, XX_vF, YY_v0, ZZ_v0);
402  DEFORM_AND_CHOOSE_CORNERS3D;
403  VECTOR_R3(v, XX_v0, YY_vF, ZZ_v0);
404  DEFORM_AND_CHOOSE_CORNERS3D;
405  VECTOR_R3(v, XX_vF, YY_vF, ZZ_v0);
406  DEFORM_AND_CHOOSE_CORNERS3D;
407  VECTOR_R3(v, XX_v0, YY_v0, ZZ_vF);
408  DEFORM_AND_CHOOSE_CORNERS3D;
409  VECTOR_R3(v, XX_vF, YY_v0, ZZ_vF);
410  DEFORM_AND_CHOOSE_CORNERS3D;
411  VECTOR_R3(v, XX_v0, YY_vF, ZZ_vF);
412  DEFORM_AND_CHOOSE_CORNERS3D;
413  VECTOR_R3(v, XX_vF, YY_vF, ZZ_vF);
414  DEFORM_AND_CHOOSE_CORNERS3D;
415 }
416 
417 /* Point inside polygon ---------------------------------------------------- */
418 bool point_inside_polygon(const std::vector< Matrix1D<double> > &polygon,
419  const Matrix1D<double> &point)
420 {
421  size_t i, j;
422  bool retval = false;
423  for (i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++)
424  {
425  if ((((YY(polygon[i]) <= YY(point)) && (YY(point) < YY(polygon[j]))) ||
426  ((YY(polygon[j]) <= YY(point)) && (YY(point) < YY(polygon[i])))) &&
427  (XX(point) < (XX(polygon[j]) - XX(polygon[i])) *
428  (YY(point) - YY(polygon[i])) /
429  (YY(polygon[j]) - YY(polygon[i])) + XX(polygon[i])))
430  retval = !retval;
431  }
432  return retval;
433 }
434 
435 /* Affine transformation ---------------------------------------------------*/
436 /*
437  * Given a point u = (ux, uy), its affine point, t = (tx,ty) is defined as t = Au + T, where, A is a 2x2 matrix, and T a translation vector
438  * An affinity is completely determined though three pairs of pois u-t {u1-t1, u2-t2, u3,t3}.
439  * This function makes uses of these three pairs and return the matrix A and the translation T
440  */
441 void def_affinity(double u1x, double u1y, double u2x, double u2y, double u3x, double u3y, double t1x,
442  double t1y, double t2x, double t2y, double t3x, double t3y, Matrix2D<double> &A, Matrix1D<double> &T, Matrix2D<double> &invW)
443 {
444  double den = (u1x*u2y - u1y*u2x - u1x*u3y + u1y*u3x + u2x*u3y - u2y*u3x);
445 
446  //std::cout << "den" <<k << std::endl;
447 
448  invW.initZeros(6,6);
449  MAT_ELEM(invW,0,0) = MAT_ELEM(invW,2,3) = (u2y - u3y)/den;
450  MAT_ELEM(invW,0,1) = MAT_ELEM(invW,2,4) = -(u1y - u3y)/den;
451  MAT_ELEM(invW,0,2) = MAT_ELEM(invW,2,5) = (u1y - u2y)/den;
452 
453  MAT_ELEM(invW,1,0) = MAT_ELEM(invW,3,3) = -(u2x - u3x)/den;
454  MAT_ELEM(invW,1,1) = MAT_ELEM(invW,3,4) = (u1x - u3x)/den;
455  MAT_ELEM(invW,1,2) = MAT_ELEM(invW,3,5) = -(u1x - u2x)/den;
456 
457  MAT_ELEM(invW,4,0) = MAT_ELEM(invW,5,3) = (u2x*u3y - u2y*u3x)/den;
458  MAT_ELEM(invW,4,1) = MAT_ELEM(invW,5,4) = -(u1x*u3y - u1y*u3x)/den;
459  MAT_ELEM(invW,4,2) = MAT_ELEM(invW,5,5) = (u1x*u2y - u1y*u2x)/den;
460 
461 
462 
463  Matrix1D<double> t_vec;
464  t_vec.initZeros(6);
465  VEC_ELEM(t_vec,0) = t1x;
466  VEC_ELEM(t_vec,1) = t2x;
467  VEC_ELEM(t_vec,2) = t3x;
468  VEC_ELEM(t_vec,3) = t1y;
469  VEC_ELEM(t_vec,4) = t2y;
470  VEC_ELEM(t_vec,5) = t3y;
471 
472  double dettt = invW.det();
473  //std::cout << "determinant" << dettt << std::endl;
474 
475  //Matrix1D<double> sol = invW*t_vec;
476  //std::cout << "sol = " << sol << std::endl;
477 
478  if (fabs(dettt) < DBL_EPSILON )
479  {
480  //std::cout << "I'm in if" << std::endl;
481  A.initZeros(2,2);
482  T.initZeros(2);
483  VEC_ELEM(T,0) = DBL_MAX ;
484  VEC_ELEM(T,1) = DBL_MAX ;
485  }
486  else
487  {
488  //std::cout << "I'm in else" << std::endl;
489  Matrix1D<double> sol = invW*t_vec;
490  //std::cout << "sol = " << sol << std::endl;
491 
492  A.initZeros(2,2);
493  T.initZeros(2);
494  //std::cout << A << std::endl;
495  MAT_ELEM(A,0,0) = VEC_ELEM(sol,0);
496  MAT_ELEM(A,0,1) = VEC_ELEM(sol,1);
497  MAT_ELEM(A,1,0) = VEC_ELEM(sol,2);
498  MAT_ELEM(A,1,1) = VEC_ELEM(sol,3);
499  VEC_ELEM(T,0) = VEC_ELEM(sol,4);
500  VEC_ELEM(T,1) = VEC_ELEM(sol,5);
501 
502  //std::cout << "A ==" << sol << std::endl;
503  }
504 }
505 
506 
507 
508 
509 /* Area of a triangle ------------------------------------------------------ */
510 /*Given the coordinates (x1,y1), (x2,y2), (x3,y3), of three points, this function calculates the area of the triangle*/
511 double triangle_area(double x1, double y1, double x2, double y2, double x3, double y3)
512 {
513  double trigarea = ((x2 - x1)*(y3 - y1) - (x3 - x1)*(y2 - y1))/2;
514  return (trigarea > 0) ? trigarea : -trigarea;
515 }
516 
517 /* Line Plane Intersection ------------------------------------------------- */
518 /*Let ax+by+cz+D=0 the equation of your plane
519 (if your plane is defined by a normal vector N + one point M, then
520 (a,b,c) are the coordinates of the normal N, and d is calculated by using
521 the coordinates of M in the above equation).
522 
523 Let your line be defined by one point P(d,e,f) and a vector V(u,v,w), the
524 points on your line are those which verify
525 
526 x=d+lu
527 y=e+lv
528 z=f+lw
529 where l takes all real values.
530 
531 for this point to be on the plane, you have to have
532 
533 ax+by+cz+D=0, so,
534 
535 a(d+lu)+b(e+lv)+c(f+lw)+D=0
536 that is
537 
538 l(au+bv+cw)=-(ad+be+cf+D)
539 
540 note that, if au+bv+cw=0, then your line is either in the plane, or
541 parallel to it... otherwise you get the value of l, and the intersection
542 has coordinates:
543 x=d+lu
544 y=e+lv
545 z=f+lw
546 where
547 
548 l = -(ad+be+cf+D)/(au+bv+cw)
549 
550 a= XX(normal_plane);
551 b= YY(normal_plane);
552 c= ZZ(normal_plane);
553 D= intersection_point;
554 
555 d=XX(point_line)
556 e=YY(point_line)
557 f=ZZ(point_line)
558 
559 u=XX(vector_line);
560 v=YY(vector_line);
561 w=ZZ(vector_line);
562 
563 XX(intersection_point)=x;
564 YY(intersection_point)=y;
565 ZZ(intersection_point)=z;
566 
567 return 0 if sucessful
568 return -1 if line parallel to plane
569 return +1 if line in the plane
570 
571 TEST data (1)
572 
573  point_line = (1,2,3)
574  vector_line = (2,3,4)
575 
576  normal_line= (5,6,7)
577  point_plane_at_x_y_zero = 0
578 
579  Point of interesection (-0.35714,-0.035714,0.28571
580 TEST data (2)
581  Same but change
582  vector_line = (-1.2,1.,0.)
583 TEST data (3)
584  Same but change
585  vector_line = (-1.2,1.,0.)
586  point_line = (0,0,0)
587 */
588 int line_plane_intersection(const Matrix1D<double> normal_plane,
589  const Matrix1D<double> vector_line,
590  Matrix1D<double> &intersection_point,
591  const Matrix1D<double> point_line,
592  double point_plane_at_x_y_zero)
593 {
594  double l;
595  intersection_point.resize(3);
596  // if au+bv+cw=0, then your line is either in the plane, or
597  // parallel to it
598  if (ABS(dotProduct(normal_plane, vector_line)) < XMIPP_EQUAL_ACCURACY)
599  {
600  intersection_point = point_line + vector_line;
601  if (ABS(dotProduct(intersection_point, normal_plane) +
602  point_plane_at_x_y_zero) < XMIPP_EQUAL_ACCURACY)
603  return(1);
604  else
605  return(-1);
606 
607  }
608  //compute intersection
609  l = -1.0 * dotProduct(point_line, normal_plane) +
610  point_plane_at_x_y_zero;
611  l /= dotProduct(normal_plane, vector_line);
612 
613  intersection_point = point_line + l * vector_line;
614  return(0);
615 }
616 
617 
618 /* ######################################################################### */
619 /* Euler Operations */
620 /* ######################################################################### */
621 
622 /* Euler angles --> matrix ------------------------------------------------- */
623 template<typename T>
624 void Euler_angles2matrix(T alpha, T beta, T gamma,
625  Matrix2D<T> &A, bool homogeneous)
626 {
627  static_assert(std::is_floating_point<T>::value, "Only float and double are allowed as template parameters");
628 
629  if (homogeneous)
630  {
631  A.initZeros(4,4);
632  MAT_ELEM(A,3,3)=1;
633  }
634  else
635  if (MAT_XSIZE(A) != 3 || MAT_YSIZE(A) != 3)
636  A.resizeNoCopy(3, 3);
637 
638  T ca = std::cos(DEG2RAD(alpha));
639  T sa = std::sin(DEG2RAD(alpha));
640  T cb = std::cos(DEG2RAD(beta));
641  T sb = std::sin(DEG2RAD(beta));
642  T cg = std::cos(DEG2RAD(gamma));
643  T sg = std::sin(DEG2RAD(gamma));
644 
645  T cc = cb * ca;
646  T cs = cb * sa;
647  T sc = sb * ca;
648  T ss = sb * sa;
649 
650  MAT_ELEM(A, 0, 0) = cg * cc - sg * sa;
651  MAT_ELEM(A, 0, 1) = cg * cs + sg * ca;
652  MAT_ELEM(A, 0, 2) = -cg * sb;
653  MAT_ELEM(A, 1, 0) = -sg * cc - cg * sa;
654  MAT_ELEM(A, 1, 1) = -sg * cs + cg * ca;
655  MAT_ELEM(A, 1, 2) = sg * sb;
656  MAT_ELEM(A, 2, 0) = sc;
657  MAT_ELEM(A, 2, 1) = ss;
658  MAT_ELEM(A, 2, 2) = cb;
659 }
660 
661 void Euler_anglesZXZ2matrix(double a, double b, double g, Matrix2D< double >& A, bool homogeneous)
662 {
663  Matrix2D<double> RZ1, RX2, RZ3;
664  rotation3DMatrix(a,'Z',RZ1,homogeneous);
665  rotation3DMatrix(b,'X',RX2,homogeneous);
666  rotation3DMatrix(g,'Z',RZ3,homogeneous);
667  A=RZ3*RX2*RZ1;
668 }
669 
670 /* Euler distance ---------------------------------------------------------- */
671 double Euler_distanceBetweenMatrices(const Matrix2D<double> &E1,
672  const Matrix2D<double> &E2)
673 {
674  double retval=0;
675  FOR_ALL_ELEMENTS_IN_MATRIX2D(E1)
676  retval+=MAT_ELEM(E1,i,j)*MAT_ELEM(E2,i,j);
677  return retval/3.0;
678 }
679 
680 template<typename T>
681 T Euler_distanceBetweenAngleSets(T rot1, T tilt1, T psi1,
682  T rot2, T tilt2, T psi2,
683  bool only_projdir)
684 {
685  static_assert(std::is_floating_point<T>::value, "Only float and double are allowed as template parameters");
686 
687  Matrix2D<T> E1, E2;
688  Euler_angles2matrix(rot1, tilt1, psi1, E1, false);
689  return Euler_distanceBetweenAngleSets_fast(E1,rot2,tilt2,psi2,only_projdir,E2);
690 }
691 
692 template<typename T>
693 T Euler_distanceBetweenAngleSets_fast(const Matrix2D<T> &E1,
694  T rot2, T tilt2, T psi2,
695  bool only_projdir, Matrix2D<T> &E2)
696 {
697  static_assert(std::is_floating_point<T>::value, "Only float and double are allowed as template parameters");
698 
699  Euler_angles2matrix(rot2, tilt2, psi2, E2, false);
700  T aux=MAT_ELEM(E1,2,0)*MAT_ELEM(E2,2,0)+
701  MAT_ELEM(E1,2,1)*MAT_ELEM(E2,2,1)+
702  MAT_ELEM(E1,2,2)*MAT_ELEM(E2,2,2);
703  T axes_dist=std::acos(CLIP(aux, -1, 1));
704  if (!only_projdir)
705  {
706  for (int i = 0; i < 2; i++)
707  {
708  T aux=MAT_ELEM(E1,i,0)*MAT_ELEM(E2,i,0)+
709  MAT_ELEM(E1,i,1)*MAT_ELEM(E2,i,1)+
710  MAT_ELEM(E1,i,2)*MAT_ELEM(E2,i,2);
711  T dist=acos(CLIP(aux, -1, 1));
712  axes_dist += dist;
713  }
714  axes_dist /= 3.0;
715  }
716  return RAD2DEG(axes_dist);
717 }
718 
719 
720 /* Euler direction --------------------------------------------------------- */
721 void Euler_direction(double alpha, double beta, double gamma,
722  Matrix1D<double> &v)
723 {
724  double ca, sa, cb, sb;
725  double sc, ss;
726 
727  v.resize(3);
728  alpha = DEG2RAD(alpha);
729  beta = DEG2RAD(beta);
730 
731  ca = cos(alpha);
732  cb = cos(beta);
733  sa = sin(alpha);
734  sb = sin(beta);
735  sc = sb * ca;
736  ss = sb * sa;
737 
738  v(0) = sc;
739  v(1) = ss;
740  v(2) = cb;
741 }
742 
743 /* Euler direction2angles ------------------------------- */
744 //gamma is useless but I keep it for simmetry
745 //with Euler_direction
746 void Euler_direction2angles(Matrix1D<double> &v0,
747  double &alpha, double &beta, double &gamma)
748 {
749  double abs_ca, sb, cb;
750  double aux_alpha;
751  double aux_beta;
752  double error, newerror;
753  Matrix1D<double> v_aux;
754  Matrix1D<double> v;
755 
756  //if not normalized do it so
757  v.resize(3);
758  v = v0;
759  v.selfNormalize();
760 
761  v_aux.resize(3);
762  cb = v(2);
763 
764  if (fabs((cb)) > 0.999847695)/*one degree */
765  {
766  std::cerr << "\nWARNING: Routine Euler_direction2angles is not reliable\n"
767  "for small tilt angles. Up to 0.001 deg it should be OK\n"
768  "for most applications but you never know";
769  }
770 
771  if (fabs((cb - 1.)) < FLT_EPSILON)
772  {
773  alpha = 0.;
774  beta = 0.;
775  }
776  else
777  {/*1*/
778 
779  aux_beta = acos(cb); /* beta between 0 and PI */
780 
781 
782  sb = sin(aux_beta);
783 
784  abs_ca = fabs(v(0)) / sb;
785  if (fabs((abs_ca - 1.)) < FLT_EPSILON)
786  aux_alpha = 0.;
787  else
788  aux_alpha = acos(abs_ca);
789 
790  v_aux(0) = sin(aux_beta) * cos(aux_alpha);
791  v_aux(1) = sin(aux_beta) * sin(aux_alpha);
792  v_aux(2) = cos(aux_beta);
793 
794  error = fabs(dotProduct(v, v_aux) - 1.);
795  alpha = aux_alpha;
796  beta = aux_beta;
797 
798  v_aux(0) = sin(aux_beta) * cos(-1. * aux_alpha);
799  v_aux(1) = sin(aux_beta) * sin(-1. * aux_alpha);
800  v_aux(2) = cos(aux_beta);
801  newerror = fabs(dotProduct(v, v_aux) - 1.);
802  if (error > newerror)
803  {
804  alpha = -1. * aux_alpha;
805  beta = aux_beta;
806  error = newerror;
807  }
808 
809  v_aux(0) = sin(-aux_beta) * cos(-1. * aux_alpha);
810  v_aux(1) = sin(-aux_beta) * sin(-1. * aux_alpha);
811  v_aux(2) = cos(-aux_beta);
812  newerror = fabs(dotProduct(v, v_aux) - 1.);
813  if (error > newerror)
814  {
815  alpha = -1. * aux_alpha;
816  beta = -1. * aux_beta;
817  error = newerror;
818  }
819 
820  v_aux(0) = sin(-aux_beta) * cos(aux_alpha);
821  v_aux(1) = sin(-aux_beta) * sin(aux_alpha);
822  v_aux(2) = cos(-aux_beta);
823  newerror = fabs(dotProduct(v, v_aux) - 1.);
824 
825  if (error > newerror)
826  {
827  alpha = aux_alpha;
828  beta = -1. * aux_beta;
829  }
830  }/*else 1 end*/
831  gamma = 0.;
832  beta = RAD2DEG(beta);
833  alpha = RAD2DEG(alpha);
834 }/*Eulerdirection2angles end*/
835 
836 /* Matrix --> Euler angles ------------------------------------------------- */
837 #define CHECK
838 //#define DEBUG
839 void Euler_matrix2angles(const Matrix2D<double> &A, double &alpha,
840  double &beta, double &gamma,bool homogeneous)
841 {
842  double abs_sb, sign_sb;
843 
844  if (homogeneous)
845  if (MAT_XSIZE(A) != 4 || MAT_YSIZE(A) != 4)
846  REPORT_ERROR(ERR_MATRIX_SIZE, "Euler_matrix2angles: The Euler matrix is not 4x4");
847  else
848  if (MAT_XSIZE(A) != 3 || MAT_YSIZE(A) != 3)
849  REPORT_ERROR(ERR_MATRIX_SIZE, "Euler_matrix2angles: The Euler matrix is not 3x3");
850 
851  abs_sb = sqrt(A(0, 2) * A(0, 2) + A(1, 2) * A(1, 2));
852  if (abs_sb > 16*FLT_EPSILON)
853  {
854  gamma = atan2(A(1, 2), -A(0, 2));
855  alpha = atan2(A(2, 1), A(2, 0));
856  if (ABS(sin(gamma)) < FLT_EPSILON)
857  sign_sb = SGN(-A(0, 2) / cos(gamma));
858  // if (sin(alpha)<FLT_EPSILON) sign_sb=SGN(-A(0,2)/cos(gamma));
859  // else sign_sb=(sin(alpha)>0) ? SGN(A(2,1)):-SGN(A(2,1));
860  else
861  sign_sb = (sin(gamma) > 0) ? SGN(A(1, 2)) : -SGN(A(1, 2));
862  beta = atan2(sign_sb * abs_sb, A(2, 2));
863  }
864  else
865  {
866  if (SGN(A(2, 2)) > 0)
867  {
868  // Let's consider the matrix as a rotation around Z
869  alpha = 0;
870  beta = 0;
871  gamma = atan2(-A(1, 0), A(0, 0));
872  }
873  else
874  {
875  alpha = 0;
876  beta = PI;
877  gamma = atan2(A(1, 0), -A(0, 0));
878  }
879  }
880 
881  gamma = RAD2DEG(gamma);
882  beta = RAD2DEG(beta);
883  alpha = RAD2DEG(alpha);
884 
885 #ifdef double
886 
887  Matrix2D<double> Ap;
888  Euler_angles2matrix(alpha, beta, gamma, Ap);
889  if (A != Ap)
890  {
891  std::cout << "---\n";
892  std::cout << "Euler_matrix2angles: I have computed angles "
893  " which doesn't match with the original matrix\n";
894  std::cout << "Original matrix\n" << A;
895  std::cout << "Computed angles alpha=" << alpha << " beta=" << beta
896  << " gamma=" << gamma << std::endl;
897  std::cout << "New matrix\n" << Ap;
898  std::cout << "---\n";
899  }
900 #endif
901 
902 #ifdef DEBUG
903  std::cout << "abs_sb " << abs_sb << std::endl;
904  std::cout << "A(1,2) " << A(1, 2) << " A(0,2) " << A(0, 2) << " gamma "
905  << gamma << std::endl;
906  std::cout << "A(2,1) " << A(2, 1) << " A(2,0) " << A(2, 0) << " alpha "
907  << alpha << std::endl;
908  std::cout << "sign sb " << sign_sb << " A(2,2) " << A(2, 2)
909  << " beta " << beta << std::endl;
910 #endif
911 }
912 #undef CHECK
913 #undef DEBUG
914 
915 #ifdef NEVERDEFINED
916 // Michael's method
917 void Euler_matrix2angles(Matrix2D<double> A, double *alpha, double *beta,
918  double *gamma)
919 {
920  double abs_sb;
921 
922  if (ABS(A(1, 1)) > FLT_EPSILON)
923  {
924  abs_sb = sqrt((-A(2, 2) * A(1, 2) * A(2, 1) - A(0, 2) * A(2, 0)) / A(1, 1));
925  }
926  else if (ABS(A(0, 1)) > FLT_EPSILON)
927  {
928  abs_sb = sqrt((-A(2, 1) * A(2, 2) * A(0, 2) + A(2, 0) * A(1, 2)) / A(0, 1));
929  }
930  else if (ABS(A(0, 0)) > FLT_EPSILON)
931  {
932  abs_sb = sqrt((-A(2, 0) * A(2, 2) * A(0, 2) - A(2, 1) * A(1, 2)) / A(0, 0));
933  }
934  else
935  EXIT_ERROR(1, "Don't know how to extract angles");
936 
937  if (abs_sb > FLT_EPSILON)
938  {
939  *beta = atan2(abs_sb, A(2, 2));
940  *alpha = atan2(A(2, 1) / abs_sb, A(2, 0) / abs_sb);
941  *gamma = atan2(A(1, 2) / abs_sb, -A(0, 2) / abs_sb);
942  }
943  else
944  {
945  *alpha = 0;
946  *beta = 0;
947  *gamma = atan2(A(1, 0), A(0, 0));
948  }
949 
950  *gamma = rad2deg(*gamma);
951  *beta = rad2deg(*beta);
952  *alpha = rad2deg(*alpha);
953 }
954 #endif
955 void Euler_Angles_after_compresion(const double rot, double tilt, double psi,
956  double &new_rot, double &new_tilt, double &new_psi, Matrix2D<double> &D)
957 {
958  Matrix1D<double> w(3);
959  Matrix1D<double> new_w(3);
960  Matrix2D<double> D_1(3, 3);
961 
962  //if D has not inverse we are not in business
963  D_1 = D.inv();
964 
965  Euler_direction(rot, tilt, psi, w);
966  if (fabs(w(2)) > 0.999847695)/*cos one degree */
967  {
968  Euler_direction(rot, 10., psi, w);
969  new_w = (Matrix1D<double>)(D_1 * w) / ((D_1 * w).module());
970  Euler_direction2angles(new_w, new_rot, new_tilt, new_psi);
971 
972  Euler_direction(rot, tilt, psi, w);
973  new_w = (Matrix1D<double>)((D_1 * w) / ((D_1 * w).module()));
974  new_tilt = SGN(new_tilt) * fabs(ACOSD(new_w(2)));
975  new_psi = psi;
976 
977  // so, for small tilt the value of the rot is not realiable
978  // doubleo overcome this problem I first calculate the rot for
979  // any arbitrary large tilt angle and the right rotation
980  // and then I calculate the new tilt.
981  // Please notice that the new_rotation is not a funcion of
982  // the old tilt angle so I can use any arbitrary tilt angle
983  }
984  else
985  {
986  new_w = (Matrix1D<double>)(D_1 * w) / ((D_1 * w).module());
987  Euler_direction2angles(new_w, new_rot, new_tilt, new_psi);
988  new_psi = psi;
989  }
990 }
991 
992 /* Euler up-down correction ------------------------------------------------ */
993 void Euler_up_down(double rot, double tilt, double psi,
994  double &newrot, double &newtilt, double &newpsi)
995 {
996  newrot = rot;
997  newtilt = tilt + 180;
998  newpsi = -(180 + psi);
999 }
1000 
1001 /* Same view, differently expressed ---------------------------------------- */
1002 void Euler_another_set(double rot, double tilt, double psi,
1003  double &newrot, double &newtilt, double &newpsi)
1004 {
1005  newrot = rot + 180;
1006  newtilt = -tilt;
1007  newpsi = -180 + psi;
1008 }
1009 
1010 /* Euler mirror Y ---------------------------------------------------------- */
1011 void Euler_mirrorY(double rot, double tilt, double psi,
1012  double &newrot, double &newtilt, double &newpsi)
1013 {
1014  newrot = rot;
1015  newtilt = tilt + 180;
1016  newpsi = -psi;
1017 }
1018 
1019 /* Euler mirror X ---------------------------------------------------------- */
1020 void Euler_mirrorX(double rot, double tilt, double psi,
1021  double &newrot, double &newtilt, double &newpsi)
1022 {
1023  newrot = rot;
1024  newtilt = tilt + 180;
1025  newpsi = 180 - psi;
1026 }
1027 
1028 /* Euler mirror XY --------------------------------------------------------- */
1029 void Euler_mirrorXY(double rot, double tilt, double psi,
1030  double &newrot, double &newtilt, double &newpsi)
1031 {
1032  newrot = rot;
1033  newtilt = tilt;
1034  newpsi = 180 + psi;
1035 }
1036 
1037 /* Apply a transformation matrix to Euler angles --------------------------- */
1038 void Euler_apply_transf(const Matrix2D<double> &L,
1039  const Matrix2D<double> &R,
1040  double rot,
1041  double tilt,
1042  double psi,
1043  double &newrot,
1044  double &newtilt,
1045  double &newpsi)
1046 {
1047 
1048  Matrix2D<double> euler(3, 3), temp;
1049  Euler_angles2matrix(rot, tilt, psi, euler);
1050  temp = L * euler * R;
1051  Euler_matrix2angles(temp, newrot, newtilt, newpsi);
1052 }
1053 
1054 
1055 //void Euler_rotation3DMatrix(double rot, double tilt, double psi, Matrix2D<double> &result)
1056 //{
1057 // Euler_angles2matrix(rot, tilt, psi, result, true);
1058 //}
1059 
1060 /* Rotate (3D) MultidimArray with 3 Euler angles ------------------------------------- */
1061 void Euler_rotate(const MultidimArray<double> &V, double rot, double tilt, double psi,
1062  MultidimArray<double> &result)
1063 {
1064  Matrix2D<double> R;
1065  Euler_angles2matrix(rot, tilt, psi, R, true);
1066  applyGeometry(1, result, V, R, xmipp_transformation::IS_NOT_INV, xmipp_transformation::DONT_WRAP);
1067 }
1068 void Euler_rotate(const MultidimArrayGeneric &V, double rot, double tilt, double psi,
1069  MultidimArray<double> &result)
1070 {
1071  Matrix2D<double> R;
1072  Euler_angles2matrix(rot, tilt, psi, R, true);
1073 #define APPLYGEO(type) applyGeometry(1, result, *((MultidimArray<type> *)V.im), R, xmipp_transformation::IS_NOT_INV, xmipp_transformation::DONT_WRAP);
1074  SWITCHDATATYPE(V.datatype, APPLYGEO)
1075 #undef APPLYGEO
1076 }
1077 
1078 void computeCircleAroundE(const Matrix2D<double> &E,
1079  double angCircle, double angStep, std::vector<double> &outputEulerAngles)
1080 {
1081  outputEulerAngles.clear();
1082 
1083  // Get the projection direction and a perpendicular direction
1084  Matrix1D<double> projectionDirection, perpendicular;
1085  E.getRow(1,perpendicular);
1086  E.getRow(2,projectionDirection);
1087  Matrix2D<double> newEt;
1088  newEt = E.transpose();
1089  Matrix2D<double> rotStep, sampling;
1090  rotation3DMatrix(angCircle,perpendicular,rotStep,false);
1091  rotation3DMatrix(angStep,projectionDirection,sampling,false);
1092 
1093  // Now rotate
1094  newEt = rotStep*newEt;
1095  for (double i = 0; i < 360; i += angStep)
1096  {
1097  newEt=sampling*newEt;
1098 
1099  // Normalize
1100  for (int c=0; c<3; c++)
1101  {
1102  Matrix1D<double> aux;
1103  newEt.getCol(c,aux);
1104  aux/=aux.module();
1105  newEt.setCol(c,aux);
1106  }
1107  Matrix2D<double> newE=newEt.transpose();
1108 
1109  double newrot, newtilt, newpsi;
1110  Euler_matrix2angles(newE,newrot,newtilt,newpsi);
1111  outputEulerAngles.push_back(newrot);
1112  outputEulerAngles.push_back(newtilt);
1113  outputEulerAngles.push_back(newpsi);
1114  }
1115 }
1116 
1117 /* ######################################################################### */
1118 /* Intersections */
1119 /* ######################################################################### */
1120 
1121 /* Intersection with a unit sphere ----------------------------------------- */
1122 double intersection_unit_sphere(
1123  const Matrix1D<double> &u, // direction
1124  const Matrix1D<double> &r) // passing point
1125 {
1126 
1127  // Some useful constants
1128  double A = XX(u) * XX(u) + YY(u) * YY(u) + ZZ(u) * ZZ(u);
1129  double B = XX(r) * XX(u) + YY(r) * YY(u) + ZZ(r) * ZZ(u);
1130  double C = XX(r) * XX(r) + YY(r) * YY(r) + ZZ(r) * ZZ(r) - 1.0;
1131  double B2_AC = B * B - A * C;
1132 
1133  // A degenerate case?
1134  if (A == 0)
1135  {
1136  if (B == 0)
1137  return -1; // The ellipsoid doesn't intersect
1138  return 0; // The ellipsoid is tangent at t=-C/2B
1139  }
1140  if (B2_AC < 0)
1141  return -1;
1142 
1143  // A normal intersection
1144  B2_AC = sqrt(B2_AC);
1145  double t1 = (-B - B2_AC) / A; // The two parameters within the line for
1146  double t2 = (-B + B2_AC) / A; // the solution
1147  return ABS(t2 -t1);
1148 }
1149 
1150 /* Intersection with a unit cylinder --------------------------------------- */
1151 double intersection_unit_cylinder(
1152  const Matrix1D<double> &u, // direction
1153  const Matrix1D<double> &r) // passing point
1154 {
1155  // Intersect with an infinite cylinder of radius=ry
1156  double A = XX(u) * XX(u) + YY(u) * YY(u);
1157  double B = XX(r) * XX(u) + YY(r) * YY(u);
1158  double C = XX(r) * XX(r) + YY(r) * YY(r) - 1;
1159 
1160  double B2_AC = B * B - A * C;
1161  if (A == 0)
1162  {
1163  if (C > 0)
1164  return 0; // Parallel ray outside the cylinder
1165  else
1166  return 1 / ZZ(u); // return height
1167  }
1168  else if (B2_AC < 0)
1169  return 0;
1170  B2_AC = sqrt(B2_AC);
1171 
1172  // Points at intersection
1173  double t1 = (-B - B2_AC) / A;
1174  double t2 = (-B + B2_AC) / A;
1175  double z1 = ZZ(r) + t1 * ZZ(u);
1176  double z2 = ZZ(r) + t2 * ZZ(u);
1177 
1178  // Check position of the intersecting points with respect to
1179  // the finite cylinder, if any is outside correct it to the
1180  // right place in the top or bottom of the cylinder
1181  if (ABS(z1) >= 0.5)
1182  t1 = (SGN(z1) * 0.5 - ZZ(r)) / ZZ(u);
1183  if (ABS(z2) >= 0.5)
1184  t2 = (SGN(z2) * 0.5 - ZZ(r)) / ZZ(u);
1185 
1186  return ABS(t1 -t2);
1187 }
1188 
1189 /* Intersection with a unit cube ------------------------------------------- */
1190 double intersection_unit_cube(
1191  const Matrix1D<double> &u, // direction
1192  const Matrix1D<double> &r) // passing point
1193 {
1194  double t1=0., t2=0., t;
1195  int found_t = 0;
1196 
1197 #define ASSIGN_IF_GOOD_ONE \
1198  if (fabs(XX(r)+t*XX(u))-XMIPP_EQUAL_ACCURACY<=0.5 && \
1199  fabs(YY(r)+t*YY(u))-XMIPP_EQUAL_ACCURACY<=0.5 && \
1200  fabs(ZZ(r)+t*ZZ(u))-XMIPP_EQUAL_ACCURACY<=0.5) {\
1201  if (found_t==0) {found_t++; t1=t;} \
1202  else if (found_t==1) {found_t++; t2=t;} \
1203  }
1204 
1205  // Intersect with x=0.5 and x=-0.5
1206  if (XX(u) != 0)
1207  {
1208  t = (0.5 - XX(r)) / XX(u);
1209  ASSIGN_IF_GOOD_ONE;
1210  t = (-0.5 - XX(r)) / XX(u);
1211  ASSIGN_IF_GOOD_ONE;
1212  }
1213 
1214  // Intersect with y=0.5 and y=-0.5
1215  if (YY(u) != 0 && found_t != 2)
1216  {
1217  t = (0.5 - YY(r)) / YY(u);
1218  ASSIGN_IF_GOOD_ONE;
1219  t = (-0.5 - YY(r)) / YY(u);
1220  ASSIGN_IF_GOOD_ONE;
1221  }
1222 
1223  // Intersect with z=0.5 and z=-0.5
1224  if (ZZ(u) != 0 && found_t != 2)
1225  {
1226  t = (0.5 - ZZ(r)) / ZZ(u);
1227  ASSIGN_IF_GOOD_ONE;
1228  t = (-0.5 - ZZ(r)) / ZZ(u);
1229  ASSIGN_IF_GOOD_ONE;
1230  }
1231 
1232  if (found_t == 2)
1233  return fabs(t1 -t2);
1234  else
1235  return 0;
1236 }
1237 
1238 double Bspline_model::evaluate(double x, double y) const
1239 {
1240  int SplineDegree_1 = SplineDegree - 1;
1241  double x_arg = (x - x0) / h_x;
1242  double y_arg = (y - y0) / h_y;
1243 
1244  int l1 = CLIP(CEIL(x_arg - SplineDegree_1), l0, lF);
1245  int l2 = CLIP(l1 + SplineDegree, l0, lF);
1246  int m1 = CLIP(CEIL(y_arg - SplineDegree_1), m0, mF);
1247  int m2 = CLIP(m1 + SplineDegree, m0, mF);
1248  double columns = 0.0;
1249  for (int m = m1; m <= m2; m++)
1250  {
1251  double rows = 0.0;
1252  for (int l = l1; l <= l2; l++)
1253  {
1254  double xminusl = x_arg - (double)l;
1255  double Coeff = c_ml(m, l);
1256  switch (SplineDegree)
1257  {
1258  case 2:
1259  rows += Coeff * Bspline02(xminusl);
1260  break;
1261  case 3:
1262  rows += Coeff * Bspline03(xminusl);
1263  break;
1264  case 4:
1265  rows += Coeff * Bspline04(xminusl);
1266  break;
1267  case 5:
1268  rows += Coeff * Bspline05(xminusl);
1269  break;
1270  case 6:
1271  rows += Coeff * Bspline06(xminusl);
1272  break;
1273  case 7:
1274  rows += Coeff * Bspline07(xminusl);
1275  break;
1276  case 8:
1277  rows += Coeff * Bspline08(xminusl);
1278  break;
1279  case 9:
1280  rows += Coeff * Bspline09(xminusl);
1281  break;
1282  }
1283  }
1284 
1285  double yminusm = y_arg - (double)m;
1286  switch (SplineDegree)
1287  {
1288  case 2:
1289  columns += rows * Bspline02(yminusm);
1290  break;
1291  case 3:
1292  columns += rows * Bspline03(yminusm);
1293  break;
1294  case 4:
1295  columns += rows * Bspline04(yminusm);
1296  break;
1297  case 5:
1298  columns += rows * Bspline05(yminusm);
1299  break;
1300  case 6:
1301  columns += rows * Bspline06(yminusm);
1302  break;
1303  case 7:
1304  columns += rows * Bspline07(yminusm);
1305  break;
1306  case 8:
1307  columns += rows * Bspline08(yminusm);
1308  break;
1309  case 9:
1310  columns += rows * Bspline09(yminusm);
1311  break;
1312  }
1313  }
1314  return columns;
1315 }
1316 
1317 // explicit instantiation
1318 template void Euler_angles2matrix<double>(double, double, double, Matrix2D<double>&, bool);
1319 template void Euler_angles2matrix<float>(float, float, float, Matrix2D<float>&, bool);
1320 template float Euler_distanceBetweenAngleSets<float>(
1321  float rot1, float tilt1, float psi1, float rot2, float tilt2, float psi2,
1322  bool only_projdir);
1323 template double Euler_distanceBetweenAngleSets<double>(
1324  double rot1, double tilt1, double psi1, double rot2, double tilt2, double psi2,
1325  bool only_projdir);
1326 template double Euler_distanceBetweenAngleSets_fast<double>(
1327  const Matrix2D<double> &E1, double rot2, double tilt2, double psi2,
1328  bool only_projdir, Matrix2D<double> &E2);
FOR_ALL_ELEMENTS_IN_MATRIX2D
#define FOR_ALL_ELEMENTS_IN_MATRIX2D(m)
Definition: matrix2d.h:104
VECTOR_R2
#define VECTOR_R2(v, x, y)
Definition: matrix1d.h:112
MAT_YSIZE
#define MAT_YSIZE(m)
Definition: matrix2d.h:124
ASSIGN_IF_GOOD_ONE
#define ASSIGN_IF_GOOD_ONE
spherical_distance
double spherical_distance(const Matrix1D< double > &r1, const Matrix1D< double > &r2)
Definition: geometry.cpp:73
DBL_EPSILON
#define DBL_EPSILON
YSIZE
#define YSIZE(v)
Definition: multidim_array_base.h:95
A2D_ELEM
#define A2D_ELEM(v, i, j)
Definition: multidim_array_base.h:354
VEC_ELEM
#define VEC_ELEM(v, i)
Definition: matrix1d.h:245
Bspline05
double Bspline05(double Argument)
point_inside_polygon
bool point_inside_polygon(const std::vector< Matrix1D< double > > &polygon, const Matrix1D< double > &point)
Definition: geometry.cpp:418
Matrix1D::module
double module() const
Definition: matrix1d.h:983
Bspline_model::y0
double y0
y0
Definition: geometry.h:288
point_line_distance_3D
double point_line_distance_3D(const Matrix1D< double > &p, const Matrix1D< double > &a, const Matrix1D< double > &v)
Definition: geometry.cpp:85
least_squares_plane_fit
void least_squares_plane_fit(FitPoint *IN_points, int Npoints, double &plane_a, double &plane_b, double &plane_c)
Definition: geometry.cpp:101
FINISHINGX
#define FINISHINGX(v)
Definition: multidim_array_base.h:55
REPORT_ERROR
#define REPORT_ERROR(nerr, ErrormMsg)
Definition: xmipp_error.h:211
Bspline_model
Definition: geometry.h:273
Matrix1D::size
size_t size() const
Definition: matrix1d.h:508
ERR_MATRIX_SIZE
Problem with matrix size.
Definition: xmipp_error.h:152
VEC_XSIZE
#define VEC_XSIZE(m)
Definition: matrix1d.h:77
Euler_angles2matrix
void Euler_angles2matrix(T alpha, T beta, T gamma, Matrix2D< T > &A, bool homogeneous)
Definition: geometry.cpp:624
c
doublereal * c
Definition: numerical_recipes.cpp:2230
Euler_angles2matrix< double >
template void Euler_angles2matrix< double >(double, double, double, Matrix2D< double > &, bool)
g
doublereal * g
Definition: numerical_recipes.cpp:2473
Bspline_model::lF
int lF
lF;
Definition: geometry.h:279
Euler_direction
void Euler_direction(double alpha, double beta, double gamma, Matrix1D< double > &v)
Definition: geometry.cpp:721
DEG2RAD
#define DEG2RAD(d)
Definition: xmipp_macros.h:312
Euler_direction2angles
void Euler_direction2angles(Matrix1D< double > &v0, double &alpha, double &beta, double &gamma)
Definition: geometry.cpp:746
xmipp_funcs.h
alglib::beta
double beta(const double a, const double b)
Definition: specialfunctions.cpp:962
def_affinity
void def_affinity(double u1x, double u1y, double u2x, double u2y, double u3x, double u3y, double t1x, double t1y, double t2x, double t2y, double t3x, double t3y, Matrix2D< double > &A, Matrix1D< double > &T, Matrix2D< double > &invW)
Definition: geometry.cpp:441
FitPoint::z
double z
z coordinate, assumed to be a function of x and y
Definition: geometry.h:190
Euler_another_set
void Euler_another_set(double rot, double tilt, double psi, double &newrot, double &newtilt, double &newpsi)
Definition: geometry.cpp:1002
sqrt
void sqrt(Image< double > &op)
Definition: image_operate.cpp:210
y
static double * y
Definition: numerical_recipes.cpp:8487
FitPoint::x
double x
x coordinate
Definition: geometry.h:186
applyGeometry
void applyGeometry(int SplineDegree, MultidimArray< std::complex< double > > &V2, const MultidimArray< std::complex< double > > &V1, const Matrix2D< double > &A, bool inv, bool wrap, std::complex< double > outside, MultidimArray< double > *BcoeffsPtr)
Definition: transformations.cpp:668
V3_MINUS_V3
#define V3_MINUS_V3(a, b, c)
Definition: matrix1d.h:202
Euler_mirrorY
void Euler_mirrorY(double rot, double tilt, double psi, double &newrot, double &newtilt, double &newpsi)
Definition: geometry.cpp:1011
Bspline_model_fitting
void Bspline_model_fitting(const std::vector< FitPoint > &IN_points, int SplineDegree, int l0, int lF, int m0, int mF, double h_x, double h_y, double x0, double y0, Bspline_model &result)
Definition: geometry.cpp:243
Matrix2D::inv
void inv(Matrix2D< T > &result) const
Definition: matrix2d.cpp:663
w
doublereal * w
Definition: numerical_recipes.cpp:2477
FitPoint::w
double w
Weight of the point in the Least-Squares problem.
Definition: geometry.h:192
Matrix1D::selfNormalize
void selfNormalize()
Definition: matrix1d.cpp:723
Matrix2D::transpose
Matrix2D< T > transpose() const
Definition: matrix2d.cpp:1314
vectorProduct
Matrix1D< T > vectorProduct(const Matrix1D< T > &v1, const Matrix1D< T > &v2)
Definition: matrix1d.h:1165
intersection_unit_cylinder
double intersection_unit_cylinder(const Matrix1D< double > &u, const Matrix1D< double > &r)
Definition: geometry.cpp:1151
fit_point2D::x
double x
x coordinate
Definition: geometry.h:234
gamma
double * gamma
Definition: numerical_recipes.cpp:7597
Euler_apply_transf
void Euler_apply_transf(const Matrix2D< double > &L, const Matrix2D< double > &R, double rot, double tilt, double psi, double &newrot, double &newtilt, double &newpsi)
Definition: geometry.cpp:1038
STARTINGX
#define STARTINGX(v)
Definition: multidim_array_base.h:51
Bspline_model::l0
int l0
l0
Definition: geometry.h:277
x
doublereal * x
Definition: numerical_recipes.cpp:2230
i
#define i
Definition: numerical_recipes.cpp:2493
APPLYGEO
#define APPLYGEO(type)
line_plane_intersection
int line_plane_intersection(const Matrix1D< double > normal_plane, const Matrix1D< double > vector_line, Matrix1D< double > &intersection_point, const Matrix1D< double > point_line, double point_plane_at_x_y_zero)
Definition: geometry.cpp:588
triangle_area
double triangle_area(double x1, double y1, double x2, double y2, double x3, double y3)
Definition: geometry.cpp:511
Bspline_model::x0
double x0
x0
Definition: geometry.h:286
DEFORM_AND_CHOOSE_CORNERS3D
#define DEFORM_AND_CHOOSE_CORNERS3D
Bspline04
double Bspline04(double Argument)
rotation3DMatrix
void rotation3DMatrix(double ang, char axis, Matrix2D< double > &result, bool homogeneous)
Definition: transformations.cpp:426
Matrix2D::resizeNoCopy
void resizeNoCopy(int Ydim, int Xdim)
Definition: matrix2d.h:534
STARTINGY
#define STARTINGY(v)
Definition: multidim_array_base.h:59
computeCircleAroundE
void computeCircleAroundE(const Matrix2D< double > &E, double angCircle, double angStep, std::vector< double > &outputEulerAngles)
Definition: geometry.cpp:1078
CLIP
#define CLIP(x, x0, xF)
Definition: xmipp_macros.h:260
MAT_ELEM
#define MAT_ELEM(m, i, j)
Definition: matrix2d.h:116
Euler_distanceBetweenAngleSets< double >
template double Euler_distanceBetweenAngleSets< double >(double rot1, double tilt1, double psi1, double rot2, double tilt2, double psi2, bool only_projdir)
b
doublereal * b
Definition: numerical_recipes.cpp:2230
box_enclosing
void box_enclosing(const Matrix1D< double > &v0, const Matrix1D< double > &vF, const Matrix2D< double > &V, Matrix1D< double > &corner1, Matrix1D< double > &corner2)
Definition: geometry.cpp:366
cs
struct _constraint * cs
Definition: numerical_recipes.cpp:4636
Euler_Angles_after_compresion
void Euler_Angles_after_compresion(const double rot, double tilt, double psi, double &new_rot, double &new_tilt, double &new_psi, Matrix2D< double > &D)
Definition: geometry.cpp:955
y0
#define y0
rectangle_enclosing
void rectangle_enclosing(const Matrix1D< double > &v0, const Matrix1D< double > &vF, const Matrix2D< double > &V, Matrix1D< double > &corner1, Matrix1D< double > &corner2)
Definition: geometry.cpp:328
x0
#define x0
XX
#define XX(v)
Definition: matrix1d.h:85
M2x2_BY_V2x1
#define M2x2_BY_V2x1(a, M, b)
Definition: matrix2d.h:225
intersection_unit_sphere
double intersection_unit_sphere(const Matrix1D< double > &u, const Matrix1D< double > &r)
Definition: geometry.cpp:1122
CEIL
#define CEIL(x)
Definition: xmipp_macros.h:225
least_squares_line_fit
void least_squares_line_fit(const std::vector< fit_point2D > &IN_points, double &line_a, double &line_b)
Definition: geometry.cpp:212
fit_point2D
Definition: geometry.h:231
XMIPP_EQUAL_ACCURACY
#define XMIPP_EQUAL_ACCURACY
Definition: xmipp_macros.h:119
Euler_distanceBetweenMatrices
double Euler_distanceBetweenMatrices(const Matrix2D< double > &E1, const Matrix2D< double > &E2)
Definition: geometry.cpp:671
Euler_distanceBetweenAngleSets_fast< double >
template double Euler_distanceBetweenAngleSets_fast< double >(const Matrix2D< double > &E1, double rot2, double tilt2, double psi2, bool only_projdir, Matrix2D< double > &E2)
Matrix1D::resize
void resize(size_t Xdim, bool copy=true)
Definition: matrix1d.h:410
sampling
#define sampling
Definition: spherical_harmonics.h:36
XSIZE
#define XSIZE(v)
Definition: multidim_array_base.h:91
Matrix2D::det
T det() const
Definition: matrix2d.cpp:38
Euler_mirrorX
void Euler_mirrorX(double rot, double tilt, double psi, double &newrot, double &newtilt, double &newpsi)
Definition: geometry.cpp:1020
Bspline_model::SplineDegree
int SplineDegree
Order of the Bspline.
Definition: geometry.h:291
SPEED_UP_temps01
#define SPEED_UP_temps01
Definition: xmipp_macros.h:398
Bspline03
double Bspline03(double Argument)
Euler_distanceBetweenAngleSets
T Euler_distanceBetweenAngleSets(T rot1, T tilt1, T psi1, T rot2, T tilt2, T psi2, bool only_projdir)
Definition: geometry.cpp:681
Euler_up_down
void Euler_up_down(double rot, double tilt, double psi, double &newrot, double &newtilt, double &newpsi)
Definition: geometry.cpp:993
Bspline07
double Bspline07(double Argument)
ABS
#define ABS(x)
Definition: xmipp_macros.h:142
Euler_mirrorXY
void Euler_mirrorXY(double rot, double tilt, double psi, double &newrot, double &newtilt, double &newpsi)
Definition: geometry.cpp:1029
Bspline08
double Bspline08(double Argument)
fit_point2D::y
double y
y coordinate (assumed to be a function of x)
Definition: geometry.h:236
direction
void direction(const MultidimArray< double > &orMap, MultidimArray< double > &qualityMap, double lambda, int size, MultidimArray< double > &dirMap, int x, int y)
Definition: fringe_processing.cpp:436
Euler_rotate
void Euler_rotate(const MultidimArray< double > &V, double rot, double tilt, double psi, MultidimArray< double > &result)
Definition: geometry.cpp:1061
FLT_EPSILON
#define FLT_EPSILON
Definition: geometry.h:33
intersection_unit_cube
double intersection_unit_cube(const Matrix1D< double > &u, const Matrix1D< double > &r)
Definition: geometry.cpp:1190
ACOSD
#define ACOSD(x)
Definition: xmipp_macros.h:338
Matrix1D::initZeros
void initZeros()
Definition: matrix1d.h:592
Euler_angles2matrix< float >
template void Euler_angles2matrix< float >(float, float, float, Matrix2D< float > &, bool)
FitPoint::y
double y
y coordinate
Definition: geometry.h:188
j
#define j
Definition: numerical_recipes.cpp:2493
M3x3_BY_V3x1
#define M3x3_BY_V3x1(a, M, b)
Definition: matrix2d.h:170
fit_point2D::w
double w
Weight of the point in the Least-Squares problem.
Definition: geometry.h:238
YY
#define YY(v)
Definition: matrix1d.h:93
m
int m
Definition: image_find_center.cpp:57
DBL_MAX
#define DBL_MAX
distance
TYPE distance(struct Point_T *p, struct Point_T *q)
Definition: point.cpp:28
error
void error(char *s)
Definition: tools.cpp:107
FINISHINGY
#define FINISHINGY(v)
Definition: multidim_array_base.h:63
Bspline02
double Bspline02(double Argument)
DEFORM_AND_CHOOSE_CORNERS2D
#define DEFORM_AND_CHOOSE_CORNERS2D
Bspline_model::evaluate
double evaluate(double x, double y) const
Evaluate the model at the point (x,y)
Definition: geometry.cpp:1238
Bspline_model::c_ml
MultidimArray< double > c_ml
Definition: geometry.h:302
dotProduct
T dotProduct(const Matrix1D< T > &v1, const Matrix1D< T > &v2)
Definition: matrix1d.h:1140
RAD2DEG
#define RAD2DEG(r)
Definition: xmipp_macros.h:320
MAT_XSIZE
#define MAT_XSIZE(m)
Definition: matrix2d.h:120
Euler_anglesZXZ2matrix
void Euler_anglesZXZ2matrix(double a, double b, double g, Matrix2D< double > &A, bool homogeneous)
Definition: geometry.cpp:661
alglib::psi
double psi(const double x)
Definition: specialfunctions.cpp:2492
Euler_matrix2angles
void Euler_matrix2angles(const Matrix2D< double > &A, double &alpha, double &beta, double &gamma, bool homogeneous)
Definition: geometry.cpp:839
VECTOR_R3
#define VECTOR_R3(v, x, y, z)
Definition: matrix1d.h:124
Matrix2D::initZeros
void initZeros()
Definition: matrix2d.h:626
Bspline09
double Bspline09(double Argument)
u
doublereal * u
Definition: numerical_recipes.cpp:2230
Euler_distanceBetweenAngleSets< float >
template float Euler_distanceBetweenAngleSets< float >(float rot1, float tilt1, float psi1, float rot2, float tilt2, float psi2, bool only_projdir)
K
constexpr int K
Definition: integration.cpp:123
r2
float r2
Definition: image_find_center.cpp:54
Bspline_model::mF
int mF
mF;
Definition: geometry.h:283
MultidimArray::initZeros
void initZeros(const MultidimArray< T1 > &op)
Definition: multidim_array.h:2723
Euler_distanceBetweenAngleSets_fast
T Euler_distanceBetweenAngleSets_fast(const Matrix2D< T > &E1, T rot2, T tilt2, T psi2, bool only_projdir, Matrix2D< T > &E2)
Definition: geometry.cpp:693
PI
#define PI
Definition: tools.h:43
Bspline06
double Bspline06(double Argument)
v0
double v0
Definition: numerical_recipes.cpp:6210
Bspline_model::h_y
double h_y
Scale Y.
Definition: geometry.h:296
Matrix2D::getRow
void getRow(size_t i, Matrix1D< T > &v) const
Definition: matrix2d.cpp:871
Uproject_to_plane
void Uproject_to_plane(const Matrix1D< double > &point, const Matrix1D< double > &direction, double distance, Matrix1D< double > &result)
Definition: geometry.cpp:39
SGN
#define SGN(x)
Definition: xmipp_macros.h:155
n
int * n
Definition: numerical_recipes.cpp:2229
ZZ
#define ZZ(v)
Definition: matrix1d.h:101
a
doublereal * a
Definition: numerical_recipes.cpp:2230
least_squares_plane_fit_All_Points
void least_squares_plane_fit_All_Points(const MultidimArray< double > &Image, double &plane_a, double &plane_b, double &plane_c)
Definition: geometry.cpp:154
SWITCHDATATYPE
#define SWITCHDATATYPE(datatype, OP)
Definition: multidim_array_generic.h:45
SPEED_UP_temps012
#define SPEED_UP_temps012
Definition: xmipp_macros.h:403
r1
float r1
Definition: image_find_center.cpp:54
Bspline_model::m0
int m0
m0
Definition: geometry.h:281
Bspline_model::h_x
double h_x
Scale X.
Definition: geometry.h:294
Generated by   doxygen 1.8.13