54 lines
1.8 KiB
C++
54 lines
1.8 KiB
C++
#include "fgc/Geometry.h"
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
|
|
namespace fgc {
|
|
|
|
double wrap360(double deg) {
|
|
double w = std::fmod(deg, 360.0);
|
|
if (w < 0.0) w += 360.0;
|
|
// fmod of a tiny negative can round to exactly 360.0 after the += above.
|
|
if (w >= 360.0) w = 0.0;
|
|
return w;
|
|
}
|
|
|
|
double wrap180(double deg) {
|
|
double w = wrap360(deg);
|
|
return w >= 180.0 ? w - 360.0 : w;
|
|
}
|
|
|
|
long AxisMap::toCounts(double deg) const {
|
|
const double lo = std::min(min_deg, max_deg);
|
|
const double hi = std::max(min_deg, max_deg);
|
|
const double clamped = std::clamp(deg, lo, hi);
|
|
return std::lround(static_cast<double>(center_count) +
|
|
(clamped - deg_at_center) * counts_per_deg);
|
|
}
|
|
|
|
double AxisMap::toDeg(long counts) const {
|
|
if (std::abs(counts_per_deg) < 1e-9) return deg_at_center;
|
|
return deg_at_center +
|
|
(static_cast<double>(counts) - static_cast<double>(center_count)) / counts_per_deg;
|
|
}
|
|
|
|
double AxisMap::headingDeg(long counts) const { return wrap360(toDeg(counts)); }
|
|
|
|
std::optional<long> AxisMap::headingToCounts(double heading_deg) const {
|
|
if (std::abs(counts_per_deg) < 1e-9) return std::nullopt;
|
|
// The travel is under a full turn, so wrapping the offset into [-180, 180)
|
|
// yields the one candidate that could be reachable - there is never a second
|
|
// solution to choose between.
|
|
const double deg = deg_at_center + wrap180(heading_deg - deg_at_center);
|
|
const double lo = std::min(min_deg, max_deg);
|
|
const double hi = std::max(min_deg, max_deg);
|
|
if (deg < lo || deg > hi) return std::nullopt;
|
|
return toCounts(deg);
|
|
}
|
|
|
|
double encoderPitchDeg(const AxisMap& pitch, std::optional<long> counts) {
|
|
return counts ? pitch.toDeg(*counts) : pitch.deg_at_center;
|
|
}
|
|
|
|
} // namespace fgc
|