BoundingSphere.h

Go to the documentation of this file.

/*  Copyright (C) 2003-2021 Free Electron Organization
    Any use of this software requires a license.  If a valid license
    was not distributed with this file, visit freeelectron.org. */

/** @file */

#ifndef __geometry_BoundingSphere_h__
#define __geometry_BoundingSphere_h__

#define FE_BOS_DEBUG            FALSE

#define FE_BOS_MULTIPLICITIVE   FALSE

namespace fe
{
namespace ext
{

const Real boundEpsilon=1e-3;
#if FE_BOS_MULTIPLICITIVE
const Real boundFactor=1.001;
#endif

/**************************************************************************//**
    @brief Bounding sphere for arbitrary points

    @ingroup geometry

    Emo Welzl's algorithm 1991

    http://www.flipcode.com/archives/Smallest_Enclosing_Spheres.shtml
*//***************************************************************************/
template <typename T>
class BoundingSphere
{
    public:
static  void    solve(const Vector<3,T>* a_pEnclosed,U32 a_enclosedCount,
                        Vector<3,T>& a_rCenter,T& a_rRadius);

                /** @brief create a sphere containing 0 to 4 points

                    If a_boundaryCount is zero or exceeds 4,
                    a zero sphere at the origin is generated. */
static  void    solveBoundary(const Vector<3,T>** a_ppBoundary,
                        U32 a_boundaryCount,
                        Vector<3,T>& a_rCenter,T& a_rRadius);

                /** @brief create a sphere containing 0 to 4 points

                    Checks for problematic redundant points,
                    then calls solveBoundary(). */
static  void    solveBoundarySafe(const Vector<3,T>** a_ppBoundary,
                        U32 a_boundaryCount,
                        Vector<3,T>& a_rCenter,T& a_rRadius);

