scontact.c 43.6 KB
Newer Older
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
1
/*
Nathan Coppin's avatar
Nathan Coppin committed
2
 * MigFlow - Copyright (C) <2010-2018>
3
4
 * <Universite catholique de Louvain (UCL), Belgium
 *  Universite de Montpellier, France>
Jonathan Lambrechts's avatar
Jonathan Lambrechts 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
8
 * Description and complete License: see LICENSE file.
 * 	
Nathan Coppin's avatar
Nathan Coppin committed
9
 * This program (MigFlow) is free software: 
10
 * 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
13
14
15
16
 * Public License as published by the Free Software Foundation, either version
 * 3 of the License, or (at your option) any later version.
 * 
 * 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.
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
18
 * 
19
20
 * You should have received a copy of the GNU Lesser General Public License
 * 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
27
28
29
30
31
#include "scontact.h" 
#include "quadtree.h"
#include "vector.h"
#include <math.h>
#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
35
36
37
38
39
40
41
42

typedef struct _Particle Particle;
typedef struct _Contact Contact;
typedef struct _Disk Disk;
typedef struct _Segment Segment;
typedef struct _Triangle Triangle;

struct _ParticleProblem{
  Particle *particles;
  Contact *contacts;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
43
  double *position, *velocity, *externalForces;
Matthieu Constant's avatar
Matthieu Constant committed
44
  Disk *disks;
45
46
47
48
  char **tagname, **materialName;
  int *diskTag, *segmentTag;
  int *diskMaterial, *segmentMaterial;
  int *particleMaterial;
Matthieu Constant's avatar
Matthieu Constant committed
49
  Segment *segments;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
50
51
52
53
54
55
#if FRICTION_ENABLED
  double *omega;
  double friction_relaxation;
  double *mu;
#endif
#if DIMENSION == 3
Matthieu Constant's avatar
Matthieu Constant committed
56
57
  Triangle *triangles;
  int *triangleTag;
58
  int *triangleMaterial;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
59
#endif
60
  int use_queue;
Matthieu Constant's avatar
Matthieu Constant committed
61
62
};

Matthieu Constant's avatar
Matthieu Constant committed
63
64
65
66
67
68
#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
69
static double dot(const double *x, const double *y) {
Nathan Coppin's avatar
Nathan Coppin committed
70
  #if DIMENSION == 3
Matthieu Constant's avatar
Matthieu Constant committed
71
  return x[0] * y[0] +  x[1] * y[1] + x[2] * y[2];
Nathan Coppin's avatar
Nathan Coppin committed
72
  #else
Matthieu Constant's avatar
Matthieu Constant committed
73
  return x[0] * y[0] +  x[1] * y[1];
Nathan Coppin's avatar
Nathan Coppin committed
74
  #endif
Matthieu Constant's avatar
Matthieu Constant committed
75
76
77
78
79
80
81
82
83
84
85
86
87
}

/* Particle */
struct _Particle{
  double r;
  double m;
};

struct _Contact {
  size_t o0, o1;
  double a0, a1;
  double D;
  double dv;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
88
89
#if FRICTION_ENABLED
  double In0, In1;
Nathan Coppin's avatar
Nathan Coppin committed
90
  double ct;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
91
#endif
Matthieu Constant's avatar
Matthieu Constant committed
92
  ContactType type;
Matthieu Constant's avatar
Matthieu Constant committed
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
  double n[DIMENSION];
};

static void particleBoundingBox(const Particle *p, const double x[DIMENSION], double pmin[DIMENSION], double pmax[DIMENSION]){
  for (int i = 0; i < DIMENSION; ++i) {
    pmin[i] = x[i] - p->r;
    pmax[i] = x[i] + p->r;
  }
}

static int particleInitContact(size_t id0, Particle *p0, double *x0, size_t id1, Particle *p1, double *x1, double alert, Contact *c) {
  double nnorm = 0;
  c->dv = 0;
  c->o0 = id0;
  c->o1 = id1;
  for (int k = 0; k < DIMENSION; ++k) {
    c->n[k] = x0[k] - x1[k];
    nnorm += c->n[k] * c->n[k];
  }
  nnorm = sqrt(nnorm);
113
  for (int i = 0; i < DIMENSION; ++i)
Matthieu Constant's avatar
Matthieu Constant committed
114
115
116
117
    c->n[i] /= -nnorm;
  c->D = fmax(0., nnorm - (p0->r + p1->r));
  c->a0 = p1->m / (p0->m + p1->m);
  c->a1 = p0->m / (p0->m + p1->m);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
118
119
#ifdef FRICTION_ENABLED
  c->ct = 0;
120
121
  c->In0 = c->a0*2/(p0->r);
  c->In1 = c->a1*2/(p1->r);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
122
#endif
Matthieu Constant's avatar
Matthieu Constant committed
123
  c->type = PARTICLE_PARTICLE;
Matthieu Constant's avatar
Matthieu Constant committed
124
125
126
127
128
129
130
  return c->D < alert;
}

struct _Disk {
  double x[DIMENSION];
  double v[DIMENSION];
  double r;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
131
#ifdef FRICTION_ENABLED
Nathan Coppin's avatar
Nathan Coppin committed
132
  double vt;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
133
#endif
Matthieu Constant's avatar
Matthieu Constant committed
134
135
136
137
138
139
140
141
142
143
};

static void diskBoundingBox(const Disk *d, double *pmin, double *pmax) {
  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;
  }
}

144
145

size_t particleProblemAddBoundaryDiskTagId(ParticleProblem *p, const double x[DIMENSION], double r, int tag, int materialTag) {
Matthieu Constant's avatar
Matthieu Constant committed
146
147
  Disk *d = vectorPush(&p->disks);
  *vectorPush(&p->diskTag) = tag;
148
  *vectorPush(&p->diskMaterial) = materialTag;
Matthieu Constant's avatar
Matthieu Constant committed
149
  d->r = r;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
150
#ifdef FRICTION_ENABLED
Nathan Coppin's avatar
Nathan Coppin committed
151
  d->vt = 0;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
152
#endif
Matthieu Constant's avatar
Matthieu Constant committed
153
154
155
156
157
158
159
  for (int i = 0; i < DIMENSION; ++i) {
    d->x[i] = x[i];
    d->v[i] = 0.;
  }
  return vectorSize(p->disks) - 1;
}

160
161
size_t particleProblemAddBoundaryDisk(ParticleProblem *p, const double x[DIMENSION], double r, const char *tag, const char *materialTag) {
  return particleProblemAddBoundaryDiskTagId(p,x,r,particleProblemGetTagId(p,tag),particleProblemGetMaterialTagId(p,materialTag));
Matthieu Constant's avatar
Matthieu Constant committed
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
}

static int diskInitContact(size_t id, const Disk *d, size_t particle, const Particle *p, double *x, double alert, Contact *c) {
  double nnorm = 0;
  c->dv = 0;
  c->o1 = particle;
  c->o0 = id;
  for (int i = 0; i < DIMENSION; ++i) {
    c->n[i] = d->x[i] - x[i];
    nnorm += c->n[i] * c->n[i];
  }
  nnorm = sqrt(nnorm);
  c->D = (nnorm - fabs(p->r + d->r)) * (d->r < 0 ? -1 : 1);
  if (c->D < 0) c->D = 0;
  for (int i = 0; i < DIMENSION; ++i) {
    c->n[i] /= nnorm * (d->r < 0 ? -1 : 1);
  }
  c->a0 = 0.;
  c->a1 = 1.;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
181
182
#ifdef FRICTION_ENABLED
  c->ct = 0;
Nathan Coppin's avatar
Nathan Coppin committed
183
  c->In0 = 0;
184
  c->In1 = c->a1*2/(p->r);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
185
#endif
Matthieu Constant's avatar
Matthieu Constant committed
186
  c->type = PARTICLE_DISK;
Matthieu Constant's avatar
Matthieu Constant committed
187
188
189
  return c->D < alert;
}

