MinimalBoundingSphere.cpp

Go to the documentation of this file.

 
#include "MinimalBoundingSphere.h"
 
#include <Eigen/Geometry>
#include <ctime>
 
#include "MathLib/GeometricBasics.h"
#include "MathLib/MathTools.h"
#include "MathLib/Point3d.h"
 
namespace GeoLib
{
MinimalBoundingSphere::MinimalBoundingSphere() = default;
 
MinimalBoundingSphere::MinimalBoundingSphere(MathLib::Point3d const& p,
                                             double radius)
    : _radius(radius), _center(p)
{
}
 
MinimalBoundingSphere::MinimalBoundingSphere(MathLib::Point3d const& p,
                                             MathLib::Point3d const& q)
    : _radius(std::numeric_limits<double>::epsilon()), _center(p)
{
    Eigen::Vector3d const a = q.asEigenVector3d() - p.asEigenVector3d();
 
    Eigen::Vector3d o = a / 2;
    _radius = o.norm() + std::numeric_limits<double>::epsilon();
    o += p.asEigenVector3d();
    _center = MathLib::Point3d{{o[0], o[1], o[2]}};
}
 
MinimalBoundingSphere::MinimalBoundingSphere(MathLib::Point3d const& p,
                                             MathLib::Point3d const& q,
                                             MathLib::Point3d const& r)
{
    auto const& vp = p.asEigenVector3d();
    Eigen::Vector3d const a = r.asEigenVector3d() - vp;
    Eigen::Vector3d const b = q.asEigenVector3d() - vp;
    Eigen::Vector3d const axb = a.cross(b);
 
    if (axb.squaredNorm() > 0)
    {
        double const denom = 2.0 * axb.dot(axb);
        Eigen::Vector3d o =
            (b.dot(b) * axb.cross(a) + a.dot(a) * b.cross(axb)) / denom;
        _radius = o.norm() + std::numeric_limits<double>::epsilon();
        o += vp;
        _center = MathLib::Point3d{{o[0], o[1], o[2]}};
    }
    else
    {
        MinimalBoundingSphere two_pnts_sphere;
        if (a.squaredNorm() > b.squaredNorm())
        {
            two_pnts_sphere = MinimalBoundingSphere(p, r);
        }
        else
        {
            two_pnts_sphere = MinimalBoundingSphere(p, q);
        }
        _radius = two_pnts_sphere.getRadius();
        _center = two_pnts_sphere.getCenter();
    }
}
 
MinimalBoundingSphere::MinimalBoundingSphere(MathLib::Point3d const& p,
                                             MathLib::Point3d const& q,
                                             MathLib::Point3d const& r,
                                             MathLib::Point3d const& s)
{
    Eigen::Vector3d const va = q.asEigenVector3d() - p.asEigenVector3d();
    Eigen::Vector3d const vb = r.asEigenVector3d() - p.asEigenVector3d();
    Eigen::Vector3d const vc = s.asEigenVector3d() - p.asEigenVector3d();
 
    if (!MathLib::isCoplanar(p, q, r, s))
    {
        double const denom = 2.0 * va.cross(vb).dot(vc);
        Eigen::Vector3d o =
            (vc.dot(vc) * va.cross(vb) + vb.dot(vb) * vc.cross(va) +
             va.dot(va) * vb.cross(vc)) /
            denom;
 
        _radius = o.norm() + std::numeric_limits<double>::epsilon();
        o += p.asEigenVector3d();
        _center = MathLib::Point3d({o[0], o[1], o[2]});
    }
    else
    {
        MinimalBoundingSphere const pqr(p, q, r);
        MinimalBoundingSphere const pqs(p, q, s);
        MinimalBoundingSphere const prs(p, r, s);
        MinimalBoundingSphere const qrs(q, r, s);
        _radius = pqr.getRadius();
        _center = pqr.getCenter();
        if (_radius < pqs.getRadius())
        {
            _radius = pqs.getRadius();
            _center = pqs.getCenter();
        }
        if (_radius < prs.getRadius())
        {
            _radius = prs.getRadius();
            _center = prs.getCenter();
        }
        if (_radius < qrs.getRadius())
        {
            _radius = qrs.getRadius();
            _center = qrs.getCenter();
        }
    }
}
 
MinimalBoundingSphere::MinimalBoundingSphere(
    std::vector<MathLib::Point3d*> const& points)
    : _radius(-1), _center({0, 0, 0})
{
    const std::vector<MathLib::Point3d*>& sphere_points(points);
    MinimalBoundingSphere const bounding_sphere =
        recurseCalculation(sphere_points, 0, sphere_points.size(), 0);
    _center = bounding_sphere.getCenter();
    _radius = bounding_sphere.getRadius();
}
 
MinimalBoundingSphere MinimalBoundingSphere::recurseCalculation(
    std::vector<MathLib::Point3d*> sphere_points,
    std::size_t start_idx,
    std::size_t length,
    std::size_t n_boundary_points)
{
    MinimalBoundingSphere sphere;
    switch (n_boundary_points)
    {
        case 0:
            sphere = MinimalBoundingSphere();
            break;
        case 1:
            sphere = MinimalBoundingSphere(*sphere_points[start_idx - 1]);
            break;
        case 2:
            sphere = MinimalBoundingSphere(*sphere_points[start_idx - 1],
                                           *sphere_points[start_idx - 2]);
            break;
        case 3:
            sphere = MinimalBoundingSphere(*sphere_points[start_idx - 1],
                                           *sphere_points[start_idx - 2],
                                           *sphere_points[start_idx - 3]);
            break;
        case 4:
            sphere = MinimalBoundingSphere(
                *sphere_points[start_idx - 1], *sphere_points[start_idx - 2],
                *sphere_points[start_idx - 3], *sphere_points[start_idx - 4]);
            return sphere;
    }
 
    for (std::size_t i = 0; i < length; ++i)
    {
        // current point is located outside of sphere
        if (sphere.pointDistanceSquared(*sphere_points[start_idx + i]) > 0)
        {
            if (i > start_idx)
            {
                using DiffType =
                    std::vector<MathLib::Point3d*>::iterator::difference_type;
                std::vector<MathLib::Point3d*> const tmp_ps(
                    sphere_points.cbegin() + static_cast<DiffType>(start_idx),
                    sphere_points.cbegin() +
                        static_cast<DiffType>(start_idx + i + 1));
                std::copy(tmp_ps.cbegin(), --tmp_ps.cend(),
                          sphere_points.begin() +
                              static_cast<DiffType>(start_idx + 1));
                sphere_points[start_idx] = tmp_ps.back();
            }
            sphere = recurseCalculation(sphere_points, start_idx + 1, i,
                                        n_boundary_points + 1);
        }
    }
    return sphere;
}
 
double MinimalBoundingSphere::pointDistanceSquared(
    MathLib::Point3d const& pnt) const
{
    return MathLib::sqrDist(_center, pnt) - (_radius * _radius);
}
 
}  // namespace GeoLib