You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
156 lines
7.6 KiB
156 lines
7.6 KiB
#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,
|
|
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(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();
|
|
aabb.min().array() -= profile_max_extent;
|
|
aabb.max().array() += profile_max_extent;
|
|
}
|
|
|
|
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,
|
|
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(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();
|
|
aabb.min().array() -= profile_max_extent;
|
|
aabb.max().array() += profile_max_extent;
|
|
}
|
|
|
|
// CAUTION: take care of relations between line's local TBN and plane's local TBN
|
|
// CAUTION: all internal calculations of polyline should assume that p is in axis/plane's local TBN space
|
|
static inline auto get_local_TBN(const polyline &line,
|
|
const Eigen::Ref<const Eigen::Vector4d> &p,
|
|
const Eigen::Ref<const Eigen::Vector3d> &q,
|
|
double t)
|
|
{
|
|
Eigen::Vector2d local_tangent, local_normal;
|
|
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]};
|
|
} else {
|
|
local_tangent = line.calculate_tangent(t);
|
|
local_normal = line.calculate_normal(t);
|
|
}
|
|
|
|
Eigen::Transform<double, 3, Eigen::Isometry> TBN{};
|
|
auto &handle = TBN.matrix();
|
|
handle.col(0)[2] = 1;
|
|
handle.col(1).topRows<2>() = std::move(local_normal);
|
|
handle.col(2).topRows<2>() = std::move(local_tangent);
|
|
handle.col(3).topRows<3>() = std::move(q);
|
|
|
|
return TBN;
|
|
}
|
|
|
|
template <typename Routine>
|
|
static inline process_return_type_t<Routine> evaluate(const extrude_polyline &desc,
|
|
[[maybe_unused]] const Eigen::Ref<const Eigen::Vector3d> &point)
|
|
{
|
|
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, _] = 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>());
|
|
|
|
// 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 * profile_closest_param.point;
|
|
}
|
|
|
|
double extrude_polyline::evaluate_sdf([[maybe_unused]] const Eigen::Ref<const Eigen::Vector3d> &point) const
|
|
{
|
|
return evaluate<evaluation_routine_tag>(*this, point);
|
|
}
|
|
|
|
Eigen::Vector3d extrude_polyline::evaluate_cpm([[maybe_unused]] const Eigen::Ref<const Eigen::Vector3d> &point) const
|
|
{
|
|
return evaluate<closest_point_routine_tag>(*this, point);
|
|
}
|
|
} // namespace internal
|