190
struct _Segment{
Matthieu Constant's avatar
Matthieu Constant committed
191
192
  double p[2][DIMENSION];
  double v[DIMENSION];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
193
#ifdef FRICTION_ENABLED
Nathan Coppin's avatar
Nathan Coppin committed
194
  double vt;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
195
#endif
Matthieu Constant's avatar
Matthieu Constant committed
196
};
Nathan Coppin's avatar
Nathan Coppin committed
197

Nathan Coppin's avatar
Nathan Coppin committed
198
199
200
201
202
203
204
static void coordAdd(double *x, double a, const double *y) { // x += a * y
  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
205

Matthieu Constant's avatar
Matthieu Constant committed
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
static void segmentBoundingBox(const Segment *s, double *pmin, double *pmax) {
  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]);
  }
}

static int segmentProjectionIsInside(const Segment *s, double *x) {
  double alpha = 0, beta = 0;
  for (int i = 0; i < DIMENSION; ++i) {
    const double d = s->p[0][i] - s->p[1][i];
    alpha += (x[i] - s->p[0][i]) * d;
    beta  += (x[i] - s->p[1][i]) * d;
  }
  return (alpha < 0 && beta > 0);
}

static int segmentInitContact(size_t id, const Segment *s, size_t particle, const Particle *p, double *x, double alert, Contact *c) {
  c->o0 = id;
  c->o1 = particle;
  c->dv = 0;
  double t[DIMENSION];
  double nt2 = 0;
  double dt = 0;
  for (int i = 0; i <DIMENSION; ++i){
    t[i] = s->p[1][i] - s->p[0][i];
    dt += t[i] * (s->p[0][i] - x[i]);
    nt2 += t[i] * t[i];
  }
  double nn2 = 0;
  for (int i = 0; i < DIMENSION; ++i) {
    c->n[i] = s->p[0][i] - x[i] - t[i] / nt2 *  dt;
    nn2 += c->n[i] * c->n[i];
  }
  const double nnorm = sqrt(nn2);
  for (int i = 0; i < DIMENSION; ++i) {
    c->n[i] /= nnorm;
  }
  c->D = nnorm - p->r;
  if (c->D < 0 && segmentProjectionIsInside(s, x)) c->D = 0;
  c->a0 = 0.;
  c->a1 = 1.;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
248
249
#ifdef FRICTION_ENABLED
  c->ct = 0;
Nathan Coppin's avatar
Nathan Coppin committed
250
  c->In0 = 0;
251
  c->In1 = c->a1*2/(p->r);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
252
#endif
Matthieu Constant's avatar
Matthieu Constant committed
253
  c->type = PARTICLE_SEGMENT;
Matthieu Constant's avatar
Matthieu Constant committed
254
255
256
257
258
259
260
261
262
  return c->D >= 0  && c->D < alert;
}

#if DIMENSION == 3
struct _Triangle {
  double p[3][DIMENSION];
  double v[DIMENSION];
};

263
void particleProblemAddBoundaryTriangleTagId(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
264
265
  Triangle *t = vectorPush(&p->triangles);
  *vectorPush(&p->triangleTag) = tag;
266
  *vectorPush(&p->triangleMaterial) = materialTag;
Matthieu Constant's avatar
Matthieu Constant committed
267
268
269
270
271
272
273
274
  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];
    t->v[i] = 0.;
  }
}

275
276
void particleProblemAddBoundaryTriangle(ParticleProblem *p, const double x0[3], const double x1[3], const double x2[3], const char *tag, const char *material) {
  return particleProblemAddBoundaryTriangleTagId(p, x0, x1, x2, particleProblemGetTagId(p, tag), particleProblemGetMaterialTagId(p, material));
Matthieu Constant's avatar
Matthieu Constant committed
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
}

static void triangleBoundingBox(const Triangle *t, double *pmin, double *pmax) {
  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]);
  }
}

static void _cross (const double *a, const double *b, double *c) {
  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];
}

static int triangleProjectionIsInside(const Triangle *t, const double *x) {
  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);
  double xp[3];
  double dp[3] = {t->p[0][0] - x[0], t->p[0][1] - x[1], t->p[0][2] - x[2]};
  const double nd = dot(n, dp);
  for (int i = 0; i < 3; ++i) 
    xp[i] = x[i] + n[i] * nd;
  double dx[3][3] = 
303
    {{xp[0] - t->p[0][0], xp[1] - t->p[0][1], xp[2] - t->p[0][2]},
Matthieu Constant's avatar
Matthieu Constant committed
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
    {xp[0] - t->p[1][0], xp[1] - t->p[1][1], xp[2] - t->p[1][2]},
    {xp[0] - t->p[2][0], xp[1] - t->p[2][1], xp[2] - t->p[2][2]}};
  double alpha[3];
  for (int i = 0; i < 3; ++i) {
    double d[3];
    _cross(dx[(i + 1) % 3], dx[(i + 2) %3], d);
    alpha[i] = 0.5 * dot(n, d);
  }
  return ((alpha[0] <= 0 && alpha[1] <= 0 &&  alpha[2]<= 0) || (alpha[0] >= 0 && alpha[1] >= 0 && alpha[2] >= 0));
}

static int triangleInitContact(size_t id, const Triangle *t, size_t particle, const Particle *p, double *x, double alert, Contact *c) {
  c->o0 = id;
  c->o1 = particle;
  c->dv = 0;
  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));
  for (int i = 0; i < 3; ++i)
    c->n[i] = N[i] / nn;
  double dd[3] = {t->p[0][0] - x[0], t->p[0][1] - x[1], t->p[0][2] - x[2]};
  c->D = dot(c->n, dd);
  if (c->D < 0) {
    for (int i = 0; i <3; ++i)
      c->n[i] = -c->n[i];
    c->D = -c->D;
  }
  c->D -= p->r;
  c->a0 = 0.;
  c->a1 = 1.;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
336
337
#ifdef FRICTION_ENABLED
  c->ct = 0;
Nathan Coppin's avatar
Nathan Coppin committed
338
  c->In0 = 0;
339
  c->In1 = c->a1*2/(p->r);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
340
#endif
Matthieu Constant's avatar
Matthieu Constant committed
341
342
  if (c->D < 0)
    c->D = 0;
Matthieu Constant's avatar
Matthieu Constant committed
343
  c->type = PARTICLE_TRIANGLE;
Matthieu Constant's avatar
Matthieu Constant committed
344
345
346
347
348
  return (c->D > 0 || triangleProjectionIsInside(t, x)) && c->D < alert;
}

#endif

