Mercurial > hg > orthanc-stone
comparison Framework/Toolbox/GeometryToolbox.cpp @ 189:964118e7e6de wasm
unit test for AlignVectorsWithRotation
author | Sebastien Jodogne <s.jodogne@gmail.com> |
---|---|
date | Fri, 16 Mar 2018 13:19:23 +0100 |
parents | 8d50e6be565d |
children | fccffbf99ba1 |
comparison
equal
deleted
inserted
replaced
188:45b03b04a777 | 189:964118e7e6de |
---|---|
413 | 413 |
414 p = origin + t * direction; | 414 p = origin + t * direction; |
415 return true; | 415 return true; |
416 } | 416 } |
417 } | 417 } |
418 | |
419 | |
420 void AlignVectorsWithRotation(Matrix& r, | |
421 const Vector& a, | |
422 const Vector& b) | |
423 { | |
424 // This is Rodrigues' rotation formula: | |
425 // https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula#Matrix_notation | |
426 | |
427 // Check also result A4.6 from "Multiple View Geometry in Computer | |
428 // Vision - 2nd edition" (p. 584) | |
429 | |
430 if (a.size() != 3 || | |
431 b.size() != 3) | |
432 { | |
433 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
434 } | |
435 | |
436 double aNorm = boost::numeric::ublas::norm_2(a); | |
437 double bNorm = boost::numeric::ublas::norm_2(b); | |
438 | |
439 if (LinearAlgebra::IsCloseToZero(aNorm) || | |
440 LinearAlgebra::IsCloseToZero(bNorm)) | |
441 { | |
442 LOG(ERROR) << "Vector with zero norm"; | |
443 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
444 } | |
445 | |
446 Vector aUnit, bUnit; | |
447 aUnit = a / aNorm; | |
448 bUnit = b / bNorm; | |
449 | |
450 Vector v; | |
451 LinearAlgebra::CrossProduct(v, aUnit, bUnit); | |
452 | |
453 double cosine = boost::numeric::ublas::inner_prod(aUnit, bUnit); | |
454 | |
455 if (LinearAlgebra::IsCloseToZero(1 + cosine)) | |
456 { | |
457 // "a == -b": TODO | |
458 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented); | |
459 } | |
460 | |
461 Matrix k; | |
462 LinearAlgebra::CreateSkewSymmetric(k, v); | |
463 | |
464 #if 0 | |
465 double sine = boost::numeric::ublas::norm_2(v); | |
466 | |
467 r = (boost::numeric::ublas::identity_matrix<double>(3) + | |
468 sine * k + | |
469 (1 - cosine) * boost::numeric::ublas::prod(k, k)); | |
470 #else | |
471 r = (boost::numeric::ublas::identity_matrix<double>(3) + | |
472 k + | |
473 boost::numeric::ublas::prod(k, k) / (1 + cosine)); | |
474 #endif | |
475 } | |
418 } | 476 } |
419 } | 477 } |