scontact.c 43.3 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

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

struct _ParticleProblem{
41
  int frictionFlag;
42
  double friction_relaxation;
Matthieu Constant's avatar
Matthieu Constant committed
43
44
  Particle *particles;
  Contact *contacts;
45
  double *position, *velocity, *externalForces, *omega, *mu;
Matthieu Constant's avatar
Matthieu Constant committed
46
47
  Disk *disks;
  char **tagname;
48
  int *diskTag, *segmentTag, *particleTag;
Matthieu Constant's avatar
Matthieu Constant committed
49
  Segment *segments;
Nathan Coppin's avatar
Nathan Coppin committed
50
  #if DIMENSION == 3
Matthieu Constant's avatar
Matthieu Constant committed
51
52
  Triangle *triangles;
  int *triangleTag;
Nathan Coppin's avatar
Nathan Coppin committed
53
  #endif
54
  int use_queue;
Matthieu Constant's avatar
Matthieu Constant committed
55
56
};

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

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

struct _Contact {
  size_t o0, o1;
  double a0, a1;
80
  double In0, In1;
Matthieu Constant's avatar
Matthieu Constant committed
81
82
  double D;
  double dv;
Nathan Coppin's avatar
Nathan Coppin committed
83
  double ct;
Matthieu Constant's avatar
Matthieu Constant committed
84
  ContactType type;
Matthieu Constant's avatar
Matthieu Constant committed
85
86
87
88
89
90
91
92
93
94
95
96
97
  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;
Nathan Coppin's avatar
Nathan Coppin committed
98
  c->ct = 0;
Matthieu Constant's avatar
Matthieu Constant committed
99
100
101
102
103
104
105
  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);
106
  for (int i = 0; i < DIMENSION; ++i)
Matthieu Constant's avatar
Matthieu Constant committed
107
108
109
110
    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);
111
112
  c->In0 = c->a0*5/(2*p0->r);
  c->In1 = c->a1*5/(2*p1->r);
Matthieu Constant's avatar
Matthieu Constant committed
113
  c->type = PARTICLE_PARTICLE;
Matthieu Constant's avatar
Matthieu Constant committed
114
115
116
117
118
119
120
  return c->D < alert;
}

struct _Disk {
  double x[DIMENSION];
  double v[DIMENSION];
  double r;
Nathan Coppin's avatar
Nathan Coppin committed
121
  double vt;
Matthieu Constant's avatar
Matthieu Constant committed
122
123
124
125
126
127
128
129
130
131
};

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

Nathan Coppin's avatar
Nathan Coppin committed
132
size_t particleProblemAddBoundaryDiskTagId(ParticleProblem *p, const double x[DIMENSION], double r, size_t tag) {
Matthieu Constant's avatar
Matthieu Constant committed
133
134
135
  Disk *d = vectorPush(&p->disks);
  *vectorPush(&p->diskTag) = tag;
  d->r = r;
Nathan Coppin's avatar
Nathan Coppin committed
136
  d->vt = 0;
Matthieu Constant's avatar
Matthieu Constant committed
137
138
139
140
141
142
143
144
145
146
147
148
149
150
  for (int i = 0; i < DIMENSION; ++i) {
    d->x[i] = x[i];
    d->v[i] = 0.;
  }
  return vectorSize(p->disks) - 1;
}

size_t particleProblemAddBoundaryDisk(ParticleProblem *p, const double x[DIMENSION], double r, const char *tag) {
  return particleProblemAddBoundaryDiskTagId(p, x, r, particleProblemGetTagId(p, tag));
}

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;
Nathan Coppin's avatar
Nathan Coppin committed
151
  c->ct = 0;
Matthieu Constant's avatar
Matthieu Constant committed
152
153
154
155
156
157
158
159
160
161
162
163
164
165
  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.;
Nathan Coppin's avatar
Nathan Coppin committed
166
  c->In0 = 0;
167
  c->In1 = c->a1*5/(2*p->r);
Matthieu Constant's avatar
Matthieu Constant committed
168
  c->type = PARTICLE_DISK;
Matthieu Constant's avatar
Matthieu Constant committed
169
170
171
  return c->D < alert;
}

