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

extrusion sdf debug

gjj
Dtouch 2 months ago
parent
cbcf2e8319
commit
4e9a701346
6 changed files with 292 additions and 85 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. 188
      application/main.cpp
  2. 4
      application/xmake.lua
  3. 6
      primitive_process/interface/internal_primitive_desc.hpp
  4. 8
      primitive_process/src/extrude_helixline.cpp
  5. 81
      primitive_process/src/extrude_polyline.cpp
  6. 90
      primitive_process/src/polyline.cpp

188
application/main.cpp
View File

@ -6,48 +6,170 @@
#include <io.h> #include <io.h>
#include <construct_helper.hpp> #include <construct_helper.hpp>
#include "Eigen/Core"
#include "primitive_descriptor.h" #include "primitive_descriptor.h"
#include "internal_primitive_desc.hpp" #include "internal_primitive_desc.hpp"
void testExtrudeSDF() { void testExtrudeSDFXoY()
printf("Testing extrude polyline SDF\n"); {
auto profilePoints = std::array{raw_vector3d_t{-1., 0., 0.}, raw_vector3d_t{1., 0., 0.}}; printf("Testing extrude polyline SDF XoY\n");
auto profileBulges = std::array{1.0, 1.0}; auto profilePoints = std::array{
polyline_descriptor_t profile{ raw_vector3d_t{-1., 0., 0.},
2, raw_vector3d_t{1., 0., 0.}
profilePoints.data(), };
2, auto profileBulges = std::array{1.0, 1.0};
profileBulges.data(), polyline_descriptor_t profile{
raw_vector3d_t{0., 0., 1.}, 2,
true profilePoints.data(),
}; 2,
auto axisPoints = std::array{raw_vector3d_t{0., 0., 0.}, raw_vector3d_t{0., 0., 2.}}; profileBulges.data(),
auto axisBulges = std::array{0.0}; raw_vector3d_t{0., 0., 1.},
polyline_descriptor_t axis{ true
2, };
axisPoints.data(), auto axisPoints = std::array{
1, raw_vector3d_t{0., 0., 0.},
axisBulges.data(), raw_vector3d_t{0., 0., 2.}
raw_vector3d_t{0., 1., 0.}, };
false auto axisBulges = std::array{0.0};
}; polyline_descriptor_t axis{
2,
extrude_polyline_descriptor_t extrude{ axisPoints.data(),
1, 1,
&profile, axisBulges.data(),
axis raw_vector3d_t{0., 1., 0.},
false
};
extrude_polyline_descriptor_t extrude{1, &profile, axis};
aabb_t<> aabb{};
internal::extrude_polyline extrude_polyline{extrude, aabb};
// Eigen::Vector3d p{1., 1., -0.1};
// Eigen::Vector3d p{0.3, 0.5, 1.9};
Eigen::Vector3d p{1., 1., 0.5};
double res = extrude_polyline.evaluate_sdf(p);
Eigen::Vector3d closest_point = extrude_polyline.evaluate_cpm(p);
printf("SDF at (%f, %f, %f) is %f, closest point: (%f, %f, %f)\n",
p.x(),
p.y(),
p.z(),
res,
closest_point.x(),
closest_point.y(),
closest_point.z());
}
void testExtrudeSDFZoX()
{
printf("Testing extrude polyline SDF ZoX\n");
auto profilePoints = std::array{
raw_vector3d_t{3., 1., 2.},
raw_vector3d_t{1., 1., 2.},
raw_vector3d_t{1., 1., 4.}
};
auto profileBulges = std::array{1.0, 1.0, 1.0};
polyline_descriptor_t profile{
3,
profilePoints.data(),
3,
profileBulges.data(),
raw_vector3d_t{0., 1., 0.},
true
}; };
auto axisPoints = std::array{
raw_vector3d_t{3., 1., 2},
raw_vector3d_t{3., 3., 2}
};
auto axisBulges = std::array{0.0};
polyline_descriptor_t axis{
2,
axisPoints.data(),
1,
axisBulges.data(),
raw_vector3d_t{0., 0., 1.},
false
};
extrude_polyline_descriptor_t extrude{1, &profile, axis};
aabb_t<> aabb{}; aabb_t<> aabb{};
internal::extrude_polyline extrude_polyline{extrude, aabb}; internal::extrude_polyline extrude_polyline{extrude, aabb};
Eigen::Vector3d p{1., 2., 1.}; // Eigen::Vector3d p{1., 1., -0.1};
double res = extrude_polyline.evaluate_sdf(p); Eigen::Vector3d p{2.3, 2.9, 1.5};
printf("SDF at (%f, %f, %f) is %f\n", p.x(), p.y(), p.z(), res); double res = extrude_polyline.evaluate_sdf(p);
Eigen::Vector3d closest_point = extrude_polyline.evaluate_cpm(p);
printf("SDF at (%f, %f, %f) is %f, closest point: (%f, %f, %f)\n",
p.x(),
p.y(),
p.z(),
res,
closest_point.x(),
closest_point.y(),
closest_point.z());
}
void testPolylineSDF()
{
printf("Testing polyline SDF\n");
auto points = std::array{
raw_vector3d_t{-1., 1., 0.},
raw_vector3d_t{1., 1., 0.}
};
// auto bulges = std::array{0.41421356237309503, 1.0}; // 0.41421356237309503 is tan(PI/8)
auto bulges = std::array{-1., -1.0}; // 0.41421356237309503 is tan(PI/8)
polyline_descriptor_t polyline{
2,
points.data(),
2,
bulges.data(),
raw_vector3d_t{0., 0., 1.},
true
};
aabb_t<2> aabb{};
internal::polyline internal_polyline;
// Eigen::Transform<double, 3, Eigen::AffineCompact> axis_to_world{};
// const auto& matrix_handle = axis_to_world.matrix();
// const Eigen::Matrix4d matrix_handle = Eigen::Matrix4d::Identity();
internal_polyline.build_as_profile(polyline,
Eigen::Vector3d{1., 0., 0.},
Eigen::Vector3d{0., 1., 0.},
Eigen::Vector3d{0., 0., 0.},
aabb);
std::array testPts = {
// Eigen::Vector3d{-1., 0., 0.},
// Eigen::Vector3d{0., 0., 0.},
// Eigen::Vector3d{1., 0., 0.},
// Eigen::Vector3d{1., 1., 0.},
// Eigen::Vector3d{1., 2., 0.},
Eigen::Vector3d{0., 1., 0.},
// Eigen::Vector3d{-1., 0., 1.},
// Eigen::Vector3d{0., 0., 1.},
// Eigen::Vector3d{1., 0., 1.},
// Eigen::Vector3d{1., 1., 1.},
// Eigen::Vector3d{1., 2., 1.},
// Eigen::Vector3d{0., 1., 1.},
};
for (const auto &pt : testPts) {
auto [axis_closest_param, distance] = internal_polyline.calculate_closest_param(pt);
printf("For query point (%f, %f, %f), closest param: %f closest point (%f, %f, %f), distance: %f\n",
pt.x(),
pt.y(),
pt.z(),
axis_closest_param.t,
axis_closest_param.point.x(),
axis_closest_param.point.y(),
axis_closest_param.point.z(),
distance);
}
} }
int main() int main()
{ {
testExtrudeSDF(); // testExtrudeSDFZoX();
testExtrudeSDFXoY();
// testPolylineSDF();
// std::cout << "Setting scene..." << std::endl; // std::cout << "Setting scene..." << std::endl;
// sphere_descriptor_t sphere1{ // sphere_descriptor_t sphere1{

4
application/xmake.lua
View File

@ -10,4 +10,8 @@ target("test_frontend")
set_kind("binary") set_kind("binary")
add_deps("frontend") add_deps("frontend")
add_files("main.cpp") add_files("main.cpp")
add_linkdirs("E:\\CProj\\ImplicitSurfaceNetwork\\build\\windows\\x64\\debug")
after_build(function(target)
print("Hello, xmake!")
end)
target_end() target_end()

6
primitive_process/interface/internal_primitive_desc.hpp
View File

@ -179,11 +179,13 @@ struct PE_API polyline {
void build_as_axis(const polyline_descriptor_t&, void build_as_axis(const polyline_descriptor_t&,
const raw_vector3d_t&, const raw_vector3d_t&,
Eigen::Transform<double, 3, Eigen::AffineCompact, Eigen::DontAlign>&, Eigen::Transform<double, 3, Eigen::AffineCompact, Eigen::DontAlign>&,
aabb_t<>&); aabb_t<>&,
const raw_vector3d_t&);
void build_as_axis(polyline_descriptor_t&&, void build_as_axis(polyline_descriptor_t&&,
const raw_vector3d_t&, const raw_vector3d_t&,
Eigen::Transform<double, 3, Eigen::AffineCompact, Eigen::DontAlign>&, Eigen::Transform<double, 3, Eigen::AffineCompact, Eigen::DontAlign>&,
aabb_t<>&); aabb_t<>&,
const raw_vector3d_t&);
void build_as_profile(const polyline_descriptor_t&, void build_as_profile(const polyline_descriptor_t&,
const Eigen::Ref<const Eigen::Vector3d>&, const Eigen::Ref<const Eigen::Vector3d>&,
const Eigen::Ref<const Eigen::Vector3d>&, const Eigen::Ref<const Eigen::Vector3d>&,

8
primitive_process/src/extrude_helixline.cpp
View File

@ -66,10 +66,10 @@ 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 (axis_closest_param.is_peak_value) { 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 // 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); 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

81
primitive_process/src/extrude_polyline.cpp
View File

@ -1,15 +1,22 @@
#include "Eigen/Core"
#include "internal_primitive_desc.hpp" #include "internal_primitive_desc.hpp"
#include <cmath>
#include <iostream>
namespace internal namespace internal
{ {
extrude_polyline::extrude_polyline(const extrude_polyline_descriptor_t &desc, aabb_t<> &aabb) extrude_polyline::extrude_polyline(const extrude_polyline_descriptor_t &desc, aabb_t<> &aabb)
{ {
this->axis.build_as_axis(desc.axis, desc.profiles[0].reference_normal, this->axis_to_world, aabb); this->axis.build_as_axis(desc.axis,
desc.profiles[0].reference_normal,
this->axis_to_world,
aabb,
desc.profiles[0].points[0]);
const auto &matrix_handle = this->axis_to_world.matrix(); const auto &matrix_handle = this->axis_to_world.matrix();
aabb_t<2> profile_aabb{}; aabb_t<2> profile_aabb{};
this->profile.build_as_profile(desc.profiles[0], this->profile.build_as_profile(desc.profiles[0],
matrix_handle.col(0),
matrix_handle.col(1), matrix_handle.col(1),
matrix_handle.col(2),
matrix_handle.col(3), matrix_handle.col(3),
profile_aabb); profile_aabb);
const auto profile_max_extent = profile_aabb.max().cwiseMax(profile_aabb.min().cwiseAbs()).maxCoeff(); const auto profile_max_extent = profile_aabb.max().cwiseMax(profile_aabb.min().cwiseAbs()).maxCoeff();
@ -19,12 +26,16 @@ extrude_polyline::extrude_polyline(const extrude_polyline_descriptor_t &desc, aa
extrude_polyline::extrude_polyline(extrude_polyline_descriptor_t &&desc, aabb_t<> &aabb) extrude_polyline::extrude_polyline(extrude_polyline_descriptor_t &&desc, aabb_t<> &aabb)
{ {
this->axis.build_as_axis(std::move(desc.axis), desc.profiles[0].reference_normal, this->axis_to_world, aabb); this->axis.build_as_axis(std::move(desc.axis),
desc.profiles[0].reference_normal,
this->axis_to_world,
aabb,
desc.profiles[0].points[0]);
const auto &matrix_handle = this->axis_to_world.matrix(); const auto &matrix_handle = this->axis_to_world.matrix();
aabb_t<2> profile_aabb{}; aabb_t<2> profile_aabb{};
this->profile.build_as_profile(std::move(desc.profiles[0]), this->profile.build_as_profile(std::move(desc.profiles[0]),
matrix_handle.col(0),
matrix_handle.col(1), matrix_handle.col(1),
matrix_handle.col(2),
matrix_handle.col(3), matrix_handle.col(3),
profile_aabb); profile_aabb);
const auto profile_max_extent = profile_aabb.max().cwiseMax(profile_aabb.min().cwiseAbs()).maxCoeff(); const auto profile_max_extent = profile_aabb.max().cwiseMax(profile_aabb.min().cwiseAbs()).maxCoeff();
@ -40,11 +51,12 @@ static inline auto get_local_TBN(const polyline &line,
double t) double t)
{ {
Eigen::Vector2d local_tangent, local_normal; Eigen::Vector2d local_tangent, local_normal;
if (t >= 0 && t <= line.start_indices.size() && std::abs(t - std::round(t)) <= EPSILON) { if (t > EPSILON && t < line.start_indices.size() - EPSILON && std::abs(t - std::round(t)) <= EPSILON
&& (q.topRows<2>() - p.topRows<2>()).norm() > EPSILON) {
// if between two segments, use local vec{pq} on the plane as normal // if between two segments, use local vec{pq} on the plane as normal
local_normal = (q.topRows<2>() - p.topRows<2>()).normalized(); local_normal = (q.topRows<2>() - p.topRows<2>()).normalized();
if (local_normal.dot(line.calculate_normal(t)) < 0) local_normal *= -1; if (local_normal.dot(line.calculate_normal(t)) < 0) local_normal *= -1;
local_tangent = {local_normal[1], -local_normal[0]}; local_tangent = {-local_normal[1], local_normal[0]};
} else { } else {
local_tangent = line.calculate_tangent(t); local_tangent = line.calculate_tangent(t);
local_normal = line.calculate_normal(t); local_normal = line.calculate_normal(t);
@ -66,25 +78,70 @@ 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, axis_closest_t] = 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); const auto TBN = get_local_TBN(desc.axis, local_p, axis_closest_param.point, axis_closest_param.t);
const auto inv_TBN = inversed_affine_transform(TBN); const auto inv_TBN = inversed_affine_transform(TBN);
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 << "TBN" << std::endl << 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>());
if (!desc.axis.isEnd(axis_closest_t) || !axis_closest_param.is_peak_value) {
// when the point is two sides away, and pmc is always true
dist *= desc.profile.pmc((inv_TBN * local_p).topRows<2>(), profile_closest_param);
}
// 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 {
// }
// dist = -fmin(dist, );
// } else {
// // outside
// dist = TBN_handle.col(2).topRows<3>().dot(TBN_handle.col(3).topRows<3>());
// }
// } else {
// // outside, nothing to do
// }
const auto local_p_3d = local_p.topRows<3>() / local_p[3];
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>)
return dist; return dist;
else else
return desc.axis_to_world * TBN * profile_closest_param.point; return desc.axis_to_world * profile_closest_param.point;
} }
double extrude_polyline::evaluate_sdf([[maybe_unused]] const Eigen::Ref<const Eigen::Vector3d> &point) const double extrude_polyline::evaluate_sdf([[maybe_unused]] const Eigen::Ref<const Eigen::Vector3d> &point) const

90
primitive_process/src/polyline.cpp
View File

@ -2,6 +2,7 @@
#include <algorithm/glue_algorithm.hpp> #include <algorithm/glue_algorithm.hpp>
#include "Eigen/Core"
#include "internal_primitive_desc.hpp" #include "internal_primitive_desc.hpp"
static inline auto manually_projection(const raw_vector3d_t &point, static inline auto manually_projection(const raw_vector3d_t &point,
@ -30,18 +31,20 @@ namespace internal
void polyline::build_as_axis(const polyline_descriptor_t &desc, void polyline::build_as_axis(const polyline_descriptor_t &desc,
const raw_vector3d_t &profile_ref_normal, const raw_vector3d_t &profile_ref_normal,
Eigen::Transform<double, 3, Eigen::AffineCompact, Eigen::DontAlign> &axis_to_world, Eigen::Transform<double, 3, Eigen::AffineCompact, Eigen::DontAlign> &axis_to_world,
aabb_t<> &aabb) aabb_t<> &aabb,
const raw_vector3d_t &anchor_point)
{ {
auto &matrix_handle = axis_to_world.matrix(); auto &matrix_handle = axis_to_world.matrix();
vec3d_conversion(desc.reference_normal, matrix_handle.col(1)); vec3d_conversion(profile_ref_normal, matrix_handle.col(0));
matrix_handle.col(1).normalize(); matrix_handle.col(0).normalize();
vec3d_conversion(profile_ref_normal, matrix_handle.col(2)); vec3d_conversion(desc.reference_normal, matrix_handle.col(2));
matrix_handle.col(2).normalize(); matrix_handle.col(2).normalize();
matrix_handle.col(0) = matrix_handle.col(1).cross(matrix_handle.col(2)); 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));
aabb_t<2> profile_aabb; aabb_t<2> profile_aabb;
build_as_profile(desc, matrix_handle.col(2), matrix_handle.col(0), matrix_handle.col(3), profile_aabb); build_as_profile(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 = {
@ -54,18 +57,20 @@ void polyline::build_as_axis(const polyline_descriptor_t
void polyline::build_as_axis(polyline_descriptor_t &&desc, void polyline::build_as_axis(polyline_descriptor_t &&desc,
const raw_vector3d_t &profile_ref_normal, const raw_vector3d_t &profile_ref_normal,
Eigen::Transform<double, 3, Eigen::AffineCompact, Eigen::DontAlign> &axis_to_world, Eigen::Transform<double, 3, Eigen::AffineCompact, Eigen::DontAlign> &axis_to_world,
aabb_t<> &aabb) aabb_t<> &aabb,
const raw_vector3d_t &anchor_point)
{ {
auto &matrix_handle = axis_to_world.matrix(); auto &matrix_handle = axis_to_world.matrix();
vec3d_conversion(std::move(desc.reference_normal), matrix_handle.col(1)); vec3d_conversion(profile_ref_normal, matrix_handle.col(0));
matrix_handle.col(1).normalize(); matrix_handle.col(0).normalize();
vec3d_conversion(profile_ref_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(0) = matrix_handle.col(1).cross(matrix_handle.col(2)); 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));
aabb_t<2> profile_aabb; aabb_t<2> profile_aabb;
build_as_profile(std::move(desc), matrix_handle.col(2), matrix_handle.col(0), matrix_handle.col(3), 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 = {
@ -105,6 +110,10 @@ void polyline::build_as_profile(const polyline_descriptor_t &desc,
} }
}); });
// i.e. iff close curve, deduce loop to simple segements by copying the first vertex to the end // i.e. iff close curve, deduce loop to simple segements by copying the first vertex to the end
Eigen::Vector3d proj_x_dir_tmp = proj_x_dir;
Eigen::Vector3d proj_y_dir_tmp = proj_y_dir;
Eigen::Vector3d proj_origin_tmp = proj_origin; // for debug
this->vertices.resize(this->start_indices.back() + 1); this->vertices.resize(this->start_indices.back() + 1);
this->start_indices.pop_back(); this->start_indices.pop_back();
circle_start_indices.shrink_to_fit(); circle_start_indices.shrink_to_fit();
@ -136,26 +145,27 @@ void polyline::build_as_profile(const polyline_descriptor_t &desc,
const auto &bulge = desc.bulges[index]; const auto &bulge = desc.bulges[index];
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 + 3];
auto &c = this->vertices[start_index + 1]; auto &c = this->vertices[start_index + 1];
auto &d = this->vertices[start_index + 2]; auto &d = this->vertices[start_index + 2];
const auto half_vec_ab = 0.5 * (a + b); const auto center_vec_ab = 0.5 * (a + b);
const auto half_vec_ab = b - center_vec_ab;
Eigen::Matrix<double, 2, 1> temp = center_vec_ab; // for debug
if (theta == PI) { if (theta == PI) {
c = std::move(half_vec_ab); c = std::move(center_vec_ab);
} else { } else {
// NOTE: bulge = tan(theta/4), so we can directly calculate tan(theta/2) // NOTE: bulge = tan(theta/4), so we can directly calculate tan(theta/2)
// and no more operations are needed for identifying the circle center // and no more operations are needed for identifying the circle center
// 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);
c = half_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]};
} }
const auto vec_ca = a - c; const auto vec_ca = a - c;
if (bulge > 0) if (bulge > 0)
d = c + Eigen::Vector2d{vec_ca[1], -vec_ca[0]}; d = c + Eigen::Vector2d{vec_ca[1], -vec_ca[0]};
else else
d = c + Eigen::Vector2d{-vec_ca[1], vec_ca[0]}; d = c + Eigen::Vector2d{-vec_ca[1], vec_ca[0]};
aabb.extend(c); aabb.extend(c);
aabb.extend(d); aabb.extend(d);
}); });
@ -222,19 +232,21 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc,
const auto &bulge = desc.bulges[index]; const auto &bulge = desc.bulges[index];
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 + 3];
auto &c = this->vertices[start_index + 1]; auto &c = this->vertices[start_index + 1];
auto &d = this->vertices[start_index + 2]; auto &d = this->vertices[start_index + 2];
const auto half_vec_ab = 0.5 * (a + b); const auto center_vec_ab = 0.5 * (a + b);
const auto half_vec_ab = b - center_vec_ab;
Eigen::Matrix<double, 2, 1> temp = center_vec_ab; // for debug
if (theta == PI) { if (theta == PI) {
c = std::move(half_vec_ab); c = std::move(center_vec_ab);
} else { } else {
// NOTE: bulge = tan(theta/4), so we can directly calculate tan(theta/2) // NOTE: bulge = tan(theta/4), so we can directly calculate tan(theta/2)
// and no more operations are needed for identifying the circle center // and no more operations are needed for identifying the circle center
// 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);
c = half_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]};
} }
const auto vec_ca = a - c; const auto vec_ca = a - c;
if (bulge > 0) if (bulge > 0)
@ -274,11 +286,11 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc,
if (this->thetas[n] <= EPSILON) { if (this->thetas[n] <= EPSILON) {
// HINT: assume the plane normal (ref_normal) to be {0, 0, 1}, then the normal should be this // HINT: assume the plane normal (ref_normal) to be {0, 0, 1}, then the normal should be this
const auto temp = (*(iter + 1) - *iter).normalized(); const auto temp = (*(iter + 1) - *iter).normalized();
return {temp.y(), -temp.x()}; return Eigen::Vector2d{-temp.y(), temp.x()}.normalized();
} else { } else {
const auto alpha = std::cos(t * this->thetas[n]); const auto alpha = std::cos(t * this->thetas[n]);
const auto beta = std::sin(t * this->thetas[n]); const auto beta = std::sin(t * this->thetas[n]);
return (alpha + beta) * *(iter + 1) - alpha * *iter - beta * *(iter + 2); return ((alpha + beta) * *(iter + 1) - alpha * *iter - beta * *(iter + 2)).normalized();
} }
} }
@ -329,7 +341,8 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc,
const auto p_vec_norm = p_vec.norm(); const auto p_vec_norm = p_vec.norm();
const auto r = base_vec1.norm(); const auto r = base_vec1.norm();
const auto dis_on_plane = p_vec_norm - r; const auto dis_on_plane = p_vec_norm - r;
const auto closest_point = p_vec / p_vec_norm * r + *(iter + 1); auto closest_point = *iter;
if (p_vec_norm >= EPSILON) { closest_point = p_vec / p_vec_norm * r + *(iter + 1); }
return { return {
{closest_point.x(), closest_point.y(), 0}, {closest_point.x(), closest_point.y(), 0},
phi / theta, phi / theta,
@ -338,6 +351,7 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc,
}; };
std::vector<comparable_closest_param_t> closest_params(this->thetas.size()); std::vector<comparable_closest_param_t> closest_params(this->thetas.size());
const Eigen::Vector3d pTmp = p; // for debugging
algorithm::transform(counting_iterator<size_t>(), algorithm::transform(counting_iterator<size_t>(),
counting_iterator<size_t>(this->thetas.size()), counting_iterator<size_t>(this->thetas.size()),
closest_params.begin(), closest_params.begin(),
@ -368,10 +382,18 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc,
dist dist
}; };
}); });
const auto back = this->vertices.back();
if (*vertices.begin() != back) {
closest_params.emplace_back(comparable_closest_param_t{
{back.x(), back.y(), 0},
static_cast<double>(thetas.size()),
std::sqrt((p.topRows<2>() - back).squaredNorm() + p[2] * p[2])
});
}
const auto iter_ = algorithm::min_element(closest_params.begin(), closest_params.end()); const auto iter_ = algorithm::min_element(closest_params.begin(), closest_params.end());
return { return {
line_closest_param_t{std::move(iter->point), iter->t, false}, line_closest_param_t{std::move(iter_->point), iter_->t, false},
iter->dist * iter->dist_sign iter_->dist * iter_->dist_sign
}; };
} }
} }

Write Preview
Loading…
Cancel
Save