From b5d73e54d3263011a2a7144c829f93f5a2b5e192 Mon Sep 17 00:00:00 2001 From: "Timur A. Fatkhullin" Date: Fri, 17 Apr 2026 16:15:50 +0300 Subject: [PATCH] fixes in PCM computing, fitting --- include/mcc/mcc_angle.h | 107 ++++++++++++++++++++++++++++------------ include/mcc/mcc_pcm.h | 105 ++++++++++++++++++++++++++++++--------- 2 files changed, 158 insertions(+), 54 deletions(-) diff --git a/include/mcc/mcc_angle.h b/include/mcc/mcc_angle.h index 94671ca..6298a21 100644 --- a/include/mcc/mcc_angle.h +++ b/include/mcc/mcc_angle.h @@ -104,6 +104,48 @@ public: NORM_KIND_90_90, // [-90,90] }; + // varous angle normalization + template + static double normalizeAngle(double ang) + { + ang = std::fmod(ang, MCC_TWO_PI); // to the range of [0, 2*PI] + + if constexpr (KIND == NORM_KIND_0_360) { + if (ang < 0.0) { + ang += MCC_TWO_PI; + } + } else if constexpr (KIND == NORM_KIND_0_180) { + if (ang < -std::numbers::pi) { + ang += MCC_TWO_PI; + } else if (ang < 0.0) { + ang = -ang; + } else if (ang > std::numbers::pi) { + ang = MCC_TWO_PI - ang; + } + } else if constexpr (KIND == NORM_KIND_180_180) { + if (ang > std::numbers::pi) { + ang -= MCC_TWO_PI; + } else if (ang < -std::numbers::pi) { + ang += MCC_TWO_PI; + } + } else if constexpr (KIND == NORM_KIND_90_90) { + if (ang >= 1.5 * std::numbers::pi) { + ang -= MCC_TWO_PI; + } else if (ang >= MCC_HALF_PI) { + ang = std::numbers::pi - ang; + } else if (ang <= -1.5 * std::numbers::pi) { + ang += MCC_TWO_PI; + } else if (ang <= -MCC_HALF_PI) { + ang = -(std::numbers::pi + ang); + } + } else { + static_assert(false, "UNKNOWN ANGLE NORMALIZATION TYPE!"); + } + + return ang; + } + + MccAngle() = default; // by default 'val' is in radians @@ -179,39 +221,42 @@ public: template MccAngle& normalize() { - // _angleInRads = std::fmod(_angleInRads, std::numbers::pi * 2.0); - _angleInRads = std::fmod(_angleInRads, MCC_TWO_PI); - - if constexpr (KIND == NORM_KIND_0_360) { - if (_angleInRads < 0.0) { - _angleInRads += 2.0 * std::numbers::pi; - } - } else if constexpr (KIND == NORM_KIND_0_180) { - if (_angleInRads < -std::numbers::pi) { - _angleInRads = MCC_TWO_PI + _angleInRads; - } else if (_angleInRads < 0.0) { - _angleInRads = -_angleInRads; - } - } else if constexpr (KIND == NORM_KIND_180_180) { - if (_angleInRads > std::numbers::pi) { - _angleInRads -= MCC_TWO_PI; - } else if (_angleInRads < -std::numbers::pi) { - _angleInRads += MCC_TWO_PI; - } - } else if constexpr (KIND == NORM_KIND_90_90) { - if (_angleInRads >= 1.5 * std::numbers::pi) { - // _angleInRads = _angleInRads - 2.0 * std::numbers::pi; - _angleInRads -= MCC_TWO_PI; - } else if (_angleInRads >= std::numbers::pi / 2.0) { - _angleInRads = std::numbers::pi - _angleInRads; - } else if (_angleInRads <= -1.5 * std::numbers::pi) { - _angleInRads += MCC_TWO_PI; - } else if (_angleInRads <= -std::numbers::pi / 2.0) { - _angleInRads = -(std::numbers::pi + _angleInRads); - } - } + _angleInRads = MccAngle::normalizeAngle(_angleInRads); return *this; + + // _angleInRads = std::fmod(_angleInRads, MCC_TWO_PI); + + // if constexpr (KIND == NORM_KIND_0_360) { + // if (_angleInRads < 0.0) { + // _angleInRads += 2.0 * std::numbers::pi; + // } + // } else if constexpr (KIND == NORM_KIND_0_180) { + // if (_angleInRads < -std::numbers::pi) { + // _angleInRads = MCC_TWO_PI + _angleInRads; + // } else if (_angleInRads < 0.0) { + // _angleInRads = -_angleInRads; + // } + // } else if constexpr (KIND == NORM_KIND_180_180) { + // if (_angleInRads > std::numbers::pi) { + // _angleInRads -= MCC_TWO_PI; + // } else if (_angleInRads < -std::numbers::pi) { + // _angleInRads += MCC_TWO_PI; + // } + // } else if constexpr (KIND == NORM_KIND_90_90) { + // if (_angleInRads >= 1.5 * std::numbers::pi) { + // // _angleInRads = _angleInRads - 2.0 * std::numbers::pi; + // _angleInRads -= MCC_TWO_PI; + // } else if (_angleInRads >= std::numbers::pi / 2.0) { + // _angleInRads = std::numbers::pi - _angleInRads; + // } else if (_angleInRads <= -1.5 * std::numbers::pi) { + // _angleInRads += MCC_TWO_PI; + // } else if (_angleInRads <= -std::numbers::pi / 2.0) { + // _angleInRads = -(std::numbers::pi + _angleInRads); + // } + // } + + // return *this; } MccAngle& normalize() diff --git a/include/mcc/mcc_pcm.h b/include/mcc/mcc_pcm.h index 822e3bb..e8c7860 100644 --- a/include/mcc/mcc_pcm.h +++ b/include/mcc/mcc_pcm.h @@ -30,6 +30,7 @@ enum class MccDefaultPCMErrorCode : int { ERROR_CCTE, #ifdef USE_BSPLINE_PCM ERROR_INVALID_INPUTS_BISPLEV, + ERROR_INVALID_BSPLINE_KNOTS, #endif ERROR_EXCEED_MAX_ITERS, ERROR_NULLPTR, @@ -59,6 +60,8 @@ struct MccDefaultPCMCategory : public std::error_category { #ifdef USE_BSPLINE_PCM case MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV: return "invalid input arguments for bispev"; + case MccDefaultPCMErrorCode::ERROR_INVALID_BSPLINE_KNOTS: + return "invalid B-spline knots vector"; #endif case MccDefaultPCMErrorCode::ERROR_EXCEED_MAX_ITERS: return "exceed maximum of iterations number"; @@ -161,14 +164,14 @@ public: size_t bsplDegreeX = 3; size_t bsplDegreeY = 3; - std::vector knotsX{}; - std::vector knotsY{}; + std::vector knotsX{}; // inner knots vector (at least 2-element) + std::vector knotsY{}; // inner knots vector (at least 2-element) - std::vector coeffsX{}; - std::vector coeffsY{}; + std::vector coeffsX{}; // B-spline coefficients for relation func(CelestialX - hardwareX) + std::vector coeffsY{}; // B-spline coefficients for relation func(CelestialY - hardwareY) - std::vector inverseCoeffsX{}; - std::vector inverseCoeffsY{}; + std::vector inverseCoeffsX{}; // B-spline coefficients for relation func(hardwareX - CelestialX) + std::vector inverseCoeffsY{}; // B-spline coefficients for relation func(hardwareY - CelestialY) }; #endif @@ -194,7 +197,7 @@ public: MccDefaultPCM() = default; MccDefaultPCM(pcm_data_t pdata) : MccDefaultPCM() { - _pcmData = std::move(pdata); + setPCMData(std::move(pdata)); } MccDefaultPCM(MccDefaultPCM&& other) = default; @@ -217,6 +220,36 @@ public: _cosPhi = std::cos(_pcmData.siteLatitude); _sinPhi = std::sin(_pcmData.siteLatitude); + +#ifdef USE_BSPLINE_PCM + // recompute full set of B-spline knots + if (_pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE || + _pcmData.type == MccDefaultPCMType::PCM_TYPE_BSPLINE) { + if (_pcmData.bspline.knotsX.size() > 1 && _pcmData.bspline.knotsY.size() > 1) { + _knotsX.resize(_pcmData.bspline.knotsX.size() + 6); + _knotsY.resize(_pcmData.bspline.knotsY.size() + 6); + } else { + _knotsX.clear(); + _knotsY.clear(); + } + + // full set of B-spline knots + _knotsX[0] = _knotsX[1] = _knotsX[2] = _pcmData.bspline.knotsX[0]; + _knotsX[_knotsX.size() - 1] = _knotsX[_knotsX.size() - 2] = _knotsX[_knotsX.size() - 3] = + _pcmData.bspline.knotsX.back(); + + _knotsY[0] = _knotsY[1] = _knotsY[2] = _pcmData.bspline.knotsY[0]; + _knotsY[_knotsY.size() - 1] = _knotsY[_knotsY.size() - 2] = _knotsY[_knotsY.size() - 3] = + _pcmData.bspline.knotsY.back(); + + for (size_t i = 0; i < _pcmData.bspline.knotsX.size(); ++i) { + _knotsX[3 + i] = _pcmData.bspline.knotsX[i]; + } + for (size_t i = 0; i < _pcmData.bspline.knotsY.size(); ++i) { + _knotsY[3 + i] = _pcmData.bspline.knotsY[i]; + } + } +#endif } @@ -262,7 +295,8 @@ public: res->pcmX = 0.0; res->pcmY = 0.0; - auto ret = _compResult(hw_coord.x(), hw_coord.y(), res, false); + // auto ret = _compResult(hw_coord.x(), hw_coord.y(), res, false); + auto ret = _computeFuncDeriv(hw_coord.x(), hw_coord.y(), res); if (ret) { return ret; } @@ -465,6 +499,8 @@ private: pcm_data_t _pcmData{}; double _cosPhi{}, _sinPhi{}; + std::vector _knotsX{}, _knotsY{}; // store a full set of B-spline knots + std::unique_ptr _pcmDataMutex{new std::mutex}; @@ -499,6 +535,12 @@ private: pcm_geom_coeffs_t* geom_coeffs = &_pcmData.geomCoefficients; #ifdef USE_BSPLINE_PCM + if (_pcmData.type == MccDefaultPCMType::PCM_TYPE_BSPLINE || + _pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE) { + if (_knotsX.empty() || _knotsY.empty()) { + return MccDefaultPCMErrorCode::ERROR_INVALID_BSPLINE_KNOTS; + } + } pcm_bspline_t* bspline = &_pcmData.bspline; // pcm_bspline_t* inv_bspline = &_pcmData.inverseBspline; #endif @@ -570,8 +612,11 @@ private: (_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, + int ret = bsplines::fitpack_eval_spl2d(_knotsX, _knotsY, bspline->coeffsX, x, y, spl_valX, bspline->bsplDegreeX, bspline->bsplDegreeY); + // 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(); @@ -580,8 +625,10 @@ private: } - ret = bsplines::fitpack_eval_spl2d(bspline->knotsX, bspline->knotsY, bspline->coeffsY, x, y, spl_valY, - bspline->bsplDegreeX, bspline->bsplDegreeY); + ret = bsplines::fitpack_eval_spl2d(_knotsX, _knotsY, bspline->coeffsY, x, y, spl_valY, bspline->bsplDegreeX, + bspline->bsplDegreeY); + // 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(); @@ -601,8 +648,10 @@ private: int ret = 0; if constexpr (!std::is_null_pointer_v) { - ret = bsplines::fitpack_parder_spl2d(bspline->knotsX, bspline->knotsY, bspline->coeffsX, x, y, - dspl_valX, 1, 0, bspline->bsplDegreeX, bspline->bsplDegreeY); + ret = bsplines::fitpack_parder_spl2d(_knotsX, _knotsY, bspline->coeffsX, x, y, dspl_valX, 1, 0, + bspline->bsplDegreeX, bspline->bsplDegreeY); + // ret = bsplines::fitpack_parder_spl2d(bspline->knotsX, bspline->knotsY, bspline->coeffsX, x, y, + // dspl_valX, 1, 0, bspline->bsplDegreeX, bspline->bsplDegreeY); if (ret) { return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV; @@ -610,8 +659,10 @@ private: derivX->pcmX += dspl_valX; // dpcmX/dX - ret = bsplines::fitpack_parder_spl2d(bspline->knotsX, bspline->knotsY, bspline->coeffsX, x, y, - dspl_valX, 0, 1, bspline->bsplDegreeX, bspline->bsplDegreeY); + ret = bsplines::fitpack_parder_spl2d(_knotsX, _knotsY, bspline->coeffsX, x, y, dspl_valX, 0, 1, + bspline->bsplDegreeX, bspline->bsplDegreeY); + // ret = bsplines::fitpack_parder_spl2d(bspline->knotsX, bspline->knotsY, bspline->coeffsX, x, y, + // dspl_valX, 0, 1, bspline->bsplDegreeX, bspline->bsplDegreeY); if (ret) { return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV; @@ -621,8 +672,10 @@ private: } if constexpr (!std::is_null_pointer_v) { - ret = bsplines::fitpack_parder_spl2d(bspline->knotsX, bspline->knotsY, bspline->coeffsY, x, y, - dspl_valY, 1, 0, bspline->bsplDegreeX, bspline->bsplDegreeY); + ret = bsplines::fitpack_parder_spl2d(_knotsX, _knotsY, bspline->coeffsY, x, y, dspl_valY, 1, 0, + bspline->bsplDegreeX, bspline->bsplDegreeY); + // ret = bsplines::fitpack_parder_spl2d(bspline->knotsX, bspline->knotsY, bspline->coeffsY, x, y, + // dspl_valY, 1, 0, bspline->bsplDegreeX, bspline->bsplDegreeY); if (ret) { return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV; @@ -630,8 +683,10 @@ private: derivY->pcmX += dspl_valY; // dpcmY/dX - ret = bsplines::fitpack_parder_spl2d(bspline->knotsX, bspline->knotsY, bspline->coeffsY, x, y, - dspl_valY, 0, 1, bspline->bsplDegreeX, bspline->bsplDegreeY); + ret = bsplines::fitpack_parder_spl2d(_knotsX, _knotsY, bspline->coeffsY, x, y, dspl_valY, 0, 1, + bspline->bsplDegreeX, bspline->bsplDegreeY); + // ret = bsplines::fitpack_parder_spl2d(bspline->knotsX, bspline->knotsY, bspline->coeffsY, x, y, + // dspl_valY, 0, 1, bspline->bsplDegreeX, bspline->bsplDegreeY); if (ret) { return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV; @@ -645,8 +700,10 @@ private: if (_pcmData.type == MccDefaultPCMType::PCM_TYPE_BSPLINE && inverse) { double spl_valX, spl_valY; - int ret = bsplines::fitpack_eval_spl2d(bspline->knotsX, bspline->knotsY, bspline->inverseCoeffsX, x, y, - spl_valX, bspline->bsplDegreeX, bspline->bsplDegreeY); + int ret = bsplines::fitpack_eval_spl2d(_knotsX, _knotsY, bspline->inverseCoeffsX, x, y, spl_valX, + bspline->bsplDegreeX, bspline->bsplDegreeY); + // int ret = bsplines::fitpack_eval_spl2d(bspline->knotsX, bspline->knotsY, bspline->inverseCoeffsX, x, y, + // spl_valX, bspline->bsplDegreeX, bspline->bsplDegreeY); if (ret) { res->pcmX = std::numeric_limits::quiet_NaN(); @@ -656,8 +713,10 @@ private: } - ret = bsplines::fitpack_eval_spl2d(bspline->knotsX, bspline->knotsY, bspline->inverseCoeffsY, x, y, - spl_valY, bspline->bsplDegreeX, bspline->bsplDegreeY); + ret = bsplines::fitpack_eval_spl2d(_knotsX, _knotsY, bspline->inverseCoeffsY, x, y, spl_valY, + bspline->bsplDegreeX, bspline->bsplDegreeY); + // ret = bsplines::fitpack_eval_spl2d(bspline->knotsX, bspline->knotsY, bspline->inverseCoeffsY, x, y, + // spl_valY, bspline->bsplDegreeX, bspline->bsplDegreeY); if (ret) { res->pcmX = std::numeric_limits::quiet_NaN();