172
struct _Segment{
Matthieu Constant's avatar
Matthieu Constant committed
173
174
  double p[2][DIMENSION];
  double v[DIMENSION];
Nathan Coppin's avatar
Nathan Coppin committed
175
  double vt;
Matthieu Constant's avatar
Matthieu Constant committed
176
};
Nathan Coppin's avatar
Nathan Coppin committed
177

Nathan Coppin's avatar
Nathan Coppin committed
178
179
180
181
182
183
184
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
185

Matthieu Constant's avatar
Matthieu Constant committed
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
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;
Nathan Coppin's avatar
Nathan Coppin committed
207
  c->ct = 0;
Matthieu Constant's avatar
Matthieu Constant committed
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
  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;
  }
225
  //printf("normal : %0.16f %0.16f  tag : %d start : %0.3f end : %0.3f \n",c->n[0],c->n[1],id,s->p[0][0],s->p[1][0]);
Matthieu Constant's avatar
Matthieu Constant committed
226
227
228
229
  c->D = nnorm - p->r;
  if (c->D < 0 && segmentProjectionIsInside(s, x)) c->D = 0;
  c->a0 = 0.;
  c->a1 = 1.;
Nathan Coppin's avatar
Nathan Coppin committed
230
  c->In0 = 0;
231
  c->In1 = c->a1*5/(2*p->r);
Matthieu Constant's avatar
Matthieu Constant committed
232
  c->type = PARTICLE_SEGMENT;
Matthieu Constant's avatar
Matthieu Constant committed
233
234
235
236
237
238
239
240
241
  return c->D >= 0  && c->D < alert;
}

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

Nathan Coppin's avatar
Nathan Coppin committed
242
void particleProblemAddBoundaryTriangleTagId(ParticleProblem *p, const double x0[3], const double x1[3], const double x2[3], size_t tag) {
Matthieu Constant's avatar
Matthieu Constant committed
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
  Triangle *t = vectorPush(&p->triangles);
  *vectorPush(&p->triangleTag) = tag;
  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.;
  }
}