349
350
351
352
353
354
#ifdef FRICTION_ENABLED
static double particleProblemGetMu(const ParticleProblem *p, int mat0, int mat1) {
  return p->mu[mat0*particleProblemNMaterial(p)+mat1];
}
#endif

Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
355
356
357
358
359
360
static int contactParticleParticleSolve(Contact *c, ParticleProblem *p, double dt, double tol) {
  double vn = c->dv;
  for (int i = 0; i<DIMENSION; ++i)
    vn += (p->velocity[c->o0*DIMENSION+i] - p->velocity[c->o1*DIMENSION+i]) * c->n[i];
  double dp = fmax(0., vn - c->D/dt);
#ifdef FRICTION_ENABLED
Nathan Coppin's avatar
Nathan Coppin committed
361
362
363
  double t[2];
  t[0] = c->n[1];
  t[1] = -c->n[0];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
364
  double ct = 0 ;
365
  double vt = 
Nathan Coppin's avatar
Nathan Coppin committed
366
367
368
    c->ct*p->particles[c->o0].r*c->In0+
    c->ct*p->particles[c->o1].r*c->In1+
    (p->velocity[c->o0*2+0]-p->velocity[c->o1*2+0])*t[0]+
Jonathan Lambrechts's avatar
pp    
Jonathan Lambrechts committed
369
370
371
    (p->velocity[c->o0*2+1]-p->velocity[c->o1*2+1])*t[1]+
    p->omega[c->o0]*p->particles[c->o0].r +
    p->omega[c->o1]*p->particles[c->o1].r;
372
  const double mu = particleProblemGetMu(p,p->particleMaterial[c->o0],p->particleMaterial[c->o1]);
373
374
375
376
  if (fabs(vt)> 3*mu*dp) vt *= 3*mu*dp/fabs(vt);
  ct = vt/3;
  if (dp == 0.)
    ct = 0;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
377
378
379
380
381
382
383
384
385
386
387
  ct -= c->ct;
  p->omega[c->o0] -= c->In0*ct;
  p->omega[c->o1] -= c->In1*ct;
  c->ct += ct;
#endif
  dp -= c->dv;
  coordAdd(&p->velocity[c->o0*DIMENSION], -dp*c->a0, c->n);
  coordAdd(&p->velocity[c->o1*DIMENSION], dp*c->a1, c->n);
  c->dv += dp;
  if(fabs(dp) > tol/dt) return 0;
#ifdef FRICTION_ENABLED
388
  if(fabs(ct) > tol/dt) return 0;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
389
390
#endif
  return 1;
Matthieu Constant's avatar
Matthieu Constant committed
391
392
393
394
395
396
397
398
399
}

static double contactParticleBndSolve(Contact *c, const double *v, double vlocfree[DIMENSION], double *vn, double dt) {
  for (int i = 0; i < DIMENSION; ++i)
    vlocfree[i] = v[i] + c->n[i] * c->dv;
  *vn = dot(vlocfree, c->n);
  return fmax(0, *vn - c->D/dt);
}

Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
400
401
static int contactParticleDiskSolve(Contact *c, ParticleProblem *p, double dt, double tol) {
  double vlocfree[DIMENSION], vn;
Matthieu Constant's avatar
Matthieu Constant committed
402
403
  double vloc[DIMENSION];
  for (int i= 0; i < DIMENSION; ++i)
Nathan Coppin's avatar
Nathan Coppin committed
404
    vloc[i] = p->velocity[c->o1*DIMENSION + i] - p->disks[c->o0].v[i];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
405
  double dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt);
406
  
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
407
408
409
410
#ifdef FRICTION_ENABLED
  double t[2];
  t[0] = c->n[1];
  t[1] = -c->n[0];
411
  double vt = 
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
412
413
414
415
416
    c->ct*p->particles[c->o1].r*c->In1+
    p->velocity[c->o1 * DIMENSION]*t[0]+
    p->velocity[c->o1 * DIMENSION+1]*t[1]+
    p->omega[c->o1]*p->particles[c->o1].r+
    p->disks[c->o0].vt;
417
  const double mu = particleProblemGetMu(p,p->particleMaterial[c->o1],p->diskMaterial[c->o0]);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
418
  double ct = 0;
419
420
421
422
  if (fabs(vt)> 3*mu*dp) vt *= 3*mu*dp/fabs(vt);
  ct = vt/3;
  if (dp == 0.)
    ct = 0;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
423
424
425
426
427
428
429
430
431
  ct -= c->ct;
  p->omega[c->o1] -= c->In1*ct;
  c->ct += ct;
#endif
  dp -=c->dv;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -dp, c->n);
  c->dv += dp;
  if(fabs(dp) > tol/dt) return 0;
#ifdef FRICTION_ENABLED
432
  if(fabs(ct) > tol/dt) return 0;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
433
434
#endif
  return 1;
Matthieu Constant's avatar
Matthieu Constant committed
435
436
}

Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
437
438
static int contactParticleSegmentSolve(Contact *c, ParticleProblem *p, double dt, double tol) {
  double vlocfree[DIMENSION], vn, xn[DIMENSION], vloc[DIMENSION];
Nathan Coppin's avatar
Nathan Coppin committed
439
  double r = p->particles[c->o1].r;
Matthieu Constant's avatar
Matthieu Constant committed
440
  for (int i= 0; i < DIMENSION; ++i)
Nathan Coppin's avatar
Nathan Coppin committed
441
    vloc[i] = p->velocity[c->o1*DIMENSION + i] - p->segments[c->o0].v[i];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
442
  double dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt);
Matthieu Constant's avatar
Matthieu Constant committed
443
  for (int i = 0; i < DIMENSION; ++i)
Nathan Coppin's avatar
Nathan Coppin committed
444
445
    xn[i] = p->position[c->o1*DIMENSION + i] + r*c->n[i] + vlocfree[i]*c->D / vn;
  if (!segmentProjectionIsInside(&p->segments[c->o0], xn))
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
446
447
448
449
450
    dp = 0;
#ifdef FRICTION_ENABLED
  double t[2];
  t[0] = c->n[1];
  t[1] = -c->n[0];
451
  double vt = 
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
452
453
454
455
456
    c->ct*p->particles[c->o1].r*c->In1+
    p->velocity[c->o1*DIMENSION]*t[0]+
    p->velocity[c->o1*DIMENSION +1]*t[1]+
    p->omega[c->o1]*p->particles[c->o1].r+
    p->segments[c->o0].vt;
457
  const double mu = particleProblemGetMu(p,p->particleMaterial[c->o1],p->segmentMaterial[c->o0]);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
458
  double ct = 0;
459
460
  if (fabs(vt)> 3*mu*dp) vt *= 3*mu*dp/fabs(vt);
  ct = vt/3;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
461
462
  if (dp == 0.)
    ct = 0;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
463
464
465
466
467
468
469
  ct -= c->ct;
  p->omega[c->o1] -= c->In1*ct;
  c->ct += ct;
#endif
  dp -= c->dv;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -dp, c->n);
  c->dv += dp;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
470
471
#if FRICTION_ENABLED
#endif
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
472
  if(fabs(dp) > tol/dt) return 0;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
473
#if FRICTION_ENABLED
474
  if(fabs(ct) > tol/dt) return 0;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
475
476
#endif
  return 1;
Matthieu Constant's avatar
Matthieu Constant committed
477
478
479
}

#if DIMENSION == 3
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
480
static int contactParticleTriangleSolve(Contact *c, ParticleProblem *p,double dt, double tol) {
Matthieu Constant's avatar
Matthieu Constant committed
481
  double vlocfree[DIMENSION], vn, xn[DIMENSION], vloc[DIMENSION];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
482
  double r = p->particles[c->o1].r;
Matthieu Constant's avatar
Matthieu Constant committed
483
  for (int i= 0; i < DIMENSION; ++i)
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
484
485
    vloc[i] = p->velocity[c->o1*DIMENSION+i] - p->triangles[c->o0].v[i];
  double dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt) - c->dv;
