| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // This file is part of INSTINCT, the INS Toolkit for Integrated | ||
| 2 | // Navigation Concepts and Training by the Institute of Navigation of | ||
| 3 | // the University of Stuttgart, Germany. | ||
| 4 | // | ||
| 5 | // This Source Code Form is subject to the terms of the Mozilla Public | ||
| 6 | // License, v. 2.0. If a copy of the MPL was not distributed with this | ||
| 7 | // file, You can obtain one at https://mozilla.org/MPL/2.0/. | ||
| 8 | |||
| 9 | /// @file CubicSpline.hpp | ||
| 10 | /// @brief Cubic Spline class | ||
| 11 | /// @author T. Hobiger (thomas.hobiger@ins.uni-stuttgart.de) | ||
| 12 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 13 | /// @date 2022-06-06 | ||
| 14 | |||
| 15 | #pragma once | ||
| 16 | |||
| 17 | #include <cstdint> | ||
| 18 | #include <cstdio> | ||
| 19 | #include <cassert> | ||
| 20 | #include <cmath> | ||
| 21 | #include <vector> | ||
| 22 | #include <algorithm> | ||
| 23 | #include <sstream> | ||
| 24 | #include <string> | ||
| 25 | #include <iostream> | ||
| 26 | |||
| 27 | #include "util/Assert.h" | ||
| 28 | |||
| 29 | namespace NAV | ||
| 30 | { | ||
| 31 | |||
| 32 | /// Cubic Spline class | ||
| 33 | template<typename Scalar> | ||
| 34 | class CubicSpline | ||
| 35 | { | ||
| 36 | public: | ||
| 37 | /// @brief Boundary conditions for the spline end-points | ||
| 38 | struct BoundaryCondition | ||
| 39 | { | ||
| 40 | /// @brief Boundary type | ||
| 41 | enum BoundaryType : uint8_t | ||
| 42 | { | ||
| 43 | FirstDerivative = 1, ///< First derivative has to match a certain value | ||
| 44 | SecondDerivative = 2, ///< Second derivative has to match a certain value | ||
| 45 | NotAKnot = 3, ///< not-a-knot: At the first and last interior break, even the third derivative is continuous (up to round-off error). | ||
| 46 | }; | ||
| 47 | BoundaryType type = SecondDerivative; ///< Type of the boundary condition | ||
| 48 | Scalar value{ 0.0 }; ///< Value of the boundary condition | ||
| 49 | }; | ||
| 50 | |||
| 51 | /// @brief Default Constructor | ||
| 52 | 714 | CubicSpline() = default; | |
| 53 | |||
| 54 | /// @brief Constructor | ||
| 55 | /// @param[in] X List of x coordinates for the spline points/knots | ||
| 56 | /// @param[in] Y List of y coordinates for the spline points/knots | ||
| 57 | /// @param[in] leftBoundaryCondition Boundary condition for the start knot | ||
| 58 | /// @param[in] rightBoundaryCondition Boundary condition for the end knot | ||
| 59 | CubicSpline(const std::vector<Scalar>& X, const std::vector<Scalar>& Y, | ||
| 60 | BoundaryCondition leftBoundaryCondition = { BoundaryCondition::SecondDerivative, 0.0 }, | ||
| 61 | BoundaryCondition rightBoundaryCondition = { BoundaryCondition::SecondDerivative, 0.0 }) | ||
| 62 | : boundaryConditionLeft(leftBoundaryCondition), boundaryConditionRight(rightBoundaryCondition) | ||
| 63 | { | ||
| 64 | setPoints(X, Y); | ||
| 65 | } | ||
| 66 | |||
| 67 | /// @brief Set the boundaries conditions. Has to be called before setPoints | ||
| 68 | /// @param[in] leftBoundaryCondition Boundary condition for the start knot | ||
| 69 | /// @param[in] rightBoundaryCondition Boundary condition for the end knot | ||
| 70 | void setBoundaries(BoundaryCondition leftBoundaryCondition, BoundaryCondition rightBoundaryCondition) | ||
| 71 | { | ||
| 72 | boundaryConditionLeft = leftBoundaryCondition; | ||
| 73 | boundaryConditionRight = rightBoundaryCondition; | ||
| 74 | } | ||
| 75 | |||
| 76 | /// @brief Set the points/knots of the spline and calculate the spline coefficients | ||
| 77 | /// @param[in] x List of x coordinates of the points | ||
| 78 | /// @param[in] y List of y coordinates of the points | ||
| 79 | 42 | void setPoints(const std::vector<Scalar>& x, const std::vector<Scalar>& y) | |
| 80 | { | ||
| 81 |
1/2✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
|
42 | vals_x = x; |
| 82 |
1/2✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
|
42 | vals_y = y; |
| 83 | 42 | size_t n = x.size(); | |
| 84 | |||
| 85 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 42 times.
|
42 | size_t n_upper = (boundaryConditionLeft.type == BoundaryCondition::NotAKnot) ? 2 : 1; |
| 86 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 42 times.
|
42 | size_t n_lower = (boundaryConditionRight.type == BoundaryCondition::NotAKnot) ? 2 : 1; |
| 87 |
1/2✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
|
42 | BandMatrix A(n, n_upper, n_lower); |
| 88 |
1/2✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
|
42 | std::vector<Scalar> rhs(n); |
| 89 |
2/2✓ Branch 0 taken 8062674 times.
✓ Branch 1 taken 42 times.
|
8062716 | for (size_t i = 1; i < n - 1; i++) |
| 90 | { | ||
| 91 |
1/2✓ Branch 3 taken 8062674 times.
✗ Branch 4 not taken.
|
8062674 | A(i, i - 1) = 1.0 / 3.0 * (x[i] - x[i - 1]); |
| 92 |
1/2✓ Branch 3 taken 8062674 times.
✗ Branch 4 not taken.
|
8062674 | A(i, i) = 2.0 / 3.0 * (x[i + 1] - x[i - 1]); |
| 93 |
1/2✓ Branch 3 taken 8062674 times.
✗ Branch 4 not taken.
|
8062674 | A(i, i + 1) = 1.0 / 3.0 * (x[i + 1] - x[i]); |
| 94 | 8062674 | rhs[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i]) - (y[i] - y[i - 1]) / (x[i] - x[i - 1]); | |
| 95 | } | ||
| 96 | // boundary conditions | ||
| 97 |
1/2✓ Branch 0 taken 42 times.
✗ Branch 1 not taken.
|
42 | if (boundaryConditionLeft.type == BoundaryCondition::SecondDerivative) |
| 98 | { | ||
| 99 | // 2*c[0] = f'' | ||
| 100 |
1/2✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
|
42 | A(0, 0) = 2.0; |
| 101 |
1/2✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
|
42 | A(0, 1) = 0.0; |
| 102 | 42 | rhs[0] = boundaryConditionLeft.value; | |
| 103 | } | ||
| 104 | ✗ | else if (boundaryConditionLeft.type == BoundaryCondition::FirstDerivative) | |
| 105 | { | ||
| 106 | // b[0] = f', needs to be re-expressed in terms of c: | ||
| 107 | // (2c[0]+c[1])(x[1]-x[0]) = 3 ((y[1]-y[0])/(x[1]-x[0]) - f') | ||
| 108 | ✗ | A(0, 0) = 2.0 * (x[1] - x[0]); | |
| 109 | ✗ | A(0, 1) = 1.0 * (x[1] - x[0]); | |
| 110 | ✗ | rhs[0] = 3.0 * ((y[1] - y[0]) / (x[1] - x[0]) - boundaryConditionLeft.value); | |
| 111 | } | ||
| 112 | ✗ | else if (boundaryConditionLeft.type == BoundaryCondition::NotAKnot) | |
| 113 | { | ||
| 114 | // f'''(x[1]) exists, i.e. d[0]=d[1], or re-expressed in c: | ||
| 115 | // -h1*c[0] + (h0+h1)*c[1] - h0*c[2] = 0 | ||
| 116 | ✗ | A(0, 0) = -(x[2] - x[1]); | |
| 117 | ✗ | A(0, 1) = x[2] - x[0]; | |
| 118 | ✗ | A(0, 2) = -(x[1] - x[0]); | |
| 119 | ✗ | rhs[0] = 0.0; | |
| 120 | } | ||
| 121 |
1/2✓ Branch 0 taken 42 times.
✗ Branch 1 not taken.
|
42 | if (boundaryConditionRight.type == BoundaryCondition::SecondDerivative) |
| 122 | { | ||
| 123 | // 2*c[n-1] = f'' | ||
| 124 |
1/2✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
|
42 | A(n - 1, n - 1) = 2.0; |
| 125 |
1/2✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
|
42 | A(n - 1, n - 2) = 0.0; |
| 126 | 42 | rhs[n - 1] = boundaryConditionRight.value; | |
| 127 | } | ||
| 128 | ✗ | else if (boundaryConditionRight.type == BoundaryCondition::FirstDerivative) | |
| 129 | { | ||
| 130 | // b[n-1] = f', needs to be re-expressed in terms of c: | ||
| 131 | // (c[n-2]+2c[n-1])(x[n-1]-x[n-2]) | ||
| 132 | // = 3 (f' - (y[n-1]-y[n-2])/(x[n-1]-x[n-2])) | ||
| 133 | ✗ | A(n - 1, n - 1) = 2.0 * (x[n - 1] - x[n - 2]); | |
| 134 | ✗ | A(n - 1, n - 2) = 1.0 * (x[n - 1] - x[n - 2]); | |
| 135 | ✗ | rhs[n - 1] = 3.0 * (boundaryConditionRight.value - (y[n - 1] - y[n - 2]) / (x[n - 1] - x[n - 2])); | |
| 136 | } | ||
| 137 | ✗ | else if (boundaryConditionRight.type == BoundaryCondition::NotAKnot) | |
| 138 | { | ||
| 139 | // f'''(x[n-2]) exists, i.e. d[n-3]=d[n-2], or re-expressed in c: | ||
| 140 | // -h_{n-2}*c[n-3] + (h_{n-3}+h_{n-2})*c[n-2] - h_{n-3}*c[n-1] = 0 | ||
| 141 | ✗ | A(n - 1, n - 3) = -(x[n - 1] - x[n - 2]); | |
| 142 | ✗ | A(n - 1, n - 2) = x[n - 1] - x[n - 3]; | |
| 143 | ✗ | A(n - 1, n - 1) = -(x[n - 2] - x[n - 3]); | |
| 144 | ✗ | rhs[0] = 0.0; | |
| 145 | } | ||
| 146 | |||
| 147 | // solve the equation system to obtain the parameters c[] | ||
| 148 |
1/2✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
|
42 | coef_c = A.lu_solve(rhs); |
| 149 | |||
| 150 | // calculate parameters b[] and d[] based on c[] | ||
| 151 |
1/2✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
|
42 | coef_d.resize(n); |
| 152 |
1/2✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
|
42 | coef_b.resize(n); |
| 153 |
2/2✓ Branch 0 taken 8062716 times.
✓ Branch 1 taken 42 times.
|
8062758 | for (size_t i = 0; i < n - 1; i++) |
| 154 | { | ||
| 155 | 8062716 | coef_d[i] = 1.0 / 3.0 * (coef_c[i + 1] - coef_c[i]) / (x[i + 1] - x[i]); | |
| 156 | 16125432 | coef_b[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i]) | |
| 157 | 8062716 | - 1.0 / 3.0 * (2.0 * coef_c[i] + coef_c[i + 1]) * (x[i + 1] - x[i]); | |
| 158 | } | ||
| 159 | // for the right extrapolation coefficients (zero cubic term) | ||
| 160 | // f_{n-1}(x) = y_{n-1} + b*(x-x_{n-1}) + c*(x-x_{n-1})^2 | ||
| 161 | 42 | auto h = x[n - 1] - x[n - 2]; | |
| 162 | // coef_c[n-1] is determined by the boundary condition | ||
| 163 | 42 | coef_d[n - 1] = 0.0; | |
| 164 | 42 | coef_b[n - 1] = 3.0 * coef_d[n - 2] * h * h + 2.0 * coef_c[n - 2] * h + coef_b[n - 2]; // = f'_{n-2}(x_{n-1}) | |
| 165 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 42 times.
|
42 | if (boundaryConditionRight.type == BoundaryCondition::FirstDerivative) |
| 166 | { | ||
| 167 | ✗ | coef_c[n - 1] = 0.0; // force linear extrapolation | |
| 168 | } | ||
| 169 | // for left extrapolation coefficients | ||
| 170 |
1/2✓ Branch 0 taken 42 times.
✗ Branch 1 not taken.
|
42 | coef_c0 = (boundaryConditionLeft.type == BoundaryCondition::FirstDerivative) ? 0.0 : coef_c[0]; |
| 171 | 42 | } | |
| 172 | |||
| 173 | /// @brief Returns the size of the spline vector | ||
| 174 | ✗ | [[nodiscard]] size_t size() const noexcept | |
| 175 | { | ||
| 176 | ✗ | return vals_x.size(); | |
| 177 | } | ||
| 178 | |||
| 179 | /// @brief Interpolates or extrapolates a value on the spline | ||
| 180 | /// @param[in] x X coordinate to inter-/extrapolate the value for | ||
| 181 | /// @return The y coordinate | ||
| 182 | 21470186 | [[nodiscard]] Scalar operator()(Scalar x) const | |
| 183 | { | ||
| 184 | 21470186 | size_t n = vals_x.size(); | |
| 185 | 21469919 | size_t idx = findClosestIdx(x); | |
| 186 | |||
| 187 | 21467858 | auto h = x - vals_x[idx]; | |
| 188 | |||
| 189 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 21479514 times.
|
21468048 | if (x < vals_x[0]) // extrapolation to the left |
| 190 | { | ||
| 191 | ✗ | return (coef_c0 * h + coef_b[0]) * h + vals_y[0]; | |
| 192 | } | ||
| 193 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 21473375 times.
|
21479514 | if (x > vals_x[n - 1]) // extrapolation to the right |
| 194 | { | ||
| 195 | ✗ | return (coef_c[n - 1] * h + coef_b[n - 1]) * h + vals_y[n - 1]; | |
| 196 | } | ||
| 197 | // interpolation | ||
| 198 | 21473375 | return ((coef_d[idx] * h + coef_c[idx]) * h + coef_b[idx]) * h + vals_y[idx]; | |
| 199 | } | ||
| 200 | |||
| 201 | /// @brief Calculates the derivative of the spline | ||
| 202 | /// @param[in] order Order of the derivative to calculate (<= 3) | ||
| 203 | /// @param[in] x X coordinate to calculate the derivative for | ||
| 204 | /// @return The derivative of y up to the given order | ||
| 205 | 29614989 | [[nodiscard]] Scalar derivative(size_t order, Scalar x) const | |
| 206 | { | ||
| 207 | 29614989 | size_t n = vals_x.size(); | |
| 208 | 29621250 | size_t idx = findClosestIdx(x); | |
| 209 | |||
| 210 | 29608369 | auto h = x - vals_x[idx]; | |
| 211 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 29621888 times.
|
29610845 | if (x < vals_x[0]) // extrapolation to the left |
| 212 | { | ||
| 213 | ✗ | switch (order) | |
| 214 | { | ||
| 215 | ✗ | case 1: | |
| 216 | ✗ | return 2.0 * coef_c0 * h + coef_b[0]; | |
| 217 | ✗ | case 2: | |
| 218 | ✗ | return 2.0 * coef_c0; | |
| 219 | ✗ | default: | |
| 220 | ✗ | return 0.0; | |
| 221 | } | ||
| 222 | } | ||
| 223 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 29632418 times.
|
29621888 | else if (x > vals_x[n - 1]) // extrapolation to the right |
| 224 | { | ||
| 225 | ✗ | switch (order) | |
| 226 | { | ||
| 227 | ✗ | case 1: | |
| 228 | ✗ | return 2.0 * coef_c[n - 1] * h + coef_b[n - 1]; | |
| 229 | ✗ | case 2: | |
| 230 | ✗ | return 2.0 * coef_c[n - 1]; | |
| 231 | ✗ | default: | |
| 232 | ✗ | return 0.0; | |
| 233 | } | ||
| 234 | } | ||
| 235 | else // interpolation | ||
| 236 | { | ||
| 237 |
3/4✓ Branch 0 taken 21097267 times.
✓ Branch 1 taken 8503654 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 31497 times.
|
29632418 | switch (order) |
| 238 | { | ||
| 239 | 21097267 | case 1: | |
| 240 | 21097267 | return (3.0 * coef_d[idx] * h + 2.0 * coef_c[idx]) * h + coef_b[idx]; | |
| 241 | 8503654 | case 2: | |
| 242 | 8503654 | return 6.0 * coef_d[idx] * h + 2.0 * coef_c[idx]; | |
| 243 | ✗ | case 3: | |
| 244 | ✗ | return 6.0 * coef_d[idx]; | |
| 245 | 31497 | default: | |
| 246 | 31497 | return 0.0; | |
| 247 | } | ||
| 248 | } | ||
| 249 | } | ||
| 250 | |||
| 251 | private: | ||
| 252 | std::vector<Scalar> vals_x; ///< x coordinates of the knots | ||
| 253 | std::vector<Scalar> vals_y; ///< y coordinates of the knots | ||
| 254 | std::vector<Scalar> coef_b; ///< Spline coefficients b | ||
| 255 | std::vector<Scalar> coef_c; ///< Spline coefficients c | ||
| 256 | std::vector<Scalar> coef_d; ///< Spline coefficients d | ||
| 257 | Scalar coef_c0{ 0.0 }; ///< Spline coefficient c0 for left extrapolation | ||
| 258 | |||
| 259 | BoundaryCondition boundaryConditionLeft; ///< Boundary condition for the left knot | ||
| 260 | BoundaryCondition boundaryConditionRight; ///< Boundary condition for the right knot | ||
| 261 | |||
| 262 | /// @brief Finds the closest index so that vals_x[idx] <= x (return 0 if x < vals_x[0]) | ||
| 263 | /// @param[in] x X coordinate to search the closest knot to | ||
| 264 | /// @return Index of a knot closest to the x coordinate given | ||
| 265 | 51085696 | [[nodiscard]] size_t findClosestIdx(Scalar x) const | |
| 266 | { | ||
| 267 |
1/2✓ Branch 3 taken 51087150 times.
✗ Branch 4 not taken.
|
51085696 | auto it = std::upper_bound(vals_x.begin(), vals_x.end(), x); |
| 268 | 51087150 | return static_cast<size_t>(std::max(static_cast<int>(it - vals_x.begin()) - 1, 0)); | |
| 269 | } | ||
| 270 | |||
| 271 | /// Sparse matrix whose non-zero entries are confined to a diagonal band, comprising the main diagonal and zero or more diagonals on either side. | ||
| 272 | class BandMatrix | ||
| 273 | { | ||
| 274 | public: | ||
| 275 | /// @brief Constructor | ||
| 276 | /// @param[in] dim Dimension of the matrix | ||
| 277 | /// @param[in] nUpper Amount of upper diagonals | ||
| 278 | /// @param[in] nLower Amount of lower diagonals | ||
| 279 | 42 | BandMatrix(size_t dim, size_t nUpper, size_t nLower) | |
| 280 |
2/4✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
|
126 | : upperBand(nUpper + 1, std::vector<Scalar>(dim)), |
| 281 |
2/4✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
|
126 | lowerBand(nLower + 1, std::vector<Scalar>(dim)) {} |
| 282 | |||
| 283 | /// @brief Access operator i ∈ [i=0,...,dim()-1] | ||
| 284 | 129003666 | Scalar& operator()(size_t i, size_t j) | |
| 285 | { | ||
| 286 | 129003666 | return const_cast<Scalar&>(static_cast<const CubicSpline::BandMatrix&>(*this)(i, j)); // NOLINT(cppcoreguidelines-pro-type-const-cast) | |
| 287 | } | ||
| 288 | /// @brief Access operator i ∈ [i=0,...,dim()-1] | ||
| 289 | 153191856 | [[nodiscard]] const Scalar& operator()(size_t i, size_t j) const | |
| 290 | { | ||
| 291 | // j - i = 0 -> diagonal, > 0 upper right part, < 0 lower left part | ||
| 292 |
2/2✓ Branch 0 taken 112878276 times.
✓ Branch 1 taken 40313580 times.
|
153191856 | return j >= i ? upperBand[j - i][i] : lowerBand[i - j][i]; |
| 293 | } | ||
| 294 | |||
| 295 | /// Solves the linear equations Ax = b for x by obtaining the LU decomposition A = LU and solving | ||
| 296 | /// - Ly = b for y | ||
| 297 | /// - Ux = y for x | ||
| 298 | 42 | [[nodiscard]] std::vector<Scalar> lu_solve(const std::vector<Scalar>& b, bool is_lu_decomposed = false) | |
| 299 | { | ||
| 300 |
1/2✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
|
42 | INS_ASSERT_USER_ERROR(dim() == b.size(), "The BandMatrix dimension must be the same as the vector b in the equation Ax = b."); |
| 301 | |||
| 302 |
1/2✓ Branch 0 taken 42 times.
✗ Branch 1 not taken.
|
42 | if (!is_lu_decomposed) |
| 303 | { | ||
| 304 |
1/2✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
|
42 | lu_decompose(); |
| 305 | } | ||
| 306 |
1/2✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
|
42 | auto y = l_solve(b); |
| 307 |
1/2✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
|
42 | auto x = u_solve(y); |
| 308 | 84 | return x; | |
| 309 | 42 | } | |
| 310 | |||
| 311 | private: | ||
| 312 | /// Returns the matrix dimension | ||
| 313 | 56439642 | [[nodiscard]] size_t dim() const | |
| 314 | { | ||
| 315 |
1/2✓ Branch 1 taken 56439642 times.
✗ Branch 2 not taken.
|
56439642 | return !upperBand.empty() ? upperBand[0].size() : 0; |
| 316 | } | ||
| 317 | /// Returns the dimension of the upper band | ||
| 318 | 24188232 | [[nodiscard]] size_t dimUpperBand() const | |
| 319 | { | ||
| 320 | 24188232 | return upperBand.size() - 1; | |
| 321 | } | ||
| 322 | /// Returns the dimension of the lower band | ||
| 323 | 40313622 | [[nodiscard]] size_t dimLowerBand() const | |
| 324 | { | ||
| 325 | 40313622 | return lowerBand.size() - 1; | |
| 326 | } | ||
| 327 | |||
| 328 | /// @brief Calculate the LU decomposition | ||
| 329 | 42 | void lu_decompose() | |
| 330 | { | ||
| 331 | // preconditioning | ||
| 332 | // normalize column i so that a_ii=1 | ||
| 333 |
2/2✓ Branch 1 taken 8062758 times.
✓ Branch 2 taken 42 times.
|
8062800 | for (size_t i = 0; i < dim(); i++) |
| 334 | { | ||
| 335 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 8062758 times.
|
8062758 | assert(this->operator()(i, i) != 0.0); |
| 336 | 8062758 | lowerBand[0][i] = 1.0 / this->operator()(i, i); | |
| 337 |
2/2✓ Branch 1 taken 8062674 times.
✓ Branch 2 taken 84 times.
|
8062758 | size_t j_min = i > dimLowerBand() ? i - dimLowerBand() : 0; |
| 338 | 8062758 | size_t j_max = std::min(dim() - 1, i + dimUpperBand()); | |
| 339 |
2/2✓ Branch 0 taken 24188190 times.
✓ Branch 1 taken 8062758 times.
|
32250948 | for (size_t j = j_min; j <= j_max; j++) |
| 340 | { | ||
| 341 | 24188190 | this->operator()(i, j) *= lowerBand[0][i]; | |
| 342 | } | ||
| 343 | 8062758 | this->operator()(i, i) = 1.0; // prevents rounding errors | |
| 344 | } | ||
| 345 | |||
| 346 | // Gauss LR-Decomposition | ||
| 347 |
2/2✓ Branch 1 taken 8062758 times.
✓ Branch 2 taken 42 times.
|
8062800 | for (size_t k = 0; k < dim(); k++) |
| 348 | { | ||
| 349 | 8062758 | size_t i_max = std::min(dim() - 1, k + dimLowerBand()); | |
| 350 |
2/2✓ Branch 0 taken 8062716 times.
✓ Branch 1 taken 8062758 times.
|
16125474 | for (size_t i = k + 1; i <= i_max; i++) |
| 351 | { | ||
| 352 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 8062716 times.
|
8062716 | assert(this->operator()(k, k) != 0.0); |
| 353 | |||
| 354 | 8062716 | auto x = -this->operator()(i, k) / this->operator()(k, k); | |
| 355 | 8062716 | this->operator()(i, k) = -x; // assembly part of L | |
| 356 | 8062716 | size_t j_max = std::min(dim() - 1, k + dimUpperBand()); | |
| 357 |
2/2✓ Branch 0 taken 8062716 times.
✓ Branch 1 taken 8062716 times.
|
16125432 | for (size_t j = k + 1; j <= j_max; j++) |
| 358 | { | ||
| 359 | // assembly part of R | ||
| 360 | 8062716 | this->operator()(i, j) = this->operator()(i, j) + x * this->operator()(k, j); | |
| 361 | } | ||
| 362 | } | ||
| 363 | } | ||
| 364 | 42 | } | |
| 365 | |||
| 366 | /// Solves the equation Lx = b for x | ||
| 367 | 42 | [[nodiscard]] std::vector<Scalar> l_solve(const std::vector<Scalar>& b) const | |
| 368 | { | ||
| 369 |
1/2✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
|
42 | INS_ASSERT_USER_ERROR(dim() == b.size(), "The BandMatrix dimension must be the same as the vector b in the equation Ax = b."); |
| 370 | |||
| 371 |
1/2✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
|
42 | std::vector<Scalar> x(dim()); |
| 372 |
2/2✓ Branch 1 taken 8062758 times.
✓ Branch 2 taken 42 times.
|
8062800 | for (size_t i = 0; i < dim(); i++) |
| 373 | { | ||
| 374 | 8062758 | Scalar sum = 0; | |
| 375 |
2/2✓ Branch 1 taken 8062674 times.
✓ Branch 2 taken 84 times.
|
8062758 | size_t j_start = i > dimLowerBand() ? i - dimLowerBand() : 0; |
| 376 |
2/2✓ Branch 0 taken 8062716 times.
✓ Branch 1 taken 8062758 times.
|
16125474 | for (size_t j = j_start; j < i; j++) |
| 377 | { | ||
| 378 | 8062716 | sum += this->operator()(i, j) * x[j]; | |
| 379 | } | ||
| 380 | 8062758 | x[i] = (b[i] * lowerBand[0][i]) - sum; | |
| 381 | } | ||
| 382 | 42 | return x; | |
| 383 | } | ||
| 384 | /// Solves the equation Ux = b for x | ||
| 385 | 42 | [[nodiscard]] std::vector<Scalar> u_solve(const std::vector<Scalar>& b) const | |
| 386 | { | ||
| 387 |
1/2✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
|
42 | INS_ASSERT_USER_ERROR(dim() == b.size(), "The BandMatrix dimension must be the same as the vector b in the equation Ax = b."); |
| 388 | |||
| 389 |
1/2✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
|
42 | std::vector<Scalar> x(dim()); |
| 390 |
2/2✓ Branch 1 taken 8062758 times.
✓ Branch 2 taken 42 times.
|
8062800 | for (size_t i = dim(); i-- > 0;) |
| 391 | { | ||
| 392 | 8062758 | Scalar sum = 0; | |
| 393 | 8062758 | size_t j_stop = std::min(dim() - 1, i + dimUpperBand()); | |
| 394 |
2/2✓ Branch 0 taken 8062716 times.
✓ Branch 1 taken 8062758 times.
|
16125474 | for (size_t j = i + 1; j <= j_stop; j++) |
| 395 | { | ||
| 396 | 8062716 | sum += this->operator()(i, j) * x[j]; | |
| 397 | } | ||
| 398 | 8062758 | x[i] = (b[i] - sum) / this->operator()(i, i); | |
| 399 | } | ||
| 400 | 42 | return x; | |
| 401 | } | ||
| 402 | |||
| 403 | std::vector<std::vector<Scalar>> upperBand; ///< upper diagonal band | ||
| 404 | std::vector<std::vector<Scalar>> lowerBand; ///< lower diagonal band | ||
| 405 | }; | ||
| 406 | }; | ||
| 407 | |||
| 408 | } // namespace NAV | ||
| 409 |