void particleProblemAddBoundaryTriangle(ParticleProblem *p, const double x0[3], const double x1[3], const double x2[3], const char *tag) {
  return particleProblemAddBoundaryTriangleTagId(p, x0, x1, x2, particleProblemGetTagId(p, tag));
}

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] = 
281
    {{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
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
    {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;
Nathan Coppin's avatar
Nathan Coppin committed
297
  c->ct = 0;
Matthieu Constant's avatar
Matthieu Constant committed
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
  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.;
Nathan Coppin's avatar
Nathan Coppin committed
315
  c->In0 = 0;
316
  c->In1 = c->a1*5/(2*p->r);
Matthieu Constant's avatar
Matthieu Constant committed
317
318
  if (c->D < 0)
    c->D = 0;
Matthieu Constant's avatar
Matthieu Constant committed
319
  c->type = PARTICLE_TRIANGLE;
Matthieu Constant's avatar
Matthieu Constant committed
320
321
322
323
324
  return (c->D > 0 || triangleProjectionIsInside(t, x)) && c->D < alert;
}

#endif

Nathan Coppin's avatar
Nathan Coppin committed
325
static int contactParticleParticleSolve(Contact *c, ParticleProblem *p, double dt, double *dp, double *ct, double tol) {
326
327
328

  #if DIMENSION == 3

Jonathan Lambrechts's avatar
pp    
Jonathan Lambrechts committed
329
  double vn = (p->velocity[c->o0*3] - p->velocity[c->o1*3]) * c->n[0] + (p->velocity[c->o0*3+1] - p->velocity[c->o1*3+1]) * c->n[1] + (p->velocity[c->o0 * 3+2] - p->velocity[c->o1 * 3+2]) * c->n[2] + c->dv;
Nathan Coppin's avatar
Nathan Coppin committed
330
331
  *dp = fmax(0., vn - c->D/dt) - c->dv;
  *ct = 0 ;
Jonathan Lambrechts's avatar
pp    
Jonathan Lambrechts committed
332
333
  coordAdd(&p->velocity[c->o0*3], -*dp*c->a0, c->n);
  coordAdd(&p->velocity[c->o1*3], *dp*c->a1, c->n);
334
335
336

  #else 

Nathan Coppin's avatar
Nathan Coppin committed
337
338
339
  double t[2];
  t[0] = c->n[1];
  t[1] = -c->n[0];
Jonathan Lambrechts's avatar
pp    
Jonathan Lambrechts committed
340
341
342
  double vn = c->dv + 
    (p->velocity[c->o0*2+0] - p->velocity[c->o1*2+0])*c->n[0] +
    (p->velocity[c->o0*2+1] - p->velocity[c->o1*2+1])*c->n[1];
343
  double vt = c->ct +
Nathan Coppin's avatar
Nathan Coppin committed
344
345
346
    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
347
348
349
    (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;
350
  //printf("vt avant : %.8f c->ct : %.8f \n",vt,c->ct);
Nathan Coppin's avatar
Nathan Coppin committed
351
  *dp = fmax(0., vn - c->D/dt);
352
353
354
355
356
  int index = p->particleTag[c->o0]*vectorSize(p->mu) + p->particleTag[c->o1];
  double mu = p->mu[index];
  if(p->frictionFlag && mu!=0){
    if(vt>(7./2.)*mu*(*dp)){
      *ct = p->friction_relaxation*mu*(*dp);//printf("Slip\n");
Nathan Coppin's avatar
Nathan Coppin committed
357
    }
358
359
    else if(vt<(-7./2.)*mu*(*dp)){
      *ct = -p->friction_relaxation*mu*(*dp);//printf("Slip\n");
Jonathan Lambrechts's avatar
pp    
Jonathan Lambrechts committed
360
361
    }
    else{
362
      *ct = p->friction_relaxation*(2./7.)*vt;//printf("Stick\n");
Jonathan Lambrechts's avatar
pp    
Jonathan Lambrechts committed
363
364
365
366
367
368
369
370
371
372
    }
    *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);
  }
  else{
    *ct = 0;
  }
Nathan Coppin's avatar
Nathan Coppin committed
373
  *dp -= c->dv;
374
  //printf("ct : %.8f dp : %.8f\n",*ct,*dp);
Nathan Coppin's avatar
Nathan Coppin committed
375
376
  coordAdd(&p->velocity[c->o0 * DIMENSION], -*dp*c->a0, c->n);
  coordAdd(&p->velocity[c->o1 * DIMENSION], *dp*c->a1, c->n);
Nathan Coppin's avatar
Nathan Coppin committed
377
  #endif  
Nathan Coppin's avatar
Nathan Coppin committed
378
  if(fabs(*dp) < tol/dt && fabs(*ct) <tol/dt){
Nathan Coppin's avatar
Nathan Coppin committed
379
    //printf("b : %d b : %d ok\n",c->o0,c->o1);
Nathan Coppin's avatar
Nathan Coppin committed
380
381
382
    return 1;
  }
  else{
Nathan Coppin's avatar
Nathan Coppin committed
383
    //printf("b : %d b : %d not ok\n",c->o0,c->o1);
Nathan Coppin's avatar
Nathan Coppin committed
384
385
    return 0;
  }
Matthieu Constant's avatar
Matthieu Constant committed
386
387
388
389
390
391
392
393
394
}

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

Nathan Coppin's avatar
Nathan Coppin committed
395
396
static int contactParticleDiskSolve(Contact *c, ParticleProblem *p, double *ct, double *dp, double dt, double tol) {
  double vlocfree[DIMENSION], t[2], vn;
Matthieu Constant's avatar
Matthieu Constant committed
397
  double vloc[DIMENSION];
Nathan Coppin's avatar
Nathan Coppin committed
398
399
  t[0] = c->n[1];
  t[1] = -c->n[0];
Matthieu Constant's avatar
Matthieu Constant committed
400
  for (int i= 0; i < DIMENSION; ++i)
Nathan Coppin's avatar
Nathan Coppin committed
401
    vloc[i] = p->velocity[c->o1*DIMENSION + i] - p->disks[c->o0].v[i];
Nathan Coppin's avatar
Nathan Coppin committed
402
  *dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt);
403
404
405
406
  
  #if DIMENSION == 2
  
  double vt = c->ct +
Nathan Coppin's avatar
Nathan Coppin committed
407
408
409
410
411
  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;
412
  //printf("vt avant : %.8f\n",vt);
413
414
415
416
417
  int index = (p->particleTag[c->o1]+1)*vectorSize(p->mu) -1;
  double mu = p->mu[index];
  if(p->frictionFlag && mu!=0){
    if(vt>(7./2.)*mu*(*dp)){
      *ct = p->friction_relaxation*mu*(*dp);//printf("Slip\n");
Nathan Coppin's avatar
Nathan Coppin committed
418
    }
419
420
    else if(vt<(-7./2.)*mu*(*dp)){
      *ct = -p->friction_relaxation*mu*(*dp);//printf("Slip\n");
Nathan Coppin's avatar
Nathan Coppin committed
421
422
    }
    else{
423
      *ct = p->friction_relaxation*(2./7.)*vt;//printf("Stick\n");
Nathan Coppin's avatar
Nathan Coppin committed
424
    }
Nathan Coppin's avatar
Nathan Coppin committed
425
    *ct -= c->ct;
Nathan Coppin's avatar
Nathan Coppin committed
426
427
428
    coordAdd(&p->velocity[c->o1 * DIMENSION], -*ct*c->a1, t);
    p->omega[c->o1] -= c->In1*(*ct);
  }
429
430
431
  else{
    *ct  = 0;
  }
Nathan Coppin's avatar
Nathan Coppin committed
432
433
  *dp -=c->dv;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -*dp, c->n);
Nathan Coppin's avatar
Nathan Coppin committed
434
//printf("dis : %d ct : %f dp : %f\n",c->o0,*ct,*dp);
Nathan Coppin's avatar
Nathan Coppin committed
435
 
436
437
438

  #else

Nathan Coppin's avatar
Nathan Coppin committed
439
440
  *dp -=c->dv;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -*dp, c->n);
