MccDefaultPCM: reverse PCM is computed with using partial derivatives

(start developing)

include/mcc/mcc_pcm.h

@@ -384,6 +384,153 @@ private:
 
     std::unique_ptr<std::mutex> _pcmDataMutex{new std::mutex};
 
+
+    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)
+    {
+        if (inverse && !(derivX && derivY)) {
+            return MccDefaultPCMErrorCode::ERROR_NULLPTR;
+        }
+
+        pcm_geom_coeffs_t* geom_coeffs = &_pcmData.geomCoefficients;
+
+#ifdef USE_BSPLINE_PCM
+        pcm_bspline_t* bspline = &_pcmData.bspline;
+        pcm_bspline_t* inv_bspline = &_pcmData.inverseBspline;
+#endif
+
+#ifdef USE_BSPLINE_PCM
+        if (_pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY ||
+            _pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE) {
+#endif
+            const auto tanY = std::tan(y);
+            const auto sinX = std::sin(x);
+            const auto cosX = std::cos(x);
+            const auto cosY = std::cos(y);
+            const auto sinY = std::sin(y);
+
+
+            if (utils::isEqual(cosY, 0.0)) {
+                res->pcmX = _pcmData.geomCoefficients.zeroPointX;
+                if (derivX) {
+                    derivX->pcmX = 0.0;  // dpcmX/dX
+                    derivX->pcmY = 0.0;  // dpcmX/dY
+                }
+            } else {
+                res->pcmX = geom_coeffs->zeroPointX + geom_coeffs->collimationErr / cosY +
+                            geom_coeffs->nonperpendErr * tanY - geom_coeffs->misalignErr1 * cosX * tanY +
+                            geom_coeffs->misalignErr2 * sinX * tanY + geom_coeffs->tubeFlexure * _cosPhi * sinX / cosY -
+                            geom_coeffs->DECaxisFlexure * (_cosPhi * cosX + _sinPhi * tanY);
+
+                if (derivX) {
+                    auto cos2Y = cosY * cosY;
+
+                    derivX->pcmX = (geom_coeffs->misalignErr1 * sinX + geom_coeffs->misalignErr2 * cosX) * tanY +
+                                   geom_coeffs->tubeFlexure * _cosPhi * cosX / cosY +
+                                   geom_coeffs->DECaxisFlexure * _cosPhi * sinX;  // dpcmX/dX
+
+                    derivX->pcmY =
+                        (geom_coeffs->collimationErr * sinY + geom_coeffs->nonperpendErr -
+                         geom_coeffs->misalignErr1 * cosX + geom_coeffs->misalignErr2 * sinX +
+                         geom_coeffs->tubeFlexure * _cosPhi * sinX * sinY - geom_coeffs->DECaxisFlexure * _sinPhi) /
+                        cos2Y;  // dpcmX/dY
+                }
+            }
+
+            res->pcmY = geom_coeffs->zeroPointY + geom_coeffs->misalignErr1 * sinX + geom_coeffs->misalignErr2 * cosX +
+                        geom_coeffs->tubeFlexure * (_cosPhi * cosX * sinY - _sinPhi * cosY);
+
+            if (derivY) {
+                derivY->pcmX = geom_coeffs->misalignErr1 * cosX - geom_coeffs->misalignErr2 * sinX -
+                               geom_coeffs->tubeFlexure * _cosPhi * sinX * sinY;  // dpcmY/dX
+
+                derivY->pcmY = geom_coeffs->tubeFlexure * (_cosPhi * cosX * cosY + _sinPhi * sinY);  // dpcmY/dY
+            }
+
+            if constexpr (pcmMountType == MccMountType::FORK_TYPE) {
+                if (!utils::isEqual(cosX, 0.0)) {
+                    res->pcmY += geom_coeffs->forkFlexure / cosX;
+
+                    if (derivY) {
+                        derivY->pcmY += geom_coeffs->forkFlexure * sinX / cosX / cosY;  // dpcmY/dY
+                    }
+                }
+            }
+#ifdef USE_BSPLINE_PCM
+        }
+#endif
+
+
+#ifdef USE_BSPLINE_PCM
+        if (_pcmData.type == MccDefaultPCMType::PCM_TYPE_BSPLINE ||
+            (_pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE && !inverse)) {
+            double spl_valX, spl_valY;
+
+            int ret = bsplines::fitpack_eval_spl2d(bspline->knotsX, bspline->knotsY, bspline->coeffsX, x, y, spl_valX,
+                                                   bspline->bsplDegreeX, bspline->bsplDegreeY);
+
+            if (ret) {
+                res->pcmX = std::numeric_limits<double>::quiet_NaN();
+                res->pcmY = std::numeric_limits<double>::quiet_NaN();
+                return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
+            }
+
+
+            ret = bsplines::fitpack_eval_spl2d(bspline->knotsX, bspline->knotsY, bspline->coeffsY, x, y, spl_valY,
+                                               bspline->bsplDegreeX, bspline->bsplDegreeY);
+
+            if (ret) {
+                res->pcmX = std::numeric_limits<double>::quiet_NaN();
+                res->pcmY = std::numeric_limits<double>::quiet_NaN();
+                return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
+            }
+
+
+            res->pcmX += spl_valX;
+            res->pcmY += spl_valY;
+        }
+
+        // compute partial derivatives of the bivariate B-spline
+        if (_pcmData.type == MccDefaultPCMType::PCM_TYPE_BSPLINE && inverse) {
+        }
+
+        // for inverse PCM the inverse spline coefficients are used (derivatives are not computed)!!!
+        if (_pcmData.type == MccDefaultPCMType::PCM_TYPE_BSPLINE && inverse) {
+            double spl_valX, spl_valY;
+
+            int ret = bsplines::fitpack_eval_spl2d(inv_bspline->knotsX, inv_bspline->knotsY, inv_bspline->coeffsX, x, y,
+                                                   spl_valX, inv_bspline->bsplDegreeX, inv_bspline->bsplDegreeY);
+
+            if (ret) {
+                res->pcmX = std::numeric_limits<double>::quiet_NaN();
+                res->pcmY = std::numeric_limits<double>::quiet_NaN();
+                return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
+            }
+
+
+            ret = bsplines::fitpack_eval_spl2d(inv_bspline->knotsX, inv_bspline->knotsY, inv_bspline->coeffsY, x, y,
+                                               spl_valY, inv_bspline->bsplDegreeX, inv_bspline->bsplDegreeY);
+
+            if (ret) {
+                res->pcmX = std::numeric_limits<double>::quiet_NaN();
+                res->pcmY = std::numeric_limits<double>::quiet_NaN();
+                return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
+            }
+
+
+            res->pcmX = spl_valX;
+            res->pcmY = spl_valY;
+        }
+#endif
+
+
+        return MccDefaultPCMErrorCode::ERROR_OK;
+    }
+
     error_t _compResult(double x, double y, mcc_pcm_result_c auto* res, bool inverse)
     {
         pcm_geom_coeffs_t* geom_coeffs;