diff --git a/Geo/GEdge.cpp b/Geo/GEdge.cpp index 30baf14d14184424330ee5cb963853597641e31e..a760fbd1a7109bb32b300bb22b868016a30c010c 100644 --- a/Geo/GEdge.cpp +++ b/Geo/GEdge.cpp @@ -517,3 +517,55 @@ void GEdge::relocateMeshVertices() } } } + +typedef struct { + SPoint3 p; + double t; + int next; +} sortedPoint; + +static double sqDistPointSegment(const SPoint3 &p, const SPoint3 &s0, const SPoint3 &s1) +{ + SVector3 d(s1 - s0); + SVector3 d0(p - s0); + SVector3 d1(p - s1); + double dn2 = crossprod(d, d0).normSq(); + double dt2 = std::max(0., std::max(-dot(d, d0), dot(d, d1))); + dt2 *= dt2; + return (dt2 + dn2) / d.normSq(); +} + +static void _discretize(double tol, GEdge * edge, std::vector<sortedPoint> &upts, int pos0) +{ + const int pos1 = upts[pos0].next; + const SPoint3 & p0 = upts[pos0].p; + const double t0 = upts[pos0].t; + const SPoint3 & p1 = upts[pos1].p; + const double t1 = upts[pos1].t; + const double tmid = 0.5 * (t0 + t1); + const SPoint3 pmid(edge->position(tmid)); + const double d2 = sqDistPointSegment(pmid, p0, p1); + if (d2 < tol * tol) + return; + upts.push_back((sortedPoint){pmid, tmid, pos1}); + const int posmid = upts.size() - 1; + upts[pos0].next = posmid; + _discretize(tol, edge, upts, pos0); + _discretize(tol, edge, upts, posmid); +} + +void GEdge::discretize(double tol, std::vector<SPoint3> &dpts, std::vector<double> &ts) +{ + std::vector<sortedPoint> upts; + upts.push_back((sortedPoint){getBeginVertex()->xyz(), 0., 1}); + upts.push_back((sortedPoint){getEndVertex()->xyz(), 1., -1}); + _discretize(tol, this, upts, 0); + dpts.clear(); + dpts.reserve(upts.size()); + ts.clear(); + ts.reserve(upts.size()); + for (int p = 0; p != -1; p = upts[p].next) { + dpts.push_back(upts[p].p); + ts.push_back(upts[p].t); + } +} diff --git a/Geo/GEdge.h b/Geo/GEdge.h index cf7b3cbfe9680c41b7a767480a07621da561db44..e08fd177cdf074c3462898b87ea68d8691e66b0b 100644 --- a/Geo/GEdge.h +++ b/Geo/GEdge.h @@ -209,7 +209,7 @@ class GEdge : public GEntity { void addLine(MLine *line){ lines.push_back(line); } bool computeDistanceFromMeshToGeometry (double &d2, double &dmax); - virtual void discretize(double tol, std::vector<SPoint3> &dpts, std::vector<double> &ts) {Msg::Fatal("not implemented");} + virtual void discretize(double tol, std::vector<SPoint3> &dpts, std::vector<double> &ts); }; #endif diff --git a/Geo/gmshEdge.cpp b/Geo/gmshEdge.cpp index 8d4a2fa398e2f3a15daec307842a49262433b21e..09e982a1a7a55b9bb44acc8505801342cd8ff5a4 100644 --- a/Geo/gmshEdge.cpp +++ b/Geo/gmshEdge.cpp @@ -528,6 +528,6 @@ void gmshEdge::discretize(double tol, std::vector<SPoint3> &pts, std::vector<dou break; } default : - Msg::Fatal("not implemented"); + GEdge::discretize(tol, pts, ts); } } diff --git a/Numeric/decasteljau.cpp b/Numeric/decasteljau.cpp index eee16bb0e13ed6f9f024f1eb71de33273d4d5b50..f967f2a7b1ccee1c695b0ae9475553efabb5d1b3 100644 --- a/Numeric/decasteljau.cpp +++ b/Numeric/decasteljau.cpp @@ -2,30 +2,31 @@ #include "SPoint3.h" #include "SVector3.h" -class discreteList { - std::vector<std::pair<SPoint3, double> > _pts; - std::vector<int> _next; - public: - int insertPoint(int pos, const SPoint3 &pt, double t) { - _pts.push_back(std::make_pair(pt, t)); - _next.push_back(_next[pos + 1]); - _next[pos + 1] = _pts.size() - 1; - return _pts.size() - 1; - } - void sort(std::vector<SPoint3> &spts, std::vector<double> &ts) { - spts.clear(); - spts.reserve(_pts.size()); - ts.clear(); - ts.reserve(_pts.size()); - for (int p = _next[0]; p != -1; p = _next[p + 1]) { - spts.push_back(_pts[p].first); - ts.push_back(_pts[p].second); - } - } - discreteList() { - _next.push_back(-1); +typedef struct { + SPoint3 p; + double t; + int next; +} sortedPoint; + +static int sortedPointInsert(const SPoint3 &p, const double t, std::vector<sortedPoint> &pts, int pos) +{ + pts.push_back((sortedPoint) {p, t, pts[pos].next}); + int newp = pts.size() - 1; + pts[pos].next = newp; + return newp; +} + +static void sortedPointToVector(const std::vector<sortedPoint> &spts, std::vector<SPoint3> &pts, std::vector<double> &ts) +{ + pts.clear(); + pts.reserve(spts.size()); + ts.clear(); + ts.reserve(spts.size()); + for (int p = 0; p != -1; p = spts[p].next) { + pts.push_back(spts[p].p); + ts.push_back(spts[p].t); } -}; +} double sqDistPointSegment(const SPoint3 &p, const SPoint3 &s0, const SPoint3 &s1) { @@ -38,7 +39,7 @@ double sqDistPointSegment(const SPoint3 &p, const SPoint3 &s0, const SPoint3 &s1 return (dt2 + dn2) / d.normSq(); } -static void decasteljau(double tol, discreteList &discrete, int pos, const SPoint3 &p0, const SPoint3 &p1, const SPoint3 &p2, double t0, double t2) +static void decasteljau(double tol, std::vector<sortedPoint> &discrete, int pos, const SPoint3 &p0, const SPoint3 &p1, const SPoint3 &p2, double t0, double t2) { if(sqDistPointSegment(p1, p0, p2) < tol * tol) return; @@ -46,21 +47,21 @@ static void decasteljau(double tol, discreteList &discrete, int pos, const SPoin SPoint3 p12((p1 + p2) * 0.5); SPoint3 p012((p01 + p12) * 0.5); double t012 = 0.5 * (t0 + t2); - int newpos = discrete.insertPoint(pos, p012, t012); + int newpos = sortedPointInsert(p012, t012, discrete, pos); decasteljau(tol, discrete, pos, p0, p01, p012, t0, t012); decasteljau(tol, discrete, newpos, p012, p12, p2, t012, t2); } void decasteljau(double tol, const SPoint3 &p0, const SPoint3 &p1, const SPoint3 &p2, std::vector<SPoint3> &pts, std::vector<double> &ts) { - discreteList discrete; - discrete.insertPoint(-1, p0, 0); - discrete.insertPoint(0, p2, 1); + std::vector<sortedPoint> discrete; + discrete.push_back((sortedPoint) {p0, 0., 1}); + discrete.push_back((sortedPoint) {p2, 1., -1}); decasteljau(tol, discrete, 0, p0, p1, p2, 0., 1); - discrete.sort(pts, ts); + sortedPointToVector(discrete, pts, ts); } -static void decasteljau(double tol, discreteList &discrete, int pos, const SPoint3 &p0, const SPoint3 &p1, const SPoint3 &p2, const SPoint3 &p3, double t0, double t3) +static void decasteljau(double tol, std::vector<sortedPoint> &discrete, int pos, const SPoint3 &p0, const SPoint3 &p1, const SPoint3 &p2, const SPoint3 &p3, double t0, double t3) { if (std::max(sqDistPointSegment(p1, p0, p3), sqDistPointSegment(p2, p0, p3)) < tol * tol) return; @@ -71,21 +72,21 @@ static void decasteljau(double tol, discreteList &discrete, int pos, const SPoin SPoint3 p123((p12 + p23) * 0.5); SPoint3 p0123((p012 + p123) * 0.5); double t0123 = 0.5 * (t0 + t3); - int newpos = discrete.insertPoint(pos, p0123, t0123); + int newpos = sortedPointInsert(p0123, t0123, discrete, pos); decasteljau(tol, discrete, pos, p0, p01, p012, p0123, t0, t0123); decasteljau(tol, discrete, newpos, p0123, p123, p23, p3, t0123, t3); } void decasteljau(double tol, const SPoint3 &p0, const SPoint3 &p1, const SPoint3 &p2, const SPoint3 &p3, std::vector<SPoint3> &pts, std::vector<double> &ts) { - discreteList discrete; - discrete.insertPoint(-1, p0, 0); - discrete.insertPoint(0, p3, 1); + std::vector<sortedPoint> discrete; + discrete.push_back((sortedPoint) {p0, 0., 1}); + discrete.push_back((sortedPoint) {p3, 1., -1}); decasteljau(tol, discrete, 0, p0, p1, p2, p3, 0., 1); - discrete.sort(pts, ts); + sortedPointToVector(discrete, pts, ts); } -static void decasteljau(double tol, discreteList &discrete, int pos, const std::vector<SPoint3> &pts, double t0, double te) +static void decasteljau(double tol, std::vector<sortedPoint> &discrete, int pos, const std::vector<SPoint3> &pts, double t0, double te) { int order = pts.size() - 1; double dmax2 = 0; @@ -101,16 +102,16 @@ static void decasteljau(double tol, discreteList &discrete, int pos, const std:: sub1[i] = (sub1[i] + sub1[i + 1]) * 0.5; } double tmid = 0.5 * (t0 + te); - int newpos = discrete.insertPoint(pos, sub1[0], tmid); + int newpos = sortedPointInsert(sub1[0], tmid, discrete, pos); decasteljau(tol, discrete, pos, sub0, t0, tmid); decasteljau(tol, discrete, newpos, sub1, tmid, te); } void decasteljau(double tol, const std::vector<SPoint3> &controlPoints, std::vector<SPoint3> &pts, std::vector<double> &ts) { - discreteList discrete; - discrete.insertPoint(-1, controlPoints[0], 0); - discrete.insertPoint(0, controlPoints.back(), 1); + std::vector<sortedPoint> discrete; + discrete.push_back((sortedPoint) {controlPoints[0], 0., 1}); + discrete.push_back((sortedPoint) {controlPoints.back(), 1., -1}); decasteljau(tol, discrete, 0, controlPoints, 0., 1); - discrete.sort(pts, ts); + sortedPointToVector(discrete, pts, ts); } diff --git a/Numeric/decasteljau.h b/Numeric/decasteljau.h index 7daa3251cf3007935319bf7a24532069857e8ef3..29afad4932a89450145a8fa34d2bb556a9784443 100644 --- a/Numeric/decasteljau.h +++ b/Numeric/decasteljau.h @@ -5,5 +5,4 @@ class SPoint3; void decasteljau(double tol, const SPoint3 &p0, const SPoint3 &p1, const SPoint3 &p2, std::vector<SPoint3> &pts, std::vector<double> &ts); void decasteljau(double tol, const SPoint3 &p0, const SPoint3 &p1, const SPoint3 &p2, const SPoint3 &p3, std::vector<SPoint3> &pts, std::vector<double> &ts); void decasteljau(double tol, const std::vector<SPoint3> &controlPoints, std::vector<SPoint3> &pts, std::vector<double> &ts); -double sqDistPointSegment(const SPoint3 &p, const SPoint3 &s0, const SPoint3 &s1); #endif