441
442
443
  *ct = 0;

  #endif
Nathan Coppin's avatar
Nathan Coppin committed
444
  if(fabs(*dp) < tol/dt &&fabs(*ct) <tol/dt){
Nathan Coppin's avatar
Nathan Coppin committed
445
    //printf("dis : %d b : %d ok\n",c->o0,c->o1);
Nathan Coppin's avatar
Nathan Coppin committed
446
447
448
    return 1;
  }
  else{
Nathan Coppin's avatar
Nathan Coppin committed
449
    //printf("dis : %d b : %d not ok\n",c->o0,c->o1);
Nathan Coppin's avatar
Nathan Coppin committed
450
451
    return 0;
  }
Matthieu Constant's avatar
Matthieu Constant committed
452
453
}

Nathan Coppin's avatar
Nathan Coppin committed
454
455
456
457
458
static int contactParticleSegmentSolve(Contact *c, ParticleProblem *p, double *dp, double *ct, double dt, double tol) {
  double vlocfree[DIMENSION], vn, xn[DIMENSION], vloc[DIMENSION],t[2];
  double r = p->particles[c->o1].r;
  t[0] = c->n[1];
  t[1] = -c->n[0];
Matthieu Constant's avatar
Matthieu Constant committed
459
  for (int i= 0; i < DIMENSION; ++i)
Nathan Coppin's avatar
Nathan Coppin committed
460
    vloc[i] = p->velocity[c->o1*DIMENSION + i] - p->segments[c->o0].v[i];
Nathan Coppin's avatar
Nathan Coppin committed
461
  *dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt);
Matthieu Constant's avatar
Matthieu Constant committed
462
  for (int i = 0; i < DIMENSION; ++i)
Nathan Coppin's avatar
Nathan Coppin committed
463
464
465
    xn[i] = p->position[c->o1*DIMENSION + i] + r*c->n[i] + vlocfree[i]*c->D / vn;
  if (!segmentProjectionIsInside(&p->segments[c->o0], xn))
    *dp = 0;
