add parabola

include/autoopt/interface.hpp

@@ -8,4 +8,9 @@ Eigen::VectorX<double> fit_ellipse(
     const std::vector<std::pair<double, double>>& data,
     const Eigen::VectorX<double>& inital_params,
     const Eigen::VectorX<double>& delta);
+
+Eigen::VectorX<double> fit_parabola(
+    const std::vector<std::pair<double, double>>& data,
+    const Eigen::VectorX<double>& inital_params,
+    const Eigen::VectorX<double>& delta);
 }  // namespace autoopt

include/autoopt/parabola.hpp

@@ -0,0 +1,27 @@
+#pragma once
+
+#include "autoopt/quadric.hpp"
+
+namespace autoopt
+{
+  
+template <typename T>
+struct parabola {
+  // T focal_length;
+  T exit_arm;
+  T entrance_angle;
+
+  quadric<T> to_quadric() const {
+    // T a = T{1} / (T{4} * focal_length);
+    // T x = T{2} * focal_length / std::tan(entrance_angle);
+    // T y = a * x * x;
+    T x = exit_arm * std::sin(T{2.0} * entrance_angle);
+    T f = T{0.5} * (T{1.0} - std::cos(T{2.0} * entrance_angle)) * exit_arm;
+    T a = T{1.0} / (T{4.0} * f);
+    T y = a * x * x;
+    quadric<T> q = quadric(a, T{0}, T{0}, T{0}, T{-1.0}, T{0});
+    return q.translated_by(-x, -y).rotated_by(entrance_angle - T{M_PI_2});
+  }
+};
+
+} // namespace autoopt