Matthieu Constant's avatar
Matthieu Constant committed
486
  for (int i = 0; i < DIMENSION; ++i)
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
487
488
    xn[i] = p->position[c->o1*DIMENSION+i] + r*c->n[i] + vlocfree[i]*c->D/vn;
  if (!triangleProjectionIsInside(&p->triangles[c->o0], xn))
Matthieu Constant's avatar
Matthieu Constant committed
489
    dp = 0;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
490
491
492
493
  dp -= c->dv;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -dp, c->n);
  c->dv += dp;
  return (fabs(dp) < tol/dt);
Matthieu Constant's avatar
Matthieu Constant committed
494
495
496
}
#endif

Nathan Coppin's avatar
Nathan Coppin committed
497

Nathan Coppin's avatar
Nathan Coppin committed
498

Matthieu Constant's avatar
Matthieu Constant committed
499
Contact *findContactSorted(Contact *c, Contact *v, size_t *i) {
Matthieu Constant's avatar
Matthieu Constant committed
500
  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
501
    (*i)++;
Matthieu Constant's avatar
Matthieu Constant committed
502
  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
503
504
505
506
507
508
509
}

static void addAlert(double alert, double *rmin, double *rmax) {
  rmin[0] -= alert;
  rmin[1] -= alert;
  rmax[0] += alert;
  rmax[1] += alert;
510
#if DIMENSION == 3
Matthieu Constant's avatar
Matthieu Constant committed
511
512
  rmin[2] -= alert;
  rmax[2] += alert;
513
#endif
Matthieu Constant's avatar
Matthieu Constant committed
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
}

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];
  }
}

static void _particleProblemBoundingBox(ParticleProblem *p, double *bbmin, double *bbmax) {
  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) {
    particleBoundingBox(&p->particles[i], &p->position[i * DIMENSION], amin, amax);
    bbadd(bbmin, bbmax, amin, amax);
  }
  for (size_t i = 0; i < vectorSize(p->disks); ++i) {
    diskBoundingBox(&p->disks[i], amin, amax);
    bbadd(bbmin, bbmax, amin, amax);
  }
  for (size_t i = 0; i < vectorSize(p->segments); ++i) {
    segmentBoundingBox(&p->segments[i], amin, amax);
    bbadd(bbmin, bbmax, amin, amax);
  }
544
#if DIMENSION == 3
Matthieu Constant's avatar
Matthieu Constant committed
545
546
547
548
  for (size_t i = 0; i < vectorSize(p->triangles); ++i) {
    triangleBoundingBox(&p->triangles[i], amin, amax);
    bbadd(bbmin, bbmax, amin, amax);
  }
549
#endif
Matthieu Constant's avatar
Matthieu Constant committed
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
}

static void _bboxtol(double *bbmin, double *bbmax) {
  double lmax = 0;
  for (int i = 0; i < DIMENSION; ++i) {
    lmax = fmax(lmax, bbmax[i] - bbmin[i]);
  }
  double tol = 1e-8 * lmax;
  for (int i = 0; i < DIMENSION; ++i) {
    bbmax[i] += tol;
    bbmin[i] -= tol;
  }
}


double volTet(double x0[3], double x1[3], double x2[3], double x3[3]) {
  // x1 * (y2 * (z3 - z4) - z2 * (y3 - y4) + y3 * z4 - y4 * z3)
  //- y1 * (x2 * (z3 - z4) - z2 * (x3 - x4) + x3 * z4 - x4 * z3)
  //+ z1 * (x2 * (y3 - y4) - y2 * (x3 - x4) + x3 * y4 - x4 * y3)
  //- (x2 * (y3 * z4 - y4 * z3) - y2 * (x3 * z4 - x4 * z3) + z2 * (x3 * y4 - x4 * y3))
  return  x0[0] * (x1[1] * (x2[2] - x3[2]) - x1[2] * (x2[1] - x3[1]) + x2[1] * x3[2] - x3[1] * x2[2])
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
571
572
573
    - x0[1] * (x1[0] * (x2[2] - x3[2]) - x1[2] * (x2[0] - x3[0]) + x2[0] * x3[2] - x3[0] * x2[2])
    + x0[2] * (x1[0] * (x2[1] - x3[1]) - x1[1] * (x2[0] - x3[0]) + x2[0] * x3[1] - x3[0] * x2[1])
    - (x1[0] * (x2[1] * x3[2] - x3[1] * x2[2]) - x1[1] * (x2[0] * x3[2] - x3[0] * x2[2]) + x1[2] * (x2[0] * x3[1] - x3[0] * x2[1]));
Matthieu Constant's avatar
Matthieu Constant committed
574
575
576
577
578
579
580
581
582
583
}

void ParticleToMesh(size_t nParticles, double *particles, int nElements, double *elements, int *elid, double *Xi)
{
  double bbmin[DIMENSION], bbmax[DIMENSION];
  int N = DIMENSION + 1;
  for (int i = 0; i < DIMENSION; ++i) {
    bbmin[i] = elements[i];
    bbmax[i] = elements[i];
  }
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
584
  for (int i = 0; i < N * nElements; ++i) {
Matthieu Constant's avatar
Matthieu Constant committed
585
586
587
588
589
590
591
592
    for (int d = 0; d < DIMENSION; ++d) {
      bbmin[d] = fmin(bbmin[d], elements[DIMENSION * i + d]);
      bbmax[d] = fmax(bbmax[d], elements[DIMENSION * i + d]);
    }
  }
  _bboxtol(bbmin, bbmax);
  Cell *tree = cellNew(bbmin, bbmax);
  double amin[DIMENSION], amax[DIMENSION];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
593
  for (int i = 0; i < nElements; ++i) {
Matthieu Constant's avatar
Matthieu Constant committed
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
    double *el = elements + (DIMENSION * N) * i;
    for (int d = 0; d < DIMENSION; ++d) {
      amin[d] = el[d];
      amax[d] = el[d];
    }
    for (int v = 1; v < N; ++v) {//only simplices
      for (int d = 0; d < DIMENSION; ++d) {
        amin[d] = fmin(amin[d], el[v * DIMENSION + d]);
        amax[d] = fmax(amax[d], el[v * DIMENSION + d]);
      }
    }
    _bboxtol(amin, amax);
    cellAdd(tree, amin, amax, i, NULL);
  }
  int *found = NULL;
  vectorPushN(&found, 100);
  vectorClear(found);
  for (size_t i = 0; i < nParticles; ++i) {
    double *x = &particles[i * DIMENSION];
    elid[i] = -1;
    cellSearch(tree, x, x, &found);
    for (size_t j = 0; j < vectorSize(found); ++j) {
      double *el = &elements[found[j] * N * DIMENSION];
      if (DIMENSION == 2)  {
        double *X[3] = {el, el + DIMENSION, el + DIMENSION * 2};
        double dx = x[0] - X[0][0], dy = x[1] - X[0][1];
        double DX[2] = {X[1][0] - X[0][0], X[2][0] - X[0][0]};
        double DY[2] = {X[1][1] - X[0][1], X[2][1] - X[0][1]};
        double det = DX[1] * DY[0] - DX[0] * DY[1];
        double xi  = (DX[1] * dy - DY[1] * dx) / det;
        double eta = -(DX[0] * dy - DY[0] * dx) / det;
        if (xi > -1e-8 && eta > -1e-8 && 1 - xi - eta > -1e-8) {
          Xi[i * DIMENSION + 0] = xi;
          Xi[i * DIMENSION + 1] = eta;
          elid[i] = found[j];
          break;
        }
      }
      else {
        double *X[4] = {el, el + DIMENSION, el + DIMENSION * 2, el + DIMENSION * 3};
        double v = volTet(X[0], X[1], X[2], X[3]);
        double v0 = volTet(x, X[1], X[2], X[3]);
        if (v0 * v < -1e-8)
          continue;
        double v1 = volTet(X[0], x, X[2], X[3]);
        if (v1 * v < -1e-8)
          continue;
        double v2 = volTet(X[0], X[1], x, X[3]);
        if (v2 * v < -1e-8)
          continue;
        double v3 = volTet(X[0], X[1], X[2], x);
        if (v3 * v < -1e-8)
          continue;
        Xi[i * 3 + 0] = v1 / v;
        Xi[i * 3 + 1] = v2 / v;
        Xi[i * 3 + 2] = v3 / v;
        elid[i] = found[j];
        break;
      }
    }
    /*if (elid[i] == -1) {
Nathan Coppin's avatar
Nathan Coppin committed
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
      printf(" PARTICLE %i OUTSIDE DOMAIN N2 search!!!\n", i);
      double toll = -1000;
      for (size_t j = 0; j < nElements; ++j) {
        double *el = &elements[j * N * DIMENSION];
        double *X[3] = {el, el + DIMENSION, el + DIMENSION * 2};
        double dx = x[0] - X[0][0], dy = x[1] - X[0][1];
        double DX[2] = {X[1][0] - X[0][0], X[2][0] - X[0][0]};
        double DY[2] = {X[1][1] - X[0][1], X[2][1] - X[0][1]};
        double det = DX[1] * DY[0] - DX[0] * DY[1];
        double xi  = (DX[1] * dy - DY[1] * dx) / det;
        double eta = -(DX[0] * dy - DY[0] * dx) / det;
        toll = fmax(toll, fmin(-xi, fmin(-eta, xi + eta -1)));
        if (xi > -1e-8 && eta > -1e-8 && 1 - xi - eta > -1e-8) {
          Xi[i * DIMENSION + 0] = xi;
          Xi[i * DIMENSION + 1] = eta;
          elid[i] = j;
          break;
        }
      }*/
      if (elid[i] == -1) {
        //printf(" PARTICLE %lu OUTSIDE DOMAIN!!! %g %g\n", i, x[0], x[1]);
        //exit(1);
      }
Matthieu Constant's avatar
Matthieu Constant committed
678
679
680
681
682
683
    //}
  }
  vectorFree(found);
  cellFree(tree);
}

