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

extrusion sdf debug

gjj
Dtouch 10 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
Split 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 <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