@ -2,7 +2,7 @@
namespace internal namespace internal
{ {
std : : pair < boo l , Eig en : : Vector2d > polyline_pattern : : eval_sdf ( Eigen : : Vector2d p ) const dou blepolyline_pattern : : eval_sdf ( Eigen : : Vector2d p ) const
{ {
auto line_cpm = [ & ] ( auto index ) { auto line_cpm = [ & ] ( auto index ) {
const auto a = this - > vertices [ index ] ; const auto a = this - > vertices [ index ] ;
@ -22,72 +22,163 @@ std::pair<bool, Eigen::Vector2d> polyline_pattern::eval_sdf(Eigen::Vector2d p) c
const auto cross = ap . x ( ) * bp . y ( ) - ap . y ( ) * bp . x ( ) ; const auto cross = ap . x ( ) * bp . y ( ) - ap . y ( ) * bp . x ( ) ;
const auto d_theta = std : : atan2 ( cross , dot ) ; const auto d_theta = std : : atan2 ( cross , dot ) ;
return std : : array { dist , d_theta , proj . x ( ) , proj . y ( ) } ; return std : : array { dist , d_theta } ;
} ; } ;
auto circle_cpm = [ & ] ( auto index ) { auto circle_cpm = [ & ] ( auto index ) {
const auto bulge = this - > bulges [ index ] ; const auto bulge = this - > bulges [ index ] ;
const auto a = this - > vertices [ index ] ; const auto a = this - > vertices [ index ] ;
const auto b = this - > vertices [ index + 1 ] ; const auto b = this - > vertices [ index + 1 ] ;
const auto half_ab = 0.5 * ( a + b ) ; const Eigen : : Vector2d half_ab = 0.5 * ( a + b ) ;
const auto bisec_vec = 0.5 * Eigen : : Vector2d { a . y ( ) - b . y ( ) , b . x ( ) - a . x ( ) } ; const Eigen : : Vector2d bisec_vec = 0.5 * Eigen : : Vector2d { a . y ( ) - b . y ( ) , b . x ( ) - a . x ( ) } ;
const auto center = half_ab + bisec_vec / bulge ; const Eigen : : Vector2d center = half_ab + bisec_vec / bulge ;
Eigen : : Vector2d ca = a - center ; Eigen : : Vector2d ca = a - center ;
Eigen : : Vector2d cb = b - center ; Eigen : : Vector2d cb = b - center ;
Eigen : : Vector2d cp = p - center ; Eigen : : Vector2d cp = p - center ;
Eigen : : Vector2d pa = a - p ; Eigen : : Vector2d pa = a - p ;
Eigen : : Vector2d pb = b - p ; Eigen : : Vector2d pb = b - p ;
const auto D = cp . norm ( ) ; // 查询点到圆心的距离
const auto D = cp . norm ( ) ; // 查询点到圆心的距离
const auto r = ca . norm ( ) ; // 圆弧半径
const auto r = ca . norm ( ) ; // 圆弧半径
if ( D < epsilon ) return std : : array { r , std : : atan ( bulge ) * 2 , a . x ( ) , a . y ( ) } ; const auto half_theta = std : : atan ( bulge ) ;
if ( D < = epsilon ) return std : : array { r , half_theta * 2 } ;
// 计算GWN
// 计算GWN
const auto term1 = std : : atan ( bulge ) ; const auto gwn_main = half_theta ;
auto dot = ca . dot ( cp ) ; const auto r_mul_D = r * D , r_plus_D = r + D , abs_r_sub_D = std : : abs ( r - D ) ;
const auto cross1 = cp . x ( ) * ca . y ( ) - ca . x ( ) * cp . y ( ) ; auto gwn = gwn_main ;
const auto term2 = ( ( r - D ) * cross1 ) / ( ( r + D ) * ( r * D + dot ) ) ; if ( abs_r_sub_D > epsilon ) {
dot = cb . dot ( cp ) ; auto dot = ca . dot ( cp ) ;
const auto cross2 = cp . x ( ) * cb . y ( ) - cb . x ( ) * cp . y ( ) ; auto cross = cp . x ( ) * ca . y ( ) - ca . x ( ) * cp . y ( ) ;
const auto term3 = ( ( r - D ) * cross2 ) / ( ( r + D ) * ( r * D + dot ) ) ; auto Y = r_plus_D * cross , X = abs_r_sub_D * ( r_mul_D + dot ) ;
const auto gwn = term1 + term3 - term2 ; auto Ga = std : : atan2 ( Y , X ) ;
if ( std : : abs ( X ) < = epsilon & & std : : abs ( Y ) < = epsilon ) Ga = std : : copysign ( pi_div_2 , - half_theta ) ;
cp / = D ; dot = cb . dot ( cp ) ;
ca / = r ; cross = cp . x ( ) * cb . y ( ) - cb . x ( ) * cp . y ( ) ;
cb / = r ; Y = r_plus_D * cross ;
X = abs_r_sub_D * ( r_mul_D + dot ) ;
// 原理：根据叉乘符号判断方向，不论何种情况cp都应该在ca和cb之间
auto Gb = std : : atan2 ( Y , X ) ;
if ( cross1 * cross2 > 0 ) { if ( std : : abs ( X ) < = epsilon & & std : : abs ( Y ) < = epsilon ) Gb = std : : copysign ( pi_div_2 , - half_theta ) ;
// 在圆弧范围内
auto gwn_fix = Gb - Ga ;
return std : : array { D - r , gwn , center . x ( ) + cp . x ( ) , center . y ( ) + cp . y ( ) } ; if ( gwn_fix * half_theta < 0 ) gwn_fix + = std : : copysign ( pi , half_theta ) ;
} else { gwn + = ( r > D ) ? gwn_fix : - gwn_fix ;
// 最近点为圆弧端点
if ( auto pa_norm = pa . norm ( ) , pb_norm = pb . norm ( ) ; pa_norm < pb_norm )
return std : : array { pa_norm , gwn , a . x ( ) , a . y ( ) } ;
else
return std : : array { pa_norm , gwn , b . x ( ) , b . y ( ) } ;
} }
return std : : array { std : : min ( { abs_r_sub_D , pa . norm ( ) , pb . norm ( ) } ) , gwn } ;
} ; } ;
// 单次遍历
// 单次遍历
std : : vector < size_t > indices ( this - > bulges . size ( ) ) ; std : : vector < size_t > indices ( this - > bulges . size ( ) ) ;
std : : iota ( indices . begin ( ) , indices . end ( ) , 0 ) ; std : : iota ( indices . begin ( ) , indices . end ( ) , 0 ) ;
auto [ min_dist , gwn , x , y ] = std : : transform_reduce ( auto [ min_dist , gwn ] = std : : transform_reduce (
indices . begin ( ) , indices . begin ( ) ,
indices . end ( ) , indices . end ( ) ,
std : : array { std : : numeric_limits < double > : : max ( ) , .0 , .0 , .0 } , std : : array { std : : numeric_limits < double > : : max ( ) , .0 } ,
[ ] ( auto & & lhs , auto & & rhs ) { [ ] ( auto & & lhs , auto & & rhs ) {
const auto gwn = lhs [ 1 ] + rhs [ 1 ] ; const auto gwn = lhs [ 1 ] + rhs [ 1 ] ;
if ( lhs [ 0 ] < rhs [ 0 ] ) const auto dist = std : : min ( lhs [ 0 ] , rhs [ 0 ] ) ;
return std : : array { lhs [ 0 ] , gwn , lhs [ 2 ] , lhs [ 3 ] } ; return std : : array { dist , gwn } ;
else
return std : : array { rhs [ 0 ] , gwn , rhs [ 2 ] , rhs [ 3 ] } ;
} , } ,
[ & ] ( size_t i ) { return ( std : : abs ( this - > bulges [ i ] ) < = epsilon ) ? line_cpm ( i ) : circle_cpm ( i ) ; } ) ; [ & ] ( size_t i ) { return ( std : : abs ( this - > bulges [ i ] ) < = epsilon ) ? line_cpm ( i ) : circle_cpm ( i ) ; } ) ;
return { // if (p.norm() > 1 && std::abs(gwn) / two_pi != .0) abort();
std : : abs ( gwn ) < two_pi * epsilon , return std : : abs ( gwn ) / two_pi < 1. ? min_dist : - min_dist ;
Eigen : : Vector2d { x , y }
} ;
} }
// double polyline_pattern::eval_sdf(Eigen::Vector2d p) const
// {
// auto line_cpm = [&](auto index) {
// const auto a = this->vertices[index];
// const auto b = this->vertices[index + 1];
// const Eigen::Vector2d line_vec = b - a;
// const Eigen::Vector2d ap = p - a;
// const Eigen::Vector2d bp = p - b;
// // 计算投影参数
// const auto t = std::clamp(line_vec.dot(ap) / line_vec.squaredNorm(), .0, 1.);
// const Eigen::Vector2d proj = a + t * line_vec;
// const auto dist = (p - proj).norm();
// // 计算GWN
// const auto dot = ap.dot(bp);
// const auto cross = ap.x() * bp.y() - ap.y() * bp.x();
// const auto d_theta = std::atan2(cross, dot);
// return std::array{dist, d_theta, proj.x(), proj.y()};
// };
// auto circle_cpm = [&](auto index) {
// const auto bulge = this->bulges[index];
// const auto a = this->vertices[index];
// const auto b = this->vertices[index + 1];
// const Eigen::Vector2d half_ab = 0.5 * (a + b);
// const Eigen::Vector2d bisec_vec = 0.5 * Eigen::Vector2d{a.y() - b.y(), b.x() - a.x()};
// const Eigen::Vector2d center = half_ab + bisec_vec / bulge;
// Eigen::Vector2d ca = a - center;
// Eigen::Vector2d cb = b - center;
// Eigen::Vector2d cp = p - center;
// Eigen::Vector2d pa = a - p;
// Eigen::Vector2d pb = b - p;
// const auto D = cp.norm(); // 查询点到圆心的距离
// const auto r = ca.norm(); // 圆弧半径
// const auto half_theta = std::atan(bulge);
// if (D < epsilon) return std::array{r, half_theta * 2, a.x(), a.y()};
// // 计算GWN
// const auto gwn_main = half_theta;
// const auto r_mul_D = r * D, r_plus_D = r + D, abs_r_sub_D = std::abs(r - D);
// const auto cross1 = cp.x() * ca.y() - ca.x() * cp.y();
// const auto cross2 = cp.x() * cb.y() - cb.x() * cp.y();
// auto gwn = gwn_main;
// if (abs_r_sub_D > epsilon) {
// auto dot = ca.dot(cp);
// const auto Ga = std::atan2(r_plus_D * cross1, abs_r_sub_D * (r_mul_D + dot));
// dot = cb.dot(cp);
// const auto Gb = std::atan2(r_plus_D * cross2, abs_r_sub_D * (r_mul_D + dot));
// auto gwn_fix = Gb - Ga;
// if (gwn_fix * half_theta < 0) gwn_fix += std::copysign(pi, half_theta);
// gwn += (r > D) ? gwn_fix : -gwn_fix;
// }
// cp /= D;
// ca /= r;
// cb /= r;
// // 原理：根据叉乘符号判断方向，不论何种情况cp都应该在ca和cb之间
// if (cross1 * cross2 > 0) {
// // 在圆弧范围内
// return std::array{D - r, gwn, center.x() + cp.x(), center.y() + cp.y()};
// } else {
// // 最近点为圆弧端点
// if (auto pa_norm = pa.norm(), pb_norm = pb.norm(); pa_norm < pb_norm)
// return std::array{pa_norm, gwn, a.x(), a.y()};
// else
// return std::array{pa_norm, gwn, b.x(), b.y()};
// }
// };
// // 单次遍历
// std::vector<size_t> indices(this->bulges.size());
// std::iota(indices.begin(), indices.end(), 0);
// auto [min_dist, gwn, x, y] = std::transform_reduce(
// indices.begin(),
// indices.end(),
// std::array{std::numeric_limits<double>::max(), .0, .0, .0},
// [](auto&& lhs, auto&& rhs) {
// const auto gwn = lhs[1] + rhs[1];
// if (lhs[0] < rhs[0])
// return std::array{lhs[0], gwn, lhs[2], lhs[3]};
// else
// return std::array{rhs[0], gwn, rhs[2], rhs[3]};
// },
// [&](size_t i) { return (std::abs(this->bulges[i]) <= epsilon) ? line_cpm(i) : circle_cpm(i); });
// return {
// // std::abs(gwn) < two_pi * epsilon,
// std::abs(gwn) == 0.,
// Eigen::Vector2d{x, y}
// };
// }
} // namespace internal
} // namespace internal
Zhicheng Wang
1 day ago