466
467
468
469

  #if DIMENSION == 2
  
  double vt = c->ct +
Nathan Coppin's avatar
Nathan Coppin committed
470
471
472
473
474
  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;
475
  //printf("ball : %d vt avant : %.8f c->ct : %.8f \n",c->o1,vt,c->ct);
476
477
478
479
480
  int index = (p->particleTag[c->o1]+1)*vectorSize(p->mu) -1;
  double mu = p->mu[index];
  if(p->frictionFlag && mu!=0){
    if(vt>(7./2.)*mu*(*dp)){
      *ct = p->friction_relaxation*mu*(*dp);//printf("Slip\n");
Nathan Coppin's avatar
Nathan Coppin committed
481
    }
482
483
    else if(vt<(-7./2.)*mu*(*dp)){
      *ct = -p->friction_relaxation*mu*(*dp);//printf("Slip\n");
Nathan Coppin's avatar
Nathan Coppin committed
484
485
    }
    else{
486
      *ct = p->friction_relaxation*(2./7.)*vt;//printf("Stick\n");
Nathan Coppin's avatar
Nathan Coppin committed
487
    }
Nathan Coppin's avatar
Nathan Coppin committed
488
    *ct -= c->ct;
Nathan Coppin's avatar
Nathan Coppin committed
489
490
491
    coordAdd(&p->velocity[c->o1 * DIMENSION], -*ct*c->a1, t);
    p->omega[c->o1] -= c->In1*(*ct);
  }
492
493
494
  else{
    *ct = 0;
  }
Nathan Coppin's avatar
Nathan Coppin committed
495
  *dp -=c->dv;
496
  //printf("seg : %d ct : %f dp : %f\n",c->o0,*ct,*dp);
Nathan Coppin's avatar
Nathan Coppin committed
497
  coordAdd(&p->velocity[c->o1 * DIMENSION], -*dp, c->n);
498
499
500
501

  #else
  
  *ct = 0;
Nathan Coppin's avatar
Nathan Coppin committed
502
503
  *dp -=c->dv;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -*dp, c->n);
504
505
  
  #endif
Nathan Coppin's avatar
Nathan Coppin committed
506
  if(fabs(*dp) < tol/dt &&fabs(*ct) <tol/dt){
Nathan Coppin's avatar
Nathan Coppin committed
507
    //printf("seg : %d b : %d ok\n",c->o0,c->o1);
Nathan Coppin's avatar
Nathan Coppin committed
508
509
510
    return 1;
  }
  else{
Nathan Coppin's avatar
Nathan Coppin committed
511
    //printf("seg : %d b : %d not ok\n",c->o0,c->o1);
Nathan Coppin's avatar
Nathan Coppin committed
512
513
    return 0;
  }
Matthieu Constant's avatar
Matthieu Constant committed
514
515
516
}

#if DIMENSION == 3
Nathan Coppin's avatar
Nathan Coppin committed
517
static int contactParticleTriangleSolve(Contact *c, const double *v, const double *x, double r, Triangle *t, double dt, double tol, double *dp, double *ct) {
Matthieu Constant's avatar
Matthieu Constant committed
518
519
520
  double vlocfree[DIMENSION], vn, xn[DIMENSION], vloc[DIMENSION];
  for (int i= 0; i < DIMENSION; ++i)
    vloc[i] = v[i] - t->v[i];
Nathan Coppin's avatar
Nathan Coppin committed
521
522
  *dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt) - c->dv;
  *ct = 0;
Matthieu Constant's avatar
Matthieu Constant committed
523
524
525
526
  for (int i = 0; i < DIMENSION; ++i)
    xn[i] = x[i] + r*c->n[i] + vlocfree[i]*c->D/vn;
  if (!triangleProjectionIsInside(t, xn))
    dp = 0;
Nathan Coppin's avatar
Nathan Coppin committed
527
528
529
530
531
532
  if(fabs(*dp) < tol/dt){
    return 1;
  }
  else{
    return 0;
  }
Matthieu Constant's avatar
Matthieu Constant committed
533
534
535
}
#endif

Nathan Coppin's avatar
Nathan Coppin committed
536

