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
Split View
  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 <construct_helper.hpp>
#include "Eigen/Core"
#include "primitive_descriptor.h"
#include "internal_primitive_desc.hpp"
void testExtrudeSDF() {
printf("Testing extrude polyline SDF\n");
auto profilePoints = std::array{raw_vector3d_t{-1., 0., 0.}, raw_vector3d_t{1., 0., 0.}};
auto profileBulges = std::array{1.0, 1.0};
polyline_descriptor_t profile{
2,
profilePoints.data(),
2,
profileBulges.data(),
raw_vector3d_t{0., 0., 1.},
true
};
auto axisPoints = std::array{raw_vector3d_t{0., 0., 0.}, raw_vector3d_t{0., 0., 2.}};
auto axisBulges = std::array{0.0};
polyline_descriptor_t axis{
2,
axisPoints.data(),
1,
axisBulges.data(),
raw_vector3d_t{0., 1., 0.},
false
};
extrude_polyline_descriptor_t extrude{
1,
&profile,
axis
void testExtrudeSDFXoY()
{
printf("Testing extrude polyline SDF XoY\n");
auto profilePoints = std::array{
raw_vector3d_t{-1., 0., 0.},
raw_vector3d_t{1., 0., 0.}
};
auto profileBulges = std::array{1.0, 1.0};
polyline_descriptor_t profile{
2,
profilePoints.data(),
2,
profileBulges.data(),
raw_vector3d_t{0., 0., 1.},
true
};
auto axisPoints = std::array{
raw_vector3d_t{0., 0., 0.},
raw_vector3d_t{0., 0., 2.}
};
auto axisBulges = std::array{0.0};
polyline_descriptor_t axis{
2,
axisPoints.data(),
1,
axisBulges.data(),
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{};
internal::extrude_polyline extrude_polyline{extrude, aabb};
Eigen::Vector3d p{1., 2., 1.};
double res = extrude_polyline.evaluate_sdf(p);
printf("SDF at (%f, %f, %f) is %f\n", p.x(), p.y(), p.z(), res);
aabb_t<> aabb{};
internal::extrude_polyline extrude_polyline{extrude, aabb};
// Eigen::Vector3d p{1., 1., -0.1};
Eigen::Vector3d p{2.3, 2.9, 1.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 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()
{
testExtrudeSDF();
// testExtrudeSDFZoX();
testExtrudeSDFXoY();
// testPolylineSDF();
// std::cout << "Setting scene..." << std::endl;
// sphere_descriptor_t sphere1{

4
application/xmake.lua
View File

@ -10,4 +10,8 @@ target("test_frontend")
set_kind("binary")
add_deps("frontend")
add_files("main.cpp")
add_linkdirs("E:\\CProj\\ImplicitSurfaceNetwork\\build\\windows\\x64\\debug")
after_build(function(target)
print("Hello, xmake!")
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&,
const raw_vector3d_t&,
Eigen::Transform<double, 3, Eigen::AffineCompact, Eigen::DontAlign>&,
aabb_t<>&);
aabb_t<>&,
const raw_vector3d_t&);
void build_as_axis(polyline_descriptor_t&&,
const raw_vector3d_t&,
Eigen::Transform<double, 3, Eigen::AffineCompact, Eigen::DontAlign>&,
aabb_t<>&);
aabb_t<>&,
const raw_vector3d_t&);
void build_as_profile(const polyline_descriptor_t&,
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
// 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>());
if (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);
}
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
// 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 <cmath>
#include <iostream>
namespace internal
{
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();
aabb_t<2> profile_aabb{};
this->profile.build_as_profile(desc.profiles[0],
matrix_handle.col(0),
matrix_handle.col(1),
matrix_handle.col(2),
matrix_handle.col(3),
profile_aabb);
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)
{
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();
aabb_t<2> profile_aabb{};
this->profile.build_as_profile(std::move(desc.profiles[0]),
matrix_handle.col(0),
matrix_handle.col(1),
matrix_handle.col(2),
matrix_handle.col(3),
profile_aabb);
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)
{
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
local_normal = (q.topRows<2>() - p.topRows<2>()).normalized();
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 {
local_tangent = line.calculate_tangent(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 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 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
// 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>());
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
// the distance should be unchanged during transformation, since no scaling is involved
if constexpr (std::is_same_v<Routine, evaluation_routine_tag>)
return dist;
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

90
primitive_process/src/polyline.cpp
View File

@ -2,6 +2,7 @@
#include <algorithm/glue_algorithm.hpp>
#include "Eigen/Core"
#include "internal_primitive_desc.hpp"
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,
const raw_vector3d_t &profile_ref_normal,
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();
vec3d_conversion(desc.reference_normal, matrix_handle.col(1));
matrix_handle.col(1).normalize();
vec3d_conversion(profile_ref_normal, matrix_handle.col(2));
vec3d_conversion(profile_ref_normal, matrix_handle.col(0));
matrix_handle.col(0).normalize();
vec3d_conversion(desc.reference_normal, matrix_handle.col(2));
matrix_handle.col(2).normalize();
matrix_handle.col(0) = matrix_handle.col(1).cross(matrix_handle.col(2));
vec3d_conversion(desc.points[0], matrix_handle.col(3));
matrix_handle.col(1) = matrix_handle.col(2).cross(matrix_handle.col(0));
// vec3d_conversion(desc.points[0], matrix_handle.col(3));
vec3d_conversion(anchor_point, matrix_handle.col(3));
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_max = profile_aabb.max();
aabb = {
@ -54,18 +57,20 @@ void polyline::build_as_axis(const polyline_descriptor_t
void polyline::build_as_axis(polyline_descriptor_t &&desc,
const raw_vector3d_t &profile_ref_normal,
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();
vec3d_conversion(std::move(desc.reference_normal), matrix_handle.col(1));
matrix_handle.col(1).normalize();
vec3d_conversion(profile_ref_normal, matrix_handle.col(2));
vec3d_conversion(profile_ref_normal, matrix_handle.col(0));
matrix_handle.col(0).normalize();
vec3d_conversion(std::move(desc.reference_normal), matrix_handle.col(2));
matrix_handle.col(2).normalize();
matrix_handle.col(0) = matrix_handle.col(1).cross(matrix_handle.col(2));
vec3d_conversion(desc.points[0], matrix_handle.col(3));
matrix_handle.col(1) = matrix_handle.col(2).cross(matrix_handle.col(0));
// vec3d_conversion(desc.points[0], matrix_handle.col(3));
vec3d_conversion(anchor_point, matrix_handle.col(3));
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_max = profile_aabb.max();
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
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->start_indices.pop_back();
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 &theta = this->thetas[index];
const auto &a = this->vertices[start_index];
const auto &b = this->vertices[start_index + 3];
auto &c = this->vertices[start_index + 1];
auto &d = this->vertices[start_index + 2];
const auto half_vec_ab = 0.5 * (a + b);
const auto &a = this->vertices[start_index];
const auto &b = this->vertices[start_index + 3];
auto &c = this->vertices[start_index + 1];
auto &d = this->vertices[start_index + 2];
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) {
c = std::move(half_vec_ab);
c = std::move(center_vec_ab);
} else {
// NOTE: bulge = tan(theta/4), so we can directly calculate tan(theta/2)
// and no more operations are needed for identifying the circle center
// since all direction info is already included in the sign & magnitude of 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;
if (bulge > 0)
d = c + Eigen::Vector2d{vec_ca[1], -vec_ca[0]};
else
d = c + Eigen::Vector2d{-vec_ca[1], vec_ca[0]};
aabb.extend(c);
aabb.extend(d);
});
@ -222,19 +232,21 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc,
const auto &bulge = desc.bulges[index];
const auto &theta = this->thetas[index];
const auto &a = this->vertices[start_index];
const auto &b = this->vertices[start_index + 3];
auto &c = this->vertices[start_index + 1];
auto &d = this->vertices[start_index + 2];
const auto half_vec_ab = 0.5 * (a + b);
const auto &a = this->vertices[start_index];
const auto &b = this->vertices[start_index + 3];
auto &c = this->vertices[start_index + 1];
auto &d = this->vertices[start_index + 2];
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) {
c = std::move(half_vec_ab);
c = std::move(center_vec_ab);
} else {
// NOTE: bulge = tan(theta/4), so we can directly calculate tan(theta/2)
// and no more operations are needed for identifying the circle center
// since all direction info is already included in the sign & magnitude of 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;
if (bulge > 0)
@ -274,11 +286,11 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc,
if (this->thetas[n] <= EPSILON) {
// 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();
return {temp.y(), -temp.x()};
return Eigen::Vector2d{-temp.y(), temp.x()}.normalized();
} else {
const auto alpha = std::cos(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 r = base_vec1.norm();
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 {
{closest_point.x(), closest_point.y(), 0},
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());
const Eigen::Vector3d pTmp = p; // for debugging
algorithm::transform(counting_iterator<size_t>(),
counting_iterator<size_t>(this->thetas.size()),
closest_params.begin(),
@ -368,10 +382,18 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc,
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());
return {
line_closest_param_t{std::move(iter->point), iter->t, false},
iter->dist * iter->dist_sign
line_closest_param_t{std::move(iter_->point), iter_->t, false},
iter_->dist * iter_->dist_sign
};
}
}

Write Preview
Loading…
Cancel
Save