From 3dbd68b21c6ec725cc2136e3e4ffec25a0ca5743 Mon Sep 17 00:00:00 2001 From: "Timur A. Fatkhullin" Date: Tue, 17 Mar 2026 17:18:11 +0300 Subject: [PATCH] ... --- include/mcc/mcc_bsplines.h | 109 ++++++++++++++++++++++++++++- include/mcc/mcc_pcm.h | 137 +++++++++++++++++++++++++++++++++---- 2 files changed, 231 insertions(+), 15 deletions(-) diff --git a/include/mcc/mcc_bsplines.h b/include/mcc/mcc_bsplines.h index 6e786bf..0b50dd5 100644 --- a/include/mcc/mcc_bsplines.h +++ b/include/mcc/mcc_bsplines.h @@ -366,4 +366,111 @@ int fitpack_eval_spl2d(const TXT& tx, return fitpack_eval_spl2d(tx, ty, coeffs, xv, yv, fv, kx, ky); } -} // namespace mcc::fitpack + +/* partial derivatives */ + +template +int fitpack_parder_spl2d(const TXT& tx, + const TYT& ty, + const CoeffT& coeffs, + const XT& x, + const YT& y, + PderT& pder, + int dx, // partial derivatives order along X + int dy, // partial derivatives order along Y + int kx = 3, + int ky = 3) +{ + static_assert(std::same_as, double> && + std::same_as, double> && + std::same_as, double> && + std::same_as, double> && + std::same_as, double> && + std::same_as, double>, + "Input ranges elements type must be double!"); + + if (kx < 0 || ky < 0 || dx < 0 || dy < 0) { + return 10; + } + + int ntx = std::ranges::size(tx); + int nty = std::ranges::size(ty); + + auto n_coeffs = (ntx - kx - 1) * (nty - ky - 1); + if (std::ranges::size(coeffs) < n_coeffs) { + return 10; + } + + int mx = std::ranges::size(x), my = std::ranges::size(y); + int N = mx * my; + + if (std::ranges::size(pder) < N) { + std::ranges::fill_n(std::back_inserter(pder), N - std::ranges::size(pder), 0.0); + } + + // mx >=1, my >=1, 0 <= nux < kx, 0 <= nuy < ky, kwrk>=mx+my + // lwrk>=mx*(kx+1-nux)+my*(ky+1-nuy)+(nx-kx-1)*(ny-ky-1), + + // compute sizes of working arrays according parder.f + int lwrk = mx * (kx + 1 - dx) + my * (ky + 1 - dy) + (ntx - kx - 1) * (nty - ky - 1); + std::vector wrk(lwrk); + + int kwrk = mx + my; + std::vector iwrk(kwrk); + + + auto tx_ptr = const_cast(std::ranges::data(tx)); + auto ty_ptr = const_cast(std::ranges::data(ty)); + auto coeffs_ptr = const_cast(std::ranges::data(coeffs)); + auto x_ptr = const_cast(std::ranges::data(x)); + auto y_ptr = const_cast(std::ranges::data(y)); + + int ier = 0; + + parder(tx_ptr, &ntx, ty_ptr, &nty, coeffs_ptr, &kx, &ky, &dx, &dy, x_ptr, &mx, y_ptr, &my, std::ranges::data(pder), + wrk.data(), &lwrk, iwrk.data(), &kwrk, &ier); + + return ier; +} + +// scalar version +template +int fitpack_parder_spl2d(const TXT& tx, + const TYT& ty, + const CoeffT& coeffs, + const XT& x, + const YT& y, + PderT& pder, + int dx, // partial derivatives order along X + int dy, // partial derivatives order along Y + int kx = 3, + int ky = 3) +{ + static_assert(std::same_as, double> && + std::same_as, double> && + std::same_as, double>, + "Input ranges elements type must be double!"); + + static_assert( + std::convertible_to && std::convertible_to && std::convertible_to, + "XT, YT and FuncT types must be a scalar convertible to double!"); + + auto xv = std::vector(1, x); + auto yv = std::vector(1, y); + auto pv = std::vector(1, pder); + + return fitpack_parder_spl2d(tx, ty, coeffs, xv, yv, pv, dx, dy, kx, ky); +} + + +} // namespace mcc::bsplines diff --git a/include/mcc/mcc_pcm.h b/include/mcc/mcc_pcm.h index a895f02..70a606e 100644 --- a/include/mcc/mcc_pcm.h +++ b/include/mcc/mcc_pcm.h @@ -26,7 +26,8 @@ enum class MccDefaultPCMErrorCode : int { ERROR_INVALID_INPUTS_BISPLEV, #endif ERROR_EXCEED_MAX_ITERS, - ERROR_NULLPTR + ERROR_NULLPTR, + ERROR_JACINV }; /* error category definition */ @@ -57,6 +58,8 @@ struct MccDefaultPCMCategory : public std::error_category { return "exceed maximum of iterations number"; case MccDefaultPCMErrorCode::ERROR_NULLPTR: return "nullptr input argument"; + case MccDefaultPCMErrorCode::ERROR_JACINV: + return "Jacobian is near singular"; default: return "UNKNOWN"; } @@ -316,14 +319,65 @@ public: std::lock_guard lock(*_pcmDataMutex); - ret = _compResult(x, y, res, true); - if (!ret) { + if (_pcmData.type != MccDefaultPCMType::PCM_TYPE_BSPLINE) { // compute using Newton-Raphson correction + struct { + double pcmX, pcmY; + } dfx, dfy, df; // partial derivatives + + ret = _computeFuncDeriv(x, y, res, true, &dfx, &dfy); + if (!ret) { + return ret; + } + + + dfx.pcmX += 1.0; // a + dfx.pcmY += 1.0; // b + dfy.pcmX += 1.0; // c + dfy.pcmY += 1.0; // d + + // Jacobian determinant + auto detJ = dfx.pcmX * dfy.pcmY - dfx.pcmY * dfy.pcmX; + if (utils::isEqual(detJ, 0.0)) { + return MccDefaultPCMErrorCode::ERROR_JACINV; + } + + // | a b | + // if A = | c d |, then + // + // -1 | d -b | + // A = 1/detA * | -c a | + // + + df.pcmX = dfy.pcmY * res->pcmX - dfx.pcmY * res->pcmY; + df.pcmY = -dfy.pcmX * res->pcmX + dfx.pcmX * res->pcmY; + + res->pcmX -= df.pcmX; + res->pcmY -= df.pcmY; + if constexpr (mcc_coord_pair_c) { - *hw_pt = - MccCoordPair{x + res->pcmX, y + res->pcmY, obs_skycoord.epoch()}; + *hw_pt = MccCoordPair{res->pcmX, res->pcmY, obs_skycoord.epoch()}; + } + } else { // for B-splines the result is computed directly from inverse B-spline coefficients + ret = _computeFuncDeriv(x, y, res); + + if (!ret) { + if constexpr (mcc_coord_pair_c) { + *hw_pt = MccCoordPair{x + res->pcmX, y + res->pcmY, + obs_skycoord.epoch()}; + } } } + + // ret = _compResult(x, y, res, true); + // if (!ret) { + // if constexpr (mcc_coord_pair_c) { + // *hw_pt = + // MccCoordPair{x + res->pcmX, y + res->pcmY, + // obs_skycoord.epoch()}; + // } + // } + return ret; } @@ -385,15 +439,21 @@ private: std::unique_ptr _pcmDataMutex{new std::mutex}; + // compute PCM function and its partial derivatives if asked + template error_t _computeFuncDeriv(double x, double y, mcc_pcm_result_c auto* res, bool inverse = false, - mcc_pcm_result_c auto* derivX = nullptr, - mcc_pcm_result_c auto* derivY = nullptr) + DXT derivX = nullptr, + DYT derivY = nullptr) + requires((std::is_null_pointer_v || mcc_pcm_result_c>) && + (std::is_null_pointer_v || mcc_pcm_result_c>)) { - if (inverse && !(derivX && derivY)) { - return MccDefaultPCMErrorCode::ERROR_NULLPTR; + if constexpr (std::is_null_pointer_v || std::is_null_pointer_v) { + if (_pcmData.type != MccDefaultPCMType::PCM_TYPE_BSPLINE && inverse) { + return MccDefaultPCMErrorCode::ERROR_NULLPTR; + } } pcm_geom_coeffs_t* geom_coeffs = &_pcmData.geomCoefficients; @@ -416,7 +476,7 @@ private: if (utils::isEqual(cosY, 0.0)) { res->pcmX = _pcmData.geomCoefficients.zeroPointX; - if (derivX) { + if constexpr (!std::is_null_pointer_v) { derivX->pcmX = 0.0; // dpcmX/dX derivX->pcmY = 0.0; // dpcmX/dY } @@ -426,7 +486,7 @@ private: geom_coeffs->misalignErr2 * sinX * tanY + geom_coeffs->tubeFlexure * _cosPhi * sinX / cosY - geom_coeffs->DECaxisFlexure * (_cosPhi * cosX + _sinPhi * tanY); - if (derivX) { + if constexpr (!std::is_null_pointer_v) { auto cos2Y = cosY * cosY; derivX->pcmX = (geom_coeffs->misalignErr1 * sinX + geom_coeffs->misalignErr2 * cosX) * tanY + @@ -444,7 +504,7 @@ private: res->pcmY = geom_coeffs->zeroPointY + geom_coeffs->misalignErr1 * sinX + geom_coeffs->misalignErr2 * cosX + geom_coeffs->tubeFlexure * (_cosPhi * cosX * sinY - _sinPhi * cosY); - if (derivY) { + if constexpr (!std::is_null_pointer_v) { derivY->pcmX = geom_coeffs->misalignErr1 * cosX - geom_coeffs->misalignErr2 * sinX - geom_coeffs->tubeFlexure * _cosPhi * sinX * sinY; // dpcmY/dX @@ -455,7 +515,7 @@ private: if (!utils::isEqual(cosX, 0.0)) { res->pcmY += geom_coeffs->forkFlexure / cosX; - if (derivY) { + if constexpr (!std::is_null_pointer_v) { derivY->pcmY += geom_coeffs->forkFlexure * sinX / cosX / cosY; // dpcmY/dY } } @@ -495,7 +555,54 @@ private: } // compute partial derivatives of the bivariate B-spline - if (_pcmData.type == MccDefaultPCMType::PCM_TYPE_BSPLINE && inverse) { + if (_pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE && inverse) { + double dspl_valX, dspl_valY; + + int ret = 0; + + if constexpr (!std::is_null_pointer_v) { + ret = + bsplines::fitpack_parder_spl2d(inv_bspline->knotsX, inv_bspline->knotsY, inv_bspline->coeffsX, x, y, + dspl_valX, 1, 0, inv_bspline->bsplDegreeX, inv_bspline->bsplDegreeY); + + if (ret) { + return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV; + } + + derivX->pcmX += dspl_valX; // dpcmX/dX + + ret = + bsplines::fitpack_parder_spl2d(inv_bspline->knotsX, inv_bspline->knotsY, inv_bspline->coeffsX, x, y, + dspl_valX, 0, 1, inv_bspline->bsplDegreeX, inv_bspline->bsplDegreeY); + + if (ret) { + return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV; + } + + derivX->pcmY += dspl_valX; // dpcmX/dY + } + + if constexpr (!std::is_null_pointer_v) { + ret = + bsplines::fitpack_parder_spl2d(inv_bspline->knotsX, inv_bspline->knotsY, inv_bspline->coeffsY, x, y, + dspl_valY, 1, 0, inv_bspline->bsplDegreeX, inv_bspline->bsplDegreeY); + + if (ret) { + return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV; + } + + derivY->pcmX += dspl_valY; // dpcmY/dX + + ret = + bsplines::fitpack_parder_spl2d(inv_bspline->knotsX, inv_bspline->knotsY, inv_bspline->coeffsY, x, y, + dspl_valY, 0, 1, inv_bspline->bsplDegreeX, inv_bspline->bsplDegreeY); + + if (ret) { + return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV; + } + + derivY->pcmY += dspl_valY; // dpcmY/dY + } } // for inverse PCM the inverse spline coefficients are used (derivatives are not computed)!!! @@ -508,6 +615,7 @@ private: if (ret) { res->pcmX = std::numeric_limits::quiet_NaN(); res->pcmY = std::numeric_limits::quiet_NaN(); + return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV; } @@ -518,6 +626,7 @@ private: if (ret) { res->pcmX = std::numeric_limits::quiet_NaN(); res->pcmY = std::numeric_limits::quiet_NaN(); + return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV; }