Nathan Coppin's avatar
Nathan Coppin committed
537

Matthieu Constant's avatar
Matthieu Constant committed
538
Contact *findContactSorted(Contact *c, Contact *v, size_t *i) {
Matthieu Constant's avatar
Matthieu Constant committed
539
  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
540
    (*i)++;
Matthieu Constant's avatar
Matthieu Constant committed
541
  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
542
543
544
545
546
547
548
}

static void addAlert(double alert, double *rmin, double *rmax) {
  rmin[0] -= alert;
  rmin[1] -= alert;
  rmax[0] += alert;
  rmax[1] += alert;
549
#if DIMENSION == 3
Matthieu Constant's avatar
Matthieu Constant committed
550
551
  rmin[2] -= alert;
  rmax[2] += alert;
552
#endif
Matthieu Constant's avatar
Matthieu Constant committed
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
}

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);
  }
583
#if DIMENSION == 3
Matthieu Constant's avatar
Matthieu Constant committed
584
585
586
587
  for (size_t i = 0; i < vectorSize(p->triangles); ++i) {
    triangleBoundingBox(&p->triangles[i], amin, amax);
    bbadd(bbmin, bbmax, amin, amax);
  }
588
#endif
Matthieu Constant's avatar
Matthieu Constant committed
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
}

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])
610
611
612
  - 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
613
614
615
616
617
618
619
620
621
622
}

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
623
  for (int i = 0; i < N * nElements; ++i) {
Matthieu Constant's avatar
Matthieu Constant committed
624
625
626
627
628
629
630
631
    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
632
  for (int i = 0; i < nElements; ++i) {
Matthieu Constant's avatar
Matthieu Constant committed
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
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
    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
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
      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
717
718
719
720
721
722
    //}
  }
  vectorFree(found);
  cellFree(tree);
}

723
724
725
726
727
728
729
730
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
731
  #if DIMENSION == 3
732
  if (amin[2] > bmax[2] || amax[2] < bmin[2]) return 0;
Nathan Coppin's avatar
Nathan Coppin committed
733
  #endif
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
  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
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
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
  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
823
824
825
826
827
          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;
        }*/
      }
828
    }
829
    start = end;
Nathan Coppin's avatar
Nathan Coppin committed
830
  }
831
  clock_t tic3 = clock();
Nathan Coppin's avatar
Nathan Coppin committed
832
833
  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);
834
835
836
  vectorFree(cc);
}

Matthieu Constant's avatar
Matthieu Constant committed
837
838
static void _particleProblemGenContacts(ParticleProblem *p, const double alert)
{
839
840
  //_particleProblemGenContacts2(p,alert);
  clock_t tic = clock();
Matthieu Constant's avatar
Matthieu Constant committed
841
842
843
844
845
846
847
848
849
850
851
  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);
852
  int ntot = 0;
Matthieu Constant's avatar
Matthieu Constant committed
853
854
855
856
857
  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
858
    for (size_t j = 0; j < vectorSize(found); ++j) {
859
      ntot++;
Matthieu Constant's avatar
Matthieu Constant committed
860
861
862
863
      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))) {
Nathan Coppin's avatar
Nathan Coppin committed
864
865
866
867
868
        double t[2];
        t[0] = c->n[1];
        t[1] = - c->n[0];
        coordAdd(&p->velocity[c->o0 * DIMENSION], -cold->dv*c->a0, c->n);
	coordAdd(&p->velocity[c->o1 * DIMENSION], cold->dv*c->a1, c->n);
Matthieu Constant's avatar
Matthieu Constant committed
869
        c->dv = cold->dv;
Nathan Coppin's avatar
Nathan Coppin committed
870
        c->ct = cold->ct;
Nathan Coppin's avatar
Nathan Coppin committed
871
	#if DIMENSION == 2
872
873
  	int index = p->particleTag[c->o0]*vectorSize(p->mu) + p->particleTag[c->o1];
  	if(p->frictionFlag && p->mu[index]!=0){
Nathan Coppin's avatar
Nathan Coppin committed
874
875
876
877
878
879
    	  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
880
881
882
      }
    }
  }
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
883
  //printf("time old %i\n", clock()-tic);