    private:
static  void    solveRecursive(const Vector<3,T>** a_ppPoints,
                        U32 a_enclosedCount,U32 a_boundaryCount,
                        Vector<3,T>& a_rCenter,T& a_rRadius);
};

template <typename T>
inline void BoundingSphere<T>::solve(
        const Vector<3,T>* a_pEnclosed,U32 a_enclosedCount,
        Vector<3,T>& a_rCenter,T& a_rRadius)
{
    const Vector<3,T>** ppPoints=new const Vector<3,T>*[a_enclosedCount];

    for(U32 i=0;i<a_enclosedCount;i++)
        ppPoints[i]=&a_pEnclosed[i];

    solveRecursive(ppPoints,a_enclosedCount,0,a_rCenter,a_rRadius);

    delete[] ppPoints;
}

template <typename T>
inline void BoundingSphere<T>::solveRecursive(
        const Vector<3,T>** a_ppPoints,
        U32 a_enclosedCount,U32 a_boundaryCount,
        Vector<3,T>& a_rCenter, T& a_rRadius)
{
//  if(a_enclosedCount>10000)
//  {
//      feLog("BoundingSphere<T>::solveRecursive"
//              " env %d bound %d center %s radius %.6G\n",
//              a_enclosedCount,a_boundaryCount,c_print(a_rCenter),a_rRadius);
//  }

#if FE_BOS_DEBUG
    feLog("solveRecursive %p %d %d\n",
            a_ppPoints,a_enclosedCount,a_boundaryCount);
    for(I32 m= -a_boundaryCount;m<0;m++)
    {
        feLog(" b %d %p (%s)\n",m,a_ppPoints[m],c_print(*a_ppPoints[m]));
    }
    for(U32 m=0;m<a_enclosedCount;m++)
    {
        feLog(" e %d %p (%s)\n",m,a_ppPoints[m],c_print(*a_ppPoints[m]));
    }
#endif

    solveBoundarySafe(a_ppPoints-a_boundaryCount,a_boundaryCount,
            a_rCenter,a_rRadius);
    if(a_boundaryCount==4)
    {
        return;
    }

#if FE_BOS_MULTIPLICITIVE
    T radiusPlus=fe::maximum(a_rRadius+boundEpsilon,a_rRadius*boundFactor);
#else
    T radiusPlus=a_rRadius+boundEpsilon;
#endif

    T r2=radiusPlus*radiusPlus;

    for(U32 i=0;i<a_enclosedCount;i++)
    {
#if FE_BOS_DEBUG
        feLog("check %d %p (%s)\n",
                i,a_ppPoints[i],c_print(*a_ppPoints[i]));
#endif
        if(magnitudeSquared(*a_ppPoints[i]-a_rCenter)>r2)
        {
#if FE_BOS_DEBUG
            feLog("fit (%s) - (%s) %.6G vs %.6G\n",
                    c_print(*a_ppPoints[i]),c_print(a_rCenter),
                    magnitudeSquared(*a_ppPoints[i]-a_rCenter),r2);
#endif
            for(U32 j=i;j>0;j--)
            {
#if FE_BOS_DEBUG
                feLog("sort %d/%d %p %p (%s) (%s)\n",
                        j,i,a_ppPoints[j-1],a_ppPoints[j],
                        c_print(*a_ppPoints[j-1]),
                        c_print(*a_ppPoints[j]));
#endif
                const Vector<3,T>* point=a_ppPoints[j];
                a_ppPoints[j]=a_ppPoints[j-1];
                a_ppPoints[j-1]=point;

            }

            solveRecursive(a_ppPoints+1,i,a_boundaryCount+1,
                    a_rCenter,a_rRadius);

#if FE_BOS_MULTIPLICITIVE
            radiusPlus=fe::maximum(a_rRadius+boundEpsilon,
                    a_rRadius*boundFactor);
#else
            radiusPlus=a_rRadius+boundEpsilon;
#endif

            r2=radiusPlus*radiusPlus;
        }
    }
}

template <typename T>
inline void BoundingSphere<T>::solveBoundarySafe(
        const Vector<3,T>** a_ppBoundary, U32 a_boundaryCount,
        Vector<3,T>& a_rCenter, T& a_rRadius)
{
#if FE_BOS_MULTIPLICITIVE
    const Real threshold=fe::maximum(boundEpsilon,
            a_rRadius*(Real(1)-boundFactor));
#else
    const Real threshold=boundEpsilon;
#endif

    for(U32 m=0;m<a_boundaryCount;m++)
    {
        for(U32 n=m+1;n<a_boundaryCount;n++)
        {
            if(magnitudeSquared(*a_ppBoundary[n]-*a_ppBoundary[m])<threshold)
            {
                if(n<a_boundaryCount-1)
                {
                    const Vector<3,T>* point=a_ppBoundary[n];
                    a_ppBoundary[n]=a_ppBoundary[a_boundaryCount-1];
                    a_ppBoundary[a_boundaryCount-1]=point;
                }
                a_boundaryCount--;
            }
        }
    }
    solveBoundary(a_ppBoundary,a_boundaryCount,a_rCenter,a_rRadius);
}

template <typename T>
inline void BoundingSphere<T>::solveBoundary(
        const Vector<3,T>** a_ppBoundary, U32 a_boundaryCount,
        Vector<3,T>& a_rCenter, T& a_rRadius)
{
#if FE_BOS_DEBUG
    feLog("solveBoundary %p %d\n",a_ppBoundary,a_boundaryCount);
    for(U32 m=0;m<a_boundaryCount;m++)
    {
        feLog(" %d %p (%s)\n",m,a_ppBoundary[m],c_print(*a_ppBoundary[m]));
    }
#endif
    switch(a_boundaryCount)
    {
        case 4:
        {
            const Vector<3,T> edge1=*a_ppBoundary[1]-*a_ppBoundary[0];
            const Vector<3,T> edge2=*a_ppBoundary[2]-*a_ppBoundary[0];
            const Vector<3,T> edge3=*a_ppBoundary[3]-*a_ppBoundary[0];
            const T denom=T(2)*determinant3x3(
                    edge1[0],edge1[1],edge1[2],
                    edge2[0],edge2[1],edge2[2],
                    edge3[0],edge3[1],edge3[2]);
            if(denom!=0.0)
            {
                const Vector<3,T> to=(dot(edge3,edge3)*cross(edge1,edge2)+
                        dot(edge2,edge2)*cross(edge3,edge1)+
                        dot(edge1,edge1)*cross(edge2,edge3))/denom;

#if FE_BOS_MULTIPLICITIVE
                a_rRadius=fe::maximum(magnitude(to)+boundEpsilon,
                        magnitude(to)*boundFactor);
#else
                a_rRadius=magnitude(to)+boundEpsilon;
#endif

                a_rCenter=*a_ppBoundary[0]+to;
                break;
            }
            //* else drop to case 3
#if FE_CPLUSPLUS >= 201703L
            [[fallthrough]];
#endif
        }
        case 3:
        {
            const Vector<3,T> edge1=*a_ppBoundary[1]-*a_ppBoundary[0];
            const Vector<3,T> edge2=*a_ppBoundary[2]-*a_ppBoundary[0];
            const Vector<3,T> perp=cross(edge1,edge2);
            const T denom=T(2)*dot(perp,perp);
            if(denom!=0.0)
            {
                const Vector<3,T> to=(dot(edge2,edge2)*cross(perp,edge1)+
                        dot(edge1,edge1)*cross(edge2,perp))/denom;

#if FE_BOS_MULTIPLICITIVE
                a_rRadius=fe::maximum(magnitude(to)+boundEpsilon,
                        magnitude(to)*boundFactor);
#else
                a_rRadius=magnitude(to)+boundEpsilon;
#endif

                a_rCenter=*a_ppBoundary[0]+to;
                break;
            }
            //* else drop to case 2
#if FE_CPLUSPLUS >= 201703L
            [[fallthrough]];
#endif
        }
        case 2:
        {
            Vector<3,T> diff=*a_ppBoundary[1]-*a_ppBoundary[0];
            Vector<3,T> half=T(0.5)*diff;

#if FE_BOS_MULTIPLICITIVE
            a_rRadius=fe::maximum(magnitude(half)+boundEpsilon,
                    magnitude(half)*boundFactor);
#else
            a_rRadius=magnitude(half)+boundEpsilon;
#endif

            a_rCenter=*a_ppBoundary[0]+half;
        }
            break;
        case 1:
            a_rCenter=*a_ppBoundary[0];
            a_rRadius=T(0);
            break;
        default:
            set(a_rCenter);
            a_rRadius=T(0);
            break;
    }
#if FE_BOS_DEBUG
    feLog(" center (%s) radius %.6G\n",c_print(a_rCenter),a_rRadius);
#endif
}

} /* namespace ext */
} /* namespace fe */

#endif /* __geometry_BoundingSphere_h__ */