scontact.c 49.2 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
  Contact *savedContacts;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
44
  double *position, *velocity, *externalForces;
45
  double *stressTensor;
Matthieu Constant's avatar
Matthieu Constant committed
46
  Disk *disks;
47
  char **tagname, **materialName;
48
  int *diskTag, *segmentTag;  
49
50
  int *diskMaterial, *segmentMaterial;
  int *particleMaterial;
Matthieu Constant's avatar
Matthieu Constant committed
51
  Segment *segments;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
52
53
54
55
56
57
#if FRICTION_ENABLED
  double *omega;
  double friction_relaxation;
  double *mu;
#endif
#if DIMENSION == 3
Matthieu Constant's avatar
Matthieu Constant committed
58
59
  Triangle *triangles;
  int *triangleTag;
60
  int *triangleMaterial;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
61
#endif
62
  int use_queue;
Matthieu Constant's avatar
Matthieu Constant committed
63
64
};

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

/* 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
90
91
#if FRICTION_ENABLED
  double In0, In1;
Nathan Coppin's avatar
Nathan Coppin committed
92
  double ct;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
93
#endif
Matthieu Constant's avatar
Matthieu Constant committed
94
  ContactType type;
Matthieu Constant's avatar
Matthieu Constant committed
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
  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);
115
  for (int i = 0; i < DIMENSION; ++i)
Matthieu Constant's avatar
Matthieu Constant committed
116
117
118
119
    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);
120
#if FRICTION_ENABLED
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
121
  c->ct = 0;
122
123
  c->In0 = c->a0*2/(p0->r);
  c->In1 = c->a1*2/(p1->r);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
124
#endif
Matthieu Constant's avatar
Matthieu Constant committed
125
  c->type = PARTICLE_PARTICLE;
Matthieu Constant's avatar
Matthieu Constant committed
126
127
128
129
130
131
132
  return c->D < alert;
}

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

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

146
147

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

162
163
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
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
}

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.;
183
#if FRICTION_ENABLED
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
184
  c->ct = 0;
Nathan Coppin's avatar
Nathan Coppin committed
185
  c->In0 = 0;
186
  c->In1 = c->a1*2/(p->r);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
187
#endif
Matthieu Constant's avatar
Matthieu Constant committed
188
  c->type = PARTICLE_DISK;
Matthieu Constant's avatar
Matthieu Constant committed
189
190
191
  return c->D < alert;
}

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

Nathan Coppin's avatar
Nathan Coppin committed
200
201
202
203
204
205
206
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
207

Matthieu Constant's avatar
Matthieu Constant committed
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
248
249
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.;
250
#if FRICTION_ENABLED
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
251
  c->ct = 0;
Nathan Coppin's avatar
Nathan Coppin committed
252
  c->In0 = 0;
253
  c->In1 = c->a1*2/(p->r);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
254
#endif
Matthieu Constant's avatar
Matthieu Constant committed
255
  c->type = PARTICLE_SEGMENT;
Matthieu Constant's avatar
Matthieu Constant committed
256
257
258
259
260
261
262
263
264
  return c->D >= 0  && c->D < alert;
}

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

265
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
266
267
  Triangle *t = vectorPush(&p->triangles);
  *vectorPush(&p->triangleTag) = tag;
268
  *vectorPush(&p->triangleMaterial) = materialTag;
Matthieu Constant's avatar
Matthieu Constant committed
269
270
271
272
273
274
275
276
  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.;
  }
}

277
278
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
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
}

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] = 
305
    {{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
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
336
337
    {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.;
338
#if FRICTION_ENABLED
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
339
  c->ct = 0;
Nathan Coppin's avatar
Nathan Coppin committed
340
  c->In0 = 0;
341
  c->In1 = c->a1*2/(p->r);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
342
#endif
Matthieu Constant's avatar
Matthieu Constant committed
343
344
  if (c->D < 0)
    c->D = 0;
Matthieu Constant's avatar
Matthieu Constant committed
345
  c->type = PARTICLE_TRIANGLE;
Matthieu Constant's avatar
Matthieu Constant committed
346
347
348
349
350
  return (c->D > 0 || triangleProjectionIsInside(t, x)) && c->D < alert;
}

#endif

351
#if FRICTION_ENABLED
352
353
354
355
356
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
357
358
359
360
361
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);
362
#if FRICTION_ENABLED
Nathan Coppin's avatar
Nathan Coppin committed
363
  double t[2];
364
365
  t[0] = -c->n[1];
  t[1] = c->n[0];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
366
  double ct = 0 ;
367
  double vt = c->ct +
Nathan Coppin's avatar
Nathan Coppin committed
368
369
370
    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
371
372
373
    (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;
374
  const double mu = particleProblemGetMu(p,p->particleMaterial[c->o0],p->particleMaterial[c->o1]);
375
  if(vt>(3./1.)*mu*(dp)){
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
376
    ct = p->friction_relaxation*mu*(dp);
Jonathan Lambrechts's avatar
pp    
Jonathan Lambrechts committed
377
  }
378
  else if(vt<(-3./1.)*mu*(dp)){
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
379
    ct = -p->friction_relaxation*mu*(dp);
Nathan Coppin's avatar
Nathan Coppin committed
380
381
  }
  else{
382
    ct = p->friction_relaxation*(1./3.)*vt;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
383
384
385
386
387
388
389
390
391
392
393
394
395
  }
  ct -= c->ct;
  coordAdd(&p->velocity[c->o0 * DIMENSION], -ct*c->a0, t);
  coordAdd(&p->velocity[c->o1 * DIMENSION], ct*c->a1, t);
  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;
396
#if FRICTION_ENABLED
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
397
398
399
  if(fabs(ct/p->friction_relaxation) > tol/dt) return 0;
#endif
  return 1;
Matthieu Constant's avatar
Matthieu Constant committed
400
401
402
403
404
405
406
407
408
}

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
409
410
static int contactParticleDiskSolve(Contact *c, ParticleProblem *p, double dt, double tol) {
  double vlocfree[DIMENSION], vn;
Matthieu Constant's avatar
Matthieu Constant committed
411
412
  double vloc[DIMENSION];
  for (int i= 0; i < DIMENSION; ++i)
Nathan Coppin's avatar
Nathan Coppin committed
413
    vloc[i] = p->velocity[c->o1*DIMENSION + i] - p->disks[c->o0].v[i];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
414
  double dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt);
415
  
416
#if FRICTION_ENABLED
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
417
  double t[2];
418
419
  t[0] = -c->n[1];
  t[1] = c->n[0];
420
  double vt = c->ct +
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
421
422
423
424
425
    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;
426
  const double mu = particleProblemGetMu(p,p->particleMaterial[c->o1],p->diskMaterial[c->o0]);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
427
  double ct = 0;
428
  if(vt>(3./1.)*mu*dp){
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
429
    ct = p->friction_relaxation*mu*dp;
Nathan Coppin's avatar
Nathan Coppin committed
430
  }
431
  else if(vt<(-3./1.)*mu*dp){
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
432
    ct = -p->friction_relaxation*mu*dp;
Nathan Coppin's avatar
Nathan Coppin committed
433
434
  }
  else{
435
    ct = p->friction_relaxation*(1./3.)*vt;
Nathan Coppin's avatar
Nathan Coppin committed
436
  }
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
437
438
439
440
441
442
443
444
445
  ct -= c->ct;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -ct*c->a1, t);
  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;
446
#if FRICTION_ENABLED
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
447
448
449
  if(fabs(ct/p->friction_relaxation) > tol/dt) return 0;
#endif
  return 1;
Matthieu Constant's avatar
Matthieu Constant committed
450
451
}

Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
452
453
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
454
  double r = p->particles[c->o1].r;
Matthieu Constant's avatar
Matthieu Constant committed
455
  for (int i= 0; i < DIMENSION; ++i)
Nathan Coppin's avatar
Nathan Coppin committed
456
    vloc[i] = p->velocity[c->o1*DIMENSION + i] - p->segments[c->o0].v[i];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
457
  double dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt);
Matthieu Constant's avatar
Matthieu Constant committed
458
  for (int i = 0; i < DIMENSION; ++i)
Nathan Coppin's avatar
Nathan Coppin committed
459
460
    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
461
    dp = 0;
462
#if FRICTION_ENABLED
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
463
  double t[2];
464
465
  t[0] = -c->n[1];
  t[1] = c->n[0];
466
  double vt = c->ct +
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
467
468
469
470
471
    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;
472
  const double mu = particleProblemGetMu(p,p->particleMaterial[c->o1],p->segmentMaterial[c->o0]);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
473
  double ct = 0;
474
  if(vt>(3./1.)*mu*dp){
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
475
    ct = p->friction_relaxation*mu*dp;
Nathan Coppin's avatar
Nathan Coppin committed
476
  }
477
  else if(vt<(-3./1.)*mu*dp){
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
478
    ct = -p->friction_relaxation*mu*dp;
Nathan Coppin's avatar
Nathan Coppin committed
479
480
  }
  else{
481
    ct = p->friction_relaxation*(1./3.)*vt;;
Nathan Coppin's avatar
Nathan Coppin committed
482
  }
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
483
484
485
486
487
488
489
490
491
  ct -= c->ct;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -ct*c->a1, t);
  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;
492
#if FRICTION_ENABLED
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
493
494
495
  if(fabs(ct/p->friction_relaxation) > tol/dt) return 0;
#endif
  return 1;
Matthieu Constant's avatar
Matthieu Constant committed
496
497
498
}

#if DIMENSION == 3
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
499
static int contactParticleTriangleSolve(Contact *c, ParticleProblem *p,double dt, double tol) {
Matthieu Constant's avatar
Matthieu Constant committed
500
  double vlocfree[DIMENSION], vn, xn[DIMENSION], vloc[DIMENSION];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
501
  double r = p->particles[c->o1].r;
Matthieu Constant's avatar
Matthieu Constant committed
502
  for (int i= 0; i < DIMENSION; ++i)
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
503
    vloc[i] = p->velocity[c->o1*DIMENSION+i] - p->triangles[c->o0].v[i];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
504
  double dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt);
Matthieu Constant's avatar
Matthieu Constant committed
505
  for (int i = 0; i < DIMENSION; ++i)
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
506
507
    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
508
    dp = 0;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
509
510
511
512
  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
513
514
515
}
#endif

Nathan Coppin's avatar
Nathan Coppin committed
516

Nathan Coppin's avatar
Nathan Coppin committed
517

Matthieu Constant's avatar
Matthieu Constant committed
518
Contact *findContactSorted(Contact *c, Contact *v, size_t *i) {
Matthieu Constant's avatar
Matthieu Constant committed
519
  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
520
    (*i)++;
Matthieu Constant's avatar
Matthieu Constant committed
521
  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
522
523
524
525
526
527
528
}

static void addAlert(double alert, double *rmin, double *rmax) {
  rmin[0] -= alert;
  rmin[1] -= alert;
  rmax[0] += alert;
  rmax[1] += alert;
529
#if DIMENSION == 3
Matthieu Constant's avatar
Matthieu Constant committed
530
531
  rmin[2] -= alert;
  rmax[2] += alert;
532
#endif
Matthieu Constant's avatar
Matthieu Constant committed
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
}

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);
  }
563
#if DIMENSION == 3
Matthieu Constant's avatar
Matthieu Constant committed
564
565
566
567
  for (size_t i = 0; i < vectorSize(p->triangles); ++i) {
    triangleBoundingBox(&p->triangles[i], amin, amax);
    bbadd(bbmin, bbmax, amin, amax);
  }
568
#endif
Matthieu Constant's avatar
Matthieu Constant committed
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
}

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
590
591
592
    - 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
593
594
595
596
597
598
599
600
601
602
}

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
603
  for (int i = 0; i < N * nElements; ++i) {
Matthieu Constant's avatar
Matthieu Constant committed
604
605
606
607
608
609
610
611
    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
612
  for (int i = 0; i < nElements; ++i) {
Matthieu Constant's avatar
Matthieu Constant committed
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
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
    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
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
      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
697
698
699
700
701
702
    //}
  }
  vectorFree(found);
  cellFree(tree);
}

703
704
705
706
707
708
709
710
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
711
  #if DIMENSION == 3
712
  if (amin[2] > bmax[2] || amax[2] < bmin[2]) return 0;
Nathan Coppin's avatar
Nathan Coppin committed
713
  #endif
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
  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
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
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
  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
803
804
805
806
807
          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;
        }*/
      }
