scontact.c 50.2 KB
Newer Older
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
1
/*
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
2
 * MigFlow - Copyright (C) <2010-2020>
3
4
 * <Universite catholique de Louvain (UCL), Belgium
 *  Universite de Montpellier, France>
Michel Henry's avatar
Michel Henry committed
5
 *
Nathan Coppin's avatar
Nathan Coppin committed
6
 * List of the contributors to the development of MigFlow: see AUTHORS file.
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
7
 * Description and complete License: see LICENSE file.
Michel Henry's avatar
Michel Henry committed
8
9
10
 *
 * This program (MigFlow) is free software:
 * you can redistribute it and/or modify it under the terms of the GNU Lesser General
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
11
12
 * Public License as published by the Free Software Foundation, either version
 * 3 of the License, or (at your option) any later version.
Michel Henry's avatar
Michel Henry committed
13
 *
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
14
15
16
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
17
 * GNU Lesser General Public License for more details.
Michel Henry's avatar
Michel Henry committed
18
 *
19
 * You should have received a copy of the GNU Lesser General Public License
Michel Henry's avatar
Michel Henry committed
20
 * along with this program (see COPYING and COPYING.LESSER files).  If not,
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
21
22
23
 * see <http://www.gnu.org/licenses/>.
 */

Matthieu Constant's avatar
Matthieu Constant committed
24
25
26
#include "quadtree.h"
#include "vector.h"
#include <math.h>
Nathan Coppin's avatar
Nathan Coppin committed
27
#include <stdint.h>
Matthieu Constant's avatar
Matthieu Constant committed
28
29
30
31
#include <stdio.h>
#include <stdlib.h>
#include <float.h>
#include <string.h>
32
#include <time.h>
Matthieu Constant's avatar
Matthieu Constant committed
33
34

typedef struct _Particle Particle;
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
35
typedef struct _ParticleProblem ParticleProblem;
Nathan Coppin's avatar
Nathan Coppin committed
36
typedef struct _Body Body;
Matthieu Constant's avatar
Matthieu Constant committed
37
38
39
typedef struct _Contact Contact;
typedef struct _Disk Disk;
typedef struct _Segment Segment;
Michel Henry's avatar
Michel Henry committed
40
typedef struct _Triangle Triangle;
41
typedef struct _PeriodicEntity PeriodicEntity;
Michel Henry's avatar
Michel Henry committed
42
typedef struct _PeriodicSegment PeriodicSegment;
43
typedef struct _PeriodicTriangle PeriodicTriangle;
Matthieu Constant's avatar
Matthieu Constant committed
44
45
46

struct _ParticleProblem{
  Particle *particles;
Nathan Coppin's avatar
Nathan Coppin committed
47
  Body *bodies;
Matthieu Constant's avatar
Matthieu Constant committed
48
49
50
  Contact *contacts;
  Disk *disks;
  Segment *segments;
Michel Henry's avatar
Michel Henry committed
51
  PeriodicEntity *periodicEntities;
Michel Henry's avatar
Michel Henry committed
52
  PeriodicSegment *periodicSegments;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
53
  double *mu;
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
54
  int n_material;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
55
#if DIMENSION == 3
Matthieu Constant's avatar
Matthieu Constant committed
56
  Triangle *triangles;
Michel Henry's avatar
Michel Henry committed
57
  PeriodicTriangle *periodicTriangles;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
58
#endif
59
  int use_queue;
Matthieu Constant's avatar
Matthieu Constant committed
60
61
62
63
};

struct _Particle{
  double r;
Nathan Coppin's avatar
Nathan Coppin committed
64
65
  size_t body;
  double x[DIMENSION];
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
66
  int material;
Nathan Coppin's avatar
Nathan Coppin committed
67
68
69
};

struct _Body{
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
70
  double invertM;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
71
  double invertI;
Jonathan Lambrechts's avatar
theta    
Jonathan Lambrechts committed
72
73
  // 3D : double invertI[6];
  double theta;
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
74
75
76
77
78
#if DIMENSION==3
  double omega[3];
#else
  double omega;
#endif
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
79
80
81
82
83
84
85
86
87
88
89
90
91
92
  double position[DIMENSION];
  double velocity[DIMENSION];
};

typedef struct {
  int material;
  int tag;
}Boundary;

struct _Disk {
  Boundary b;
  double x[DIMENSION];
  double v[DIMENSION];
  double r;
Matthieu Constant's avatar
Matthieu Constant committed
93
94
};

Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
struct _Segment{
  Boundary b;
  double p[2][DIMENSION];
  double v[DIMENSION];
};

struct _Triangle {
  Boundary b;
  double p[3][DIMENSION];
  double v[DIMENSION];
};

#if DIMENSION == 3
typedef enum {PARTICLE_PARTICLE=0, PARTICLE_DISK, PARTICLE_SEGMENT, PARTICLE_TRIANGLE} ContactType;
#else
typedef enum {PARTICLE_PARTICLE=0, PARTICLE_DISK, PARTICLE_SEGMENT} ContactType;
#endif

Matthieu Constant's avatar
Matthieu Constant committed
113
struct _Contact {
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
114
  // constant over one time step
Matthieu Constant's avatar
Matthieu Constant committed
115
  size_t o0, o1;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
116
  ContactType type;
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
117
118
  double D0; // initial interpenetration (should be ~0)
  // contact state
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
119
  double r0[DIMENSION], r1[DIMENSION]; // vector from body center to contact point
120
  double reaction[DIMENSION];          // force on o1 expressed in global basis
Matthieu Constant's avatar
Matthieu Constant committed
121
122
};

Michel Henry's avatar
Michel Henry committed
123
124
125
126
127
128
struct _PeriodicEntity{
  int etag;
  int edim;
  double transform[DIMENSION];
};

Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
struct _PeriodicSegment{
  size_t entity_id;
  double p[2][DIMENSION];
};
#if DIMENSION == 3
struct _PeriodicTriangle {
  size_t entity_id;
  double p[3][DIMENSION];
};
#endif

static double dot(const double *x, const double *y) {
  #if DIMENSION == 3
  return x[0] * y[0] +  x[1] * y[1] + x[2] * y[2];
  #else
  return x[0] * y[0] +  x[1] * y[1];
  #endif
}

