GetFEM  5.4.2
getfem_contact_and_friction_common.cc
1 /*===========================================================================
2 
3  Copyright (C) 2013-2020 Yves Renard, Konstantinos Poulios.
4 
5  This file is a part of GetFEM
6 
7  GetFEM is free software; you can redistribute it and/or modify it
8  under the terms of the GNU Lesser General Public License as published
9  by the Free Software Foundation; either version 3 of the License, or
10  (at your option) any later version along with the GCC Runtime Library
11  Exception either version 3.1 or (at your option) any later version.
12  This program is distributed in the hope that it will be useful, but
13  WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
14  or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
15  License and GCC Runtime Library Exception for more details.
16  You should have received a copy of the GNU Lesser General Public License
17  along with this program; if not, write to the Free Software Foundation,
18  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA.
19 
20 ===========================================================================*/
21 
24 #ifndef _WIN32
25 #include <unistd.h>
26 #endif
27 
28 namespace getfem {
29 
30  bool boundary_has_fem_nodes(bool slave_flag, int nodes_mode) {
31  return (slave_flag && nodes_mode) ||
32  (!slave_flag && nodes_mode == 2);
33  }
34 
35  void compute_normal(const fem_interpolation_context &ctx,
36  size_type face, bool in_reference_conf,
37  const model_real_plain_vector &coeff,
38  base_node &n0, base_node &n,
39  base_matrix &grad) {
40  n0 = bgeot::compute_normal(ctx, face);
41  if (in_reference_conf) {
42  n = n0;
43  } else {
44  ctx.pf()->interpolation_grad(ctx, coeff, grad, dim_type(ctx.N()));
45  gmm::add(gmm::identity_matrix(), grad);
46  scalar_type J = bgeot::lu_inverse(&(*(grad.begin())), ctx.N());
47  if (J <= scalar_type(0)) {GMM_WARNING1("Inverted element !" << J);
48  gmm::mult(gmm::transposed(grad), n0, n);
49  gmm::scale(n, gmm::sgn(J));
50  //cout<< "n0 =" << n0 << "n =" <<n<< endl;
51  //cout<< "x0=" << ctx.xref() << "x =" <<ctx.xreal()<< endl;
52  }
53  else {
54  gmm::mult(gmm::transposed(grad), n0, n);
55  gmm::scale(n, gmm::sgn(J));} // Test
56  }
57  }
58 
59  //=========================================================================
60  //
61  // Structure which store the contact boundaries, rigid obstacles and
62  // computes the contact pairs in large sliding/large deformation.
63  //
64  //=========================================================================
65 
66  size_type multi_contact_frame::add_U
67  (const model_real_plain_vector *U, const std::string &name,
68  const model_real_plain_vector *w, const std::string &wname) {
69  if (!U) return size_type(-1);
70  size_type i = 0;
71  for (; i < Us.size(); ++i) if (Us[i] == U) return i;
72  Us.push_back(U);
73  Ws.push_back(w);
74  Unames.push_back(name);
75  Wnames.push_back(wname);
76  ext_Us.resize(Us.size());
77  ext_Ws.resize(Us.size());
78  return i;
79  }
80 
81  size_type multi_contact_frame::add_lambda
82  (const model_real_plain_vector *lambda, const std::string &name) {
83  if (!lambda) return size_type(-1);
84  size_type i = 0;
85  for (; i < lambdas.size(); ++i) if (lambdas[i] == lambda) return i;
86  lambdas.push_back(lambda);
87  lambdanames.push_back(name);
88  ext_lambdas.resize(lambdas.size());
89  return i;
90  }
91 
92  void multi_contact_frame::extend_vectors() {
93  dal::bit_vector iU, ilambda;
94  for (size_type i = 0; i < contact_boundaries.size(); ++i) {
95  size_type ind_U = contact_boundaries[i].ind_U;
96  if (!(iU[ind_U])) {
97  const mesh_fem &mf = *(contact_boundaries[i].mfu);
98  gmm::resize(ext_Us[ind_U], mf.nb_basic_dof());
99  mf.extend_vector(*(Us[ind_U]), ext_Us[ind_U]);
100  if (Ws[ind_U]) {
101  gmm::resize(ext_Ws[ind_U], mf.nb_basic_dof());
102  mf.extend_vector(*(Ws[ind_U]), ext_Ws[ind_U]);
103  } else gmm::resize(ext_Ws[ind_U], 0);
104  iU.add(ind_U);
105  }
106  size_type ind_lambda = contact_boundaries[i].ind_lambda;
107  if (ind_lambda != size_type(-1) && !(ilambda[ind_lambda])) {
108  const mesh_fem &mf = *(contact_boundaries[i].mflambda);
109  gmm::resize(ext_lambdas[ind_lambda], mf.nb_basic_dof());
110  mf.extend_vector(*(lambdas[ind_lambda]), ext_lambdas[ind_lambda]);
111  ilambda.add(ind_lambda);
112  }
113  }
114  }
115 
116  void multi_contact_frame::normal_cone_simplification() {
117  if (nodes_mode) {
118  scalar_type threshold = ::cos(cut_angle);
119  for (size_type i = 0; i < boundary_points_info.size(); ++i) {
120  normal_cone &nc = boundary_points_info[i].normals;
121  if (nc.size() > 1) {
122  base_small_vector n_mean = nc[0];
123  for (size_type j = 1; j < nc.size(); ++j) n_mean += nc[j];
124  scalar_type nn_mean = gmm::vect_norm2(n_mean);
125  GMM_ASSERT1(nn_mean != scalar_type(0), "oupssss");
126  if (nn_mean != scalar_type(0)) {
127  gmm::scale(n_mean, scalar_type(1)/nn_mean);
128  bool reduce = true;
129  for (size_type j = 0; j < nc.size(); ++j)
130  if (gmm::vect_sp(n_mean, nc[j]) < threshold)
131  { reduce = false; break; }
132  if (reduce) {
133  boundary_points_info[i].normals = normal_cone(n_mean);
134  }
135  }
136  }
137  }
138  }
139  }
140 
141  bool multi_contact_frame::test_normal_cones_compatibility
142  (const normal_cone &nc1, const normal_cone &nc2) {
143  for (size_type i = 0; i < nc1.size(); ++i)
144  for (size_type j = 0; j < nc2.size(); ++j)
145  if (gmm::vect_sp(nc1[i], nc2[j]) < scalar_type(0))
146  return true;
147  return false;
148  }
149 
150  bool multi_contact_frame::test_normal_cones_compatibility
151  (const base_small_vector &n, const normal_cone &nc2) {
152  for (size_type j = 0; j < nc2.size(); ++j)
153  if (gmm::vect_sp(n, nc2[j]) < scalar_type(0))
154  return true;
155  return false;
156  }
157 
158  bool multi_contact_frame::are_dof_linked(size_type ib1, size_type idof1,
159  size_type ib2, size_type idof2) {
160  const mesh_fem &mf1 = mfdisp_of_boundary(ib1);
161  const mesh_fem &mf2 = mfdisp_of_boundary(ib2);
162  if ( &(mf1.linked_mesh()) != &(mf2.linked_mesh())) return false;
163  GMM_ASSERT1(!(mf1.is_reduced()) && !(mf2.is_reduced()),
164  "Nodal strategy can only be applied for non reduced fems");
165  const mesh::ind_cv_ct &ic1 = mf1.convex_to_basic_dof(idof1);
166  const mesh::ind_cv_ct &ic2 = mf2.convex_to_basic_dof(idof2);
167  bool lk = false;
168  for (size_type i = 0; i < ic1.size(); ++i) aux_dof_cv.add(ic1[i]);
169  for (size_type i = 0; i < ic2.size(); ++i)
170  if (aux_dof_cv.is_in(ic2[i])) { lk = true; break; }
171  for (size_type i = 0; i < ic1.size(); ++i) aux_dof_cv.sup(ic1[i]);
172  return lk;
173  }
174 
175  bool multi_contact_frame::is_dof_linked(size_type ib1, size_type idof1,
176  size_type ib2, size_type cv) {
177  const mesh_fem &mf1 = mfdisp_of_boundary(ib1);
178  const mesh_fem &mf2 = mfdisp_of_boundary(ib2);
179  if ( &(mf1.linked_mesh()) != &(mf2.linked_mesh())) return false;
180  GMM_ASSERT1(!(mf1.is_reduced()) && !(mf2.is_reduced()),
181  "Nodal strategy can only be applied for non reduced fems");
182  const mesh::ind_cv_ct &ic1 = mf1.convex_to_basic_dof(idof1);
183  for (size_type i = 0; i < ic1.size(); ++i)
184  if (cv == ic1[i]) return true;
185  return false;
186  }
187 
188  void multi_contact_frame::add_potential_contact_face
189  (size_type ip, size_type ib, size_type ie, short_type iff) {
190  bool found = false;
191  std::vector<face_info> &sfi = potential_pairs[ip];
192  for (size_type k = 0; k < sfi.size(); ++k)
193  if (sfi[k].ind_boundary == ib &&
194  sfi[k].ind_element == ie &&
195  sfi[k].ind_face == iff) found = true;
196 
197  if (!found) sfi.push_back(face_info(ib, ie, iff));
198  }
199 
200  void multi_contact_frame::clear_aux_info() {
201  boundary_points = std::vector<base_node>();
202  boundary_points_info = std::vector<boundary_point>();
203  element_boxes.clear();
204  element_boxes_info = std::vector<influence_box>();
205  potential_pairs = std::vector<std::vector<face_info> >();
206  }
207 
208  multi_contact_frame::multi_contact_frame(size_type NN, scalar_type r_dist,
209  bool dela, bool selfc,
210  scalar_type cut_a,
211  bool rayt, int nmode, bool refc)
212  : N(NN), self_contact(selfc), ref_conf(refc), use_delaunay(dela),
213  nodes_mode(nmode), raytrace(rayt), release_distance(r_dist),
214  cut_angle(cut_a), EPS(1E-8), md(0), coordinates(N), pt(N) {
215  if (N > 0) coordinates[0] = "x";
216  if (N > 1) coordinates[1] = "y";
217  if (N > 2) coordinates[2] = "z";
218  if (N > 3) coordinates[3] = "w";
219  GMM_ASSERT1(N <= 4, "Complete the definition for contact in "
220  "dimension greater than 4");
221  }
222 
223  multi_contact_frame::multi_contact_frame(const model &mdd, size_type NN,
224  scalar_type r_dist,
225  bool dela, bool selfc,
226  scalar_type cut_a,
227  bool rayt, int nmode, bool refc)
228  : N(NN), self_contact(selfc), ref_conf(refc),
229  use_delaunay(dela), nodes_mode(nmode), raytrace(rayt),
230  release_distance(r_dist), cut_angle(cut_a), EPS(1E-8), md(&mdd),
231  coordinates(N), pt(N) {
232  if (N > 0) coordinates[0] = "x";
233  if (N > 1) coordinates[1] = "y";
234  if (N > 2) coordinates[2] = "z";
235  if (N > 3) coordinates[3] = "w";
236  GMM_ASSERT1(N <= 4, "Complete the definition for contact in "
237  "dimension greater than 4");
238  }
239 
240  size_type multi_contact_frame::add_obstacle(const std::string &obs) {
241  size_type ind = obstacles.size();
242  obstacles.push_back(obs);
243  obstacles_velocities.push_back("");
244  obstacles_gw.push_back(ga_workspace());
245  pt.resize(N); ptx.resize(1); pty.resize(1); ptz.resize(1); ptw.resize(1);
246  obstacles_gw.back().add_fixed_size_constant("X", pt);
247  if (N >= 4) obstacles_gw.back().add_fixed_size_constant("w", ptw);
248  if (N >= 3) obstacles_gw.back().add_fixed_size_constant("z", ptz);
249  if (N >= 2) obstacles_gw.back().add_fixed_size_constant("y", pty);
250  if (N >= 1) obstacles_gw.back().add_fixed_size_constant("x", ptx);
251  obstacles_f.push_back(ga_function(obstacles_gw.back(), obs));
252  obstacles_f.back().compile();
253  return ind;
254  }
255 
256 
257 
258  size_type multi_contact_frame::add_master_boundary
259  (const mesh_im &mim, const mesh_fem *mfu,
260  const model_real_plain_vector *U, size_type reg,
261  const mesh_fem *mflambda, const model_real_plain_vector *lambda,
262  const model_real_plain_vector *w,
263  const std::string &vvarname,
264  const std::string &mmultname, const std::string &wname) {
265  GMM_ASSERT1(mfu->linked_mesh().dim() == N,
266  "Mesh dimension is " << mfu->linked_mesh().dim()
267  << "should be " << N << ".");
268  GMM_ASSERT1(&(mfu->linked_mesh()) == &(mim.linked_mesh()),
269  "Integration and finite element are not on the same mesh !");
270  if (mflambda)
271  GMM_ASSERT1(&(mflambda->linked_mesh()) == &(mim.linked_mesh()),
272  "Integration and finite element are not on the same mesh !");
273  contact_boundary cb(reg, mfu, mim, add_U(U, vvarname, w, wname),
274  mflambda, add_lambda(lambda, mmultname));
275  contact_boundaries.push_back(cb);
276  return size_type(contact_boundaries.size() - 1);
277  }
278 
279  size_type multi_contact_frame::add_slave_boundary
280  (const mesh_im &mim, const mesh_fem *mfu,
281  const model_real_plain_vector *U, size_type reg,
282  const mesh_fem *mflambda, const model_real_plain_vector *lambda,
283  const model_real_plain_vector *w,
284  const std::string &vvarname,
285  const std::string &mmultname, const std::string &wname) {
286  size_type ind
287  = add_master_boundary(mim, mfu, U, reg, mflambda, lambda, w,
288  vvarname, mmultname, wname);
289  contact_boundaries[ind].slave = true;
290  return ind;
291  }
292 
293 
294  size_type multi_contact_frame::add_master_boundary
295  (const mesh_im &mim, size_type reg, const std::string &vvarname,
296  const std::string &mmultname, const std::string &wname) {
297  GMM_ASSERT1(md, "This multi contact frame object is not linked "
298  "to a model");
299  const mesh_fem *mfl(0);
300  const model_real_plain_vector *l(0);
301  if (mmultname.size()) {
302  mfl = &(md->mesh_fem_of_variable(mmultname));
303  l = &(md->real_variable(mmultname));
304  }
305  const model_real_plain_vector *w(0);
306  if (wname.compare(vvarname) == 0) {
307  GMM_ASSERT1(md->n_iter_of_variable(vvarname) > 1, "More than one "
308  "versions of the displacement variable were expected here");
309  w = &(md->real_variable(vvarname,1));
310  }
311  else if (wname.size()) {
312  GMM_ASSERT1(&(md->mesh_fem_of_variable(wname))
313  == &(md->mesh_fem_of_variable(vvarname)), "The previous "
314  "displacement should be defined on the same mesh_fem as the "
315  "current one");
316  w = &(md->real_variable(wname));
317  }
318  return add_master_boundary(mim, &(md->mesh_fem_of_variable(vvarname)),
319  &(md->real_variable(vvarname)), reg, mfl, l, w,
320  vvarname, mmultname, wname);
321  }
322 
323  size_type multi_contact_frame::add_slave_boundary
324  (const mesh_im &mim, size_type reg, const std::string &vvarname,
325  const std::string &mmultname, const std::string &wname) {
326  GMM_ASSERT1(md, "This multi contact frame object is not linked "
327  "to a model");
328  const mesh_fem *mfl(0);
329  const model_real_plain_vector *l(0);
330  if (mmultname.size()) {
331  mfl = &(md->mesh_fem_of_variable(mmultname));
332  l = &(md->real_variable(mmultname));
333  }
334  const model_real_plain_vector *w(0);
335  if (wname.compare(vvarname) == 0) {
336  GMM_ASSERT1(md->n_iter_of_variable(vvarname) > 1, "More than one "
337  "versions of the displacement variable were expected here");
338  w = &(md->real_variable(vvarname,1));
339  }
340  else if (wname.size()) {
341  GMM_ASSERT1(&(md->mesh_fem_of_variable(wname))
342  == &(md->mesh_fem_of_variable(vvarname)), "The previous "
343  "displacement should be defined on the same mesh_fem as the "
344  "current one");
345  w = &(md->real_variable(wname));
346  }
347  return add_slave_boundary(mim, &(md->mesh_fem_of_variable(vvarname)),
348  &(md->real_variable(vvarname)), reg, mfl, l, w,
349  vvarname, mmultname, wname);
350  }
351 
352 
353  void multi_contact_frame::compute_boundary_points(bool slave_only) {
354  fem_precomp_pool fppool;
355  base_matrix G;
356  model_real_plain_vector coeff;
357 
358  for (size_type i = 0; i < contact_boundaries.size(); ++i)
359  if (!slave_only || is_slave_boundary(i)) {
360  size_type bnum = region_of_boundary(i);
361  const mesh_fem &mfu = mfdisp_of_boundary(i);
362  const mesh_im &mim = mim_of_boundary(i);
363  const model_real_plain_vector &U = disp_of_boundary(i);
364  const mesh &m = mfu.linked_mesh();
365  bool on_fem_nodes =
366  boundary_has_fem_nodes(is_slave_boundary(i), nodes_mode);
367 
368  base_node val(N), bmin(N), bmax(N);
369  base_small_vector n0(N), n(N), n_mean(N);
370  base_matrix grad(N,N);
371  mesh_region region = m.region(bnum);
372  GMM_ASSERT1(mfu.get_qdim() == N, "Wrong mesh_fem qdim");
373 
374 
375  dal::bit_vector dof_already_interpolated;
376  std::vector<size_type> dof_ind(mfu.nb_basic_dof());
377  for (getfem::mr_visitor v(region,m); !v.finished(); ++v) {
378  size_type cv = v.cv();
379  bgeot::pgeometric_trans pgt = m.trans_of_convex(cv);
380  pfem pf_s = mfu.fem_of_element(cv);
381 
382  if (!ref_conf)
383  slice_vector_on_basic_dof_of_element(mfu, U, cv, coeff);
384  mfu.linked_mesh().points_of_convex(cv, G);
385 
386  pfem_precomp pfp(0);
387  size_type nbptf(0);
388  std::vector<size_type> indpt, indpfp;
389  if (on_fem_nodes) {
390  dim_type qqdim = mfu.get_qdim() / pf_s->target_dim();
391  pfp = fppool(pf_s, pf_s->node_tab(cv));
392  nbptf = pf_s->node_convex(cv).structure()->nb_points_of_face(v.f());
393  indpt.resize(nbptf); indpfp.resize(nbptf);
394  for (short_type ip = 0; ip < nbptf; ++ip) {
395  indpt[ip] =
396  mfu.ind_basic_dof_of_face_of_element(cv,v.f())[ip*qqdim];
397  indpfp[ip] =
398  pf_s->node_convex(cv).structure()->ind_points_of_face(v.f())[ip];
399  }
400  }
401  else {
402  pintegration_method pim = mim.int_method_of_element(cv);
403  GMM_ASSERT1(pim, "Integration method should be defined");
404  pfp = fppool(pf_s, pim->approx_method()->pintegration_points());
405  nbptf = pim->approx_method()->nb_points_on_face(v.f());
406  indpt.resize(nbptf); indpfp.resize(nbptf);
407  for (short_type ip = 0; ip < nbptf; ++ip)
408  indpt[ip] = indpfp[ip] =
409  pim->approx_method()->ind_first_point_on_face(v.f())+ip;
410  }
411  fem_interpolation_context ctx(pgt,pfp,size_type(-1),G,cv,v.f());
412 
413  for (short_type ip = 0; ip < nbptf; ++ip) {
414  ctx.set_ii(indpfp[ip]);
415 
416  size_type ind = indpt[ip];
417  if (!(on_fem_nodes && dof_already_interpolated[ind])) {
418  if (!ref_conf) {
419  pf_s->interpolation(ctx, coeff, val, dim_type(N));
420  val += ctx.xreal();
421  } else {
422  val = ctx.xreal();
423  }
424  if (on_fem_nodes) dof_ind[ind] = boundary_points.size();
425 
426  }
427 
428  // unit normal vector computation
429  compute_normal(ctx, v.f(), ref_conf, coeff, n0, n, grad);
430  n /= gmm::vect_norm2(n);
431 
432  if (on_fem_nodes && dof_already_interpolated[ind]) {
433  boundary_points_info[dof_ind[ind]].normals.add_normal(n);
434  } else {
435  boundary_points.push_back(val);
436  boundary_points_info.push_back(boundary_point(ctx.xreal(), i, cv,
437  v.f(), ind, n));
438  }
439 
440  if (on_fem_nodes) dof_already_interpolated.add(ind);
441  }
442  }
443  }
444  }
445 
446  void multi_contact_frame::compute_potential_contact_pairs_delaunay() {
447 
448  compute_boundary_points();
449  normal_cone_simplification();
450  potential_pairs = std::vector<std::vector<face_info> >();
451  potential_pairs.resize(boundary_points.size());
452 
453  gmm::dense_matrix<size_type> simplexes;
454  base_small_vector rr(N);
455  // Necessary ?
456  // for (size_type i = 0; i < boundary_points.size(); ++i) {
457  // gmm::fill_random(rr);
458  // boundary_points[i] += 1E-9*rr;
459  // }
460  bgeot::qhull_delaunay(boundary_points, simplexes);
461 
462  // connectivity analysis
463  for (size_type is = 0; is < gmm::mat_ncols(simplexes); ++is) {
464 
465  for (size_type i = 1; i <= N; ++i)
466  for (size_type j = 0; j < i; ++j) {
467  size_type ipt1 = simplexes(i, is), ipt2 = simplexes(j, is);
468  boundary_point *pt_info1 = &(boundary_points_info[ipt1]);
469  boundary_point *pt_info2 = &(boundary_points_info[ipt2]);
470  size_type ib1 = pt_info1->ind_boundary;
471  size_type ib2 = pt_info2->ind_boundary;
472  bool sl1 = is_slave_boundary(ib1);
473  bool sl2 = is_slave_boundary(ib2);
474  if (!sl1 && sl2) { // The slave in first if any
475  std::swap(ipt1, ipt2);
476  std::swap(pt_info1, pt_info2);
477  std::swap(ib1, ib2);
478  std::swap(sl1, sl2);
479  }
480  size_type ir1 = region_of_boundary(ib1);
481  size_type ir2 = region_of_boundary(ib2);
482  const mesh_fem &mf1 = mfdisp_of_boundary(ib1);
483  const mesh_fem &mf2 = mfdisp_of_boundary(ib2);
484 
485  // CRITERION 1 : The unit normal cone / vector are compatible
486  // and the two points are not in the same element.
487  if (
488  // slave-master case
489  ((sl1 && !sl2)
490  // master-master self-contact case
491  || (self_contact && !sl1 && !sl2))
492  // test of unit normal vectors or cones
493  && test_normal_cones_compatibility(pt_info1->normals,
494  pt_info2->normals)
495  // In case of self-contact, test if the two points share the
496  // same element.
497  && (sl1
498  || ((nodes_mode < 2)
499  && (( &(mf1.linked_mesh()) != &(mf2.linked_mesh()))
500  || (pt_info1->ind_element != pt_info2->ind_element)))
501  || ((nodes_mode == 2)
502  && !(are_dof_linked(ib1, pt_info1->ind_pt,
503  ib2, pt_info2->ind_pt)))
504  )
505  ) {
506 
507  // Store the potential contact pairs
508 
509  if (boundary_has_fem_nodes(sl2, nodes_mode)) {
510  const mesh::ind_cv_ct &ic2
511  = mf2.convex_to_basic_dof(pt_info2->ind_pt);
512  for (size_type k = 0; k < ic2.size(); ++k) {
513  mesh_region::face_bitset fbs
514  = mf2.linked_mesh().region(ir2).faces_of_convex(ic2[k]);
515  short_type nbf = mf2.linked_mesh().nb_faces_of_convex(ic2[k]);
516  for (short_type f = 0; f < nbf; ++f)
517  if (fbs.test(f))
518  add_potential_contact_face(ipt1,
519  pt_info2->ind_boundary,
520  ic2[k], f);
521  }
522  } else
523  add_potential_contact_face(ipt1, pt_info2->ind_boundary,
524  pt_info2->ind_element,
525  pt_info2->ind_face);
526 
527  if (self_contact && !sl1 && !sl2) {
528  if (boundary_has_fem_nodes(sl2, nodes_mode)) {
529  const mesh::ind_cv_ct &ic1
530  = mf1.convex_to_basic_dof(pt_info1->ind_pt);
531  for (size_type k = 0; k < ic1.size(); ++k) {
532  mesh_region::face_bitset fbs
533  = mf1.linked_mesh().region(ir1).faces_of_convex(ic1[k]);
534  short_type nbf = mf1.linked_mesh().nb_faces_of_convex(ic1[k]);
535  for (short_type f = 0; f < nbf; ++f)
536  if (fbs.test(f))
537  add_potential_contact_face(ipt2,
538  pt_info1->ind_boundary,
539  ic1[k], f);
540  }
541  } else
542  add_potential_contact_face(ipt2, pt_info1->ind_boundary,
543  pt_info1->ind_element,
544  pt_info1->ind_face);
545  }
546 
547  }
548 
549  }
550  }
551  }
552 
553 
554  void multi_contact_frame::compute_influence_boxes() {
555  fem_precomp_pool fppool;
556  bool avert = false;
557  base_matrix G;
558  model_real_plain_vector coeff;
559 
560  for (size_type i = 0; i < contact_boundaries.size(); ++i)
561  if (!is_slave_boundary(i)) {
562  size_type bnum = region_of_boundary(i);
563  const mesh_fem &mfu = mfdisp_of_boundary(i);
564  const model_real_plain_vector &U = disp_of_boundary(i);
565  const mesh &m = mfu.linked_mesh();
566 
567  base_node val(N), bmin(N), bmax(N);
568  base_small_vector n0(N), n(N), n_mean(N);
569  base_matrix grad(N,N);
570  mesh_region region = m.region(bnum);
571  GMM_ASSERT1(mfu.get_qdim() == N, "Wrong mesh_fem qdim");
572 
573  dal::bit_vector points_already_interpolated;
574  std::vector<base_node> transformed_points(m.nb_max_points());
575  for (getfem::mr_visitor v(region,m); !v.finished(); ++v) {
576  size_type cv = v.cv();
577  bgeot::pgeometric_trans pgt = m.trans_of_convex(cv);
578  pfem pf_s = mfu.fem_of_element(cv);
579  pfem_precomp pfp = fppool(pf_s, pgt->pgeometric_nodes());
580  if (!ref_conf)
581  slice_vector_on_basic_dof_of_element(mfu, U, cv, coeff);
582  mfu.linked_mesh().points_of_convex(cv, G);
583  fem_interpolation_context ctx(pgt,pfp,size_type(-1), G, cv);
584 
585  size_type nb_pt_on_face = 0;
586  dal::bit_vector points_on_face;
587  bgeot::pconvex_structure cvs = pgt->structure();
588  for (size_type k = 0; k < cvs->nb_points_of_face(v.f()); ++k)
589  points_on_face.add(cvs->ind_points_of_face(v.f())[k]);
590 
591  gmm::clear(n_mean);
592  size_type nbd_t = pgt->nb_points();
593  for (short_type ip = 0; ip < nbd_t; ++ip) {
594  size_type ind = m.ind_points_of_convex(cv)[ip];
595 
596  // computation of transformed vertex
597  ctx.set_ii(ip);
598  if (!(points_already_interpolated.is_in(ind))) {
599  if (!ref_conf) {
600  pf_s->interpolation(ctx, coeff, val, dim_type(N));
601  val += ctx.xreal();
602  transformed_points[ind] = val;
603  } else {
604  transformed_points[ind] = ctx.xreal();
605  }
606  points_already_interpolated.add(ind);
607  } else {
608  val = transformed_points[ind];
609  }
610  // computation of unit normal vector if the vertex is on the face
611  if (points_on_face[ip]) {
612  compute_normal(ctx, v.f(), ref_conf, coeff, n0, n, grad);
613  n /= gmm::vect_norm2(n);
614  n_mean += n;
615  ++nb_pt_on_face;
616  }
617 
618  if (ip == 0) // computation of bounding box
619  bmin = bmax = val;
620  else {
621  for (size_type k = 0; k < N; ++k) {
622  bmin[k] = std::min(bmin[k], val[k]);
623  bmax[k] = std::max(bmax[k], val[k]);
624  }
625  }
626 
627  }
628 
629  // is nb_pt_on_face really necessary, is this possible to occur?
630  GMM_ASSERT1(nb_pt_on_face,
631  "This element has no vertex on considered face !");
632 
633  // Computation of influence box :
634  // offset of the bounding box relatively to the release distance
635  scalar_type h = bmax[0] - bmin[0];
636  for (size_type k = 1; k < N; ++k) h = std::max(h, bmax[k]-bmin[k]);
637  if (h < release_distance/scalar_type(40) && !avert) {
638  GMM_WARNING1("Found an element whose size is smaller than 1/40 "
639  "of the release distance. You should probably "
640  "adapt the release distance.");
641  avert = true;
642  }
643  for (size_type k = 0; k < N; ++k)
644  { bmin[k] -= release_distance; bmax[k] += release_distance; }
645 
646  // Store the influence box and additional information.
647  element_boxes.add_box(bmin, bmax, element_boxes_info.size());
648  n_mean /= gmm::vect_norm2(n_mean);
649  element_boxes_info.push_back(influence_box(i, cv, v.f(), n_mean));
650  }
651  }
652  element_boxes.build_tree();
653  }
654 
655  void multi_contact_frame::compute_potential_contact_pairs_influence_boxes() {
656  compute_influence_boxes();
657  compute_boundary_points(!self_contact); // vraiment necessaire ?
658  normal_cone_simplification();
659  potential_pairs = std::vector<std::vector<face_info> >();
660  potential_pairs.resize(boundary_points.size());
661 
662  for (size_type ip = 0; ip < boundary_points.size(); ++ip) {
663 
664  bgeot::rtree::pbox_set bset;
665  element_boxes.find_boxes_at_point(boundary_points[ip], bset);
666  boundary_point *pt_info = &(boundary_points_info[ip]);
667  const mesh_fem &mf1 = mfdisp_of_boundary(pt_info->ind_boundary);
668  size_type ib1 = pt_info->ind_boundary;
669 
670  bgeot::rtree::pbox_set::iterator it = bset.begin();
671  for (; it != bset.end(); ++it) {
672  influence_box &ibx = element_boxes_info[(*it)->id];
673  size_type ib2 = ibx.ind_boundary;
674  const mesh_fem &mf2 = mfdisp_of_boundary(ib2);
675 
676  // CRITERION 1 : The unit normal cone / vector are compatible
677  // and the two points are not in the same element.
678  if (
679  test_normal_cones_compatibility(ibx.mean_normal,
680  pt_info->normals)
681  // In case of self-contact, test if the points and the face
682  // share the same element.
683  && (((nodes_mode < 2)
684  && (( &(mf1.linked_mesh()) != &(mf2.linked_mesh()))
685  || (pt_info->ind_element != ibx.ind_element)))
686  || ((nodes_mode == 2)
687  && !(is_dof_linked(ib1, pt_info->ind_pt,
688  ibx.ind_boundary, ibx.ind_element)))
689  )
690  ) {
691 
692  add_potential_contact_face(ip, ibx.ind_boundary, ibx.ind_element,
693  ibx.ind_face);
694  }
695  }
696 
697  }
698  }
699 
700  struct proj_pt_surf_cost_function_object {
701  size_type N;
702  scalar_type EPS;
703  const base_node &x0, &x;
704  fem_interpolation_context &ctx;
705  const model_real_plain_vector &coeff;
706  const std::vector<base_small_vector> &ti;
707  bool ref_conf;
708  mutable base_node dxy;
709  mutable base_matrix grad, gradtot;
710 
711  scalar_type operator()(const base_small_vector& a) const {
712  base_node xx = x0;
713  for (size_type i= 0; i < N-1; ++i) xx += a[i] * ti[i];
714  ctx.set_xref(xx);
715  if (!ref_conf) {
716  ctx.pf()->interpolation(ctx, coeff, dxy, dim_type(N));
717  dxy += ctx.xreal() - x;
718  } else
719  dxy = ctx.xreal() - x;
720  return gmm::vect_norm2(dxy)/scalar_type(2);
721  }
722 
723  scalar_type operator()(const base_small_vector& a,
724  base_small_vector &grada) const {
725  base_node xx = x0;
726  for (size_type i = 0; i < N-1; ++i) xx += a[i] * ti[i];
727  ctx.set_xref(xx);
728  if (!ref_conf) {
729  ctx.pf()->interpolation(ctx, coeff, dxy, dim_type(N));
730  dxy += ctx.xreal() - x;
731  ctx.pf()->interpolation_grad(ctx, coeff, grad, dim_type(N));
732  gmm::add(gmm::identity_matrix(), grad);
733  gmm::mult(grad, ctx.K(), gradtot);
734  } else {
735  dxy = ctx.xreal() - x;
736  gmm::copy(ctx.K(), gradtot);
737  }
738  for (size_type i = 0; i < N-1; ++i)
739  grada[i] = gmm::vect_sp(gradtot, ti[i], dxy);
740  return gmm::vect_norm2(dxy)/scalar_type(2);
741  }
742  void operator()(const base_small_vector& a,
743  base_matrix &hessa) const {
744  base_small_vector b = a;
745  base_small_vector grada(N-1), gradb(N-1);
746  (*this)(b, grada);
747  for (size_type i = 0; i < N-1; ++i) {
748  b[i] += EPS;
749  (*this)(b, gradb);
750  for (size_type j = 0; j < N-1; ++j)
751  hessa(j, i) = (gradb[j] - grada[j])/EPS;
752  b[i] -= EPS;
753  }
754  }
755 
756  proj_pt_surf_cost_function_object
757  (const base_node &x00, const base_node &xx,
758  fem_interpolation_context &ctxx,
759  const model_real_plain_vector &coefff,
760  const std::vector<base_small_vector> &tii,
761  scalar_type EPSS, bool rc)
762  : N(gmm::vect_size(x00)), EPS(EPSS), x0(x00), x(xx),
763  ctx(ctxx), coeff(coefff), ti(tii), ref_conf(rc),
764  dxy(N), grad(N,N), gradtot(N,N) {}
765 
766  };
767 
768  struct raytrace_pt_surf_cost_function_object {
769  size_type N;
770  const base_node &x0, &x;
771  fem_interpolation_context &ctx;
772  const model_real_plain_vector &coeff;
773  const std::vector<base_small_vector> &ti;
774  const std::vector<base_small_vector> &Ti;
775  bool ref_conf;
776  mutable base_node dxy;
777  mutable base_matrix grad, gradtot;
778 
779  void operator()(const base_small_vector& a,
780  base_small_vector &res) const {
781  base_node xx = x0;
782  for (size_type i = 0; i < N-1; ++i) xx += a[i] * ti[i];
783  ctx.set_xref(xx);
784  if (!ref_conf) {
785  ctx.pf()->interpolation(ctx, coeff, dxy, dim_type(N));
786  dxy += ctx.xreal() - x;
787  } else
788  dxy = ctx.xreal() - x;
789  for (size_type i = 0; i < N-1; ++i)
790  res[i] = gmm::vect_sp(dxy, Ti[i]);
791  }
792 
793  void operator()(const base_small_vector& a,
794  base_matrix &hessa) const {
795  base_node xx = x0;
796  for (size_type i = 0; i < N-1; ++i) xx += a[i] * ti[i];
797  ctx.set_xref(xx);
798  if (!ref_conf) {
799  ctx.pf()->interpolation_grad(ctx, coeff, grad, dim_type(N));
800  gmm::add(gmm::identity_matrix(), grad);
801  gmm::mult(grad, ctx.K(), gradtot);
802  } else {
803  gmm::copy(ctx.K(), gradtot);
804  }
805  for (size_type i = 0; i < N-1; ++i)
806  for (size_type j = 0; j < N-1; ++j)
807  hessa(j, i) = gmm::vect_sp(gradtot, ti[i], Ti[j]);
808  }
809 
810 
811  raytrace_pt_surf_cost_function_object
812  (const base_node &x00, const base_node &xx,
813  fem_interpolation_context &ctxx,
814  const model_real_plain_vector &coefff,
815  const std::vector<base_small_vector> &tii,
816  const std::vector<base_small_vector> &Tii,
817  bool rc)
818  : N(gmm::vect_size(x00)), x0(x00), x(xx),
819  ctx(ctxx), coeff(coefff), ti(tii), Ti(Tii), ref_conf(rc),
820  dxy(N), grad(N,N), gradtot(N,N) {}
821 
822  };
823 
824  // Ideas to improve efficiency :
825  // - From an iteration to another, is it possible to simplify the
826  // computation ? For instance in testing the old contact pairs ...
827  // But how to detect new contact situations ?
828  // - A pre-test before projection (for Delaunay) : if the distance to a
829  // node is greater than the release distance + h then give up.
830  // - Case J3 of valid/invalid contact situations is not really taken into
831  // account. How to take it into account in a cheap way ?
832 
833  void multi_contact_frame::compute_contact_pairs() {
834  base_matrix G, grad(N,N);
835  model_real_plain_vector coeff;
836  base_small_vector a(N-1), ny(N);
837  base_node y(N);
838  std::vector<base_small_vector> ti(N-1), Ti(N-1);
839  size_type nbwarn(0);
840 
841  // double time = dal::uclock_sec();
842 
843  clear_aux_info();
844  contact_pairs = std::vector<contact_pair>();
845 
846  if (!ref_conf) extend_vectors();
847 
848  bool only_slave(true), only_master(true);
849  for (size_type i = 0; i < contact_boundaries.size(); ++i)
850  if (is_slave_boundary(i)) only_master = false;
851  else only_slave = false;
852 
853  if (only_master && !self_contact) {
854  GMM_WARNING1("There is only master boundary and no self-contact to detect. Exiting");
855  return;
856  }
857 
858  if (only_slave) {
859  compute_boundary_points();
860  potential_pairs.resize(boundary_points.size());
861  }
862  else if (use_delaunay)
863  compute_potential_contact_pairs_delaunay();
864  else
865  compute_potential_contact_pairs_influence_boxes();
866 
867  // cout << "Time for computing potential pairs: " << dal::uclock_sec() - time << endl; time = dal::uclock_sec();
868 
869 
870  // Scan of potential pairs
871  for (size_type ip = 0; ip < potential_pairs.size(); ++ip) {
872  bool first_pair_found = false;
873  const base_node &x = boundary_points[ip];
874  boundary_point &bpinfo = boundary_points_info[ip];
875  size_type ibx = bpinfo.ind_boundary;
876  bool slx = is_slave_boundary(ibx);
877  scalar_type d0 = 1E300, d1, d2;
878 
879  base_small_vector nx = bpinfo.normals[0];
880  if (raytrace) {
881  if (bpinfo.normals.size() > 1) { // take the mean normal vector
882  for (size_type i = 1; i < bpinfo.normals.size(); ++i)
883  gmm::add(bpinfo.normals[i], nx);
884  scalar_type nnx = gmm::vect_norm2(nx);
885  GMM_ASSERT1(nnx != scalar_type(0), "Invalid normal cone");
886  gmm::scale(nx, scalar_type(1)/nnx);
887  }
888  }
889 
890  if (self_contact || slx) {
891  // Detect here the nearest rigid obstacle (taking into account
892  // the release distance)
893  size_type irigid_obstacle(-1);
894  gmm::copy(x, pt);
895  if (N >= 4) ptw[0] = pt[3];
896  if (N >= 3) ptz[0] = pt[2];
897  if (N >= 2) pty[0] = pt[1];
898  if (N >= 1) ptx[0] = pt[0];
899  for (size_type i = 0; i < obstacles.size(); ++i) {
900  d1 = (obstacles_f[i].eval())[0];
901  if (gmm::abs(d1) < release_distance && d1 < d0) {
902 
903  for (size_type j=0; j < bpinfo.normals.size(); ++j) {
904  gmm::add(gmm::scaled(bpinfo.normals[j], EPS), pt);
905  if (N >= 4) ptw[0] = pt[3];
906  if (N >= 3) ptz[0] = pt[2];
907  if (N >= 2) pty[0] = pt[1];
908  if (N >= 1) ptx[0] = pt[0];
909  d2 = (obstacles_f[i].eval())[0];
910  if (d2 < d1) { d0 = d1; irigid_obstacle = i; break; }
911  gmm::copy(x, pt);
912  if (N >= 4) ptw[0] = pt[3];
913  if (N >= 3) ptz[0] = pt[2];
914  if (N >= 2) pty[0] = pt[1];
915  if (N >= 1) ptx[0] = pt[0];
916  }
917  }
918  }
919 
920  if (irigid_obstacle != size_type(-1)) {
921 
922  gmm::copy(x, pt);
923  if (N >= 4) ptw[0] = pt[3];
924  if (N >= 3) ptz[0] = pt[2];
925  if (N >= 2) pty[0] = pt[1];
926  if (N >= 1) ptx[0] = pt[0];
927  gmm::copy(x, y);
928  size_type nit = 0, nb_fail = 0;
929  scalar_type alpha(0), beta(0);
930  d1 = d0;
931 
932  while (++nit < 50 && nb_fail < 3) {
933  for (size_type k = 0; k < N; ++k) {
934  pt[k] += EPS;
935  switch(N) {
936  case 4: ptw[0] += EPS; break;
937  case 3: ptz[0] += EPS; break;
938  case 2: pty[0] += EPS; break;
939  case 1: ptx[0] += EPS; break;
940  }
941  d2 = (obstacles_f[irigid_obstacle].eval())[0];
942  ny[k] = (d2 - d1) / EPS;
943  pt[k] -= EPS;
944  switch(N) {
945  case 4: ptw[0] -= EPS; break;
946  case 3: ptz[0] -= EPS; break;
947  case 2: pty[0] -= EPS; break;
948  case 1: ptx[0] -= EPS; break;
949  }
950  }
951 
952  if (gmm::abs(d1) < 1E-13)
953  break; // point already lies on the rigid obstacle surface
954 
955  // ajouter un test de divergence ...
956  for (scalar_type lambda(1); lambda >= 1E-3; lambda /= scalar_type(2)) {
957  if (raytrace) {
958  alpha = beta - lambda * d1 / gmm::vect_sp(ny, nx);
959  gmm::add(x, gmm::scaled(nx, alpha), pt);
960  } else {
961  gmm::add(gmm::scaled(ny, -d1/gmm::vect_norm2_sqr(ny)), y, pt);
962  }
963  if (N >= 4) ptw[0] = pt[3];
964  if (N >= 3) ptz[0] = pt[2];
965  if (N >= 2) pty[0] = pt[1];
966  if (N >= 1) ptx[0] = pt[0];
967  d2 = (obstacles_f[irigid_obstacle].eval())[0];
968 // if (nit > 10)
969 // cout << "nit = " << nit << " lambda = " << lambda
970 // << " alpha = " << alpha << " d2 = " << d2
971 // << " d1 = " << d1 << endl;
972  if (gmm::abs(d2) < gmm::abs(d1)) break;
973  }
974  if (raytrace &&
975  gmm::abs(beta - d1 / gmm::vect_sp(ny, nx)) > scalar_type(500))
976  nb_fail++;
977  gmm::copy(pt, y); beta = alpha; d1 = d2;
978  }
979 
980  if (gmm::abs(d1) > 1E-8) {
981  GMM_WARNING1("Projection/raytrace on rigid obstacle failed");
982  continue;
983  }
984 
985  // CRITERION 4 for rigid bodies : Apply the release distance
986  if (gmm::vect_dist2(y, x) > release_distance)
987  continue;
988 
989  gmm::copy(pt, y);
990  ny /= gmm::vect_norm2(ny);
991 
992  d0 = gmm::vect_dist2(y, x) * gmm::sgn(d0);
993  contact_pair ct(x, nx, bpinfo, y, ny, irigid_obstacle, d0);
994 
995  contact_pairs.push_back(ct);
996  first_pair_found = true;
997  }
998  }
999 
1000  // if (potential_pairs[ip].size())
1001  // cout << "number of potential pairs for point " << ip << " : " << potential_pairs[ip].size() << endl;
1002  for (size_type ipf = 0; ipf < potential_pairs[ip].size(); ++ipf) {
1003  // Point to surface projection. Principle :
1004  // - One parametrizes first the face on the reference element by
1005  // obtaining a point x_0 on that face and t_i, i=1..d-1 some
1006  // orthonormals tangent vectors to the face.
1007  // - Let y_0 be the point to be projected and y the searched
1008  // projected point. Then one searches for the minimum of
1009  // J = (1/2)|| y - x ||
1010  // with
1011  // y = \phi(x0 + a_i t_i)
1012  // (with a summation on i), where \phi = I+u(\tau(x)), and \tau
1013  // the geometric transformation between reference and real
1014  // elements.
1015  // - The gradient of J with respect to a_i is
1016  // \partial_{a_j} J = (\phi(x0 + a_i t_i) - x)
1017  // . (\nabla \phi(x0 + a_i t_i) t_j
1018  // - A Newton algorithm is applied.
1019  // - If it fails, a BFGS is called.
1020 
1021  const face_info &fi = potential_pairs[ip][ipf];
1022  size_type ib = fi.ind_boundary;
1023  size_type cv = fi.ind_element;
1024  short_type iff = fi.ind_face;
1025 
1026  const mesh_fem &mfu = mfdisp_of_boundary(ib);
1027  const mesh &m = mfu.linked_mesh();
1028  pfem pf_s = mfu.fem_of_element(cv);
1029  bgeot::pgeometric_trans pgt = m.trans_of_convex(cv);
1030 
1031  if (!ref_conf)
1032  slice_vector_on_basic_dof_of_element(mfu, disp_of_boundary(ib),
1033  cv, coeff);
1034 
1035  m.points_of_convex(cv, G);
1036 
1037  const base_node &x0 = pf_s->ref_convex(cv)->points_of_face(iff)[0];
1038  fem_interpolation_context ctx(pgt, pf_s, x0, G, cv, iff);
1039 
1040  const base_small_vector &n0 = pf_s->ref_convex(cv)->normals()[iff];
1041  for (size_type k = 0; k < N-1; ++k) { // A basis for the face
1042  gmm::resize(ti[k], N);
1043  scalar_type norm(0);
1044  while(norm < 1E-5) {
1045  gmm::fill_random(ti[k]);
1046  ti[k] -= gmm::vect_sp(ti[k], n0) * n0;
1047  for (size_type l = 0; l < k; ++l)
1048  ti[k] -= gmm::vect_sp(ti[k], ti[l]) * ti[l];
1049  norm = gmm::vect_norm2(ti[k]);
1050  }
1051  ti[k] /= norm;
1052  }
1053 
1054  bool converged = false;
1055  scalar_type residual(0);
1056 
1057 
1058  if (raytrace) { // Raytrace search for y by a Newton algorithm
1059 
1060  base_small_vector res(N-1), res2(N-1), dir(N-1), b(N-1);
1061 
1062  base_matrix hessa(N-1, N-1);
1063  gmm::clear(a);
1064 
1065  for (size_type k = 0; k < N-1; ++k) {
1066  gmm::resize(Ti[k], N);
1067  scalar_type norm(0);
1068  while (norm < 1E-5) {
1069  gmm::fill_random(Ti[k]);
1070  Ti[k] -= gmm::vect_sp(Ti[k], nx) * nx;
1071  for (size_type l = 0; l < k; ++l)
1072  Ti[k] -= gmm::vect_sp(Ti[k], Ti[l]) * Ti[l];
1073  norm = gmm::vect_norm2(Ti[k]);
1074  }
1075  Ti[k] /= norm;
1076  }
1077 
1078  raytrace_pt_surf_cost_function_object pps(x0, x, ctx, coeff, ti, Ti,
1079  ref_conf);
1080 
1081  pps(a, res);
1082  residual = gmm::vect_norm2(res);
1083  scalar_type residual2(0), det(0);
1084  bool exited = false;
1085  size_type nbfail = 0, niter = 0;
1086  for (;residual > 2E-12 && niter <= 30; ++niter) {
1087 
1088  for (size_type subiter(0);;) {
1089  pps(a, hessa);
1090  det = gmm::abs(bgeot::lu_inverse(&(*(hessa.begin())),N-1, false));
1091  if (det > 1E-15) break;
1092  for (size_type i = 0; i < N-1; ++i)
1093  a[i] += gmm::random() * 1E-7;
1094  if (++subiter > 4) break;
1095  }
1096  if (det <= 1E-15) break;
1097  // Computation of the descent direction
1098  gmm::mult(hessa, gmm::scaled(res, scalar_type(-1)), dir);
1099 
1100  if (gmm::vect_norm2(dir) > scalar_type(10)) nbfail++;
1101  if (nbfail >= 4) break;
1102 
1103  // Line search
1104  scalar_type lambda(1);
1105  for (size_type j = 0; j < 5; ++j) {
1106  gmm::add(a, gmm::scaled(dir, lambda), b);
1107  pps(b, res2);
1108  residual2 = gmm::vect_norm2(res2);
1109  if (residual2 < residual) break;
1110  lambda /= ((j < 3) ? scalar_type(2) : scalar_type(5));
1111  }
1112 
1113  residual = residual2;
1114  gmm::copy(res2, res);
1115  gmm::copy(b, a);
1116  scalar_type dist_ref = gmm::vect_norm2(a);
1117 // if (niter == 15)
1118 // cout << "more than 15 iterations " << a
1119 // << " dir " << dir << " nbfail : " << nbfail << endl;
1120  if (niter > 1 && dist_ref > 15) break;
1121  if (niter > 5 && dist_ref > 8) break;
1122  if ((niter > 1 && dist_ref > 7) || nbfail == 3) exited = true;
1123  }
1124  converged = (gmm::vect_norm2(res) < 2E-6);
1125  GMM_ASSERT1(!((exited && converged &&
1126  pf_s->ref_convex(cv)->is_in(ctx.xref()) < 1E-6)),
1127  "A non conformal case !! " << gmm::vect_norm2(res)
1128  << " : " << nbfail << " : " << niter);
1129 
1130  } else { // Classical projection for y
1131 
1132  proj_pt_surf_cost_function_object pps(x0, x, ctx, coeff, ti,
1133  EPS, ref_conf);
1134 
1135  // Projection could be ameliorated by finding a starting point near
1136  // x (with respect to the integration method, for instance).
1137 
1138  // A specific (Quasi) Newton algorithm for computing the projection
1139  base_small_vector grada(N-1), dir(N-1), b(N-1);
1140  gmm::clear(a);
1141  base_matrix hessa(N-1, N-1);
1142  scalar_type det(0);
1143 
1144  scalar_type dist = pps(a, grada);
1145  for (size_type niter = 0;
1146  gmm::vect_norm2(grada) > 1E-12 && niter <= 50; ++niter) {
1147 
1148  for (size_type subiter(0);;) {
1149  pps(a, hessa);
1150  det = gmm::abs(bgeot::lu_inverse(&(*(hessa.begin())),N-1, false));
1151  if (det > 1E-15) break;
1152  for (size_type i = 0; i < N-1; ++i)
1153  a[i] += gmm::random() * 1E-7;
1154  if (++subiter > 4) break;
1155  }
1156  if (det <= 1E-15) break;
1157  // Computation of the descent direction
1158  gmm::mult(hessa, gmm::scaled(grada, scalar_type(-1)), dir);
1159 
1160  // Line search
1161  for (scalar_type lambda(1);
1162  lambda >= 1E-3; lambda /= scalar_type(2)) {
1163  gmm::add(a, gmm::scaled(dir, lambda), b);
1164  if (pps(b) < dist) break;
1165  gmm::add(a, gmm::scaled(dir, -lambda), b);
1166  if (pps(b) < dist) break;
1167  }
1168  gmm::copy(b, a);
1169  dist = pps(a, grada);
1170  }
1171 
1172  converged = (gmm::vect_norm2(grada) < 2E-6);
1173 
1174  if (!converged) { // Try with BFGS
1175  gmm::iteration iter(1E-12, 0 /* noisy*/, 100 /*maxiter*/);
1176  gmm::clear(a);
1177  gmm::bfgs(pps, pps, a, 10, iter, 0, 0.5);
1178  residual = gmm::abs(iter.get_res());
1179  converged = (residual < 2E-5);
1180  }
1181  }
1182 
1183  bool is_in = (pf_s->ref_convex(cv)->is_in(ctx.xref()) < 1E-6);
1184 
1185  if (is_in || (!converged && !raytrace)) {
1186  if (!ref_conf) {
1187  ctx.pf()->interpolation(ctx, coeff, y, dim_type(N));
1188  y += ctx.xreal();
1189  } else {
1190  y = ctx.xreal();
1191  }
1192  }
1193 
1194  // CRITERION 2 : The contact pair is eliminated when
1195  // projection/raytrace do not converge.
1196  if (!converged) {
1197  if (!raytrace && nbwarn < 4) {
1198  GMM_WARNING3("Projection or raytrace algorithm did not converge "
1199  "for point " << x << " residual " << residual
1200  << " projection computed " << y);
1201  ++nbwarn;
1202  }
1203  continue;
1204  }
1205 
1206  // CRITERION 3 : The projected point is inside the element
1207  // The test should be completed: If the point is outside
1208  // the element, a rapid reprojection on the face
1209  // (on the reference element, with a linear algorithm)
1210  // can be applied and a test with a neigbhour element
1211  // to decide if the point is in fact ok ...
1212  // (to be done only if there is no projection on other
1213  // element which coincides and with a test on the
1214  // distance ... ?) To be specified (in this case,
1215  // change xref).
1216  if (!is_in) continue;
1217 
1218  // CRITERION 4 : Apply the release distance
1219  scalar_type signed_dist = gmm::vect_dist2(y, x);
1220  if (signed_dist > release_distance) continue;
1221 
1222  // compute the unit normal vector at y and the signed distance.
1223  base_small_vector ny0(N);
1224  compute_normal(ctx, iff, ref_conf, coeff, ny0, ny, grad);
1225  // ny /= gmm::vect_norm2(ny); // Useful only if the unit normal is kept
1226  signed_dist *= gmm::sgn(gmm::vect_sp(x - y, ny));
1227 
1228  // CRITERION 5 : comparison with rigid obstacles
1229  // CRITERION 7 : smallest signed distance on contact pairs
1230  if (first_pair_found && contact_pairs.back().signed_dist < signed_dist)
1231  continue;
1232 
1233  // CRITERION 1 : again on found unit normal vector
1234  if (!(test_normal_cones_compatibility(ny, bpinfo.normals)))
1235  continue;
1236 
1237  // CRITERION 6 : for self-contact only : apply a test on
1238  // unit normals in reference configuration.
1239  if (&m == &(mfdisp_of_boundary(ibx).linked_mesh())) {
1240 
1241  base_small_vector diff = bpinfo.ref_point - ctx.xreal();
1242  scalar_type ref_dist = gmm::vect_norm2(diff);
1243 
1244  if ( (ref_dist < scalar_type(4) * release_distance)
1245  && (gmm::vect_sp(diff, ny0) < - 0.01 * ref_dist) )
1246  continue;
1247  }
1248 
1249  contact_pair ct(x, nx, bpinfo, ctx.xref(), y, ny, fi, signed_dist);
1250  if (first_pair_found) {
1251  contact_pairs.back() = ct;
1252  } else {
1253  contact_pairs.push_back(ct);
1254  first_pair_found = true;
1255  }
1256 
1257  }
1258  }
1259 
1260  // cout << "Time for computing pairs: " << dal::uclock_sec() - time << endl; time = dal::uclock_sec();
1261 
1262  clear_aux_info();
1263  }
1264 
1265  //=========================================================================
1266  //
1267  // Raytracing interpolate transformation for generic assembly
1268  //
1269  //=========================================================================
1270 
1271  class raytracing_interpolate_transformation
1272  : public virtual_interpolate_transformation {
1273  protected:
1274  // Structure describing a contact boundary
1275  struct contact_boundary {
1276  size_type region; // Boundary number
1277  const getfem::mesh_fem *mfu; // F.e.m. for the displacement.
1278  std::string dispname; // Variable name for the displacement
1279  mutable const model_real_plain_vector *U; // Displacement
1280  mutable model_real_plain_vector U_unred; // Unreduced displacement
1281  bool slave;
1282 
1283  contact_boundary()
1284  : region(-1), mfu(0), dispname(""), U(0), U_unred(0), slave(false) {}
1285  contact_boundary(size_type r, const mesh_fem *mf, const std::string &dn,
1286  bool sl)
1287  : region(r), mfu(mf), dispname(dn), slave(sl) {}
1288  };
1289 
1290  struct face_box_info { // Additional information for a face box
1291  size_type ind_boundary; // Boundary number
1292  size_type ind_element; // Element number
1293  short_type ind_face; // Face number in element
1294  base_small_vector mean_normal; // Mean outward normal unit vector
1295  face_box_info()
1296  : ind_boundary(-1), ind_element(-1), ind_face(-1), mean_normal(0) {}
1297  face_box_info(size_type ib, size_type ie,
1298  short_type iff, const base_small_vector &n)
1299  : ind_boundary(ib), ind_element(ie), ind_face(iff), mean_normal(n) {}
1300  };
1301 
1302  scalar_type release_distance; // Limit distance beyond which the contact
1303  // will not be considered.
1304 
1305  std::vector<contact_boundary> contact_boundaries;
1306  typedef std::map<const mesh *, std::vector<size_type> > mesh_boundary_cor;
1307  mesh_boundary_cor boundary_for_mesh;
1308 
1309  class obstacle {
1310  const model *md;
1311  const ga_workspace *parent_workspace;
1312  std::string expr;
1313 
1314  mutable base_vector X;
1315  mutable ga_function f, der_f;
1316  mutable bool compiled;
1317 
1318  void compile() const {
1319  if (md)
1320  f = ga_function(*md, expr);
1321  else if (parent_workspace)
1322  f = ga_function(*parent_workspace, expr);
1323  else
1324  f = ga_function(expr);
1325  size_type N = gmm::vect_size(X);
1326  f.workspace().add_fixed_size_variable("X", gmm::sub_interval(0, N), X);
1327  if (N >= 1) f.workspace().add_macro("x", "X(1)");
1328  if (N >= 2) f.workspace().add_macro("y", "X(2)");
1329  if (N >= 3) f.workspace().add_macro("z", "X(3)");
1330  if (N >= 4) f.workspace().add_macro("w", "X(4)");
1331  f.compile();
1332  der_f = f;
1333  der_f.derivative("X");
1334  compiled = true;
1335  }
1336 
1337  public:
1338 
1339  base_vector &point() const { return X; }
1340 
1341  const base_tensor &eval() const {
1342  if (!compiled) compile();
1343  return f.eval();
1344  }
1345  const base_tensor &eval_derivative() const {
1346  if (!compiled) compile();
1347  return der_f.eval();
1348  }
1349 
1350  obstacle()
1351  : md(0), parent_workspace(0), expr(""), X(0), f(), der_f() {}
1352  obstacle(const model &md_, const std::string &expr_, size_type N)
1353  : md(&md_), parent_workspace(0), expr(expr_), X(N),
1354  f(), der_f(), compiled(false) {}
1355  obstacle(const ga_workspace &parent_workspace_,
1356  const std::string &expr_, size_type N)
1357  : md(0), parent_workspace(&parent_workspace_), expr(expr_), X(N),
1358  f(), der_f(), compiled(false) {}
1359  obstacle(const obstacle &obs)
1360  : md(obs.md), parent_workspace(obs.parent_workspace), expr(obs.expr),
1361  X(obs.X), f(), der_f(), compiled(false) {}
1362  obstacle &operator =(const obstacle& obs) {
1363  md = obs.md;
1364  parent_workspace = obs.parent_workspace;
1365  expr = obs.expr;
1366  X = obs.X;
1367  f = ga_function();
1368  der_f = ga_function();
1369  compiled = false;
1370  return *this;
1371  }
1372  ~obstacle() {}
1373  };
1374 
1375  std::vector<obstacle> obstacles;
1376 
1377  mutable bgeot::rtree face_boxes;
1378  mutable std::vector<face_box_info> face_boxes_info;
1379 
1380 
1381  void compute_face_boxes() const { // called by init
1382  fem_precomp_pool fppool;
1383  base_matrix G;
1384  model_real_plain_vector coeff;
1385  face_boxes.clear();
1386  face_boxes_info.resize(0);
1387 
1388  for (size_type i = 0; i < contact_boundaries.size(); ++i) {
1389  const contact_boundary &cb = contact_boundaries[i];
1390  if (! cb.slave) {
1391  size_type bnum = cb.region;
1392  const mesh_fem &mfu = *(cb.mfu);
1393  const model_real_plain_vector &U = *(cb.U);
1394  const mesh &m = mfu.linked_mesh();
1395  size_type N = m.dim();
1396 
1397  base_node val(N), bmin(N), bmax(N);
1398  base_small_vector n0_x(N), n_x(N), n0_y(N), n_y(N), n_mean(N);
1399  base_matrix grad(N,N);
1400  mesh_region region = m.region(bnum);
1401  GMM_ASSERT1(mfu.get_qdim() == N, "Wrong mesh_fem qdim");
1402 
1403  dal::bit_vector points_already_interpolated;
1404  std::vector<base_node> transformed_points(m.nb_max_points());
1405  for (getfem::mr_visitor v(region,m); !v.finished(); ++v) {
1406  size_type cv = v.cv();
1407  bgeot::pgeometric_trans pgt = m.trans_of_convex(cv);
1408  pfem pf_s = mfu.fem_of_element(cv);
1409  pfem_precomp pfp = fppool(pf_s, pgt->pgeometric_nodes());
1410  slice_vector_on_basic_dof_of_element(mfu, U, cv, coeff);
1411  mfu.linked_mesh().points_of_convex(cv, G);
1412  fem_interpolation_context ctx(pgt, pfp, size_type(-1), G, cv);
1413 
1414  bgeot::pconvex_structure cvs = pgt->structure();
1415  size_type nb_pt_on_face = cvs->nb_points_of_face(v.f());
1416  GMM_ASSERT1(nb_pt_on_face >= 2, "This element has less than two "
1417  "vertices on considered face !");
1418  gmm::clear(n_mean);
1419 
1420  for (size_type k = 0; k < nb_pt_on_face; ++k) {
1421  size_type ip = cvs->ind_points_of_face(v.f())[k];
1422  size_type ind = m.ind_points_of_convex(cv)[ip];
1423 
1424  // computation of transformed vertex
1425  ctx.set_ii(ip);
1426  if (!(points_already_interpolated.is_in(ind))) {
1427  pf_s->interpolation(ctx, coeff, val, dim_type(N));
1428  val += ctx.xreal();
1429  transformed_points[ind] = val;
1430  points_already_interpolated.add(ind);
1431  } else {
1432  val = transformed_points[ind];
1433  }
1434  // computation of unit normal vector if the vertex is on the face
1435  compute_normal(ctx, v.f(), false, coeff, n0_x, n_x, grad);
1436  n_x /= gmm::vect_norm2(n_x);
1437  n_mean += n_x;
1438 
1439  if (k == 0) // computation of bounding box
1440  bmin = bmax = val;
1441  else {
1442  for (size_type l = 0; l < N; ++l) {
1443  bmin[l] = std::min(bmin[l], val[l]);
1444  bmax[l] = std::max(bmax[l], val[l]);
1445  }
1446  }
1447  }
1448 
1449  // Security coefficient of 1.3 (for nonlinear transformations)
1450  scalar_type h = bmax[0] - bmin[0];
1451  for (size_type k = 1; k < N; ++k) h = std::max(h, bmax[k]-bmin[k]);
1452  for (size_type k = 0; k < N; ++k)
1453  { bmin[k] -= h * 0.15; bmax[k] += h * 0.15; }
1454 
1455  // Store the bounding box and additional information.
1456  face_boxes.add_box(bmin, bmax, face_boxes_info.size());
1457  n_mean /= gmm::vect_norm2(n_mean);
1458  face_boxes_info.push_back(face_box_info(i, cv, v.f(), n_mean));
1459  }
1460  }
1461  }
1462  face_boxes.build_tree();
1463  }
1464 
1465  public:
1466 
1467  void add_rigid_obstacle(const model &md, const std::string &expr,
1468  size_type N) {
1469  obstacles.push_back(obstacle(md, expr, N));
1470  }
1471 
1472  void add_rigid_obstacle(const ga_workspace &parent_workspace,
1473  const std::string &expr, size_type N) {
1474  obstacles.push_back(obstacle(parent_workspace, expr, N));
1475  }
1476 
1477  void add_contact_boundary(const model &md, const mesh &m,
1478  const std::string dispname,
1479  size_type region, bool slave) {
1480  const mesh_fem *mf = 0;
1481  if (md.variable_group_exists(dispname)) {
1482  for (const std::string &t : md.variable_group(dispname)) {
1483  const mesh_fem *mf2 = md.pmesh_fem_of_variable(t);
1484  if (mf2 && &(mf2->linked_mesh()) == &m)
1485  { mf = mf2; break; }
1486  }
1487  } else
1488  mf = md.pmesh_fem_of_variable(dispname);
1489 
1490  GMM_ASSERT1(mf, "Displacement should be a fem variable");
1491  contact_boundary cb(region, mf, dispname, slave);
1492  boundary_for_mesh[&(mf->linked_mesh())]
1493  .push_back(contact_boundaries.size());
1494  contact_boundaries.push_back(cb);
1495  }
1496 
1497  void add_contact_boundary(const ga_workspace &workspace, const mesh &m,
1498  const std::string dispname,
1499  size_type region, bool slave) {
1500  const mesh_fem *mf = 0;
1501  if (workspace.variable_group_exists(dispname)) {
1502  for (const std::string &t : workspace.variable_group(dispname)) {
1503  const mesh_fem *mf2 = workspace.associated_mf(t);
1504  if (mf2 && &(mf2->linked_mesh()) == &m)
1505  { mf = mf2; break; }
1506  }
1507  } else
1508  mf = workspace.associated_mf(dispname);
1509 
1510  GMM_ASSERT1(mf, "Displacement should be a fem variable");
1511  contact_boundary cb(region, mf, dispname, slave);
1512  boundary_for_mesh[&(mf->linked_mesh())]
1513  .push_back(contact_boundaries.size());
1514  contact_boundaries.push_back(cb);
1515  }
1516 
1517  void extract_variables(const ga_workspace &workspace,
1518  std::set<var_trans_pair> &vars,
1519  bool ignore_data, const mesh &m_x,
1520  const std::string &interpolate_name) const {
1521 
1522  bool expand_groups = !ignore_data;
1523  // const mesh_fem *mf = workspace.associated_mf(name);
1524  // GMM_ASSERT1(mf, "Internal error");
1525  // const mesh &m_x = mf->linked_mesh();
1526 
1527  mesh_boundary_cor::const_iterator it = boundary_for_mesh.find(&m_x);
1528  GMM_ASSERT1(it != boundary_for_mesh.end(), "Raytracing interpolate "
1529  "transformation: Mesh with no declared contact boundary");
1530  for (const size_type &boundary_ind : it->second) {
1531  const contact_boundary &cb = contact_boundaries[boundary_ind];
1532  const std::string &dispname_x
1533  = workspace.variable_in_group(cb.dispname, m_x);
1534  if (!ignore_data || !(workspace.is_constant(dispname_x)))
1535  vars.insert(var_trans_pair(dispname_x, ""));
1536  }
1537 
1538  for (const contact_boundary &cb : contact_boundaries) {
1539  if (!(cb.slave)) {
1540  if (expand_groups && workspace.variable_group_exists(cb.dispname)
1541  && (!ignore_data || !(workspace.is_constant(cb.dispname)))) {
1542  for (const std::string &t : workspace.variable_group(cb.dispname))
1543  vars.insert(var_trans_pair(t, interpolate_name));
1544  } else {
1545  if (!ignore_data || !(workspace.is_constant(cb.dispname)))
1546  vars.insert(var_trans_pair(cb.dispname, interpolate_name));
1547  }
1548  }
1549  }
1550  }
1551 
1552  void init(const ga_workspace &workspace) const {
1553  for (const contact_boundary &cb : contact_boundaries) {
1554  const mesh_fem &mfu = *(cb.mfu);
1555  const std::string dispname_x
1556  = workspace.variable_in_group(cb.dispname, mfu.linked_mesh());
1557 
1558  if (mfu.is_reduced()) {
1559  gmm::resize(cb.U_unred, mfu.nb_basic_dof());
1560  mfu.extend_vector(workspace.value(dispname_x), cb.U_unred);
1561  cb.U = &(cb.U_unred);
1562  } else {
1563  cb.U = &(workspace.value(dispname_x));
1564  }
1565  }
1566  compute_face_boxes();
1567  };
1568 
1569  void finalize() const {
1570  face_boxes.clear();
1571  face_boxes_info = std::vector<face_box_info>();
1572  for (const contact_boundary &cb : contact_boundaries)
1573  cb.U_unred = model_real_plain_vector();
1574  }
1575 
1576  int transform(const ga_workspace &workspace, const mesh &m_x,
1577  fem_interpolation_context &ctx_x,
1578  const base_small_vector &/*Normal*/,
1579  const mesh **m_t,
1580  size_type &cv, short_type &face_num, base_node &P_ref,
1581  base_small_vector &N_y,
1582  std::map<var_trans_pair, base_tensor> &derivatives,
1583  bool compute_derivatives) const {
1584  size_type cv_x = ctx_x.convex_num();
1585  short_type face_x = ctx_x.face_num();
1586  GMM_ASSERT1(face_x != short_type(-1), "The contact transformation can "
1587  "only be applied to a boundary");
1588 
1589  //
1590  // Find the right (slave) contact boundary
1591  //
1592  mesh_boundary_cor::const_iterator it = boundary_for_mesh.find(&m_x);
1593  GMM_ASSERT1(it != boundary_for_mesh.end(),
1594  "Mesh with no declared contact boundary");
1595  size_type ib_x = size_type(-1);
1596  for (const size_type &boundary_ind : it->second) {
1597  const contact_boundary &cb = contact_boundaries[boundary_ind];
1598  if (m_x.region(cb.region).is_in(cv_x, face_x))
1599  { ib_x = boundary_ind; break; }
1600  }
1601  GMM_ASSERT1(ib_x != size_type(-1),
1602  "No contact region found for this point");
1603  const contact_boundary &cb_x = contact_boundaries[ib_x];
1604  const mesh_fem &mfu_x = *(cb_x.mfu);
1605  pfem pfu_x = mfu_x.fem_of_element(cv_x);
1606  size_type N = mfu_x.linked_mesh().dim();
1607  GMM_ASSERT1(mfu_x.get_qdim() == N,
1608  "Displacment field with wrong dimension");
1609 
1610  model_real_plain_vector coeff_x, coeff_y, stored_coeff_y;
1611  base_small_vector a(N-1), b(N-1), pt_x(N), pt_y(N), n_x(N);
1612  base_small_vector stored_pt_y(N), stored_n_y(N), stored_pt_y_ref(N);
1613  base_small_vector n0_x, n_y(N), n0_y, res(N-1), res2(N-1), dir(N-1);
1614  base_matrix G_x, G_y, grad(N,N), hessa(N-1, N-1);
1615  std::vector<base_small_vector> ti(N-1), Ti(N-1);
1616  scalar_type stored_signed_distance(0);
1617  std::string stored_dispname;
1618  scalar_type d0 = 1E300, d1, d2;
1619  const mesh *stored_m_y(0);
1620  size_type stored_cv_y(-1);
1621  short_type stored_face_y(-1);
1622  fem_interpolation_context stored_ctx_y;
1623 
1624  //
1625  // Computation of the deformed point and unit normal vectors
1626  //
1627  slice_vector_on_basic_dof_of_element(mfu_x, *(cb_x.U), cv_x, coeff_x);
1628  m_x.points_of_convex(cv_x, G_x);
1629  ctx_x.set_pf(pfu_x);
1630  pfu_x->interpolation(ctx_x, coeff_x, pt_x, dim_type(N));
1631  pt_x += ctx_x.xreal();
1632  compute_normal(ctx_x, face_x, false, coeff_x, n0_x, n_x, grad);
1633  n_x /= gmm::vect_norm2(n_x);
1634 
1635  //
1636  // Determine the nearest rigid obstacle, taking into account
1637  // the release distance.
1638  //
1639 
1640  bool first_pair_found = false;
1641  size_type irigid_obstacle(-1);
1642  for (size_type i = 0; i < obstacles.size(); ++i) {
1643  const obstacle &obs = obstacles[i];
1644  gmm::copy(pt_x, obs.point());
1645  const base_tensor &t = obs.eval();
1646 
1647  GMM_ASSERT1(t.size() == 1, "Obstacle level set function as to be "
1648  "a scalar valued one");
1649  d1 = t[0];
1650  // cout << "d1 = " << d1 << endl;
1651  if (gmm::abs(d1) < release_distance && d1 < d0) {
1652  const base_tensor &t_der = obs.eval_derivative();
1653  GMM_ASSERT1(t_der.size() == n_x.size(), "Bad derivative size");
1654  if (gmm::vect_sp(t_der.as_vector(), n_x) < scalar_type(0)) {
1655  d0 = d1;
1656  irigid_obstacle = i;
1657  gmm::copy(t_der.as_vector(), n_y);
1658  }
1659  }
1660  }
1661 
1662  if (irigid_obstacle != size_type(-1)) {
1663  // cout << "Testing obstacle " << irigid_obstacle << endl;
1664  const obstacle &obs = obstacles[irigid_obstacle];
1665  gmm::copy(pt_x, obs.point());
1666  gmm::copy(pt_x, pt_y);
1667  size_type nit = 0, nb_fail = 0;
1668  scalar_type alpha(0), beta(0);
1669  d1 = d0;
1670 
1671  while (gmm::abs(d1) > 1E-13 && ++nit < 50 && nb_fail < 3) {
1672  if (nit != 1) gmm::copy(obs.eval_derivative().as_vector(), n_y);
1673 
1674  for (scalar_type lambda(1); lambda >= 1E-3; lambda/=scalar_type(2)) {
1675  alpha = beta - lambda * d1 / gmm::vect_sp(n_y, n_x);
1676  gmm::add(pt_x, gmm::scaled(n_x, alpha), obs.point());
1677  d2 = obs.eval()[0];
1678  if (gmm::abs(d2) < gmm::abs(d1)) break;
1679  }
1680  if (gmm::abs(beta - d1 / gmm::vect_sp(n_y, n_x)) > scalar_type(500))
1681  nb_fail++;
1682  beta = alpha; d1 = d2;
1683  }
1684  gmm::copy(obs.point(), pt_y);
1685 
1686  if (gmm::abs(d1) > 1E-8) {
1687  GMM_WARNING1("Raytrace on rigid obstacle failed");
1688  } // CRITERION 4 for rigid bodies : Apply the release distance
1689  else if (gmm::vect_dist2(pt_y, pt_x) <= release_distance) {
1690  n_y /= gmm::vect_norm2(n_y);
1691  d0 = gmm::vect_dist2(pt_y, pt_x) * gmm::sgn(d0);
1692  stored_pt_y = stored_pt_y_ref = pt_y;
1693  stored_n_y = n_y;
1694  stored_signed_distance = d0;
1695  first_pair_found = true;
1696  } else
1697  irigid_obstacle = size_type(-1);
1698  }
1699 
1700  //
1701  // Determine the potential contact pairs with deformable bodies
1702  //
1703  bgeot::rtree::pbox_set bset;
1704  base_node bmin(pt_x), bmax(pt_x);
1705  for (size_type i = 0; i < N; ++i)
1706  { bmin[i] -= release_distance; bmax[i] += release_distance; }
1707 
1708  face_boxes.find_line_intersecting_boxes(pt_x, n_x, bmin, bmax, bset);
1709 
1710  //
1711  // Iteration on potential contact pairs and application
1712  // of selection criteria
1713  //
1714  for (const auto &pbox : bset) {
1715  face_box_info &fbox_y = face_boxes_info[pbox->id];
1716  size_type ib_y = fbox_y.ind_boundary;
1717  const contact_boundary &cb_y = contact_boundaries[ib_y];
1718  const mesh_fem &mfu_y = *(cb_y.mfu);
1719  const mesh &m_y = mfu_y.linked_mesh();
1720  size_type cv_y = fbox_y.ind_element;
1721  pfem pfu_y = mfu_y.fem_of_element(cv_y);
1722  short_type face_y = fbox_y.ind_face;
1723  bgeot::pgeometric_trans pgt_y= m_y.trans_of_convex(cv_y);
1724 
1725  // CRITERION 1 : The unit normal vector are compatible
1726  // and the two points are not in the same element.
1727  if (gmm::vect_sp(fbox_y.mean_normal, n_x) >= scalar_type(0) ||
1728  (cv_x == cv_y && &m_x == &m_y))
1729  continue;
1730 
1731  //
1732  // Raytrace search for y by Newton's algorithm
1733  //
1734 
1735  m_y.points_of_convex(cv_y, G_y);
1736  const base_node &Y0
1737  = pfu_y->ref_convex(cv_y)->points_of_face(face_y)[0];
1738  fem_interpolation_context ctx_y(pgt_y, pfu_y, Y0, G_y, cv_y, face_y);
1739 
1740  const base_small_vector &NY0
1741  = pfu_y->ref_convex(cv_y)->normals()[face_y];
1742  for (size_type k = 0; k < N-1; ++k) { // A basis for the face
1743  gmm::resize(ti[k], N);
1744  scalar_type norm(0);
1745  while(norm < 1E-5) {
1746  gmm::fill_random(ti[k]);
1747  ti[k] -= gmm::vect_sp(ti[k], NY0) * NY0;
1748  for (size_type l = 0; l < k; ++l)
1749  ti[k] -= gmm::vect_sp(ti[k], ti[l]) * ti[l];
1750  norm = gmm::vect_norm2(ti[k]);
1751  }
1752  ti[k] /= norm;
1753  }
1754 
1755  gmm::clear(a);
1756 
1757  for (size_type k = 0; k < N-1; ++k) {
1758  gmm::resize(Ti[k], N);
1759  scalar_type norm(0);
1760  while (norm < 1E-5) {
1761  gmm::fill_random(Ti[k]);
1762  Ti[k] -= gmm::vect_sp(Ti[k], n_x) * n_x;
1763  for (size_type l = 0; l < k; ++l)
1764  Ti[k] -= gmm::vect_sp(Ti[k], Ti[l]) * Ti[l];
1765  norm = gmm::vect_norm2(Ti[k]);
1766  }
1767  Ti[k] /= norm;
1768  }
1769 
1770  slice_vector_on_basic_dof_of_element(mfu_y, *(cb_y.U), cv_y, coeff_y);
1771 
1772  raytrace_pt_surf_cost_function_object pps(Y0, pt_x, ctx_y, coeff_y,
1773  ti, Ti, false);
1774  pps(a, res);
1775  scalar_type residual = gmm::vect_norm2(res);
1776  scalar_type residual2(0), det(0);
1777  bool exited = false;
1778  size_type nbfail = 0, niter = 0;
1779  for (;residual > 2E-12 && niter <= 30; ++niter) {
1780 
1781  for (size_type subiter(0); subiter <= 4; ++subiter) {
1782  pps(a, hessa);
1783  det = gmm::abs(bgeot::lu_inverse(&(*(hessa.begin())), N-1, false));
1784  if (det > 1E-15) break;
1785  for (size_type i = 0; i < N-1; ++i)
1786  a[i] += gmm::random() * 1E-7;
1787  }
1788  if (det <= 1E-15) break;
1789  // Computation of the descent direction
1790  gmm::mult(hessa, gmm::scaled(res, scalar_type(-1)), dir);
1791 
1792  if (gmm::vect_norm2(dir) > scalar_type(10)) nbfail++;
1793  if (nbfail >= 4) break;
1794 
1795  // Line search
1796  scalar_type lambda(1);
1797  for (size_type j = 0; j < 5; ++j) {
1798  gmm::add(a, gmm::scaled(dir, lambda), b);
1799  pps(b, res2);
1800  residual2 = gmm::vect_norm2(res2);
1801  if (residual2 < residual) break;
1802  lambda /= ((j < 3) ? scalar_type(2) : scalar_type(5));
1803  }
1804 
1805  residual = residual2;
1806  gmm::copy(res2, res);
1807  gmm::copy(b, a);
1808  scalar_type dist_ref = gmm::vect_norm2(a);
1809 
1810  if (niter > 1 && dist_ref > 15) break;
1811  if (niter > 5 && dist_ref > 8) break;
1812  if (/*(niter > 1 && dist_ref > 7) ||*/ nbfail == 3) exited = true;
1813  }
1814  bool converged = (gmm::vect_norm2(res) < 2E-6);
1815  bool is_in = (pfu_y->ref_convex(cv_y)->is_in(ctx_y.xref()) < 1E-6);
1816  // GMM_ASSERT1(!(exited && converged && is_in),
1817  // "A non conformal case !! " << gmm::vect_norm2(res)
1818  // << " : " << nbfail << " : " << niter);
1819  if (is_in) {
1820  ctx_y.pf()->interpolation(ctx_y, coeff_y, pt_y, dim_type(N));
1821  pt_y += ctx_y.xreal();
1822  }
1823 
1824  // CRITERION 2 : The contact pair is eliminated when
1825  // raytrace do not converge.
1826  if (!converged) continue;
1827 
1828  // CRITERION 3 : The raytraced point is inside the element
1829  if (!is_in) continue;
1830 
1831  // CRITERION 4 : Apply the release distance
1832  scalar_type signed_dist = gmm::vect_dist2(pt_y, pt_x);
1833  if (signed_dist > release_distance) continue;
1834 
1835  // compute the unit normal vector at y and the signed distance.
1836  compute_normal(ctx_y, face_y, false, coeff_y, n0_y, n_y, grad);
1837  n_y /= gmm::vect_norm2(n_y);
1838  signed_dist *= gmm::sgn(gmm::vect_sp(pt_x - pt_y, n_y));
1839 
1840  // CRITERION 5 : comparison with rigid obstacles
1841  // CRITERION 7 : smallest signed distance on contact pairs
1842  if (first_pair_found && stored_signed_distance < signed_dist)
1843  continue;
1844 
1845  // CRITERION 1 : again on found unit normal vector
1846  if (gmm::vect_sp(n_y, n_x) >= scalar_type(0)) continue;
1847 
1848  // CRITERION 6 : for self-contact only : apply a test on
1849  // unit normals in reference configuration.
1850  if (&m_x == &m_y) {
1851  base_small_vector diff = ctx_x.xreal() - ctx_y.xreal();
1852  scalar_type ref_dist = gmm::vect_norm2(diff);
1853  if ( (ref_dist < scalar_type(4) * release_distance)
1854  && (gmm::vect_sp(diff, n0_y) < - 0.01 * ref_dist) )
1855  continue;
1856  }
1857 
1858  stored_pt_y = pt_y; stored_pt_y_ref = ctx_y.xref();
1859  stored_m_y = &m_y; stored_cv_y = cv_y; stored_face_y = face_y;
1860  stored_n_y = n_y;
1861  stored_ctx_y = ctx_y;
1862  stored_coeff_y = coeff_y;
1863  stored_signed_distance = signed_dist;
1864  stored_dispname = cb_y.dispname;
1865  first_pair_found = true;
1866  irigid_obstacle = size_type(-1);
1867  }
1868 
1869  int ret_type = 0;
1870  *m_t = 0;
1871  cv = size_type(-1);
1872  face_num = short_type(-1);
1873  if (irigid_obstacle != size_type(-1)) {
1874  P_ref = stored_pt_y; N_y = stored_n_y;
1875  ret_type = 2;
1876  } else if (first_pair_found) {
1877  *m_t = stored_m_y; cv = stored_cv_y; face_num = stored_face_y;
1878  P_ref = stored_pt_y_ref; N_y = stored_n_y;
1879  ret_type = 1;
1880  }
1881 
1882  // Note on derivatives of the transformation : for efficiency and
1883  // simplicity reasons, the derivative should be computed with
1884  // the value of corresponding test functions. This means that
1885  // for a transformation F(u) the conputed derivative is F'(u).Test_u
1886  // including the Test_u.
1887  if (compute_derivatives) {
1888  if (ret_type >= 1) {
1889  fem_interpolation_context &ctx_y = stored_ctx_y;
1890  size_type cv_y = 0;
1891  if (ret_type == 1) cv_y = ctx_y.convex_num();
1892 
1893  base_matrix I_nxny(N,N); // I - nx@ny/nx.ny
1894  gmm::copy(gmm::identity_matrix(), I_nxny);
1895  gmm::rank_one_update(I_nxny, n_x,
1896  gmm::scaled(stored_n_y,scalar_type(-1)
1897  / gmm::vect_sp(n_x, stored_n_y)));
1898 
1899  // Computation of F_y
1900  base_matrix F_y(N,N), F_y_inv(N,N), M1(N, N), M2(N, N);
1901  pfem pfu_y = 0;
1902  if (ret_type == 1) {
1903  pfu_y = ctx_y.pf();
1904  pfu_y->interpolation_grad(ctx_y, stored_coeff_y, F_y, dim_type(N));
1905  gmm::add(gmm::identity_matrix(), F_y);
1906  gmm::copy(F_y, F_y_inv);
1907  bgeot::lu_inverse(&(*(F_y_inv.begin())), N);
1908  } else {
1909  gmm::copy(gmm::identity_matrix(), F_y);
1910  gmm::copy(gmm::identity_matrix(), F_y_inv);
1911  }
1912 
1913  // Computation of F_x
1914  base_matrix F_x(N,N), F_x_inv(N,N);
1915  pfu_x->interpolation_grad(ctx_x, coeff_x, F_x, dim_type(N));
1916  gmm::add(gmm::identity_matrix(), F_x);
1917  gmm::copy(F_x, F_x_inv);
1918  bgeot::lu_inverse(&(*(F_x_inv.begin())), N);
1919 
1920 
1921  base_tensor base_ux;
1922  base_matrix vbase_ux;
1923  ctx_x.base_value(base_ux);
1924  size_type qdim_ux = pfu_x->target_dim();
1925  size_type ndof_ux = pfu_x->nb_dof(cv_x) * N / qdim_ux;
1926  vectorize_base_tensor(base_ux, vbase_ux, ndof_ux, qdim_ux, N);
1927 
1928  base_tensor base_uy;
1929  base_matrix vbase_uy;
1930  size_type ndof_uy = 0;
1931  if (ret_type == 1) {
1932  ctx_y.base_value(base_uy);
1933  size_type qdim_uy = pfu_y->target_dim();
1934  ndof_uy = pfu_y->nb_dof(cv_y) * N / qdim_uy;
1935  vectorize_base_tensor(base_uy, vbase_uy, ndof_uy, qdim_uy, N);
1936  }
1937 
1938  base_tensor grad_base_ux, vgrad_base_ux;
1939  ctx_x.grad_base_value(grad_base_ux);
1940  vectorize_grad_base_tensor(grad_base_ux, vgrad_base_ux, ndof_ux,
1941  qdim_ux, N);
1942 
1943  // Derivative : F_y^{-1}*I_nxny*(Test_u(X)-Test_u(Y)+gDn_x[Test_u])
1944  // with Dn_x[Test_u] =-(I-nx@nx)*F_x^{-T}*Grad_Test_u^{T}*n_x
1945  // and I_nxny*(I - nx@nx) = I_nxny
1946 
1947  // F_y^{-1}*I_nxny*Test_u(X)
1948  gmm::mult(F_y_inv, I_nxny, M1);
1949  base_matrix der_x(ndof_ux, N);
1950  gmm::mult(vbase_ux, gmm::transposed(M1), der_x);
1951 
1952  // -F_y^{-1}*I_nxny*Test_u(Y)
1953  base_matrix der_y(ndof_uy, N);
1954  if (ret_type == 1) {
1955  gmm::mult(vbase_uy, gmm::transposed(M1), der_y);
1956  gmm::scale(der_y, scalar_type(-1));
1957  }
1958 
1959  // F_y^{-1}*I_nxny*gDn_x[Test_u]
1960  gmm::mult(M1, gmm::transposed(F_x_inv), M2);
1961  for (size_type i = 0; i < ndof_ux; ++i)
1962  for (size_type j = 0; j < N; ++j)
1963  for (size_type k = 0; k < N; ++k)
1964  for (size_type l = 0; l < N; ++l)
1965  der_x(i, j) -= M2(j, k) * vgrad_base_ux(i, l, k)
1966  * n_x[l] * stored_signed_distance;
1967 
1968  const std::string &dispname_x
1969  = workspace.variable_in_group(cb_x.dispname, m_x);
1970 
1971  for (auto&& d : derivatives) {
1972  if (dispname_x.compare(d.first.varname) == 0 &&
1973  d.first.transname.size() == 0) {
1974  d.second.adjust_sizes(ndof_ux, N);
1975  gmm::copy(der_x.as_vector(), d.second.as_vector());
1976  } else if (ret_type == 1 &&
1977  stored_dispname.compare(d.first.varname) == 0 &&
1978  d.first.transname.size() != 0) {
1979  d.second.adjust_sizes(ndof_uy, N);
1980  gmm::copy(der_y.as_vector(), d.second.as_vector());
1981  } else
1982  d.second.adjust_sizes(0, 0);
1983  }
1984  } else {
1985  for (auto&& d : derivatives)
1986  d.second.adjust_sizes(0, 0);
1987  }
1988  }
1989  return ret_type;
1990  }
1991 
1992  raytracing_interpolate_transformation(scalar_type d)
1993  : release_distance(d) {}
1994  };
1995 
1996 
1997  //=========================================================================
1998  //
1999  // Projection interpolate transformation for generic assembly
2000  //
2001  //=========================================================================
2002 
2003  class projection_interpolate_transformation
2004  : public raytracing_interpolate_transformation {
2005 
2006  scalar_type release_distance; // Limit distance beyond which the contact
2007  // will not be considered.
2008  public:
2009  int transform(const ga_workspace &workspace, const mesh &m_x,
2010  fem_interpolation_context &ctx_x,
2011  const base_small_vector &/*Normal*/,
2012  const mesh **m_t,
2013  size_type &cv, short_type &face_num, base_node &P_ref,
2014  base_small_vector &N_y,
2015  std::map<var_trans_pair, base_tensor> &derivatives,
2016  bool compute_derivatives) const {
2017  size_type cv_x = ctx_x.convex_num();
2018  short_type face_x = ctx_x.face_num();
2019  GMM_ASSERT1(face_x != short_type(-1), "The contact transformation can "
2020  "only be applied to a boundary");
2021 
2022  //
2023  // Find the right (slave) contact boundary
2024  //
2025  mesh_boundary_cor::const_iterator it = boundary_for_mesh.find(&m_x);
2026  GMM_ASSERT1(it != boundary_for_mesh.end(),
2027  "Mesh with no declared contact boundary");
2028  size_type ib_x = size_type(-1);
2029  for (const auto &boundary_ind : it->second) {
2030  const contact_boundary &cb = contact_boundaries[boundary_ind];
2031  if (m_x.region(cb.region).is_in(cv_x, face_x))
2032  { ib_x = boundary_ind; break; }
2033  }
2034  GMM_ASSERT1(ib_x != size_type(-1),
2035  "No contact region found for this point");
2036  const contact_boundary &cb_x = contact_boundaries[ib_x];
2037  const mesh_fem &mfu_x = *(cb_x.mfu);
2038  pfem pfu_x = mfu_x.fem_of_element(cv_x);
2039  size_type N = mfu_x.linked_mesh().dim();
2040  GMM_ASSERT1(mfu_x.get_qdim() == N,
2041  "Displacment field with wrong dimension");
2042 
2043  model_real_plain_vector coeff_x, coeff_y, stored_coeff_y;
2044  base_small_vector a(N-1), b(N-1), pt_x(N), pt_y(N), n_x(N);
2045  base_small_vector stored_pt_y(N), stored_n_y(N), stored_pt_y_ref(N);
2046  base_small_vector n0_x, n_y(N), n0_y, res(N-1), res2(N-1), dir(N-1);
2047  base_matrix G_x, G_y, grad(N,N), hessa(N-1, N-1);
2048  std::vector<base_small_vector> ti(N-1);
2049  scalar_type stored_signed_distance(0);
2050  std::string stored_dispname;
2051  scalar_type d0 = 1E300, d1, d2(0);
2052  const mesh *stored_m_y(0);
2053  size_type stored_cv_y(-1);
2054  short_type stored_face_y(-1);
2055  fem_interpolation_context stored_ctx_y;
2056  size_type nbwarn(0);
2057 
2058 
2059  //
2060  // Computation of the deformed point and unit normal vectors
2061  //
2062  slice_vector_on_basic_dof_of_element(mfu_x, *(cb_x.U), cv_x, coeff_x);
2063  bgeot::vectors_to_base_matrix(G_x, m_x.points_of_convex(cv_x));
2064  ctx_x.set_pf(pfu_x);
2065  pfu_x->interpolation(ctx_x, coeff_x, pt_x, dim_type(N));
2066  pt_x += ctx_x.xreal();
2067  compute_normal(ctx_x, face_x, false, coeff_x, n0_x, n_x, grad);
2068  n_x /= gmm::vect_norm2(n_x);
2069 
2070  //
2071  // Determine the nearest rigid obstacle, taking into account
2072  // the release distance.
2073  //
2074 
2075  bool first_pair_found = false;
2076  size_type irigid_obstacle(-1);
2077  for (size_type i = 0; i < obstacles.size(); ++i) {
2078  const obstacle &obs = obstacles[i];
2079  gmm::copy(pt_x, obs.point());
2080  const base_tensor &t = obs.eval();
2081 
2082  GMM_ASSERT1(t.size() == 1, "Obstacle level set function as to be "
2083  "a scalar valued one");
2084  d1 = t[0];
2085  // cout << "d1 = " << d1 << endl;
2086  if (gmm::abs(d1) < release_distance && d1 < d0) {
2087  const base_tensor &t_der = obs.eval_derivative();
2088  GMM_ASSERT1(t_der.size() == n_x.size(), "Bad derivative size");
2089  if (gmm::vect_sp(t_der.as_vector(), n_x) < scalar_type(0))
2090  { d0 = d1; irigid_obstacle = i; gmm::copy(t_der.as_vector(),n_y); }
2091  }
2092  }
2093 
2094  if (irigid_obstacle != size_type(-1)) {
2095  // cout << "Testing obstacle " << irigid_obstacle << endl;
2096  const obstacle &obs = obstacles[irigid_obstacle];
2097  gmm::copy(pt_x, obs.point());
2098  gmm::copy(pt_x, pt_y);
2099  size_type nit = 0, nb_fail = 0;
2100  scalar_type alpha(0), beta(0);
2101  d1 = d0;
2102 
2103  while (gmm::abs(d1) > 1E-13 && ++nit < 50 && nb_fail < 3) {
2104  if (nit != 1) gmm::copy(obs.eval_derivative().as_vector(), n_y);
2105 
2106  for (scalar_type lambda(1); lambda >= 1E-3; lambda/=scalar_type(2)) {
2107  gmm::add(gmm::scaled(n_y, -d1/gmm::vect_norm2_sqr(n_y)), pt_y, pt_x);
2108 
2109  if (gmm::abs(d2) < gmm::abs(d1)) break;
2110  }
2111  if (gmm::abs(beta - d1 / gmm::vect_sp(n_y, n_x)) > scalar_type(500))
2112  nb_fail++;
2113  beta = alpha; d1 = d2;
2114  }
2115  gmm::copy(obs.point(), pt_y);
2116 
2117  if (gmm::abs(d1) > 1E-8) {
2118  GMM_WARNING1("Raytrace on rigid obstacle failed");
2119  } // CRITERION 4 for rigid bodies : Apply the release distance
2120  else if (gmm::vect_dist2(pt_y, pt_x) <= release_distance) {
2121  n_y /= gmm::vect_norm2(n_y);
2122  d0 = gmm::vect_dist2(pt_y, pt_x) * gmm::sgn(d0);
2123  stored_pt_y = stored_pt_y_ref = pt_y; stored_n_y = n_y,
2124  stored_signed_distance = d0;
2125  first_pair_found = true;
2126  } else
2127  irigid_obstacle = size_type(-1);
2128  }
2129 
2130  //
2131  // Determine the potential contact pairs with deformable bodies
2132  //
2133  bgeot::rtree::pbox_set bset;
2134  base_node bmin(pt_x), bmax(pt_x);
2135  for (size_type i = 0; i < N; ++i)
2136  { bmin[i] -= release_distance; bmax[i] += release_distance; }
2137 
2138  face_boxes.find_line_intersecting_boxes(pt_x, n_x, bmin, bmax, bset);
2139  //
2140  // Iteration on potential contact pairs and application
2141  // of selection criteria
2142  //
2143  for (const auto &pbox : bset) {
2144  face_box_info &fbox_y = face_boxes_info[pbox->id];
2145  size_type ib_y = fbox_y.ind_boundary;
2146  const contact_boundary &cb_y = contact_boundaries[ib_y];
2147  const mesh_fem &mfu_y = *(cb_y.mfu);
2148  const mesh &m_y = mfu_y.linked_mesh();
2149  bool ref_conf=false;
2150  size_type cv_y = fbox_y.ind_element;
2151  pfem pfu_y = mfu_y.fem_of_element(cv_y);
2152  short_type face_y = fbox_y.ind_face;
2153  bgeot::pgeometric_trans pgt_y= m_y.trans_of_convex(cv_y);
2154 
2155  // CRITERION 1 : The unit normal vectors are compatible
2156  // and the two points are not in the same element.
2157  if (gmm::vect_sp(fbox_y.mean_normal, n_x) >= scalar_type(0) ||
2158  (cv_x == cv_y && &m_x == &m_y))
2159  continue;
2160 
2161  //
2162  // Classical projection for y by quasi Newton algorithm
2163  //
2164  bgeot::vectors_to_base_matrix(G_y, m_y.points_of_convex(cv_y));
2165  const base_node &Y0
2166  = pfu_y->ref_convex(cv_y)->points_of_face(face_y)[0];
2167  fem_interpolation_context ctx_y(pgt_y, pfu_y, Y0, G_y, cv_y, face_y);
2168 
2169  const base_small_vector &NY0
2170  = pfu_y->ref_convex(cv_y)->normals()[face_y];
2171 
2172  gmm::clear(a);
2173 
2174  for (size_type k = 0; k < N-1; ++k) { // A basis for the face
2175  gmm::resize(ti[k], N);
2176  scalar_type norm(0);
2177  while(norm < 1E-5) {
2178  gmm::fill_random(ti[k]);
2179  ti[k] -= gmm::vect_sp(ti[k], NY0) * NY0;
2180  for (size_type l = 0; l < k; ++l)
2181  ti[k] -= gmm::vect_sp(ti[k], ti[l]) * ti[l];
2182  norm = gmm::vect_norm2(ti[k]);
2183  }
2184  ti[k] /= norm;
2185  }
2186  slice_vector_on_basic_dof_of_element(mfu_y, *(cb_y.U), cv_y, coeff_y);
2187  proj_pt_surf_cost_function_object pps(Y0, pt_x, ctx_y, coeff_y, ti,
2188  1E-10, ref_conf);
2189  base_small_vector grada(N-1);
2190  scalar_type det(0);
2191  scalar_type residual = gmm::vect_norm2(res);
2192  scalar_type dist = pps(a, grada);
2193  pps(a, res);
2194  for (size_type niter = 0;
2195  gmm::vect_norm2(grada) > 1E-7 && niter <= 50; ++niter) {
2196 
2197  for (size_type subiter(0);;) {
2198  pps(a, hessa);
2199  det = gmm::abs(gmm::lu_inverse(hessa, false));
2200  if (det > 1E-15) break;
2201  for (size_type i = 0; i < N-1; ++i)
2202  a[i] += gmm::random() * 1E-7;
2203  if (++subiter > 4) break;
2204  }
2205  if (det <= 1E-15) break;
2206  // Computation of the descent direction
2207  gmm::mult(hessa, gmm::scaled(grada, scalar_type(-1)), dir);
2208 
2209  // Line search
2210  for (scalar_type lambda(1);
2211  lambda >= 1E-3; lambda /= scalar_type(2)) {
2212  gmm::add(a, gmm::scaled(dir, lambda), b);
2213  if (pps(b) < dist) break;
2214  gmm::add(a, gmm::scaled(dir, -lambda), b);
2215  if (pps(b) < dist) break;
2216  }
2217  //cout<< "b =" << b <<endl;
2218  gmm::copy(b, a);
2219  dist = pps(a, grada);
2220  }
2221 
2222  bool converged = (gmm::vect_norm2(grada) < 2E-6);
2223  if (!converged) { // Try with BFGS
2224  gmm::iteration iter(1E-10, 0 /* noisy*/, 100 /*maxiter*/);
2225  gmm::clear(a);
2226  gmm::bfgs(pps, pps, a, 10, iter, 0, 0.5);
2227  residual = gmm::abs(iter.get_res());
2228  converged = (residual < 2E-5);
2229  }
2230 
2231  bool is_in = (pfu_y->ref_convex(cv_y)->is_in(ctx_y.xref()) < 1E-6);
2232 
2233  //cout<< "y_ref =" << ctx_y.xref() <<endl;
2234  //cout<< "x_ref =" << ctx_x.xref() <<endl;
2235  // cout<< "y =" << ctx_y.xreal() <<endl;
2236  // cout<< "x =" << ctx_x.xreal() <<endl;
2237  // cout<< "is_in =" << is_in <<endl;
2238 
2239  if (is_in || (!converged )) {
2240  if (!ref_conf) {
2241  ctx_y.pf()->interpolation(ctx_y, coeff_y, pt_y, dim_type(N));
2242  pt_y += ctx_y.xreal();
2243  } else {
2244  pt_y = ctx_y.xreal();
2245  }
2246  }
2247  // CRITERION 2 : The contact pair is eliminated when
2248  // projection/raytrace do not converge.
2249  if (!converged) {
2250  if ( nbwarn < 4) {
2251  GMM_WARNING3("Projection algorithm did not converge "
2252  "for point " << pt_x << " residual " << residual
2253  << " projection computed " << pt_y);
2254  ++nbwarn;
2255  }
2256  continue;
2257  }
2258 
2259  // CRITERION 3 : The projected point is inside the element
2260  // The test should be completed: If the point is outside
2261  // the element, a rapid reprojection on the face
2262  // (on the reference element, with a linear algorithm)
2263  // can be applied and a test with a neigbhour element
2264  // to decide if the point is in fact ok ...
2265  // (to be done only if there is no projection on other
2266  // element which coincides and with a test on the
2267  // distance ... ?) To be specified (in this case,
2268  // change xref).
2269  if (!is_in) continue;
2270 
2271  // CRITERION 4 : Apply the release distance
2272 
2273  scalar_type signed_dist = gmm::vect_dist2(pt_y, pt_x);
2274  //cout<< "signd_dist="<< signed_dist << "relaese_dist="<< release_distance <<endl;
2275  if (signed_dist > release_distance) continue;
2276 
2277  // compute the unit normal vector at y and the signed distance.
2278  base_small_vector ny0(N);
2279  compute_normal(ctx_y, face_y, ref_conf, coeff_y, ny0, n_y, grad);
2280  // ny /= gmm::vect_norm2(ny); // Useful only if the unit normal is kept
2281  signed_dist *= gmm::sgn(gmm::vect_sp(pt_x - pt_y, n_y));
2282 
2283  // CRITERION 5 : comparison with rigid obstacles
2284  // CRITERION 7 : smallest signed distance on contact pairs
2285  if (first_pair_found && stored_signed_distance < signed_dist)
2286  continue;
2287 
2288  // CRITERION 1 : again on found unit normal vector
2289  if (gmm::vect_sp(n_y, n_x) >= scalar_type(0)) continue;
2290 
2291  // CRITERION 6 : for self-contact only : apply a test on
2292  // unit normals in reference configuration.
2293  if (&m_x == &m_y) {
2294 
2295  base_small_vector diff = ctx_x.xreal() - ctx_y.xreal();
2296  scalar_type ref_dist = gmm::vect_norm2(diff);
2297 
2298  if ( (ref_dist < scalar_type(4) * release_distance)
2299  && (gmm::vect_sp(diff, ny0) < - 0.01 * ref_dist) )
2300  continue;
2301  }
2302 
2303  stored_pt_y = pt_y; stored_pt_y_ref = ctx_y.xref();
2304  stored_m_y = &m_y; stored_cv_y = cv_y; stored_face_y = face_y;
2305  stored_n_y = n_y;
2306  stored_ctx_y = ctx_y;
2307  stored_coeff_y = coeff_y;
2308  stored_signed_distance = signed_dist;
2309  stored_dispname = cb_y.dispname;
2310  first_pair_found = true;
2311  irigid_obstacle = size_type(-1);
2312 
2313  // Projection could be ameliorated by finding a starting point near
2314  // x (with respect to the integration method, for instance).
2315 
2316  // A specific (Quasi) Newton algorithm for computing the projection
2317  }
2318 
2319  int ret_type = 0;
2320  *m_t = 0; cv = size_type(-1); face_num = short_type(-1);
2321  if (irigid_obstacle != size_type(-1)) {
2322  P_ref = stored_pt_y; N_y = stored_n_y;
2323  ret_type = 2;
2324  } else if (first_pair_found) {
2325  *m_t = stored_m_y; cv = stored_cv_y; face_num = stored_face_y;
2326  P_ref = stored_pt_y_ref;
2327  ret_type = 1;
2328  }
2329  // Note on derivatives of the transformation : for efficiency and
2330  // simplicity reasons, the derivative should be computed with
2331  // the value of corresponding test functions. This means that
2332  // for a transformation F(u) the conputed derivative is F'(u).Test_u
2333  // including the Test_u.
2334 
2335  if (compute_derivatives) {
2336  if (ret_type >= 1) {
2337  fem_interpolation_context &ctx_y = stored_ctx_y;
2338  size_type cv_y = 0;
2339  if (ret_type == 1) cv_y = ctx_y.convex_num();
2340 
2341 
2342  base_matrix I_nyny(N,N); // I - ny@ny
2343  gmm::copy(gmm::identity_matrix(), I_nyny);
2344  gmm::rank_one_update(I_nyny, stored_n_y,
2345  gmm::scaled(stored_n_y,scalar_type(-1)));
2346 
2347  // Computation of F_y
2348  base_matrix F_y(N,N), F_y_inv(N,N), M1(N, N), M2(N, N);
2349  pfem pfu_y = 0;
2350  if (ret_type == 1) {
2351  pfu_y = ctx_y.pf();
2352  pfu_y->interpolation_grad(ctx_y, stored_coeff_y, F_y, dim_type(N));
2353  gmm::add(gmm::identity_matrix(), F_y);
2354  gmm::copy(F_y, F_y_inv);
2355  gmm::lu_inverse(F_y_inv);
2356  } else {
2357  gmm::copy(gmm::identity_matrix(), F_y);
2358  gmm::copy(gmm::identity_matrix(), F_y_inv);
2359  }
2360 
2361 
2362  base_tensor base_ux;
2363  base_matrix vbase_ux;
2364  ctx_x.base_value(base_ux);
2365  size_type qdim_ux = pfu_x->target_dim();
2366  size_type ndof_ux = pfu_x->nb_dof(cv_x) * N / qdim_ux;
2367  vectorize_base_tensor(base_ux, vbase_ux, ndof_ux, qdim_ux, N);
2368 
2369  base_tensor base_uy;
2370  base_matrix vbase_uy;
2371  size_type ndof_uy = 0;
2372  if (ret_type == 1) {
2373  ctx_y.base_value(base_uy);
2374  size_type qdim_uy = pfu_y->target_dim();
2375  ndof_uy = pfu_y->nb_dof(cv_y) * N / qdim_uy;
2376  vectorize_base_tensor(base_uy, vbase_uy, ndof_uy, qdim_uy, N);
2377  }
2378 
2379  base_tensor grad_base_ux, vgrad_base_ux;
2380  ctx_x.grad_base_value(grad_base_ux);
2381  vectorize_grad_base_tensor(grad_base_ux, vgrad_base_ux, ndof_ux,
2382  qdim_ux, N);
2383 
2384  // Derivative : F_y^{-1}*I_nyny*(Test_u(X)-Test_u(Y))
2385  // and I_nyny*(I - ny@ny) = I_nyny
2386 
2387  // F_y^{-1}*I_nyny*Test_u(X)
2388  gmm::mult(F_y_inv, I_nyny, M1);
2389  base_matrix der_x(ndof_ux, N);
2390  gmm::mult(vbase_ux, gmm::transposed(M1), der_x);
2391 
2392  // -F_y^{-1}*I_nyny*Test_u(Y)
2393  base_matrix der_y(ndof_uy, N);
2394  if (ret_type == 1) {
2395  gmm::mult(vbase_uy, gmm::transposed(M1), der_y);
2396  gmm::scale(der_y, scalar_type(-1));
2397  }
2398 
2399  const std::string &dispname_x
2400  = workspace.variable_in_group(cb_x.dispname, m_x);
2401 
2402  for (auto&& d : derivatives) {
2403  if (dispname_x.compare(d.first.varname) == 0 &&
2404  d.first.transname.size() == 0) {
2405  d.second.adjust_sizes(ndof_ux, N);
2406  gmm::copy(der_x.as_vector(), d.second.as_vector());
2407  } else if (ret_type == 1 &&
2408  stored_dispname.compare(d.first.varname) == 0 &&
2409  d.first.transname.size() != 0) {
2410  d.second.adjust_sizes(ndof_uy, N);
2411  gmm::copy(der_y.as_vector(), d.second.as_vector());
2412  } else
2413  d.second.adjust_sizes(0, 0);
2414  }
2415  } else {
2416  for (auto&& d : derivatives)
2417  d.second.adjust_sizes(0, 0);
2418  }
2419  }
2420  return ret_type;
2421  }
2422  projection_interpolate_transformation(const scalar_type &d)
2423  :raytracing_interpolate_transformation(d), release_distance(d) {}
2424  };
2426  (model &md, const std::string &transname, scalar_type d) {
2427  pinterpolate_transformation
2428  p = std::make_shared<raytracing_interpolate_transformation>(d);
2429  md.add_interpolate_transformation(transname, p);
2430  }
2431 
2433  (ga_workspace &workspace, const std::string &transname, scalar_type d) {
2434  pinterpolate_transformation
2435  p = std::make_shared<raytracing_interpolate_transformation>(d);
2436  workspace.add_interpolate_transformation(transname, p);
2437  }
2438 
2440  (model &md, const std::string &transname, const mesh &m,
2441  const std::string &dispname, size_type region) {
2442  raytracing_interpolate_transformation *p
2443  = dynamic_cast<raytracing_interpolate_transformation *>
2444  (const_cast<virtual_interpolate_transformation *>
2445  (&(*(md.interpolate_transformation(transname)))));
2446  p->add_contact_boundary(md, m, dispname, region, false);
2447  }
2448 
2450  (model &md, const std::string &transname, const mesh &m,
2451  const std::string &dispname, size_type region) {
2452  raytracing_interpolate_transformation *p
2453  = dynamic_cast<raytracing_interpolate_transformation *>
2454  (const_cast<virtual_interpolate_transformation *>
2455  (&(*(md.interpolate_transformation(transname)))));
2456  p->add_contact_boundary(md, m, dispname, region, true);
2457  }
2458 
2460  (ga_workspace &workspace, const std::string &transname, const mesh &m,
2461  const std::string &dispname, size_type region) {
2462  raytracing_interpolate_transformation *p
2463  = dynamic_cast<raytracing_interpolate_transformation *>
2464  (const_cast<virtual_interpolate_transformation *>
2465  (&(*(workspace.interpolate_transformation(transname)))));
2466  p->add_contact_boundary(workspace, m, dispname, region, false);
2467  }
2468 
2470  (ga_workspace &workspace, const std::string &transname, const mesh &m,
2471  const std::string &dispname, size_type region) {
2472  raytracing_interpolate_transformation *p
2473  = dynamic_cast<raytracing_interpolate_transformation *>
2474  (const_cast<virtual_interpolate_transformation *>
2475  (&(*(workspace.interpolate_transformation(transname)))));
2476  p->add_contact_boundary(workspace, m, dispname, region, true);
2477  }
2478 
2480  (model &md, const std::string &transname,
2481  const std::string &expr, size_type N) {
2482  raytracing_interpolate_transformation *p
2483  = dynamic_cast<raytracing_interpolate_transformation *>
2484  (const_cast<virtual_interpolate_transformation *>
2485  (&(*(md.interpolate_transformation(transname)))));
2486  p->add_rigid_obstacle(md, expr, N);
2487  }
2488 
2490  (ga_workspace &workspace, const std::string &transname,
2491  const std::string &expr, size_type N) {
2492  raytracing_interpolate_transformation *p
2493  = dynamic_cast<raytracing_interpolate_transformation *>
2494  (const_cast<virtual_interpolate_transformation *>
2495  (&(*(workspace.interpolate_transformation(transname)))));
2496  p->add_rigid_obstacle(workspace, expr, N);
2497  }
2498 
2500  (model &md, const std::string &transname, scalar_type d) {
2501  pinterpolate_transformation
2502  p = std::make_shared<projection_interpolate_transformation>(d);
2503  md.add_interpolate_transformation(transname, p);
2504  }
2505 
2507  (ga_workspace &workspace, const std::string &transname, scalar_type d) {
2508  pinterpolate_transformation
2509  p = std::make_shared<projection_interpolate_transformation>(d);
2510  workspace.add_interpolate_transformation(transname, p);
2511  }
2512 
2514  (model &md, const std::string &transname, const mesh &m,
2515  const std::string &dispname, size_type region) {
2516  projection_interpolate_transformation *p
2517  = dynamic_cast<projection_interpolate_transformation *>
2518  (const_cast<virtual_interpolate_transformation *>
2519  (&(*(md.interpolate_transformation(transname)))));
2520  p->add_contact_boundary(md, m, dispname, region, false);
2521  }
2522 
2524  (model &md, const std::string &transname, const mesh &m,
2525  const std::string &dispname, size_type region) {
2526  projection_interpolate_transformation *p
2527  = dynamic_cast<projection_interpolate_transformation *>
2528  (const_cast<virtual_interpolate_transformation *>
2529  (&(*(md.interpolate_transformation(transname)))));
2530  p->add_contact_boundary(md, m, dispname, region, true);
2531  }
2532 
2534  (ga_workspace &workspace, const std::string &transname, const mesh &m,
2535  const std::string &dispname, size_type region) {
2536  projection_interpolate_transformation *p
2537  = dynamic_cast<projection_interpolate_transformation *>
2538  (const_cast<virtual_interpolate_transformation *>
2539  (&(*(workspace.interpolate_transformation(transname)))));
2540  p->add_contact_boundary(workspace, m, dispname, region, false);
2541  }
2542 
2544  (ga_workspace &workspace, const std::string &transname, const mesh &m,
2545  const std::string &dispname, size_type region) {
2546  projection_interpolate_transformation *p
2547  = dynamic_cast<projection_interpolate_transformation *>
2548  (const_cast<virtual_interpolate_transformation *>
2549  (&(*(workspace.interpolate_transformation(transname)))));
2550  p->add_contact_boundary(workspace, m, dispname, region, true);
2551  }
2552 
2554  (model &md, const std::string &transname,
2555  const std::string &expr, size_type N) {
2556  projection_interpolate_transformation *p
2557  = dynamic_cast<projection_interpolate_transformation *>
2558  (const_cast<virtual_interpolate_transformation *>
2559  (&(*(md.interpolate_transformation(transname)))));
2560  p->add_rigid_obstacle(md, expr, N);
2561  }
2562 
2564  (ga_workspace &workspace, const std::string &transname,
2565  const std::string &expr, size_type N) {
2566  projection_interpolate_transformation *p
2567  = dynamic_cast<projection_interpolate_transformation *>
2568  (const_cast<virtual_interpolate_transformation *>
2569  (&(*(workspace.interpolate_transformation(transname)))));
2570  p->add_rigid_obstacle(workspace, expr, N);
2571  }
2572 
2573 
2574 
2575  //=========================================================================
2576  //
2577  // Specific nonlinear operator of the high-level generic assembly language
2578  // dedicated to contact/friction
2579  //
2580  //=========================================================================
2581 
2582  // static void ga_init_scalar(bgeot::multi_index &mi) { mi.resize(0); }
2583  static void ga_init_vector(bgeot::multi_index &mi, size_type N)
2584  { mi.resize(1); mi[0] = N; }
2585  // static void ga_init_square_matrix(bgeot::multi_index &mi, size_type N)
2586  // { mi.resize(2); mi[0] = mi[1] = N; }
2587 
2588 
2589  // Transformed_unit_vector(Grad_u, n) = (I+Grad_u)^{-T}n / ||(I+Grad_u)^{-T}n||
2590  struct Transformed_unit_vector : public ga_nonlinear_operator {
2591  bool result_size(const arg_list &args, bgeot::multi_index &sizes) const {
2592  if (args.size() != 2 || args[0]->sizes().size() != 2
2593  || args[1]->size() != args[0]->sizes()[0]
2594  || args[0]->sizes()[0] != args[0]->sizes()[1]) return false;
2595  ga_init_vector(sizes, args[0]->sizes()[0]);
2596  return true;
2597  }
2598 
2599  // Value : (I+Grad_u)^{-T}n / ||(I+Grad_u)^{-T}n||
2600  void value(const arg_list &args, base_tensor &result) const {
2601  size_type N = args[0]->sizes()[0];
2602  base_matrix F(N, N);
2603  gmm::copy(args[0]->as_vector(), F.as_vector());
2604  gmm::add(gmm::identity_matrix(), F);
2605  bgeot::lu_inverse(&(*(F.begin())), N);
2606  gmm::mult(gmm::transposed(F), args[1]->as_vector(), result.as_vector());
2607  gmm::scale(result.as_vector(),
2608  scalar_type(1)/gmm::vect_norm2(result.as_vector()));
2609  }
2610 
2611  // Derivative / Grad_u: -(I - n@n)(I+Grad_u)^{-T}Test_Grad_u n
2612  // Implementation: A{ijk} = -G{ik}ndef{j}
2613  // with G = (I - n@n)(I+Grad_u)^{-T}
2614  // and ndef the transformed normal
2615  // Derivative / n: ((I+Grad_u)^{-T}Test_n
2616  // - ndef(ndef.Test_n))/||(I+Grad_u)^{-T}n||
2617  // Implementation: A{ij} = (F{ij} - ndef{i}ndef{j})/norm_ndef
2618  // with F = (I+Grad_u)^{-1}
2619  void derivative(const arg_list &args, size_type nder,
2620  base_tensor &result) const {
2621  size_type N = args[0]->sizes()[0];
2622  base_matrix F(N, N), G(N, N);
2623  base_small_vector ndef(N), aux(N);
2624  gmm::copy(args[0]->as_vector(), F.as_vector());
2625  gmm::add(gmm::identity_matrix(), F);
2626  bgeot::lu_inverse(&(*(F.begin())), N);
2627  gmm::mult(gmm::transposed(F), args[1]->as_vector(), ndef);
2628  scalar_type norm_ndef = gmm::vect_norm2(ndef);
2629  gmm::scale(ndef, scalar_type(1)/norm_ndef);
2630  gmm::copy(gmm::transposed(F), G);
2631  gmm::mult(F, ndef, aux);
2632  gmm::rank_one_update(G, gmm::scaled(ndef, scalar_type(-1)), aux);
2633  base_tensor::iterator it = result.begin();
2634  switch (nder) {
2635  case 1:
2636  for (size_type k = 0; k < N; ++k)
2637  for (size_type j = 0; j < N; ++j)
2638  for (size_type i = 0; i < N; ++i, ++it)
2639  *it = -G(i, k) * ndef[j];
2640  break;
2641  case 2:
2642  for (size_type j = 0; j < N; ++j)
2643  for (size_type i = 0; i < N; ++i, ++it)
2644  *it = (F(j,i) - ndef[i]*ndef[j])/norm_ndef;
2645  break;
2646  default: GMM_ASSERT1(false, "Internal error");
2647  }
2648  GMM_ASSERT1(it == result.end(), "Internal error");
2649  }
2650 
2651  // Second derivative : not implemented
2652  void second_derivative(const arg_list &, size_type, size_type,
2653  base_tensor &) const {
2654  GMM_ASSERT1(false, "Sorry, second derivative not implemented");
2655  }
2656  };
2657 
2658 
2659  // Coulomb_friction_coupled_projection(lambda, n, Vs, g, f, r)
2660  struct Coulomb_friction_coupled_projection : public ga_nonlinear_operator {
2661  bool result_size(const arg_list &args, bgeot::multi_index &sizes) const {
2662  if (args.size() != 6) return false;
2663  size_type N = args[0]->size();
2664  if (N == 0 || args[1]->size() != N || args[2]->size() != N
2665  || args[3]->size() != 1 || args[4]->size() > 3
2666  || args[4]->size() == 0 || args[5]->size() != 1 ) return false;
2667  ga_init_vector(sizes, N);
2668  return true;
2669  }
2670 
2671  // Value : (lambda.n+rg)_- n - P_B(n, f(lambda.n+rg)_-)(lambda-r Vs)
2672  void value(const arg_list &args, base_tensor &result) const {
2673  const base_vector &lambda = *(args[0]);
2674  const base_vector &n = *(args[1]);
2675  const base_vector &Vs = *(args[2]);
2676  base_vector &F = result;
2677  scalar_type g = (*(args[3]))[0];
2678  const base_vector &f = *(args[4]);
2679  scalar_type r = (*(args[5]))[0];
2680 
2681 
2682  scalar_type nn = gmm::vect_norm2(n);
2683  scalar_type lambdan = gmm::vect_sp(lambda, n)/nn;
2684  scalar_type lambdan_aug = gmm::neg(lambdan + r * g);
2685  size_type s_f = gmm::vect_size(f);
2686  scalar_type tau = ((s_f >= 3) ? f[2] : scalar_type(0)) + f[0]*lambdan_aug;
2687  if (s_f >= 2) tau = std::min(tau, f[1]);
2688 
2689  if (tau > scalar_type(0)) {
2690  gmm::add(lambda, gmm::scaled(Vs, -r), F);
2691  scalar_type mu = gmm::vect_sp(F, n)/nn;
2692  gmm::add(gmm::scaled(n, -mu/nn), F);
2693  scalar_type norm = gmm::vect_norm2(F);
2694  if (norm > tau) gmm::scale(F, tau / norm);
2695  } else { gmm::clear(F); }
2696 
2697  gmm::add(gmm::scaled(n, -lambdan_aug/nn), F);
2698  }
2699 
2700  // Derivative / Grad_u: -(I - n@n)(I+Grad_u)^{-T}Test_Grad_u n
2701  // Implementation: A{ijk} = -G{kj}ndef{i}
2702  // with G = (I - n@n)(I+Grad_u)^{-T}
2703  // and ndef the transformed normal
2704  // Derivative / n: ((I+Grad_u)^{-T}Test_n
2705  // - ndef(ndef.Test_n))/||(I+Grad_u)^{-T}n||
2706  // Implementation: A{ij} = (F{ij} - ndef{i}ndef{j})/norm_ndef
2707  // with F = (I+Grad_u)^{-1}
2708  void derivative(const arg_list &args, size_type nder,
2709  base_tensor &result) const { // Can be optimized ?
2710  size_type N = args[0]->size();
2711  const base_vector &lambda = *(args[0]);
2712  const base_vector &n = *(args[1]);
2713  const base_vector &Vs = *(args[2]);
2714  base_vector F(N), dg(N);
2715  base_matrix dVs(N,N), dn(N,N);
2716  scalar_type g = (*(args[3]))[0];
2717  const base_vector &f = *(args[4]);
2718  scalar_type r = (*(args[5]))[0];
2719 
2720  scalar_type nn = gmm::vect_norm2(n);
2721  scalar_type lambdan = gmm::vect_sp(lambda, n)/nn;
2722  scalar_type lambdan_aug = gmm::neg(lambdan + r * g);
2723  size_type s_f = gmm::vect_size(f);
2724  scalar_type tau = ((s_f >= 3) ? f[2] : scalar_type(0))+f[0]*lambdan_aug;
2725  if (s_f >= 2) tau = std::min(tau, f[1]);
2726  scalar_type norm(0);
2727 
2728  if (tau > scalar_type(0)) {
2729  gmm::add(lambda, gmm::scaled(Vs, -r), F); // F <-- lambda -r*Vs
2730  scalar_type mu = gmm::vect_sp(F, n)/nn; // mu <-- (lambda -r*Vs).n/|n|
2731  gmm::add(gmm::scaled(n, -mu/nn), F); // F <-- (lambda -r*Vs)*(I-n x n / |n|²)
2732  norm = gmm::vect_norm2(F);
2733  gmm::copy(gmm::identity_matrix(), dn); // dn <-- I
2734  gmm::scale(dn, -mu/nn); // dn <-- -(lambda -r*Vs).n/|n|² I
2735  gmm::rank_one_update(dn, gmm::scaled(n, mu/(nn*nn*nn)), n); // dn <-- -(lambda -r*Vs).n/|n|² (I - n x n/|n|²)
2736  gmm::rank_one_update(dn, gmm::scaled(n, scalar_type(-1)/(nn*nn)), F); // dn <-- -(lambda -r*Vs).n/|n|² (I - n x n/|n|²) + n x ((lambda -r*Vs)*(I-n x n / |n|²)) /|n|²
2737  gmm::copy(gmm::identity_matrix(), dVs); // dVs <-- I
2738  gmm::rank_one_update(dVs, n, gmm::scaled(n, scalar_type(-1)/(nn*nn))); // dVs <-- I - n x n/|n|²
2739 
2740  if (norm > tau) { // slip
2741  gmm::rank_one_update(dVs, F,
2742  gmm::scaled(F, scalar_type(-1)/(norm*norm)));
2743  gmm::scale(dVs, tau / norm);
2744  gmm::copy(gmm::scaled(F, scalar_type(1)/norm), dg); // dg <-- Normalized((lambda -r*Vs)*(I-n x n / |n|²))
2745  gmm::rank_one_update(dn, gmm::scaled(F, mu/(norm*norm*nn)), F);
2746  gmm::scale(dn, tau / norm);
2747  gmm::scale(F, tau / norm);
2748  } // else gmm::clear(dg);
2749 
2750  } // else { gmm::clear(dg); gmm::clear(dVs); gmm::clear(F); gmm::clear(dn); }
2751  // At this stage, F = P_{B_T}, dVs = d_q P_{B_T}, dn = d_n P_{B_T}
2752  // and dg = d_tau P_{B_T}.
2753 
2754 
2755  base_tensor::iterator it = result.begin();
2756  switch (nder) {
2757  case 1: // Derivative with respect to lambda
2758  if (norm > tau && ((s_f <= 1) || tau < f[1]) &&
2759  ((s_f <= 2) || tau > f[2]))
2760  gmm::rank_one_update(dVs, dg, gmm::scaled(n, -f[0]/nn));
2761  if (lambdan_aug > scalar_type(0))
2762  gmm::rank_one_update(dVs, n, gmm::scaled(n, scalar_type(1)/(nn*nn)));
2763  for (size_type j = 0; j < N; ++j)
2764  for (size_type i = 0; i < N; ++i, ++it)
2765  *it = dVs(i, j);
2766  break;
2767  case 2: // Derivative with respect to n
2768  if (norm > tau && ((s_f <= 1) || tau < f[1])
2769  && ((s_f <= 2) || tau > f[2])) {
2770  gmm::rank_one_update(dn, dg, gmm::scaled(lambda, -f[0]/nn));
2771  gmm::rank_one_update(dn, dg, gmm::scaled(n, f[0]*lambdan/(nn*nn)));
2772  }
2773  if (lambdan_aug > scalar_type(0)) {
2774  gmm::rank_one_update(dn, gmm::scaled(n, scalar_type(1)/(nn*nn)),
2775  lambda);
2776  gmm::rank_one_update(dn,
2777  gmm::scaled(n,(lambdan_aug-lambdan)/(nn*nn*nn)), n);
2778  for (size_type j = 0; j < N; ++j) dn(j,j) -= lambdan_aug/nn;
2779  }
2780  for (size_type j = 0; j < N; ++j)
2781  for (size_type i = 0; i < N; ++i, ++it)
2782  *it = dn(i, j);
2783  break;
2784  case 3:
2785  gmm::scale(dVs, -r);
2786  for (size_type j = 0; j < N; ++j)
2787  for (size_type i = 0; i < N; ++i, ++it)
2788  *it = dVs(i, j);
2789  break;
2790  case 4:
2791  if (norm > tau && ((s_f <= 1) || tau < f[1])
2792  && ((s_f <= 2) || tau > f[2]))
2793  gmm::scale(dg, -f[0]*r);
2794  else
2795  gmm::clear(dg);
2796  if (lambdan_aug > scalar_type(0))
2797  gmm::add(gmm::scaled(n, r/nn), dg);
2798  for (size_type i = 0; i < N; ++i, ++it)
2799  *it = dg[i];
2800  break;
2801  case 5:
2802  if (norm > tau && ((s_f <= 1) || tau < f[1])
2803  && ((s_f <= 2) || tau > f[2]))
2804  gmm::scale(dg, -f[0]*g);
2805  else
2806  gmm::clear(dg);
2807  gmm::mult_add(dVs, gmm::scaled(Vs, scalar_type(-1)), dg);
2808  if (lambdan_aug > scalar_type(0))
2809  gmm::add(gmm::scaled(n, g/nn), dg);
2810  for (size_type i = 0; i < N; ++i, ++it)
2811  *it = dg[i];
2812  it = result.end();
2813  break;
2814  case 6:
2815  base_small_vector dtau_df(s_f);
2816  if ((s_f <= 1) || tau < f[1]) dtau_df[0] = lambdan_aug;
2817  if (s_f >= 2 && tau == f[1]) dtau_df[1] = 1;
2818  if (s_f >= 3 && tau < f[1]) dtau_df[2] = 1;
2819  for (size_type j = 0; j < s_f; ++j)
2820  for (size_type i = 0; i < N; ++i, ++it)
2821  *it = dg[i] * dtau_df[j];
2822  break;
2823  }
2824  GMM_ASSERT1(it == result.end(), "Internal error");
2825  }
2826 
2827  // Second derivative : not implemented
2828  void second_derivative(const arg_list &, size_type, size_type,
2829  base_tensor &) const {
2830  GMM_ASSERT1(false, "Sorry, second derivative not implemented");
2831  }
2832  };
2833 
2834  static bool init_predef_operators() {
2835 
2836  ga_predef_operator_tab &PREDEF_OPERATORS
2838 
2839  PREDEF_OPERATORS.add_method("Transformed_unit_vector",
2840  std::make_shared<Transformed_unit_vector>());
2841  PREDEF_OPERATORS.add_method("Coulomb_friction_coupled_projection",
2842  std::make_shared<Coulomb_friction_coupled_projection>());
2843 
2844  return true;
2845  }
2846 
2847  // declared in getfem_generic_assembly.cc
2848  bool predef_operators_contact_initialized
2849  = init_predef_operators();
2850 
2851 } /* end of namespace getfem. */
dal::singleton::instance
static T & instance()
Instance from the current thread.
Definition: dal_singleton.h:165
bgeot::compute_normal
base_small_vector compute_normal(const geotrans_interpolation_context &c, size_type face)
norm of returned vector is the ratio between the face surface on the real element and the face surfac...
Definition: bgeot_geometric_trans.cc:1082
getfem::add_rigid_obstacle_to_projection_transformation
void add_rigid_obstacle_to_projection_transformation(model &md, const std::string &transname, const std::string &expr, size_type N)
Add a rigid obstacle whose geometry corresponds to the zero level-set of the high-level generic assem...
Definition: getfem_contact_and_friction_common.cc:2554
gmm::resize
void resize(M &v, size_type m, size_type n)
*‍/
Definition: gmm_blas.h:231
getfem::add_projection_transformation
void add_projection_transformation(model &md, const std::string &transname, scalar_type release_distance)
Add a projection interpolate transformation called 'transname' to a model to be used by the generic a...
Definition: getfem_contact_and_friction_common.cc:2500
bgeot::size_type
size_t size_type
used as the common size type in the library
Definition: bgeot_poly.h:49
gmm::clear
void clear(L &l)
clear (fill with zeros) a vector or matrix.
Definition: gmm_blas.h:59
getfem::add_master_contact_boundary_to_projection_transformation
void add_master_contact_boundary_to_projection_transformation(model &md, const std::string &transname, const mesh &m, const std::string &dispname, size_type region)
Add a master boundary with corresponding displacement variable 'dispname' on a specific boundary 'reg...
Definition: getfem_contact_and_friction_common.cc:2514
gmm::vect_dist2
number_traits< typename linalg_traits< V1 >::value_type >::magnitude_type vect_dist2(const V1 &v1, const V2 &v2)
Euclidean distance between two vectors.
Definition: gmm_blas.h:597
getfem::mesh_fem
Describe a finite element method linked to a mesh.
Definition: getfem_mesh_fem.h:148
getfem::model
`‘Model’' variables store the variables, the data and the description of a model.
Definition: getfem_models.h:114
getfem::add_slave_contact_boundary_to_raytracing_transformation
void add_slave_contact_boundary_to_raytracing_transformation(model &md, const std::string &transname, const mesh &m, const std::string &dispname, size_type region)
Add a slave boundary with corresponding displacement variable 'dispname' on a specific boundary 'regi...
Definition: getfem_contact_and_friction_common.cc:2450
bgeot::short_type
gmm::uint16_type short_type
used as the common short type integer in the library
Definition: bgeot_config.h:72
bgeot::rtree
Balanced tree of n-dimensional rectangles.
Definition: bgeot_rtree.h:98
gmm::iteration
The Iteration object calculates whether the solution has reached the desired accuracy,...
Definition: gmm_iter.h:53
getfem
GEneric Tool for Finite Element Methods.
Definition: getfem_accumulated_distro.h:46
getfem_generic_assembly.h
A language for generic assembly of pde boundary value problems.
getfem::mesh_region::visitor
"iterator" class for regions.
Definition: getfem_mesh_region.h:237
getfem::model::add_interpolate_transformation
void add_interpolate_transformation(const std::string &name, pinterpolate_transformation ptrans)
Add an interpolate transformation to the model to be used with the generic assembly.
Definition: getfem_models.h:1106
getfem::add_raytracing_transformation
void add_raytracing_transformation(model &md, const std::string &transname, scalar_type release_distance)
Add a raytracing interpolate transformation called 'transname' to a model to be used by the generic a...
Definition: getfem_contact_and_friction_common.cc:2426
gmm::vect_norm2
number_traits< typename linalg_traits< V >::value_type >::magnitude_type vect_norm2(const V &v)
Euclidean norm of a vector.
Definition: gmm_blas.h:557
getfem::pfem
std::shared_ptr< const getfem::virtual_fem > pfem
type of pointer on a fem description
Definition: getfem_fem.h:244
getfem::add_master_contact_boundary_to_raytracing_transformation
void add_master_contact_boundary_to_raytracing_transformation(model &md, const std::string &transname, const mesh &m, const std::string &dispname, size_type region)
Add a master boundary with corresponding displacement variable 'dispname' on a specific boundary 'reg...
Definition: getfem_contact_and_friction_common.cc:2440
gmm::fill_random
void fill_random(L &l, double cfill)
*‍/
Definition: gmm_blas.h:154
bgeot::alpha
size_type alpha(short_type n, short_type d)
Return the value of which is the number of monomials of a polynomial of variables and degree .
Definition: bgeot_poly.cc:47
bgeot::pconvex_structure
std::shared_ptr< const convex_structure > pconvex_structure
Pointer on a convex structure description.
Definition: bgeot_convex_structure.h:54
getfem::slice_vector_on_basic_dof_of_element
void slice_vector_on_basic_dof_of_element(const mesh_fem &mf, const VEC1 &vec, size_type cv, VEC2 &coeff, size_type qmult1=size_type(-1), size_type qmult2=size_type(-1))
Given a mesh_fem.
Definition: getfem_mesh_fem.h:659
getfem::add_slave_contact_boundary_to_projection_transformation
void add_slave_contact_boundary_to_projection_transformation(model &md, const std::string &transname, const mesh &m, const std::string &dispname, size_type region)
Add a slave boundary with corresponding displacement variable 'dispname' on a specific boundary 'regi...
Definition: getfem_contact_and_friction_common.cc:2524
getfem::mesh
Describe a mesh (collection of convexes (elements) and points).
Definition: getfem_mesh.h:95
bgeot::pgeometric_trans
std::shared_ptr< const bgeot::geometric_trans > pgeometric_trans
pointer type for a geometric transformation
Definition: bgeot_geometric_trans.h:186
getfem::model::interpolate_transformation
pinterpolate_transformation interpolate_transformation(const std::string &name) const
Get a pointer to the interpolate transformation name.
Definition: getfem_models.h:1120
getfem_contact_and_friction_common.h
Comomon tools for unilateral contact and Coulomb friction bricks.
gmm::mult_add
void mult_add(const L1 &l1, const L2 &l2, L3 &l3)
*‍/
Definition: gmm_blas.h:1781
getfem::add_rigid_obstacle_to_raytracing_transformation
void add_rigid_obstacle_to_raytracing_transformation(model &md, const std::string &transname, const std::string &expr, size_type N)
Add a rigid obstacle whose geometry corresponds to the zero level-set of the high-level generic assem...
Definition: getfem_contact_and_friction_common.cc:2480