Mercurial > hg > orthanc-stone

comparison Framework/Toolbox/GeometryToolbox.cpp @ 158:a053ca7fa5c6 wasm

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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 }