/* Particle */
149
static void particle_bounding_box(const Particle *p, const double x[DIMENSION], double pmin[DIMENSION], double pmax[DIMENSION]){
Matthieu Constant's avatar
Matthieu Constant committed
150
  for (int i = 0; i < DIMENSION; ++i) {
Jonathan Lambrechts's avatar
theta    
Jonathan Lambrechts committed
151
152
    pmin[i] = x[i] - p->r;
    pmax[i] = x[i] + p->r;
Matthieu Constant's avatar
Matthieu Constant committed
153
154
  }
}
155
static double particle_problem_get_mu(const ParticleProblem *p, int mat0, int mat1) {
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
156
  return p->mu[mat0*p->n_material+mat1];
Nathan Coppin's avatar
Nathan Coppin committed
157
}
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
158
static void _cross (const double *a, const double *b, double *c) {
Nathan Coppin's avatar
Nathan Coppin committed
159
160
161
#if DIMENSION==2
  c[0] = a[0]*b[1] - a[1]*b[0];
#else
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
162
163
164
  c[0] = a[1] * b[2] - a[2] * b[1];
  c[1] = a[2] * b[0] - a[0] * b[2];
  c[2] = a[0] * b[1] - a[1] * b[0];
Nathan Coppin's avatar
Nathan Coppin committed
165
#endif
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
166
167
}

Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
168
169
// fill basis[1] and basis[2] from basis[0]
static void basis_from_normal_vector(double basis[DIMENSION][DIMENSION]) {
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
170
#if DIMENSION==3
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
171
172
173
174
  basis[1][0] = -basis[0][2];
  basis[1][1] = basis[0][0];
  basis[1][2] = -basis[0][1];
  double ddot = dot(basis[0],basis[1]);
Nathan Coppin's avatar
Nathan Coppin committed
175
176
  double norm = 0;
  for(int i = 0;i<3;i++){
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
177
178
    basis[1][i] -= ddot*basis[0][i];
    norm += basis[1][i]*basis[1][i];
Nathan Coppin's avatar
Nathan Coppin committed
179
180
181
  }
  norm = norm == 0 ? 1 : sqrt(norm);
  for(int i = 0;i<3;i++){
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
182
    basis[1][i] /= norm;
Nathan Coppin's avatar
Nathan Coppin committed
183
  }
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
184
  _cross(basis[0], basis[1], basis[2]);
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
185
#else
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
186
187
  basis[1][0] = -basis[0][1];
  basis[1][1] = basis[0][0];
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
188
189
190
#endif
}

Jonathan Lambrechts's avatar
theta    
Jonathan Lambrechts committed
191
static void particle_get_position(const ParticleProblem *p, const Particle *particle, double x[DIMENSION]) {
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
192
  Body *body = &p->bodies[particle->body];
Jonathan Lambrechts's avatar
theta    
Jonathan Lambrechts committed
193
  const double *xp = particle->x;
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
194
  double theta = body->theta;
Jonathan Lambrechts's avatar
theta    
Jonathan Lambrechts committed
195
#if DIMENSION == 2
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
196
197
  x[0] = body->position[0] + cos(theta)*xp[0] - sin(theta)*xp[1];
  x[1] = body->position[1] + sin(theta)*xp[0] + cos(theta)*xp[1];
Jonathan Lambrechts's avatar
theta    
Jonathan Lambrechts committed
198
199
200
201
202
#else
  // TODO
#endif
}

Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
203
void particle_problem_get_particles_position(ParticleProblem *p, double *x){
Nathan Coppin's avatar
Nathan Coppin committed
204
  for(int i = 0;i<vectorSize(p->particles);i++){
Jonathan Lambrechts's avatar
theta    
Jonathan Lambrechts committed
205
    particle_get_position(p, &p->particles[i], x+i*DIMENSION);
Nathan Coppin's avatar
Nathan Coppin committed
206
207
208
  }
}

Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
209
void particle_problem_get_particles_velocity(ParticleProblem *p, double *v){
Nathan Coppin's avatar
Nathan Coppin committed
210
  for(int i = 0;i<vectorSize(p->particles);i++){
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
211
    const Body *body = &p->bodies[p->particles[i].body];
Nathan Coppin's avatar
Nathan Coppin committed
212
213
214
    double r[3], res[3], omega[3];
    for (int d = 0;d<DIMENSION;d++){
#if DIMENSION==3
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
215
      omega[d] = p->bodies[p->particles[i].body].omega[DIMENSION+d];
Nathan Coppin's avatar
Nathan Coppin committed
216
217
218
219
220
221
#else
      omega[d] = 0;
#endif
      r[d] = p->particles[i].x[d];
    }
#if DIMENSION==2
Jonathan Lambrechts's avatar
fix    
Jonathan Lambrechts committed
222
    r[2] = 0; // TODO FIXME !!!! cross does not work in 2D
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
223
    omega[2] =  p->bodies[p->particles[i].body].omega;
Nathan Coppin's avatar
Nathan Coppin committed
224
225
226
#endif
    _cross(omega,r,res);
    for(int d = 0;d<DIMENSION;d++){
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
227
      v[i*DIMENSION + d] = body->velocity[d] + res[d];
Nathan Coppin's avatar
Nathan Coppin committed
228
229
230
231
    }
  }
}

Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
232
233
234
static double contact_update_particle_particle(const Contact *c, const ParticleProblem *p,
                                               const double *x0, const double *x1,
                                               double basis[DIMENSION][DIMENSION], double r0[DIMENSION], double r1[DIMENSION]) {
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
235
236
  const Particle *p0 = &p->particles[c->o0];
  const Particle *p1 = &p->particles[c->o1];
Matthieu Constant's avatar
Matthieu Constant committed
237
238
  double nnorm = 0;
  for (int k = 0; k < DIMENSION; ++k) {
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
239
240
    basis[0][k] = x1[k] - x0[k];
    nnorm += basis[0][k] * basis[0][k];
Matthieu Constant's avatar
Matthieu Constant committed
241
242
  }
  nnorm = sqrt(nnorm);
243
  for (int i = 0; i < DIMENSION; ++i)
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
244
245
    basis[0][i] /= nnorm;
  basis_from_normal_vector(basis);
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
246
247
  const double *xbody0 = p->bodies[p0->body].position;
  const double *xbody1 = p->bodies[p1->body].position;
Nathan Coppin's avatar
Nathan Coppin committed
248
  for (int i = 0;i<DIMENSION;i++){
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
249
    r0[i] = x0[i]-xbody0[i] + basis[0][i]*p0->r;
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
250
    r1[i] = x1[i]-xbody1[i] - basis[0][i]*p1->r;
Nathan Coppin's avatar
Nathan Coppin committed
251
  }
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
252
  return nnorm - (p0->r + p1->r);
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
253
254
255
256
257
258
}


