diff --git a/include/mcc/mcc_pcm.h b/include/mcc/mcc_pcm.h index e2e0cc4..a895f02 100644 --- a/include/mcc/mcc_pcm.h +++ b/include/mcc/mcc_pcm.h @@ -384,6 +384,153 @@ private: std::unique_ptr _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::quiet_NaN(); + res->pcmY = std::numeric_limits::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::quiet_NaN(); + res->pcmY = std::numeric_limits::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::quiet_NaN(); + res->pcmY = std::numeric_limits::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::quiet_NaN(); + res->pcmY = std::numeric_limits::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;