Revision 5284175198d16fcfb4ba114587da7a763f389a2d authored by Thomas Fischer on 06 July 2021, 11:56:44 UTC, committed by Thomas Fischer on 08 July 2021, 18:30:22 UTC
1 parent 9431373
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/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)
{
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 ...