static int contact_init_particle_particle(Contact *c, const ParticleProblem *p, size_t id0, const double *x0, size_t id1, const double *x1, double alert) {
  const Particle *p0 = &p->particles[id0];
  const Particle *p1 = &p->particles[id1];
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
259
260
  const Body *body0 = &p->bodies[p0->body];
  const Body *body1 = &p->bodies[p1->body];
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
261
262
  if(p0->body==p1->body)
    return 0;
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
263
  if(body0->invertM == 0 && body1->invertM == 0 && body0->invertI == 0 && body1->invertI == 0)
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
264
    return 0;
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
265
266
  c->o0 = id0;
  c->o1 = id1;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
267
  for (int i = 0; i < DIMENSION; ++i) {
268
269
270
    c->reaction[i] = 0;
    c->r0[i] = 0;
    c->r1[i] = 0;
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
271
  }
Matthieu Constant's avatar
Matthieu Constant committed
272
  c->type = PARTICLE_PARTICLE;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
273
  double basis[DIMENSION][DIMENSION];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
274
  double D = contact_update_particle_particle(c, p, x0, x1, basis, c->r0, c->r1);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
275
276
  c->D0 = fmin(D, 0);
  return D < alert;
Matthieu Constant's avatar
Matthieu Constant committed
277
}
Nathan Coppin's avatar
Nathan Coppin committed
278

279
static void disk_bounding_box(const Disk *d, double *pmin, double *pmax) {
Matthieu Constant's avatar
Matthieu Constant committed
280
281
282
283
284
285
286
  const double r = fabs(d->r);
  for (int i = 0; i < DIMENSION; ++i) {
    pmin[i] = d->x[i] - r;
    pmax[i] = d->x[i] + r;
  }
}

287

Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
288
size_t particle_problem_add_boundary_disk(ParticleProblem *p, const double x[DIMENSION], double r, int tag, int materialTag) {
Matthieu Constant's avatar
Matthieu Constant committed
289
  Disk *d = vectorPush(&p->disks);
Jonathan Lambrechts's avatar
clean    
Jonathan Lambrechts committed
290
291
  d->b.tag = tag;
  d->b.material = materialTag;
Matthieu Constant's avatar
Matthieu Constant committed
292
293
294
  d->r = r;
  for (int i = 0; i < DIMENSION; ++i) {
    d->x[i] = x[i];
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
295
    d->v[i] = 0.;
Matthieu Constant's avatar
Matthieu Constant committed
296
297
298
299
  }
  return vectorSize(p->disks) - 1;
}

Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
300
static double contact_update_disk_particle(const Contact *c, const ParticleProblem *p, double *x, double basis[DIMENSION][DIMENSION], double r0[DIMENSION], double r1[DIMENSION]) {
Matthieu Constant's avatar
Matthieu Constant committed
301
  double nnorm = 0;
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
302
303
  const Particle *p1 = &p->particles[c->o1];
  const Disk *d = &p->disks[c->o0];
Matthieu Constant's avatar
Matthieu Constant committed
304
  for (int i = 0; i < DIMENSION; ++i) {
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
305
306
    basis[0][i] = x[i] - d->x[i];
    nnorm += basis[0][i] * basis[0][i];
Matthieu Constant's avatar
Matthieu Constant committed
307
308
309
  }
  nnorm = sqrt(nnorm);
  for (int i = 0; i < DIMENSION; ++i) {
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
310
    basis[0][i] /= (nnorm == 0 ? 1 : nnorm) * (d->r < 0 ? -1 : 1);
Matthieu Constant's avatar
Matthieu Constant committed
311
  }
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
312
  basis_from_normal_vector(basis);
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
313
  const double *xbody = p->bodies[p1->body].position;
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
314
  for (int i = 0;i<DIMENSION;i++){
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
315
316
    r0[i] = 0; // todo change when disks move
    r1[i] = (x[i]-xbody[i] -basis[0][i]*p1->r);
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
317
  }
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
318
  return (nnorm - fabs(p1->r + d->r)) * (d->r < 0 ? -1 : 1);
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
319
320
321
322
323
}

static int contact_init_disk_particle(Contact *c, ParticleProblem *p, size_t disk_id, size_t particle_id, double *x, double alert) {
  const Disk *d = &p->disks[disk_id];
  const Particle *particle = &p->particles[particle_id];
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
324
325
  const Body *body = &p->bodies[particle->body];
  if (body->invertI == 0 && body->invertM == 0)
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
326
    return 0;
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
327
328
  c->o0 = disk_id;
  c->o1 = particle_id;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
329
  for (int i = 0; i < DIMENSION; ++i) {
330
331
332
    c->reaction[i] = 0;
    c->r0[i] = 0;
    c->r1[i] = 0;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
333
  }
Matthieu Constant's avatar
Matthieu Constant committed
334
  c->type = PARTICLE_DISK;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
335
  double basis[DIMENSION][DIMENSION];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
336
  double D = contact_update_disk_particle(c, p, x, basis, c->r0, c->r1);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
337
338
  c->D0 = fmin(D, 0);
  return D < alert;
Matthieu Constant's avatar
Matthieu Constant committed
339
}
Michel Henry's avatar
Michel Henry committed
340

341
static void periodic_segment_bounding_box(const PeriodicSegment *s, double *pmin, double *pmax) {
Michel Henry's avatar
Michel Henry committed
342
343
344
345
346
347
  for (int i = 0; i < DIMENSION; ++i) {
    pmin[i] = fmin(s->p[0][i], s->p[1][i]);
    pmax[i] = fmax(s->p[0][i], s->p[1][i]);
  }
}

