scontact.c 54.5 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
#include "scontact.h" 
#include "quadtree.h"
#include "vector.h"
#include <math.h>
Nathan Coppin's avatar
Nathan Coppin committed
28
#include <stdint.h>
Matthieu Constant's avatar
Matthieu Constant committed
29
30
31
32
#include <stdio.h>
#include <stdlib.h>
#include <float.h>
#include <string.h>
33
#include <time.h>
Matthieu Constant's avatar
Matthieu Constant committed
34
35
36
37
38
39
40
41
42
43

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
44
  Contact *savedContacts;
45
  double *position, *velocity, *contactForces;
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;
51
  int *ForcedFlag;
Matthieu Constant's avatar
Matthieu Constant committed
52
  Segment *segments;
Nathan Coppin's avatar
Nathan Coppin committed
53
54
  double *momentum;
  int get_momentum;
55
  double a;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
56
#if FRICTION_ENABLED
Nathan Coppin's avatar
Nathan Coppin committed
57
#if ROTATION_ENABLED
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
58
  double *omega;
Nathan Coppin's avatar
Nathan Coppin committed
59
#endif
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
60
61
62
  double *mu;
#endif
#if DIMENSION == 3
Matthieu Constant's avatar
Matthieu Constant committed
63
64
  Triangle *triangles;
  int *triangleTag;
65
  int *triangleMaterial;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
66
#endif
67
  int use_queue;
Matthieu Constant's avatar
Matthieu Constant committed
68
69
};

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

