Mercurial > hg > orthanc-stone
diff Framework/Toolbox/GeometryToolbox.cpp @ 0:351ab0da0150
initial commit
author | Sebastien Jodogne <s.jodogne@gmail.com> |
---|---|
date | Fri, 14 Oct 2016 15:34:11 +0200 |
parents | |
children | ff1e935768e7 |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Framework/Toolbox/GeometryToolbox.cpp Fri Oct 14 15:34:11 2016 +0200 @@ -0,0 +1,351 @@ +/** + * Stone of Orthanc + * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics + * Department, University Hospital of Liege, Belgium + * + * This program is free software: you can redistribute it and/or + * modify it under the terms of the GNU General Public License as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * In addition, as a special exception, the copyright holders of this + * program give permission to link the code of its release with the + * OpenSSL project's "OpenSSL" library (or with modified versions of it + * that use the same license as the "OpenSSL" library), and distribute + * the linked executables. You must obey the GNU General Public License + * in all respects for all of the code used other than "OpenSSL". If you + * modify file(s) with this exception, you may extend this exception to + * your version of the file(s), but you are not obligated to do so. If + * you do not wish to do so, delete this exception statement from your + * version. If you delete this exception statement from all source files + * in the program, then also delete it here. + * + * 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 + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + **/ + + +#include "GeometryToolbox.h" + +#include "../Orthanc/Core/OrthancException.h" +#include "../Orthanc/Core/Toolbox.h" + +#include <stdio.h> +#include <boost/lexical_cast.hpp> + +namespace OrthancStone +{ + namespace GeometryToolbox + { + void Print(const Vector& v) + { + for (size_t i = 0; i < v.size(); i++) + { + printf("%8.2f\n", v[i]); + } + printf("\n"); + } + + + bool ParseVector(Vector& target, + const std::string& value) + { + std::vector<std::string> items; + Orthanc::Toolbox::TokenizeString(items, value, '\\'); + + target.resize(items.size()); + + for (size_t i = 0; i < items.size(); i++) + { + try + { + target[i] = boost::lexical_cast<double>(Orthanc::Toolbox::StripSpaces(items[i])); + } + catch (boost::bad_lexical_cast&) + { + target.clear(); + return false; + } + } + + return true; + } + + + void AssignVector(Vector& v, + double v1, + double v2) + { + v.resize(2); + v[0] = v1; + v[1] = v2; + } + + + void AssignVector(Vector& v, + double v1, + double v2, + double v3) + { + v.resize(3); + v[0] = v1; + v[1] = v2; + v[2] = v3; + } + + + bool IsNear(double x, + double y) + { + // As most input is read as single-precision numbers, we take the + // epsilon machine for float32 into consideration to compare numbers + return IsNear(x, y, 10.0 * std::numeric_limits<float>::epsilon()); + } + + + void NormalizeVector(Vector& u) + { + double norm = boost::numeric::ublas::norm_2(u); + if (!IsCloseToZero(norm)) + { + u = u / norm; + } + } + + + void CrossProduct(Vector& result, + const Vector& u, + const Vector& v) + { + if (u.size() != 3 || + v.size() != 3) + { + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + + result.resize(3); + + result[0] = u[1] * v[2] - u[2] * v[1]; + result[1] = u[2] * v[0] - u[0] * v[2]; + result[2] = u[0] * v[1] - u[1] * v[0]; + } + + + void ProjectPointOntoPlane(Vector& result, + const Vector& point, + const Vector& planeNormal, + const Vector& planeOrigin) + { + double norm = boost::numeric::ublas::norm_2(planeNormal); + if (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; + } + + + + 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 (IsCloseToZero(normU) || + 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 (IsCloseToZero(cosAngle - 1.0)) + { + isOpposite = false; + return true; + } + else if (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 + CrossProduct(direction, normal1, normal2); + + double norm = boost::numeric::ublas::norm_2(direction); + if (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; + + 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 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 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]; + AssignVector(x[0], xmin, ymin, 1.0); + AssignVector(x[1], xmax, ymin, 1.0); + AssignVector(x[2], xmax, ymax, 1.0); + AssignVector(x[3], xmin, ymax, 1.0); + + // Move to homogoneous coordinates in 2D + Vector p; + + { + Vector a, b; + AssignVector(a, ax, ay, 1.0); + AssignVector(b, bx, by, 1.0); + 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; + CrossProduct(e, x[i], x[(i + 1) % 4]); + CrossProduct(a, p, e); + CrossProduct(e, x[j], x[(j + 1) % 4]); + 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; + } + } + } +}