...

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 <std::ranges::contiguous_range TXT,
+          std::ranges::contiguous_range TYT,
+          std::ranges::contiguous_range XT,
+          std::ranges::contiguous_range YT,
+          std::ranges::contiguous_range CoeffT,
+          std::ranges::contiguous_range PderT>
+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<std::ranges::range_value_t<TXT>, double> &&
+                      std::same_as<std::ranges::range_value_t<TYT>, double> &&
+                      std::same_as<std::ranges::range_value_t<XT>, double> &&
+                      std::same_as<std::ranges::range_value_t<YT>, double> &&
+                      std::same_as<std::ranges::range_value_t<CoeffT>, double> &&
+                      std::same_as<std::ranges::range_value_t<PderT>, 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<double> wrk(lwrk);
+
+    int kwrk = mx + my;
+    std::vector<int> iwrk(kwrk);
+
+
+    auto tx_ptr = const_cast<double*>(std::ranges::data(tx));
+    auto ty_ptr = const_cast<double*>(std::ranges::data(ty));
+    auto coeffs_ptr = const_cast<double*>(std::ranges::data(coeffs));
+    auto x_ptr = const_cast<double*>(std::ranges::data(x));
+    auto y_ptr = const_cast<double*>(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 <std::ranges::contiguous_range TXT,
+          std::ranges::contiguous_range TYT,
+          typename XT,
+          typename YT,
+          std::ranges::contiguous_range CoeffT,
+          typename PderT>
+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<std::ranges::range_value_t<TXT>, double> &&
+                      std::same_as<std::ranges::range_value_t<TYT>, double> &&
+                      std::same_as<std::ranges::range_value_t<CoeffT>, double>,
+                  "Input ranges elements type must be double!");
+
+    static_assert(
+        std::convertible_to<XT, double> && std::convertible_to<YT, double> && std::convertible_to<PderT, double>,
+        "XT, YT and FuncT types must be a scalar convertible to double!");
+
+    auto xv = std::vector<double>(1, x);
+    auto yv = std::vector<double>(1, y);
+    auto pv = std::vector<double>(1, pder);
+
+    return fitpack_parder_spl2d(tx, ty, coeffs, xv, yv, pv, dx, dy, kx, ky);
+}
+
+
+}  // namespace mcc::bsplines

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,13 +319,64 @@ public:
 
         std::lock_guard lock(*_pcmDataMutex);
 
-        ret = _compResult(x, y, res, true);
+        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<T>) {
+                *hw_pt = MccCoordPair<typename T::x_t, typename T::y_t>{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<T>) {
-                *hw_pt =
+                    *hw_pt = MccCoordPair<typename T::x_t, typename T::y_t>{x + res->pcmX, y + res->pcmY,
-                    MccCoordPair<typename T::x_t, typename T::y_t>{x + res->pcmX, y + res->pcmY, obs_skycoord.epoch()};
+                                                                            obs_skycoord.epoch()};
                 }
             }
+        }
+
+
+        // ret = _compResult(x, y, res, true);
+        // if (!ret) {
+        //     if constexpr (mcc_coord_pair_c<T>) {
+        //         *hw_pt =
+        //             MccCoordPair<typename T::x_t, typename T::y_t>{x + res->pcmX, y + res->pcmY,
+        //             obs_skycoord.epoch()};
+        //     }
+        // }
 
         return ret;
     }
@@ -385,16 +439,22 @@ private:
     std::unique_ptr<std::mutex> _pcmDataMutex{new std::mutex};
 
 
+    // compute PCM function and its partial derivatives if asked
+    template <typename DXT = std::nullptr_t, typename DYT = std::nullptr_t>
     error_t _computeFuncDeriv(double x,
                               double y,
                               mcc_pcm_result_c auto* res,
                               bool inverse = false,
-                              mcc_pcm_result_c auto* derivX = nullptr,
+                              DXT derivX = nullptr,
-                              mcc_pcm_result_c auto* derivY = nullptr)
+                              DYT derivY = nullptr)
+        requires((std::is_null_pointer_v<DXT> || mcc_pcm_result_c<std::remove_pointer_t<DXT>>) &&
+                 (std::is_null_pointer_v<DYT> || mcc_pcm_result_c<std::remove_pointer_t<DYT>>))
     {
-        if (inverse && !(derivX && derivY)) {
+        if constexpr (std::is_null_pointer_v<DXT> || std::is_null_pointer_v<DYT>) {
+            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<DXT>) {
                     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<DXT>) {
                     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<DYT>) {
                 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<DYT>) {
                         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<DXT>) {
+                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<DYT>) {
+                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<double>::quiet_NaN();
                 res->pcmY = std::numeric_limits<double>::quiet_NaN();
+
                 return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
             }
 
@@ -518,6 +626,7 @@ private:
             if (ret) {
                 res->pcmX = std::numeric_limits<double>::quiet_NaN();
                 res->pcmY = std::numeric_limits<double>::quiet_NaN();
+
                 return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
             }