From e9d130f022316fd1fa94a41ec985c9f3d764b4fb Mon Sep 17 00:00:00 2001 From: Victor Olesen <Victor.Olesen@gmx.de> Date: Mon, 26 Oct 2020 14:11:34 +0100 Subject: [PATCH] ENH: support general searchable spheroid (issue #1901) - a sphere/spheroid can be specified as a single radius or three radii. If all three values happen to be identical, they are collapsed to a single value. Examples, radius 2; radius (2 2 2); radius (2 3 4); radius (2 2 4); The search for nearest point on an ellipse or ellipsoid follows the description given by Geometric Tools (David Eberly), which also include some pseudo code. The content is CC-BY 4.0 In the search algorithm, symmetry is exploited and the searching is confined to the first (+x,+y,+z) octant, and the radii are ordered from largest to smallest. Searching is optimized for sphere, prolate and oblate spheroids. --- .../searchableSphere/Test-searchableSphere.C | 20 + .../searchableSphere/searchableSphere.C | 644 +++++++++++++++++- .../searchableSphere/searchableSphere.H | 38 +- 3 files changed, 676 insertions(+), 26 deletions(-) diff --git a/applications/test/searchableSphere/Test-searchableSphere.C b/applications/test/searchableSphere/Test-searchableSphere.C index 5522aa5457e..ae76fd437f5 100644 --- a/applications/test/searchableSphere/Test-searchableSphere.C +++ b/applications/test/searchableSphere/Test-searchableSphere.C @@ -219,6 +219,26 @@ int main(int argc, char *argv[]) ) ); + doTest1 + ( + searchableSphere + ( + io, + point(0.5, 0.5, 0.5), + vector(1.999, 2, 2.001) + ) + ); + + doTest1 + ( + searchableSphere + ( + io, + point(0, 0, 0), + vector(3, 3, 4) + ) + ); + Info<< "\nDone\n" << endl; return 0; } diff --git a/src/meshTools/searchableSurfaces/searchableSphere/searchableSphere.C b/src/meshTools/searchableSurfaces/searchableSphere/searchableSphere.C index cdb9f0002d1..3ee39e7c582 100644 --- a/src/meshTools/searchableSurfaces/searchableSphere/searchableSphere.C +++ b/src/meshTools/searchableSurfaces/searchableSphere/searchableSphere.C @@ -24,6 +24,17 @@ License You should have received a copy of the GNU General Public License along with OpenFOAM. If not, see <http://www.gnu.org/licenses/>. +Description + The search for nearest point on an ellipse or ellipsoid follows the + description given by Geometric Tools (David Eberly), which also + include some pseudo code. The content is CC-BY 4.0 + +https://www.geometrictools.com/Documentation/DistancePointEllipseEllipsoid.pdf + + In the search algorithm, symmetry is exploited and the searching is + confined to the first (+x,+y,+z) octant, and the radii are ordered + from largest to smallest. + \*---------------------------------------------------------------------------*/ #include "searchableSphere.H" @@ -89,6 +100,410 @@ inline static void applyOctant(point& p, unsigned octant) if (octant & 4) { p.z() = -p.z(); } } + +// Vector magnitudes + +inline static scalar vectorMagSqr +( + const scalar x, + const scalar y +) +{ + return (sqr(x) + sqr(y)); +} + +inline static scalar vectorMagSqr +( + const scalar x, + const scalar y, + const scalar z +) +{ + return (sqr(x) + sqr(y) + sqr(z)); +} + +inline static scalar vectorMag +( + const scalar x, + const scalar y +) +{ + return hypot(x, y); +} + +inline static scalar vectorMag +( + const scalar x, + const scalar y, + const scalar z +) +{ + return ::sqrt(vectorMagSqr(x, y, z)); +} + + +} // End namespace Foam + + +// * * * * * * * * * * * * * * * Local Functions * * * * * * * * * * * * * * // + +// Searching +namespace Foam +{ + +// Max iterations for root finding +static constexpr int maxIters = 100; + +// Relative ellipse size within the root finding (1) +static constexpr scalar tolCloseness = 1e-3; + + +// Find root for distance to ellipse +static scalar findRootEllipseDistance +( + const scalar r0, //!< Ratio of major/minor + const scalar z0, //!< Search point y0, scaled by e0 (major) + const scalar z1, //!< Search point y1, scaled by e1 (minor) + scalar g //!< Evaluated ellipse, implicit form +) +{ + const scalar n0 = r0*z0; + + scalar s0 = z1 - 1; + scalar s1 = (g < 0 ? 0 : vectorMag(n0, z1) - 1); + scalar s = 0; + + int nIters = 0; + while (nIters++ < maxIters) + { + s = (s0 + s1) / 2; + if (equal(s, s0) || equal(s, s1)) + { + break; + } + + g = sqr(n0/(s+r0)) + sqr(z1/(s+1)) - 1; + + if (mag(g) < tolCloseness) + { + break; + } + else if (g > 0) + { + s0 = s; + } + else // g < 0 + { + s1 = s; + } + } + + #ifdef FULLDEBUG + InfoInFunction + << "Located root in " << nIters << " iterations" << endl; + #endif + + return s; +} + + +// Find root for distance to ellipsoid +static scalar findRootEllipsoidDistance +( + const scalar r0, //!< Ratio of major/minor + const scalar r1, //!< Ratio of mezzo/minor + const scalar z0, //!< Search point y0, scaled by e0 (major) + const scalar z1, //!< Search point y1, scaled by e1 (mezzo) + const scalar z2, //!< Search point y2, scaled by e2 (minor) + scalar g //!< Evaluated ellipsoid, implicit form +) +{ + const scalar n0 = r0*z0; + const scalar n1 = r1*z1; + + scalar s0 = z2 - 1; + scalar s1 = (g < 0 ? 0 : vectorMag(n0, n1, z2) - 1); + scalar s = 0; + + int nIters = 0; + while (nIters++ < maxIters) + { + s = (s0 + s1) / 2; + if (equal(s, s0) || equal(s, s1)) + { + break; + } + + g = vectorMagSqr(n0/(s+r0), n1/(s+r1), z2/(s+1)) - 1; + + if (mag(g) < tolCloseness) + { + break; + } + else if (g > 0) + { + s0 = s; + } + else // g < 0 + { + s1 = s; + } + } + + #ifdef FULLDEBUG + InfoInFunction + << "root at " << s << " found in " << nIters + << " iterations" << endl; + #endif + + return s; +} + + +// Distance (squared) to an ellipse (2D) +static scalar distanceToEllipse +( + // [in] Ellipse characteristics. e0 >= e1 + const scalar e0, const scalar e1, + // [in] search point. y0 >= 0, y1 >= 0 + const scalar y0, const scalar y1, + // [out] nearest point on ellipse + scalar& x0, scalar& x1 +) +{ + if (equal(y1, 0)) + { + // On the y1 = 0 axis + + const scalar numer0 = e0*y0; + const scalar denom0 = sqr(e0) - sqr(e1); + + if (numer0 < denom0) + { + const scalar xde0 = numer0/denom0; // Is always < 1 + + x0 = e0*xde0; + x1 = e1*sqrt(1 - sqr(xde0)); + + return vectorMagSqr((x0-y0), x1); + } + + // Fallthrough + x0 = e0; + x1 = 0; + + return sqr(y0-e0); + } + else if (equal(y0, 0)) + { + // On the y0 = 0 axis, in the y1 > 0 half + x0 = 0; + x1 = e1; + return sqr(y1-e1); + } + else + { + // In the y0, y1 > 0 quadrant + + const scalar z0 = y0 / e0; + const scalar z1 = y1 / e1; + scalar eval = sqr(z0) + sqr(z1); + + scalar g = eval - 1; + + if (mag(g) < tolCloseness) + { + x0 = y0; + x1 = y1; + + if (!equal(eval, 1)) + { + // Very close, scale accordingly. + eval = sqrt(eval); + x0 /= eval; + x1 /= eval; + } + + return sqr(x0-y0) + sqr(x1-y1); + } + + + // General search. + // Uses root find to get tbar of F(t) on (-e1^2,+inf) + + // Ratio major/minor + const scalar r0 = sqr(e0 / e1); + + const scalar sbar = + findRootEllipseDistance(r0, z0, z1, g); + + x0 = r0 * y0 / (sbar + r0); + x1 = y1 / (sbar + 1); + + // Re-evaluate + eval = sqr(x0/e0) + sqr(x1/e1); + + if (!equal(eval, 1)) + { + // Very close, scale accordingly. + // + // This is not exact - the point is projected at a + // slight angle, but we are only correcting for + // rounding in the first place. + + eval = sqrt(eval); + x0 /= eval; + x1 /= eval; + } + + return sqr(x0-y0) + sqr(x1-y1); + } + + // Code never reaches here + FatalErrorInFunction + << "Programming/logic error" << nl + << exit(FatalError); + + return 0; +} + + +// Distance (squared) to an ellipsoid (3D) +static scalar distanceToEllipsoid +( + // [in] Ellipsoid characteristics. e0 >= e1 >= e2 + const scalar e0, const scalar e1, const scalar e2, + // [in] search point. y0 >= 0, y1 >= 0, y2 >= 0 + const scalar y0, const scalar y1, const scalar y2, + // [out] nearest point on ellipsoid + scalar& x0, scalar& x1, scalar& x2 +) +{ + if (equal(y2, 0)) + { + // On the y2 = 0 plane. Can use 2D ellipse finding + + const scalar numer0 = e0*y0; + const scalar numer1 = e1*y1; + const scalar denom0 = sqr(e0) - sqr(e2); + const scalar denom1 = sqr(e1) - sqr(e2); + + if (numer0 < denom0 && numer1 < denom1) + { + const scalar xde0 = numer0/denom0; // Is always < 1 + const scalar xde1 = numer1/denom1; // Is always < 1 + + const scalar disc = (1 - sqr(xde0) - sqr(xde1)); + + if (disc > 0) + { + x0 = e0*xde0; + x1 = e1*xde1; + x2 = e2*sqrt(disc); + + return vectorMagSqr((x0-y0), (x1-y1), x2); + } + } + + // Fallthrough - use 2D form + x2 = 0; + return distanceToEllipse(e0,e1, y0,y1, x0,x1); + } + else if (equal(y1, 0)) + { + // On the y1 = 0 plane, in the y2 > 0 half + x1 = 0; + if (equal(y0, 0)) + { + x0 = 0; + x2 = e2; + return sqr(y2-e2); + } + else // y0 > 0 + { + return distanceToEllipse(e0,e2, y0,y2, x0,x2); + } + } + else if (equal(y0, 0)) + { + // On the y1 = 0 plane, in the y1, y2 > 0 quadrant + x0 = 0; + return distanceToEllipse(e1,e2, y1,y2, x1,x2); + } + else + { + // In the y0, y1, y2 > 0 octant + + const scalar z0 = y0/e0; + const scalar z1 = y1/e1; + const scalar z2 = y2/e2; + scalar eval = vectorMagSqr(z0, z1, z2); + + scalar g = eval - 1; + + if (mag(g) < tolCloseness) + { + x0 = y0; + x1 = y1; + x2 = y2; + + if (equal(eval, 1)) + { + // Exactly on the ellipsoid - we are done + return 0; + } + + // Very close, scale accordingly. + eval = sqrt(eval); + x0 /= eval; + x1 /= eval; + x2 /= eval; + + return vectorMagSqr((x0-y0), (x1-y1), (x2-y2)); + } + + + // General search. + // Compute the unique root tbar of F(t) on (-e2^2,+inf) + + const scalar r0 = sqr(e0/e2); + const scalar r1 = sqr(e1/e2); + + const scalar sbar = + findRootEllipsoidDistance(r0,r1, z0,z1,z2, g); + + x0 = r0*y0/(sbar+r0); + x1 = r1*y1/(sbar+r1); + x2 = y2/(sbar+1); + + // Reevaluate + eval = vectorMagSqr((x0/e0), (x1/e1), (x2/e2)); + + if (!equal(eval, 1)) + { + // Not exactly on ellipsoid? + // + // Scale accordingly. This is not exact - the point + // is projected at a slight angle, but we are only + // correcting for rounding in the first place. + + eval = sqrt(eval); + x0 /= eval; + x1 /= eval; + x2 /= eval; + } + + return vectorMagSqr((x0-y0), (x1-y1), (x2-y2)); + } + + // Code never reaches here + FatalErrorInFunction + << "Programming/logic error" << nl + << exit(FatalError); + + return 0; +} + } // End namespace Foam @@ -151,8 +566,7 @@ Foam::pointIndexHit Foam::searchableSphere::findNearest // Handle special cases first - // if (order_.shape == shapeType::SPHERE) - if (true) + if (order_.shape == shapeType::SPHERE) { // Point relative to origin, simultaneously the normal on the sphere const vector n(sample - origin_); @@ -174,7 +588,95 @@ Foam::pointIndexHit Foam::searchableSphere::findNearest return info; } - //[code] + + // + // Non-sphere + // + + // Local point relative to the origin + vector relPt(sample - origin_); + + // Detect -ve octants + const unsigned octant = getOctant(relPt); + + // Flip everything into positive octant. + // That is what the algorithm expects. + applyOctant(relPt, octant); + + + // TODO - quick reject for things that are too far away + + point& near = info.rawPoint(); + scalar distSqr{0}; + + if (order_.shape == shapeType::OBLATE) + { + // Oblate (major = mezzo > minor) - use 2D algorithm + // Distance from the minor axis to relPt + const scalar axisDist = hypot(relPt[order_.major], relPt[order_.mezzo]); + + // Distance from the minor axis to near + scalar nearAxis; + + distSqr = distanceToEllipse + ( + radii_[order_.major], radii_[order_.minor], + axisDist, relPt[order_.minor], + nearAxis, near[order_.minor] + ); + + // Now nearAxis is the ratio, by which their components have changed + nearAxis /= (axisDist + VSMALL); + + near[order_.major] = relPt[order_.major] * nearAxis; + near[order_.mezzo] = relPt[order_.mezzo] * nearAxis; + // near[order_.minor] = already calculated + } + else if (order_.shape == shapeType::PROLATE) + { + // Prolate (major > mezzo = minor) - use 2D algorithm + // Distance from the major axis to relPt + const scalar axisDist = hypot(relPt[order_.mezzo], relPt[order_.minor]); + + // Distance from the major axis to near + scalar nearAxis; + + distSqr = distanceToEllipse + ( + radii_[order_.major], radii_[order_.minor], + relPt[order_.major], axisDist, + near[order_.major], nearAxis + ); + + // Now nearAxis is the ratio, by which their components have changed + nearAxis /= (axisDist + VSMALL); + + // near[order_.major] = already calculated + near[order_.mezzo] = relPt[order_.mezzo] * nearAxis; + near[order_.minor] = relPt[order_.minor] * nearAxis; + } + else // General case + { + distSqr = distanceToEllipsoid + ( + radii_[order_.major], radii_[order_.mezzo], radii_[order_.minor], + relPt[order_.major], relPt[order_.mezzo], relPt[order_.minor], + near[order_.major], near[order_.mezzo], near[order_.minor] + ); + } + + // Flip everything back to original octant + applyOctant(near, octant); + + // From local to global + near += origin_; + + + // Accept/reject based on distance + if (distSqr <= nearestDistSqr) + { + info.setHit(); + } return info; } @@ -192,8 +694,7 @@ void Foam::searchableSphere::findLineAll near.setMiss(); far.setMiss(); - // if (order_.shape == shapeType::SPHERE) - if (true) + if (order_.shape == shapeType::SPHERE) { vector dir(end-start); const scalar magSqrDir = magSqr(dir); @@ -217,22 +718,59 @@ void Foam::searchableSphere::findLineAll if (nearParam >= 0 && sqr(nearParam) <= magSqrDir) { - near.setHit(); - near.setPoint(start + nearParam*dir); - near.setIndex(0); + near.hitPoint(start + nearParam*dir, 0); } if (farParam >= 0 && sqr(farParam) <= magSqrDir) { - far.setHit(); - far.setPoint(start + farParam*dir); - far.setIndex(0); + far.hitPoint(start + farParam*dir, 0); } } } + + return; } - //[code] + + // General case + + // Similar to intersection of sphere with ray (Graphics Gems), + // but we scale x/y/z components according to radii + // to have a unit spheroid for the interactions. + // When finished, we unscale to get the real points + + // Note - can also be used for the spherical case + + const point relStart = scalePoint(start); + + vector dir(scalePoint(end) - relStart); + const scalar magSqrDir = magSqr(dir); + + if (magSqrDir > ROOTVSMALL) + { + dir /= Foam::sqrt(magSqrDir); + + const scalar v = -(relStart & dir); + + const scalar disc = scalar(1) - (magSqr(relStart) - sqr(v)); + + if (disc >= 0) + { + const scalar d = Foam::sqrt(disc); + + const scalar nearParam = v - d; + const scalar farParam = v + d; + + if (nearParam >= 0 && sqr(nearParam) <= magSqrDir) + { + near.hitPoint(unscalePoint(relStart + nearParam*dir), 0); + } + if (farParam >= 0 && sqr(farParam) <= magSqrDir) + { + far.hitPoint(unscalePoint(relStart + farParam*dir), 0); + } + } + } } @@ -258,8 +796,7 @@ Foam::searchableSphere::searchableSphere : searchableSurface(io), origin_(origin), - // radii_(radii), - radii_(vector::uniform(cmptMax(radii))) /* Transition */, + radii_(radii), order_{getOrdering(radii_)} { bounds().min() = (centre() - radii_); @@ -318,8 +855,7 @@ Foam::vector Foam::searchableSphere::surfaceNormal bool Foam::searchableSphere::overlaps(const boundBox& bb) const { - // if (order_.shape == shapeType::SPHERE) - if (true) + if (order_.shape == shapeType::SPHERE) { return bb.overlaps(origin_, sqr(radius())); } @@ -329,7 +865,41 @@ bool Foam::searchableSphere::overlaps(const boundBox& bb) const return false; } - //[code] + // Code largely as per + // boundBox::overlaps(const point& centre, const scalar radiusSqr) + // but normalized for a unit size + + // Find out where centre is in relation to bb. + // Find nearest point on bb. + + // Note: no major advantage in treating sphere specially + + scalar distSqr = 0; + for (direction dir = 0; dir < vector::nComponents; ++dir) + { + const scalar d0 = bb.min()[dir] - origin_[dir]; + const scalar d1 = bb.max()[dir] - origin_[dir]; + + if ((d0 > 0) == (d1 > 0)) + { + // Both min/max are on the same side of the origin + // ie, box does not span spheroid in this direction + + if (Foam::mag(d0) < Foam::mag(d1)) + { + distSqr += Foam::sqr(d0/radii_[dir]); + } + else + { + distSqr += Foam::sqr(d1/radii_[dir]); + } + + if (distSqr > 1) + { + return false; + } + } + } return true; } @@ -499,7 +1069,23 @@ void Foam::searchableSphere::getNormal { if (info[i].hit()) { - normal[i] = normalised(info[i].point() - origin_); + if (order_.shape == shapeType::SPHERE) + { + // Special case (sphere) + normal[i] = normalised(info[i].hitPoint() - origin_); + } + else + { + // General case + // Normal is (x0/r0^2, x1/r1^2, x2/r2^2) + + normal[i] = scalePoint(info[i].hitPoint()); + + normal[i].x() /= radii_.x(); + normal[i].y() /= radii_.y(); + normal[i].z() /= radii_.z(); + normal[i].normalise(); + } } else { @@ -518,9 +1104,10 @@ void Foam::searchableSphere::getVolumeType { volType.resize(points.size()); - // if (order_.shape == shapeType::SPHERE) - if (true) + if (order_.shape == shapeType::SPHERE) { + // Special case. Minor advantage in treating specially + const scalar rad2 = sqr(radius()); forAll(points, pointi) @@ -533,9 +1120,24 @@ void Foam::searchableSphere::getVolumeType ? volumeType::INSIDE : volumeType::OUTSIDE ); } + + return; } - //[code] + // General case - could also do component-wise (manually) + // Evaluate: (x/r0)^2 + (y/r1)^2 + (z/r2)^2 - 1 = 0 + // [sphere]: x^2 + y^2 + z^2 - R^2 = 0 + + forAll(points, pointi) + { + const point p = scalePoint(points[pointi]); + + volType[pointi] = + ( + (magSqr(p) <= 1) + ? volumeType::INSIDE : volumeType::OUTSIDE + ); + } } diff --git a/src/meshTools/searchableSurfaces/searchableSphere/searchableSphere.H b/src/meshTools/searchableSurfaces/searchableSphere/searchableSphere.H index c630a8c4e2c..febf3c4dd23 100644 --- a/src/meshTools/searchableSurfaces/searchableSphere/searchableSphere.H +++ b/src/meshTools/searchableSurfaces/searchableSphere/searchableSphere.H @@ -28,18 +28,21 @@ Class Foam::searchableSphere Description - Searching on sphere + Searching on general spheroid. \heading Dictionary parameters \table Property | Description | Required | Default type | sphere | selector | origin | The origin (centre) of the sphere | yes | - radius | The (outside) radius of sphere | yes | + radius | The (outside) radius/radiii of sphere | yes | centre | Alternative name for 'origin' | no | \endtable Note + The \c radius can be specified as a single \em scalar (for a sphere) + or a \em vector of three values (for a general spheroid). + Longer type name : \c searchableSphere SourceFiles @@ -114,17 +117,42 @@ private: //- Determine sorted order and classify the shape inline static componentOrder getOrdering(const vector& radii); + //- Shift point relative to origin + //- and scale relative to spheroid dimensions + inline point scalePoint(const point& p) const + { + return point + ( + (p.x() - origin_.x()) / radii_.x(), + (p.y() - origin_.y()) / radii_.y(), + (p.z() - origin_.z()) / radii_.z() + ); + } + + //- Undo scalePoint(): unscale point and unshift relative to origin + inline point unscalePoint(const point& p) const + { + return point + ( + p.x() * radii_.x() + origin_.x(), + p.y() * radii_.y() + origin_.y(), + p.z() * radii_.z() + origin_.z() + ); + } + + //- Inherit findNearest from searchableSurface using searchableSurface::findNearest; - //- Find nearest point on sphere. + //- Find nearest point on general spheroid. + // With some optimization for special shapes pointIndexHit findNearest ( const point& sample, const scalar nearestDistSqr ) const; - //- Find intersection with sphere + //- Find intersection with general spheroid void findLineAll ( const point& start, @@ -202,7 +230,7 @@ public: //- The type of shape enum shapeType shape() const noexcept { - return shapeType::SPHERE; + return order_.shape; } //- A point on the sphere at given location -- GitLab