736 lines
25 KiB
C++
736 lines
25 KiB
C++
|
|
#pragma once
|
|
|
|
|
|
/****************************************************************************************
|
|
* *
|
|
* MOUNT CONTROL COMPONENTS LIBRARY *
|
|
* *
|
|
* *
|
|
* A REFERENCE "POINTING-CORRECTION-MODEL" CLASS IMPLEMENTATION *
|
|
* *
|
|
****************************************************************************************/
|
|
|
|
|
|
|
|
#include <mutex>
|
|
|
|
#ifdef USE_BSPLINE_PCM
|
|
#include "mcc_bsplines.h"
|
|
#endif
|
|
|
|
#include "mcc_concepts.h"
|
|
#include "mcc_coordinate.h"
|
|
|
|
namespace mcc::impl
|
|
{
|
|
|
|
enum class MccDefaultPCMErrorCode : int {
|
|
ERROR_OK,
|
|
ERROR_CCTE,
|
|
#ifdef USE_BSPLINE_PCM
|
|
ERROR_INVALID_INPUTS_BISPLEV,
|
|
#endif
|
|
ERROR_EXCEED_MAX_ITERS,
|
|
ERROR_NULLPTR,
|
|
ERROR_JACINV
|
|
};
|
|
|
|
/* error category definition */
|
|
|
|
// error category
|
|
struct MccDefaultPCMCategory : public std::error_category {
|
|
MccDefaultPCMCategory() : std::error_category() {}
|
|
|
|
const char* name() const noexcept
|
|
{
|
|
return "MCC-DEFAULT-PCM-ERROR-CATEGORY";
|
|
}
|
|
|
|
std::string message(int ec) const
|
|
{
|
|
MccDefaultPCMErrorCode err = static_cast<MccDefaultPCMErrorCode>(ec);
|
|
|
|
switch (err) {
|
|
case MccDefaultPCMErrorCode::ERROR_OK:
|
|
return "OK";
|
|
case MccDefaultPCMErrorCode::ERROR_CCTE:
|
|
return "CCTE calculation error";
|
|
#ifdef USE_BSPLINE_PCM
|
|
case MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV:
|
|
return "invalid input arguments for bispev";
|
|
#endif
|
|
case MccDefaultPCMErrorCode::ERROR_EXCEED_MAX_ITERS:
|
|
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";
|
|
}
|
|
}
|
|
|
|
static const MccDefaultPCMCategory& get()
|
|
{
|
|
static const MccDefaultPCMCategory constInst;
|
|
return constInst;
|
|
}
|
|
};
|
|
|
|
|
|
inline std::error_code make_error_code(MccDefaultPCMErrorCode ec)
|
|
{
|
|
return std::error_code(static_cast<int>(ec), MccDefaultPCMCategory::get());
|
|
}
|
|
} // namespace mcc::impl
|
|
|
|
|
|
namespace std
|
|
{
|
|
|
|
template <>
|
|
class is_error_code_enum<mcc::impl::MccDefaultPCMErrorCode> : public true_type
|
|
{
|
|
};
|
|
|
|
} // namespace std
|
|
|
|
|
|
|
|
namespace mcc::impl
|
|
{
|
|
|
|
|
|
// type of PCM corrections (algorithm used):
|
|
// PCM_TYPE_GEOMETRY - "classic" geometry-based correction coefficients
|
|
// PCM_TYPE_GEOMETRY_BSPLINE - previous one and additional 2D B-spline corrections
|
|
// PCM_TYPE_BSPLINE - pure 2D B-spline corrections
|
|
enum class MccDefaultPCMType { PCM_TYPE_GEOMETRY, PCM_TYPE_GEOMETRY_BSPLINE, PCM_TYPE_BSPLINE };
|
|
|
|
template <MccDefaultPCMType TYPE>
|
|
static constexpr std::string_view MccDefaultPCMTypeString =
|
|
TYPE == MccDefaultPCMType::PCM_TYPE_GEOMETRY ? "GEOMETRY"
|
|
#ifdef USE_BSPLINE_PCM
|
|
: TYPE == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE ? "GEOMETRY-BSPLINE"
|
|
: TYPE == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE ? "BSPLINE"
|
|
#endif
|
|
: "UNKNOWN";
|
|
|
|
template <MccMountType MOUNT_TYPE>
|
|
class MccDefaultPCM : public mcc_pcm_interface_t<std::error_code>
|
|
{
|
|
public:
|
|
// static constexpr MccMountType mountType = MOUNT_TYPE;
|
|
static constexpr MccMountType pcmMountType = MOUNT_TYPE;
|
|
|
|
#ifdef USE_BSPLINE_PCM
|
|
static constexpr std::string_view pcmName{"MCC-GEOMETRY-BSPLINES-PCM"};
|
|
#else
|
|
static constexpr std::string_view pcmName{"MCC-GEOMETRY-ONLY-PCM"};
|
|
#endif
|
|
|
|
typedef std::error_code error_t;
|
|
typedef double coord_t;
|
|
|
|
|
|
// "classic" geometric PCM coefficients
|
|
struct pcm_geom_coeffs_t {
|
|
typedef double coeff_t;
|
|
|
|
coeff_t zeroPointX;
|
|
coeff_t zeroPointY;
|
|
coeff_t collimationErr; // tube collimation error
|
|
coeff_t nonperpendErr; // X-Y axes nonperpendicularity
|
|
coeff_t misalignErr1; // misalignment of hour-angle/azimuth axis: left-right for equatorial, East-West for
|
|
// alt-azimuthal
|
|
coeff_t misalignErr2; // misalignment of hour-angle/azimuth axis: vertical for equatorial, North-South for
|
|
// alt-azimuthal
|
|
|
|
coeff_t tubeFlexure;
|
|
coeff_t forkFlexure;
|
|
coeff_t DECaxisFlexure; // declination axis flexure
|
|
};
|
|
|
|
#ifdef USE_BSPLINE_PCM
|
|
// B-splines related data structure (coefficients, knots ...)
|
|
struct pcm_bspline_t {
|
|
typedef double knot_t;
|
|
typedef double coeff_t;
|
|
|
|
size_t bsplDegreeX = 3;
|
|
size_t bsplDegreeY = 3;
|
|
|
|
std::vector<knot_t> knotsX{};
|
|
std::vector<knot_t> knotsY{};
|
|
|
|
std::vector<coeff_t> coeffsX{};
|
|
std::vector<coeff_t> coeffsY{};
|
|
|
|
std::vector<coeff_t> inverseCoeffsX{};
|
|
std::vector<coeff_t> inverseCoeffsY{};
|
|
};
|
|
#endif
|
|
|
|
struct pcm_data_t {
|
|
MccDefaultPCMType type{MccDefaultPCMType::PCM_TYPE_GEOMETRY};
|
|
double siteLatitude{0.0}; // in radians
|
|
|
|
// from encoder to observed celestial
|
|
pcm_geom_coeffs_t geomCoefficients{};
|
|
#ifdef USE_BSPLINE_PCM
|
|
pcm_bspline_t bspline{};
|
|
#endif
|
|
|
|
// from observed celestial to encoder
|
|
pcm_geom_coeffs_t inverseGeomCoefficients{};
|
|
#ifdef USE_BSPLINE_PCM
|
|
pcm_bspline_t inverseBspline{};
|
|
#endif
|
|
};
|
|
|
|
// constructors
|
|
|
|
MccDefaultPCM() = default;
|
|
MccDefaultPCM(pcm_data_t pdata) : MccDefaultPCM()
|
|
{
|
|
_pcmData = std::move(pdata);
|
|
}
|
|
|
|
MccDefaultPCM(MccDefaultPCM&& other) = default;
|
|
MccDefaultPCM& operator=(MccDefaultPCM&& other) = default;
|
|
|
|
MccDefaultPCM(const MccDefaultPCM&) = delete;
|
|
MccDefaultPCM& operator=(const MccDefaultPCM&) = delete;
|
|
|
|
virtual ~MccDefaultPCM() = default;
|
|
|
|
// virtual ~MccDefaultPCM() {
|
|
// // _pcmData.geomCoefficients.DECaxisFlexure = 0.0;
|
|
// };
|
|
|
|
void setPCMData(pcm_data_t pdata)
|
|
{
|
|
std::lock_guard lock(*_pcmDataMutex);
|
|
|
|
_pcmData = std::move(pdata);
|
|
|
|
_cosPhi = std::cos(_pcmData.siteLatitude);
|
|
_sinPhi = std::sin(_pcmData.siteLatitude);
|
|
}
|
|
|
|
|
|
pcm_data_t getPCMData() const
|
|
{
|
|
std::lock_guard lock(*_pcmDataMutex);
|
|
|
|
return _pcmData;
|
|
}
|
|
|
|
void setPCMType(MccDefaultPCMType type)
|
|
{
|
|
std::lock_guard lock(*_pcmDataMutex);
|
|
|
|
#ifdef USE_BSPLINE_PCM
|
|
_pcmData.type = type;
|
|
#else
|
|
_pcmData.type = MccDefaultPCMType::PCM_TYPE_GEOMETRY;
|
|
#endif
|
|
}
|
|
|
|
MccDefaultPCMType getPCMType() const
|
|
{
|
|
std::lock_guard lock(*_pcmDataMutex);
|
|
|
|
return _pcmData.type;
|
|
}
|
|
|
|
// The computed PCM quantities must be interpretated as:
|
|
// observed_X = encoder_X + res.pcmX
|
|
// observed_Y = encoder_Y + res.pcmY
|
|
// so, input hw_coord is assumed to be coordinates pair of mount axis encoder readouts
|
|
template <typename T = std::nullptr_t>
|
|
error_t computePCM(mcc_coord_pair_c auto const& hw_coord, mcc_pcm_result_c auto* res, T* obs_skycoord = nullptr)
|
|
requires(mcc_skypoint_c<T> || std::is_null_pointer_v<T>)
|
|
{
|
|
if (res == nullptr) {
|
|
return MccDefaultPCMErrorCode::ERROR_NULLPTR;
|
|
}
|
|
|
|
std::lock_guard lock(*_pcmDataMutex);
|
|
|
|
res->pcmX = 0.0;
|
|
res->pcmY = 0.0;
|
|
|
|
auto ret = _compResult(hw_coord.x(), hw_coord.y(), res, false);
|
|
if (ret) {
|
|
return ret;
|
|
}
|
|
|
|
if constexpr (mcc_is_equatorial_mount<MOUNT_TYPE>) { // equatorial (HA and DEC axes)
|
|
if constexpr (mcc_skypoint_c<T>) {
|
|
obs_skycoord->from(MccSkyHADEC_OBS{double(hw_coord.x() + res->pcmX), double(hw_coord.y() + res->pcmY),
|
|
hw_coord.epoch()});
|
|
}
|
|
|
|
} else if constexpr (mcc_is_altaz_mount<MOUNT_TYPE>) {
|
|
if constexpr (mcc_skypoint_c<T>) {
|
|
obs_skycoord->from(
|
|
MccSkyAZZD{double(hw_coord.x() + res->pcmX), double(hw_coord.y() + res->pcmY), hw_coord.epoch()});
|
|
}
|
|
} else {
|
|
static_assert(false, "UNSUPPORTED MOUNT TYPE");
|
|
}
|
|
|
|
|
|
return MccDefaultPCMErrorCode::ERROR_OK;
|
|
}
|
|
|
|
//
|
|
// computed inverse PCM corrections must be interpretated as:
|
|
// X_encoder = X_observed + res->pcmX
|
|
// Y_encoder = Y_observed + res->pcmY
|
|
//
|
|
template <typename T>
|
|
error_t computeInversePCM(mcc_skypoint_c auto const& obs_skycoord, mcc_pcm_result_c auto* res, T* hw_pt = nullptr)
|
|
requires(mcc_coord_pair_c<T> || std::is_null_pointer_v<T>)
|
|
{
|
|
error_t ret = MccDefaultPCMErrorCode::ERROR_OK;
|
|
|
|
if constexpr (mcc_coord_pair_c<T>) {
|
|
static_assert(
|
|
T::pairKind == MccCoordPairKind::COORDS_KIND_GENERIC || T::pairKind == MccCoordPairKind::COORDS_KIND_XY,
|
|
"Invalid output coordinate pair type!");
|
|
}
|
|
|
|
// to be computed as observed celestial X and Y cordinate according to mount type (HA-DEC or AZ-ZD)
|
|
double x, y;
|
|
|
|
auto getXY = [&, this](auto& cp) -> error_t {
|
|
auto err = obs_skycoord.toAtSameEpoch(cp);
|
|
if (err) {
|
|
return mcc_deduced_err(err, MccDefaultPCMErrorCode::ERROR_CCTE);
|
|
}
|
|
|
|
x = cp.x();
|
|
y = cp.y();
|
|
|
|
return MccDefaultPCMErrorCode::ERROR_OK;
|
|
};
|
|
|
|
if constexpr (mcc_is_equatorial_mount<MOUNT_TYPE>) {
|
|
MccSkyHADEC_OBS hadec;
|
|
ret = getXY(hadec);
|
|
} else if constexpr (mcc_is_altaz_mount<MOUNT_TYPE>) {
|
|
MccSkyAZZD azzd;
|
|
ret = getXY(azzd);
|
|
}
|
|
|
|
std::lock_guard lock(*_pcmDataMutex);
|
|
|
|
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 = MccCoordPair<typename T::x_t, typename T::y_t>{x + res->pcmX, y + res->pcmY,
|
|
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;
|
|
}
|
|
|
|
/*
|
|
template <typename T>
|
|
error_t computeInversePCM(mcc_skypoint_c auto const& obs_skycoord,
|
|
mcc_pcm_result_c auto* result,
|
|
T* hw_pt = nullptr)
|
|
requires(mcc_coord_pair_c<T> || std::is_null_pointer_v<T>)
|
|
{
|
|
error_t ret = MccDefaultPCMErrorCode::ERROR_OK;
|
|
|
|
if constexpr (mcc_coord_pair_c<T>) {
|
|
static_assert(
|
|
T::pairKind == MccCoordPairKind::COORDS_KIND_GENERIC || T::pairKind == MccCoordPairKind::COORDS_KIND_XY,
|
|
"Invalid output coordinate pair type!");
|
|
}
|
|
|
|
double x, y;
|
|
|
|
auto comp_func = [&, this](auto& cp) {
|
|
auto err = obs_skycoord.toAtSameEpoch(cp);
|
|
if (err) {
|
|
return mcc_deduced_err(err, MccDefaultPCMErrorCode::ERROR_CCTE);
|
|
}
|
|
|
|
x = cp.x();
|
|
y = cp.y();
|
|
|
|
return computePCM(cp, result);
|
|
};
|
|
|
|
|
|
// for small corrections only!!!
|
|
if constexpr (mcc_is_equatorial_mount<MOUNT_TYPE>) {
|
|
MccSkyHADEC_OBS hadec;
|
|
ret = comp_func(hadec);
|
|
} else if constexpr (mcc_is_altaz_mount<MOUNT_TYPE>) {
|
|
MccSkyAZZD azzd;
|
|
ret = comp_func(azzd);
|
|
}
|
|
|
|
if (ret) {
|
|
return ret;
|
|
}
|
|
|
|
if constexpr (mcc_coord_pair_c<T>) {
|
|
*hw_pt = MccCoordPair<typename T::x_t, typename T::y_t>{x - result->pcmX, y - result->pcmY};
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
*/
|
|
|
|
private:
|
|
pcm_data_t _pcmData{};
|
|
double _cosPhi{}, _sinPhi{};
|
|
|
|
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,
|
|
DXT derivX = 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 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;
|
|
|
|
#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 constexpr (!std::is_null_pointer_v<DXT>) {
|
|
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 constexpr (!std::is_null_pointer_v<DXT>) {
|
|
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 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
|
|
|
|
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 constexpr (!std::is_null_pointer_v<DYT>) {
|
|
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_GEOMETRY_BSPLINE && inverse) {
|
|
double dspl_valX, dspl_valY;
|
|
|
|
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);
|
|
|
|
if (ret) {
|
|
return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
|
|
}
|
|
|
|
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);
|
|
|
|
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(bspline->knotsX, bspline->knotsY, bspline->coeffsY, x, y,
|
|
dspl_valY, 1, 0, bspline->bsplDegreeX, bspline->bsplDegreeY);
|
|
|
|
if (ret) {
|
|
return MccDefaultPCMErrorCode::ERROR_INVALID_INPUTS_BISPLEV;
|
|
}
|
|
|
|
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);
|
|
|
|
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)!!!
|
|
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);
|
|
|
|
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->inverseCoeffsY, 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;
|
|
}
|
|
#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;
|
|
#ifdef USE_BSPLINE_PCM
|
|
pcm_bspline_t* bspline;
|
|
#endif
|
|
|
|
geom_coeffs = inverse ? &_pcmData.inverseGeomCoefficients : &_pcmData.geomCoefficients;
|
|
#ifdef USE_BSPLINE_PCM
|
|
bspline = inverse ? &_pcmData.inverseBspline : &_pcmData.bspline;
|
|
#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);
|
|
|
|
|
|
if (utils::isEqual(cosY, 0.0)) {
|
|
res->pcmX = _pcmData.geomCoefficients.zeroPointX;
|
|
} 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);
|
|
}
|
|
|
|
res->pcmY = geom_coeffs->zeroPointY + geom_coeffs->misalignErr1 * sinX + geom_coeffs->misalignErr2 * cosX +
|
|
geom_coeffs->tubeFlexure * (_cosPhi * cosX * std::sin(y) - _sinPhi * cosY);
|
|
|
|
// if constexpr (mountType == MccMountType::FORK_TYPE) {
|
|
if constexpr (pcmMountType == MccMountType::FORK_TYPE) {
|
|
if (!utils::isEqual(cosX, 0.0)) {
|
|
res->pcmY += geom_coeffs->forkFlexure / cosX;
|
|
}
|
|
}
|
|
#ifdef USE_BSPLINE_PCM
|
|
}
|
|
#endif
|
|
|
|
|
|
#ifdef USE_BSPLINE_PCM
|
|
if (_pcmData.type == MccDefaultPCMType::PCM_TYPE_BSPLINE ||
|
|
_pcmData.type == MccDefaultPCMType::PCM_TYPE_GEOMETRY_BSPLINE) {
|
|
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;
|
|
}
|
|
#endif
|
|
|
|
return MccDefaultPCMErrorCode::ERROR_OK;
|
|
}
|
|
};
|
|
|
|
|
|
typedef MccDefaultPCM<MccMountType::ALTAZ_TYPE> MccMountDefaultAltAzPec;
|
|
typedef MccDefaultPCM<MccMountType::FORK_TYPE> MccMountDefaultForkPec;
|
|
|
|
static_assert(mcc_pcm_c<MccMountDefaultForkPec>, "");
|
|
static_assert(std::movable<MccMountDefaultForkPec>);
|
|
|
|
|
|
} // namespace mcc::impl
|