Nathan Coppin's avatar
Nathan Coppin committed
348
static void coordAdd(double *x, double a, const double *y) {
Nathan Coppin's avatar
Nathan Coppin committed
349
350
351
352
353
354
  x[0] += a * y[0];
  x[1] += a * y[1];
#if DIMENSION == 3
  x[2] += a * y[2];
#endif
}
Nathan Coppin's avatar
Nathan Coppin committed
355

356
static void segment_bounding_box(const Segment *s, double *pmin, double *pmax) {
Matthieu Constant's avatar
Matthieu Constant committed
357
358
359
360
361
362
  for (int i = 0; i < DIMENSION; ++i) {
    pmin[i] = fmin(s->p[0][i], s->p[1][i]);
    pmax[i] = fmax(s->p[0][i], s->p[1][i]);
  }
}

Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
363
static double contact_update_segment_particle(const Contact *c, const ParticleProblem *p, double *x1, double basis[DIMENSION][DIMENSION], double r0[DIMENSION], double r1[DIMENSION]) {
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
364
365
  const Segment *s = &p->segments[c->o0];
  const Particle *p1 = &p->particles[c->o1];
Matthieu Constant's avatar
Matthieu Constant committed
366
367
  double nt2 = 0;
  double dt = 0;
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
368
  double t[DIMENSION];
Matthieu Constant's avatar
Matthieu Constant committed
369
370
  for (int i = 0; i <DIMENSION; ++i){
    t[i] = s->p[1][i] - s->p[0][i];
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
371
    dt += t[i] * (s->p[0][i] - x1[i]);
372
    nt2 += t[i]*t[i];
Matthieu Constant's avatar
Matthieu Constant committed
373
374
  }
  double nn2 = 0;
375
  double xi = dt/nt2;
Matthieu Constant's avatar
Matthieu Constant committed
376
  for (int i = 0; i < DIMENSION; ++i) {
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
377
378
    basis[0][i] = x1[i] -(s->p[0][i]-t[i]*xi);
    nn2 += basis[0][i] * basis[0][i];
Matthieu Constant's avatar
Matthieu Constant committed
379
  }
Nathan Coppin's avatar
Nathan Coppin committed
380
  const double nnorm = nn2 == 0 ? 1 : sqrt(nn2);
Matthieu Constant's avatar
Matthieu Constant committed
381
  for (int i = 0; i < DIMENSION; ++i) {
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
382
    basis[0][i] /= nnorm;
Matthieu Constant's avatar
Matthieu Constant committed
383
  }
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
384
  basis_from_normal_vector(basis);
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
385
  const double *xbody = p->bodies[p1->body].position;
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
386
  for (int i = 0;i<DIMENSION;i++){
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
387
388
    r0[i] = 0; // todo change if segment moves
    r1[i] = x1[i] - basis[0][i]*p1->r -xbody[i];
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
389
  }
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
390
  return nnorm - p1->r;
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
391
392
393
394
395
396
397
}

static int contact_init_segment_particle(Contact *c, ParticleProblem *p, size_t segment_id, size_t particle_id, double *x, double alert) {
  const Segment *s = &p->segments[segment_id];
  double xcheck[3];
  particle_get_position(p, &p->particles[particle_id], xcheck);
  const Particle *particle = &p->particles[particle_id];
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
398
399
  const Body *body= &p->bodies[particle->body];
  if (body->invertI == 0 && body->invertM == 0)
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
400
    return 0;
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
401
402
  c->o0 = segment_id;
  c->o1 = particle_id;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
403
  for (int i = 0; i < DIMENSION; ++i) {
404
405
406
    c->reaction[i] = 0;
    c->r0[i] = 0;
    c->r1[i] = 0;
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
407
  }
Matthieu Constant's avatar
Matthieu Constant committed
408
  c->type = PARTICLE_SEGMENT;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
409
  double basis[DIMENSION][DIMENSION];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
410
  double D = contact_update_segment_particle(c, p, x, basis, c->r0, c->r1);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
411
412
  c->D0 = fmin(D, 0);
  return D < alert;
Matthieu Constant's avatar
Matthieu Constant committed
413
414
}

415

Matthieu Constant's avatar
Matthieu Constant committed
416
#if DIMENSION == 3
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
417
void particle_problem_add_boundary_triangle(ParticleProblem *p, const double x0[3], const double x1[3], const double x2[3], int tag, int materialTag) {
Matthieu Constant's avatar
Matthieu Constant committed
418
  Triangle *t = vectorPush(&p->triangles);
Jonathan Lambrechts's avatar
clean    
Jonathan Lambrechts committed
419
420
  t->b.tag = tag;
  t->b.material = materialTag;
Matthieu Constant's avatar
Matthieu Constant committed
421
422
423
424
  for (int i = 0; i < DIMENSION; ++i) {
    t->p[0][i] = x0[i];
    t->p[1][i] = x1[i];
    t->p[2][i] = x2[i];
425
    t->v[i] = 0.;
Matthieu Constant's avatar
Matthieu Constant committed
426
427
428
  }
}

429
size_t particle_problem_add_boundary_periodic_triangle(ParticleProblem  *p, const double x0[DIMENSION], const double x1[DIMENSION], const double x2[DIMENSION], const int etag){
Michel Henry's avatar
Michel Henry committed
430
  PeriodicTriangle *t = vectorPush(&p->periodicTriangles);
Michel Henry's avatar
Michel Henry committed
431
  t->entity_id = -1;
Michel Henry's avatar
Michel Henry committed
432
  for(size_t ie = 0; ie < vectorSize(p->periodicEntities); ++ie){
Michel Henry's avatar
Michel Henry committed
433
    PeriodicEntity *pe = &p->periodicEntities[ie];
Michel Henry's avatar
Michel Henry committed
434
    if(etag == pe->etag){
Michel Henry's avatar
Michel Henry committed
435
      t->entity_id = ie;
Michel Henry's avatar
Michel Henry committed
436
437
438
      break;
    }
  }
Michel Henry's avatar
Michel Henry committed
439
  if (t->entity_id == -1) printf("Entity not found in periodic triangles !\n");
Michel Henry's avatar
Michel Henry committed
440
441
442
443
444
445
446
447
  for(int i = 0; i < DIMENSION; ++i){
    t->p[0][i] = x0[i];
    t->p[1][i] = x1[i];
    t->p[2][i] = x2[i];
  }
  return vectorSize(p->periodicTriangles) - 1;
}

