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.

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;
 }

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 (order_.shape == shapeType::SPHERE)
-    if (true)
     {
         // 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 (order_.shape == shapeType::SPHERE)
-    if (true)
     {
         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.hitPoint(start + nearParam*dir, 0);
-                    near.setPoint(start + nearParam*dir);
-                    near.setIndex(0);
                 }
 
                 if (farParam >= 0 && sqr(farParam) <= magSqrDir)
                 {
-                    far.setHit();
+                    far.hitPoint(start + farParam*dir, 0);
-                    far.setPoint(start + farParam*dir);
-                    far.setIndex(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_(radii),
-    radii_(vector::uniform(cmptMax(radii))) /* Transition */,
     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 (order_.shape == shapeType::SPHERE)
-    if (true)
     {
         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 (order_.shape == shapeType::SPHERE)
-    if (true)
     {
+        // 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
+        );
+    }
 }
 
 

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