808
    }
809
    start = end;
Nathan Coppin's avatar
Nathan Coppin committed
810
  }
811
  clock_t tic3 = clock();
Nathan Coppin's avatar
Nathan Coppin committed
812
813
  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);
814
815
816
  vectorFree(cc);
}

Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
817

818

Matthieu Constant's avatar
Matthieu Constant committed
819
820
static void _particleProblemGenContacts(ParticleProblem *p, const double alert)
{
821
822
  //_particleProblemGenContacts2(p,alert);
  clock_t tic = clock();
Matthieu Constant's avatar
Matthieu Constant committed
823
824
825
826
827
828
829
830
831
832
833
  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);
834
  int ntot = 0;
Matthieu Constant's avatar
Matthieu Constant committed
835
836
837
838
839
  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
840
    for (size_t j = 0; j < vectorSize(found); ++j) {
841
      ntot++;
Matthieu Constant's avatar
Matthieu Constant committed
842
843
844
845
      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
846
847
848
849
        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
850
        double t[2];
851
852
        t[0] = -c->n[1];
        t[1] = c->n[0];
Nathan Coppin's avatar
Nathan Coppin committed
853
        c->ct = cold->ct;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
854
855
856
857
858
        coordAdd(&p->velocity[c->o0 * DIMENSION], -c->ct*c->a0, t);
        coordAdd(&p->velocity[c->o1 * DIMENSION], c->ct*c->a1, t);
        p->omega[c->o0] -= c->In0*(c->ct);
        p->omega[c->o1] -= c->In1*(c->ct);
#endif
Matthieu Constant's avatar
Matthieu Constant committed
859
860
861
      }
    }
  }
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
862
  //printf("time old %i\n", clock()-tic);