448
static void periodic_triangle_bounding_box(const PeriodicTriangle *t, double *pmin, double *pmax) {
Michel Henry's avatar
Michel Henry committed
449
450
451
452
453
454
455
  for (int i = 0; i < 3; ++i) {
    pmin[i] = fmin(fmin(t->p[0][i], t->p[1][i]), t->p[2][i]);
    pmax[i] = fmax(fmax(t->p[0][i], t->p[1][i]), t->p[2][i]);
  }
}


456
static void triangle_bounding_box(const Triangle *t, double *pmin, double *pmax) {
Matthieu Constant's avatar
Matthieu Constant committed
457
458
459
460
461
462
  for (int i = 0; i < 3; ++i) {
    pmin[i] = fmin(fmin(t->p[0][i], t->p[1][i]), t->p[2][i]);
    pmax[i] = fmax(fmax(t->p[0][i], t->p[1][i]), t->p[2][i]);
  }
}

463
464
static int contact_init_triangle_particle(Contact *c, const ParticleProblem *p,
    size_t triangle_id, size_t particle_id, double *x, double alert) {
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
465
  double basis[DIMENSION][DIMENSION];
466
  const Particle *particle = &p->particles[particle_id];
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
467
468
  const Body *body = &p->bodies[particle->body];
  if (body->invertI == 0 && body->invertM == 0)
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
469
    return 0;
470
471
472
  const Triangle *t = &p->triangles[triangle_id];
  c->o0 = triangle_id;
  c->o1 = particle_id;
Matthieu Constant's avatar
Matthieu Constant committed
473
474
475
476
477
  double d0[3] = {t->p[1][0] - t->p[0][0], t->p[1][1] - t->p[0][1], t->p[1][2] - t->p[0][2]};
  double d1[3] = {t->p[2][0] - t->p[0][0], t->p[2][1] - t->p[0][1], t->p[2][2] - t->p[0][2]};
  double N[3];
  _cross(d0, d1, N);
  const double nn = sqrt(dot(N, N));
478
  for (int i = 0; i < 3; ++i){
479
    c->reaction[i] = 0;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
480
    basis[0][i] = N[i] / (nn == 0 ? 1 : nn);
481
482
    c->r0[i] = 0;
    c->r1[i] = 0;
483
  }
Nathan Coppin's avatar
Nathan Coppin committed
484
  double dd[3] = {x[0]+particle->x[0] - t->p[0][0],x[1] + particle->x[1]- t->p[0][1],x[2] + particle->x[2]- t->p[0][2]};
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
485
486
  double D = dot(basis[0], dd);
  if (D < 0) {
Matthieu Constant's avatar
Matthieu Constant committed
487
    for (int i = 0; i <3; ++i)
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
488
      basis[0][i] = -basis[0][i];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
489
    D = -D;
Matthieu Constant's avatar
Matthieu Constant committed
490
  }
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
491
  basis_from_normal_vector(basis);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
492
  D -= particle->r;
Nathan Coppin's avatar
Nathan Coppin committed
493
494
  for (int i = 0;i<DIMENSION;i++){
    c->r0[i] = 0;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
495
    c->r1[i] = (particle->x[i] - basis[0][i]*particle->r);
Nathan Coppin's avatar
Nathan Coppin committed
496
  }
Matthieu Constant's avatar
Matthieu Constant committed
497
  c->type = PARTICLE_TRIANGLE;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
498
499
  c->D0 = fmin(D, 0);
  return D < alert;
Matthieu Constant's avatar
Matthieu Constant committed
500
501
502
}
#endif

Nathan Coppin's avatar
Nathan Coppin committed
503
static void contact_get_omega_pointers(const Contact *c, ParticleProblem *p, double **omega0, double **omega1) {
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
504
505
506
507
508
#if DIMENSION == 2
  *omega1 = &p->bodies[p->particles[c->o1].body].omega;
#else
  *omega1 = p->bodies[p->particles[c->o1].body].omega;
#endif
509
  if (c->type == PARTICLE_PARTICLE) {
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
510
511
512
513
514
#if DIMENSION == 2
    *omega0 = &p->bodies[p->particles[c->o0].body].omega;
#else
    *omega0 = p->bodies[p->particles[c->o0].body].omega;
#endif
515
516
517
518
  }
  else {
    *omega0 = NULL;
  }
519
520
}

521
522

static void contact_get_velocity_pointers(const Contact *c, ParticleProblem *p, double **v0, double **v1) {
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
523
  *v1 = p->bodies[p->particles[c->o1].body].velocity;
524
  if (c->type == PARTICLE_PARTICLE) {
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
525
    *v0 = p->bodies[p->particles[c->o0].body].velocity;
Nathan Coppin's avatar
Nathan Coppin committed
526
  }
527
528
529
530
531
  else if (c->type == PARTICLE_DISK) {
    *v0 = p->disks[c->o0].v;
  }
  else if (c->type == PARTICLE_SEGMENT) {
    *v0 = p->segments[c->o0].v;
532
  }
Nathan Coppin's avatar
haha    
Nathan Coppin committed
533
#if DIMENSION == 3
534
535
536
  else if (c->type == PARTICLE_TRIANGLE) {
    *v0 = p->triangles[c->o0].v;
  }
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
537
#endif
Matthieu Constant's avatar
Matthieu Constant committed
538
539
}