/* 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
95
#if FRICTION_ENABLED
Nathan Coppin's avatar
Nathan Coppin committed
96
#if ROTATION_ENABLED
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
97
  double In0, In1;
Nathan Coppin's avatar
Nathan Coppin committed
98
#endif
Nathan Coppin's avatar
Nathan Coppin committed
99
  double ct;
Nathan Coppin's avatar
haha    
Nathan Coppin committed
100
101
102
#if DIMENSION == 3
  double cs;
#endif
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
103
#endif
Matthieu Constant's avatar
Matthieu Constant committed
104
  ContactType type;
Matthieu Constant's avatar
Matthieu Constant committed
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;
  }
}

115
static int particleInitContact(ParticleProblem *p, size_t id0, Particle *p0, double *x0, size_t id1, Particle *p1, double *x1, double alert, Contact *c) {
Matthieu Constant's avatar
Matthieu Constant committed
116
117
  double nnorm = 0;
  c->dv = 0;
118
119
120
121
122
123
#if FRICTION_ENABLED
  c->ct = 0;
#if DIMENSION == 3
  c->cs = 0;
#endif
#endif
Matthieu Constant's avatar
Matthieu Constant committed
124
125
126
127
128
129
130
  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);
131
  for (int i = 0; i < DIMENSION; ++i)
Matthieu Constant's avatar
Matthieu Constant committed
132
133
    c->n[i] /= -nnorm;
  c->D = fmax(0., nnorm - (p0->r + p1->r));
134
135
136
137
138
139
140
141
142
143
144
145
if(p->ForcedFlag[id0]&&!p->ForcedFlag[id1]){
    c->a0 = 0;
    c->a1 = 1;
  }
  else if(p->ForcedFlag[id1]&&!p->ForcedFlag[id0]){
    c->a0 = 1;
    c->a1 = 0;
  }
  else{
    c->a0 = p1->m / (p0->m + p1->m);
    c->a1 = p0->m / (p0->m + p1->m);
  }
Matthieu Constant's avatar
Matthieu Constant committed
146
  c->type = PARTICLE_PARTICLE;
Matthieu Constant's avatar
Matthieu Constant committed
147
148
149
150
151
152
153
  return c->D < alert;
}

struct _Disk {
  double x[DIMENSION];
  double v[DIMENSION];
  double r;
154
#if FRICTION_ENABLED
Nathan Coppin's avatar
Nathan Coppin committed
155
  double vt;
Nathan Coppin's avatar
haha    
Nathan Coppin committed
156
157
158
#if DIMENSION == 3
  double vs;
#endif
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
159
#endif
Matthieu Constant's avatar
Matthieu Constant committed
160
161
162
163
164
165
166
167
168
169
};

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

170
171

size_t particleProblemAddBoundaryDiskTagId(ParticleProblem *p, const double x[DIMENSION], double r, int tag, int materialTag) {
Matthieu Constant's avatar
Matthieu Constant committed
172
173
  Disk *d = vectorPush(&p->disks);
  *vectorPush(&p->diskTag) = tag;
174
  *vectorPush(&p->diskMaterial) = materialTag;
Matthieu Constant's avatar
Matthieu Constant committed
175
  d->r = r;
176
#if FRICTION_ENABLED
Nathan Coppin's avatar
Nathan Coppin committed
177
  d->vt = 0;
178
179
180
#if DIMENSION == 3
  d->vs = 0;
#endif
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
181
#endif
Matthieu Constant's avatar
Matthieu Constant committed
182
183
184
185
186
187
188
  for (int i = 0; i < DIMENSION; ++i) {
    d->x[i] = x[i];
    d->v[i] = 0.;
  }
  return vectorSize(p->disks) - 1;
}

189
190
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
191
192
}

193
static int diskInitContact(size_t id, const Disk *d, size_t particle, const Particle *p, double *x, double alert, Contact *c) {
Matthieu Constant's avatar
Matthieu Constant committed
194
195
196
197
  double nnorm = 0;
  c->dv = 0;
  c->o1 = particle;
  c->o0 = id;
198
199
200
201
202
203
  #if FRICTION_ENABLED
  c->ct = 0;
#if DIMENSION == 3
  c->cs = 0;
#endif
#endif
Matthieu Constant's avatar
Matthieu Constant committed
204
205
206
207
208
209
210
211
212
213
214
215
  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.;
Matthieu Constant's avatar
Matthieu Constant committed
216
  c->type = PARTICLE_DISK;
Matthieu Constant's avatar
Matthieu Constant committed
217
218
219
  return c->D < alert;
}

220
struct _Segment{
Matthieu Constant's avatar
Matthieu Constant committed
221
222
  double p[2][DIMENSION];
  double v[DIMENSION];
223
#if FRICTION_ENABLED
Nathan Coppin's avatar
Nathan Coppin committed
224
  double vt;
225
226
227
#if DIMENSION == 3
  double vs;
#endif
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
228
#endif
Matthieu Constant's avatar
Matthieu Constant committed
229
};
Nathan Coppin's avatar
Nathan Coppin committed
230

Nathan Coppin's avatar
Nathan Coppin committed
231
232
233
234
235
236
237
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
238

Matthieu Constant's avatar
Matthieu Constant committed
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
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);
}

256
static int segmentInitContact(size_t id, const Segment *s, size_t particle, const Particle *p, double *x, double alert, Contact *c) {
Matthieu Constant's avatar
Matthieu Constant committed
257
258
259
  c->o0 = id;
  c->o1 = particle;
  c->dv = 0;
260
261
262
263
264
265
#if FRICTION_ENABLED
  c->ct = 0;
#if DIMENSION == 3
  c->cs = 0;
#endif
#endif
Matthieu Constant's avatar
Matthieu Constant committed
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
  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.;
Matthieu Constant's avatar
Matthieu Constant committed
287
  c->type = PARTICLE_SEGMENT;
Matthieu Constant's avatar
Matthieu Constant committed
288
289
290
  return c->D >= 0  && c->D < alert;
}

291

Matthieu Constant's avatar
Matthieu Constant committed
292
293
294
295
#if DIMENSION == 3
struct _Triangle {
  double p[3][DIMENSION];
  double v[DIMENSION];
296
297
298
#if FRICTION_ENABLED
  double vt, vs;
#endif
Matthieu Constant's avatar
Matthieu Constant committed
299
300
};

301
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
302
303
  Triangle *t = vectorPush(&p->triangles);
  *vectorPush(&p->triangleTag) = tag;
304
  *vectorPush(&p->triangleMaterial) = materialTag;
Matthieu Constant's avatar
Matthieu Constant committed
305
306
307
308
309
310
  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.;
  }
311
312
313
314
#if FRICTION_ENABLED
  t->vt = 0;
  t->vs = 0;
#endif
Matthieu Constant's avatar
Matthieu Constant committed
315
316
}

317
318
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
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
}

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] = 
345
    {{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
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
    {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;
361
362
363
364
#if FRICTION_ENABLED
  c->ct = 0;
  c->cs = 0;
#endif
Matthieu Constant's avatar
Matthieu Constant committed
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
  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.;
  if (c->D < 0)
    c->D = 0;
Matthieu Constant's avatar
Matthieu Constant committed
384
  c->type = PARTICLE_TRIANGLE;
Matthieu Constant's avatar
Matthieu Constant committed
385
386
387
388
389
  return (c->D > 0 || triangleProjectionIsInside(t, x)) && c->D < alert;
}

#endif

390
391
392
393
394
static void build_basis(const double *a, double *b
#if DIMENSION==3
, double *c){
  b[0] = -a[2];
  b[1] = a[0];
395
396
397
398
399
400
401
402
403
404
405
  b[2] = -a[1]; 
  double provi[3];
  double ddot = dot(a,b);
  provi[0] = b[0] - ddot*a[0];
  provi[1] = b[1] - ddot*a[1];
  provi[2] = b[2] - ddot*a[2];
  double norm = sqrt(provi[0]*provi[0] + provi[1]*provi[1] + provi[2]*provi[2]);
  if(norm==0) norm=1.;
  b[0] = provi[0]/norm;
  b[1] = provi[1]/norm;
  b[2] = provi[2]/norm;
406
  _cross(a, b, c);
407

408
409
410
411
412
413
414
#else
){
  b[0] = -a[1];
  b[1] = a[0];
#endif
}

415
#if FRICTION_ENABLED
416
417
418
419
420
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
421
static int contactParticleParticleSolve(Contact *c, ParticleProblem *p, double dt, double tol) {
422
423
424
  if(p->ForcedFlag[c->o0] == 1 && p->ForcedFlag[c->o1] == 1){
    return 1;
  }
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
425
426
427
428
  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);
429
#if FRICTION_ENABLED
430
#if DIMENSION == 2
Nathan Coppin's avatar
Nathan Coppin committed
431
  double t[2];
432
  build_basis(c->n,t);
Nathan Coppin's avatar
Nathan Coppin committed
433
  double ct = 0;
Nathan Coppin's avatar
haha    
Nathan Coppin committed
434
  double vt = c->ct +
435
436
    (p->velocity[c->o0*2+0]-p->velocity[c->o1*2+0])*t[0]+
    (p->velocity[c->o0*2+1]-p->velocity[c->o1*2+1])*t[1];
Nathan Coppin's avatar
Nathan Coppin committed
437
#if ROTATION_ENABLED
438
    vt += 
Nathan Coppin's avatar
Nathan Coppin committed
439
440
441
442
     p->omega[c->o0]*p->particles[c->o0].r+
     p->omega[c->o1]*p->particles[c->o1].r;
#endif

443
444
#else
  double t[3], s[3];
Nathan Coppin's avatar
Nathan Coppin committed
445
  build_basis(c->n,t,s);
446
  double ct, cs = 0;
447
448
  double vt = c->ct;
  double vs = c->cs;
Nathan Coppin's avatar
Nathan Coppin committed
449
450
451
452
453
#if ROTATION_ENABLED
  double res0[3], res1[3];
  _cross(&p->omega[c->o0*3], c->n, res0);
  _cross(&p->omega[c->o1*3], c->n, res1);
#endif
454
455
456
457
458
459
460
461
  for(int i = 0;i<DIMENSION;i++){
    vt += (p->velocity[c->o0*3+i]-p->velocity[c->o1*3+i])*t[i];
    vs += (p->velocity[c->o0*3+i]-p->velocity[c->o1*3+i])*s[i];
#if ROTATION_ENABLED
    vt += (res1[i]*p->particles[c->o1].r+res0[i]*p->particles[c->o0].r)*t[i];
    vs += (res1[i]*p->particles[c->o1].r+res0[i]*p->particles[c->o0].r)*s[i];
#endif
  }
Nathan Coppin's avatar
Nathan Coppin committed
462
463
#endif

Nathan Coppin's avatar
haha    
Nathan Coppin committed
464
  const double mu = particleProblemGetMu(p,p->particleMaterial[c->o0],p->particleMaterial[c->o1]);
Nathan Coppin's avatar
Nathan Coppin committed
465
#if DIMENSION==2
466
  if(isnan(dp)){printf("hello\n");}
Nathan Coppin's avatar
Nathan Coppin committed
467
  if (fabs(vt)> ((p->a+1)/p->a)*mu*dp) vt *= ((p->a+1)/p->a)*mu*dp/fabs(vt);
Nathan Coppin's avatar
Nathan Coppin committed
468
  ct = vt;
469
470
  if (dp == 0.){
    ct = 0;
Nathan Coppin's avatar
Nathan Coppin committed
471
  }
Nathan Coppin's avatar
Nathan Coppin committed
472

Nathan Coppin's avatar
Nathan Coppin committed
473
#else
Nathan Coppin's avatar
Nathan Coppin committed
474
475
476
477
  double norme = sqrt(vt*vt + vs*vs);
  if(norme>3.5*mu*dp){
    vt *= 3.5*mu*dp/norme;
    vs *= 3.5*mu*dp/norme;
Nathan Coppin's avatar
Nathan Coppin committed
478
479
  }
  ct = vt;
Nathan Coppin's avatar
Nathan Coppin committed
480
  cs = vs;
481
482
  if (dp == 0.){
    cs = 0;
Nathan Coppin's avatar
Nathan Coppin committed
483
    ct = 0;
Nathan Coppin's avatar
haha    
Nathan Coppin committed
484
  }
Nathan Coppin's avatar
Nathan Coppin committed
485

486
#endif
Nathan Coppin's avatar
Nathan Coppin committed
487
#if ROTATION_ENABLED
488
  ct -= c->ct;
Nathan Coppin's avatar
Nathan Coppin committed
489
#if DIMENSION==2
Nathan Coppin's avatar
Nathan Coppin committed
490
491
  coordAdd(&p->velocity[c->o0 * DIMENSION], -ct*(p->a/(p->a+1))*c->a0, t);
  coordAdd(&p->velocity[c->o1 * DIMENSION], ct*(p->a/(p->a+1))*c->a1, t);
492
493
  p->omega[c->o0] -= (1/p->particles[c->o0].r)*(1/((p->a+1)))*ct*c->a0;
  p->omega[c->o1] -= (1/p->particles[c->o1].r)*(1/((p->a+1)))*ct*c->a1;
Nathan Coppin's avatar
Nathan Coppin committed
494
495

#else
Nathan Coppin's avatar
message    
Nathan Coppin committed
496
  cs -= c->cs;
Nathan Coppin's avatar
Nathan Coppin committed
497
498
  coordAdd(&p->velocity[c->o0 * DIMENSION], -ct*(p->a/(p->a+1))*c->a0, t);
  coordAdd(&p->velocity[c->o1 * DIMENSION], ct*(p->a/(p->a+1))*c->a1, t);
Nathan Coppin's avatar
Nathan Coppin committed
499
500
  coordAdd(&p->velocity[c->o0 * DIMENSION], -cs*(p->a/(p->a+1))*c->a0, s);
  coordAdd(&p->velocity[c->o1 * DIMENSION], cs*(p->a/(p->a+1))*c->a1, s);
Nathan Coppin's avatar
Nathan Coppin committed
501
  for (int i = 0; i<3; i++){   
502
503
504
505
    p->omega[c->o0*3 + i] += -(1/p->particles[c->o0].r)*(1/((p->a+1)))*ct*s[i]*c->a0 
                           + (1/p->particles[c->o0].r)*(1/((p->a+1)))*cs*t[i]*c->a0;
    p->omega[c->o1*3 + i] += -(1/p->particles[c->o1].r)*(1/((p->a+1)))*ct*s[i]*c->a1 
                           + (1/p->particles[c->o1].r)*(1/((p->a+1)))*cs*t[i]*c->a1;
Nathan Coppin's avatar
Nathan Coppin committed
506
  }
Nathan Coppin's avatar
Nathan Coppin committed
507
  c->cs += cs;
Nathan Coppin's avatar
Nathan Coppin committed
508
#endif
Nathan Coppin's avatar
haha    
Nathan Coppin committed
509
#else
510
511
  
  ct -= c->ct;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
512
513
  coordAdd(&p->velocity[c->o0 * DIMENSION], -ct*c->a0, t);
  coordAdd(&p->velocity[c->o1 * DIMENSION], ct*c->a1, t);
Nathan Coppin's avatar
Nathan Coppin committed
514
#if DIMENSION==3
515
  
Nathan Coppin's avatar
message    
Nathan Coppin committed
516
  
Nathan Coppin's avatar
Nathan Coppin committed
517
518
519
520
  cs -= c->cs;
  coordAdd(&p->velocity[c->o0 * DIMENSION], -cs*c->a0, s);
  coordAdd(&p->velocity[c->o1 * DIMENSION], cs*c->a1, s);
  c->cs += cs;
Nathan Coppin's avatar
haha    
Nathan Coppin committed
521
#endif
522
523
#endif
  c->ct += ct;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
524
525
526
527
528
529
#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;
530
#if FRICTION_ENABLED
Nathan Coppin's avatar
haha    
Nathan Coppin committed
531
#if DIMENSION == 3
532
  if(fabs(cs) > tol/dt) return 0;
Nathan Coppin's avatar
haha    
Nathan Coppin committed
533
#endif
534
  if(fabs(ct) > tol/dt) return 0;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
535
536
#endif
  return 1;
Matthieu Constant's avatar
Matthieu Constant committed
537
538
539
540
541
542
543
544
545
}

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
546
static int contactParticleDiskSolve(Contact *c, ParticleProblem *p, double dt, double tol) {
547
548
  if(p->ForcedFlag[c->o1])
    return 1;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
549
  double vlocfree[DIMENSION], vn;
Matthieu Constant's avatar
Matthieu Constant committed
550
551
  double vloc[DIMENSION];
  for (int i= 0; i < DIMENSION; ++i)
Nathan Coppin's avatar
Nathan Coppin committed
552
    vloc[i] = p->velocity[c->o1*DIMENSION + i] - p->disks[c->o0].v[i];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
553
  double dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt);
554
#if FRICTION_ENABLED
555
#if DIMENSION == 2
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
556
  double t[2];
557
  build_basis(c->n,t);
Nathan Coppin's avatar
Nathan Coppin committed
558
  double ct = 0;
559
  double vt = c->ct +
560
561
    (p->velocity[c->o1*2+0]- p->disks[c->o0].v[0])*t[0]+
    (p->velocity[c->o1*2+1]- p->disks[c->o0].v[1])*t[1]+
Nathan Coppin's avatar
Nathan Coppin committed
562
563
    p->disks[c->o0].vt;
#if ROTATION_ENABLED
564
    vt += 
Nathan Coppin's avatar
Nathan Coppin committed
565
566
     p->omega[c->o1]*p->particles[c->o1].r;
#endif
567
568
#else
  double t[3], s[3];
Nathan Coppin's avatar
Nathan Coppin committed
569
  build_basis(c->n,t,s);
570
  double ct, cs = 0;
571
572
  double vt = c->ct + p->disks[c->o0].vt;
  double vs = c->cs + p->disks[c->o0].vs;
Nathan Coppin's avatar
Nathan Coppin committed
573
574
575
576
#if ROTATION_ENABLED
  double res1[3];
  _cross(&p->omega[c->o1*3], c->n, res1);
#endif
577
578
579
580
581
582
583
584
  for(int i = 0;i<DIMENSION;i++){
    vt += (p->velocity[c->o1*3+i]- p->disks[c->o0].v[i])*t[i];
    vs += (p->velocity[c->o1*3+i]- p->disks[c->o0].v[i])*s[i];
#if ROTATION_ENABLED
    vt += (res1[i]*p->particles[c->o1].r)*t[i];
    vs += (res1[i]*p->particles[c->o1].r)*s[i];
#endif
  }
Nathan Coppin's avatar
Nathan Coppin committed
585
#endif
586
  const double mu = particleProblemGetMu(p,p->particleMaterial[c->o1],p->diskMaterial[c->o0]);
Nathan Coppin's avatar
Nathan Coppin committed
587
#if DIMENSION==2
588
  
Nathan Coppin's avatar
Nathan Coppin committed
589
  if (fabs(vt)> ((p->a+1)/p->a)*mu*dp) vt *= ((p->a+1)/p->a)*mu*dp/fabs(vt);
Nathan Coppin's avatar
Nathan Coppin committed
590
  ct = vt;
Nathan Coppin's avatar
Nathan Coppin committed
591
592
  if (dp == 0.){
    ct = 0;
Nathan Coppin's avatar
Nathan Coppin committed
593
  }
Nathan Coppin's avatar
Nathan Coppin committed
594
#else
Nathan Coppin's avatar
Nathan Coppin committed
595
596
597
598
  double norme = sqrt(vt*vt + vs*vs);
  if(norme>3.5*mu*dp){
    vt *= 3.5*mu*dp/norme;
    vs *= 3.5*mu*dp/norme;
Nathan Coppin's avatar
Nathan Coppin committed
599
600
  }
  ct = vt;
Nathan Coppin's avatar
Nathan Coppin committed
601
  cs = vs;
Nathan Coppin's avatar
Nathan Coppin committed
602
603
  if (dp == 0.){
    cs = 0;
Nathan Coppin's avatar
Nathan Coppin committed
604
    ct = 0;
Nathan Coppin's avatar
Nathan Coppin committed
605
  }
606
607
608
#endif
#if ROTATION_ENABLED
  ct -= c->ct;
Nathan Coppin's avatar
Nathan Coppin committed
609
#if DIMENSION==2
Nathan Coppin's avatar
Nathan Coppin committed
610
  coordAdd(&p->velocity[c->o1 * DIMENSION], -ct*(p->a/(p->a+1))*c->a1, t);
611
  p->omega[c->o1] -= (1/p->particles[c->o1].r)*(1/((p->a+1)))*ct*c->a1;
Nathan Coppin's avatar
Nathan Coppin committed
612
#else
Nathan Coppin's avatar
message    
Nathan Coppin committed
613
  cs -= c->cs;
Nathan Coppin's avatar
Nathan Coppin committed
614
  coordAdd(&p->velocity[c->o1 * DIMENSION], -ct*(p->a/(p->a+1))*c->a1, t);
Nathan Coppin's avatar
Nathan Coppin committed
615
  coordAdd(&p->velocity[c->o1 * DIMENSION], -cs*(p->a/(p->a+1))*c->a1, s);
Nathan Coppin's avatar
Nathan Coppin committed
616
  for (int i = 0; i<3; i++){
617
618
    p->omega[c->o1*3 + i] += -(1/p->particles[c->o1].r)*(1/((p->a+1)))*ct*s[i]*c->a1 
                           + (1/p->particles[c->o1].r)*(1/((p->a+1)))*cs*t[i]*c->a1;
Nathan Coppin's avatar
Nathan Coppin committed
619
  }
Nathan Coppin's avatar
Nathan Coppin committed
620
  c->cs += cs;
Nathan Coppin's avatar
Nathan Coppin committed
621
#endif
622
623
624
625
#else 
  ct -= c->ct;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -ct*c->a1, t);
#if DIMENSION==3
Nathan Coppin's avatar
Nathan Coppin committed
626
627
628
629
630
  cs -= c->cs;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -cs*c->a1, s);
  c->cs += cs;
#endif
#endif
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
631
632
  c->ct += ct;
#endif
633
634
  dp -= c->dv;
  coordAdd(&p->velocity[c->o1*DIMENSION], -dp*c->a1, c->n);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
635
636
  c->dv += dp;
  if(fabs(dp) > tol/dt) return 0;
637
#if FRICTION_ENABLED
Nathan Coppin's avatar
Nathan Coppin committed
638
#if DIMENSION == 3
639
  if(fabs(cs) > tol/dt) return 0;
Nathan Coppin's avatar
Nathan Coppin committed
640
#endif
641
  if(fabs(ct) > tol/dt) return 0;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
642
643
#endif
  return 1;
Matthieu Constant's avatar
Matthieu Constant committed
644
645
}

Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
646
static int contactParticleSegmentSolve(Contact *c, ParticleProblem *p, double dt, double tol) {
647
648
  if(p->ForcedFlag[c->o1])
    return 1;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
649
  double vlocfree[DIMENSION], vn, xn[DIMENSION], vloc[DIMENSION];
Nathan Coppin's avatar
Nathan Coppin committed
650
  double r = p->particles[c->o1].r;
Matthieu Constant's avatar
Matthieu Constant committed
651
  for (int i= 0; i < DIMENSION; ++i)
Nathan Coppin's avatar
Nathan Coppin committed
652
    vloc[i] = p->velocity[c->o1*DIMENSION + i] - p->segments[c->o0].v[i];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
653
  double dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt);
Matthieu Constant's avatar
Matthieu Constant committed
654
  for (int i = 0; i < DIMENSION; ++i)
Nathan Coppin's avatar
Nathan Coppin committed
655
656
    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
657
    dp = 0;
658
#if FRICTION_ENABLED
659
#if DIMENSION == 2
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
660
  double t[2];
661
  build_basis(c->n,t);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
662
  double ct = 0;
Nathan Coppin's avatar
Nathan Coppin committed
663
  double vt = c->ct +
664
665
    (p->velocity[c->o1*2+0]- p->segments[c->o0].v[0])*t[0]+
    (p->velocity[c->o1*2+1]- p->segments[c->o0].v[1])*t[1]+
Nathan Coppin's avatar
Nathan Coppin committed
666
    p->segments[c->o0].vt;
Nathan Coppin's avatar
Nathan Coppin committed
667
#if ROTATION_ENABLED
668
    vt += 
Nathan Coppin's avatar
Nathan Coppin committed
669
670
     p->omega[c->o1]*p->particles[c->o1].r;
#endif
671
672
#else
  double t[3], s[3];
Nathan Coppin's avatar
Nathan Coppin committed
673
  build_basis(c->n,t,s);
674
  double ct, cs = 0;
Nathan Coppin's avatar
Nathan Coppin committed
675
676
  double vt = c->ct ;
  double vs = c->cs ;
Nathan Coppin's avatar
Nathan Coppin committed
677
#if ROTATION_ENABLED
678
  double res1[3];
Nathan Coppin's avatar
Nathan Coppin committed
679
680
  _cross(&p->omega[c->o1*3], c->n, res1);
#endif
681
682
683
684
685
686
687
688
  for(int i=0;i<DIMENSION;i++){
    vt += (p->velocity[c->o1*3+i]- p->segments[c->o0].v[i])*t[i];
    vs += (p->velocity[c->o1*3+i]- p->segments[c->o0].v[i])*s[i];
#if ROTATION_ENABLED
    vt += (res1[i]*p->particles[c->o1].r)*t[i];
    vs += (res1[i]*p->particles[c->o1].r)*s[i];
#endif
  }
Nathan Coppin's avatar
Nathan Coppin committed
689
#endif
Nathan Coppin's avatar
Nathan Coppin committed
690
  const double mu = particleProblemGetMu(p,p->particleMaterial[c->o1],p->segmentMaterial[c->o0]);
Nathan Coppin's avatar
Nathan Coppin committed
691
#if DIMENSION==2
692

Nathan Coppin's avatar
Nathan Coppin committed
693
if (fabs(vt)> ((p->a+1)/p->a)*mu*dp) vt *= ((p->a+1)/p->a)*mu*dp/fabs(vt);
Nathan Coppin's avatar
Nathan Coppin committed
694
  ct = vt;
Nathan Coppin's avatar
Nathan Coppin committed
695
696
  if (dp == 0.){
    ct = 0;
Nathan Coppin's avatar
Nathan Coppin committed
697
  }
Nathan Coppin's avatar
Nathan Coppin committed
698
#else
Nathan Coppin's avatar
Nathan Coppin committed
699
700
701
702
  double norme = sqrt(vt*vt + vs*vs);
  if(norme>3.5*mu*dp){
    vt *= 3.5*mu*dp/norme;
    vs *= 3.5*mu*dp/norme;
Nathan Coppin's avatar
Nathan Coppin committed
703
704
  }
  ct = vt;
Nathan Coppin's avatar
Nathan Coppin committed
705
  cs = vs;
Nathan Coppin's avatar
Nathan Coppin committed
706
707
  if (dp == 0.){
    cs = 0;
Nathan Coppin's avatar
Nathan Coppin committed
708
    ct = 0;
Nathan Coppin's avatar
Nathan Coppin committed
709
  }
Nathan Coppin's avatar
Nathan Coppin committed
710

711
712
713
#endif
#if ROTATION_ENABLED
  ct -= c->ct;
Nathan Coppin's avatar
Nathan Coppin committed
714
#if DIMENSION==2
Nathan Coppin's avatar
Nathan Coppin committed
715
  coordAdd(&p->velocity[c->o1 * DIMENSION], -ct*(p->a/(p->a+1))*c->a1, t);
716
  p->omega[c->o1] -= (1/p->particles[c->o1].r)*(1/((p->a+1)))*ct*c->a1;
Nathan Coppin's avatar
Nathan Coppin committed
717
  
Nathan Coppin's avatar
Nathan Coppin committed
718
#else
Nathan Coppin's avatar
message    
Nathan Coppin committed
719
  cs -= c->cs;
Nathan Coppin's avatar
Nathan Coppin committed
720
  coordAdd(&p->velocity[c->o1 * DIMENSION], -ct*(p->a/(p->a+1))*c->a1, t);
Nathan Coppin's avatar
Nathan Coppin committed
721
  coordAdd(&p->velocity[c->o1 * DIMENSION], -cs*(p->a/(p->a+1))*c->a1, s);
Nathan Coppin's avatar
Nathan Coppin committed
722
  for (int i = 0; i<3; i++){
723
724
    p->omega[c->o1*3 + i] += -(1/p->particles[c->o1].r)*(1/((p->a+1)))*ct*s[i]*c->a1 
                           + (1/p->particles[c->o1].r)*(1/((p->a+1)))*cs*t[i]*c->a1;
Nathan Coppin's avatar
Nathan Coppin committed
725
  }
Nathan Coppin's avatar
Nathan Coppin committed
726
  c->cs += cs;
Nathan Coppin's avatar
Nathan Coppin committed
727
#endif
728
729
730
731
#else 
  ct -= c->ct;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -ct*c->a1, t);
#if DIMENSION==3
Nathan Coppin's avatar
Nathan Coppin committed
732
733
734
735
736
  cs -= c->cs;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -cs*c->a1, s);
  c->cs += cs;
#endif
#endif
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
737
738
  c->ct += ct;
#endif
739
740
  dp -= c->dv;
  coordAdd(&p->velocity[c->o1*DIMENSION], -dp*c->a1, c->n);
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
741
742
  c->dv += dp;
  if(fabs(dp) > tol/dt) return 0;
743
#if FRICTION_ENABLED
Nathan Coppin's avatar
Nathan Coppin committed
744
#if DIMENSION == 3
745
  if(fabs(cs) > tol/dt) return 0;
Nathan Coppin's avatar
Nathan Coppin committed
746
#endif
747
  if(fabs(ct) > tol/dt) return 0;
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
748
749
#endif
  return 1;
Matthieu Constant's avatar
Matthieu Constant committed
750
751
752
}

#if DIMENSION == 3
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
753
static int contactParticleTriangleSolve(Contact *c, ParticleProblem *p,double dt, double tol) {
754
755
  if(p->ForcedFlag[c->o1])
    return 1;
Matthieu Constant's avatar
Matthieu Constant committed
756
  double vlocfree[DIMENSION], vn, xn[DIMENSION], vloc[DIMENSION];
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
757
  double r = p->particles[c->o1].r;
Matthieu Constant's avatar
Matthieu Constant committed
758
  for (int i= 0; i < DIMENSION; ++i)
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
759
    vloc[i] = p->velocity[c->o1*DIMENSION+i] - p->triangles[c->o0].v[i];
Nathan Coppin's avatar
Nathan Coppin committed
760
  double dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt);
Matthieu Constant's avatar
Matthieu Constant committed
761
  for (int i = 0; i < DIMENSION; ++i)
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
762
763
    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
764
    dp = 0;
Nathan Coppin's avatar
Nathan Coppin committed
765
766
#if FRICTION_ENABLED
  double t[3], s[3];
Nathan Coppin's avatar
Nathan Coppin committed
767
  build_basis(c->n,t,s);
768
769
770
  double ct, cs = 0;
  double vt = c->ct + p->triangles[c->o0].vt;
  double vs = c->cs + p->triangles[c->o0].vs;
771
  
772
 #if ROTATION_ENABLED
773

774
  double res1[3];
Nathan Coppin's avatar
Nathan Coppin committed
775
776
  _cross(&p->omega[c->o1*3], c->n, res1);
#endif
777
  for(int i = 0;i<DIMENSION;i++){
778
779
    vt += (p->velocity[c->o1*3+i])*t[i];
    vs += (p->velocity[c->o1*3+i])*s[i];
780
781
782
783
784
#if ROTATION_ENABLED
    vt += (res1[i]*p->particles[c->o1].r)*t[i];
    vs += (res1[i]*p->particles[c->o1].r)*s[i];
#endif
  }
Nathan Coppin's avatar
Nathan Coppin committed
785
  const double mu = particleProblemGetMu(p,p->particleMaterial[c->o1],p->triangleMaterial[c->o0]);
Nathan Coppin's avatar
Nathan Coppin committed
786
787
788
789
  double norme = sqrt(vt*vt + vs*vs);
  if(norme>3.5*mu*dp){
    vt *= 3.5*mu*dp/norme;
    vs *= 3.5*mu*dp/norme;
Nathan Coppin's avatar
Nathan Coppin committed
790
  }
791

Nathan Coppin's avatar
Nathan Coppin committed
792
793
  ct = vt;
  cs = vs;
Nathan Coppin's avatar
Nathan Coppin committed
794
795
796
797
798
799
  if (dp == 0.){
    cs = 0;
    ct = 0;
  }
  cs -= c->cs;
  ct -= c->ct;
Nathan Coppin's avatar
Nathan Coppin committed
800
  #if ROTATION_ENABLED
Nathan Coppin's avatar
Nathan Coppin committed
801
  coordAdd(&p->velocity[c->o1 * DIMENSION], -cs*(p->a/(p->a+1))*c->a1, s);
Nathan Coppin's avatar
Nathan Coppin committed
802
  coordAdd(&p->velocity[c->o1 * DIMENSION], -ct*(p->a/(p->a+1))*c->a1, t);
Nathan Coppin's avatar
Nathan Coppin committed
803
  for (int i = 0; i<3; i++){
804
805
    p->omega[c->o1*3 + i] += -(1/p->particles[c->o1].r)*(1/((p->a+1)))*ct*s[i]*c->a1
                           + (1/p->particles[c->o1].r)*(1/((p->a+1)))*cs*t[i]*c->a1;
Nathan Coppin's avatar
Nathan Coppin committed
806
  }
Nathan Coppin's avatar
Nathan Coppin committed
807
808
809
  #else
  coordAdd(&p->velocity[c->o1 * DIMENSION], -cs*c->a1, s);
  coordAdd(&p->velocity[c->o1 * DIMENSION], -ct*c->a1, t);
Nathan Coppin's avatar
Nathan Coppin committed
810
  #endif
Nathan Coppin's avatar
Nathan Coppin committed
811
812
813
  c->ct += ct;
  c->cs += cs;
#endif
Jonathan Lambrechts's avatar
Jonathan Lambrechts committed
814
815
816
  dp -= c->dv;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -dp, c->n);
  c->dv += dp;
Nathan Coppin's avatar
Nathan Coppin committed
817
818
if(fabs(dp) > tol/dt) return 0;
#if FRICTION_ENABLED
819
820
  if(fabs(ct) > tol/dt) return 0;
  if(fabs(cs) > tol/dt) return 0;
Nathan Coppin's avatar
Nathan Coppin committed
821
822
#endif
  return 1;
Matthieu Constant's avatar
Matthieu Constant committed
823
824
825
}
#endif

Nathan Coppin's avatar
Nathan Coppin committed
826

Nathan Coppin's avatar
Nathan Coppin committed
827

Matthieu Constant's avatar
Matthieu Constant committed
828
Contact *findContactSorted(Contact *c, Contact *v, size_t *i) {
Matthieu Constant's avatar
Matthieu Constant committed
829
  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
830
    (*i)++;
Matthieu Constant's avatar
Matthieu Constant committed
831
  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
832
833
834
835
836
837
838
}

static void addAlert(double alert, double *rmin, double *rmax) {
  rmin[0] -= alert;
  rmin[1] -= alert;
  rmax[0] += alert;
  rmax[1] += alert;
839
#if DIMENSION == 3
Matthieu Constant's avatar
Matthieu Constant committed
840
841
  rmin[2] -= alert;
  rmax[2] += alert;
842
#endif
Matthieu Constant's avatar
Matthieu Constant committed
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
}

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);
  }
873
#if DIMENSION == 3
Matthieu Constant's avatar
Matthieu Constant committed
874
875
876
877
  for (size_t i = 0; i < vectorSize(p->triangles); ++i) {
    triangleBoundingBox(&p->triangles[i], amin, amax);
    bbadd(bbmin, bbmax, amin, amax);
  }
878
#endif
Matthieu Constant's avatar
Matthieu Constant committed
879
880
881
882
}

static void _particleProblemGenContacts(ParticleProblem *p, const double alert)
{
883
  clock_t tic = clock();
Matthieu Constant's avatar
Matthieu Constant committed
884
885
886
887
888
889
890
891
892
893
894
  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);
895
  int ntot = 0;
Matthieu Constant's avatar
Matthieu Constant committed
896
897
898
899
900
  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
901
    for (size_t j = 0; j < vectorSize(found); ++j) {
902
      ntot++;
Matthieu Constant's avatar
Matthieu Constant committed
903
      Contact *c= vectorPush(&p->contacts);
904
      if(!particleInitContact(p, i, &p->particles[i], &p->position[i * DIMENSION], found[j], &p->particles[found[j]], &p->position[found[j] * DIMENSION], alert, c)||(p->ForcedFlag[c->o0]&&p->ForcedFlag[c->o1]))
Matthieu Constant's avatar
Matthieu Constant committed
905
906
        vectorPop(p->contacts);
      else if ((cold = findContactSorted(c