init

include/autoopt/ellipse.hpp

@@ -0,0 +1,50 @@
+#pragma once
+
+#include "autoopt/quadric.hpp"
+
+namespace autoopt {
+
+template <typename T>
+struct ellipse {
+  T left_arm;
+  T right_arm;
+  T entrance_angle;
+
+  constexpr quadric<T> to_quadric() const {
+    T a = (left_arm + right_arm) / T{2};
+    T a2 = a * a;
+    
+    T left_x = -left_arm * std::cos(entrance_angle);
+    T left_y = left_arm * std::sin(entrance_angle);
+
+    T right_x = right_arm * std::cos(entrance_angle);
+    T right_y = right_arm * std::sin(entrance_angle);
+
+    T c_x = (left_x + right_x) / T{2};
+    T c_y = (left_y + right_y) / T{2};
+
+    T c2 = (left_x - c_x) * (left_x - c_x) + (left_y - c_y) * (left_y - c_y);
+    T b2 = a2 - c2;
+    
+
+    // source: https://en.wikipedia.org/wiki/Ellipse#General_ellipse
+
+    T theta = std::atan2(right_y - left_y, right_x - left_x);
+    T cos_t = std::cos(theta);
+    T sin_t = std::sin(theta);
+    T cos_t2 = cos_t * cos_t;
+    T sin_t2 = sin_t * sin_t;
+    T sico_t = sin_t * cos_t;
+
+    T A = a2 * sin_t2 + b2 * cos_t2;
+    T B = T{2} * (b2 - a2) * sico_t;
+    T C = a2 * cos_t2 + b2 * sin_t2;
+    T D = -T{2} * A * c_x - B * c_y;
+    T E = -B * c_x - T{2} * C * c_y;
+    T F = A * c_x * c_x + B * c_x * c_y + C * c_y * c_y - a2 * b2;
+    
+    return quadric<T>(A, B, C, D, E, F);
+  }
+};
+
+} // namespace autoopt