684
685
686
687
688
689
690
691
int listcmp(const void *i0, const void *i1) {
  const u_int32_t j0 = *(u_int32_t*)i0, j1 = *(u_int32_t*)i1;
  return j0 - j1;
}

static int intersect(const double *amin, const double *amax, const double *bmin, const double *bmax) {
  if (amin[0] > bmax[0] || amax[0] < bmin[0]) return 0;
  if (amin[1] > bmax[1] || amax[1] < bmin[1]) return 0;
Nathan Coppin's avatar
Nathan Coppin committed
692
  #if DIMENSION == 3
693
  if (amin[2] > bmax[2] || amax[2] < bmin[2]) return 0;
Nathan Coppin's avatar
Nathan Coppin committed
694
  #endif
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
  return 1;
}

static void _particleProblemGenContacts2(ParticleProblem *p, const double alert)
{
  clock_t tic = clock();
  u_int32_t *list = NULL;
  double bbmin[DIMENSION], bbmax[DIMENSION];
  _particleProblemBoundingBox(p,bbmin,bbmax);
  addAlert(alert,bbmin, bbmax);
  u_int16_t N[DIMENSION];
  double delta = alert*6;
  for (int i = 0; i < DIMENSION; ++i) {
    N[i] = (bbmax[i]-bbmin[i])/delta;
  }
Nathan Coppin's avatar
Nathan Coppin committed
710

711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
  for (size_t i = 0; i < vectorSize(p->particles); ++i) {
    double amin[DIMENSION], amax[DIMENSION];
    particleBoundingBox(&p->particles[i], &p->position[i*DIMENSION],amin,amax);
    addAlert(alert/2,amin,amax);
    u_int16_t from[DIMENSION],to[DIMENSION];
    for (int i = 0; i < DIMENSION; ++i){
      from[i] = (amin[i]-bbmin[i])/delta;
      to[i] = (amax[i]-bbmin[i])/delta;
    }
#if DIMENSION == 2
    for (int j = from[0]; j <= to[0]; ++j) {
      for (int k = from[1]; k <= to[1]; ++k) {
        u_int32_t bid = j*N[1]+k;
        *vectorPush(&list) = bid;
        *vectorPush(&list) = i;
      }
    }
#else
    for (int j = from[0]; j <= to[0]; ++j) {
      for (int k = from[1]; k <= to[1]; ++k) {
        for (int l = from[2]; l <= to[2]; ++l) {
          u_int32_t bid = (j*N[1]+k)*N[2]+l;
          *vectorPush(&list) = bid;
          *vectorPush(&list) = i;
        }
      }
    }
#endif
  }
  clock_t tic1 = clock();
  qsort(list, vectorSize(list)/2, 2*sizeof(u_int32_t), listcmp);
  clock_t tic2 = clock();
  int ntot = 0;
  Contact *cc=NULL;
  vectorPushN(&cc, 10000000);
  vectorClear(cc);
  double amin[DIMENSION], amax[DIMENSION];
  double bmin[DIMENSION], bmax[DIMENSION];
  for (int start = 0; start < vectorSize(list);) {
    int end = start;
    for (; end< vectorSize(list);end+=2) {
      if (list[end] != list[start])
        break;
    }
    for (int i = start; i < end; i+= 2) {
      int ii = list[i+1];
      Particle *p0 = &p->particles[ii];
      double *x_i = &p->position[ii*DIMENSION];
      for (int j = i; j < end; j+= 2) {
        int jj = list[j+1];
        Particle *p1 = &p->particles[jj];
        double *x_j = &p->position[jj*DIMENSION];
        Contact c;
        c.dv = 0;
        c.o0 = ii; c.o1 = jj;
        double nnorm = 0;
        for (int k = 0; k < DIMENSION; ++k) {
          c.n[k] = x_i[k] - x_j[k];
          nnorm += c.n[k]*c.n[k];
        }
        double dd =p0->r+p1->r+alert;
        if (nnorm > dd*dd)
          continue;
        nnorm = sqrt(nnorm);
        for (int i = 0; i < DIMENSION; ++i)
          c.n[i] /= -nnorm;
        c.D = fmax(0., nnorm - (p0->r + p1->r));
        c.a0 = p1->m / (p0->m + p1->m);
        c.a1 = p0->m / (p0->m + p1->m);
        c.type = PARTICLE_PARTICLE;
        ntot++;
        *vectorPush(&cc) = c;
        /*else if ((cold = findContactSorted(c, oldContacts, &iold))) {
Nathan Coppin's avatar
Nathan Coppin committed
784
785
786
787
788
          coordAdd(&p->velocity[c->o0 * DIMENSION], -cold->dv * c->a0, c->n);
          coordAdd(&p->velocity[c->o1 * DIMENSION], cold->dv * c->a1, c->n);
          c->dv = cold->dv;
        }*/
      }
789
    }
790
    start = end;
Nathan Coppin's avatar
Nathan Coppin committed
791
  }
792
  clock_t tic3 = clock();
Nathan Coppin's avatar
Nathan Coppin committed
793
794
  printf("time new %lu : %lu %lu %lu\n", tic3-tic, tic3-tic2,tic2-tic1, tic1-tic);
  printf("%lu / %i (%lu%%)\n", vectorSize(cc), ntot, vectorSize(cc)*100/ntot);
795
796
797
  vectorFree(cc);
}

