Mercurial > hg > orthanc-stone
comparison OrthancStone/Sources/Toolbox/LinearAlgebra.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/LinearAlgebra.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 "LinearAlgebra.h" | |
23 | |
24 #include "../StoneException.h" | |
25 #include "GenericToolbox.h" | |
26 | |
27 #include <Logging.h> | |
28 #include <OrthancException.h> | |
29 #include <Toolbox.h> | |
30 | |
31 #include <boost/lexical_cast.hpp> | |
32 #include <boost/numeric/ublas/lu.hpp> | |
33 | |
34 #include <stdio.h> | |
35 #include <iostream> | |
36 #include <cstdlib> | |
37 | |
38 namespace OrthancStone | |
39 { | |
40 namespace LinearAlgebra | |
41 { | |
42 void Print(const Vector& v) | |
43 { | |
44 for (size_t i = 0; i < v.size(); i++) | |
45 { | |
46 printf("%g\n", v[i]); | |
47 //printf("%8.2f\n", v[i]); | |
48 } | |
49 printf("\n"); | |
50 } | |
51 | |
52 | |
53 void Print(const Matrix& m) | |
54 { | |
55 for (size_t i = 0; i < m.size1(); i++) | |
56 { | |
57 for (size_t j = 0; j < m.size2(); j++) | |
58 { | |
59 printf("%g ", m(i,j)); | |
60 //printf("%8.2f ", m(i,j)); | |
61 } | |
62 printf("\n"); | |
63 } | |
64 printf("\n"); | |
65 } | |
66 | |
67 | |
68 bool ParseVector(Vector& target, | |
69 const std::string& value) | |
70 { | |
71 std::vector<std::string> items; | |
72 Orthanc::Toolbox::TokenizeString(items, Orthanc::Toolbox::StripSpaces(value), '\\'); | |
73 | |
74 target.resize(items.size()); | |
75 | |
76 for (size_t i = 0; i < items.size(); i++) | |
77 { | |
78 /** | |
79 * SJO - 2019-11-19 - WARNING: I reverted from "std::stod()" | |
80 * to "boost::lexical_cast", as both "std::stod()" and | |
81 * "std::strtod()" are sensitive to locale settings, making | |
82 * this code non portable and very dangerous as it fails | |
83 * silently. A string such as "1.3671875\1.3671875" is | |
84 * interpreted as "1\1", because "std::stod()" expects a comma | |
85 * (",") instead of a point ("."). This problem is notably | |
86 * seen in Qt-based applications, that somehow set locales | |
87 * aggressively. | |
88 * | |
89 * "boost::lexical_cast<>" is also dependent on the locale | |
90 * settings, but apparently not in a way that makes this | |
91 * function fail with Qt. The Orthanc core defines macro | |
92 * "-DBOOST_LEXICAL_CAST_ASSUME_C_LOCALE" in static builds to | |
93 * this end. | |
94 **/ | |
95 | |
96 #if 0 // __cplusplus >= 201103L // Is C++11 enabled? | |
97 /** | |
98 * We try and avoid the use of "boost::lexical_cast<>" here, | |
99 * as it is very slow, and as Stone has to parse many doubles. | |
100 * https://tinodidriksen.com/2011/05/cpp-convert-string-to-double-speed/ | |
101 **/ | |
102 | |
103 try | |
104 { | |
105 target[i] = std::stod(items[i]); | |
106 } | |
107 catch (std::exception&) | |
108 { | |
109 target.clear(); | |
110 return false; | |
111 } | |
112 | |
113 #elif 0 | |
114 /** | |
115 * "std::strtod()" is the recommended alternative to | |
116 * "std::stod()". It is apparently as fast as plain-C | |
117 * "atof()", with more security. | |
118 **/ | |
119 char* end = NULL; | |
120 target[i] = std::strtod(items[i].c_str(), &end); | |
121 if (end == NULL || | |
122 end != items[i].c_str() + items[i].size()) | |
123 { | |
124 return false; | |
125 } | |
126 | |
127 #elif 1 | |
128 /** | |
129 * Use of our homemade implementation of | |
130 * "boost::lexical_cast<double>()". It is much faster than boost. | |
131 **/ | |
132 if (!GenericToolbox::StringToDouble(target[i], items[i].c_str())) | |
133 { | |
134 return false; | |
135 } | |
136 | |
137 #else | |
138 /** | |
139 * Fallback implementation using Boost (slower, but somehow | |
140 * independent to locale contrarily to "std::stod()", and | |
141 * generic as it does not use our custom implementation). | |
142 **/ | |
143 try | |
144 { | |
145 target[i] = boost::lexical_cast<double>(items[i]); | |
146 } | |
147 catch (boost::bad_lexical_cast&) | |
148 { | |
149 target.clear(); | |
150 return false; | |
151 } | |
152 #endif | |
153 } | |
154 | |
155 return true; | |
156 } | |
157 | |
158 | |
159 bool ParseVector(Vector& target, | |
160 const Orthanc::DicomMap& dataset, | |
161 const Orthanc::DicomTag& tag) | |
162 { | |
163 std::string value; | |
164 return (dataset.LookupStringValue(value, tag, false) && | |
165 ParseVector(target, value)); | |
166 } | |
167 | |
168 | |
169 void NormalizeVector(Vector& u) | |
170 { | |
171 double norm = boost::numeric::ublas::norm_2(u); | |
172 if (!IsCloseToZero(norm)) | |
173 { | |
174 u = u / norm; | |
175 } | |
176 } | |
177 | |
178 void CrossProduct(Vector& result, | |
179 const Vector& u, | |
180 const Vector& v) | |
181 { | |
182 if (u.size() != 3 || | |
183 v.size() != 3) | |
184 { | |
185 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
186 } | |
187 | |
188 result.resize(3); | |
189 | |
190 result[0] = u[1] * v[2] - u[2] * v[1]; | |
191 result[1] = u[2] * v[0] - u[0] * v[2]; | |
192 result[2] = u[0] * v[1] - u[1] * v[0]; | |
193 } | |
194 | |
195 double DotProduct(const Vector& u, const Vector& v) | |
196 { | |
197 if (u.size() != 3 || | |
198 v.size() != 3) | |
199 { | |
200 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
201 } | |
202 | |
203 return (u[0] * v[0] + u[1] * v[1] + u[2] * v[2]); | |
204 } | |
205 | |
206 void FillMatrix(Matrix& target, | |
207 size_t rows, | |
208 size_t columns, | |
209 const double values[]) | |
210 { | |
211 target.resize(rows, columns); | |
212 | |
213 size_t index = 0; | |
214 | |
215 for (size_t y = 0; y < rows; y++) | |
216 { | |
217 for (size_t x = 0; x < columns; x++, index++) | |
218 { | |
219 target(y, x) = values[index]; | |
220 } | |
221 } | |
222 } | |
223 | |
224 | |
225 void FillVector(Vector& target, | |
226 size_t size, | |
227 const double values[]) | |
228 { | |
229 target.resize(size); | |
230 | |
231 for (size_t i = 0; i < size; i++) | |
232 { | |
233 target[i] = values[i]; | |
234 } | |
235 } | |
236 | |
237 | |
238 void Convert(Matrix& target, | |
239 const Vector& source) | |
240 { | |
241 const size_t n = source.size(); | |
242 | |
243 target.resize(n, 1); | |
244 | |
245 for (size_t i = 0; i < n; i++) | |
246 { | |
247 target(i, 0) = source[i]; | |
248 } | |
249 } | |
250 | |
251 | |
252 double ComputeDeterminant(const Matrix& a) | |
253 { | |
254 if (a.size1() != a.size2()) | |
255 { | |
256 LOG(ERROR) << "Determinant only exists for square matrices"; | |
257 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
258 } | |
259 | |
260 // https://en.wikipedia.org/wiki/Rule_of_Sarrus | |
261 if (a.size1() == 1) | |
262 { | |
263 return a(0,0); | |
264 } | |
265 else if (a.size1() == 2) | |
266 { | |
267 return a(0,0) * a(1,1) - a(0,1) * a(1,0); | |
268 } | |
269 else if (a.size1() == 3) | |
270 { | |
271 return (a(0,0) * a(1,1) * a(2,2) + | |
272 a(0,1) * a(1,2) * a(2,0) + | |
273 a(0,2) * a(1,0) * a(2,1) - | |
274 a(2,0) * a(1,1) * a(0,2) - | |
275 a(2,1) * a(1,2) * a(0,0) - | |
276 a(2,2) * a(1,0) * a(0,1)); | |
277 } | |
278 else | |
279 { | |
280 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented); | |
281 } | |
282 } | |
283 | |
284 | |
285 bool IsOrthogonalMatrix(const Matrix& q, | |
286 double threshold) | |
287 { | |
288 // https://en.wikipedia.org/wiki/Orthogonal_matrix | |
289 | |
290 if (q.size1() != q.size2()) | |
291 { | |
292 LOG(ERROR) << "An orthogonal matrix must be squared"; | |
293 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
294 } | |
295 | |
296 using namespace boost::numeric::ublas; | |
297 | |
298 const Matrix check = prod(trans(q), q) - identity_matrix<double>(q.size1()); | |
299 | |
300 type_traits<double>::real_type norm = norm_inf(check); | |
301 | |
302 return (norm <= threshold); | |
303 } | |
304 | |
305 | |
306 bool IsOrthogonalMatrix(const Matrix& q) | |
307 { | |
308 return IsOrthogonalMatrix(q, 10.0 * std::numeric_limits<float>::epsilon()); | |
309 } | |
310 | |
311 | |
312 bool IsRotationMatrix(const Matrix& r, | |
313 double threshold) | |
314 { | |
315 return (IsOrthogonalMatrix(r, threshold) && | |
316 IsNear(ComputeDeterminant(r), 1.0, threshold)); | |
317 } | |
318 | |
319 | |
320 bool IsRotationMatrix(const Matrix& r) | |
321 { | |
322 return IsRotationMatrix(r, 10.0 * std::numeric_limits<float>::epsilon()); | |
323 } | |
324 | |
325 | |
326 void InvertUpperTriangularMatrix(Matrix& output, | |
327 const Matrix& k) | |
328 { | |
329 if (k.size1() != k.size2()) | |
330 { | |
331 LOG(ERROR) << "Determinant only exists for square matrices"; | |
332 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
333 } | |
334 | |
335 output.resize(k.size1(), k.size2()); | |
336 | |
337 for (size_t i = 1; i < k.size1(); i++) | |
338 { | |
339 for (size_t j = 0; j < i; j++) | |
340 { | |
341 if (!IsCloseToZero(k(i, j))) | |
342 { | |
343 LOG(ERROR) << "Not an upper triangular matrix"; | |
344 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
345 } | |
346 | |
347 output(i, j) = 0; // The output is also upper triangular | |
348 } | |
349 } | |
350 | |
351 if (k.size1() == 3) | |
352 { | |
353 // https://math.stackexchange.com/a/1004181 | |
354 double a = k(0, 0); | |
355 double b = k(0, 1); | |
356 double c = k(0, 2); | |
357 double d = k(1, 1); | |
358 double e = k(1, 2); | |
359 double f = k(2, 2); | |
360 | |
361 if (IsCloseToZero(a) || | |
362 IsCloseToZero(d) || | |
363 IsCloseToZero(f)) | |
364 { | |
365 LOG(ERROR) << "Singular upper triangular matrix"; | |
366 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
367 } | |
368 else | |
369 { | |
370 output(0, 0) = 1.0 / a; | |
371 output(0, 1) = -b / (a * d); | |
372 output(0, 2) = (b * e - c * d) / (a * f * d); | |
373 output(1, 1) = 1.0 / d; | |
374 output(1, 2) = -e / (f * d); | |
375 output(2, 2) = 1.0 / f; | |
376 } | |
377 } | |
378 else | |
379 { | |
380 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented); | |
381 } | |
382 } | |
383 | |
384 | |
385 static void GetGivensComponent(double& c, | |
386 double& s, | |
387 const Matrix& a, | |
388 size_t i, | |
389 size_t j) | |
390 { | |
391 assert(i < 3 && j < 3); | |
392 | |
393 double x = a(i, i); | |
394 double y = a(i, j); | |
395 double n = sqrt(x * x + y * y); | |
396 | |
397 if (IsCloseToZero(n)) | |
398 { | |
399 c = 1; | |
400 s = 0; | |
401 } | |
402 else | |
403 { | |
404 c = x / n; | |
405 s = -y / n; | |
406 } | |
407 } | |
408 | |
409 | |
410 /** | |
411 * This function computes the RQ decomposition of a 3x3 matrix, | |
412 * using Givens rotations. Reference: Algorithm A4.1 (page 579) of | |
413 * "Multiple View Geometry in Computer Vision" (2nd edition). The | |
414 * output matrix "Q" is a rotation matrix, and "R" is upper | |
415 * triangular. | |
416 **/ | |
417 void RQDecomposition3x3(Matrix& r, | |
418 Matrix& q, | |
419 const Matrix& a) | |
420 { | |
421 using namespace boost::numeric::ublas; | |
422 | |
423 if (a.size1() != 3 || | |
424 a.size2() != 3) | |
425 { | |
426 LOG(ERROR) << "Only applicable to a 3x3 matrix"; | |
427 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
428 } | |
429 | |
430 r.resize(3, 3); | |
431 q.resize(3, 3); | |
432 | |
433 r = a; | |
434 q = identity_matrix<double>(3); | |
435 | |
436 { | |
437 // Set A(2,1) to zero | |
438 double c, s; | |
439 GetGivensComponent(c, s, r, 2, 1); | |
440 | |
441 double v[9] = { 1, 0, 0, | |
442 0, c, -s, | |
443 0, s, c }; | |
444 | |
445 Matrix g; | |
446 FillMatrix(g, 3, 3, v); | |
447 | |
448 r = prod(r, g); | |
449 q = prod(trans(g), q); | |
450 } | |
451 | |
452 | |
453 { | |
454 // Set A(2,0) to zero | |
455 double c, s; | |
456 GetGivensComponent(c, s, r, 2, 0); | |
457 | |
458 double v[9] = { c, 0, -s, | |
459 0, 1, 0, | |
460 s, 0, c }; | |
461 | |
462 Matrix g; | |
463 FillMatrix(g, 3, 3, v); | |
464 | |
465 r = prod(r, g); | |
466 q = prod(trans(g), q); | |
467 } | |
468 | |
469 | |
470 { | |
471 // Set A(1,0) to zero | |
472 double c, s; | |
473 GetGivensComponent(c, s, r, 1, 0); | |
474 | |
475 double v[9] = { c, -s, 0, | |
476 s, c, 0, | |
477 0, 0, 1 }; | |
478 | |
479 Matrix g; | |
480 FillMatrix(g, 3, 3, v); | |
481 | |
482 r = prod(r, g); | |
483 q = prod(trans(g), q); | |
484 } | |
485 | |
486 if (!IsCloseToZero(norm_inf(prod(r, q) - a)) || | |
487 !IsRotationMatrix(q) || | |
488 !IsCloseToZero(r(1, 0)) || | |
489 !IsCloseToZero(r(2, 0)) || | |
490 !IsCloseToZero(r(2, 1))) | |
491 { | |
492 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); | |
493 } | |
494 } | |
495 | |
496 | |
497 bool InvertMatrixUnsafe(Matrix& target, | |
498 const Matrix& source) | |
499 { | |
500 if (source.size1() != source.size2()) | |
501 { | |
502 LOG(ERROR) << "Inverse only exists for square matrices"; | |
503 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
504 } | |
505 | |
506 if (source.size1() < 4) | |
507 { | |
508 // For matrices with size below 4, use direct computations | |
509 // instead of LU decomposition | |
510 | |
511 if (source.size1() == 0) | |
512 { | |
513 // By convention, the inverse of the empty matrix, is itself the empty matrix | |
514 target.resize(0, 0); | |
515 return true; | |
516 } | |
517 | |
518 double determinant = ComputeDeterminant(source); | |
519 | |
520 if (IsCloseToZero(determinant)) | |
521 { | |
522 return false; | |
523 } | |
524 | |
525 double denominator = 1.0 / determinant; | |
526 | |
527 target.resize(source.size1(), source.size2()); | |
528 | |
529 if (source.size1() == 1) | |
530 { | |
531 target(0, 0) = denominator; | |
532 | |
533 return true; | |
534 } | |
535 else if (source.size1() == 2) | |
536 { | |
537 // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_2_%C3%97_2_matrices | |
538 target(0, 0) = source(1, 1) * denominator; | |
539 target(0, 1) = -source(0, 1) * denominator; | |
540 target(1, 0) = -source(1, 0) * denominator; | |
541 target(1, 1) = source(0, 0) * denominator; | |
542 | |
543 return true; | |
544 } | |
545 else if (source.size1() == 3) | |
546 { | |
547 // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices | |
548 const double a = source(0, 0); | |
549 const double b = source(0, 1); | |
550 const double c = source(0, 2); | |
551 const double d = source(1, 0); | |
552 const double e = source(1, 1); | |
553 const double f = source(1, 2); | |
554 const double g = source(2, 0); | |
555 const double h = source(2, 1); | |
556 const double i = source(2, 2); | |
557 | |
558 target(0, 0) = (e * i - f * h) * denominator; | |
559 target(0, 1) = -(b * i - c * h) * denominator; | |
560 target(0, 2) = (b * f - c * e) * denominator; | |
561 target(1, 0) = -(d * i - f * g) * denominator; | |
562 target(1, 1) = (a * i - c * g) * denominator; | |
563 target(1, 2) = -(a * f - c * d) * denominator; | |
564 target(2, 0) = (d * h - e * g) * denominator; | |
565 target(2, 1) = -(a * h - b * g) * denominator; | |
566 target(2, 2) = (a * e - b * d) * denominator; | |
567 | |
568 return true; | |
569 } | |
570 else | |
571 { | |
572 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); | |
573 } | |
574 } | |
575 else | |
576 { | |
577 // General case, using LU decomposition | |
578 | |
579 Matrix a = source; // Copy the source matrix, as "lu_factorize()" modifies it | |
580 | |
581 boost::numeric::ublas::permutation_matrix<size_t> permutation(source.size1()); | |
582 | |
583 if (boost::numeric::ublas::lu_factorize(a, permutation) != 0) | |
584 { | |
585 return false; | |
586 } | |
587 else | |
588 { | |
589 target = boost::numeric::ublas::identity_matrix<double>(source.size1()); | |
590 lu_substitute(a, permutation, target); | |
591 return true; | |
592 } | |
593 } | |
594 } | |
595 | |
596 | |
597 | |
598 void InvertMatrix(Matrix& target, | |
599 const Matrix& source) | |
600 { | |
601 if (!InvertMatrixUnsafe(target, source)) | |
602 { | |
603 LOG(ERROR) << "Cannot invert singular matrix"; | |
604 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
605 } | |
606 } | |
607 | |
608 | |
609 void CreateSkewSymmetric(Matrix& s, | |
610 const Vector& v) | |
611 { | |
612 if (v.size() != 3) | |
613 { | |
614 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
615 } | |
616 | |
617 s.resize(3, 3); | |
618 s(0,0) = 0; | |
619 s(0,1) = -v[2]; | |
620 s(0,2) = v[1]; | |
621 s(1,0) = v[2]; | |
622 s(1,1) = 0; | |
623 s(1,2) = -v[0]; | |
624 s(2,0) = -v[1]; | |
625 s(2,1) = v[0]; | |
626 s(2,2) = 0; | |
627 } | |
628 | |
629 | |
630 Matrix InvertScalingTranslationMatrix(const Matrix& t) | |
631 { | |
632 if (t.size1() != 4 || | |
633 t.size2() != 4 || | |
634 !LinearAlgebra::IsCloseToZero(t(0,1)) || | |
635 !LinearAlgebra::IsCloseToZero(t(0,2)) || | |
636 !LinearAlgebra::IsCloseToZero(t(1,0)) || | |
637 !LinearAlgebra::IsCloseToZero(t(1,2)) || | |
638 !LinearAlgebra::IsCloseToZero(t(2,0)) || | |
639 !LinearAlgebra::IsCloseToZero(t(2,1)) || | |
640 !LinearAlgebra::IsCloseToZero(t(3,0)) || | |
641 !LinearAlgebra::IsCloseToZero(t(3,1)) || | |
642 !LinearAlgebra::IsCloseToZero(t(3,2))) | |
643 { | |
644 LOG(ERROR) << "This matrix is more than a zoom/translate transform"; | |
645 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); | |
646 } | |
647 | |
648 const double sx = t(0,0); | |
649 const double sy = t(1,1); | |
650 const double sz = t(2,2); | |
651 const double w = t(3,3); | |
652 | |
653 if (LinearAlgebra::IsCloseToZero(sx) || | |
654 LinearAlgebra::IsCloseToZero(sy) || | |
655 LinearAlgebra::IsCloseToZero(sz) || | |
656 LinearAlgebra::IsCloseToZero(w)) | |
657 { | |
658 LOG(ERROR) << "Singular transform"; | |
659 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); | |
660 } | |
661 | |
662 const double tx = t(0,3); | |
663 const double ty = t(1,3); | |
664 const double tz = t(2,3); | |
665 | |
666 Matrix m = IdentityMatrix(4); | |
667 | |
668 m(0,0) = 1.0 / sx; | |
669 m(1,1) = 1.0 / sy; | |
670 m(2,2) = 1.0 / sz; | |
671 m(3,3) = 1.0 / w; | |
672 | |
673 m(0,3) = -tx / (sx * w); | |
674 m(1,3) = -ty / (sy * w); | |
675 m(2,3) = -tz / (sz * w); | |
676 | |
677 return m; | |
678 } | |
679 | |
680 | |
681 bool IsShearMatrix(const Matrix& shear) | |
682 { | |
683 return (shear.size1() == 4 && | |
684 shear.size2() == 4 && | |
685 LinearAlgebra::IsNear(1.0, shear(0,0)) && | |
686 LinearAlgebra::IsNear(0.0, shear(0,1)) && | |
687 LinearAlgebra::IsNear(0.0, shear(0,3)) && | |
688 LinearAlgebra::IsNear(0.0, shear(1,0)) && | |
689 LinearAlgebra::IsNear(1.0, shear(1,1)) && | |
690 LinearAlgebra::IsNear(0.0, shear(1,3)) && | |
691 LinearAlgebra::IsNear(0.0, shear(2,0)) && | |
692 LinearAlgebra::IsNear(0.0, shear(2,1)) && | |
693 LinearAlgebra::IsNear(1.0, shear(2,2)) && | |
694 LinearAlgebra::IsNear(0.0, shear(2,3)) && | |
695 LinearAlgebra::IsNear(0.0, shear(3,0)) && | |
696 LinearAlgebra::IsNear(0.0, shear(3,1)) && | |
697 LinearAlgebra::IsNear(1.0, shear(3,3))); | |
698 } | |
699 | |
700 | |
701 Matrix InvertShearMatrix(const Matrix& shear) | |
702 { | |
703 if (!IsShearMatrix(shear)) | |
704 { | |
705 LOG(ERROR) << "Not a valid shear matrix"; | |
706 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); | |
707 } | |
708 | |
709 Matrix m = IdentityMatrix(4); | |
710 m(0,2) = -shear(0,2); | |
711 m(1,2) = -shear(1,2); | |
712 m(3,2) = -shear(3,2); | |
713 | |
714 return m; | |
715 } | |
716 } | |
717 | |
718 std::ostream& operator<<(std::ostream& s, const Vector& vec) | |
719 { | |
720 s << "("; | |
721 for (size_t i = 0; i < vec.size(); ++i) | |
722 { | |
723 s << vec(i); | |
724 if (i < (vec.size() - 1)) | |
725 s << ", "; | |
726 } | |
727 s << ")"; | |
728 return s; | |
729 } | |
730 | |
731 std::ostream& operator<<(std::ostream& s, const Matrix& m) | |
732 { | |
733 ORTHANC_ASSERT(m.size1() == m.size2()); | |
734 s << "("; | |
735 for (size_t i = 0; i < m.size1(); ++i) | |
736 { | |
737 s << "("; | |
738 for (size_t j = 0; j < m.size2(); ++j) | |
739 { | |
740 s << m(i,j); | |
741 if (j < (m.size2() - 1)) | |
742 s << ", "; | |
743 } | |
744 s << ")"; | |
745 if (i < (m.size1() - 1)) | |
746 s << ", "; | |
747 } | |
748 s << ")"; | |
749 return s; | |
750 } | |
751 } |