540
static void contact_apply(const Contact *c, ParticleProblem *p, double dt) {
541
542
  double *v1, *v0;
  contact_get_velocity_pointers(c, p, &v0, &v1);
Nathan Coppin's avatar
Nathan Coppin committed
543
544
  double *omega0, *omega1;
  contact_get_omega_pointers(c, p, &omega0, &omega1);
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
545
  double imass1 = p->bodies[p->particles[c->o1].body].invertM;
Jonathan Lambrechts's avatar
fix    
Jonathan Lambrechts committed
546
  for (int i = 0; i < DIMENSION; ++i) {
547
    v1[i] += imass1*(dt*c->reaction[i]);
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
548
549
550
551
  }
  if(c->type == PARTICLE_PARTICLE) {
    double imass0 = p->bodies[p->particles[c->o0].body].invertM;
    for (int i = 0; i < DIMENSION; ++i) {
552
      v0[i] -= imass0*(dt*c->reaction[i]);
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
553
    }
Jonathan Lambrechts's avatar
fix    
Jonathan Lambrechts committed
554
  }
555
#if DIMENSION == 2
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
556
557
  if(omega0) {
    double iinertmom0 = p->bodies[p->particles[c->o0].body].invertI;
558
    omega0[0] -= iinertmom0*(c->r0[0]*(dt*c->reaction[1]) - c->r0[1]*(dt*c->reaction[0]));
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
559
560
  }
  double iinertmom1 = p->bodies[p->particles[c->o1].body].invertI;
561
  omega1[0] += iinertmom1*(c->r1[0]*(dt*c->reaction[1]) - c->r1[1]*(dt*c->reaction[0]));
562
#else
Nathan Coppin's avatar
Nathan Coppin committed
563
//TODO
Nathan Coppin's avatar
Nathan Coppin committed
564
#endif
565
}
Jonathan Lambrechts's avatar
broken    
Jonathan Lambrechts committed
566

Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
567
static void contact_get_local_velocity(Contact *c, ParticleProblem *p, double *v, double basis[DIMENSION][DIMENSION]) {
Jonathan Lambrechts's avatar
fix    
Jonathan Lambrechts committed
568
569
570
  // v1 - v0 in local basis
  double *v0, *v1;
  contact_get_velocity_pointers(c, p, &v0, &v1);
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
571
572
  double *omega0, *omega1;
  contact_get_omega_pointers(c, p, &omega0, &omega1);
Jonathan Lambrechts's avatar
fix    
Jonathan Lambrechts committed
573
#if DIMENSION==2
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
574
  double vrot0[2] = {0};
Jonathan Lambrechts's avatar
fix    
Jonathan Lambrechts committed
575
  double vrot1[2];
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
576
577
578
579
580
581
  if (omega0) {
    vrot0[0] = -(*omega0)*c->r0[1];
    vrot0[1] = (*omega0)*c->r0[0];
  }
  vrot1[0] = -(*omega1)*c->r1[1];
  vrot1[1] = (*omega1)*c->r1[0];
Jonathan Lambrechts's avatar
fix    
Jonathan Lambrechts committed
582
583
  for(int d0 = 0; d0< DIMENSION; ++d0) {
    for(int d = 0; d<DIMENSION; d++){
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
584
      v[d0] += (v1[d]+vrot1[d]-v0[d]-vrot0[d]) * basis[d0][d];
Nathan Coppin's avatar
Nathan Coppin committed
585
    }
586
  }
Nathan Coppin's avatar
Nathan Coppin committed
587
#else
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
588
589
590
  double res0[3]={0}, res1[3];
  if(omega0)
    _cross(omega0,c->r0,res0);
Jonathan Lambrechts's avatar
fix    
Jonathan Lambrechts committed
591
592
593
  _cross(omega1,c->r1,res1);
  for(int d = 0;d<DIMENSION;d++){
    for(int i = 0;i<DIMENSION;i++)
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
594
      v[d] += (v1[i]+res1[i] -v0[i]-res0[i]) * basis[d][i];
Jonathan Lambrechts's avatar
fix    
Jonathan Lambrechts committed
595
  }
Nathan Coppin's avatar
Nathan Coppin committed
596
#endif
Matthieu Constant's avatar
Matthieu Constant committed
597
}
598

Jonathan Lambrechts's avatar
clean    
Jonathan Lambrechts committed
599

Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
600
const Contact *findContactSorted(Contact *c, const Contact *v, size_t *i) {
Matthieu Constant's avatar
Matthieu Constant committed
601
  while (*i < vectorSize(v) && (v[*i].type < c->type || v[*i].o0 < c->o0 || (v[*i].o0 == c->o0 && v[*i].o1 < c->o1)))
Matthieu Constant's avatar
Matthieu Constant committed
602
    (*i)++;
Matthieu Constant's avatar
Matthieu Constant committed
603
  return (*i < vectorSize(v) && v[*i].type == c->type && v[*i].o0 == c->o0 && v[*i].o1 == c->o1) ? &v[*i] : NULL;
Matthieu Constant's avatar
Matthieu Constant committed
604
605
606
607
608
609
610
}

static void addAlert(double alert, double *rmin, double *rmax) {
  rmin[0] -= alert;
  rmin[1] -= alert;
  rmax[0] += alert;
  rmax[1] += alert;
611
#if DIMENSION == 3
Matthieu Constant's avatar
Matthieu Constant committed
612
613
  rmin[2] -= alert;
  rmax[2] += alert;
614
#endif
Matthieu Constant's avatar
Matthieu Constant committed
615
616
617
618
619
620
621
622
623
624
625
626
}

static void bbadd(double *amin, double *amax, const double *bmin, const double *bmax)
{
  for (int i = 0; i < DIMENSION; ++i) {
    if (bmin[i] < amin[i])
      amin[i] = bmin[i];
    if (bmax[i] > amax[i])
      amax[i] = bmax[i];
  }
}