Matthieu Constant's avatar
Matthieu Constant committed
798
799
static void _particleProblemGenContacts(ParticleProblem *p, const double alert)
{
800
801
  //_particleProblemGenContacts2(p,alert);
  clock_t tic = clock();
Matthieu Constant's avatar
Matthieu Constant committed
802
803
804
805
806
807
808
809
810
811
812
  size_t iold = 0;
  double bbmin[DIMENSION], bbmax[DIMENSION];
  _particleProblemBoundingBox(p, bbmin, bbmax);
  Cell *tree = cellNew(bbmin, bbmax);
  double amin[DIMENSION], amax[DIMENSION];
  int *found = NULL;
  vectorPushN(&found, 100);
  vectorClear(found);
  // Particles
  Contact *oldContacts = vectorDup(p->contacts), *cold;
  vectorClear(p->contacts);
813
  int ntot = 0;
Matthieu Constant's avatar
Matthieu Constant committed
814
815
816
817
818
  for (size_t i = 0; i < vectorSize(p->particles); ++i) {
    particleBoundingBox(&p->particles[i], &p->position[i * DIMENSION], amin, amax);
    addAlert(alert/2, amin, amax);
    vectorClear(found);
    cellAdd(tree, amin, amax, i, &found);
Matthieu Constant's avatar
Matthieu Constant committed
819
    for (size_t j = 0; j < vectorSize(found); ++j) {
820
      ntot++;
Matthieu Constant's avatar
Matthieu Constant committed
821
822
823
824
      Contact *c= vectorPush(&p->contacts);
      if(!particleInitContact(i, &p->particles[i], &p->position[i * DIMENSION], found[j], &p->particles[found[j]], &p->position[found[j] * DIMENSION], alert, c))
        vectorPop(p->contacts);
      else if ((cold = findContactSorted(c, oldContacts, &iold))) {
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
825
826
827
828
        coordAdd(&p->velocity[c->o0 * DIMENSION], -cold->dv * c->a0, c->n);
        coordAdd(&p->velocity[c->o1 * DIMENSION], cold->dv * c->a1, c->n);
        c->dv = cold->dv;
#if FRICTION_ENABLED
Nathan Coppin's avatar
Nathan Coppin committed
829
        c->ct = cold->ct;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
830
831
832
        p->omega[c->o0] -= c->In0*(c->ct);
        p->omega[c->o1] -= c->In1*(c->ct);
#endif
Matthieu Constant's avatar
Matthieu Constant committed
833
834
835
      }
    }
  }
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
836
  //printf("time old %i\n", clock()-tic);
837
  //exit(0);
Matthieu Constant's avatar
Matthieu Constant committed
838
839
840
841
842
843
  // Disks
  for (size_t i = 0; i < vectorSize(p->disks); ++i) {
    diskBoundingBox(&p->disks[i], amin, amax);
    addAlert(alert/2, amin, amax);
    vectorClear(found);
    cellSearch(tree, amin, amax, &found);
Matthieu Constant's avatar
Matthieu Constant committed
844
845
    for (size_t j = 0; j < vectorSize(found); ++j) {
      Contact *c = vectorPush(&p->contacts);
Matthieu Constant's avatar
Matthieu Constant committed
846
      if(!diskInitContact(i, &p->disks[i], found[j], &p->particles[found[j]], &p->position[found[j] * DIMENSION], alert, c))
Matthieu Constant's avatar
Matthieu Constant committed
847
        vectorPop(p->contacts);
Matthieu Constant's avatar
Matthieu Constant committed
848
      else if ((cold = findContactSorted(c, oldContacts, &iold))) {
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
849
850
851
        coordAdd(&p->velocity[c->o1 * DIMENSION], -cold->dv, c->n);
        c->dv = cold->dv;
#if FRICTION_ENABLED
Nathan Coppin's avatar
Nathan Coppin committed
852
        c->ct = cold->ct;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
853
854
        p->omega[c->o1] -= c->In1*(c->ct);
#endif
Matthieu Constant's avatar
Matthieu Constant committed
855
856
857
858
859
860
861
862
863
      }
    }
  }
  // Segments
  for (size_t i = 0; i < vectorSize(p->segments); ++i) {
    segmentBoundingBox(&p->segments[i], amin, amax);
    addAlert(alert/2, amin, amax);
    vectorClear(found);
    cellSearch(tree, amin, amax, &found);
Matthieu Constant's avatar
Matthieu Constant committed
864
865
    for (size_t j = 0; j < vectorSize(found); ++j) {
      Contact *c = vectorPush(&p->contacts);
Matthieu Constant's avatar
Matthieu Constant committed
866
      if (!segmentInitContact(i, &p->segments[i], found[j], &p->particles[found[j]], &p->position[found[j] * DIMENSION], alert, c))
Matthieu Constant's avatar
Matthieu Constant committed
867
        vectorPop(p->contacts);
Matthieu Constant's avatar
Matthieu Constant committed
868
869
870
      else if ((cold = findContactSorted(c, oldContacts, &iold))) {
        coordAdd(&p->velocity[c->o1 * DIMENSION], -cold->dv, c->n);
        c->dv = cold->dv;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
871
872
873
874
#if FRICTION_ENABLED
        c->ct = cold->ct;
        p->omega[c->o1] -= c->In1*(c->ct);
#endif
Matthieu Constant's avatar
Matthieu Constant committed
875
876
877
878
      }
    }
  }
  // Triangles 
Nathan Coppin's avatar
Nathan Coppin committed
879
  #if DIMENSION == 3
Matthieu Constant's avatar
Matthieu Constant committed
880
881
882
883
884
885
  for (size_t i = 0; i < vectorSize(p->triangles); ++i) {
    triangleBoundingBox(&p->triangles[i], amin, amax);
    addAlert(alert/2, amin, amax);
    vectorClear(found);
    cellSearch(tree, amin, amax, &found);
    for (int j = 0; j < vectorSize(found); ++j) {
Matthieu Constant's avatar
Matthieu Constant committed
886
      Contact *c =  vectorPush(&p->contacts);
Matthieu Constant's avatar
Matthieu Constant committed
887
      if (!triangleInitContact(i, &p->triangles[i], found[j], &p->particles[found[j]], &p->position[found[j] * DIMENSION], alert, c))
Matthieu Constant's avatar
Matthieu Constant committed
888
        vectorPop(p->contacts);
Matthieu Constant's avatar
Matthieu Constant committed
889
890
891
892
893
894
      else if ((cold = findContactSorted(c, oldContacts, &iold))) {
        coordAdd(&p->velocity[c->o1 * DIMENSION], -cold->dv, c->n);
        c->dv = cold->dv;
      }
    }
  }
Nathan Coppin's avatar
Nathan Coppin committed
895
  #endif
Matthieu Constant's avatar
Matthieu Constant committed
896
  vectorFree(oldContacts);
Matthieu Constant's avatar
Matthieu Constant committed
897
898
899
900
  vectorFree(found);
  cellFree(tree);
}

