Mercurial > hg > orthanc-stone
comparison Framework/Toolbox/GeometryToolbox.cpp @ 158:a053ca7fa5c6 wasm
LinearAlgebra toolbox
author | Sebastien Jodogne <s.jodogne@gmail.com> |
---|---|
date | Wed, 14 Feb 2018 08:58:31 +0100 |
parents | 2309e8d86efe |
children | 8d50e6be565d |
comparison
equal
deleted
inserted
replaced
157:2309e8d86efe | 158:a053ca7fa5c6 |
---|---|
21 | 21 |
22 #include "GeometryToolbox.h" | 22 #include "GeometryToolbox.h" |
23 | 23 |
24 #include <Core/Logging.h> | 24 #include <Core/Logging.h> |
25 #include <Core/OrthancException.h> | 25 #include <Core/OrthancException.h> |
26 #include <Core/Toolbox.h> | 26 |
27 | 27 #include <cassert> |
28 #include <stdio.h> | |
29 #include <boost/lexical_cast.hpp> | |
30 | 28 |
31 namespace OrthancStone | 29 namespace OrthancStone |
32 { | 30 { |
33 namespace GeometryToolbox | 31 namespace GeometryToolbox |
34 { | 32 { |
35 void Print(const Vector& v) | |
36 { | |
37 for (size_t i = 0; i < v.size(); i++) | |
38 { | |
39 printf("%g\n", v[i]); | |
40 //printf("%8.2f\n", v[i]); | |
41 } | |
42 printf("\n"); | |
43 } | |
44 | |
45 | |
46 void Print(const Matrix& m) | |
47 { | |
48 for (size_t i = 0; i < m.size1(); i++) | |
49 { | |
50 for (size_t j = 0; j < m.size2(); j++) | |
51 { | |
52 printf("%g ", m(i,j)); | |
53 //printf("%8.2f ", m(i,j)); | |
54 } | |
55 printf("\n"); | |
56 } | |
57 printf("\n"); | |
58 } | |
59 | |
60 | |
61 bool ParseVector(Vector& target, | |
62 const std::string& value) | |
63 { | |
64 std::vector<std::string> items; | |
65 Orthanc::Toolbox::TokenizeString(items, value, '\\'); | |
66 | |
67 target.resize(items.size()); | |
68 | |
69 for (size_t i = 0; i < items.size(); i++) | |
70 { | |
71 try | |
72 { | |
73 target[i] = boost::lexical_cast<double>(Orthanc::Toolbox::StripSpaces(items[i])); | |
74 } | |
75 catch (boost::bad_lexical_cast&) | |
76 { | |
77 target.clear(); | |
78 return false; | |
79 } | |
80 } | |
81 | |
82 return true; | |
83 } | |
84 | |
85 | |
86 bool ParseVector(Vector& target, | |
87 const Orthanc::DicomMap& dataset, | |
88 const Orthanc::DicomTag& tag) | |
89 { | |
90 std::string value; | |
91 return (dataset.CopyToString(value, tag, false) && | |
92 ParseVector(target, value)); | |
93 } | |
94 | |
95 | |
96 void AssignVector(Vector& v, | |
97 double v1, | |
98 double v2) | |
99 { | |
100 v.resize(2); | |
101 v[0] = v1; | |
102 v[1] = v2; | |
103 } | |
104 | |
105 | |
106 void AssignVector(Vector& v, | |
107 double v1, | |
108 double v2, | |
109 double v3) | |
110 { | |
111 v.resize(3); | |
112 v[0] = v1; | |
113 v[1] = v2; | |
114 v[2] = v3; | |
115 } | |
116 | |
117 | |
118 bool IsNear(double x, | |
119 double y) | |
120 { | |
121 // As most input is read as single-precision numbers, we take the | |
122 // epsilon machine for float32 into consideration to compare numbers | |
123 return IsNear(x, y, 10.0 * std::numeric_limits<float>::epsilon()); | |
124 } | |
125 | |
126 | |
127 void NormalizeVector(Vector& u) | |
128 { | |
129 double norm = boost::numeric::ublas::norm_2(u); | |
130 if (!IsCloseToZero(norm)) | |
131 { | |
132 u = u / norm; | |
133 } | |
134 } | |
135 | |
136 | |
137 void CrossProduct(Vector& result, | |
138 const Vector& u, | |
139 const Vector& v) | |
140 { | |
141 if (u.size() != 3 || | |
142 v.size() != 3) | |
143 { | |
144 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
145 } | |
146 | |
147 result.resize(3); | |
148 | |
149 result[0] = u[1] * v[2] - u[2] * v[1]; | |
150 result[1] = u[2] * v[0] - u[0] * v[2]; | |
151 result[2] = u[0] * v[1] - u[1] * v[0]; | |
152 } | |
153 | |
154 | |
155 void ProjectPointOntoPlane(Vector& result, | 33 void ProjectPointOntoPlane(Vector& result, |
156 const Vector& point, | 34 const Vector& point, |
157 const Vector& planeNormal, | 35 const Vector& planeNormal, |
158 const Vector& planeOrigin) | 36 const Vector& planeOrigin) |
159 { | 37 { |
160 double norm = boost::numeric::ublas::norm_2(planeNormal); | 38 double norm = boost::numeric::ublas::norm_2(planeNormal); |
161 if (IsCloseToZero(norm)) | 39 if (LinearAlgebra::IsCloseToZero(norm)) |
162 { | 40 { |
163 // Division by zero | 41 // Division by zero |
164 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | 42 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); |
165 } | 43 } |
166 | 44 |
185 // https://en.wikipedia.org/wiki/Dot_product | 63 // https://en.wikipedia.org/wiki/Dot_product |
186 | 64 |
187 double normU = boost::numeric::ublas::norm_2(u); | 65 double normU = boost::numeric::ublas::norm_2(u); |
188 double normV = boost::numeric::ublas::norm_2(v); | 66 double normV = boost::numeric::ublas::norm_2(v); |
189 | 67 |
190 if (IsCloseToZero(normU) || | 68 if (LinearAlgebra::IsCloseToZero(normU) || |
191 IsCloseToZero(normV)) | 69 LinearAlgebra::IsCloseToZero(normV)) |
192 { | 70 { |
193 return false; | 71 return false; |
194 } | 72 } |
195 | 73 |
196 double cosAngle = boost::numeric::ublas::inner_prod(u, v) / (normU * normV); | 74 double cosAngle = boost::numeric::ublas::inner_prod(u, v) / (normU * normV); |
197 | 75 |
198 // The angle must be zero, so the cosine must be almost equal to | 76 // The angle must be zero, so the cosine must be almost equal to |
199 // cos(0) == 1 (or cos(180) == -1 if allowOppositeDirection == true) | 77 // cos(0) == 1 (or cos(180) == -1 if allowOppositeDirection == true) |
200 | 78 |
201 if (IsCloseToZero(cosAngle - 1.0)) | 79 if (LinearAlgebra::IsCloseToZero(cosAngle - 1.0)) |
202 { | 80 { |
203 isOpposite = false; | 81 isOpposite = false; |
204 return true; | 82 return true; |
205 } | 83 } |
206 else if (IsCloseToZero(fabs(cosAngle) - 1.0)) | 84 else if (LinearAlgebra::IsCloseToZero(fabs(cosAngle) - 1.0)) |
207 { | 85 { |
208 isOpposite = true; | 86 isOpposite = true; |
209 return true; | 87 return true; |
210 } | 88 } |
211 else | 89 else |
235 // Intersect Point" of: | 113 // Intersect Point" of: |
236 // http://geomalgorithms.com/a05-_intersect-1.html | 114 // http://geomalgorithms.com/a05-_intersect-1.html |
237 | 115 |
238 // The direction of the line of intersection is orthogonal to the | 116 // The direction of the line of intersection is orthogonal to the |
239 // normal of both planes | 117 // normal of both planes |
240 CrossProduct(direction, normal1, normal2); | 118 LinearAlgebra::CrossProduct(direction, normal1, normal2); |
241 | 119 |
242 double norm = boost::numeric::ublas::norm_2(direction); | 120 double norm = boost::numeric::ublas::norm_2(direction); |
243 if (IsCloseToZero(norm)) | 121 if (LinearAlgebra::IsCloseToZero(norm)) |
244 { | 122 { |
245 // The two planes are parallel or coincident | 123 // The two planes are parallel or coincident |
246 return false; | 124 return false; |
247 } | 125 } |
248 | 126 |
249 double d1 = -boost::numeric::ublas::inner_prod(normal1, origin1); | 127 double d1 = -boost::numeric::ublas::inner_prod(normal1, origin1); |
250 double d2 = -boost::numeric::ublas::inner_prod(normal2, origin2); | 128 double d2 = -boost::numeric::ublas::inner_prod(normal2, origin2); |
251 Vector tmp = d2 * normal1 - d1 * normal2; | 129 Vector tmp = d2 * normal1 - d1 * normal2; |
252 | 130 |
253 CrossProduct(p, tmp, direction); | 131 LinearAlgebra::CrossProduct(p, tmp, direction); |
254 p /= norm; | 132 p /= norm; |
255 | 133 |
256 return true; | 134 return true; |
257 } | 135 } |
258 | 136 |
310 3, | 188 3, |
311 255 /* none */ }; | 189 255 /* none */ }; |
312 | 190 |
313 // Create the coordinates of the rectangle | 191 // Create the coordinates of the rectangle |
314 Vector x[4]; | 192 Vector x[4]; |
315 AssignVector(x[0], xmin, ymin, 1.0); | 193 LinearAlgebra::AssignVector(x[0], xmin, ymin, 1.0); |
316 AssignVector(x[1], xmax, ymin, 1.0); | 194 LinearAlgebra::AssignVector(x[1], xmax, ymin, 1.0); |
317 AssignVector(x[2], xmax, ymax, 1.0); | 195 LinearAlgebra::AssignVector(x[2], xmax, ymax, 1.0); |
318 AssignVector(x[3], xmin, ymax, 1.0); | 196 LinearAlgebra::AssignVector(x[3], xmin, ymax, 1.0); |
319 | 197 |
320 // Move to homogoneous coordinates in 2D | 198 // Move to homogoneous coordinates in 2D |
321 Vector p; | 199 Vector p; |
322 | 200 |
323 { | 201 { |
324 Vector a, b; | 202 Vector a, b; |
325 AssignVector(a, ax, ay, 1.0); | 203 LinearAlgebra::AssignVector(a, ax, ay, 1.0); |
326 AssignVector(b, bx, by, 1.0); | 204 LinearAlgebra::AssignVector(b, bx, by, 1.0); |
327 CrossProduct(p, a, b); | 205 LinearAlgebra::CrossProduct(p, a, b); |
328 } | 206 } |
329 | 207 |
330 uint8_t c = 0; | 208 uint8_t c = 0; |
331 | 209 |
332 for (unsigned int k = 0; k < 4; k++) | 210 for (unsigned int k = 0; k < 4; k++) |
347 return false; // No intersection | 225 return false; // No intersection |
348 } | 226 } |
349 else | 227 else |
350 { | 228 { |
351 Vector a, b, e; | 229 Vector a, b, e; |
352 CrossProduct(e, x[i], x[(i + 1) % 4]); | 230 LinearAlgebra::CrossProduct(e, x[i], x[(i + 1) % 4]); |
353 CrossProduct(a, p, e); | 231 LinearAlgebra::CrossProduct(a, p, e); |
354 CrossProduct(e, x[j], x[(j + 1) % 4]); | 232 LinearAlgebra::CrossProduct(e, x[j], x[(j + 1) % 4]); |
355 CrossProduct(b, p, e); | 233 LinearAlgebra::CrossProduct(b, p, e); |
356 | 234 |
357 // Go back to non-homogeneous coordinates | 235 // Go back to non-homogeneous coordinates |
358 x1 = a[0] / a[2]; | 236 x1 = a[0] / a[2]; |
359 y1 = a[1] / a[2]; | 237 y1 = a[1] / a[2]; |
360 x2 = b[0] / b[2]; | 238 x2 = b[0] / b[2]; |
369 double& spacingY, | 247 double& spacingY, |
370 const Orthanc::DicomMap& dicom) | 248 const Orthanc::DicomMap& dicom) |
371 { | 249 { |
372 Vector v; | 250 Vector v; |
373 | 251 |
374 if (ParseVector(v, dicom, Orthanc::DICOM_TAG_PIXEL_SPACING)) | 252 if (LinearAlgebra::ParseVector(v, dicom, Orthanc::DICOM_TAG_PIXEL_SPACING)) |
375 { | 253 { |
376 if (v.size() != 2 || | 254 if (v.size() != 2 || |
377 v[0] <= 0 || | 255 v[0] <= 0 || |
378 v[1] <= 0) | 256 v[1] <= 0) |
379 { | 257 { |
511 | 389 |
512 p = origin + t * direction; | 390 p = origin + t * direction; |
513 return true; | 391 return true; |
514 } | 392 } |
515 } | 393 } |
516 | |
517 | |
518 void FillMatrix(Matrix& target, | |
519 size_t rows, | |
520 size_t columns, | |
521 const double values[]) | |
522 { | |
523 target.resize(rows, columns); | |
524 | |
525 size_t index = 0; | |
526 | |
527 for (size_t y = 0; y < rows; y++) | |
528 { | |
529 for (size_t x = 0; x < columns; x++, index++) | |
530 { | |
531 target(y, x) = values[index]; | |
532 } | |
533 } | |
534 } | |
535 | |
536 | |
537 void FillVector(Vector& target, | |
538 size_t size, | |
539 const double values[]) | |
540 { | |
541 target.resize(size); | |
542 | |
543 for (size_t i = 0; i < size; i++) | |
544 { | |
545 target[i] = values[i]; | |
546 } | |
547 } | |
548 | |
549 | |
550 void Convert(Matrix& target, | |
551 const Vector& source) | |
552 { | |
553 const size_t n = source.size(); | |
554 | |
555 target.resize(n, 1); | |
556 | |
557 for (size_t i = 0; i < n; i++) | |
558 { | |
559 target(i, 0) = source[i]; | |
560 } | |
561 } | |
562 } | 394 } |
563 } | 395 } |