884
  //exit(0);
Matthieu Constant's avatar
Matthieu Constant committed
885
886
887
888
889
890
  // 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
891
892
    for (size_t j = 0; j < vectorSize(found); ++j) {
      Contact *c = vectorPush(&p->contacts);
Matthieu Constant's avatar
Matthieu Constant committed
893
      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
894
        vectorPop(p->contacts);
Matthieu Constant's avatar
Matthieu Constant committed
895
      else if ((cold = findContactSorted(c, oldContacts, &iold))) {
Nathan Coppin's avatar
Nathan Coppin committed
896
897
898
        double t[2];
        t[0] = c->n[1];
        t[1] = - c->n[0];
Matthieu Constant's avatar
Matthieu Constant committed
899
        coordAdd(&p->velocity[c->o1 * DIMENSION], -cold->dv, c->n);
900
        c->dv = cold->dv;
Nathan Coppin's avatar
Nathan Coppin committed
901
        c->ct = cold->ct;
Nathan Coppin's avatar
Nathan Coppin committed
902
	#if DIMENSION == 2
903
904
  	int index = (p->particleTag[c->o1]+1)*vectorSize(p->mu) -1;
  	if(p->frictionFlag && p->mu[index]!=0){
Nathan Coppin's avatar
Nathan Coppin committed
905
906
907
908
    	  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
913
914
915
916
917
      }
    }
  }
  // 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
918
919
    for (size_t j = 0; j < vectorSize(found); ++j) {
      Contact *c = vectorPush(&p->contacts);
Matthieu Constant's avatar
Matthieu Constant committed
920
      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
921
        vectorPop(p->contacts);
Matthieu Constant's avatar
Matthieu Constant committed
922
      else if ((cold = findContactSorted(c, oldContacts, &iold))) {
Nathan Coppin's avatar
Nathan Coppin committed
923
924
925
        double t[2];
        t[0] = c->n[1];
        t[1] = - c->n[0];
Matthieu Constant's avatar
Matthieu Constant committed
926
927
        coordAdd(&p->velocity[c->o1 * DIMENSION], -cold->dv, c->n);
        c->dv = cold->dv;
Nathan Coppin's avatar
Nathan Coppin committed
928
	c->ct = cold->ct;
Nathan Coppin's avatar
Nathan Coppin committed
929
	#if DIMENSION == 2
930
931
  	int index = (p->particleTag[c->o1]+1)*vectorSize(p->mu) -1;
  	if(p->frictionFlag && p->mu[index]!=0){
Nathan Coppin's avatar
Nathan Coppin committed
932
933
934
935
    	  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
936
937
938
939
      }
    }
  }
  // Triangles 
Nathan Coppin's avatar
Nathan Coppin committed
940
  #if DIMENSION == 3
Matthieu Constant's avatar
Matthieu Constant committed
941
942
943
944
945
946
  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
947
      Contact *c =  vectorPush(&p->contacts);
Matthieu Constant's avatar
Matthieu Constant committed
948
      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
949
        vectorPop(p->contacts);
Matthieu Constant's avatar
Matthieu Constant committed
950
951
952
      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
953
        c->ct = 0;
Matthieu Constant's avatar
Matthieu Constant committed
954
955
956
      }
    }
  }
Nathan Coppin's avatar
Nathan Coppin committed
957
  #endif
Matthieu Constant's avatar
Matthieu Constant committed
958
  vectorFree(oldContacts);
Matthieu Constant's avatar
Matthieu Constant committed
959
960
961
962
  vectorFree(found);
  cellFree(tree);
}

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
988
989
990
991
992
993
994
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);
}

995
996
997
static void _particleProblemSolveContacts(ParticleProblem *p, double dt, double tol) {
  int converged = 0;
  while (! converged) {
Nathan Coppin's avatar
Nathan Coppin committed
998
    converged = 1;
999
1000
    for (int ic = vectorSize(p->contacts)-1; ic >= 0;  --ic) {
      Contact *c = &p->contacts[ic];
For faster browsing, not all history is shown. View entire blame