diff --git a/libs/MeshKernel/include/MeshKernel/Entities.hpp b/libs/MeshKernel/include/MeshKernel/Entities.hpp index 7b526199c..04492a651 100644 --- a/libs/MeshKernel/include/MeshKernel/Entities.hpp +++ b/libs/MeshKernel/include/MeshKernel/Entities.hpp @@ -124,35 +124,11 @@ namespace meshkernel /// @brief Inplace multiply point by scalar. Point& operator*=(const double p); - /// @brief Overloads addition with another Point - [[nodiscard]] Point operator+(Point const& rhs) const { return Point(x + rhs.x, y + rhs.y); } - - /// @brief Overloads addition with a double - [[nodiscard]] Point operator+(double const& rhs) const { return Point(x + rhs, y + rhs); } - - /// @brief Overloads subtraction with another Point - [[nodiscard]] Point operator-(Point const& rhs) const { return Point(x - rhs.x, y - rhs.y); } - - /// @brief Overloads subtraction with a double - [[nodiscard]] Point operator-(double const& rhs) const { return Point(x - rhs, y - rhs); } - - /// @brief Overloads multiplication with another Point - [[nodiscard]] Point operator*(Point const& rhs) const { return Point(x * rhs.x, y * rhs.y); } - - /// @brief Overloads multiplication with a double - [[nodiscard]] Point operator*(double const& rhs) const { return Point(x * rhs, y * rhs); } - - /// @brief Overloads multiplication with a double + /// @brief Overloads multiplication with a integer [[nodiscard]] Point operator*(int const& rhs) const { return Point(x * rhs, y * rhs); } - /// @brief Overloads division with another Point - [[nodiscard]] Point operator/(Point const& rhs) const { return Point(x / rhs.x, y / rhs.y); } - - /// @brief Overloads division with a double - [[nodiscard]] Point operator/(double const& rhs) const { return Point(x / rhs, y / rhs); } - - /// @brief Overloads equality with another Point - bool operator==(const Point& rhs) const { return IsEqual(x, rhs.x) && IsEqual(y, rhs.y); } + // /// @brief Overloads equality with another Point + // bool operator==(const Point& rhs) const { return IsEqual(x, rhs.x) && IsEqual(y, rhs.y); } /// @brief Transforms spherical coordinates to cartesian void TransformSphericalToCartesian(double referenceLatitude) @@ -178,6 +154,48 @@ namespace meshkernel constants::geometric::earth_radius; ///< Factor used in the transformation from spherical to Cartesian coordinates }; + /// @brief Add two points + Point operator+(const Point& p1, const Point& p2); + + /// @brief Add points and scalar + Point operator+(const Point& p, const double x); + + /// @brief Add points and scalar + Point operator+(const double x, const Point& p); + + /// @brief Subtract two points + Point operator-(const Point& p1, const Point& p2); + + /// @brief Subtract scalar from point + Point operator-(const Point& p, const double x); + + /// @brief Subtract point from scalar + Point operator-(const double x, const Point& p); + + /// @brief Multiply point by a point + /// + /// Computes a point-wise product. + Point operator*(const Point& p1, const Point& p2); + + /// @brief Multiply point by a scalar + Point operator*(const double x, const Point& p); + + /// @brief Multiply point by a scalar + Point operator*(const Point& p, const double x); + + /// @brief Divide point by a point + /// + /// Computes a point-wise division. + /// \note No check is made to determine is divisors are zero. + Point operator/(const Point& p1, const Point& p2); + + /// @brief Divide point by a scalar + /// \note No check is made to determine is divisor is zero. + Point operator/(const Point& p, const double x); + + /// @brief Test points for equality + bool operator==(const Point& p1, const Point& p2); + /// @brief Describes an edge with two indices using Edge = std::pair; @@ -368,3 +386,63 @@ inline meshkernel::Point& meshkernel::Point::operator*=(const double p) y *= p; return *this; } + +inline meshkernel::Point meshkernel::operator+(const Point& p1, const Point& p2) +{ + return Point(p1.x + p2.x, p1.y + p2.y); +} + +inline meshkernel::Point meshkernel::operator+(const Point& p, const double x) +{ + return Point(p.x + x, p.y + x); +} + +inline meshkernel::Point meshkernel::operator+(const double x, const Point& p) +{ + return Point(p.x + x, p.y + x); +} + +inline meshkernel::Point meshkernel::operator-(const Point& p1, const Point& p2) +{ + return Point(p1.x - p2.x, p1.y - p2.y); +} + +inline meshkernel::Point meshkernel::operator-(const Point& p, const double x) +{ + return Point(p.x - x, p.y - x); +} + +inline meshkernel::Point meshkernel::operator-(const double x, const Point& p) +{ + return Point(x - p.x, x - p.y); +} + +inline meshkernel::Point meshkernel::operator*(const Point& p1, const Point& p2) +{ + return Point(p1.x * p2.x, p1.y * p2.y); +} + +inline meshkernel::Point meshkernel::operator*(const double x, const Point& p) +{ + return Point(x * p.x, x * p.y); +} + +inline meshkernel::Point meshkernel::operator*(const Point& p, const double x) +{ + return Point(x * p.x, x * p.y); +} + +inline meshkernel::Point meshkernel::operator/(const Point& p1, const Point& p2) +{ + return Point(p1.x / p2.x, p1.y / p2.y); +} + +inline meshkernel::Point meshkernel::operator/(const Point& p, const double x) +{ + return Point(p.x / x, p.y / x); +} + +inline bool meshkernel::operator==(const Point& p1, const Point& p2) +{ + return IsEqual(p1.x, p2.x) && IsEqual(p1.y, p2.y); +} diff --git a/libs/MeshKernel/include/MeshKernel/LandBoundaries.hpp b/libs/MeshKernel/include/MeshKernel/LandBoundaries.hpp index dd17f96dc..5a9e1aa6a 100644 --- a/libs/MeshKernel/include/MeshKernel/LandBoundaries.hpp +++ b/libs/MeshKernel/include/MeshKernel/LandBoundaries.hpp @@ -86,6 +86,14 @@ namespace meshkernel /// @return The number of land boundary nodes. auto GetNumNodes() const { return m_nodes.size(); } + // toland + void nearestPointOnLandBoundary(const Point& pnt, + const int jainview, + Point& nearestPoint, + double& shortestDistance, + UInt& segmentStartIndex, + double& scaledDistanceToStart) const; + std::vector m_meshNodesLandBoundarySegments; ///< Mesh nodes to land boundary mapping (lanseg_map) private: diff --git a/libs/MeshKernel/include/MeshKernel/Splines.hpp b/libs/MeshKernel/include/MeshKernel/Splines.hpp index 0ad8a74ee..9d796d8c0 100644 --- a/libs/MeshKernel/include/MeshKernel/Splines.hpp +++ b/libs/MeshKernel/include/MeshKernel/Splines.hpp @@ -27,7 +27,9 @@ #pragma once +#include "MeshKernel/Utilities/LinearAlgebra.hpp" #include +#include #include namespace meshkernel @@ -48,11 +50,11 @@ namespace meshkernel Splines() = default; /// @brief Ctor, set projection - /// @brief[in] projection The map projection + /// @brief[in] projection The map projection explicit Splines(Projection projection); /// @brief Ctor from grids, each gridline is converted to spline, first the first m_n horizontal lines then the m_m vertical lines - /// @brief[in] The curvilinear grid + /// @brief[in] grid The curvilinear grid explicit Splines(CurvilinearGrid const& grid); /// @brief Adds a new spline to m_splineCornerPoints @@ -75,6 +77,28 @@ namespace meshkernel /// @returns coordinatesDerivatives The second order derivative at corner points (x derivative or y derivative) [[nodiscard]] static std::vector SecondOrderDerivative(const std::vector& coordinates, UInt startIndex, UInt endIndex); + static lin_alg::ColumnVector ComputeSplineWeights(const std::vector& splinePoints, + const UInt totalNumberOfPoints, + const Projection projection); + + /// @brief Evaluate a spline function (splint) + static Point evaluate(const std::vector& coordinates, const std::vector& secondDerivative, const double evaluationPoint); + + // sample_spline + static void sampleSpline(const std::vector& controlPoints, + const size_t intermediatePointCount, + std::vector& samplePoints); + + // comp_afinespline + static void compAfinespline(const UInt n, const UInt numRef, UInt& count, lin_alg::MatrixColMajor& mat); + + static lin_alg::MatrixColMajor ComputeInterpolationMatrix(const lin_alg::MatrixColMajor& splineCoefficients, + const lin_alg::ColumnVector& weights); + + // snap_spline + void snapSpline(const LandBoundaries& landBoundary, + const size_t splineIndex); + /// @brief Computes the intersection of two splines (sect3r) /// @param[in] first The index of the first spline /// @param[in] second The index of the second spline diff --git a/libs/MeshKernel/include/MeshKernel/Utilities/LinearAlgebra.hpp b/libs/MeshKernel/include/MeshKernel/Utilities/LinearAlgebra.hpp index 7dd755f7a..2d00fb21f 100644 --- a/libs/MeshKernel/include/MeshKernel/Utilities/LinearAlgebra.hpp +++ b/libs/MeshKernel/include/MeshKernel/Utilities/LinearAlgebra.hpp @@ -13,4 +13,10 @@ namespace lin_alg /// @tparam T Data type template using MatrixColMajor = Eigen::Matrix; + + /// @brief A dynamically sized column vector + /// @tparam T Data type + template + using ColumnVector = Eigen::Matrix; + } // namespace lin_alg diff --git a/libs/MeshKernel/src/LandBoundaries.cpp b/libs/MeshKernel/src/LandBoundaries.cpp index 931116345..235bbda56 100644 --- a/libs/MeshKernel/src/LandBoundaries.cpp +++ b/libs/MeshKernel/src/LandBoundaries.cpp @@ -1030,3 +1030,50 @@ void LandBoundaries::SnapMeshToLandBoundaries() } } } + +void LandBoundaries::nearestPointOnLandBoundary(const Point& samplePoint, + const int jainview, + Point& nearestPoint, + double& minimumDistance, + UInt& segmentStartIndex, + double& scaledDistanceToStart) const +{ + + nearestPoint = samplePoint; + segmentStartIndex = constants::missing::intValue; + scaledDistanceToStart = -1.0; + + minimumDistance = 9.0e33; + + for (size_t i = 0; i < m_nodes.size() - 1; ++i) + { + Point firstPoint = m_nodes[i]; + Point nextPoint = m_nodes[i + 1]; + + bool doIt = true; + + if (jainview == 2) + { + doIt = IsPointInPolygonNodes(firstPoint, m_nodes, m_mesh->m_projection) && IsPointInPolygonNodes(nextPoint, m_nodes, m_mesh->m_projection); + } + + if (doIt) + { + auto [distance, linePoint, distanceFromFirstNode] = DistanceFromLine(samplePoint, firstPoint, nextPoint, m_mesh->m_projection); + // TODO What is this and from where do I get it? + int ja = 1; + + if (ja == 1) + { + + if (distance < minimumDistance) + { + nearestPoint = linePoint; + minimumDistance = distance; + segmentStartIndex = i; + scaledDistanceToStart = distanceFromFirstNode; + } + } + } + } +} diff --git a/libs/MeshKernel/src/Splines.cpp b/libs/MeshKernel/src/Splines.cpp index 3dd6c531a..99cc0d4b8 100644 --- a/libs/MeshKernel/src/Splines.cpp +++ b/libs/MeshKernel/src/Splines.cpp @@ -25,9 +25,13 @@ // //------------------------------------------------------------------------------ +#include +#include + #include #include #include +#include #include #include @@ -36,6 +40,7 @@ using meshkernel::Splines; Splines::Splines(Projection projection) : m_projection(projection) {} Splines::Splines(CurvilinearGrid const& grid) + { // first the m_n m_m-gridlines std::vector> mGridLines(grid.m_numN, std::vector(grid.m_numM)); @@ -461,6 +466,306 @@ std::vector Splines::SecondOrderDerivative(const std::vector& co return coordinatesDerivatives; } +meshkernel::Point Splines::evaluate(const std::vector& coordinates, const std::vector& secondDerivative, const double evaluationPoint) +{ + // Start and end index of interval containing evaluationPoint + size_t startIndex = static_cast(evaluationPoint); + + constexpr double splfac = 1.0; + + double a = static_cast(startIndex + 1) - evaluationPoint; + double b = evaluationPoint - static_cast(startIndex); + + // TODO add check to see if point lies close to coordinate point + + Point result = a * coordinates[startIndex] + b * coordinates[startIndex + 1]; + result += (splfac / 6.0) * ((a * a * a - a) * secondDerivative[startIndex] + (b * b * b - b) * secondDerivative[startIndex + 1]); + + return result; +} + +void Splines::sampleSpline(const std::vector& splinePoints, + const size_t intermediatePointCount, + std::vector& samplePoints) +{ + + if (splinePoints.size() == 0) + { + throw meshkernel::ConstraintError("Spline is empty"); + } + + size_t sampleCount = splinePoints.size() + (splinePoints.size() - 1) * intermediatePointCount; + + samplePoints.resize(sampleCount); + + std::vector evaluatedSpline(splinePoints.size()); + // TODO Can I use the derivatives that are saved in the splines object? + std::vector secondDerivative = SecondOrderDerivative(splinePoints, 0, splinePoints.size() - 1); + + double intermediatePointCountFloat = static_cast(intermediatePointCount + 1); + double evaluationPoint; + + size_t count = 0; + + for (size_t i = 0; i < splinePoints.size() - 1; ++i) + { + double floatI = static_cast(i); + + for (size_t j = 0; j <= intermediatePointCount; ++j) + { + evaluationPoint = floatI + static_cast(j) / intermediatePointCountFloat; + samplePoints[count] = evaluate(splinePoints, secondDerivative, evaluationPoint); + ++count; + } + } + + evaluationPoint = static_cast(splinePoints.size() - 1); + samplePoints[count] = evaluate(splinePoints, secondDerivative, evaluationPoint); +} + +void Splines::compAfinespline(const UInt n, const UInt numRef, UInt& count, lin_alg::MatrixColMajor& mat) +{ + + if (n < 1) + { + throw meshkernel::ConstraintError(VariadicErrorMessage("Invalid spline point count: {}", n)); + } + + count = n + (n - 1) * numRef; + + // if ( Nr_in.lt.Nr ) then + // ierror = 2 + // goto 1234 + // end if + + mat.resize(count, n); + + Point p; + p.x = 0.0; + p.y = 0.0; + std::vector loc(n, p); + std::vector xf; + + for (UInt i = 0; i < n; ++i) + { + loc[i].x = 1.0; + sampleSpline(loc, numRef, xf); + + // Assign values to column of matrix. + for (UInt j = 0; j < xf.size(); ++j) + { + mat(j, i) = xf[j].x; + } + + loc[i].x = 0.0; + } +} + +lin_alg::MatrixColMajor Splines::ComputeInterpolationMatrix(const lin_alg::MatrixColMajor& splineCoefficients, + const lin_alg::ColumnVector& weights) +{ + + lin_alg::MatrixColMajor atwa(splineCoefficients.cols(), splineCoefficients.cols()); + + // Compute matrix A^t W A + // least squares + for (int i = 0; i < splineCoefficients.cols(); ++i) + { + for (int j = 0; j < splineCoefficients.cols(); ++j) + { + atwa(i, j) = 0.0; + + for (int k = 0; k < splineCoefficients.rows(); ++k) + { + atwa(i, j) += splineCoefficients(k, i) * weights(k) * splineCoefficients(k, j); + } + } + } + + return atwa.inverse(); +} + +lin_alg::ColumnVector Splines::ComputeSplineWeights(const std::vector& splinePoints, + const UInt totalNumberOfPoints, + const Projection projection) +{ + + lin_alg::ColumnVector weights(totalNumberOfPoints); + + // Compute weights + for (size_t i = 1; i <= totalNumberOfPoints; ++i) + { + size_t il = std::max(1, i - 1); + size_t ir = std::min(totalNumberOfPoints, i + 1); + weights(i - 1) = 1.0 / std::sqrt(ComputeDistance(splinePoints[il - 1], splinePoints[ir - 1], projection) / static_cast(ir - il)); + } + + return weights; +} + +void Splines::snapSpline(const LandBoundaries& landBoundary, + const size_t splineIndex) +{ + if (splineIndex > GetNumSplines()) + { + throw meshkernel::ConstraintError(VariadicErrorMessage("Invalid spline index: {}", splineIndex)); + } + + if (m_splineNodes[splineIndex].empty()) + { + throw meshkernel::ConstraintError(VariadicErrorMessage("Empty spline at index: {}", splineIndex)); + } + + constexpr int MaxNumberOfIterations = 10; + constexpr double tolerance = 1.0e-5; + + // Number of additional points between spline control points for sampled spline. + constexpr UInt numberRefinements = 19; + constexpr UInt numberOfConstraints = 2; + + std::vector& splinePoints(m_splineNodes[splineIndex]); + std::vector& splineDerivative(m_splineDerivatives[splineIndex]); + + // Save original spline end points + Point startPoint = splinePoints.front(); + Point endPoint = splinePoints.back(); + UInt totalNumberOfPoints = 0; + + lin_alg::MatrixColMajor aMatrix; + + // Compute spline coefficients. + // Resizes the matrix to be totalNumberOfPoints by splinePoints.size() + compAfinespline(splinePoints.size(), numberRefinements, totalNumberOfPoints, aMatrix); + + // Vectors containing x- and y-coordinates + // Simplifies linear algebra operations to have then in Eigen::vector format. + // TODO Only problem, they are used only once, to compute a gemv. Is there an easier way? + lin_alg::ColumnVector vecX(splinePoints.size()); + lin_alg::ColumnVector vecY(splinePoints.size()); + + // extract spline points to vectors. + for (size_t i = 0; i < splinePoints.size(); ++i) + { + vecX(i) = splinePoints[i].x; + vecY(i) = splinePoints[i].y; + } + + lin_alg::ColumnVector xf(aMatrix * vecX); + lin_alg::ColumnVector yf(aMatrix * vecY); + lin_alg::ColumnVector xfOld(xf); + lin_alg::ColumnVector yfOld(yf); + lin_alg::ColumnVector weights(ComputeSplineWeights(splinePoints, totalNumberOfPoints, m_projection)); + + auto [startCurvature, startNormal, startTangent] = ComputeCurvatureOnSplinePoint(splineIndex, 0.0); + auto [endCurvature, endNormal, endTangent] = ComputeCurvatureOnSplinePoint(splineIndex, static_cast(splinePoints.size() - 1)); + + lin_alg::MatrixColMajor atwaInverse(ComputeInterpolationMatrix(aMatrix, weights)); + lin_alg::MatrixColMajor bMatrix(numberOfConstraints, splinePoints.size()); + lin_alg::MatrixColMajor cMatrix(numberOfConstraints, splinePoints.size()); + lin_alg::ColumnVector dVector(numberOfConstraints); + lin_alg::ColumnVector lambda(numberOfConstraints); + + // Compute the matrix for the constraints. + bMatrix.setZero(); + cMatrix.setZero(); + + bMatrix(0, 0) = startNormal.y; + cMatrix(0, 0) = -startNormal.x; + dVector(0) = startNormal.y * startPoint.x - startNormal.x * startPoint.y; + bMatrix(1, splinePoints.size() - 1) = endNormal.y; + cMatrix(1, splinePoints.size() - 1) = -endNormal.x; + dVector(1) = endNormal.y * endPoint.x - endNormal.x * endPoint.y; + + lin_alg::MatrixColMajor eMatrix = bMatrix * atwaInverse * bMatrix.transpose() + cMatrix * atwaInverse * cMatrix.transpose(); + + lambda.setZero(); + // Inplace inversion of the e-matrix. + eMatrix = eMatrix.inverse(); + + bool converged = true; + + lin_alg::ColumnVector xbVec(totalNumberOfPoints); + lin_alg::ColumnVector ybVec(totalNumberOfPoints); + + lin_alg::ColumnVector atwxb(splinePoints.size()); + lin_alg::ColumnVector atwyb(splinePoints.size()); + + lin_alg::ColumnVector rhsx(numberOfConstraints); + lin_alg::ColumnVector rhsy(numberOfConstraints); + + lin_alg::ColumnVector xVals(numberOfConstraints); + lin_alg::ColumnVector yVals(numberOfConstraints); + + int iterationCount = 0; + + while (!converged && iterationCount <= MaxNumberOfIterations) + { + Point nearestPoint; + + // These variable are unused in the rest of the algorithm + double smallestDistance; + double scaledDistance; + UInt segmentIndex; + + for (size_t i = 0; i < splinePoints.size(); ++i) + { + Point point(xf(i), yf(i)); + landBoundary.nearestPointOnLandBoundary(point, 2, nearestPoint, smallestDistance, segmentIndex, scaledDistance); + // toLand(xf[i], 1, mxlan, 2, nearestPoint, smallestDistance, segmentIndex, scaledDistance); + xbVec(i) = nearestPoint.x; + ybVec(i) = nearestPoint.y; + } + + // Could be replaced with gemv. + for (size_t i = 0; i < splinePoints.size(); ++i) + { + atwxb(i) = 0.0; + atwyb(i) = 0.0; + + for (size_t j = 0; j < splinePoints.size(); ++j) + { + atwxb(i) += aMatrix(j, i) * weights(j) * xbVec(j); + atwyb(i) += aMatrix(j, i) * weights(j) * ybVec(j); + } + } + + lambda = eMatrix * (bMatrix * atwaInverse * atwxb + cMatrix * atwaInverse * atwyb - dVector); + + rhsx = atwxb - bMatrix.transpose() * lambda; + rhsy = atwyb - cMatrix.transpose() * lambda; + + xVals = atwaInverse * rhsx; + yVals = atwaInverse * rhsy; + + xf = aMatrix * xVals; + yf = aMatrix * yVals; + + // TODO is there a better check for convergence? + // AND what should the tolerance be? + converged = (xf - xfOld).norm() + (yf - yfOld).norm() < tolerance; + xfOld = xf; + yfOld = yf; + ++iterationCount; + } + + if (converged) + { + // Copy vectors back to array of points. + for (size_t i = 0; i < splinePoints.size(); ++i) + { + splinePoints[i].x = xf(i); + splinePoints[i].y = yf(i); + } + + splineDerivative = SecondOrderDerivative(splinePoints, 0, splinePoints.size() - 1); + } + else + { + throw AlgorithmError(VariadicErrorMessage("Failed to reach convergence for snapping spline {} to land boundary after {} iterations for {} tolerance", + splineIndex, MaxNumberOfIterations, tolerance)); + } +} + std::tuple, std::vector> Splines::ComputePointOnSplineFromAdimensionalDistance(UInt index, double maximumGridHeight,