init

include/autoopt/quadric.hpp

@@ -0,0 +1,60 @@
+#pragma once
+
+#include <cmath>
+#include <autoopt/derivative.hpp>
+
+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 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