627
static void particle_problem_bounding_box(ParticleProblem *p, double *bbmin, double *bbmax) {
Matthieu Constant's avatar
Matthieu Constant committed
628
629
630
631
632
633
  double amin[DIMENSION], amax[DIMENSION];
  for (int i = 0; i < DIMENSION; ++i) {
    bbmin[i] = DBL_MAX;
    bbmax[i] = -DBL_MAX;
  }
  for (size_t i = 0; i < vectorSize(p->particles); ++i) {
Jonathan Lambrechts's avatar
theta    
Jonathan Lambrechts committed
634
635
    double position[DIMENSION];
    particle_get_position(p, &p->particles[i], position);
636
    particle_bounding_box(&p->particles[i], position, amin, amax);
Matthieu Constant's avatar
Matthieu Constant committed
637
638
639
    bbadd(bbmin, bbmax, amin, amax);
  }
  for (size_t i = 0; i < vectorSize(p->disks); ++i) {
640
    disk_bounding_box(&p->disks[i], amin, amax);
Matthieu Constant's avatar
Matthieu Constant committed
641
642
643
    bbadd(bbmin, bbmax, amin, amax);
  }
  for (size_t i = 0; i < vectorSize(p->segments); ++i) {
644
    segment_bounding_box(&p->segments[i], amin, amax);
Matthieu Constant's avatar
Matthieu Constant committed
645
646
    bbadd(bbmin, bbmax, amin, amax);
  }
Michel Henry's avatar
Michel Henry committed
647
  for(size_t i = 0; i < vectorSize(p->periodicSegments); ++i){
648
    periodic_segment_bounding_box(&p->periodicSegments[i], amin, amax);
Michel Henry's avatar
Michel Henry committed
649
650
    bbadd(bbmin, bbmax, amin, amax);
  }
651
#if DIMENSION == 3
Matthieu Constant's avatar
Matthieu Constant committed
652
  for (size_t i = 0; i < vectorSize(p->triangles); ++i) {
653
    triangle_bounding_box(&p->triangles[i], amin, amax);
Matthieu Constant's avatar
Matthieu Constant committed
654
655
    bbadd(bbmin, bbmax, amin, amax);
  }
Michel Henry's avatar
Michel Henry committed
656
  for(size_t i = 0; i < vectorSize(p->periodicTriangles); ++i){
657
    periodic_triangle_bounding_box(&p->periodicTriangles[i], amin, amax);
Michel Henry's avatar
Michel Henry committed
658
659
    bbadd(bbmin, bbmax, amin, amax);
  }
660
#endif
Matthieu Constant's avatar
Matthieu Constant committed
661
}
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
662
663
664
665
666

typedef enum {CPARTICLE=0, CGHOST, CPERIODIC} ContactTreeType;
typedef struct {
  Cell *tree;
  int *id;
Michel Henry's avatar
Michel Henry committed
667
  ContactTreeType *type;
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
668
669
670
671
672
673
674
675
676
677
678
  int *tmp0;
  int *ghost_id;
  ParticleProblem *problem;
  double *ghost_position;
  double *periodic_translate;
  int *periodic_tag;
  double alert;
} ContactTree;

ContactTree *contact_tree_create(ParticleProblem *p, double alert) {
  double bbmin[DIMENSION], bbmax[DIMENSION];
679
  particle_problem_bounding_box(p, bbmin, bbmax);
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
  ContactTree *tree = malloc(sizeof(ContactTree));
  tree->tree = cellNew(bbmin, bbmax);
  tree->tmp0 = NULL;
  tree->id = NULL;
  tree->type = NULL;
  tree->ghost_position = NULL;
  tree->ghost_id = NULL;
  tree->periodic_translate = NULL;
  tree->periodic_tag = NULL;
  tree->alert = alert;
  tree->problem = p;
  return tree;
}

void contact_tree_free(ContactTree *tree) {
  vectorFree(tree->tmp0);
  vectorFree(tree->id);
  vectorFree(tree->type);
  vectorFree(tree->ghost_position);
  vectorFree(tree->ghost_id);
  vectorFree(tree->periodic_translate);
  vectorFree(tree->periodic_tag);
  cellFree(tree->tree);
  free(tree);
}

void contact_tree_add_periodic_segment(ContactTree *tree, PeriodicSegment *segment) {
  int i = vectorSize(tree->id);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
708
  *vectorPush(&tree->id) = vectorSize(tree->periodic_tag);
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
709
710
  *vectorPush(&tree->type) = CPERIODIC;
  for (int d = 0; d < DIMENSION; ++d) {
Michel Henry's avatar
Michel Henry committed
711
    *vectorPush(&tree->periodic_translate) = (tree->problem->periodicEntities[segment->entity_id]).transform[d];
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
712
  }
Michel Henry's avatar
Michel Henry committed
713
  *vectorPush(&tree->periodic_tag) = segment->entity_id;
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
714
  double amin[DIMENSION], amax[DIMENSION];
715
  periodic_segment_bounding_box(segment, amin, amax);
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
716
717
718
  addAlert(tree->alert/2, amin, amax);
  cellAdd(tree->tree, amin, amax, i, NULL);
}
Michel Henry's avatar
Michel Henry committed
719
720
721
722
723
724
#if DIMENSION==3
void contact_tree_add_periodic_triangle(ContactTree *tree, PeriodicTriangle *triangle) {
  int i = vectorSize(tree->id);
  *vectorPush(&tree->id) = vectorSize(tree->periodic_tag);
  *vectorPush(&tree->type) = CPERIODIC;
  for (int d = 0; d < DIMENSION; ++d) {
Michel Henry's avatar
Michel Henry committed
725
    *vectorPush(&tree->periodic_translate) = (tree->problem->periodicEntities[triangle->entity_id]).transform[d];
Michel Henry's avatar
Michel Henry committed
726
  }
Michel Henry's avatar
Michel Henry committed
727
  *vectorPush(&tree->periodic_tag) = triangle->entity_id;
Michel Henry's avatar
Michel Henry committed
728
  double amin[DIMENSION], amax[DIMENSION];
729
  periodic_triangle_bounding_box(triangle, amin, amax);
Michel Henry's avatar
Michel Henry committed
730
731
732
733
  addAlert(tree->alert/2, amin, amax);
  cellAdd(tree->tree, amin, amax, i, NULL);
}
#endif
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
734
735
736

void contact_tree_add_ghost_particle(ContactTree *tree, int id, const double *translation) {
  int i = vectorSize(tree->id);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
737
  *vectorPush(&tree->id) = vectorSize(tree->ghost_id);
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
738
739
740
  *vectorPush(&tree->type) = CGHOST;
  *vectorPush(&tree->ghost_id) = id;
  for (int d = 0; d < DIMENSION; ++d) {
Nathan Coppin's avatar
Nathan Coppin committed
741
    *vectorPush(&tree->ghost_position) = tree->problem->particles[id].x[d]+translation[d];
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
742
743
  }
  double amin[DIMENSION], amax[DIMENSION];
744
  particle_bounding_box(&tree->problem->particles[id], tree->ghost_position+vectorSize(tree->ghost_position)-DIMENSION, amin, amax);
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
745
746
747
748
749
750
751
752
753
  addAlert(tree->alert/2, amin, amax);
  cellAdd(tree->tree, amin, amax, i, NULL);
}

