Revision bc8203fe68760fd81e7bf85b4163080ee2bebbcd authored by Wenqing Wang on 15 April 2020, 10:09:05 UTC, committed by Wenqing Wang on 16 February 2021, 12:16:27 UTC
1 parent 8fe86de
MinimalBoundingSphere.cpp
/**
* \file
* \author Karsten Rink
* \date 2014-07-11
* \brief Calculation of a minimum bounding sphere for a vector of points.
*
* \copyright
* Copyright (c) 2012-2021, OpenGeoSys Community (http://www.opengeosys.org)
* Distributed under a Modified BSD License.
* See accompanying file LICENSE.txt or
* http://www.opengeosys.org/project/license
*
*/
#include "MinimalBoundingSphere.h"
#include <ctime>
#include "MathLib/Point3d.h"
#include "MathLib/GeometricBasics.h"
#include "MathLib/MathTools.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)
{
auto const vp = Eigen::Map<Eigen::Vector3d const>(p.getCoords());
auto const vq = Eigen::Map<Eigen::Vector3d const>(q.getCoords());
Eigen::Vector3d const a = vq - vp;
Eigen::Vector3d o = a / 2;
_radius = o.norm() + std::numeric_limits<double>::epsilon();
o += vp;
_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 = Eigen::Map<Eigen::Vector3d const>(p.getCoords());
auto const vq = Eigen::Map<Eigen::Vector3d const>(q.getCoords());
auto const vr = Eigen::Map<Eigen::Vector3d const>(r.getCoords());
Eigen::Vector3d const a = vr - vp;
Eigen::Vector3d const b = vq - 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)
{
auto const vp = Eigen::Map<Eigen::Vector3d const>(p.getCoords());
auto const vq = Eigen::Map<Eigen::Vector3d const>(q.getCoords());
auto const vr = Eigen::Map<Eigen::Vector3d const>(r.getCoords());
auto const vs = Eigen::Map<Eigen::Vector3d const>(s.getCoords());
Eigen::Vector3d const va = vq - vp;
Eigen::Vector3d const vb = vr - vp;
Eigen::Vector3d const vc = vs - vp;
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 += vp;
_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
![swh spinner](/static/img/swh-spinner.gif)
Computing file changes ...