ZCWang
/
ImplicitSurfaceNetwork
1
0
Fork 0
Code Issues 1 Pull Requests 1 Projects Releases Wiki Activity
Browse Source

fix bugs

gjj
Dtouch 2 months ago
parent
1bb69ee626
commit
b51c395dc6
6 changed files with 231 additions and 82 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 20
      application/main.cpp
  2. 2
      primitive_process/interface/internal_primitive_desc.hpp
  3. 54
      primitive_process/src/extrude_helixline.cpp
  4. 119
      primitive_process/src/extrude_polyline.cpp
  5. 7
      primitive_process/src/helixline.cpp
  6. 111
      primitive_process/src/polyline.cpp

20
application/main.cpp
View File

@ -27,8 +27,8 @@ void testExtrudeSDFXoY()
true true
}; };
auto axisPoints = std::array{ auto axisPoints = std::array{
raw_vector3d_t{0., 0., 0.}, raw_vector3d_t{1., 0., 0.},
raw_vector3d_t{0., 0., 2.} raw_vector3d_t{1., 0., 2.}
}; };
auto axisBulges = std::array{0.0}; auto axisBulges = std::array{0.0};
polyline_descriptor_t axis{ polyline_descriptor_t axis{
@ -46,7 +46,7 @@ void testExtrudeSDFXoY()
internal::extrude_polyline extrude_polyline{extrude, aabb}; internal::extrude_polyline extrude_polyline{extrude, aabb};
// Eigen::Vector3d p{1., 1., -0.1}; // Eigen::Vector3d p{1., 1., -0.1};
// Eigen::Vector3d p{0.3, 0.5, 1.9}; // Eigen::Vector3d p{0.3, 0.5, 1.9};
Eigen::Vector3d p{1., 1., 0.5}; Eigen::Vector3d p{.7, .8, 0.6};
double res = extrude_polyline.evaluate_sdf(p); double res = extrude_polyline.evaluate_sdf(p);
Eigen::Vector3d closest_point = extrude_polyline.evaluate_cpm(p); Eigen::Vector3d closest_point = extrude_polyline.evaluate_cpm(p);
printf("SDF at (%f, %f, %f) is %f, closest point: (%f, %f, %f)\n", printf("SDF at (%f, %f, %f) is %f, closest point: (%f, %f, %f)\n",
@ -65,20 +65,20 @@ void testExtrudeSDFZoX()
auto profilePoints = std::array{ auto profilePoints = std::array{
raw_vector3d_t{3., 1., 2.}, raw_vector3d_t{3., 1., 2.},
raw_vector3d_t{1., 1., 2.}, raw_vector3d_t{1., 1., 2.},
raw_vector3d_t{1., 1., 4.} // raw_vector3d_t{1., 1., 4.}
}; };
auto profileBulges = std::array{1.0, 1.0, 1.0}; auto profileBulges = std::array{1.0, 1.0};
polyline_descriptor_t profile{ polyline_descriptor_t profile{
3, 2,
profilePoints.data(), profilePoints.data(),
3, 2,
profileBulges.data(), profileBulges.data(),
raw_vector3d_t{0., 1., 0.}, raw_vector3d_t{0., 1., 0.},
true true
}; };
auto axisPoints = std::array{ auto axisPoints = std::array{
raw_vector3d_t{3., 1., 2}, raw_vector3d_t{3., 1., 1.5},
raw_vector3d_t{3., 3., 2} raw_vector3d_t{3., 3., 1.5}
}; };
auto axisBulges = std::array{0.0}; auto axisBulges = std::array{0.0};
polyline_descriptor_t axis{ polyline_descriptor_t axis{
@ -95,7 +95,7 @@ void testExtrudeSDFZoX()
aabb_t<> aabb{}; aabb_t<> aabb{};
internal::extrude_polyline extrude_polyline{extrude, aabb}; internal::extrude_polyline extrude_polyline{extrude, aabb};
// Eigen::Vector3d p{1., 1., -0.1}; // Eigen::Vector3d p{1., 1., -0.1};
Eigen::Vector3d p{2.3, 2.9, 1.5}; Eigen::Vector3d p{2.6, 2.9, 1.2};
double res = extrude_polyline.evaluate_sdf(p); double res = extrude_polyline.evaluate_sdf(p);
Eigen::Vector3d closest_point = extrude_polyline.evaluate_cpm(p); Eigen::Vector3d closest_point = extrude_polyline.evaluate_cpm(p);
printf("SDF at (%f, %f, %f) is %f, closest point: (%f, %f, %f)\n", printf("SDF at (%f, %f, %f) is %f, closest point: (%f, %f, %f)\n",

2
primitive_process/interface/internal_primitive_desc.hpp
View File

@ -175,6 +175,7 @@ struct PE_API polyline {
// construct local XY coordinate system // construct local XY coordinate system
std::vector<double> thetas{}; // for a straight line, this should be 0 std::vector<double> thetas{}; // for a straight line, this should be 0
std::vector<uint8_t> start_indices{}; // has the same size as thetas, indicating the starting index of each segment std::vector<uint8_t> start_indices{}; // has the same size as thetas, indicating the starting index of each segment
double bugle;
void build_as_axis(const polyline_descriptor_t&, void build_as_axis(const polyline_descriptor_t&,
const raw_vector3d_t&, const raw_vector3d_t&,
@ -220,6 +221,7 @@ struct helixline {
[[nodiscard]] Eigen::Vector3d calculate_tangent(double) const; [[nodiscard]] Eigen::Vector3d calculate_tangent(double) const;
[[nodiscard]] Eigen::Vector3d calculate_normal(double) const; [[nodiscard]] Eigen::Vector3d calculate_normal(double) const;
[[nodiscard]] Eigen::Vector3d evaluate(double) const;
// CAUTION: make sure the input point is in the local space of the helixline // CAUTION: make sure the input point is in the local space of the helixline
[[nodiscard]] line_closest_param_t calculate_closest_param(const Eigen::Ref<const Eigen::Vector3d>&) const; [[nodiscard]] line_closest_param_t calculate_closest_param(const Eigen::Ref<const Eigen::Vector3d>&) const;
[[nodiscard]] bool isEnd(const double t) const; [[nodiscard]] bool isEnd(const double t) const;

54
primitive_process/src/extrude_helixline.cpp
View File

@ -45,8 +45,8 @@ static inline auto get_local_TBN(const helixline &line, const Eigen::Ref<const E
const auto T = line.calculate_tangent(t); const auto T = line.calculate_tangent(t);
const auto N = line.calculate_normal(t); const auto N = line.calculate_normal(t);
const auto B = N.cross(T); const auto B = N.cross(T);
handle.col(0).topRows<3>() = std::move(B); handle.col(0).topRows<3>() = std::move(N);
handle.col(1).topRows<3>() = std::move(N); handle.col(1).topRows<3>() = std::move(B);
handle.col(2).topRows<3>() = std::move(T); handle.col(2).topRows<3>() = std::move(T);
handle.col(3).topRows<3>() = std::move(q); handle.col(3).topRows<3>() = std::move(q);
@ -66,11 +66,55 @@ static inline process_return_type_t<Routine> evaluate(const extrude_helixline
// TODO: add support for non-parallel (ray vs. profile plane) case // TODO: add support for non-parallel (ray vs. profile plane) case
// for now just assume that profile plane is parallel to axis // for now just assume that profile plane is parallel to axis
auto [profile_closest_param, dist] = desc.profile.calculate_closest_param((inv_TBN * local_p).topRows<3>()); auto [profile_closest_param, dist] = desc.profile.calculate_closest_param((inv_TBN * local_p).topRows<3>());
if (!desc.axis.isEnd(axis_closest_param.t) || axis_closest_param.is_peak_value) {
// if !is_peak_value, the point is two sides away, and pmc is always true bool isOut = desc.profile.pmc((inv_TBN * local_p).topRows<2>(), profile_closest_param);
dist *= desc.profile.pmc((inv_TBN * local_p).topRows<2>(), profile_closest_param) * 2 - 1; profile_closest_param.point = TBN * profile_closest_param.point;
if (!isOut) {
// inside or projected point inside
const auto local_p_3d = local_p.topRows<3>() / local_p[3];
if (!desc.axis.isEnd(axis_closest_param.t) || axis_closest_param.is_peak_value) {
// inside
// max() with two planes
dist = -dist;
Eigen::Vector3d start_tangent = desc.axis.calculate_tangent(0);
auto sdf_start_plane = -start_tangent.dot(local_p_3d);
if (sdf_start_plane > dist) {
dist = sdf_start_plane;
profile_closest_param.point = local_p_3d + start_tangent * dist;
} else {
Eigen::Vector3d final_v = desc.axis.evaluate(1);
Eigen::Vector3d final_tangent = desc.axis.calculate_tangent(1);
auto sdf_end_plane = final_tangent.dot(local_p_3d - final_v);
if (sdf_end_plane > dist) {
dist = sdf_end_plane;
profile_closest_param.point = local_p_3d - final_tangent * dist;
}
}
} else {
// outside, must closer to the plane
if (axis_closest_param.t < EPSILON) {
Eigen::Vector3d start_tangent = desc.axis.calculate_tangent(0);
auto sdf_start_plane = -start_tangent.dot(local_p_3d);
dist = sdf_start_plane;
profile_closest_param.point = local_p_3d + start_tangent * dist;
} else {
Eigen::Vector3d final_v = desc.axis.evaluate(1);
Eigen::Vector3d final_tangent = desc.axis.calculate_tangent(1);
auto sdf_end_plane = final_tangent.dot(local_p_3d - final_v);
dist = sdf_end_plane;
profile_closest_param.point = local_p_3d - final_tangent * dist;
}
}
} else {
// outside, nothing to do
} }
// if (!desc.axis.isEnd(axis_closest_param.t) || axis_closest_param.is_peak_value) {
// // if !is_peak_value, the point is two sides away, and pmc is always true
// dist *= desc.profile.pmc((inv_TBN * local_p).topRows<2>(), profile_closest_param) * 2 - 1;
// }
// transform closest point to world space // transform closest point to world space
// the distance should be unchanged during transformation, since no scaling is involved // the distance should be unchanged during transformation, since no scaling is involved
if constexpr (std::is_same_v<Routine, evaluation_routine_tag>) if constexpr (std::is_same_v<Routine, evaluation_routine_tag>)

119
primitive_process/src/extrude_polyline.cpp
View File

@ -79,63 +79,88 @@ static inline process_return_type_t<Routine> evaluate(const extrude_polyline
const auto world_to_axis = inversed_affine_transform(desc.axis_to_world); const auto world_to_axis = inversed_affine_transform(desc.axis_to_world);
const auto local_p = world_to_axis * Eigen::Vector4d{point[0], point[1], point[2], 1}; const auto local_p = world_to_axis * Eigen::Vector4d{point[0], point[1], point[2], 1};
const auto [axis_closest_param, _] = desc.axis.calculate_closest_param(local_p.topRows<3>()); const auto [axis_closest_param, _] = desc.axis.calculate_closest_param(local_p.topRows<3>());
const auto TBN = get_local_TBN(desc.axis, local_p, axis_closest_param.point, axis_closest_param.t); Eigen::Vector3d axisStart;
const auto inv_TBN = inversed_affine_transform(TBN); axisStart << desc.axis.vertices[0][0], desc.axis.vertices[0][1], 0;
const auto TBN = get_local_TBN(desc.axis, local_p, axis_closest_param.point - axisStart, axis_closest_param.t);
const auto inv_TBN = inversed_affine_transform(TBN);
std::cout << "world_to_axis: " << std::endl << world_to_axis.matrix() << std::endl; // std::cout << "world_to_axis: " << std::endl << world_to_axis.matrix() << std::endl;
std::cout << "axis_to_world: " << std::endl << desc.axis_to_world.matrix() << std::endl; // std::cout << "axis_to_world: " << std::endl << desc.axis_to_world.matrix() << std::endl;
std::cout << "TBN" << std::endl << TBN.matrix() << std::endl; // std::cout << "TBN" << std::endl << TBN.matrix() << std::endl;
std::cout << "inv_TBN" << std::endl << inv_TBN.matrix() << std::endl; // std::cout << "inv_TBN" << std::endl << inv_TBN.matrix() << std::endl;
// TODO: add support for non-parallel (ray vs. profile plane) case // TODO: add support for non-parallel (ray vs. profile plane) case
// for now just assume that profile plane is parallel to axis // for now just assume that profile plane is parallel to axis
auto [profile_closest_param, dist] = desc.profile.calculate_closest_param((inv_TBN * local_p).topRows<3>()); auto [profile_closest_param, dist] = desc.profile.calculate_closest_param((inv_TBN * local_p).topRows<3>());
// bool isOut = desc.profile.pmc((inv_TBN * local_p).topRows<2>(), profile_closest_param);
// if (!isOut) {
// const auto TBN_handle = TBN.matrix();
// // inside or projected point inside
// if (!desc.axis.isEnd(axis_closest_param.t) || axis_closest_param.is_peak_value) {
// // inside
// auto dist_start_plane =
// fabs(TBN_handle.col(2).topRows<3>().dot(TBN_handle.col(3).topRows<3>())); if
// (dist_start_plane < dist) { dist = dist_start_plane;
// profile_closest_param.point = } else {
// } bool isOut = desc.profile.pmc((inv_TBN * local_p).topRows<2>(), profile_closest_param);
// dist = -fmin(dist, ); const auto tmp = inv_TBN * local_p;
// } else { profile_closest_param.point = TBN * profile_closest_param.point;
// // outside if (!isOut) {
// dist = TBN_handle.col(2).topRows<3>().dot(TBN_handle.col(3).topRows<3>()); // inside or projected point inside
// } const auto local_p_3d = local_p.topRows<3>() / local_p[3];
// } else { if (!desc.axis.isEnd(axis_closest_param.t) || axis_closest_param.is_peak_value) {
// // outside, nothing to do // inside
// } // max() with two planes
dist = -dist;
const auto local_p_3d = local_p.topRows<3>() / local_p[3]; Eigen::Vector2d start_tangent = desc.axis.calculate_tangent(0);
auto profile_p = inv_TBN * local_p_3d; auto sdf_start_plane = -start_tangent.dot(local_p_3d.topRows<2>());
if (!desc.axis.isEnd(axis_closest_param.t) || axis_closest_param.is_peak_value) { if (sdf_start_plane > dist) {
// when the point is two sides away, and pmc is always true dist = sdf_start_plane;
bool pmc = desc.profile.pmc((inv_TBN * local_p_3d).topRows<2>(), profile_closest_param); profile_closest_param.point = local_p_3d + Eigen::Vector3d(start_tangent[0], start_tangent[1], 0) * dist;
dist *= pmc * 2 - 1; } else {
} Eigen::Vector2d final_v = desc.axis.vertices.back();
// max() with two planes Eigen::Vector2d final_tangent = desc.axis.calculate_tangent(desc.axis.thetas.size());
profile_closest_param.point = TBN * profile_closest_param.point; auto sdf_end_plane = final_tangent.dot(local_p_3d.topRows<2>() - final_v);
Eigen::Vector2d start_tangent = desc.axis.calculate_tangent(0); if (sdf_end_plane > dist) {
auto sdf_start_plane = -start_tangent.dot(local_p_3d.topRows<2>()); dist = sdf_end_plane;
if (sdf_start_plane > dist) { profile_closest_param.point = local_p_3d - Eigen::Vector3d(final_tangent[0], final_tangent[1], 0) * dist;
dist = sdf_start_plane; }
profile_closest_param.point = local_p_3d + Eigen::Vector3d(start_tangent[0], start_tangent[1], 0) * dist; }
} else { } else {
Eigen::Vector2d final_v = desc.axis.vertices.back(); // outside, must closer to the plane
Eigen::Vector2d final_tangent = desc.axis.calculate_tangent(desc.axis.thetas.size()); if (axis_closest_param.t < EPSILON) {
auto sdf_end_plane = final_tangent.dot(local_p_3d.topRows<2>() - final_v); Eigen::Vector2d start_tangent = desc.axis.calculate_tangent(0);
if (sdf_end_plane > dist) { auto sdf_start_plane = -start_tangent.dot(local_p_3d.topRows<2>());
dist = sdf_end_plane; dist = sdf_start_plane;
profile_closest_param.point = local_p_3d - Eigen::Vector3d(final_tangent[0], final_tangent[1], 0) * dist; profile_closest_param.point = local_p_3d + Eigen::Vector3d(start_tangent[0], start_tangent[1], 0) * dist;
} else {
Eigen::Vector2d final_v = desc.axis.vertices.back();
Eigen::Vector2d final_tangent = desc.axis.calculate_tangent(desc.axis.thetas.size());
auto sdf_end_plane = final_tangent.dot(local_p_3d.topRows<2>() - final_v);
dist = sdf_end_plane;
profile_closest_param.point = local_p_3d - Eigen::Vector3d(final_tangent[0], final_tangent[1], 0) * dist;
}
} }
} else {
// outside, nothing to do
} }
// const auto profile_p = inv_TBN * local_p_3d;
// if (!desc.axis.isEnd(axis_closest_param.t) || axis_closest_param.is_peak_value) {
// // when the point is two sides away, and pmc is always true
// bool pmc = desc.profile.pmc((inv_TBN * local_p_3d).topRows<2>(), profile_closest_param);
// dist *= pmc * 2 - 1;
// }
// // max() with two planes
// profile_closest_param.point = TBN * profile_closest_param.point;
// Eigen::Vector2d start_tangent = desc.axis.calculate_tangent(0);
// auto sdf_start_plane = -start_tangent.dot(local_p_3d.topRows<2>());
// if (sdf_start_plane > dist) {
// dist = sdf_start_plane;
// profile_closest_param.point = local_p_3d + Eigen::Vector3d(start_tangent[0], start_tangent[1], 0) * dist;
// } else {
// Eigen::Vector2d final_v = desc.axis.vertices.back();
// Eigen::Vector2d final_tangent = desc.axis.calculate_tangent(desc.axis.thetas.size());
// auto sdf_end_plane = final_tangent.dot(local_p_3d.topRows<2>() - final_v);
// if (sdf_end_plane > dist) {
// dist = sdf_end_plane;
// profile_closest_param.point = local_p_3d - Eigen::Vector3d(final_tangent[0], final_tangent[1], 0) * dist;
// }
// }
// transform closest point to world space // transform closest point to world space
// the distance should be unchanged during transformation, since no scaling is involved // the distance should be unchanged during transformation, since no scaling is involved
if constexpr (std::is_same_v<Routine, evaluation_routine_tag>) if constexpr (std::is_same_v<Routine, evaluation_routine_tag>)

7
primitive_process/src/helixline.cpp
View File

@ -75,6 +75,13 @@ helixline::helixline(helixline_descriptor_t
return {-std::cos(t * this->total_theta), -std::sin(t * this->total_theta), 0.}; return {-std::cos(t * this->total_theta), -std::sin(t * this->total_theta), 0.};
} }
[[nodiscard]] Eigen::Vector3d helixline::evaluate(double t) const
{
assert(t >= 0 && t <= 1);
double theta = t * total_theta;
return {radius * std::cos(theta), radius * std::sin(theta), t * height};
}
[[nodiscard]] line_closest_param_t helixline::calculate_closest_param(const Eigen::Ref<const Eigen::Vector3d> &p) const [[nodiscard]] line_closest_param_t helixline::calculate_closest_param(const Eigen::Ref<const Eigen::Vector3d> &p) const
{ {
const auto h_p = p.z(); const auto h_p = p.z();

111
primitive_process/src/polyline.cpp
View File

@ -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; }

Write Preview
Loading…
Cancel
Save