| 1 |
#ifndef SPHERE_HEADER |
| 2 |
#define SPHERE_HEADER |
| 3 |
#include "basic.h" |
| 4 |
#include <MiscLib/Vector.h> |
| 5 |
#include <stdexcept> |
| 6 |
#include <GfxTL/HyperplaneCoordinateSystem.h> |
| 7 |
#include <utility> |
| 8 |
#include "PointCloud.h" |
| 9 |
#include <ostream> |
| 10 |
#include <istream> |
| 11 |
#include <stdio.h> |
| 12 |
#include <utility> |
| 13 |
#include <MiscLib/NoShrinkVector.h> |
| 14 |
#include "LevMarLSWeight.h" |
| 15 |
#include "LevMarFitting.h" |
| 16 |
|
| 17 |
#ifndef DLL_LINKAGE |
| 18 |
#define DLL_LINKAGE |
| 19 |
#endif |
| 20 |
|
| 21 |
struct DLL_LINKAGE InvalidTetrahedonError |
| 22 |
: public std::runtime_error |
| 23 |
{ |
| 24 |
InvalidTetrahedonError(); |
| 25 |
}; |
| 26 |
|
| 27 |
class DLL_LINKAGE Sphere |
| 28 |
{ |
| 29 |
public: |
| 30 |
enum { RequiredSamples = 2 }; |
| 31 |
Sphere(); |
| 32 |
Sphere(const Vec3f ¢er, float radius); |
| 33 |
Sphere(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, |
| 34 |
const Vec3f &p4); |
| 35 |
bool Init(const MiscLib::Vector< Vec3f > &samples); |
| 36 |
bool Init(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, |
| 37 |
const Vec3f &p4); |
| 38 |
bool Init2(const Vec3f &p1, const Vec3f &p2, const Vec3f &n1, |
| 39 |
const Vec3f &n2); |
| 40 |
bool Init(bool binary, std::istream *i); |
| 41 |
void Init(FILE *i); |
| 42 |
void Init(float *array); |
| 43 |
inline float Distance(const Vec3f &p) const; |
| 44 |
inline void Normal(const Vec3f &p, Vec3f *normal) const; |
| 45 |
inline float DistanceAndNormal(const Vec3f &p, Vec3f *normal) const; |
| 46 |
inline float SignedDistance(const Vec3f &p) const; |
| 47 |
void Project(const Vec3f &p, Vec3f *pp) const; |
| 48 |
const Vec3f &Center() const; |
| 49 |
void Center(const Vec3f ¢er) { m_center = center; } |
| 50 |
float Radius() const; |
| 51 |
void Radius(float radius) { m_radius = radius; } |
| 52 |
bool LeastSquaresFit(const PointCloud &pc, |
| 53 |
MiscLib::Vector< size_t >::const_iterator begin, |
| 54 |
MiscLib::Vector< size_t >::const_iterator end); |
| 55 |
template< class IteratorT > |
| 56 |
bool LeastSquaresFit(IteratorT begin, IteratorT end); |
| 57 |
bool Fit(const PointCloud &pc, |
| 58 |
MiscLib::Vector< size_t >::const_iterator begin, |
| 59 |
MiscLib::Vector< size_t >::const_iterator end) |
| 60 |
{ return LeastSquaresFit(pc, begin, end); } |
| 61 |
static bool Interpolate(const MiscLib::Vector< Sphere > &spheres, |
| 62 |
const MiscLib::Vector< float > &weights, Sphere *is); |
| 63 |
void Serialize(bool binary, std::ostream *o) const; |
| 64 |
static size_t SerializedSize(); |
| 65 |
void Serialize(FILE *o) const; |
| 66 |
void Serialize(float* array) const; |
| 67 |
static size_t SerializedFloatSize(); |
| 68 |
|
| 69 |
void Transform(float scale, const Vec3f &translate); |
| 70 |
inline unsigned int Intersect(const Vec3f &p, const Vec3f &r, |
| 71 |
float *first, float *second) const; |
| 72 |
|
| 73 |
private: |
| 74 |
template< class WeightT > |
| 75 |
class LevMarSimpleSphere |
| 76 |
: public WeightT |
| 77 |
{ |
| 78 |
public: |
| 79 |
enum { NumParams = 4 }; |
| 80 |
typedef float ScalarType; |
| 81 |
|
| 82 |
template< class IteratorT > |
| 83 |
ScalarType Chi(const ScalarType *params, IteratorT begin, IteratorT end, |
| 84 |
ScalarType *values, ScalarType *temp) const |
| 85 |
{ |
| 86 |
ScalarType chi = 0; |
| 87 |
size_t size = end - begin; |
| 88 |
#ifdef DOPARALLEL |
| 89 |
#pragma omp parallel for schedule(static) reduction(+:chi) |
| 90 |
#endif |
| 91 |
for(int idx = 0; idx < size; ++idx) |
| 92 |
{ |
| 93 |
float s = begin[idx][0] - params[0]; |
| 94 |
s *= s; |
| 95 |
for(unsigned int j = 1; j < 3; ++j) |
| 96 |
{ |
| 97 |
float ss = begin[idx][j] - params[j]; |
| 98 |
s += ss * ss; |
| 99 |
} |
| 100 |
values[idx] = WeightT::Weigh(std::sqrt(s) - params[3]); |
| 101 |
chi += values[idx] * values[idx]; |
| 102 |
} |
| 103 |
return chi; |
| 104 |
} |
| 105 |
|
| 106 |
template< class IteratorT > |
| 107 |
void Derivatives(const ScalarType *params, IteratorT begin, IteratorT end, |
| 108 |
const ScalarType *values, const ScalarType *temp, ScalarType *matrix) const |
| 109 |
{ |
| 110 |
int size = static_cast<int>(end - begin); |
| 111 |
#ifdef DOPARALLEL |
| 112 |
#pragma omp parallel for schedule(static) |
| 113 |
#endif |
| 114 |
for(int idx = 0; idx < size; ++idx) |
| 115 |
{ |
| 116 |
float s[3]; |
| 117 |
s[0] = begin[idx][0] - params[0]; |
| 118 |
float sl = s[0] * s[0]; |
| 119 |
for(unsigned int i = 1; i < 3; ++i) |
| 120 |
{ |
| 121 |
s[i] = begin[idx][i] - params[i]; |
| 122 |
sl += s[i] * s[i]; |
| 123 |
} |
| 124 |
sl = std::sqrt(sl); |
| 125 |
matrix[idx * NumParams + 0] = -s[0] / sl; |
| 126 |
matrix[idx * NumParams + 1] = -s[1] / sl; |
| 127 |
matrix[idx * NumParams + 2] = -s[2] / sl; |
| 128 |
matrix[idx * NumParams + 3] = -1; |
| 129 |
WeightT::template DerivWeigh< NumParams >(sl - params[3], |
| 130 |
matrix + idx * NumParams); |
| 131 |
} |
| 132 |
} |
| 133 |
|
| 134 |
void Normalize(ScalarType *) const |
| 135 |
{} |
| 136 |
}; |
| 137 |
|
| 138 |
template< class WeightT > |
| 139 |
class LevMarSphere |
| 140 |
: public WeightT |
| 141 |
{ |
| 142 |
public: |
| 143 |
enum { NumParams = 7 }; |
| 144 |
typedef float ScalarType; |
| 145 |
|
| 146 |
// parametrization: params[0] - params[2] = normal |
| 147 |
// params[3] - params[5] = point |
| 148 |
// params[6] = 1 / radius |
| 149 |
|
| 150 |
template< class IteratorT > |
| 151 |
ScalarType Chi(const ScalarType *params, IteratorT begin, IteratorT end, |
| 152 |
ScalarType *values, ScalarType *temp) const |
| 153 |
{ |
| 154 |
ScalarType chi = 0; |
| 155 |
ScalarType radius = 1 / params[6]; |
| 156 |
Vec3f center = -radius * Vec3f(params[0], params[1], params[2]) |
| 157 |
+ Vec3f(params[3], params[4], params[5]); |
| 158 |
int size = end - begin; |
| 159 |
#ifdef DOPARALLEL |
| 160 |
#pragma omp parallel for schedule(static) reduction(+:chi) |
| 161 |
#endif |
| 162 |
for(int idx = 0; idx < size; ++idx) |
| 163 |
{ |
| 164 |
temp[idx] = (begin[idx] - center).length(); |
| 165 |
chi += (values[idx] = WeightT::Weigh(temp[idx] - radius)) |
| 166 |
* values[idx]; |
| 167 |
} |
| 168 |
return chi; |
| 169 |
} |
| 170 |
|
| 171 |
template< class IteratorT > |
| 172 |
void Derivatives(const ScalarType *params, IteratorT begin, IteratorT end, |
| 173 |
const ScalarType *values, const ScalarType *temp, ScalarType *matrix) const |
| 174 |
{ |
| 175 |
Vec3f normal(params[0], params[1], params[2]); |
| 176 |
Vec3f point(params[3], params[4], params[5]); |
| 177 |
int size = end - begin; |
| 178 |
#ifdef DOPARALLEL |
| 179 |
#pragma omp parallel for schedule(static) |
| 180 |
#endif |
| 181 |
for(int idx = 0; idx < size; ++idx) |
| 182 |
{ |
| 183 |
ScalarType denominator = -1.f / temp[idx] * params[6]; |
| 184 |
matrix[idx * NumParams + 0] = |
| 185 |
(matrix[idx * NumParams + 3] = (point[0] - normal[0] * params[6] - begin[idx][0])) |
| 186 |
* denominator; |
| 187 |
matrix[idx * NumParams + 1] = |
| 188 |
(matrix[idx * NumParams + 4] = (point[1] - normal[1] * params[6] - begin[idx][1])) |
| 189 |
* denominator; |
| 190 |
matrix[idx * NumParams + 2] = |
| 191 |
(matrix[idx * NumParams + 5] = (point[2] - normal[2] * params[6] - begin[idx][2])) |
| 192 |
* denominator; |
| 193 |
matrix[idx * NumParams + 3] /= temp[idx]; |
| 194 |
matrix[idx * NumParams + 4] /= temp[idx]; |
| 195 |
matrix[idx * NumParams + 5] /= temp[idx]; |
| 196 |
matrix[idx * NumParams + 6] = (normal[0] * matrix[idx * NumParams + 3] |
| 197 |
+ normal[1] * matrix[idx * NumParams + 4] |
| 198 |
+ normal[2] * matrix[idx * NumParams + 5] + 1) * params[6] * params[6]; |
| 199 |
WeightT::template DerivWeigh< NumParams >(temp[idx] - 1.f / params[6], |
| 200 |
matrix + idx * NumParams); |
| 201 |
} |
| 202 |
} |
| 203 |
|
| 204 |
void Normalize(ScalarType *params) const |
| 205 |
{ |
| 206 |
ScalarType len = std::sqrt(params[0] * params[0] |
| 207 |
+ params[1] * params[1] + params[2] * params[2]); |
| 208 |
params[0] /= len; |
| 209 |
params[1] /= len; |
| 210 |
params[2] /= len; |
| 211 |
} |
| 212 |
}; |
| 213 |
|
| 214 |
private: |
| 215 |
Vec3f m_center; |
| 216 |
float m_radius; |
| 217 |
}; |
| 218 |
|
| 219 |
inline float Sphere::Distance(const Vec3f &p) const |
| 220 |
{ |
| 221 |
return fabs((m_center - p).length() - m_radius); |
| 222 |
} |
| 223 |
|
| 224 |
inline void Sphere::Normal(const Vec3f &p, Vec3f *normal) const |
| 225 |
{ |
| 226 |
*normal = p - m_center; |
| 227 |
normal->normalize(); |
| 228 |
} |
| 229 |
|
| 230 |
inline float Sphere::DistanceAndNormal(const Vec3f &p, Vec3f *normal) const |
| 231 |
{ |
| 232 |
*normal = p - m_center; |
| 233 |
float l = normal->length(); |
| 234 |
if(l > 0) |
| 235 |
*normal /= l; |
| 236 |
return fabs(l - m_radius); |
| 237 |
} |
| 238 |
|
| 239 |
inline float Sphere::SignedDistance(const Vec3f &p) const |
| 240 |
{ |
| 241 |
return (m_center - p).length() - m_radius; |
| 242 |
} |
| 243 |
|
| 244 |
template< class IteratorT > |
| 245 |
bool Sphere::LeastSquaresFit(IteratorT begin, IteratorT end) |
| 246 |
{ |
| 247 |
LevMarSimpleSphere< LevMarLSWeight > levMarSphere; |
| 248 |
float param[4]; |
| 249 |
for(size_t i = 0; i < 3; ++i) |
| 250 |
param[i] = m_center[i]; |
| 251 |
param[3] = m_radius; |
| 252 |
if(!LevMar(begin, end, levMarSphere, param)) |
| 253 |
return false; |
| 254 |
for(size_t i = 0; i < 3; ++i) |
| 255 |
m_center[i] = param[i]; |
| 256 |
m_radius = param[3]; |
| 257 |
return true; |
| 258 |
} |
| 259 |
|
| 260 |
inline unsigned int Sphere::Intersect(const Vec3f &p, const Vec3f &r, |
| 261 |
float *first, float *second) const |
| 262 |
{ |
| 263 |
using namespace std; |
| 264 |
Vec3f kDiff = p - m_center; |
| 265 |
float fA0 = kDiff.dot(kDiff) - m_radius*m_radius; |
| 266 |
float fA1, fDiscr, fRoot; |
| 267 |
if (fA0 <= 0) |
| 268 |
{ |
| 269 |
// P is inside the sphere |
| 270 |
fA1 = r.dot(kDiff); |
| 271 |
fDiscr = fA1*fA1 - fA0; |
| 272 |
fRoot = sqrt(fDiscr); |
| 273 |
*first = -fA1 + fRoot; |
| 274 |
return 1; |
| 275 |
} |
| 276 |
// else: P is outside the sphere |
| 277 |
fA1 = r.dot(kDiff); |
| 278 |
if (fA1 >= 0) |
| 279 |
return 0; |
| 280 |
fDiscr = fA1*fA1 - fA0; |
| 281 |
if(fDiscr < 0) |
| 282 |
return 0; |
| 283 |
else if(fDiscr >= /* zero tolerance eps */ 1e-7f) |
| 284 |
{ |
| 285 |
fRoot = sqrt(fDiscr); |
| 286 |
*first = -fA1 - fRoot; |
| 287 |
*second = -fA1 + fRoot; |
| 288 |
return 2; |
| 289 |
} |
| 290 |
*first = -fA1; |
| 291 |
return 1; |
| 292 |
} |
| 293 |
|
| 294 |
class DLL_LINKAGE SphereAsSquaresParametrization |
| 295 |
{ |
| 296 |
public: |
| 297 |
SphereAsSquaresParametrization() {} |
| 298 |
SphereAsSquaresParametrization(const Sphere &sphere, |
| 299 |
const Vec3f &planeNormal); |
| 300 |
void Init(const Sphere &sphere, const Vec3f &planeNormal); |
| 301 |
|
| 302 |
// returns < 0 if point is on lower hemisphere |
| 303 |
float Parameters(const Vec3f &p, |
| 304 |
std::pair< float, float > *param) const; |
| 305 |
bool InSpace(const std::pair< float, float > ¶m, bool lower, |
| 306 |
Vec3f *p) const; |
| 307 |
bool InSpace(const std::pair< float, float > ¶m, bool lower, |
| 308 |
Vec3f *p, Vec3f *n) const; |
| 309 |
void Transform(const GfxTL::MatrixXX< 3, 3, float > &rot, |
| 310 |
const GfxTL::Vector3Df &trans); |
| 311 |
void HyperplaneCoordinateSystem( Vec3f* hcs0, Vec3f* hcs1, Vec3f* hcs2 ) const; |
| 312 |
|
| 313 |
private: |
| 314 |
void Hemisphere2Disk(const Vec3f &p, |
| 315 |
std::pair< float, float > *inDisk) const; |
| 316 |
void Disk2Square(const std::pair< float, float > &inDisk, |
| 317 |
std::pair< float, float > *inSquare) const; |
| 318 |
void Square2Disk(const std::pair< float, float > &inSquare, |
| 319 |
std::pair< float, float > *inDisk) const; |
| 320 |
void Disk2Hemisphere(const std::pair< float, float > &inDisk, |
| 321 |
Vec3f *p) const; |
| 322 |
|
| 323 |
private: |
| 324 |
Sphere m_sphere; |
| 325 |
Vec3f m_planeNormal; |
| 326 |
GfxTL::HyperplaneCoordinateSystem< float, 3 > m_hcs; |
| 327 |
}; |
| 328 |
|
| 329 |
class DLL_LINKAGE UpperSphereAsSquaresParametrization |
| 330 |
: public SphereAsSquaresParametrization |
| 331 |
{ |
| 332 |
public: |
| 333 |
UpperSphereAsSquaresParametrization() {} |
| 334 |
UpperSphereAsSquaresParametrization(const SphereAsSquaresParametrization &p) |
| 335 |
: SphereAsSquaresParametrization(p) {} |
| 336 |
bool InSpace(const std::pair< float, float > ¶m, Vec3f *p) const |
| 337 |
{ return SphereAsSquaresParametrization::InSpace(param, false, p); } |
| 338 |
bool InSpace(const std::pair< float, float > ¶m, Vec3f *p, Vec3f *n) const |
| 339 |
{ return SphereAsSquaresParametrization::InSpace(param, false, p, n); } |
| 340 |
bool InSpace(float u, float v, Vec3f *p) const |
| 341 |
{ return SphereAsSquaresParametrization::InSpace(std::make_pair(u, v), false, p); } |
| 342 |
bool InSpace(float u, float v, Vec3f *p, Vec3f *n) const |
| 343 |
{ return SphereAsSquaresParametrization::InSpace(std::make_pair(u, v), false, p, n); } |
| 344 |
}; |
| 345 |
|
| 346 |
class DLL_LINKAGE LowerSphereAsSquaresParametrization |
| 347 |
: public SphereAsSquaresParametrization |
| 348 |
{ |
| 349 |
public: |
| 350 |
LowerSphereAsSquaresParametrization() {} |
| 351 |
LowerSphereAsSquaresParametrization(const SphereAsSquaresParametrization &p) |
| 352 |
: SphereAsSquaresParametrization(p) {} |
| 353 |
bool InSpace(const std::pair< float, float > ¶m, Vec3f *p) const |
| 354 |
{ return SphereAsSquaresParametrization::InSpace(param, true, p); } |
| 355 |
bool InSpace(const std::pair< float, float > ¶m, Vec3f *p, Vec3f *n) const |
| 356 |
{ return SphereAsSquaresParametrization::InSpace(param, true, p, n); } |
| 357 |
bool InSpace(float u, float v, Vec3f *p) const |
| 358 |
{ return SphereAsSquaresParametrization::InSpace(std::make_pair(u, v), true, p); } |
| 359 |
bool InSpace(float u, float v, Vec3f *p, Vec3f *n) const |
| 360 |
{ return SphereAsSquaresParametrization::InSpace(std::make_pair(u, v), true, p, n); } |
| 361 |
}; |
| 362 |
|
| 363 |
#endif |