static int intcmp(const void *p0, const void *p1) {
  return *(int*)p0 - *(int*)p1;
}

static void contact_tree_get_particle_and_position(
Jonathan Lambrechts's avatar
theta    
Jonathan Lambrechts committed
754
    const ContactTree *tree, int tree_id, int *p_id, double *p_position) {
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
755
756
  if (tree->type[tree_id] == CPARTICLE) {
    *p_id = tree->id[tree_id];
Jonathan Lambrechts's avatar
theta    
Jonathan Lambrechts committed
757
    particle_get_position(tree->problem, &tree->problem->particles[*p_id], p_position);
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
758
759
760
761
  }
  else if (tree->type[tree_id] == CGHOST) {
    int ghost_id = tree->id[tree_id];
    *p_id = tree->ghost_id[ghost_id];
Jonathan Lambrechts's avatar
theta    
Jonathan Lambrechts committed
762
763
    for (int i = 0; i < DIMENSION; ++i)
    p_position[i] = tree->ghost_position[ghost_id*DIMENSION+i];
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
764
765
766
  }
}

767
void contact_tree_add_particle(ContactTree *tree, int id, const Contact *old_contacts, size_t *iold) {
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
768
769
770
771
772
  ParticleProblem *p = tree->problem;
  int i = vectorSize(tree->id);
  *vectorPush(&tree->id) = id;
  *vectorPush(&tree->type) = CPARTICLE;
  double amin[DIMENSION], amax[DIMENSION];
Jonathan Lambrechts's avatar
theta    
Jonathan Lambrechts committed
773
774
  double position[DIMENSION];
  particle_get_position(p, &p->particles[id], position);
775
  particle_bounding_box(&p->particles[id], position, amin, amax);
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
  addAlert(tree->alert/2, amin, amax);
  vectorClear(tree->tmp0);
  cellAdd(tree->tree, amin, amax, i, &tree->tmp0);
  int last_periodic_tag = -1;
  for (size_t j = 0; j < vectorSize(tree->tmp0); ++j) {
    int jj = tree->tmp0[j];
    if (tree->type[jj] == CPERIODIC) {
      if (last_periodic_tag == tree->periodic_tag[tree->id[jj]]) continue;
      last_periodic_tag = tree->periodic_tag[tree->id[jj]];
      const double *t = &tree->periodic_translate[tree->id[jj]*DIMENSION];
      contact_tree_add_ghost_particle(tree, id, t);
    }
    else {
      int id1;
      const Contact *cold;
Jonathan Lambrechts's avatar
theta    
Jonathan Lambrechts committed
791
792
      double position1[DIMENSION];
      contact_tree_get_particle_and_position(tree, jj, &id1, position1);
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
793
      Contact *c = vectorPush(&p->contacts);
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
794
      if(!contact_init_particle_particle(c, p, id, position, id1, position1, tree->alert)){
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
795
        vectorPop(p->contacts);
Matthieu Constant's avatar
Matthieu Constant committed
796
      }
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
797
798
799
      else {
        if ((cold = findContactSorted(c, old_contacts, iold))) {
          for (int d = 0; d < DIMENSION; ++d) {
800
            c->reaction[d] = cold->reaction[d];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
801
          }
802
        }
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
803
      }
Matthieu Constant's avatar
Matthieu Constant committed
804
805
    }
  }
806
}
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
807

808
static void contact_tree_gen_boundary_contact(ContactTree *tree, ContactType type, int id, const Contact *old_contacts, size_t *iold) {
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
809
810
811
  double amin[DIMENSION], amax[DIMENSION];
  ParticleProblem *p = tree->problem;
  if (type == PARTICLE_DISK)
812
      disk_bounding_box(&p->disks[id], amin, amax);
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
813
  else if (type == PARTICLE_SEGMENT)
814
      segment_bounding_box(&p->segments[id], amin, amax);
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
815
816
#if DIMENSION == 3
  else if (type == PARTICLE_TRIANGLE)
817
      triangle_bounding_box(&p->triangles[id], amin, amax);
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
818
#endif
Jonathan Lambrechts's avatar
fix    
Jonathan Lambrechts committed
819
  else printf("invalid type !!!!\n");
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
820
821
822
823
824
  addAlert(tree->alert/2, amin, amax);
  vectorClear(tree->tmp0);
  cellSearch(tree->tree, amin, amax, &tree->tmp0);
  for (size_t j = 0; j < vectorSize(tree->tmp0); ++j) {
    int jj = tree->tmp0[j];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
825
826
    if (tree->type[jj] == CPERIODIC)
      continue;
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
827
    int id1;
Jonathan Lambrechts's avatar
theta    
Jonathan Lambrechts committed
828
829
    double position1[DIMENSION];
    contact_tree_get_particle_and_position(tree, jj, &id1, position1);
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
830
831
    Contact *c = vectorPush(&p->contacts);
    int r = 0;
Michel Henry's avatar
Michel Henry committed
832
    if (type == PARTICLE_DISK){
833
834
835
836
      r = contact_init_disk_particle(c, p, id, id1, position1, tree->alert);
    }
    else if (type == PARTICLE_SEGMENT) {
      r = contact_init_segment_particle(c, p, id, id1, position1, tree->alert);
Michel Henry's avatar
Michel Henry committed
837
    }
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
838
#if DIMENSION == 3
839
840
841
    else if (type == PARTICLE_TRIANGLE) {
      r = contact_init_triangle_particle(c, p, id, id1, position1, tree->alert);
    }
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
842
843
#endif
    const Contact *cold;
Jonathan Lambrechts's avatar
wip    
Jonathan Lambrechts committed
844
    if(!r)
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
845
      vectorPop(p->contacts);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
846
847
848
    else {
      if ((cold = findContactSorted(c, old_contacts, iold))) {
        for (int d = 0; d < DIMENSION; ++d) {
849
          c->reaction[d] = cold->reaction[d];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
850
        }
851
      }
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
852
    }
Jonathan Lambrechts's avatar
tmp    
Jonathan Lambrechts committed
853
854
855
  }
}

856
static void particle_problem_gen_contacts(ParticleProblem *p, const double alert)
857
858
{
  clock_t tic = clock