Mercurial > hg > orthanc-stone
diff 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 |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/OrthancStone/Sources/Toolbox/GeometryToolbox.cpp Tue Jul 07 16:21:02 2020 +0200 @@ -0,0 +1,572 @@ +/** + * Stone of Orthanc + * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics + * Department, University Hospital of Liege, Belgium + * Copyright (C) 2017-2020 Osimis S.A., Belgium + * + * This program is free software: you can redistribute it and/or + * modify it under the terms of the GNU Affero General Public License + * as published by the Free Software Foundation, either version 3 of + * the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + **/ + + +#include "GeometryToolbox.h" + +#include <Logging.h> +#include <OrthancException.h> + +#include <cassert> + +namespace OrthancStone +{ + namespace GeometryToolbox + { + void ProjectPointOntoPlane(Vector& result, + const Vector& point, + const Vector& planeNormal, + const Vector& planeOrigin) + { + double norm = boost::numeric::ublas::norm_2(planeNormal); + if (LinearAlgebra::IsCloseToZero(norm)) + { + // Division by zero + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + + // Make sure the norm of the normal is 1 + Vector n; + n = planeNormal / norm; + + // Algebraic form of line–plane intersection, where the line passes + // through "point" along the direction "normal" (thus, l == n) + // https://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection#Algebraic_form + result = boost::numeric::ublas::inner_prod(planeOrigin - point, n) * n + point; + } + + /* + undefined results if vector are not 3D + */ + void ProjectPointOntoPlane2( + double& resultX, + double& resultY, + double& resultZ, + const Vector& point, + const Vector& planeNormal, + const Vector& planeOrigin) + { + double pointX = point[0]; + double pointY = point[1]; + double pointZ = point[2]; + + double planeNormalX = planeNormal[0]; + double planeNormalY = planeNormal[1]; + double planeNormalZ = planeNormal[2]; + + double planeOriginX = planeOrigin[0]; + double planeOriginY = planeOrigin[1]; + double planeOriginZ = planeOrigin[2]; + + double normSq = (planeNormalX * planeNormalX) + (planeNormalY * planeNormalY) + (planeNormalZ * planeNormalZ); + + // Algebraic form of line–plane intersection, where the line passes + // through "point" along the direction "normal" (thus, l == n) + // https://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection#Algebraic_form + + if (LinearAlgebra::IsNear(1.0, normSq)) + { + double nX = planeNormalX; + double nY = planeNormalY; + double nZ = planeNormalZ; + + double prod = (planeOriginX - pointX) * nX + (planeOriginY - pointY) * nY + (planeOriginZ - pointZ) * nZ; + + resultX = prod * nX + pointX; + resultY = prod * nY + pointY; + resultZ = prod * nZ + pointZ; + } + else + { + double norm = sqrt(normSq); + if (LinearAlgebra::IsCloseToZero(norm)) + { + // Division by zero + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + double invNorm = 1.0 / norm; + double nX = planeNormalX * invNorm; + double nY = planeNormalY * invNorm; + double nZ = planeNormalZ * invNorm; + + double prod = (planeOriginX - pointX) * nX + (planeOriginY - pointY) * nY + (planeOriginZ - pointZ) * nZ; + + resultX = prod * nX + pointX; + resultY = prod * nY + pointY; + resultZ = prod * nZ + pointZ; + } + } + + bool IsParallelOrOpposite(bool& isOpposite, + const Vector& u, + const Vector& v) + { + // The dot product of the two vectors gives the cosine of the angle + // between the vectors + // https://en.wikipedia.org/wiki/Dot_product + + double normU = boost::numeric::ublas::norm_2(u); + double normV = boost::numeric::ublas::norm_2(v); + + if (LinearAlgebra::IsCloseToZero(normU) || + LinearAlgebra::IsCloseToZero(normV)) + { + return false; + } + + double cosAngle = boost::numeric::ublas::inner_prod(u, v) / (normU * normV); + + // The angle must be zero, so the cosine must be almost equal to + // cos(0) == 1 (or cos(180) == -1 if allowOppositeDirection == true) + + if (LinearAlgebra::IsCloseToZero(cosAngle - 1.0)) + { + isOpposite = false; + return true; + } + else if (LinearAlgebra::IsCloseToZero(fabs(cosAngle) - 1.0)) + { + isOpposite = true; + return true; + } + else + { + return false; + } + } + + + bool IsParallel(const Vector& u, + const Vector& v) + { + bool isOpposite; + return (IsParallelOrOpposite(isOpposite, u, v) && + !isOpposite); + } + + + bool IntersectTwoPlanes(Vector& p, + Vector& direction, + const Vector& origin1, + const Vector& normal1, + const Vector& origin2, + const Vector& normal2) + { + // This is "Intersection of 2 Planes", possibility "(C) 3 Plane + // Intersect Point" of: + // http://geomalgorithms.com/a05-_intersect-1.html + + // The direction of the line of intersection is orthogonal to the + // normal of both planes + LinearAlgebra::CrossProduct(direction, normal1, normal2); + + double norm = boost::numeric::ublas::norm_2(direction); + if (LinearAlgebra::IsCloseToZero(norm)) + { + // The two planes are parallel or coincident + return false; + } + + double d1 = -boost::numeric::ublas::inner_prod(normal1, origin1); + double d2 = -boost::numeric::ublas::inner_prod(normal2, origin2); + Vector tmp = d2 * normal1 - d1 * normal2; + + LinearAlgebra::CrossProduct(p, tmp, direction); + p /= norm; + + return true; + } + + + bool ClipLineToRectangle(double& x1, // Coordinates of the clipped line (out) + double& y1, + double& x2, + double& y2, + const double ax, // Two points defining the line (in) + const double ay, + const double bx, + const double by, + const double& xmin, // Coordinates of the rectangle (in) + const double& ymin, + const double& xmax, + const double& ymax) + { + // This is Skala algorithm for rectangles, "A new approach to line + // and line segment clipping in homogeneous coordinates" + // (2005). This is a direct, non-optimized translation of Algorithm + // 2 in the paper. + + static const uint8_t tab1[16] = { 255 /* none */, + 0, + 0, + 1, + 1, + 255 /* na */, + 0, + 2, + 2, + 0, + 255 /* na */, + 1, + 1, + 0, + 0, + 255 /* none */ }; + + + static const uint8_t tab2[16] = { 255 /* none */, + 3, + 1, + 3, + 2, + 255 /* na */, + 2, + 3, + 3, + 2, + 255 /* na */, + 2, + 3, + 1, + 3, + 255 /* none */ }; + + // Create the coordinates of the rectangle + Vector x[4]; + LinearAlgebra::AssignVector(x[0], xmin, ymin, 1.0); + LinearAlgebra::AssignVector(x[1], xmax, ymin, 1.0); + LinearAlgebra::AssignVector(x[2], xmax, ymax, 1.0); + LinearAlgebra::AssignVector(x[3], xmin, ymax, 1.0); + + // Move to homogoneous coordinates in 2D + Vector p; + + { + Vector a, b; + LinearAlgebra::AssignVector(a, ax, ay, 1.0); + LinearAlgebra::AssignVector(b, bx, by, 1.0); + LinearAlgebra::CrossProduct(p, a, b); + } + + uint8_t c = 0; + + for (unsigned int k = 0; k < 4; k++) + { + if (boost::numeric::ublas::inner_prod(p, x[k]) >= 0) + { + c |= (1 << k); + } + } + + assert(c < 16); + + uint8_t i = tab1[c]; + uint8_t j = tab2[c]; + + if (i == 255 || j == 255) + { + return false; // No intersection + } + else + { + Vector a, b, e; + LinearAlgebra::CrossProduct(e, x[i], x[(i + 1) % 4]); + LinearAlgebra::CrossProduct(a, p, e); + LinearAlgebra::CrossProduct(e, x[j], x[(j + 1) % 4]); + LinearAlgebra::CrossProduct(b, p, e); + + // Go back to non-homogeneous coordinates + x1 = a[0] / a[2]; + y1 = a[1] / a[2]; + x2 = b[0] / b[2]; + y2 = b[1] / b[2]; + + return true; + } + } + + + void GetPixelSpacing(double& spacingX, + double& spacingY, + const Orthanc::DicomMap& dicom) + { + Vector v; + + if (LinearAlgebra::ParseVector(v, dicom, Orthanc::DICOM_TAG_PIXEL_SPACING)) + { + if (v.size() != 2 || + v[0] <= 0 || + v[1] <= 0) + { + LOG(ERROR) << "Bad value for PixelSpacing tag"; + throw Orthanc::OrthancException(Orthanc::ErrorCode_BadFileFormat); + } + else + { + // WARNING: X/Y are swapped (Y comes first) + spacingX = v[1]; + spacingY = v[0]; + } + } + else + { + // The "PixelSpacing" is of type 1C: It could be absent, use + // default value in such a case + spacingX = 1; + spacingY = 1; + } + } + + + Matrix CreateRotationMatrixAlongX(double a) + { + // Rotate along X axis (R_x) + // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations + Matrix r(3, 3); + r(0,0) = 1; + r(0,1) = 0; + r(0,2) = 0; + r(1,0) = 0; + r(1,1) = cos(a); + r(1,2) = -sin(a); + r(2,0) = 0; + r(2,1) = sin(a); + r(2,2) = cos(a); + return r; + } + + + Matrix CreateRotationMatrixAlongY(double a) + { + // Rotate along Y axis (R_y) + // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations + Matrix r(3, 3); + r(0,0) = cos(a); + r(0,1) = 0; + r(0,2) = sin(a); + r(1,0) = 0; + r(1,1) = 1; + r(1,2) = 0; + r(2,0) = -sin(a); + r(2,1) = 0; + r(2,2) = cos(a); + return r; + } + + + Matrix CreateRotationMatrixAlongZ(double a) + { + // Rotate along Z axis (R_z) + // https://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations + Matrix r(3, 3); + r(0,0) = cos(a); + r(0,1) = -sin(a); + r(0,2) = 0; + r(1,0) = sin(a); + r(1,1) = cos(a); + r(1,2) = 0; + r(2,0) = 0; + r(2,1) = 0; + r(2,2) = 1; + return r; + } + + + Matrix CreateTranslationMatrix(double dx, + double dy, + double dz) + { + Matrix m = LinearAlgebra::IdentityMatrix(4); + m(0,3) = dx; + m(1,3) = dy; + m(2,3) = dz; + return m; + } + + + Matrix CreateScalingMatrix(double sx, + double sy, + double sz) + { + Matrix m = LinearAlgebra::IdentityMatrix(4); + m(0,0) = sx; + m(1,1) = sy; + m(2,2) = sz; + return m; + } + + + bool IntersectPlaneAndSegment(Vector& p, + const Vector& normal, + double d, + const Vector& edgeFrom, + const Vector& edgeTo) + { + // http://geomalgorithms.com/a05-_intersect-1.html#Line-Plane-Intersection + + // Check for parallel line and plane + Vector direction = edgeTo - edgeFrom; + double denominator = boost::numeric::ublas::inner_prod(direction, normal); + + if (fabs(denominator) < 100.0 * std::numeric_limits<double>::epsilon()) + { + return false; + } + else + { + // Compute intersection + double t = -(normal[0] * edgeFrom[0] + + normal[1] * edgeFrom[1] + + normal[2] * edgeFrom[2] + d) / denominator; + + if (t >= 0 && t <= 1) + { + // The intersection lies inside edge segment + p = edgeFrom + t * direction; + return true; + } + else + { + return false; + } + } + } + + + bool IntersectPlaneAndLine(Vector& p, + const Vector& normal, + double d, + const Vector& origin, + const Vector& direction) + { + // http://geomalgorithms.com/a05-_intersect-1.html#Line-Plane-Intersection + + // Check for parallel line and plane + double denominator = boost::numeric::ublas::inner_prod(direction, normal); + + if (fabs(denominator) < 100.0 * std::numeric_limits<double>::epsilon()) + { + return false; + } + else + { + // Compute intersection + double t = -(normal[0] * origin[0] + + normal[1] * origin[1] + + normal[2] * origin[2] + d) / denominator; + + p = origin + t * direction; + return true; + } + } + + + void AlignVectorsWithRotation(Matrix& r, + const Vector& a, + const Vector& b) + { + // This is Rodrigues' rotation formula: + // https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula#Matrix_notation + + // Check also result A4.6 from "Multiple View Geometry in Computer + // Vision - 2nd edition" (p. 584) + + if (a.size() != 3 || + b.size() != 3) + { + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + + double aNorm = boost::numeric::ublas::norm_2(a); + double bNorm = boost::numeric::ublas::norm_2(b); + + if (LinearAlgebra::IsCloseToZero(aNorm) || + LinearAlgebra::IsCloseToZero(bNorm)) + { + LOG(ERROR) << "Vector with zero norm"; + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + + Vector aUnit, bUnit; + aUnit = a / aNorm; + bUnit = b / bNorm; + + Vector v; + LinearAlgebra::CrossProduct(v, aUnit, bUnit); + + double cosine = boost::numeric::ublas::inner_prod(aUnit, bUnit); + + if (LinearAlgebra::IsCloseToZero(1 + cosine)) + { + // "a == -b": TODO + throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented); + } + + Matrix k; + LinearAlgebra::CreateSkewSymmetric(k, v); + +#if 0 + double sine = boost::numeric::ublas::norm_2(v); + + r = (boost::numeric::ublas::identity_matrix<double>(3) + + sine * k + + (1 - cosine) * boost::numeric::ublas::prod(k, k)); +#else + r = (boost::numeric::ublas::identity_matrix<double>(3) + + k + + boost::numeric::ublas::prod(k, k) / (1 + cosine)); +#endif + } + + + void ComputeNormalFromCosines(Vector& normal, + const Vector& cosines) + { + if (cosines.size() != 6) + { + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + else + { + normal.resize(3); + normal[0] = cosines[1] * cosines[5] - cosines[2] * cosines[4]; + normal[1] = cosines[2] * cosines[3] - cosines[0] * cosines[5]; + normal[2] = cosines[0] * cosines[4] - cosines[1] * cosines[3]; + } + } + + + bool ComputeNormal(Vector& normal, + const Orthanc::DicomMap& dicom) + { + Vector cosines; + if (LinearAlgebra::ParseVector(cosines, dicom, Orthanc::DICOM_TAG_IMAGE_ORIENTATION_PATIENT) && + cosines.size() == 6) + { + ComputeNormalFromCosines(normal, cosines); + return true; + } + else + { + return false; + } + } + } +}