|
|
@ -2,7 +2,10 @@ |
|
|
|
|
|
|
|
#include "common.hpp" |
|
|
|
#include "real.hpp" |
|
|
|
#include <cassert> |
|
|
|
#include <cstddef> |
|
|
|
#include <utility> |
|
|
|
#include <vector> |
|
|
|
#include "vec.hpp" |
|
|
|
#include "line.hpp" |
|
|
|
|
|
|
@ -12,88 +15,110 @@ public: |
|
|
|
|
|
|
|
virtual real sdf(const Vec3 &p) = 0; |
|
|
|
}; |
|
|
|
|
|
|
|
Vec2 get2DRepOf3DPt(const Vec3 &pt3D, const Vec3 &u, const Vec3 &v, const Vec3 &localO) { |
|
|
|
inline Vec2 get2DRepOf3DPt(const Vec3 &pt3D, const Vec3 &u, const Vec3 &v, const Vec3 &localO) { |
|
|
|
Vec3 OP = pt3D - localO; |
|
|
|
return {OP.dot(u), OP.dot(v)}; |
|
|
|
} |
|
|
|
|
|
|
|
Vec2 get2DRepOf3DDir(const Vec3 &dir, const Vec3 &u, const Vec3 &v) { |
|
|
|
inline Vec2 get2DRepOf3DDir(const Vec3 &dir, const Vec3 &u, const Vec3 &v) { |
|
|
|
return Vec2{dir.dot(u), dir.dot(v)}.normalize(); |
|
|
|
} |
|
|
|
|
|
|
|
class IExtrudedSolid : public ISolid { |
|
|
|
public: |
|
|
|
Polyline _profile; // TODO: may be replaced by const ref to profile
|
|
|
|
std::vector<Polyline> _profiles; // TODO: may be replaced by const ref to profile
|
|
|
|
real _rScale; |
|
|
|
|
|
|
|
public: |
|
|
|
IExtrudedSolid(Polyline profile, real rScale) : _profile(std::move(profile)), _rScale(rScale) {} |
|
|
|
IExtrudedSolid(std::vector<Polyline> profiles, real rScale) |
|
|
|
: _profiles(std::move(profiles)), _rScale(rScale) {} |
|
|
|
}; |
|
|
|
|
|
|
|
/**
|
|
|
|
* calculate winding number of a point w.r.t. a segment ab |
|
|
|
*/ |
|
|
|
real unsignedWindingNumberSegment(const Vec3 &p, const Vec3 &a, const Vec3 &b, |
|
|
|
const Vec3 &refNormal) { |
|
|
|
Vec3 pa = a - p; |
|
|
|
Vec3 pb = b - p; |
|
|
|
return std::acos(std::clamp(pa.dot(pb) / (pa.norm() * pb.norm()), static_cast<real>(-1.), |
|
|
|
static_cast<real>(1.))) |
|
|
|
/ (std::numbers::pi * 2); |
|
|
|
} |
|
|
|
// inline real unsignedWindingNumberSegment(const Vec3 &p, const Vec3 &a, const Vec3 &b,
|
|
|
|
// const Vec3 &refNormal) {
|
|
|
|
// Vec3 pa = a - p;
|
|
|
|
// Vec3 pb = b - p;
|
|
|
|
// return std::acos(std::clamp(pa.dot(pb) / (pa.norm() * pb.norm()), static_cast<real>(-1.),
|
|
|
|
// static_cast<real>(1.)))
|
|
|
|
// / (std::numbers::pi * 2);
|
|
|
|
// }
|
|
|
|
|
|
|
|
class ExtrudedSolidPolyline : public IExtrudedSolid { |
|
|
|
private: |
|
|
|
Polyline _axis; |
|
|
|
Pt2Array _localProfile2D; |
|
|
|
std::vector<CircularArc<Vec2>> _localArcs2d; |
|
|
|
std::vector<Pt2Array> _localProfiles2D; |
|
|
|
std::vector<std::vector<CircularArc<Vec2>>> _localArcs2d; |
|
|
|
// Pt2Array _localCircleCenter2D;
|
|
|
|
// Pt2Array _localInCircleDir;
|
|
|
|
|
|
|
|
public: |
|
|
|
ExtrudedSolidPolyline(Polyline profile, Polyline axis, real rScale) |
|
|
|
: IExtrudedSolid(std::move(profile), rScale), _axis(std::move(axis)) { |
|
|
|
assert(_profile.isClosed()); |
|
|
|
ExtrudedSolidPolyline(std::vector<Polyline> profiles, Polyline axis, real rScale) |
|
|
|
: IExtrudedSolid(std::move(profiles), rScale), _axis(std::move(axis)) { |
|
|
|
assert(!_profiles.empty()); |
|
|
|
for (const auto &_profile : _profiles) { |
|
|
|
assert(_profile.isClosed()); |
|
|
|
} |
|
|
|
// TODO: project profile at st point to 2D
|
|
|
|
Vec3 T = _axis.der1(0).normalize(); |
|
|
|
Vec3 N = _axis.der2(0).normalize(); |
|
|
|
Vec3 B = T.cross(N); |
|
|
|
Vec3 Q = _axis.eval(0); |
|
|
|
int segCount = _profile.getPoints().size(); |
|
|
|
_localProfile2D.resize(segCount); |
|
|
|
_localArcs2d.resize(segCount); |
|
|
|
// _localInCircleDir.resize(segCount);
|
|
|
|
// _localCircleCenter2D.resize(segCount);
|
|
|
|
for (int i = 0; i < segCount; ++i) { |
|
|
|
_localProfile2D[i] = get2DRepOf3DPt(_profile.getPoints()[i] - Q, N, B, Q); |
|
|
|
auto &arc2d = _localArcs2d[i]; |
|
|
|
const auto &arc3d = _profile.getCircularArcs()[i]; |
|
|
|
arc2d.center = get2DRepOf3DPt(arc3d.center - Q, N, B, Q); |
|
|
|
arc2d.inCircleDir = get2DRepOf3DDir(arc3d.inCircleDir, N, B); |
|
|
|
arc2d.radius = arc3d.radius; |
|
|
|
size_t profileCount = _profiles.size(); |
|
|
|
_localProfiles2D.resize(profileCount); |
|
|
|
_localArcs2d.resize(profileCount); |
|
|
|
for (int i = 0; i < _profiles.size(); ++i) { |
|
|
|
const auto &profile = _profiles[i]; |
|
|
|
size_t segCount = profile.getPoints().size(); |
|
|
|
_localProfiles2D[i].resize(segCount); |
|
|
|
_localArcs2d[i].resize(segCount); |
|
|
|
for (int j = 0; j < segCount; ++j) { |
|
|
|
// TODO:
|
|
|
|
_localProfiles2D[i][j] = get2DRepOf3DPt(profile.getPoints()[j] - Q, N, B, Q); |
|
|
|
auto &arc2d = _localArcs2d[i][j]; |
|
|
|
const auto &arc3d = profile.getCircularArcs()[j]; |
|
|
|
arc2d.center = get2DRepOf3DPt(arc3d.center - Q, N, B, Q); |
|
|
|
arc2d.inCircleDir = get2DRepOf3DDir(arc3d.inCircleDir, N, B); |
|
|
|
arc2d.radius = arc3d.radius; |
|
|
|
arc2d.theta = arc3d.theta; |
|
|
|
arc2d.h = arc3d.h; |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
real sdf(const Vec3 &p) override { |
|
|
|
ClosestDescOnSeg closestDesc = _axis.getClosestParam(p); |
|
|
|
ClosestDescOnSeg closestDescToAxis = _axis.getClosestParam(p); |
|
|
|
// TNB coordinate system
|
|
|
|
auto t = closestDesc.t; |
|
|
|
auto t = closestDescToAxis.t; |
|
|
|
Vec3 T = _axis.der1(t).normalize(); |
|
|
|
Vec3 N = _axis.der2(t).normalize(); |
|
|
|
Vec3 B = T.cross(N); |
|
|
|
Vec3 Q = _axis.eval(t); |
|
|
|
Vec3 QP = p - Q; |
|
|
|
auto p2D = get2DRepOf3DPt(QP, N, B, Q); |
|
|
|
// TODO: to test if p2D is in _localProfile2D
|
|
|
|
// for (auto i = 0; i < _localProfile2D.size(); ++i) {
|
|
|
|
// }
|
|
|
|
PtBoundaryRelation ptProfileRelation = getPtProfileRelation(p2D); |
|
|
|
if (ptProfileRelation == OnBoundary) { |
|
|
|
return 0; // TODO: 判断OnBoundary的过程可以加一点容差
|
|
|
|
// PMC
|
|
|
|
PtBoundaryRelation ptProfileRelation = Inside; |
|
|
|
for (int i = 0; i < _localArcs2d.size(); ++i) { |
|
|
|
PtBoundaryRelation relationTmp = |
|
|
|
getPtProfileRelation(p2D, _localProfiles2D[i], _localArcs2d[i]); |
|
|
|
if (relationTmp == OnBoundary) { |
|
|
|
return 0; // TODO: 判断OnBoundary的过程可以加一点容差
|
|
|
|
} |
|
|
|
if ((relationTmp == Outside && i == 0) || (relationTmp == Inside && i != 0)) { |
|
|
|
ptProfileRelation = Outside; |
|
|
|
break; |
|
|
|
} |
|
|
|
} |
|
|
|
ClosestDescOnSeg closestDescOnProfile = distance2Profile2D(p2D); |
|
|
|
return closestDescOnProfile.dis * static_cast<int>(ptProfileRelation); |
|
|
|
// distance
|
|
|
|
ClosestDescOnSeg closestDescToProfile{}; |
|
|
|
for (int i = 0; i < _localArcs2d.size(); ++i) { |
|
|
|
ClosestDescOnSeg closestDescTemp = |
|
|
|
distance2Profile2D(p2D, _localProfiles2D[i], _localArcs2d[i]); |
|
|
|
if (closestDescTemp.dis < closestDescToProfile.dis) { |
|
|
|
closestDescToProfile = closestDescTemp; |
|
|
|
} |
|
|
|
} |
|
|
|
return closestDescToProfile.dis * static_cast<int>(ptProfileRelation); |
|
|
|
} |
|
|
|
|
|
|
|
private: |
|
|
@ -103,19 +128,18 @@ private: |
|
|
|
* out + in = in |
|
|
|
* out + out = out |
|
|
|
*/ |
|
|
|
PtBoundaryRelation getPtProfileRelation(const Vec2 &p2D) { |
|
|
|
assert(_profile.isClosed()); |
|
|
|
|
|
|
|
int segCount = _profile.getBugles().size(); |
|
|
|
static PtBoundaryRelation getPtProfileRelation(const Vec2 &p2D, const Pt2Array &profile2D, |
|
|
|
const std::vector<CircularArc<Vec2>> &arcs2d) { |
|
|
|
size_t segCount = arcs2d.size(); |
|
|
|
// 先判断是否在outline上
|
|
|
|
// 顺便判断点-扇的位置关系
|
|
|
|
bool inFan = false; |
|
|
|
int onLinesegButHasBugle = -1; |
|
|
|
for (int i = 0; i < segCount; ++i) { |
|
|
|
const Vec2 &a = _localProfile2D[i]; |
|
|
|
const Vec2 &b = _localProfile2D[(i + 1) % segCount]; |
|
|
|
if (_profile.getBugles()[i] == 0) { |
|
|
|
//line segment
|
|
|
|
const Vec2 &a = profile2D[i]; |
|
|
|
const Vec2 &b = profile2D[(i + 1) % segCount]; |
|
|
|
if (arcs2d[i].h <= EPS) { |
|
|
|
//straight line segment
|
|
|
|
if (isPointOnSegment(p2D, a, b)) { |
|
|
|
return OnBoundary; |
|
|
|
} |
|
|
@ -125,9 +149,9 @@ private: |
|
|
|
onLinesegButHasBugle = i; |
|
|
|
break; |
|
|
|
} |
|
|
|
const auto &arc = _profile.getCircularArcs()[i]; |
|
|
|
real po = (p2D - _localArcs2d[i].center).norm(); |
|
|
|
if ((p2D - a).dot(_localArcs2d[i].inCircleDir) > 0) { |
|
|
|
const auto &arc = arcs2d[i]; |
|
|
|
real po = (p2D - arc.center).norm(); |
|
|
|
if ((p2D - a).dot(arc.inCircleDir) > 0) { |
|
|
|
if (po == arc.radius) { |
|
|
|
return OnBoundary; |
|
|
|
} |
|
|
@ -151,13 +175,13 @@ private: |
|
|
|
int majorityIn = 0; // 在多边形内的射线计数
|
|
|
|
int majorityOut = 0; // 在多边形外的射线计数
|
|
|
|
for (int rayIdx = 0; rayIdx < numRays; ++rayIdx) { |
|
|
|
double angle = (2.0 * std::numbers::pi * rayIdx) / numRays; |
|
|
|
double angle = (PI2 * rayIdx) / numRays; |
|
|
|
Vec2 rayDir(cos(angle), sin(angle)); |
|
|
|
int crossings = 0; |
|
|
|
|
|
|
|
for (int i = 0; i < segCount; ++i) { |
|
|
|
const Vec2 &a = _localProfile2D[i]; |
|
|
|
const Vec2 &b = _localProfile2D[(i + 1) % segCount]; |
|
|
|
const Vec2 &a = profile2D[i]; |
|
|
|
const Vec2 &b = profile2D[(i + 1) % segCount]; |
|
|
|
assert(isPointOnSegment(p, a, b)); |
|
|
|
// if (isPointOnSegment(p2D, a, b))
|
|
|
|
// {
|
|
|
@ -195,23 +219,20 @@ private: |
|
|
|
if (onLinesegButHasBugle != -1) { |
|
|
|
// 需要特殊考虑的情况
|
|
|
|
// 从p2D向inCircle方向前进一小步
|
|
|
|
Vec2 samplePt = p2D |
|
|
|
+ _localArcs2d[onLinesegButHasBugle].center |
|
|
|
* std::numeric_limits<real>::epsilon() * 1e6; |
|
|
|
Vec2 samplePt = p2D + arcs2d[onLinesegButHasBugle].inCircleDir * EPS; |
|
|
|
return !ptInPolygon(samplePt) ? Inside : Outside; // 取反
|
|
|
|
} |
|
|
|
return ptInPolygon(p2D) ^ inFan ? Inside : Outside; |
|
|
|
// TODO: 返回on的情况
|
|
|
|
} |
|
|
|
|
|
|
|
ClosestDescOnSeg distance2Profile2D(const Vec2 &p2D) { |
|
|
|
// TODO: 2D 下点到圆弧的距离应该可以直接算,不用这么迭代!
|
|
|
|
assert(_profile.isClosed()); |
|
|
|
static ClosestDescOnSeg distance2Profile2D(const Vec2 &p2D, const Pt2Array &profile2D, |
|
|
|
const std::vector<CircularArc<Vec2>> &arcs2d) { |
|
|
|
size_t segCount = arcs2d.size(); |
|
|
|
assert(profile2D.size() == segCount); |
|
|
|
ClosestDescOnSeg res{}; |
|
|
|
for (int i = 0; i < _localArcs2d.size(); ++i) { |
|
|
|
for (int i = 0; i < segCount; ++i) { |
|
|
|
auto disDesc = |
|
|
|
distance2Arc2D(p2D, _localProfile2D[i], |
|
|
|
_localProfile2D[(i + 1) % _localArcs2d.size()], _localArcs2d[i]); |
|
|
|
distance2Arc2D(p2D, profile2D[i], profile2D[(i + 1) % segCount], arcs2d[i]); |
|
|
|
if (res.dis > disDesc.dis) { |
|
|
|
res.dis = disDesc.dis; |
|
|
|
res.t = i + disDesc.t; |
|
|
@ -220,20 +241,20 @@ private: |
|
|
|
return res; |
|
|
|
} |
|
|
|
|
|
|
|
ClosestDescOnSeg distance2Arc2D(const Vec2 &p2D, const Vec2 &a, const Vec2 &b, |
|
|
|
const CircularArc<Vec2> &arc) { |
|
|
|
static ClosestDescOnSeg distance2Arc2D(const Vec2 &p2D, const Vec2 &a, const Vec2 &b, |
|
|
|
const CircularArc<Vec2> &arc) { |
|
|
|
const Vec2 ¢er = arc.center; |
|
|
|
Vec2 op = p2D - center; |
|
|
|
Vec2 q = center + arc.radius * op.normalize(); // closest pt on circle
|
|
|
|
Vec2 oq = q - center; |
|
|
|
Vec2 oa = a - center; |
|
|
|
// 判断q是否在弧上
|
|
|
|
if ((q - a).dot(arc.inCircleDir) > 0) { |
|
|
|
// 计算参数
|
|
|
|
real normMulti = arc.radius * oq.norm(); |
|
|
|
real cosTheta = (oa).dot(oq) / normMulti; |
|
|
|
real sinTheta = (oa).cross(oq) / normMulti; |
|
|
|
return {atan2(sinTheta, cosTheta), (p2D - q).norm()}; |
|
|
|
Vec2 oq = q - center; |
|
|
|
Vec2 oa = a - center; |
|
|
|
real R2 = arc.radius * arc.radius; |
|
|
|
real cosTheta = (oa).dot(oq) / R2; |
|
|
|
real sinTheta = (oa).cross(oq) / R2; |
|
|
|
return {atan2(sinTheta, cosTheta) / arc.theta, (p2D - q).norm()}; |
|
|
|
} |
|
|
|
|
|
|
|
real paDis = (a - p2D).norm(); |
|
|
@ -243,12 +264,13 @@ private: |
|
|
|
return {1, pbDis}; |
|
|
|
} |
|
|
|
|
|
|
|
bool isOn2DPolyline(const Vec2 &p2D) { |
|
|
|
int segCount = _profile.getBugles().size(); |
|
|
|
bool isOn2DPolyline(const Vec2 &p2D, const Pt2Array &profile2D, |
|
|
|
const std::vector<CircularArc<Vec2>> &arcs2d) { |
|
|
|
size_t segCount = arcs2d.size(); |
|
|
|
for (int i = 0; i < segCount; ++i) { |
|
|
|
const Vec2 &a = _localProfile2D[i]; |
|
|
|
const Vec2 &b = _localProfile2D[(i + 1) % segCount]; |
|
|
|
if (_profile.getBugles()[i] == 0) { |
|
|
|
const Vec2 &a = profile2D[i]; |
|
|
|
const Vec2 &b = profile2D[(i + 1) % segCount]; |
|
|
|
if (arcs2d[i].h <= EPS) { |
|
|
|
//line segment
|
|
|
|
if (isPointOnSegment(p2D, a, b)) { |
|
|
|
return true; |
|
|
@ -256,13 +278,16 @@ private: |
|
|
|
continue; |
|
|
|
} |
|
|
|
} |
|
|
|
// TODO: 没写完,但是暂时用不到
|
|
|
|
return true; |
|
|
|
} |
|
|
|
|
|
|
|
bool isPointOnSegment(const Vec2 p, const Vec2 &a, const Vec2 &b) { |
|
|
|
static bool isPointOnSegment(const Vec2 &p, const Vec2 &a, const Vec2 &b) { |
|
|
|
// check collinearity
|
|
|
|
double crossProduct = (p[1] - a[1]) * (b[0] - a[0]) - (p[0] - a[0]) * (b[1] - a[1]); |
|
|
|
if (!isEqual(crossProduct, 0)) |
|
|
|
if (!isEqual(crossProduct, 0)) { |
|
|
|
return false; // Not collinear
|
|
|
|
} |
|
|
|
|
|
|
|
// Check if point is within segment bounds
|
|
|
|
return (p[0] >= std::min(a[0], b[0]) && p[0] <= std::max(a[0], b[0]) |
|
|
|