|
@ -46,7 +46,7 @@ void polyline::build_as_axis(const polyline_descriptor_t |
|
|
aabb_t<2> profile_aabb; |
|
|
aabb_t<2> profile_aabb; |
|
|
Eigen::Vector3d proj_origin; |
|
|
Eigen::Vector3d proj_origin; |
|
|
vec3d_conversion(desc.points[0], proj_origin); |
|
|
vec3d_conversion(desc.points[0], proj_origin); |
|
|
build_as_profile(std::move(desc), matrix_handle.col(0), matrix_handle.col(1), proj_origin, profile_aabb); |
|
|
build_as_profile(std::move(desc), matrix_handle.col(0), matrix_handle.col(1), matrix_handle.col(3), profile_aabb); |
|
|
const auto &profile_aabb_min = profile_aabb.min(); |
|
|
const auto &profile_aabb_min = profile_aabb.min(); |
|
|
const auto &profile_aabb_max = profile_aabb.max(); |
|
|
const auto &profile_aabb_max = profile_aabb.max(); |
|
|
aabb = { |
|
|
aabb = { |
|
@ -68,13 +68,13 @@ void polyline::build_as_axis(polyline_descriptor_t |
|
|
vec3d_conversion(std::move(desc.reference_normal), matrix_handle.col(2)); |
|
|
vec3d_conversion(std::move(desc.reference_normal), matrix_handle.col(2)); |
|
|
matrix_handle.col(2).normalize(); |
|
|
matrix_handle.col(2).normalize(); |
|
|
matrix_handle.col(1) = matrix_handle.col(2).cross(matrix_handle.col(0)); |
|
|
matrix_handle.col(1) = matrix_handle.col(2).cross(matrix_handle.col(0)); |
|
|
// vec3d_conversion(desc.points[0], matrix_handle.col(3));
|
|
|
vec3d_conversion(desc.points[0], matrix_handle.col(3)); |
|
|
vec3d_conversion(anchor_point, matrix_handle.col(3)); |
|
|
// vec3d_conversion(anchor_point, matrix_handle.col(3));
|
|
|
|
|
|
|
|
|
aabb_t<2> profile_aabb; |
|
|
aabb_t<2> profile_aabb; |
|
|
Eigen::Vector3d proj_origin; |
|
|
Eigen::Vector3d proj_origin; |
|
|
vec3d_conversion(desc.points[0], proj_origin); |
|
|
vec3d_conversion(desc.points[0], proj_origin); |
|
|
build_as_profile(std::move(desc), matrix_handle.col(0), matrix_handle.col(1), proj_origin, profile_aabb); |
|
|
build_as_profile(std::move(desc), matrix_handle.col(0), matrix_handle.col(1), matrix_handle.col(3), profile_aabb); |
|
|
const auto &profile_aabb_min = profile_aabb.min(); |
|
|
const auto &profile_aabb_min = profile_aabb.min(); |
|
|
const auto &profile_aabb_max = profile_aabb.max(); |
|
|
const auto &profile_aabb_max = profile_aabb.max(); |
|
|
aabb = { |
|
|
aabb = { |
|
@ -150,7 +150,7 @@ void polyline::build_as_profile(const polyline_descriptor_t &desc, |
|
|
const auto &theta = this->thetas[index]; |
|
|
const auto &theta = this->thetas[index]; |
|
|
|
|
|
|
|
|
const auto &a = this->vertices[start_index]; |
|
|
const auto &a = this->vertices[start_index]; |
|
|
const auto &b = this->vertices[start_index + 3]; |
|
|
const auto &b = this->vertices[start_index + 2]; |
|
|
auto &c = this->vertices[start_index + 1]; |
|
|
auto &c = this->vertices[start_index + 1]; |
|
|
const auto center_vec_ab = 0.5 * (a + b); |
|
|
const auto center_vec_ab = 0.5 * (a + b); |
|
|
Eigen::Matrix<double, 2, 1> temp = center_vec_ab; // for debug
|
|
|
Eigen::Matrix<double, 2, 1> temp = center_vec_ab; // for debug
|
|
@ -162,7 +162,7 @@ void polyline::build_as_profile(const polyline_descriptor_t &desc, |
|
|
// since all direction info is already included in the sign & magnitude of bulge
|
|
|
// since all direction info is already included in the sign & magnitude of bulge
|
|
|
const auto inv_tan_half_theta = (1 - bulge * bulge) / (2 * bulge); |
|
|
const auto inv_tan_half_theta = (1 - bulge * bulge) / (2 * bulge); |
|
|
const auto half_vec_ab = center_vec_ab - a; |
|
|
const auto half_vec_ab = center_vec_ab - a; |
|
|
c = center_vec_ab + inv_tan_half_theta * Eigen::Vector2d{-half_vec_ab[1], half_vec_ab[0]}; |
|
|
c = center_vec_ab + inv_tan_half_theta * Eigen::Vector2d{half_vec_ab[1], -half_vec_ab[0]}; |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
aabb.extend(c); |
|
|
aabb.extend(c); |
|
@ -231,7 +231,7 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, |
|
|
const auto &theta = this->thetas[index]; |
|
|
const auto &theta = this->thetas[index]; |
|
|
|
|
|
|
|
|
const auto &a = this->vertices[start_index]; |
|
|
const auto &a = this->vertices[start_index]; |
|
|
const auto &b = this->vertices[start_index + 3]; |
|
|
const auto &b = this->vertices[start_index + 2]; |
|
|
auto &c = this->vertices[start_index + 1]; |
|
|
auto &c = this->vertices[start_index + 1]; |
|
|
const auto center_vec_ab = 0.5 * (a + b); |
|
|
const auto center_vec_ab = 0.5 * (a + b); |
|
|
Eigen::Matrix<double, 2, 1> temp = center_vec_ab; // for debug
|
|
|
Eigen::Matrix<double, 2, 1> temp = center_vec_ab; // for debug
|
|
@ -243,7 +243,7 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, |
|
|
// since all direction info is already included in the sign & magnitude of bulge
|
|
|
// since all direction info is already included in the sign & magnitude of bulge
|
|
|
const auto inv_tan_half_theta = (1 - bulge * bulge) / (2 * bulge); |
|
|
const auto inv_tan_half_theta = (1 - bulge * bulge) / (2 * bulge); |
|
|
const auto half_vec_ab = center_vec_ab - a; |
|
|
const auto half_vec_ab = center_vec_ab - a; |
|
|
c = center_vec_ab + inv_tan_half_theta * Eigen::Vector2d{-half_vec_ab[1], half_vec_ab[0]}; |
|
|
c = center_vec_ab + inv_tan_half_theta * Eigen::Vector2d{half_vec_ab[1], -half_vec_ab[0]}; |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
aabb.extend(c); |
|
|
aabb.extend(c); |
|
@ -254,8 +254,15 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, |
|
|
{ |
|
|
{ |
|
|
assert(t >= 0 && t <= this->start_indices.size()); |
|
|
assert(t >= 0 && t <= this->start_indices.size()); |
|
|
|
|
|
|
|
|
double frac; |
|
|
double int_part; |
|
|
const auto n = static_cast<uint32_t>(std::modf(t, &frac)); |
|
|
double frac; |
|
|
|
|
|
if (t == static_cast<double>(this->start_indices.size()) && *vertices.begin() != *vertices.rbegin()) { |
|
|
|
|
|
frac = 1; |
|
|
|
|
|
int_part = t - 1; |
|
|
|
|
|
} else { |
|
|
|
|
|
frac = std::modf(t, &int_part); |
|
|
|
|
|
} |
|
|
|
|
|
const auto n = static_cast<uint32_t>(int_part); |
|
|
const auto iter = this->vertices.begin() + this->start_indices[n]; |
|
|
const auto iter = this->vertices.begin() + this->start_indices[n]; |
|
|
const auto &theta = this->thetas[n]; |
|
|
const auto &theta = this->thetas[n]; |
|
|
|
|
|
|
|
@ -268,7 +275,7 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, |
|
|
// so it's tangent can be simply calculated as M_R' * vec{CA}
|
|
|
// so it's tangent can be simply calculated as M_R' * vec{CA}
|
|
|
// and M_R' can be simply obtained by adding a PI/2 rotation to the rotation matrix
|
|
|
// and M_R' can be simply obtained by adding a PI/2 rotation to the rotation matrix
|
|
|
// CAUTION: we do not need normalize here anymore
|
|
|
// CAUTION: we do not need normalize here anymore
|
|
|
Eigen::Rotation2Dd rotation_matrix_deriv{t * this->thetas[n] + PI2}; |
|
|
Eigen::Rotation2Dd rotation_matrix_deriv{t * this->thetas[n] + PI2 * sign(theta)}; |
|
|
return rotation_matrix_deriv.toRotationMatrix() * -line_vec; |
|
|
return rotation_matrix_deriv.toRotationMatrix() * -line_vec; |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
@ -278,8 +285,15 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, |
|
|
{ |
|
|
{ |
|
|
assert(t >= 0 && t <= this->start_indices.size()); |
|
|
assert(t >= 0 && t <= this->start_indices.size()); |
|
|
|
|
|
|
|
|
double frac; |
|
|
double int_part; |
|
|
const auto n = static_cast<uint32_t>(std::modf(t, &frac)); |
|
|
double frac; |
|
|
|
|
|
if (t == static_cast<double>(this->start_indices.size()) && *vertices.begin() != *vertices.rbegin()) { |
|
|
|
|
|
frac = 1; |
|
|
|
|
|
int_part = t - 1; |
|
|
|
|
|
} else { |
|
|
|
|
|
frac = std::modf(t, &int_part); |
|
|
|
|
|
} |
|
|
|
|
|
const auto n = static_cast<uint32_t>(int_part); |
|
|
const auto iter = this->vertices.begin() + this->start_indices[n]; |
|
|
const auto iter = this->vertices.begin() + this->start_indices[n]; |
|
|
const auto &theta = this->thetas[n]; |
|
|
const auto &theta = this->thetas[n]; |
|
|
|
|
|
|
|
@ -291,8 +305,8 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, |
|
|
// as mentioned above, the matrix M_R'' can be obtained by adding a PI rotation to the rotation matrix
|
|
|
// as mentioned above, the matrix M_R'' can be obtained by adding a PI rotation to the rotation matrix
|
|
|
// caution: raw direction always points to the inside of the circle
|
|
|
// caution: raw direction always points to the inside of the circle
|
|
|
// but we want it to point the inside of the polyline, so we need to reverse the direction when theta < 0
|
|
|
// but we want it to point the inside of the polyline, so we need to reverse the direction when theta < 0
|
|
|
Eigen::Rotation2Dd rotation_matrix_deriv{t * this->thetas[n] + PI}; |
|
|
Eigen::Rotation2Dd rotation_matrix_deriv{t * this->thetas[n] + PI * sign(theta)}; |
|
|
return sign(theta) * rotation_matrix_deriv.toRotationMatrix() * -line_vec; |
|
|
return -sign(theta) * rotation_matrix_deriv.toRotationMatrix() * -line_vec; |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
@ -334,6 +348,8 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, |
|
|
const auto base_vec2 = Eigen::Vector2d{-base_vec1[1], base_vec1[0]}; |
|
|
const auto base_vec2 = Eigen::Vector2d{-base_vec1[1], base_vec1[0]}; |
|
|
const auto p_vec = p.topRows<2>() - *(iter + 1); |
|
|
const auto p_vec = p.topRows<2>() - *(iter + 1); |
|
|
|
|
|
|
|
|
|
|
|
const Eigen::Vector2d p_vec_tmp = p_vec; // for debugging
|
|
|
|
|
|
|
|
|
const auto local_x = base_vec1.dot(p_vec); |
|
|
const auto local_x = base_vec1.dot(p_vec); |
|
|
const auto local_y = base_vec2.dot(p_vec); |
|
|
const auto local_y = base_vec2.dot(p_vec); |
|
|
const auto phi = std::atan2(local_y, local_x); |
|
|
const auto phi = std::atan2(local_y, local_x); |
|
@ -403,12 +419,67 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, |
|
|
|
|
|
|
|
|
[[nodiscard]] bool polyline::pmc(const Eigen::Ref<const Eigen::Vector2d> &p, const line_closest_param_t &closest_param) const |
|
|
[[nodiscard]] bool polyline::pmc(const Eigen::Ref<const Eigen::Vector2d> &p, const line_closest_param_t &closest_param) const |
|
|
{ |
|
|
{ |
|
|
if (closest_param.is_peak_value) { |
|
|
// straight polyline pmc
|
|
|
return (p - closest_param.point.topRows<2>()).dot(this->calculate_normal(closest_param.t)) < 0; |
|
|
const auto ptInPolygon = [&](const Eigen::Ref<const Eigen::Vector2d> &p) { |
|
|
|
|
|
int intersectionCount = 0; |
|
|
|
|
|
// int onSegIdx = -1;
|
|
|
|
|
|
constexpr int numRays = 3; |
|
|
|
|
|
int majorityIn = 0; |
|
|
|
|
|
for (int rayIdx = 0; rayIdx < numRays; ++rayIdx) { |
|
|
|
|
|
double angle = (PI2 * rayIdx) / numRays; |
|
|
|
|
|
Eigen::Vector2d rayDir(cos(angle), sin(angle)); |
|
|
|
|
|
int crossings = 0; |
|
|
|
|
|
|
|
|
|
|
|
for (int i = 0; i < start_indices.size(); ++i) { |
|
|
|
|
|
const Eigen::Vector2d &a = vertices[start_indices[i]]; |
|
|
|
|
|
const Eigen::Vector2d &b = -EPSILON <= thetas[i] && thetas[i] <= EPSILON ? vertices[start_indices[i] + 1] |
|
|
|
|
|
: vertices[start_indices[i] + 2]; |
|
|
|
|
|
double dx1 = b[0] - a[0]; |
|
|
|
|
|
double dy1 = b[1] - a[1]; |
|
|
|
|
|
double dx2 = rayDir[0]; |
|
|
|
|
|
double dy2 = rayDir[1]; |
|
|
|
|
|
double determinant = dx1 * dy2 - dy1 * dx2; |
|
|
|
|
|
|
|
|
|
|
|
// if determinant is 0,line is parallel with the ray,no intersections
|
|
|
|
|
|
if (determinant == 0) continue; |
|
|
|
|
|
|
|
|
|
|
|
double t1 = ((p[0] - a[0]) * dy2 - (p[1] - a[1]) * dx2) / determinant; |
|
|
|
|
|
double t2 = ((p[0] - a[0]) * dy1 - (p[1] - a[1]) * dx1) / determinant; |
|
|
|
|
|
|
|
|
|
|
|
if (t1 >= 0 && t1 <= 1 && t2 >= 0) { crossings++; } |
|
|
|
|
|
} |
|
|
|
|
|
if (crossings % 2 != 0) { majorityIn++; } |
|
|
|
|
|
} |
|
|
|
|
|
return majorityIn > numRays / 2; |
|
|
|
|
|
}; |
|
|
|
|
|
|
|
|
|
|
|
bool inFan = false; |
|
|
|
|
|
int onLinesegButHasBugle = -1; |
|
|
|
|
|
for (int i = 0; i < start_indices.size(); ++i) { |
|
|
|
|
|
const Eigen::Vector2d &a = vertices[start_indices[i]]; |
|
|
|
|
|
const auto theta = thetas[i]; |
|
|
|
|
|
if (-EPSILON <= thetas[i] && thetas[i] <= EPSILON) continue; |
|
|
|
|
|
const Eigen::Vector2d &b = vertices[start_indices[i] + 2]; |
|
|
|
|
|
const auto &c = vertices[start_indices[i] + 1]; |
|
|
|
|
|
const auto ca = a - c; |
|
|
|
|
|
if ((p - c).norm() < ca.norm()) { |
|
|
|
|
|
const auto theta_2 = theta / 2; |
|
|
|
|
|
const auto cos_theta_2 = std::cos(theta_2), sin_theta_2 = std::sin(theta_2); |
|
|
|
|
|
const Eigen::Vector2d ab_arc_mid{a[0] + ca[0] * cos_theta_2 - ca[1] * sin_theta_2, |
|
|
|
|
|
a[1] + ca[0] * sin_theta_2 + ca[1] * cos_theta_2}; |
|
|
|
|
|
const auto in_circle_dir = Eigen::Vector2d{ab_arc_mid - c}.normalized(); |
|
|
|
|
|
if ((p - a).dot(in_circle_dir) > 0) { |
|
|
|
|
|
inFan = true; |
|
|
|
|
|
break; |
|
|
|
|
|
} |
|
|
|
|
|
} |
|
|
} |
|
|
} |
|
|
// the closest point is the vertex of the line
|
|
|
|
|
|
// todo: may not be robust for rare cases
|
|
|
// if (closest_param.is_peak_value) {
|
|
|
return (p - closest_param.point.topRows<2>()).dot(this->calculate_normal(closest_param.t)) < 0; |
|
|
// return (p - closest_param.point.topRows<2>()).dot(this->calculate_normal(closest_param.t)) > 0;
|
|
|
|
|
|
// }
|
|
|
|
|
|
auto isInPolygon = ptInPolygon(p); |
|
|
|
|
|
return !(ptInPolygon(p) ^ inFan); |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
[[nodiscard]] bool polyline::isEnd(const double t) const { return t >= thetas.size() - EPSILON || t < EPSILON; } |
|
|
[[nodiscard]] bool polyline::isEnd(const double t) const { return t >= thetas.size() - EPSILON || t < EPSILON; } |
|
|