Matthieu Constant's avatar
Matthieu Constant committed
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
typedef struct {
  size_t *data;
  size_t *b, *e;
  size_t size;
}Fifo;

static Fifo *fifoNew(size_t maxSize) {
  Fifo *f = malloc(sizeof(Fifo));
  f->b = f->e = f->data = malloc(sizeof(size_t)*(maxSize+1));
  f->size = maxSize+1;
  return f;
}

static void fifoPush(Fifo *f, size_t v) {
  *(f->e++) = v;
  if (f->e == f->data + f->size) f->e = f->data;
}

#define FIFO_EMPTY ((size_t)-1)

static size_t fifoPop(Fifo *f) {
  if (f->b == f->e) return FIFO_EMPTY;
  size_t v = *(f->b++);
  if (f->b == f->data + f->size) f->b = f->data;
  return v;
}

static void fifoFree(Fifo *f) {
  free(f->data);
  free(f);
}

933
static void _particleProblemSolveContacts(ParticleProblem *p, double dt, double tol) {
934
935
  int converged = 0;
  while (! converged) {
Nathan Coppin's avatar
Nathan Coppin committed
936
    converged = 1;
937
938
    for (int ic = vectorSize(p->contacts)-1; ic >= 0;  --ic) {
      Contact *c = &p->contacts[ic];
Nathan Coppin's avatar
Nathan Coppin committed
939
      int conv;
940
      switch (c->type) {
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
941
942
943
944
        case PARTICLE_PARTICLE :
          conv = contactParticleParticleSolve(c, p, dt,tol);
          break;
        case PARTICLE_DISK :
945
          conv = contactParticleDiskSolve(c, p, dt, tol);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
946
947
948
949
950
951
952
953
954
955
956
957
958
          break;
        case PARTICLE_SEGMENT :
          conv = contactParticleSegmentSolve(c, p, dt, tol);
          break;
#if DIMENSION == 3
        case PARTICLE_TRIANGLE :
          converged = contactParticleTriangleSolve(c, p, dt, tol);
          break;
#endif
      }
      if (!conv) {
        converged = 0;
      }
959
960
961
962
    }
  }
}

Matthieu Constant's avatar
Matthieu Constant committed
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
static void _particleProblemSolveContactsQueue(ParticleProblem *p, double dt, double tol) {
  size_t *nContactByParticle = malloc(sizeof(size_t)*(vectorSize(p->particles)));
  for (size_t i = 0; i < vectorSize(p->particles); ++i) {
    nContactByParticle[i] = 0;
  }
  for (size_t i = 0; i < vectorSize(p->contacts); ++i) {
    Contact *c = &p->contacts[i];
    if(c->type == PARTICLE_PARTICLE)
      nContactByParticle[c->o0]++;
    nContactByParticle[c->o1]++;
  }
  size_t *contactByParticleP = malloc(sizeof(size_t)*(vectorSize(p->particles)+1));
  contactByParticleP[0] = 0;
  for (size_t i = 0; i < vectorSize(p->particles); ++i){
    contactByParticleP[i+1] = contactByParticleP[i] + nContactByParticle[i];
    nContactByParticle[i] = 0;
  }
  size_t *contactByParticle = malloc(contactByParticleP[vectorSize(p->particles)]*sizeof(size_t));
  for (size_t i = 0; i < vectorSize(p->contacts); ++i) {
    Contact *c = &p->contacts[i];
    if (c->type == PARTICLE_PARTICLE)
      contactByParticle[contactByParticleP[c->o0]+(nContactByParticle[c->o0]++)] = i;
    contactByParticle[contactByParticleP[c->o1]+(nContactByParticle[c->o1]++)] = i;
  }
  free(nContactByParticle);
Nathan Coppin's avatar
Nathan Coppin committed
988

Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
989
  Fifo *queue = fifoNew(vectorSize(p->contacts));
Matthieu Constant's avatar
Matthieu Constant committed
990
991
992
  int *activeContact = malloc(sizeof(int)*vectorSize(p->contacts));
  for (size_t i = 0; i < vectorSize(p->contacts); ++i) {
    fifoPush(queue, i);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
993
    activeContact[i] = 1;
Matthieu Constant's avatar
Matthieu Constant committed
994
995
996
  }
  for (size_t ic = fifoPop(queue); ic != FIFO_EMPTY; ic=fifoPop(queue)){
    Contact *c = &p->contacts[ic];
Nathan Coppin's avatar
Nathan Coppin committed
997
    int converged;
Matthieu Constant's avatar
Matthieu Constant committed
998
    switch (c->type) {
999
    case PARTICLE_PARTICLE :
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
1000
      converged = contactParticleParticleSolve(c, p, dt, tol);
1001
1002
      break;
    case PARTICLE_DISK :
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
1003
      converged = contactParticleDiskSolve(c, p, dt, tol);
1004
1005
      break;
    case PARTICLE_SEGMENT :
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
1006
      converged = contactParticleSegmentSolve(c, p, dt, tol);
1007
      break;
Matthieu Constant's avatar
Matthieu Constant committed
1008
#if DIMENSION == 3
1009
    case PARTICLE_TRIANGLE :
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
1010
      converged = contactParticleTriangleSolve(c, p, dt, tol);
1011
      break;
Matthieu Constant's avatar
Matthieu Constant committed
1012
#endif
Matthieu Constant's avatar
Matthieu Constant committed
1013
    }
Nathan Coppin's avatar
Nathan Coppin committed
1014
    if (!converged) {
Matthieu Constant's avatar
Matthieu Constant committed
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
      if(c->type == PARTICLE_PARTICLE) {
        for (size_t j = contactByParticleP[c->o0]; j < contactByParticleP[c->o0+1]; ++j) {
          if (!activeContact[contactByParticle[j]]){
            activeContact[contactByParticle[j]] = 1;
            fifoPush(queue, contactByParticle[j]);
          }
        }
      }
      for (size_t j = contactByParticleP[c->o1]; j < contactByParticleP[c->o1+1]; ++j) {
        if (!activeContact[contactByParticle[j]]){
          activeContact[contactByParticle[j]] = 1;
          fifoPush(queue, contactByParticle[j]);
        }
      }
Matthieu Constant's avatar
Matthieu Constant committed
1029
    }
Matthieu Constant's avatar
Matthieu Constant committed
1030
    activeContact[ic] = 0;
Matthieu Constant's avatar
Matthieu Constant committed
1031
  }
Nathan Coppin's avatar
Nathan Coppin committed
1032

Matthieu Constant's avatar
Matthieu Constant committed
1033
1034
1035
1036
  fifoFree(queue);
  free(activeContact);
  free(contactByParticleP);
  free(contactByParticle);
Matthieu Constant's avatar
Matthieu Constant committed
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
}

double particleProblemMaxDt(const ParticleProblem *p, double alert) {
  double VM2 = 0;
  double FM2 = 0;
  for (size_t j = 0; j < vectorSize(p->particles); ++j) {
    VM2 = fmax(VM2, dot(&p->velocity[j * DIMENSION], &p->velocity[j * DIMENSION]));
    FM2 = fmax(FM2, dot(&p->externalForces[j * DIMENSION], &p->externalForces[j * DIMENSION]) / (p->particles[j].m * p->particles[j].m));
  }
  const double VM = sqrt(VM2);
  const double FM = sqrt(FM2);
  const double q = VM + sqrt(VM2 + 4 * FM * alert);
  return alert / q;
}

