00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #ifndef MILENA_APPS_MESH_SEGM_SKEL_MISC_HH
00031 # define MILENA_APPS_MESH_SEGM_SKEL_MISC_HH
00032
00033 # include <algorithm>
00034 # include <utility>
00035
00036 # include <mln/algebra/vec.hh>
00037 # include <mln/algebra/mat.hh>
00038
00039 # include <mln/norm/l2.hh>
00040
00041 # include <mln/data/fill.hh>
00042
00043
00050
00051 #ifndef likely
00052 # if !defined(__GNUC__) || (__GNUC__ == 2 && __GNUC_MINOR__ < 96)
00053 # define likely(x) (x)
00054 # define unlikely(x) (x)
00055 # else
00056 # define likely(x) (__builtin_expect((x), 1))
00057 # define unlikely(x) (__builtin_expect((x), 0))
00058 # endif
00059 #endif
00060
00061 namespace mln
00062 {
00063
00064 namespace algebra
00065 {
00066
00067
00068
00069
00070
00076 template <unsigned N, typename T>
00077 inline
00078 bool
00079 ldlt_decomp(mat<N, N, T>& A, vec<N, T>& rdiag)
00080 {
00081 vec<N - 1, T> v;
00082 for (unsigned i = 0; i < N; ++i)
00083 {
00084 for (unsigned k = 0; k < i; ++k)
00085 v[k] = A(i, k) * rdiag[k];
00086 for (unsigned j = i; j < N; ++j)
00087 {
00088 T sum = A(i, j);
00089 for (unsigned k = 0; k < i; k++)
00090 sum -= v[k] * A(j, k);
00091 if (i == j)
00092 {
00093 if (unlikely(sum <= T(0)))
00094 return false;
00095 rdiag[i] = T(1) / sum;
00096 }
00097 else
00098 A(j, i) = sum;
00099 }
00100 }
00101 return true;
00102 }
00103
00104
00105
00106
00107
00109 template <unsigned N, typename T>
00110 inline
00111 void
00112 ldlt_solve(const mat<N, N, T>& A, const vec<N, T>& rdiag,
00113 const vec<N, T>& B, vec<N, T>& x)
00114 {
00115 for (unsigned i = 0; i < N; ++i)
00116 {
00117 T sum = B[i];
00118 for (unsigned k = 0; k < i; ++k)
00119 sum -= A(i, k) * x[k];
00120 x[i] = sum * rdiag[i];
00121 }
00122 for (int i = N - 1; i >= 0; --i)
00123 {
00124 T sum = 0;
00125 for (unsigned k = i + 1; k < N; ++k)
00126 sum += A(k, i) * x[k];
00127 x[i] -= sum * rdiag[i];
00128 }
00129 }
00130
00131 }
00132
00133 }
00134
00135
00136
00137
00138
00139
00140 namespace mln
00141 {
00142
00143 namespace geom
00144 {
00145
00146
00147
00148
00149
00159 inline
00160 complex_image< 2, mln::space_2complex_geometry, algebra::vec<3, float> >
00161 mesh_normal(const p_complex<2, space_2complex_geometry>& mesh)
00162 {
00163
00164 static const unsigned D = 2;
00165 typedef space_2complex_geometry G;
00166 typedef algebra::vec<3, float> vec3f;
00167 typedef complex_image< D, G, vec3f > normal_t;
00168
00169 normal_t normal(mesh);
00170 data::fill(normal, literal::zero);
00171
00172 mln::p_n_faces_fwd_piter<D, G> f(mesh, 2);
00173
00174
00175 typedef mln::complex_m_face_neighborhood<D, G> adj_vertices_nbh_t;
00176 adj_vertices_nbh_t adj_vertices_nbh;
00177 mln_niter_(adj_vertices_nbh_t) adj_v(adj_vertices_nbh, f);
00178
00179
00180
00181
00182 adj_v.iter().set_m(0);
00183
00184
00185 for_all(f)
00186 {
00188 std::vector<mln_psite_(normal_t)> p;
00189 p.reserve(3);
00190 for_all(adj_v)
00191 p.push_back(adj_v);
00193 mln_assertion(p.size() == 3);
00194
00195
00196
00197
00198
00199
00200
00201
00202 vec3f a =
00203 p[0].to_site().front().to_vec() - p[1].to_site().front().to_vec();
00204 vec3f b =
00205 p[1].to_site().front().to_vec() - p[2].to_site().front().to_vec();
00206 vec3f c =
00207 p[2].to_site().front().to_vec() - p[0].to_site().front().to_vec();
00208
00209
00210 float l2a = norm::sqr_l2(a);
00211 float l2b = norm::sqr_l2(b);
00212 float l2c = norm::sqr_l2(c);
00213 vec3f face_normal = algebra::vprod(a, b);
00214
00215 normal(p[0]) += face_normal * (1.0f / (l2a * l2c));
00216 normal(p[1]) += face_normal * (1.0f / (l2b * l2a));
00217 normal(p[2]) += face_normal * (1.0f / (l2c * l2b));
00218 }
00219
00220
00221
00222 mln::p_n_faces_fwd_piter<D, G> v(mesh, 0);
00223 for_all(v)
00224 normal(v).normalize();
00225
00226 return normal;
00227 }
00228
00229
00230
00231
00244 inline
00245 std::pair<
00246 complex_image< 2, mln::space_2complex_geometry, algebra::vec<3, float> >,
00247 complex_image< 2, mln::space_2complex_geometry, float >
00248 >
00249 mesh_corner_point_area(const p_complex<2, space_2complex_geometry>& mesh)
00250 {
00251
00252 static const unsigned D = 2;
00253 typedef space_2complex_geometry G;
00254 typedef algebra::vec<3, float> vec3f;
00255
00256
00257 typedef complex_image< D, G, vec3f > corner_area_t;
00258
00259 typedef complex_image< D, G, float > point_area_t;
00260
00261 typedef std::pair<corner_area_t, point_area_t> output_t;
00262
00263
00264 output_t output(mesh, mesh);
00265 corner_area_t& corner_area = output.first;
00266 point_area_t& point_area = output.second;
00267 data::fill(corner_area, literal::zero);
00268 data::fill(point_area, literal::zero);
00269
00270 mln::p_n_faces_fwd_piter<D, G> f(mesh, 2);
00271
00272
00273 typedef mln::complex_m_face_neighborhood<D, G> adj_vertices_nbh_t;
00274 adj_vertices_nbh_t adj_vertices_nbh;
00275 mln_niter_(adj_vertices_nbh_t) adj_v(adj_vertices_nbh, f);
00276
00277
00278
00279
00280 adj_v.iter().set_m(0);
00281
00282 for_all(f)
00283 {
00285 std::vector<mln_psite_(corner_area_t)> p;
00286 p.reserve(3);
00287 for_all(adj_v)
00288 p.push_back(adj_v);
00290 mln_assertion(p.size() == 3);
00291
00292
00293 algebra::vec<3, vec3f> e;
00294
00295 e.set
00296 (p[2].to_site().front().to_vec() - p[1].to_site().front().to_vec(),
00297 p[0].to_site().front().to_vec() - p[2].to_site().front().to_vec(),
00298 p[1].to_site().front().to_vec() - p[0].to_site().front().to_vec());
00299
00300
00301 float area = 0.5f * norm::l2(algebra::vprod(e[0], e[1]));
00302 vec3f sqr_norm;
00303 sqr_norm.set(norm::sqr_l2(e[0]),
00304 norm::sqr_l2(e[1]),
00305 norm::sqr_l2(e[2]));
00306 vec3f edge_weight;
00307 edge_weight.set
00308 (sqr_norm[0] * (sqr_norm[1] + sqr_norm[2] - sqr_norm[0]),
00309 sqr_norm[1] * (sqr_norm[2] + sqr_norm[0] - sqr_norm[1]),
00310 sqr_norm[2] * (sqr_norm[0] + sqr_norm[1] - sqr_norm[2]));
00311
00312 if (edge_weight[0] <= 0.0f)
00313 {
00314 corner_area(f)[1] = -0.25f * sqr_norm[2] * area / (e[0] * e[2]);
00315 corner_area(f)[2] = -0.25f * sqr_norm[1] * area / (e[0] * e[1]);
00316 corner_area(f)[0] = area - corner_area(f)[1] - corner_area(f)[2];
00317 }
00318 else if (edge_weight[1] <= 0.0f)
00319 {
00320 corner_area(f)[2] = -0.25f * sqr_norm[0] * area / (e[1] * e[0]);
00321 corner_area(f)[0] = -0.25f * sqr_norm[2] * area / (e[1] * e[2]);
00322 corner_area(f)[1] = area - corner_area(f)[2] - corner_area(f)[0];
00323 }
00324 else if (edge_weight[2] <= 0.0f)
00325 {
00326 corner_area(f)[0] = -0.25f * sqr_norm[1] * area / (e[2] * e[1]);
00327 corner_area(f)[1] = -0.25f * sqr_norm[0] * area / (e[2] * e[0]);
00328 corner_area(f)[2] = area - corner_area(f)[0] - corner_area(f)[1];
00329 }
00330 else
00331 {
00332 float ewscale =
00333 0.5f * area / (edge_weight[0] + edge_weight[1] + edge_weight[2]);
00334 for (int i = 0; i < 3; ++i)
00335 corner_area(f)[i] =
00336 ewscale * (edge_weight[(i+1)%3] + edge_weight[(i+2)%3]);
00337 }
00338
00339 for (int i = 0; i < 3; ++i)
00340 point_area(p[i]) += corner_area(f)[i];
00341 }
00342
00343 return output;
00344 }
00345
00346
00347 namespace internal
00348 {
00349
00365 static inline unsigned next(unsigned i) { return (i + 1) % 3; }
00366 static inline unsigned prev(unsigned i) { return (i - 1) % 3; }
00370
00371 inline
00372 void
00373 rot_coord_sys(const algebra::vec<3, float> &old_u,
00374 const algebra::vec<3, float> &old_v,
00375 const algebra::vec<3, float> &new_norm,
00376 algebra::vec<3, float> &new_u,
00377 algebra::vec<3, float> &new_v)
00378 {
00379 new_u = old_u;
00380 new_v = old_v;
00381 algebra::vec<3, float> old_norm = vprod(old_u, old_v);
00382 float ndot = old_norm * new_norm;
00383 if (unlikely(ndot <= -1.0f))
00384 {
00385 new_u = -new_u;
00386 new_v = -new_v;
00387 return;
00388 }
00389 algebra::vec<3, float> perp_old = new_norm - ndot * old_norm;
00390 algebra::vec<3, float> dperp =
00391 1.0f / (1 + ndot) * (old_norm + new_norm);
00392 new_u -= dperp * (new_u * perp_old);
00393 new_v -= dperp * (new_v * perp_old);
00394 }
00395
00396
00397
00398
00399
00400
00401
00402 inline
00403 void
00404 proj_curv(const algebra::vec<3, float>& old_u,
00405 const algebra::vec<3, float>& old_v,
00406 float old_ku, float old_kuv, float old_kv,
00407 const algebra::vec<3, float>& new_u,
00408 const algebra::vec<3, float>& new_v,
00409 float& new_ku, float& new_kuv, float& new_kv)
00410 {
00411 algebra::vec<3, float> r_new_u, r_new_v;
00412 rot_coord_sys(new_u, new_v, vprod(old_u, old_v), r_new_u, r_new_v);
00413
00414 float u1 = r_new_u * old_u;
00415 float v1 = r_new_u * old_v;
00416 float u2 = r_new_v * old_u;
00417 float v2 = r_new_v * old_v;
00418 new_ku = old_ku * u1*u1 + old_kuv * (2.0f * u1*v1) + old_kv * v1*v1;
00419 new_kuv = old_ku * u1*u2 + old_kuv * (u1*v2 + u2*v1) + old_kv * v1*v2;
00420 new_kv = old_ku * u2*u2 + old_kuv * (2.0f * u2*v2) + old_kv * v2*v2;
00421 }
00422
00426 inline
00427 void
00428 diagonalize_curv(const algebra::vec<3, float>& old_u,
00429 const algebra::vec<3, float>& old_v,
00430 float ku, float kuv, float kv,
00431 const algebra::vec<3, float>& new_norm,
00432 algebra::vec<3, float>& pdir1,
00433 algebra::vec<3, float>& pdir2,
00434 float& k1, float& k2)
00435 {
00436 algebra::vec<3, float> r_old_u, r_old_v;
00437 rot_coord_sys(old_u, old_v, new_norm, r_old_u, r_old_v);
00438
00439 float c = 1, s = 0, tt = 0;
00440 if (likely(kuv != 0.0f))
00441 {
00442
00443 float h = 0.5f * (kv - ku) / kuv;
00444 tt = (h < 0.0f) ?
00445 1.0f / (h - sqrt(1.0f + h*h)) :
00446 1.0f / (h + sqrt(1.0f + h*h));
00447 c = 1.0f / sqrt(1.0f + tt*tt);
00448 s = tt * c;
00449 }
00450
00451 k1 = ku - tt * kuv;
00452 k2 = kv + tt * kuv;
00453
00454 if (fabs(k1) >= fabs(k2))
00455 pdir1 = c*r_old_u - s*r_old_v;
00456 else
00457 {
00458 std::swap(k1, k2);
00459 pdir1 = s*r_old_u + c*r_old_v;
00460 }
00461 pdir2 = vprod(new_norm, pdir1);
00462 }
00463
00464 }
00465
00466
00478
00479
00480
00481 inline
00482 std::pair<
00483 complex_image< 2, mln::space_2complex_geometry, float >,
00484 complex_image< 2, mln::space_2complex_geometry, float >
00485 >
00486 mesh_curvature(const p_complex<2, space_2complex_geometry>& mesh)
00487 {
00488
00489 static const unsigned D = 2;
00490 typedef space_2complex_geometry G;
00491 typedef algebra::vec<3, float> vec3f;
00492 typedef algebra::mat<3, 3, float> mat3f;
00493
00494 typedef complex_image< D, G, vec3f > corner_area_t;
00495 typedef complex_image< D, G, float > point_area_t;
00496
00497
00498 typedef complex_image< 2, mln::space_2complex_geometry, vec3f > normal_t;
00499 normal_t normal = mesh_normal(mesh);
00500
00501
00502 typedef complex_image< D, G, vec3f > corner_area_t;
00503 typedef complex_image< D, G, float > point_area_t;
00504 typedef std::pair<corner_area_t, point_area_t> corner_point_area_t;
00505 corner_point_area_t corner_point_area = mesh_corner_point_area(mesh);
00506
00507 corner_area_t& corner_area = corner_point_area.first;
00508 point_area_t& point_area = corner_point_area.second;
00509
00510
00511 typedef complex_image< 2, mln::space_2complex_geometry, float > curv_t;
00512 typedef std::pair<curv_t, curv_t> output_t;
00513 output_t output(mesh, mesh);
00514 curv_t& curv1 = output.first;
00515 curv_t& curv2 = output.second;
00516
00517 complex_image< 2, mln::space_2complex_geometry, float > curv12(mesh);
00518
00519 complex_image< 2, mln::space_2complex_geometry, vec3f > pdir1(mesh);
00520 complex_image< 2, mln::space_2complex_geometry, vec3f > pdir2(mesh);
00521
00522
00523
00524
00525
00526 mln::p_n_faces_fwd_piter<D, G> f(mesh, 2);
00527
00528
00529 typedef mln::complex_m_face_neighborhood<D, G> adj_vertices_nbh_t;
00530 adj_vertices_nbh_t adj_vertices_nbh;
00531 mln_niter_(adj_vertices_nbh_t) adj_v(adj_vertices_nbh, f);
00532
00533
00534
00535
00536 adj_v.iter().set_m(0);
00537
00538 for_all(f)
00539 {
00541 std::vector<mln_psite_(curv_t)> p;
00542 p.reserve(3);
00543 for_all(adj_v)
00544 p.push_back(adj_v);
00546 mln_assertion(p.size() == 3);
00547
00548
00549 pdir1(p[0]) =
00550 p[1].to_site().front().to_vec() - p[0].to_site().front().to_vec();
00551 pdir1(p[1]) =
00552 p[2].to_site().front().to_vec() - p[1].to_site().front().to_vec();
00553 pdir1(p[2]) =
00554 p[0].to_site().front().to_vec() - p[2].to_site().front().to_vec();
00555 }
00556
00557 mln::p_n_faces_fwd_piter<D, G> v(mesh, 0);
00558 for_all(v)
00559 {
00560 pdir1(v) = algebra::vprod(pdir1(v), normal(v));
00561 pdir1(v).normalize();
00562 pdir2(v) = algebra::vprod(normal(v), pdir1(v));
00563 }
00564
00565
00566
00567
00568
00569 for_all(f)
00570 {
00571 std::vector<mln_psite_(curv_t)> p;
00572 p.reserve(3);
00573 for_all(adj_v)
00574 p.push_back(adj_v);
00575 mln_assertion(p.size() == 3);
00576
00577
00578 algebra::vec<3, vec3f> e;
00579
00580 e.set
00581 (p[2].to_site().front().to_vec() - p[1].to_site().front().to_vec(),
00582 p[0].to_site().front().to_vec() - p[2].to_site().front().to_vec(),
00583 p[1].to_site().front().to_vec() - p[0].to_site().front().to_vec());
00584
00585
00586 vec3f t = e[0];
00587 t.normalize();
00588 vec3f n = algebra::vprod(e[0], e[1]);
00589
00590
00591 vec3f b = algebra::vprod(n, t);
00592 b.normalize();
00593
00594
00595 vec3f m = literal::zero;
00596 mat3f w = literal::zero;
00597 for (int j = 0; j < 3; ++j)
00598 {
00599 float u = e[j] * t;
00600 float v = e[j] * b;
00601 w(0, 0) += u * u;
00602 w(0, 1) += u * v;
00603
00604
00605
00606
00607
00608
00609
00610 w(2, 2) += v * v;
00611 vec3f dn =
00612 normal(p[internal::prev(j)]) - normal(p[internal::next(j)]);
00613 float dnu = dn * t;
00614 float dnv = dn * b;
00615 m[0] += dnu*u;
00616 m[1] += dnu*v + dnv*u;
00617 m[2] += dnv*v;
00618 }
00619 w(1, 1) = w(0, 0) + w(2, 2);
00620 w(1, 2) = w(0, 1);
00621
00622
00623 vec3f diag;
00624 #ifndef NDEBUG
00625 bool ldlt_decomp_sucess_p = algebra::ldlt_decomp(w, diag);
00626 #endif // ! NDEBUG
00627 mln_assertion(ldlt_decomp_sucess_p);
00628 algebra::ldlt_solve(w, diag, m, m);
00629
00630
00631 for (int j = 0; j < 3; ++j)
00632 {
00633 float c1, c12, c2;
00634 internal::proj_curv(t, b, m[0], m[1], m[2],
00635 pdir1(p[j]), pdir2(p[j]), c1, c12, c2);
00636 float wt = corner_area(f)[j] / point_area(p[j]);
00637 curv1(p[j]) += wt * c1;
00638 curv12(p[j]) += wt * c12;
00639 curv2(p[j]) += wt * c2;
00640 }
00641 }
00642
00643
00644
00645
00646
00647 for_all(v)
00648 internal::diagonalize_curv(pdir1(v), pdir2(v),
00649 curv1(v), curv12(v), curv2(v),
00650 normal(v), pdir1(v), pdir2(v),
00651 curv1(v), curv2(v));
00652
00653 return output;
00654 }
00655
00656 }
00657
00658 }
00659
00660 #endif // ! MILENA_APPS_MESH_SEGM_SKEL_MISC_HH