863
  //exit(0);
Matthieu Constant's avatar
Matthieu Constant committed
864
865
866
867
868
869
  // 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
870
871
    for (size_t j = 0; j < vectorSize(found); ++j) {
      Contact *c = vectorPush(&p->contacts);
Matthieu Constant's avatar
Matthieu Constant committed
872
      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
873
        vectorPop(p->contacts);
Matthieu Constant's avatar
Matthieu Constant committed
874
      else if ((cold = findContactSorted(c, oldContacts, &iold))) {
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
875
876
877
        coordAdd(&p->velocity[c->o1 * DIMENSION], -cold->dv, c->n);
        c->dv = cold->dv;
#if FRICTION_ENABLED
Nathan Coppin's avatar
Nathan Coppin committed
878
        double t[2];
879
880
        t[0] = -c->n[1];
        t[1] = c->n[0];
Nathan Coppin's avatar
Nathan Coppin committed
881
        c->ct = cold->ct;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
882
883
884
        coordAdd(&p->velocity[c->o1 * DIMENSION], -c->ct*c->a1, t);
        p->omega[c->o1] -= c->In1*(c->ct);
#endif
Matthieu Constant's avatar
Matthieu Constant committed
885
886
887
888
889
890
891
892
893
      }
    }
  }
  // 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