1052
void particleProblemSolve(ParticleProblem *p, double alert, double dt, double tol, int maxit)
Matthieu Constant's avatar
Matthieu Constant committed
1053
1054
{
  _particleProblemGenContacts(p, alert);
1055
  if (p->use_queue)
1056
    _particleProblemSolveContactsQueue(p,dt,tol);
1057
1058
  else
    _particleProblemSolveContacts(p,dt,tol);
Matthieu Constant's avatar
Matthieu Constant committed
1059
1060
}

Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077
1078
1079
static void particleProblemApplyCtTranslation(ParticleProblem *p)
{
#if FRICTION_ENABLED
  for (int i = 0; i < vectorSize(p->contacts); ++i) {
    const Contact *c = p->contacts+i;
    double t[2] = {c->n[1], -c->n[0]};
    double ct = c->ct;
    if(c->type == PARTICLE_PARTICLE) {
      coordAdd(&p->velocity[c->o0 * DIMENSION], -ct*c->a0, t);
      coordAdd(&p->velocity[c->o0 * DIMENSION], ct*c->a1, t);
    }
    else {
      coordAdd(&p->velocity[c->o1 * DIMENSION], -ct*c->a1, t);
    }
  }
#endif
}


1080
void particleProblemIterate(ParticleProblem *p, double alert, double dt, double tol, int maxit)
Nathan Coppin's avatar
Nathan Coppin committed
1081
{
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
1082
  int r = 0;
1083
1084
1085
  for (size_t j = 0; j < vectorSize(p->particles); ++j)
    for (size_t i = 0; i < DIMENSION; ++i)
      p->velocity[j * DIMENSION + i] += p->externalForces[j * DIMENSION + i] * dt / p->particles[j].m;
1086
  particleProblemSolve(p, alert, dt, tol, maxit);
Nathan Coppin's avatar
Nathan Coppin committed
1087
  for (size_t i = 0; i < vectorSize(p->position); ++i){
Matthieu Constant's avatar
Matthieu Constant committed
1088
    p->position[i] += p->velocity[i] * dt;
Nathan Coppin's avatar
Nathan Coppin committed
1089
  }
Matthieu Constant's avatar
Matthieu Constant committed
1090
1091
1092
1093
1094
1095
1096
1097
  for (size_t i = 0; i < vectorSize(p->disks); ++i)
    for (int j = 0; j < DIMENSION; ++j)
      p->disks[i].x[j] += p->disks[i].v[j] * dt;
  for (size_t i = 0; i < vectorSize(p->segments); ++i)
    for (int j = 0; j < DIMENSION; ++j) {
      p->segments[i].p[0][j] += p->segments[i].v[j] * dt;
      p->segments[i].p[1][j] += p->segments[i].v[j] * dt;
    }
Nathan Coppin's avatar
Nathan Coppin committed
1098
1099
1100
1101
1102
1103
1104
1105
1106
  #if DIMENSION == 3
  for (size_t i = 0; i < vectorSize(p->triangles); ++i) {
    for (int j = 0; j < DIMENSION; ++j) {
      p->triangles[i].p[0][j] += p->triangles[i].v[j] * dt;
      p->triangles[i].p[1][j] += p->triangles[i].v[j] * dt;
      p->triangles[i].p[2][j] += p->triangles[i].v[j] * dt;
    }
  }
  #endif
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
1107
  particleProblemApplyCtTranslation(p);
Matthieu Constant's avatar
Matthieu Constant committed
1108
1109
}

1110
size_t particleProblemAddBoundarySegmentTagId(ParticleProblem *p, const double x0[DIMENSION], const double x1[DIMENSION], int tag, int materialTag) {
Matthieu Constant's avatar
Matthieu Constant committed
1111
1112
  Segment *s = vectorPush(&p->segments);
  *vectorPush(&p->segmentTag) = tag;
1113
  *vectorPush(&p->segmentMaterial) = materialTag;
Matthieu Constant's avatar
Matthieu Constant committed
1114
1115
1116
1117
1118
  for (int i = 0; i < DIMENSION; ++i) {
    s->p[0][i] = x0[i];
    s->p[1][i] = x1[i];
    s->v[i] = 0.;
  }
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
1119
#ifdef FRICTION_ENABLED
Nathan Coppin's avatar
Nathan Coppin committed
1120
  s->vt = 0.0;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
1121
#endif
Matthieu Constant's avatar
Matthieu Constant committed
1122
1123
1124
  return vectorSize(p->segments) - 1;
}

1125
1126
size_t particleProblemAddBoundarySegment(ParticleProblem *p, const double x0[DIMENSION], const double x1[DIMENSION], const char *tag, const char *material) {
  return particleProblemAddBoundarySegmentTagId(p, x0, x1, particleProblemGetTagId(p, tag),particleProblemGetMaterialTagId(p, material));
Matthieu Constant's avatar
Matthieu Constant committed
1127
1128
}

1129
void particleProblemAddParticleMaterialTagId(ParticleProblem *p, const double x[DIMENSION], double r, double m, int tag) {
Matthieu Constant's avatar
Matthieu Constant committed
1130
1131
1132
1133
1134
1135
1136
  size_t n = vectorSize(p->particles);
  Particle *particle = vectorPush(&p->particles);
  particle->r = r;
  particle->m = m;
  vectorPushN(&p->position, DIMENSION);
  vectorPushN(&p->velocity, DIMENSION);
  vectorPushN(&p->externalForces, DIMENSION);
1137
  vectorPushN(&p->particleMaterial,1);
Matthieu Constant's avatar
Matthieu Constant committed
1138
1139
  for (int i = 0; i < DIMENSION; ++i) {
    p->position[n * DIMENSION +  i] = x[i];
Nathan Coppin's avatar
Nathan Coppin committed
1140
    p->velocity[n * DIMENSION +  i] = 0;
Matthieu Constant's avatar
Matthieu Constant committed
1141
1142
    p->externalForces[n * DIMENSION + i] = 0;
  }
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
1143
1144
#ifdef FRICTION_ENABLED
  vectorPushN(&p->omega,1);
Nathan Coppin's avatar
Nathan Coppin committed
1145
  p->omega[n] = 0.0;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
1146
#endif
1147
1148
1149
1150
1151
  p->particleMaterial[n] = tag;
}

void particleProblemAddParticle(ParticleProblem *p, const double x[DIMENSION], double r, double m, const char *material) {
  particleProblemAddParticleMaterialTagId(p,x,r,m,particleProblemGetMaterialTagId(p,material));
1152
}
1153

Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
1154
#ifdef FRICTION_ENABLED
1155
1156
1157
1158
1159
void particleProblemSetFrictionCoefficient(ParticleProblem *p, double mu, const char *mat0, const char *mat1) {
  int imat0 = particleProblemGetMaterialTagId(p,mat0);
  int imat1 = particleProblemGetMaterialTagId(p,mat1);
  int n = particleProblemNMaterial(p);
  p->mu[imat0*n+imat1] = p->mu[imat1*n+imat0] = mu;
Matthieu Constant's avatar
Matthieu Constant committed
1160
}
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
1161

Nathan Coppin's avatar
Nathan Coppin committed
1162
1163
1164
1165
1166
1167
1168
1169
1170
1171
1172
1173
1174
1175
1176
1177
void particleProblemSetBoundaryTangentVelocity(ParticleProblem *p, const char *tag, double v){ 
  int ttag = (int)particleProblemGetTagId(p,tag);
  int d = vectorSize(p->diskTag);
  int s = vectorSize(p->segmentTag);
  int i;
  for(i=0;i<d;i++){
    if(</