|
2 | 2 | #define __BOUNDINGSPHERE_H__
|
3 | 3 |
|
4 | 4 | #include "Common/Common.h"
|
| 5 | +#include <vector> |
5 | 6 |
|
6 | 7 | namespace PBD
|
7 | 8 | {
|
8 |
| - |
| 9 | + /** |
| 10 | + * \brief Computes smallest enclosing spheres of pointsets using Welzl's algorithm |
| 11 | + * \Author: Tassilo Kugelstadt |
| 12 | + */ |
9 | 13 | class BoundingSphere
|
10 | 14 | {
|
11 | 15 | public:
|
| 16 | + /** |
| 17 | + * \brief default constructor sets the center and radius to zero. |
| 18 | + */ |
| 19 | + BoundingSphere() : m_x(Vector3r::Zero()), m_r(0.0) {} |
| 20 | + |
| 21 | + /** |
| 22 | + * \brief constructor which sets the center and radius |
| 23 | + * |
| 24 | + * \param x 3d coordiantes of the center point |
| 25 | + * \param r radius of the sphere |
| 26 | + */ |
| 27 | + BoundingSphere(Vector3r const& x, const Real r) : m_x(x), m_r(r) {} |
| 28 | + |
| 29 | + /** |
| 30 | + * \brief constructs a sphere for one point (with radius 0) |
| 31 | + * |
| 32 | + * \param a 3d coordinates of point a |
| 33 | + */ |
| 34 | + BoundingSphere(const Vector3r& a) |
| 35 | + { |
| 36 | + m_x = a; |
| 37 | + m_r = 0.0; |
| 38 | + } |
| 39 | + |
| 40 | + /** |
| 41 | + * \brief constructs the smallest enclosing sphere for two points |
| 42 | + * |
| 43 | + * \param a 3d coordinates of point a |
| 44 | + * \param b 3d coordinates of point b |
| 45 | + */ |
| 46 | + BoundingSphere(const Vector3r& a, const Vector3r& b) |
| 47 | + { |
| 48 | + const Vector3r ba = b - a; |
| 49 | + |
| 50 | + m_x = (a + b) * 0.5; |
| 51 | + m_r = 0.5 * ba.norm(); |
| 52 | + } |
| 53 | + |
| 54 | + /** |
| 55 | + * \brief constructs the smallest enclosing sphere for three points |
| 56 | + * |
| 57 | + * \param a 3d coordinates of point a |
| 58 | + * \param b 3d coordinates of point b |
| 59 | + * \param c 3d coordinates of point c |
| 60 | + */ |
| 61 | + BoundingSphere(const Vector3r& a, const Vector3r& b, const Vector3r& c) |
| 62 | + { |
| 63 | + const Vector3r ba = b - a; |
| 64 | + const Vector3r ca = c - a; |
| 65 | + const Vector3r baxca = ba.cross(ca); |
| 66 | + Vector3r r; |
| 67 | + Matrix3r T; |
| 68 | + T << ba[0], ba[1], ba[2], |
| 69 | + ca[0], ca[1], ca[2], |
| 70 | + baxca[0], baxca[1], baxca[2]; |
| 71 | + |
| 72 | + r[0] = 0.5 * ba.squaredNorm(); |
| 73 | + r[1] = 0.5 * ca.squaredNorm(); |
| 74 | + r[2] = 0.0; |
| 75 | + |
| 76 | + m_x = T.inverse() * r; |
| 77 | + m_r = m_x.norm(); |
| 78 | + m_x += a; |
| 79 | + } |
| 80 | + |
| 81 | + /** |
| 82 | + * \brief constructs the smallest enclosing sphere for four points |
| 83 | + * |
| 84 | + * \param a 3d coordinates of point a |
| 85 | + * \param b 3d coordinates of point b |
| 86 | + * \param c 3d coordinates of point c |
| 87 | + * \param d 3d coordinates of point d |
| 88 | + */ |
| 89 | + BoundingSphere(const Vector3r& a, const Vector3r& b, const Vector3r& c, const Vector3r& d) |
| 90 | + { |
| 91 | + const Vector3r ba = b - a; |
| 92 | + const Vector3r ca = c - a; |
| 93 | + const Vector3r da = d - a; |
| 94 | + Vector3r r; |
| 95 | + Matrix3r T; |
| 96 | + T << ba[0], ba[1], ba[2], |
| 97 | + ca[0], ca[1], ca[2], |
| 98 | + da[0], da[1], da[2]; |
| 99 | + |
| 100 | + r[0] = 0.5 * ba.squaredNorm(); |
| 101 | + r[1] = 0.5 * ca.squaredNorm(); |
| 102 | + r[2] = 0.5 * da.squaredNorm(); |
| 103 | + m_x = T.inverse() * r; |
| 104 | + m_r = m_x.norm(); |
| 105 | + m_x += a; |
| 106 | + } |
12 | 107 |
|
13 |
| - BoundingSphere() = default; |
14 |
| - BoundingSphere(Vector3r const& x, Real r) : m_x(x), m_r(r) {} |
| 108 | + /** |
| 109 | + * \brief constructs the smallest enclosing sphere a given pointset |
| 110 | + * |
| 111 | + * \param p vertices of the points |
| 112 | + */ |
| 113 | + BoundingSphere(const std::vector<Vector3r>& p) |
| 114 | + { |
| 115 | + m_r = 0; |
| 116 | + m_x.setZero(); |
| 117 | + setPoints(p); |
| 118 | + } |
15 | 119 |
|
| 120 | + /** |
| 121 | + * \brief Getter for the center of the sphere |
| 122 | + * |
| 123 | + * \return const reference of the sphere center |
| 124 | + */ |
16 | 125 | Vector3r const& x() const { return m_x; }
|
| 126 | + |
| 127 | + /** |
| 128 | + * \brief Access function for center of the sphere |
| 129 | + * |
| 130 | + * \return reference of the sphere center |
| 131 | + */ |
17 | 132 | Vector3r& x() { return m_x; }
|
18 | 133 |
|
| 134 | + /** |
| 135 | + * \brief Getter for the radius |
| 136 | + * |
| 137 | + * \return Radius of the sphere |
| 138 | + */ |
19 | 139 | Real r() const { return m_r; }
|
| 140 | + |
| 141 | + /** |
| 142 | + * \brief Access function for the radius |
| 143 | + * |
| 144 | + * \return Reference to the radius of the sphere |
| 145 | + */ |
20 | 146 | Real& r() { return m_r; }
|
21 | 147 |
|
| 148 | + /** |
| 149 | + * \brief constructs the smallest enclosing sphere a given pointset |
| 150 | + * |
| 151 | + * \param p vertices of the points |
| 152 | + */ |
| 153 | + void setPoints(const std::vector<Vector3r>& p) |
| 154 | + { |
| 155 | + //remove duplicates |
| 156 | + std::vector<Vector3r> v(p); |
| 157 | + std::sort(v.begin(), v.end(), [](const Vector3r& a, const Vector3r& b) |
| 158 | + { |
| 159 | + if (a[0] < b[0]) return true; |
| 160 | + if (a[0] > b[0]) return false; |
| 161 | + if (a[1] < b[1]) return true; |
| 162 | + if (a[1] > b[1]) return false; |
| 163 | + return (a[2] < b[2]); |
| 164 | + }); |
| 165 | + v.erase(std::unique(v.begin(), v.end(), [](Vector3r& a, Vector3r& b) { return a.isApprox(b); }), v.end()); |
| 166 | + |
| 167 | + Vector3r d; |
| 168 | + const int n = int(v.size()); |
| 169 | + |
| 170 | + //generate random permutation of the points and permute the points by epsilon to avoid corner cases |
| 171 | + const double epsilon = 1.0e-6; |
| 172 | + for (int i = n - 1; i > 0; i--) |
| 173 | + { |
| 174 | + const Vector3r epsilon_vec = epsilon * Vector3r::Random(); |
| 175 | + const int j = static_cast<int>(floor(i * double(rand()) / RAND_MAX)); |
| 176 | + d = v[i] + epsilon_vec; |
| 177 | + v[i] = v[j] - epsilon_vec; |
| 178 | + v[j] = d; |
| 179 | + } |
| 180 | + |
| 181 | + BoundingSphere S = BoundingSphere(v[0], v[1]); |
| 182 | + |
| 183 | + for (int i = 2; i < n; i++) |
| 184 | + { |
| 185 | + //SES0 |
| 186 | + d = v[i] - S.x(); |
| 187 | + if (d.squaredNorm() > S.r()* S.r()) |
| 188 | + S = ses1(i, v, v[i]); |
| 189 | + } |
| 190 | + |
| 191 | + m_x = S.m_x; |
| 192 | + m_r = S.m_r + epsilon; //add epsilon to make sure that all non-pertubated points are inside the sphere |
| 193 | + } |
| 194 | + |
| 195 | + /** |
| 196 | + * \brief intersection test for two spheres |
| 197 | + * |
| 198 | + * \param other other sphere to be tested for intersection |
| 199 | + * \return returns true when this sphere and the other sphere are intersecting |
| 200 | + */ |
22 | 201 | bool overlaps(BoundingSphere const& other) const
|
23 | 202 | {
|
24 |
| - Real rr = m_r + other.m_r; |
| 203 | + double rr = m_r + other.m_r; |
25 | 204 | return (m_x - other.m_x).squaredNorm() < rr * rr;
|
26 | 205 | }
|
27 | 206 |
|
| 207 | + /** |
| 208 | + * \brief tests whether the given sphere other is contained in the sphere |
| 209 | + * |
| 210 | + * \param other bounding sphere |
| 211 | + * \return returns true when the other is contained in this sphere or vice versa |
| 212 | + */ |
28 | 213 | bool contains(BoundingSphere const& other) const
|
29 | 214 | {
|
30 |
| - Real rr = r() - other.r(); |
| 215 | + double rr = r() - other.r(); |
31 | 216 | return (x() - other.x()).squaredNorm() < rr * rr;
|
32 | 217 | }
|
33 | 218 |
|
| 219 | + /** |
| 220 | + * \brief tests whether the given point other is contained in the sphere |
| 221 | + * |
| 222 | + * \param other 3d coordinates of a point |
| 223 | + * \return returns true when the point is contained in the sphere |
| 224 | + */ |
34 | 225 | bool contains(Vector3r const& other) const
|
35 | 226 | {
|
36 | 227 | return (x() - other).squaredNorm() < m_r * m_r;
|
37 | 228 | }
|
38 | 229 |
|
39 | 230 | private:
|
| 231 | + /** |
| 232 | + * \brief constructs the smallest enclosing sphere for n points with the points q1, q2, and q3 on the surface of the sphere |
| 233 | + * |
| 234 | + * \param n number of points |
| 235 | + * \param p vertices of the points |
| 236 | + * \param q1 3d coordinates of a point on the surface |
| 237 | + * \param q2 3d coordinates of a second point on the surface |
| 238 | + * \param q3 3d coordinates of a third point on the surface |
| 239 | + * \return smallest enclosing sphere |
| 240 | + */ |
| 241 | + BoundingSphere ses3(int n, std::vector<Vector3r>& p, Vector3r& q1, Vector3r& q2, Vector3r& q3) |
| 242 | + { |
| 243 | + BoundingSphere S(q1, q2, q3); |
| 244 | + |
| 245 | + for (int i = 0; i < n; i++) |
| 246 | + { |
| 247 | + Vector3r d = p[i] - S.x(); |
| 248 | + if (d.squaredNorm() > S.r()* S.r()) |
| 249 | + S = BoundingSphere(q1, q2, q3, p[i]); |
| 250 | + } |
| 251 | + return S; |
| 252 | + } |
| 253 | + |
| 254 | + /** |
| 255 | + * \brief constructs the smallest enclosing sphere for n points with the points q1 and q2 on the surface of the sphere |
| 256 | + * |
| 257 | + * \param n number of points |
| 258 | + * \param p vertices of the points |
| 259 | + * \param q1 3d coordinates of a point on the surface |
| 260 | + * \param q2 3d coordinates of a second point on the surface |
| 261 | + * \return smallest enclosing sphere |
| 262 | + */ |
| 263 | + BoundingSphere ses2(int n, std::vector<Vector3r>& p, Vector3r& q1, Vector3r& q2) |
| 264 | + { |
| 265 | + BoundingSphere S(q1, q2); |
| 266 | + |
| 267 | + for (int i = 0; i < n; i++) |
| 268 | + { |
| 269 | + Vector3r d = p[i] - S.x(); |
| 270 | + if (d.squaredNorm() > S.r()* S.r()) |
| 271 | + S = ses3(i, p, q1, q2, p[i]); |
| 272 | + } |
| 273 | + return S; |
| 274 | + } |
| 275 | + /** |
| 276 | + * \brief constructs the smallest enclosing sphere for n points with the point q1 on the surface of the sphere |
| 277 | + * |
| 278 | + * \param n number of points |
| 279 | + * \param p vertices of the points |
| 280 | + * \param q1 3d coordinates of a point on the surface |
| 281 | + * \return smallest enclosing sphere |
| 282 | + */ |
| 283 | + BoundingSphere ses1(int n, std::vector<Vector3r>& p, Vector3r& q1) |
| 284 | + { |
| 285 | + BoundingSphere S(p[0], q1); |
| 286 | + |
| 287 | + for (int i = 1; i < n; i++) |
| 288 | + { |
| 289 | + Vector3r d = p[i] - S.x(); |
| 290 | + if (d.squaredNorm() > S.r()* S.r()) |
| 291 | + S = ses2(i, p, q1, p[i]); |
| 292 | + } |
| 293 | + return S; |
| 294 | + } |
40 | 295 |
|
41 | 296 | Vector3r m_x;
|
42 | 297 | Real m_r;
|
|
0 commit comments