894
895
    for (size_t j = 0; j < vectorSize(found); ++j) {
      Contact *c = vectorPush(&p->contacts);
Matthieu Constant's avatar
Matthieu Constant committed
896
      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
897
        vectorPop(p->contacts);
Matthieu Constant's avatar
Matthieu Constant committed
898
899
900
      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
901
902
#if FRICTION_ENABLED
        double t[2];
903
904
        t[0] = -c->n[1];
        t[1] = c->n[0];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
905
906
907
908
        c->ct = cold->ct;
        coordAdd(&p->velocity[c->o1 * DIMENSION], -c->ct*c->a1, t);
        p->omega[c->o1] -= c->In1*(c->ct);
#endif
Matthieu Constant's avatar
Matthieu Constant committed
909
910
911
912
      }
    }
  }
  // Triangles 
Nathan Coppin's avatar
Nathan Coppin committed
913
  #if DIMENSION == 3
Matthieu Constant's avatar
Matthieu Constant committed
914
915
916
917
918
919
  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
920
      Contact *c =  vectorPush(&p->contacts);
Matthieu Constant's avatar
Matthieu Constant committed
921
      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
922
        vectorPop(p->contacts);
Matthieu Constant's avatar
Matthieu Constant committed
923
924
925
926
927
928
      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
929
  #endif
Matthieu Constant's avatar
Matthieu Constant committed
930
  vectorFree(oldContacts);
Matthieu Constant's avatar
Matthieu Constant committed
931
932
933
934
  vectorFree(found);
  cellFree(tree);
}

Matthieu Constant's avatar
Matthieu Constant committed
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
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);
}

967
static void _particleProblemSolveContacts(ParticleProblem *p, double dt, double tol) {
968
969
970
971
  int converged = 0;
#if FRICTION_ENABLED
  int iter = 0;
#endif
972
  while (! converged) {
973
#if FRICTION_ENABLED
974
    if(iter>100*vectorSize(p->particles)) break;
975
#endif
Nathan Coppin's avatar
Nathan Coppin committed
976
    converged = 1;
977
978
    for (int ic = vectorSize(p->contacts)-1; ic >= 0;  --ic) {
      Contact *c = &p->contacts[ic];
Nathan Coppin's avatar
Nathan Coppin committed
979
      int conv;
980
      switch (c->type) {
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
        case PARTICLE_PARTICLE :
          conv = contactParticleParticleSolve(c, p, dt,tol);
          break;
        case PARTICLE_DISK :
          conv = contactParticleDiskSolve(c, p, dt, tol);
          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;
      }
999
    }
1000
#if FRICTION_ENABLED
For faster browsing, not all history is shown. View entire blame