60 lines
1.6 KiB
C++
60 lines
1.6 KiB
C++
#pragma once
|
|
|
|
#include "autoopt/parameters.hpp"
|
|
#include "autoopt/quadric.hpp"
|
|
|
|
namespace autoopt {
|
|
|
|
struct ellipse_new : curve_base {
|
|
// T focal_length;
|
|
ellipse_new() : curve_base() {
|
|
initial = Eigen::VectorXd::Zero(4);
|
|
delta = Eigen::VectorXd::Zero(4);
|
|
param_names =
|
|
std::vector<const char*>{"left arm", "right arm", "entrance angle", "rotation"};
|
|
}
|
|
|
|
template <typename T>
|
|
quadric<T> grapher_from_params(const Eigen::VectorX<T>& params) const {
|
|
T left_arm = params(0);
|
|
T right_arm = params(1);
|
|
T entrance_angle = params(2);
|
|
T alpha = params(3);
|
|
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).rotated_by(alpha);
|
|
}
|
|
};
|
|
|
|
} // namespace autoopt
|