Mercurial > hg > orthanc-stone
comparison OrthancStone/Sources/Toolbox/GeometryToolbox.cpp @ 1512:244ad1e4e76a
reorganization of folders
author | Sebastien Jodogne <s.jodogne@gmail.com> |
---|---|
date | Tue, 07 Jul 2020 16:21:02 +0200 |
parents | Framework/Toolbox/GeometryToolbox.cpp@5732edec7cbd |
children | 4fb8fdf03314 |
comparison
equal
deleted
inserted
replaced
1511:9dfeee74c1e6 | 1512:244ad1e4e76a |
---|---|
1 /** | |
2 * Stone of Orthanc | |
3 * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics | |
4 * Department, University Hospital of Liege, Belgium | |
5 * Copyright (C) 2017-2020 Osimis S.A., Belgium | |
6 * | |
7 * This program is free software: you can redistribute it and/or | |
8 * modify it under the terms of the GNU Affero General Public License | |
9 * as published by the Free Software Foundation, either version 3 of | |
10 * the License, or (at your option) any later version. | |
11 * | |
12 * This program is distributed in the hope that it will be useful, but | |
13 * WITHOUT ANY WARRANTY; without even the implied warranty of | |
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU | |
15 * Affero General Public License for more details. | |
16 * | |
17 * You should have received a copy of the GNU Affero General Public License | |
18 * along with this program. If not, see <http://www.gnu.org/licenses/>. | |
19 **/ | |
20 | |
21 | |
22 #include "GeometryToolbox.h" | |
23 | |
24 #include <Logging.h> | |
25 #include <OrthancException.h> | |
26 | |
27 #include <cassert> | |
28 | |
29 namespace OrthancStone | |
30 { | |
31 namespace GeometryToolbox | |
32 { | |
33 void ProjectPointOntoPlane(Vector& result, | |
34 const Vector& point, | |
35 const Vector& planeNormal, | |
36 const Vector& planeOrigin) | |
37 { | |
38 double norm = boost::numeric::ublas::norm_2(planeNormal); | |
39 if (LinearAlgebra::IsCloseToZero(norm)) | |
40 { | |
41 // Division by zero | |
42 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
43 } | |
44 | |
45 // Make sure the norm of the normal is 1 | |
46 Vector n; | |
47 n = planeNormal / norm; | |
48 | |
49 // Algebraic form of line–plane intersection, where the line passes | |
50 // through "point" along the direction "normal" (thus, l == n) | |
51 // https://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection#Algebraic_form | |
52 result = boost::numeric::ublas::inner_prod(planeOrigin - point, n) * n + point; | |
53 } | |
54 | |
55 /* | |
56 undefined results if vector are not 3D | |
57 */ | |
58 void ProjectPointOntoPlane2( | |
59 double& resultX, | |
60 double& resultY, | |
61 double& resultZ, | |
62 const Vector& point, | |
63 const Vector& planeNormal, | |
64 const Vector& planeOrigin) | |
65 { | |
66 double pointX = point[0]; | |
67 double pointY = point[1]; | |
68 double pointZ = point[2]; | |
69 | |
70 double planeNormalX = planeNormal[0]; | |
71 double planeNormalY = planeNormal[1]; | |
72 double planeNormalZ = planeNormal[2]; | |
73 | |
74 double planeOriginX = planeOrigin[0]; | |
75 double planeOriginY = planeOrigin[1]; | |
76 double planeOriginZ = planeOrigin[2]; | |
77 | |
78 double normSq = (planeNormalX * planeNormalX) + (planeNormalY * planeNormalY) + (planeNormalZ * planeNormalZ); | |
79 | |
80 // Algebraic form of line–plane intersection, where the line passes | |
81 // through "point" along the direction "normal" (thus, l == n) | |
82 // https://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection#Algebraic_form | |
83 | |
84 if (LinearAlgebra::IsNear(1.0, normSq)) | |
85 { | |
86 double nX = planeNormalX; | |
87 double nY = planeNormalY; | |
88 double nZ = planeNormalZ; | |
89 | |
90 double prod = (planeOriginX - pointX) * nX + (planeOriginY - pointY) * nY + (planeOriginZ - pointZ) * nZ; | |
91 | |
92 resultX = prod * nX + pointX; | |
93 resultY = prod * nY + pointY; | |
94 resultZ = prod * nZ + pointZ; | |
95 } | |
96 else | |
97 { | |
98 double norm = sqrt(normSq); | |
99 if (LinearAlgebra::IsCloseToZero(norm)) | |
100 { | |
101 // Division by zero | |
102 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
103 } | |
104 double invNorm = 1.0 / norm; | |
105 double nX = planeNormalX * invNorm; | |
106 double nY = planeNormalY * invNorm; | |
107 double nZ = planeNormalZ * invNorm; | |
108 | |
109 double prod = (planeOriginX - pointX) * nX + (planeOriginY - pointY) * nY + (planeOriginZ - pointZ) * nZ; | |
110 | |
111 resultX = prod * nX + pointX; | |
112 resultY = prod * nY + pointY; | |
113 resultZ = prod * nZ + pointZ; | |
114 } | |
115 } | |
116 | |
117 bool IsParallelOrOpposite(bool& isOpposite, | |
118 const Vector& u, | |
119 const Vector& v) | |
120 { | |
121 // The dot product of the two vectors gives the cosine of the angle | |
122 // between the vectors | |
123 // https://en.wikipedia.org/wiki/Dot_product | |
124 | |
125 double normU = boost::numeric::ublas::norm_2(u); | |
126 double normV = boost::numeric::ublas::norm_2(v); | |
127 | |
128 if (LinearAlgebra::IsCloseToZero(normU) || | |
129 LinearAlgebra::IsCloseToZero(normV)) | |
130 { | |
131 return false; | |
132 } | |
133 | |
134 double cosAngle = boost::numeric::ublas::inner_prod(u, v) / (normU * normV); | |
135 | |
136 // The angle must be zero, so the cosine must be almost equal to | |
137 // cos(0) == 1 (or cos(180) == -1 if allowOppositeDirection == true) | |
138 | |
139 if (LinearAlgebra::IsCloseToZero(cosAngle - 1.0)) | |
140 { | |
141 isOpposite = false; | |
142 return true; | |
143 } | |
144 else if (LinearAlgebra::IsCloseToZero(fabs(cosAngle) - 1.0)) | |
145 { | |
146 isOpposite = true; | |
147 return true; | |
148 } | |
149 else | |
150 { | |
151 return false; | |
152 } | |
153 } | |
154 | |
155 | |
156 bool IsParallel(const Vector& u, | |
157 const Vector& v) | |
158 { | |
159 bool isOpposite; | |
160 return (IsParallelOrOpposite(isOpposite, u, v) && | |
161 !isOpposite); | |
162 } | |
163 | |
164 | |
165 bool IntersectTwoPlanes(Vector& p, | |
166 Vector& direction, | |
167 const Vector& origin1, | |
168 const Vector& normal1, | |
169 const Vector& origin2, | |
170 const Vector& normal2) | |
171 { | |
172 // This is "Intersection of 2 Planes", possibility "(C) 3 Plane | |
173 // Intersect Point" of: | |
174 // http://geomalgorithms.com/a05-_intersect-1.html | |
175 | |
176 // The direction of the line of intersection is orthogonal to the | |
177 // normal of both planes | |
178 LinearAlgebra::CrossProduct(direction, normal1, normal2); | |
179 | |
180 double norm = boost::numeric::ublas::norm_2(direction); | |
181 if (LinearAlgebra::IsCloseToZero(norm)) | |
182 { | |
183 // The two planes are parallel or coincident | |
184 return false; | |
185 } | |
186 | |
187 double d1 = -boost::numeric::ublas::inner_prod(normal1, origin1); | |
188 double d2 = -boost::numeric::ublas::inner_prod(normal2, origin2); | |
189 Vector tmp = d2 * normal1 - d1 * normal2; | |
190 | |
191 LinearAlgebra::CrossProduct(p, tmp, direction); | |
192 p /= norm; | |
193 | |
194 return true; | |
195 } | |
196 | |
197 | |
198 bool ClipLineToRectangle(double& x1, // Coordinates of the clipped line (out) | |
199 double& y1, | |
200 double& x2, | |
201 double& y2, | |
202 const double ax, // Two points defining the line (in) | |
203 const double ay, | |
204 const double bx, | |
205 const double by, | |
206 const double& xmin, // Coordinates of the rectangle (in) | |
207 const double& ymin, | |
208 const double& xmax, | |
209 const double& ymax) | |
210 { | |
211 // This is Skala algorithm for rectangles, "A new approach to line | |
212 // and line segment clipping in homogeneous coordinates" | |
213 // (2005). This is a direct, non-optimized translation of Algorithm | |
214 // 2 in the paper. | |
215 | |
216 static const uint8_t tab1[16] = { 255 /* none */, | |
217 0, | |
218 0, | |
219 1, | |
220 1, | |
221 255 /* na */, | |
222 0, | |
223 2, | |
224 2, | |
225 0, | |
226 255 /* na */, | |
227 1, | |
228 1, | |
229 0, | |
230 0, | |
231 255 /* none */ }; | |
232 | |
233 | |
234 static const uint8_t tab2[16] = { 255 /* none */, | |
235 3, | |
236 1, | |
237 3, | |
238 2, | |
239 255 /* na */, | |
240 2, | |
241 3, | |
242 3, | |
243 2, | |
244 255 /* na */, | |
245 2, | |
246 3, | |
247 1, | |
248 3, | |
249 255 /* none */ }; | |
250 | |
251 // Create the coordinates of the rectangle | |
252 Vector x[4]; | |
253 LinearAlgebra::AssignVector(x[0], xmin, ymin, 1.0); | |
254 LinearAlgebra::AssignVector(x[1], xmax, ymin, 1.0); | |
255 LinearAlgebra::AssignVector(x[2], xmax, ymax, 1.0); | |
256 LinearAlgebra::AssignVector(x[3], xmin, ymax, 1.0); | |
257 | |
258 // Move to homogoneous coordinates in 2D | |
259 Vector p; | |
260 | |
261 { | |
262 Vector a, b; | |
263 LinearAlgebra::AssignVector(a, ax, ay, 1.0); | |
264 LinearAlgebra::AssignVector(b, bx, by, 1.0); | |
265 LinearAlgebra::CrossProduct(p, a, b); | |
266 } | |
267 | |
268 uint8_t c = 0; | |
269 | |
270 for (unsigned int k = 0; k < 4; k++) | |
271 { | |
272 if (boost::numeric::ublas::inner_prod(p, x[k]) >= 0) | |
273 { | |
274 c |= (1 << k); | |
275 } | |
276 } | |
277 | |
278 assert(c < 16); | |
279 | |
280 uint8_t i = tab1[c]; | |
281 uint8_t j = tab2[c]; | |
282 | |
283 if (i == 255 || j == 255) | |
284 { | |
285 return false; // No intersection | |
286 } | |
287 else | |
288 { | |
289 Vector a, b, e; | |
290 LinearAlgebra::CrossProduct(e, x[i], x[(i + 1) % 4]); | |
291 LinearAlgebra::CrossProduct(a, p, e); | |
292 LinearAlgebra::CrossProduct(e, x[j], x[(j + 1) % 4]); | |
293 LinearAlgebra::CrossProduct(b, p, e); | |
294 | |
295 // Go back to non-homogeneous coordinates | |
296 x1 = a[0] / a[2]; | |
297 y1 = a[1] / a[2]; | |
298 x2 = b[0] / b[2]; | |
299 y2 = b[1] / b[2]; | |
300 | |
301 return true; | |
302 } | |
303 } | |
304 | |
305 | |
306 void GetPixelSpacing(double& spacingX, | |
307 double& spacingY, | |
308 const Orthanc::DicomMap& dicom) | |
309 { | |
310 Vector v; | |
311 | |
312 if (LinearAlgebra::ParseVector(v, dicom, Orthanc::DICOM_TAG_PIXEL_SPACING)) | |
313 { | |
314 if (v.size() != 2 || | |
315 v[0] <= 0 || | |
316 v[1] <= 0) | |
317 { | |
318 LOG(ERROR) << "Bad value for PixelSpacing tag"; | |
319 throw Orthanc::OrthancException(Orthanc::ErrorCode_BadFileFormat); | |
320 } | |
321 else | |
322 { | |
323 // WARNING: X/Y are swapped (Y comes first) | |
324 spacingX = v[1]; | |
325 spacingY = v[0]; | |
326 } | |
327 } | |
328 else | |
329 { | |
330 // The "PixelSpacing" is of type 1C: It could be absent, use | |
331 // default value in such a case | |
332 spacingX = 1; | |
333 spacingY = 1; | |
334 } | |
335 } | |
336 | |
337 | |
338 Matrix CreateRotationMatrixAlongX(double a) | |
339 { | |
340 // Rotate along X axis (R_x) | |
341 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations | |
342 Matrix r(3, 3); | |
343 r(0,0) = 1; | |
344 r(0,1) = 0; | |
345 r(0,2) = 0; | |
346 r(1,0) = 0; | |
347 r(1,1) = cos(a); | |
348 r(1,2) = -sin(a); | |
349 r(2,0) = 0; | |
350 r(2,1) = sin(a); | |
351 r(2,2) = cos(a); | |
352 return r; | |
353 } | |
354 | |
355 | |
356 Matrix CreateRotationMatrixAlongY(double a) | |
357 { | |
358 // Rotate along Y axis (R_y) | |
359 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations | |
360 Matrix r(3, 3); | |
361 r(0,0) = cos(a); | |
362 r(0,1) = 0; | |
363 r(0,2) = sin(a); | |
364 r(1,0) = 0; | |
365 r(1,1) = 1; | |
366 r(1,2) = 0; | |
367 r(2,0) = -sin(a); | |
368 r(2,1) = 0; | |
369 r(2,2) = cos(a); | |
370 return r; | |
371 } | |
372 | |
373 | |
374 Matrix CreateRotationMatrixAlongZ(double a) | |
375 { | |
376 // Rotate along Z axis (R_z) | |
377 // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations | |
378 Matrix r(3, 3); | |
379 r(0,0) = cos(a); | |
380 r(0,1) = -sin(a); | |
381 r(0,2) = 0; | |
382 r(1,0) = sin(a); | |
383 r(1,1) = cos(a); | |
384 r(1,2) = 0; | |
385 r(2,0) = 0; | |
386 r(2,1) = 0; | |
387 r(2,2) = 1; | |
388 return r; | |
389 } | |
390 | |
391 | |
392 Matrix CreateTranslationMatrix(double dx, | |
393 double dy, | |
394 double dz) | |
395 { | |
396 Matrix m = LinearAlgebra::IdentityMatrix(4); | |
397 m(0,3) = dx; | |
398 m(1,3) = dy; | |
399 m(2,3) = dz; | |
400 return m; | |
401 } | |
402 | |
403 | |
404 Matrix CreateScalingMatrix(double sx, | |
405 double sy, | |
406 double sz) | |
407 { | |
408 Matrix m = LinearAlgebra::IdentityMatrix(4); | |
409 m(0,0) = sx; | |
410 m(1,1) = sy; | |
411 m(2,2) = sz; | |
412 return m; | |
413 } | |
414 | |
415 | |
416 bool IntersectPlaneAndSegment(Vector& p, | |
417 const Vector& normal, | |
418 double d, | |
419 const Vector& edgeFrom, | |
420 const Vector& edgeTo) | |
421 { | |
422 // http://geomalgorithms.com/a05-_intersect-1.html#Line-Plane-Intersection | |
423 | |
424 // Check for parallel line and plane | |
425 Vector direction = edgeTo - edgeFrom; | |
426 double denominator = boost::numeric::ublas::inner_prod(direction, normal); | |
427 | |
428 if (fabs(denominator) < 100.0 * std::numeric_limits<double>::epsilon()) | |
429 { | |
430 return false; | |
431 } | |
432 else | |
433 { | |
434 // Compute intersection | |
435 double t = -(normal[0] * edgeFrom[0] + | |
436 normal[1] * edgeFrom[1] + | |
437 normal[2] * edgeFrom[2] + d) / denominator; | |
438 | |
439 if (t >= 0 && t <= 1) | |
440 { | |
441 // The intersection lies inside edge segment | |
442 p = edgeFrom + t * direction; | |
443 return true; | |
444 } | |
445 else | |
446 { | |
447 return false; | |
448 } | |
449 } | |
450 } | |
451 | |
452 | |
453 bool IntersectPlaneAndLine(Vector& p, | |
454 const Vector& normal, | |
455 double d, | |
456 const Vector& origin, | |
457 const Vector& direction) | |
458 { | |
459 // http://geomalgorithms.com/a05-_intersect-1.html#Line-Plane-Intersection | |
460 | |
461 // Check for parallel line and plane | |
462 double denominator = boost::numeric::ublas::inner_prod(direction, normal); | |
463 | |
464 if (fabs(denominator) < 100.0 * std::numeric_limits<double>::epsilon()) | |
465 { | |
466 return false; | |
467 } | |
468 else | |
469 { | |
470 // Compute intersection | |
471 double t = -(normal[0] * origin[0] + | |
472 normal[1] * origin[1] + | |
473 normal[2] * origin[2] + d) / denominator; | |
474 | |
475 p = origin + t * direction; | |
476 return true; | |
477 } | |
478 } | |
479 | |
480 | |
481 void AlignVectorsWithRotation(Matrix& r, | |
482 const Vector& a, | |
483 const Vector& b) | |
484 { | |
485 // This is Rodrigues' rotation formula: | |
486 // https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula#Matrix_notation | |
487 | |
488 // Check also result A4.6 from "Multiple View Geometry in Computer | |
489 // Vision - 2nd edition" (p. 584) | |
490 | |
491 if (a.size() != 3 || | |
492 b.size() != 3) | |
493 { | |
494 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
495 } | |
496 | |
497 double aNorm = boost::numeric::ublas::norm_2(a); | |
498 double bNorm = boost::numeric::ublas::norm_2(b); | |
499 | |
500 if (LinearAlgebra::IsCloseToZero(aNorm) || | |
501 LinearAlgebra::IsCloseToZero(bNorm)) | |
502 { | |
503 LOG(ERROR) << "Vector with zero norm"; | |
504 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
505 } | |
506 | |
507 Vector aUnit, bUnit; | |
508 aUnit = a / aNorm; | |
509 bUnit = b / bNorm; | |
510 | |
511 Vector v; | |
512 LinearAlgebra::CrossProduct(v, aUnit, bUnit); | |
513 | |
514 double cosine = boost::numeric::ublas::inner_prod(aUnit, bUnit); | |
515 | |
516 if (LinearAlgebra::IsCloseToZero(1 + cosine)) | |
517 { | |
518 // "a == -b": TODO | |
519 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented); | |
520 } | |
521 | |
522 Matrix k; | |
523 LinearAlgebra::CreateSkewSymmetric(k, v); | |
524 | |
525 #if 0 | |
526 double sine = boost::numeric::ublas::norm_2(v); | |
527 | |
528 r = (boost::numeric::ublas::identity_matrix<double>(3) + | |
529 sine * k + | |
530 (1 - cosine) * boost::numeric::ublas::prod(k, k)); | |
531 #else | |
532 r = (boost::numeric::ublas::identity_matrix<double>(3) + | |
533 k + | |
534 boost::numeric::ublas::prod(k, k) / (1 + cosine)); | |
535 #endif | |
536 } | |
537 | |
538 | |
539 void ComputeNormalFromCosines(Vector& normal, | |
540 const Vector& cosines) | |
541 { | |
542 if (cosines.size() != 6) | |
543 { | |
544 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
545 } | |
546 else | |
547 { | |
548 normal.resize(3); | |
549 normal[0] = cosines[1] * cosines[5] - cosines[2] * cosines[4]; | |
550 normal[1] = cosines[2] * cosines[3] - cosines[0] * cosines[5]; | |
551 normal[2] = cosines[0] * cosines[4] - cosines[1] * cosines[3]; | |
552 } | |
553 } | |
554 | |
555 | |
556 bool ComputeNormal(Vector& normal, | |
557 const Orthanc::DicomMap& dicom) | |
558 { | |
559 Vector cosines; | |
560 if (LinearAlgebra::ParseVector(cosines, dicom, Orthanc::DICOM_TAG_IMAGE_ORIENTATION_PATIENT) && | |
561 cosines.size() == 6) | |
562 { | |
563 ComputeNormalFromCosines(normal, cosines); | |
564 return true; | |
565 } | |
566 else | |
567 { | |
568 return false; | |
569 } | |
570 } | |
571 } | |
572 } |