71 lines
1.8 KiB
C++
71 lines
1.8 KiB
C++
#pragma once
|
|
|
|
#include <autoopt/derivative.hpp>
|
|
#include <cmath>
|
|
|
|
namespace autoopt {
|
|
|
|
template <typename T>
|
|
struct quadric {
|
|
T _A; // x² coefficient
|
|
T _B; // xy coefficient
|
|
T _C; // y² coefficient
|
|
T _D; // x coefficient
|
|
T _E; // y coefficient
|
|
T _F; // constant term
|
|
|
|
constexpr quadric(T A = 0, T B = 0, T C = 0, T D = 0, T E = 0, T F = 0)
|
|
: _A(A), _B(B), _C(C), _D(D), _E(E), _F(F) {}
|
|
|
|
constexpr quadric rotated_by(T angle_rad) const {
|
|
T co = std::cos(angle_rad);
|
|
T si = std::sin(angle_rad);
|
|
T co_2 = co * co;
|
|
T si_2 = si * si;
|
|
T co_si = co * si;
|
|
|
|
T A_new = _A * co_2 - _B * co_si + _C * si_2;
|
|
T B_new = T{2} * (_A - _C) * co_si + _B * (co_2 - si_2);
|
|
T C_new = _A * si_2 + _B * co_si + _C * co_2;
|
|
T D_new = _D * co - _E * si;
|
|
T E_new = _D * si + _E * co;
|
|
T F_new = _F;
|
|
|
|
return quadric(A_new, B_new, C_new, D_new, E_new, F_new);
|
|
}
|
|
|
|
constexpr quadric translated_by(T x_o, T y_o) const {
|
|
T D_new = _D - T{2} * _A * x_o - _B * y_o;
|
|
T E_new = _E - _B * x_o - T{2} * _C * y_o;
|
|
T F_new = _F + _A * x_o * x_o + _B * x_o * y_o + _C * y_o * y_o - _D * x_o -
|
|
_E * y_o;
|
|
return quadric(_A, _B, _C, D_new, E_new, F_new);
|
|
}
|
|
|
|
constexpr T at(T x) const {
|
|
T sign = T{-1};
|
|
T Bx_E = _B * x + _E;
|
|
T Ax2_Dx_F = _A * x * x + _D * x + _F;
|
|
|
|
if (_C == T{0}) {
|
|
return -Ax2_Dx_F / Bx_E;
|
|
}
|
|
|
|
T discriminant = Bx_E * Bx_E - T{4} * _C * Ax2_Dx_F;
|
|
T num = sign * std::sqrt(discriminant) - Bx_E;
|
|
T denom = T{2} * _C;
|
|
return num / denom;
|
|
}
|
|
|
|
constexpr T slope_at(T x) const {
|
|
return derivative(
|
|
[&]<typename U>(U x_val) {
|
|
quadric<U> q{U{_A}, U{_B}, U{_C}, U{_D}, U{_E}, U{_F}};
|
|
return q.at(x_val);
|
|
},
|
|
x);
|
|
}
|
|
};
|
|
|
|
} // namespace autoopt
|