| 1 |
#include "Sphere.h" |
| 2 |
#include <MiscLib/Performance.h> |
| 3 |
#include <GfxTL/IndexedIterator.h> |
| 4 |
#include <GfxTL/MathHelper.h> |
| 5 |
#ifdef DOPARALLEL |
| 6 |
#include <omp.h> |
| 7 |
#endif |
| 8 |
|
| 9 |
extern int dmat_solve ( int n, int rhs_num, double a[] ); |
| 10 |
|
| 11 |
void tetrahedron_circumsphere_3d ( double tetra[3*4], double *r, double pc[3] ) |
| 12 |
|
| 13 |
//****************************************************************************** |
| 14 |
// |
| 15 |
// Purpose: |
| 16 |
// |
| 17 |
// TETRAHEDRON_CIRCUMSPHERE_3D computes the circumsphere of a tetrahedron in 3D. |
| 18 |
// |
| 19 |
// Discussion: |
| 20 |
// |
| 21 |
// The circumsphere, or circumscribed sphere, of a tetrahedron is the sphere that |
| 22 |
// passes through the four vertices. The circumsphere is not necessarily |
| 23 |
// the smallest sphere that contains the tetrahedron. |
| 24 |
// |
| 25 |
// Surprisingly, the diameter of the sphere can be found by solving |
| 26 |
// a 3 by 3 linear system. This is because the vectors P2 - P1, |
| 27 |
// P3 - P1 and P4 - P1 are secants of the sphere, and each forms a |
| 28 |
// right triangle with the diameter through P1. Hence, the dot product of |
| 29 |
// P2 - P1 with that diameter is equal to the square of the length |
| 30 |
// of P2 - P1, and similarly for P3 - P1 and P4 - P1. This determines |
| 31 |
// the diameter vector originating at P1, and hence the radius and |
| 32 |
// center. |
| 33 |
// |
| 34 |
// Modified: |
| 35 |
// |
| 36 |
// 10 August 2005 |
| 37 |
// |
| 38 |
// Author: |
| 39 |
// |
| 40 |
// John Burkardt |
| 41 |
// |
| 42 |
// Reference: |
| 43 |
// |
| 44 |
// Adrian Bowyer and John Woodwark, |
| 45 |
// A Programmer's Geometry, |
| 46 |
// Butterworths, 1983. |
| 47 |
// |
| 48 |
// Parameters: |
| 49 |
// |
| 50 |
// Input, double TETRA[3*4], the vertices of the tetrahedron. |
| 51 |
// |
| 52 |
// Output, double *R, PC[3], the coordinates of the center of the |
| 53 |
// circumscribed sphere, and its radius. If the linear system is |
| 54 |
// singular, then R = -1, PC[] = 0. |
| 55 |
// |
| 56 |
{ |
| 57 |
# define DIM_NUM 3 |
| 58 |
# define RHS_NUM 1 |
| 59 |
|
| 60 |
double a[DIM_NUM*(DIM_NUM+RHS_NUM)]; |
| 61 |
int info; |
| 62 |
// |
| 63 |
// Set up the linear system. |
| 64 |
// |
| 65 |
a[0+0*3] = tetra[0+1*3] - tetra[0+0*3]; |
| 66 |
a[0+1*3] = tetra[1+1*3] - tetra[1+0*3]; |
| 67 |
a[0+2*3] = tetra[2+1*3] - tetra[2+0*3]; |
| 68 |
a[0+3*3] = std::pow ( tetra[0+1*3] - tetra[0+0*3], 2 ) |
| 69 |
+ std::pow ( tetra[1+1*3] - tetra[1+0*3], 2 ) |
| 70 |
+ std::pow ( tetra[2+1*3] - tetra[2+0*3], 2 ); |
| 71 |
|
| 72 |
a[1+0*3] = tetra[0+2*3] - tetra[0+0*3]; |
| 73 |
a[1+1*3] = tetra[1+2*3] - tetra[1+0*3]; |
| 74 |
a[1+2*3] = tetra[2+2*3] - tetra[2+0*3]; |
| 75 |
a[1+3*3] = std::pow ( tetra[0+2*3] - tetra[0+0*3], 2 ) |
| 76 |
+ std::pow ( tetra[1+2*3] - tetra[1+0*3], 2 ) |
| 77 |
+ std::pow ( tetra[2+2*3] - tetra[2+0*3], 2 ); |
| 78 |
|
| 79 |
a[2+0*3] = tetra[0+3*3] - tetra[0+0*3]; |
| 80 |
a[2+1*3] = tetra[1+3*3] - tetra[1+0*3]; |
| 81 |
a[2+2*3] = tetra[2+3*3] - tetra[2+0*3]; |
| 82 |
a[2+3*3] = std::pow ( tetra[0+3*3] - tetra[0+0*3], 2 ) |
| 83 |
+ std::pow ( tetra[1+3*3] - tetra[1+0*3], 2 ) |
| 84 |
+ std::pow ( tetra[2+3*3] - tetra[2+0*3], 2 ); |
| 85 |
// |
| 86 |
// Solve the linear system. |
| 87 |
// |
| 88 |
info = dmat_solve ( DIM_NUM, RHS_NUM, a ); |
| 89 |
// |
| 90 |
// If the system was singular, return a consolation prize. |
| 91 |
// |
| 92 |
if ( info != 0 ) |
| 93 |
{ |
| 94 |
*r = -1.0; |
| 95 |
for(size_t i = 0; i < DIM_NUM; ++i) |
| 96 |
pc[i] = 0; |
| 97 |
//dvec_zero ( DIM_NUM, pc ); |
| 98 |
return; |
| 99 |
} |
| 100 |
// |
| 101 |
// Compute the radius and center. |
| 102 |
// |
| 103 |
*r = 0.5 * std::sqrt |
| 104 |
( a[0+3*3] * a[0+3*3] |
| 105 |
+ a[1+3*3] * a[1+3*3] |
| 106 |
+ a[2+3*3] * a[2+3*3] ); |
| 107 |
|
| 108 |
pc[0] = tetra[0+0*3] + 0.5 * a[0+3*3]; |
| 109 |
pc[1] = tetra[1+0*3] + 0.5 * a[1+3*3]; |
| 110 |
pc[2] = tetra[2+0*3] + 0.5 * a[2+3*3]; |
| 111 |
|
| 112 |
return; |
| 113 |
# undef DIM_NUM |
| 114 |
# undef RHS_NUM |
| 115 |
} |
| 116 |
|
| 117 |
bool Midpoint(const Vec3f &p1, const Vec3f &n1, const Vec3f &p2, const Vec3f &n2, |
| 118 |
Vec3f *mid) |
| 119 |
{ |
| 120 |
float d1343,d4321,d1321,d4343,d2121; |
| 121 |
float numer,denom, mua, mub; |
| 122 |
|
| 123 |
Vec3f p13 = p1 - p2; |
| 124 |
// p43 = n2 |
| 125 |
// p21 = n1 |
| 126 |
d1343 = p13[0] * n2[0] + p13[1] * n2[1] + p13[2] * n2[2]; |
| 127 |
d4321 = n2[0] * n1[0] + n2[1] * n1[1] + n2[2] * n1[2]; |
| 128 |
d1321 = p13[0] * n1[0] + p13[1] * n1[1] + p13[2] * n1[2]; |
| 129 |
d4343 = n2[0] * n2[0] + n2[1] * n2[1] + n2[2] * n2[2]; |
| 130 |
d2121 = n1[0] * n1[0] + n1[1] * n1[1] + n1[2] * n1[2]; |
| 131 |
|
| 132 |
denom = d2121 * d4343 - d4321 * d4321; |
| 133 |
if(fabs(denom) < 1.0e-6) |
| 134 |
return false; |
| 135 |
numer = d1343 * d4321 - d1321 * d4343; |
| 136 |
|
| 137 |
mua = numer / denom; |
| 138 |
mub = (d1343 + d4321 * (mua)) / d4343; |
| 139 |
|
| 140 |
Vec3f pa, pb; |
| 141 |
pa = p1 + mua * n1; |
| 142 |
pb = p2 + mub * n2; |
| 143 |
*mid = 0.5f * (pa + pb); |
| 144 |
return true; |
| 145 |
} |
| 146 |
|
| 147 |
InvalidTetrahedonError::InvalidTetrahedonError() |
| 148 |
: std::runtime_error("Invalid tetrahedon") |
| 149 |
{} |
| 150 |
|
| 151 |
Sphere::Sphere() |
| 152 |
{} |
| 153 |
|
| 154 |
Sphere::Sphere(const Vec3f ¢er, float radius) |
| 155 |
: m_center(center) |
| 156 |
, m_radius(radius) |
| 157 |
{} |
| 158 |
|
| 159 |
Sphere::Sphere(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, |
| 160 |
const Vec3f &p4) |
| 161 |
{ |
| 162 |
if(!Init(p1, p2, p3, p4)) |
| 163 |
throw InvalidTetrahedonError(); |
| 164 |
} |
| 165 |
|
| 166 |
bool Sphere::Init(const MiscLib::Vector< Vec3f > &samples) |
| 167 |
{ |
| 168 |
if(samples.size() < 4) |
| 169 |
return false; |
| 170 |
// get center |
| 171 |
size_t c = samples.size() / 2; |
| 172 |
m_center = Vec3f(0, 0, 0); |
| 173 |
size_t midCount = 0; |
| 174 |
for(size_t i = 0; i < c - 1; ++i) |
| 175 |
for(size_t j = i + 1; j < c; ++j) |
| 176 |
{ |
| 177 |
Vec3f mid; |
| 178 |
if(!Midpoint(samples[i], samples[i + c], samples[j], samples[j + c], &mid)) |
| 179 |
continue; |
| 180 |
m_center += mid; |
| 181 |
++midCount; |
| 182 |
} |
| 183 |
if(!midCount) |
| 184 |
return false; |
| 185 |
m_center /= static_cast<float>(midCount); |
| 186 |
m_radius = 0; |
| 187 |
for(size_t i = 0; i < c; ++i) |
| 188 |
{ |
| 189 |
float d = (samples[i] - m_center).length(); |
| 190 |
m_radius += d; |
| 191 |
} |
| 192 |
m_radius /= c; |
| 193 |
return true; |
| 194 |
} |
| 195 |
|
| 196 |
bool Sphere::Init(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, |
| 197 |
const Vec3f &p4) |
| 198 |
{ |
| 199 |
// convert to double array |
| 200 |
double tetra[4 * 3]; |
| 201 |
for(size_t i = 0; i < 3; ++i) |
| 202 |
tetra[0 * 3 + i] = p1[i]; |
| 203 |
for(size_t i = 0; i < 3; ++i) |
| 204 |
tetra[1 * 3 + i] = p2[i]; |
| 205 |
for(size_t i = 0; i < 3; ++i) |
| 206 |
tetra[2 * 3 + i] = p3[i]; |
| 207 |
for(size_t i = 0; i < 3; ++i) |
| 208 |
tetra[3 * 3 + i] = p4[i]; |
| 209 |
double r, pc[3]; |
| 210 |
tetrahedron_circumsphere_3d(tetra, &r, pc); |
| 211 |
if(r < 0) |
| 212 |
return false; |
| 213 |
m_radius = static_cast<float>(r); |
| 214 |
m_center[0] = static_cast<float>(pc[0]); |
| 215 |
m_center[1] = static_cast<float>(pc[1]); |
| 216 |
m_center[2] = static_cast<float>(pc[2]); |
| 217 |
return true; |
| 218 |
} |
| 219 |
|
| 220 |
bool Sphere::Init2(const Vec3f &p1, const Vec3f &p2, const Vec3f &n1, |
| 221 |
const Vec3f &n2) |
| 222 |
{ |
| 223 |
/* |
| 224 |
Calculate the line segment PaPb that is the shortest route between |
| 225 |
two lines P1P2 and P3P4. Calculate also the values of mua and mub where |
| 226 |
Pa = P1 + mua (P2 - P1) |
| 227 |
Pb = P3 + mub (P4 - P3) |
| 228 |
Return FALSE if no solution exists. |
| 229 |
*/ |
| 230 |
float d1343,d4321,d1321,d4343,d2121; |
| 231 |
float numer,denom, mua, mub; |
| 232 |
|
| 233 |
Vec3f p13 = p1 - p2; |
| 234 |
// p43 = n2 |
| 235 |
// p21 = n1 |
| 236 |
d1343 = p13[0] * n2[0] + p13[1] * n2[1] + p13[2] * n2[2]; |
| 237 |
d4321 = n2[0] * n1[0] + n2[1] * n1[1] + n2[2] * n1[2]; |
| 238 |
d1321 = p13[0] * n1[0] + p13[1] * n1[1] + p13[2] * n1[2]; |
| 239 |
d4343 = n2[0] * n2[0] + n2[1] * n2[1] + n2[2] * n2[2]; |
| 240 |
d2121 = n1[0] * n1[0] + n1[1] * n1[1] + n1[2] * n1[2]; |
| 241 |
|
| 242 |
denom = d2121 * d4343 - d4321 * d4321; |
| 243 |
if(fabs(denom) < 1.0e-6) |
| 244 |
return false; |
| 245 |
numer = d1343 * d4321 - d1321 * d4343; |
| 246 |
|
| 247 |
mua = numer / denom; |
| 248 |
mub = (d1343 + d4321 * (mua)) / d4343; |
| 249 |
|
| 250 |
Vec3f pa, pb; |
| 251 |
pa = p1 + mua * n1; |
| 252 |
pb = p2 + mub * n2; |
| 253 |
|
| 254 |
// get the midpoint between pa and pb and make it the center |
| 255 |
m_center = 0.5f * (pa + pb); |
| 256 |
// make the radius the average of the two distances |
| 257 |
float da = (p1 - m_center).length(); |
| 258 |
float db = (p2 - m_center).length(); |
| 259 |
m_radius = 0.5f * (da + db); |
| 260 |
// do some plausability checks |
| 261 |
// lets say the actual distance should not deviate by more than 10% |
| 262 |
float dev = da / m_radius; |
| 263 |
if(dev < 0.9f || dev > 1.1f) |
| 264 |
return false; |
| 265 |
dev = db / m_radius; |
| 266 |
if(dev < 0.9f || dev > 1.1f) |
| 267 |
return false; |
| 268 |
// distance between pa and pb should not be greater than 10% of the radius |
| 269 |
dev = (pa - pb).length() / m_radius; |
| 270 |
if(dev > 0.1f) |
| 271 |
return false; |
| 272 |
return true; |
| 273 |
} |
| 274 |
|
| 275 |
bool Sphere::Init(bool binary, std::istream *i) |
| 276 |
{ |
| 277 |
if(binary) |
| 278 |
{ |
| 279 |
i->read((char *)&m_center, sizeof(m_center)); |
| 280 |
i->read((char *)&m_radius, sizeof(m_radius)); |
| 281 |
} |
| 282 |
else |
| 283 |
{ |
| 284 |
for(size_t j = 0; j < 3; ++j) |
| 285 |
(*i) >> m_center[j]; |
| 286 |
(*i) >> m_radius; |
| 287 |
} |
| 288 |
return true; |
| 289 |
} |
| 290 |
|
| 291 |
void Sphere::Init(FILE *i) |
| 292 |
{ |
| 293 |
size_t readrtn; //unused return warning suppresion |
| 294 |
readrtn = fread(&m_center, sizeof(m_center), 1, i); |
| 295 |
readrtn = fread(&m_radius, sizeof(m_radius), 1, i); |
| 296 |
} |
| 297 |
|
| 298 |
void Sphere::Init(float *array) |
| 299 |
{ |
| 300 |
for(int i=0;i<3;i++){ |
| 301 |
m_center[i] = array[i]; |
| 302 |
} |
| 303 |
m_radius=array[3]; |
| 304 |
} |
| 305 |
|
| 306 |
void Sphere::Project(const Vec3f &p, Vec3f *pp) const |
| 307 |
{ |
| 308 |
*pp = p - m_center; |
| 309 |
float l = pp->length(); |
| 310 |
*pp *= m_radius / l; |
| 311 |
*pp += m_center; |
| 312 |
} |
| 313 |
|
| 314 |
const Vec3f &Sphere::Center() const |
| 315 |
{ |
| 316 |
return m_center; |
| 317 |
} |
| 318 |
|
| 319 |
float Sphere::Radius() const |
| 320 |
{ |
| 321 |
return m_radius; |
| 322 |
} |
| 323 |
|
| 324 |
float SphereDistance(const float *param, const float *x) |
| 325 |
{ |
| 326 |
float s = x[0] - param[0]; |
| 327 |
s *= s; |
| 328 |
for(unsigned int i = 1; i < 3; ++i) |
| 329 |
{ |
| 330 |
float ss = x[i] - param[i]; |
| 331 |
s += ss * ss; |
| 332 |
} |
| 333 |
return std::sqrt(s) - param[3]; |
| 334 |
} |
| 335 |
|
| 336 |
void SphereDistanceDerivatives(const float *param, const float *x, |
| 337 |
float *gradient) |
| 338 |
{ |
| 339 |
float s[3]; |
| 340 |
s[0] = x[0] - param[0]; |
| 341 |
float sl = s[0] * s[0]; |
| 342 |
for(unsigned int i = 1; i < 3; ++i) |
| 343 |
{ |
| 344 |
s[i] = x[i] - param[i]; |
| 345 |
sl += s[i] * s[i]; |
| 346 |
} |
| 347 |
sl = std::sqrt(sl); |
| 348 |
gradient[0] = -s[0] / sl; |
| 349 |
gradient[1] = -s[1] / sl; |
| 350 |
gradient[2] = -s[2] / sl; |
| 351 |
gradient[3] = -1; |
| 352 |
} |
| 353 |
|
| 354 |
void NormalizeSphereParams(float *param) |
| 355 |
{} |
| 356 |
|
| 357 |
bool Sphere::LeastSquaresFit(const PointCloud &pc, |
| 358 |
MiscLib::Vector< size_t >::const_iterator begin, |
| 359 |
MiscLib::Vector< size_t >::const_iterator end) |
| 360 |
{ |
| 361 |
bool retVal = LeastSquaresFit(GfxTL::IndexIterate(begin, pc.begin()), |
| 362 |
GfxTL::IndexIterate(end, pc.begin())); |
| 363 |
return retVal; |
| 364 |
} |
| 365 |
|
| 366 |
bool Sphere::Interpolate(const MiscLib::Vector< Sphere > &spheres, |
| 367 |
const MiscLib::Vector< float > &weights, Sphere *is) |
| 368 |
{ |
| 369 |
Vec3f center(0, 0, 0); |
| 370 |
float radius = 0; |
| 371 |
for(size_t i = 0; i < spheres.size(); ++i) |
| 372 |
{ |
| 373 |
center += weights[i] * spheres[i].Center(); |
| 374 |
radius += weights[i] * spheres[i].Radius(); |
| 375 |
} |
| 376 |
is->Center(center); |
| 377 |
is->Radius(radius); |
| 378 |
return true; |
| 379 |
} |
| 380 |
|
| 381 |
void Sphere::Serialize(bool binary, std::ostream *o) const |
| 382 |
{ |
| 383 |
if(binary) |
| 384 |
{ |
| 385 |
o->write((const char *)&m_center, sizeof(m_center)); |
| 386 |
o->write((const char *)&m_radius, sizeof(m_radius)); |
| 387 |
} |
| 388 |
else |
| 389 |
{ |
| 390 |
(*o) << m_center[0] << " " << m_center[1] << " " << m_center[2] << " " |
| 391 |
<< m_radius << " "; |
| 392 |
} |
| 393 |
} |
| 394 |
|
| 395 |
size_t Sphere::SerializedSize() |
| 396 |
{ |
| 397 |
return sizeof(Vec3f) |
| 398 |
+ sizeof(float); |
| 399 |
} |
| 400 |
|
| 401 |
size_t Sphere::SerializedFloatSize() |
| 402 |
{ |
| 403 |
return 4; |
| 404 |
} |
| 405 |
|
| 406 |
void Sphere::Serialize(FILE *o) const |
| 407 |
{ |
| 408 |
fwrite(&m_center, sizeof(m_center), 1, o); |
| 409 |
fwrite(&m_radius, sizeof(m_radius), 1, o); |
| 410 |
} |
| 411 |
|
| 412 |
void Sphere::Serialize(float *array) const |
| 413 |
{ |
| 414 |
for(int i=0;i<3;i++){ |
| 415 |
array[i] = m_center[i]; |
| 416 |
} |
| 417 |
array[3] = m_radius; |
| 418 |
} |
| 419 |
|
| 420 |
|
| 421 |
void Sphere::Transform(float scale, const Vec3f &translate) |
| 422 |
{ |
| 423 |
m_center *= scale; |
| 424 |
m_center += translate; |
| 425 |
m_radius *= scale; |
| 426 |
} |
| 427 |
|
| 428 |
SphereAsSquaresParametrization::SphereAsSquaresParametrization(const Sphere &sphere, |
| 429 |
const Vec3f &planeNormal) |
| 430 |
: m_sphere(sphere) |
| 431 |
, m_planeNormal(planeNormal) |
| 432 |
, m_hcs(GfxTL::Vector3Df(planeNormal)) |
| 433 |
{} |
| 434 |
|
| 435 |
void SphereAsSquaresParametrization::Init(const Sphere &sphere, |
| 436 |
const Vec3f &planeNormal) |
| 437 |
{ |
| 438 |
m_sphere = sphere; |
| 439 |
m_planeNormal = planeNormal; |
| 440 |
m_hcs.FromNormal(planeNormal[0], planeNormal[1], planeNormal[2]); |
| 441 |
} |
| 442 |
|
| 443 |
float SphereAsSquaresParametrization::Parameters(const Vec3f &p, |
| 444 |
std::pair< float, float > *param) const |
| 445 |
{ |
| 446 |
// convert to hemisphere coordinates |
| 447 |
Vec3f s = p - m_sphere.Center(); |
| 448 |
s.normalize(); |
| 449 |
Vec3f hs; |
| 450 |
hs[0] = s.dot(m_hcs[0].Data()); |
| 451 |
hs[1] = s.dot(m_hcs[1].Data()); |
| 452 |
hs[2] = s.dot(m_planeNormal); |
| 453 |
float ret = hs[2]; |
| 454 |
hs[2] = fabs(hs[2]); |
| 455 |
std::pair< float, float > inDisk; |
| 456 |
Hemisphere2Disk(hs, &inDisk); |
| 457 |
Disk2Square(inDisk, param); |
| 458 |
return ret; |
| 459 |
} |
| 460 |
|
| 461 |
bool SphereAsSquaresParametrization::InSpace( |
| 462 |
const std::pair< float, float > ¶m, bool lower, Vec3f *p) const |
| 463 |
{ |
| 464 |
if(param.first < -0.1 || param.first > 1.1 |
| 465 |
|| param.second < -0.1 || param.second > 1.1) |
| 466 |
return false; |
| 467 |
std::pair< float, float > clampedParam; |
| 468 |
clampedParam.first = GfxTL::Math< float >::Clamp(param.first, 0, 1); |
| 469 |
clampedParam.second = GfxTL::Math< float >::Clamp(param.second, 0, 1); |
| 470 |
std::pair< float, float > inDisk; |
| 471 |
Square2Disk(clampedParam, &inDisk); |
| 472 |
Vec3f s; |
| 473 |
Disk2Hemisphere(inDisk, &s); |
| 474 |
*p = Vec3f((s[0] * m_hcs[0] + s[1] * m_hcs[1] + |
| 475 |
GfxTL::Vector3Df((lower? -1 : 1) * s[2] * m_planeNormal)).Data()); |
| 476 |
*p *= m_sphere.Radius(); |
| 477 |
*p += m_sphere.Center(); |
| 478 |
return true; |
| 479 |
} |
| 480 |
|
| 481 |
bool SphereAsSquaresParametrization::InSpace( |
| 482 |
const std::pair< float, float > ¶m, bool lower, Vec3f *p, |
| 483 |
Vec3f *n) const |
| 484 |
{ |
| 485 |
if(param.first < -0.1 || param.first > 1.1 |
| 486 |
|| param.second < -0.1 || param.second > 1.1) |
| 487 |
return false; |
| 488 |
std::pair< float, float > clampedParam; |
| 489 |
clampedParam.first = GfxTL::Math< float >::Clamp(param.first, 0, 1); |
| 490 |
clampedParam.second = GfxTL::Math< float >::Clamp(param.second, 0, 1); |
| 491 |
std::pair< float, float > inDisk; |
| 492 |
Square2Disk(clampedParam, &inDisk); |
| 493 |
Vec3f s; |
| 494 |
Disk2Hemisphere(inDisk, &s); |
| 495 |
if(lower) |
| 496 |
s[2] *= -1; |
| 497 |
*n = Vec3f((s[0] * m_hcs[0] + s[1] * m_hcs[1] + |
| 498 |
GfxTL::Vector3Df(s[2] * m_planeNormal)).Data()); |
| 499 |
*p = m_sphere.Radius() * (*n); |
| 500 |
*p += m_sphere.Center(); |
| 501 |
return true; |
| 502 |
} |
| 503 |
|
| 504 |
void SphereAsSquaresParametrization::Transform( |
| 505 |
const GfxTL::MatrixXX< 3, 3, float > &rot, const GfxTL::Vector3Df &trans) |
| 506 |
{ |
| 507 |
m_sphere = Sphere(Vec3f((rot * GfxTL::Vector3Df(m_sphere.Center()) |
| 508 |
+ trans).Data()), m_sphere.Radius()); |
| 509 |
m_planeNormal = Vec3f((rot * GfxTL::Vector3Df(m_planeNormal)).Data()); |
| 510 |
m_hcs[0] = rot * m_hcs[0]; |
| 511 |
m_hcs[1] = rot * m_hcs[1]; |
| 512 |
} |
| 513 |
|
| 514 |
void SphereAsSquaresParametrization::HyperplaneCoordinateSystem( Vec3f* hcs0, Vec3f* hcs1, Vec3f* hcs2 ) const |
| 515 |
{ |
| 516 |
hcs0->setValue( m_hcs[0] ); |
| 517 |
hcs1->setValue( m_hcs[1] ); |
| 518 |
hcs2->setValue( m_hcs[2] ); |
| 519 |
} |
| 520 |
|
| 521 |
|
| 522 |
void SphereAsSquaresParametrization::Hemisphere2Disk(const Vec3f &p, |
| 523 |
std::pair< float, float > *inDisk) const |
| 524 |
{ |
| 525 |
inDisk->first = std::sqrt(1 - p[2]); |
| 526 |
inDisk->second = std::atan2(p[1], p[0]); |
| 527 |
} |
| 528 |
|
| 529 |
void SphereAsSquaresParametrization::Disk2Square( |
| 530 |
const std::pair< float, float > &inDisk, |
| 531 |
std::pair< float, float > *inSquare) const |
| 532 |
{ |
| 533 |
float r = inDisk.first; |
| 534 |
float phi = inDisk.second; |
| 535 |
float a, b; |
| 536 |
|
| 537 |
if(phi < float(-M_PI / 4.0)) |
| 538 |
phi += float(2 * M_PI); |
| 539 |
|
| 540 |
if(phi < float(M_PI / 4.0)) |
| 541 |
{ |
| 542 |
a = r; |
| 543 |
b = phi * a / float(M_PI / 4.0); |
| 544 |
} |
| 545 |
else if(phi < float( 3 * M_PI / 4.0)) |
| 546 |
{ |
| 547 |
b = r; |
| 548 |
a = -(phi - float(M_PI / 2.0))* b / float(M_PI / 4.0); |
| 549 |
} |
| 550 |
else if(phi < float(5 * M_PI / 4.0)) |
| 551 |
{ |
| 552 |
a = -r; |
| 553 |
b = (phi - float(M_PI))*a / float(M_PI / 4.0); |
| 554 |
} |
| 555 |
else |
| 556 |
{ |
| 557 |
b = -r; |
| 558 |
a = -(phi - float(3 * M_PI / 2.0))* b /float(M_PI / 4.0); |
| 559 |
} |
| 560 |
|
| 561 |
inSquare->first = (a + float(1.0)) / float(2.0); |
| 562 |
inSquare->second = (b + float(1.0)) / float(2.0); |
| 563 |
} |
| 564 |
|
| 565 |
void SphereAsSquaresParametrization::Square2Disk( |
| 566 |
const std::pair< float, float > &inSquare, |
| 567 |
std::pair< float, float > *inDisk) const |
| 568 |
{ |
| 569 |
float phi, r; |
| 570 |
float a = 2 * inSquare.first - 1; |
| 571 |
float b = 2 * inSquare.second - 1; |
| 572 |
|
| 573 |
if(a > -b) |
| 574 |
{ |
| 575 |
if(a > b) |
| 576 |
{ |
| 577 |
r = a; |
| 578 |
phi = float(M_PI / 4.0) * (b / a); |
| 579 |
} |
| 580 |
else |
| 581 |
{ |
| 582 |
r = b; |
| 583 |
phi = float(M_PI / 4.0) * (2 - (a / b)); |
| 584 |
} |
| 585 |
} |
| 586 |
else |
| 587 |
{ |
| 588 |
if(a < b) |
| 589 |
{ |
| 590 |
r = -a; |
| 591 |
phi = float(M_PI / 4.0) * (4 + (b / a)); |
| 592 |
} |
| 593 |
else |
| 594 |
{ |
| 595 |
r = -b; |
| 596 |
if(b != 0) |
| 597 |
phi = float(M_PI / 4.0) * (6 - (a / b)); |
| 598 |
else |
| 599 |
phi = 0; |
| 600 |
} |
| 601 |
} |
| 602 |
|
| 603 |
inDisk->first = r; |
| 604 |
inDisk->second = phi; |
| 605 |
} |
| 606 |
|
| 607 |
void SphereAsSquaresParametrization::Disk2Hemisphere( |
| 608 |
const std::pair< float, float > &inDisk, Vec3f *p) const |
| 609 |
{ |
| 610 |
(*p)[0] = inDisk.first * std::sqrt(2 - inDisk.first * inDisk.first) |
| 611 |
* std::cos(inDisk.second); |
| 612 |
(*p)[1] = inDisk.first * std::sqrt(2 - inDisk.first * inDisk.first) |
| 613 |
* std::sin(inDisk.second); |
| 614 |
(*p)[2] = 1 - inDisk.first * inDisk.first; |
| 615 |
} |