timur/mcc
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fixes in PCM computing, fitting

Browse Source
This commit is contained in:
Timur A. Fatkhullin
2026-04-17 16:15:50 +03:00
parent 4fbe456892
commit b5d73e54d3
2 changed files with 158 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

107
include/mcc/mcc_angle.h
View File

@@ -104,6 +104,48 @@ public:
NORM_KIND_90_90, // [-90,90]
};
// varous angle normalization
template <norm_kind_t KIND>
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 <norm_kind_t KIND>
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<KIND>(_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()

105
include/mcc/mcc_pcm.h
View File

@@ -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<knot_t> knotsX{};
std::vector<knot_t> knotsY{};
std::vector<knot_t> knotsX{}; // inner knots vector (at least 2-element)
std::vector<knot_t> knotsY{}; // inner knots vector (at least 2-element)
std::vector<coeff_t> coeffsX{};
std::vector<coeff_t> coeffsY{};
std::vector<coeff_t> coeffsX{}; // B-spline coefficients for relation func(CelestialX - hardwareX)
std::vector<coeff_t> coeffsY{}; // B-spline coefficients for relation func(CelestialY - hardwareY)
std::vector<coeff_t> inverseCoeffsX{};
std::vector<coeff_t> inverseCoeffsY{};
std::vector<coeff_t> inverseCoeffsX{}; // B-spline coefficients for relation func(hardwareX - CelestialX)
std::vector<coeff_t> 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<double> _knotsX{}, _knotsY{}; // store a full set of B-spline knots
std::unique_ptr<std::mutex> _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<double>::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<double>::quiet_NaN();
@@ -601,8 +648,10 @@ private:
int ret = 0;
if constexpr (!std::is_null_pointer_v<DXT>) {
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<DYT>) {
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<double>::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<double>::quiet_NaN();