scontact.c

/*
 * MigFlow - Copyright (C) <2010-2018>
 * <Universite catholique de Louvain (UCL), Belgium
 *  Universite de Montpellier, France>
 * 	
 * List of the contributors to the development of MigFlow: see AUTHORS file.
 * Description and complete License: see LICENSE file.
 * 	
 * This program (MigFlow) is free software: 
 * you can redistribute it and/or modify it under the terms of the GNU Lesser General 
 * 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
 * GNU Lesser General Public License for more details.
 * 
 * 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, 
 * see <http://www.gnu.org/licenses/>.
 */

#include "scontact.h" 
#include "quadtree.h"
#include "vector.h"
#include <math.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <float.h>
#include <string.h>
#include <time.h>

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;
  Contact *savedContacts;
  double *position, *velocity, *contactForces;
  Disk *disks;
  char **tagname, **materialName;
  int *diskTag, *segmentTag;  
  int *diskMaterial, *segmentMaterial;
  int *particleMaterial;
  int *ForcedFlag;
  Segment *segments;
  double *momentum;
  int get_momentum;
  double a;
#if FRICTION_ENABLED
#if ROTATION_ENABLED
  double *omega;
#endif
  double *mu;
#endif
#if DIMENSION == 3
  Triangle *triangles;
  int *triangleTag;
  int *triangleMaterial;
#endif
  int use_queue;
};

#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

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

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

struct _Contact {
  size_t o0, o1;
  double a0, a1;
  double D;
  double dv;
#if FRICTION_ENABLED
#if ROTATION_ENABLED
  double In0, In1;
#endif
  double ct;
#if DIMENSION == 3
  double cs;
#endif
#endif
  ContactType type;
  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(ParticleProblem *p, size_t id0, Particle *p0, double *x0, size_t id1, Particle *p1, double *x1, double alert, Contact *c) {
  double nnorm = 0;
  c->dv = 0;
#if FRICTION_ENABLED
  c->ct = 0;
#if DIMENSION == 3
  c->cs = 0;
#endif
#endif
  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);
  for (int i = 0; i < DIMENSION; ++i)
    c->n[i] /= -nnorm;
  c->D = fmax(0., nnorm - (p0->r + p1->r));
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);
  }
  c->type = PARTICLE_PARTICLE;
  return c->D < alert;
}

struct _Disk {
  double x[DIMENSION];
  double v[DIMENSION];
  double r;
#if FRICTION_ENABLED
  double vt;
#if DIMENSION == 3
  double vs;
#endif
#endif
};

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


size_t particleProblemAddBoundaryDiskTagId(ParticleProblem *p, const double x[DIMENSION], double r, int tag, int materialTag) {
  Disk *d = vectorPush(&p->disks);
  *vectorPush(&p->diskTag) = tag;
  *vectorPush(&p->diskMaterial) = materialTag;
  d->r = r;
#if FRICTION_ENABLED
  d->vt = 0;
#if DIMENSION == 3
  d->vs = 0;
#endif
#endif
  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, const char *materialTag) {
  return particleProblemAddBoundaryDiskTagId(p,x,r,particleProblemGetTagId(p,tag),particleProblemGetMaterialTagId(p,materialTag));
}

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;
  #if FRICTION_ENABLED
  c->ct = 0;
#if DIMENSION == 3
  c->cs = 0;
#endif
#endif
  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.;
  c->type = PARTICLE_DISK;
  return c->D < alert;
}

struct _Segment{
  double p[2][DIMENSION];
  double v[DIMENSION];
#if FRICTION_ENABLED
  double vt;
#if DIMENSION == 3
  double vs;
#endif
#endif
};

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
}

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;
#if FRICTION_ENABLED
  c->ct = 0;
#if DIMENSION == 3
  c->cs = 0;
#endif
#endif
  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.;
  c->type = PARTICLE_SEGMENT;
  return c->D >= 0  && c->D < alert;
}


#if DIMENSION == 3
struct _Triangle {
  double p[3][DIMENSION];
  double v[DIMENSION];
#if FRICTION_ENABLED
  double vt, vs;
#endif
};

void particleProblemAddBoundaryTriangleTagId(ParticleProblem *p, const double x0[3], const double x1[3], const double x2[3], int tag, int materialTag) {
  Triangle *t = vectorPush(&p->triangles);
  *vectorPush(&p->triangleTag) = tag;
  *vectorPush(&p->triangleMaterial) = materialTag;
  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.;
  }
#if FRICTION_ENABLED
  t->vt = 0;
  t->vs = 0;
#endif
}

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

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] = 
    {{xp[0] - t->p[0][0], xp[1] - t->p[0][1], xp[2] - t->p[0][2]},
    {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;
#if FRICTION_ENABLED
  c->ct = 0;
  c->cs = 0;
#endif
  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;
  c->type = PARTICLE_TRIANGLE;
  return (c->D > 0 || triangleProjectionIsInside(t, x)) && c->D < alert;
}

#endif

static void build_basis(const double *a, double *b
#if DIMENSION==3
, double *c){
  b[0] = -a[2];
  b[1] = a[0];
  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;
  _cross(a, b, c);

#else
){
  b[0] = -a[1];
  b[1] = a[0];
#endif
}

#if FRICTION_ENABLED
static double particleProblemGetMu(const ParticleProblem *p, int mat0, int mat1) {
  return p->mu[mat0*particleProblemNMaterial(p)+mat1];
}
#endif

static int contactParticleParticleSolve(Contact *c, ParticleProblem *p, double dt, double tol) {
  if(p->ForcedFlag[c->o0] == 1 && p->ForcedFlag[c->o1] == 1){
    return 1;
  }
  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);
#if FRICTION_ENABLED
#if DIMENSION == 2
  double t[2];
  build_basis(c->n,t);
  double ct = 0;
  double vt = c->ct +
    (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];
#if ROTATION_ENABLED
    vt += 
     p->omega[c->o0]*p->particles[c->o0].r+
     p->omega[c->o1]*p->particles[c->o1].r;
#endif

#else
  double t[3], s[3];
  build_basis(c->n,t,s);
  double ct, cs = 0;
  double vt = c->ct;
  double vs = c->cs;
#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
  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
  }
#endif

  const double mu = particleProblemGetMu(p,p->particleMaterial[c->o0],p->particleMaterial[c->o1]);
#if DIMENSION==2
  if(isnan(dp)){printf("hello\n");}
  if (fabs(vt)> ((p->a+1)/p->a)*mu*dp) vt *= ((p->a+1)/p->a)*mu*dp/fabs(vt);
  ct = vt;
  if (dp == 0.){
    ct = 0;
  }

#else
  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;
  }
  ct = vt;
  cs = vs;
  if (dp == 0.){
    cs = 0;
    ct = 0;
  }

#endif
#if ROTATION_ENABLED
  ct -= c->ct;
#if DIMENSION==2
  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);
  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;

#else
  cs -= c->cs;
  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);
  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);
  for (int i = 0; i<3; i++){   
    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;
  }
  c->cs += cs;
#endif
#else
  
  ct -= c->ct;
  coordAdd(&p->velocity[c->o0 * DIMENSION], -ct*c->a0, t);
  coordAdd(&p->velocity[c->o1 * DIMENSION], ct*c->a1, t);
#if DIMENSION==3
  
  
  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;
#endif
#endif
  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;
#if FRICTION_ENABLED
#if DIMENSION == 3
  if(fabs(cs) > tol/dt) return 0;
#endif
  if(fabs(ct) > tol/dt) return 0;
#endif
  return 1;
}

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

static int contactParticleDiskSolve(Contact *c, ParticleProblem *p, double dt, double tol) {
  if(p->ForcedFlag[c->o1])
    return 1;
  double vlocfree[DIMENSION], vn;
  double vloc[DIMENSION];
  for (int i= 0; i < DIMENSION; ++i)
    vloc[i] = p->velocity[c->o1*DIMENSION + i] - p->disks[c->o0].v[i];
  double dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt);
#if FRICTION_ENABLED
#if DIMENSION == 2
  double t[2];
  build_basis(c->n,t);
  double ct = 0;
  double vt = c->ct +
    (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]+
    p->disks[c->o0].vt;
#if ROTATION_ENABLED
    vt += 
     p->omega[c->o1]*p->particles[c->o1].r;
#endif
#else
  double t[3], s[3];
  build_basis(c->n,t,s);
  double ct, cs = 0;
  double vt = c->ct + p->disks[c->o0].vt;
  double vs = c->cs + p->disks[c->o0].vs;
#if ROTATION_ENABLED
  double res1[3];
  _cross(&p->omega[c->o1*3], c->n, res1);
#endif
  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
  }
#endif
  const double mu = particleProblemGetMu(p,p->particleMaterial[c->o1],p->diskMaterial[c->o0]);
#if DIMENSION==2
  
  if (fabs(vt)> ((p->a+1)/p->a)*mu*dp) vt *= ((p->a+1)/p->a)*mu*dp/fabs(vt);
  ct = vt;
  if (dp == 0.){
    ct = 0;
  }
#else
  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;
  }
  ct = vt;
  cs = vs;
  if (dp == 0.){
    cs = 0;
    ct = 0;
  }
#endif
#if ROTATION_ENABLED
  ct -= c->ct;
#if DIMENSION==2
  coordAdd(&p->velocity[c->o1 * DIMENSION], -ct*(p->a/(p->a+1))*c->a1, t);
  p->omega[c->o1] -= (1/p->particles[c->o1].r)*(1/((p->a+1)))*ct*c->a1;
#else
  cs -= c->cs;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -ct*(p->a/(p->a+1))*c->a1, t);
  coordAdd(&p->velocity[c->o1 * DIMENSION], -cs*(p->a/(p->a+1))*c->a1, s);
  for (int i = 0; i<3; i++){
    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;
  }
  c->cs += cs;
#endif
#else 
  ct -= c->ct;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -ct*c->a1, t);
#if DIMENSION==3
  cs -= c->cs;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -cs*c->a1, s);
  c->cs += cs;
#endif
#endif
  c->ct += ct;
#endif
  dp -= c->dv;
  coordAdd(&p->velocity[c->o1*DIMENSION], -dp*c->a1, c->n);
  c->dv += dp;
  if(fabs(dp) > tol/dt) return 0;
#if FRICTION_ENABLED
#if DIMENSION == 3
  if(fabs(cs) > tol/dt) return 0;
#endif
  if(fabs(ct) > tol/dt) return 0;
#endif
  return 1;
}

static int contactParticleSegmentSolve(Contact *c, ParticleProblem *p, double dt, double tol) {
  if(p->ForcedFlag[c->o1])
    return 1;
  double vlocfree[DIMENSION], vn, xn[DIMENSION], vloc[DIMENSION];
  double r = p->particles[c->o1].r;
  for (int i= 0; i < DIMENSION; ++i)
    vloc[i] = p->velocity[c->o1*DIMENSION + i] - p->segments[c->o0].v[i];
  double dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt);
  for (int i = 0; i < DIMENSION; ++i)
    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;
#if FRICTION_ENABLED
#if DIMENSION == 2
  double t[2];
  build_basis(c->n,t);
  double ct = 0;
  double vt = c->ct +
    (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]+
    p->segments[c->o0].vt;
#if ROTATION_ENABLED
    vt += 
     p->omega[c->o1]*p->particles[c->o1].r;
#endif
#else
  double t[3], s[3];
  build_basis(c->n,t,s);
  double ct, cs = 0;
  double vt = c->ct ;
  double vs = c->cs ;
#if ROTATION_ENABLED
  double res1[3];
  _cross(&p->omega[c->o1*3], c->n, res1);
#endif
  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
  }
#endif
  const double mu = particleProblemGetMu(p,p->particleMaterial[c->o1],p->segmentMaterial[c->o0]);
#if DIMENSION==2

if (fabs(vt)> ((p->a+1)/p->a)*mu*dp) vt *= ((p->a+1)/p->a)*mu*dp/fabs(vt);
  ct = vt;
  if (dp == 0.){
    ct = 0;
  }
#else
  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;
  }
  ct = vt;
  cs = vs;
  if (dp == 0.){
    cs = 0;
    ct = 0;
  }

#endif
#if ROTATION_ENABLED
  ct -= c->ct;
#if DIMENSION==2
  coordAdd(&p->velocity[c->o1 * DIMENSION], -ct*(p->a/(p->a+1))*c->a1, t);
  p->omega[c->o1] -= (1/p->particles[c->o1].r)*(1/((p->a+1)))*ct*c->a1;
  
#else
  cs -= c->cs;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -ct*(p->a/(p->a+1))*c->a1, t);
  coordAdd(&p->velocity[c->o1 * DIMENSION], -cs*(p->a/(p->a+1))*c->a1, s);
  for (int i = 0; i<3; i++){
    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;
  }
  c->cs += cs;
#endif
#else 
  ct -= c->ct;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -ct*c->a1, t);
#if DIMENSION==3
  cs -= c->cs;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -cs*c->a1, s);
  c->cs += cs;
#endif
#endif
  c->ct += ct;
#endif
  dp -= c->dv;
  coordAdd(&p->velocity[c->o1*DIMENSION], -dp*c->a1, c->n);
  c->dv += dp;
  if(fabs(dp) > tol/dt) return 0;
#if FRICTION_ENABLED
#if DIMENSION == 3
  if(fabs(cs) > tol/dt) return 0;
#endif
  if(fabs(ct) > tol/dt) return 0;
#endif
  return 1;
}

#if DIMENSION == 3
static int contactParticleTriangleSolve(Contact *c, ParticleProblem *p,double dt, double tol) {
  if(p->ForcedFlag[c->o1])
    return 1;
  double vlocfree[DIMENSION], vn, xn[DIMENSION], vloc[DIMENSION];
  double r = p->particles[c->o1].r;
  for (int i= 0; i < DIMENSION; ++i)
    vloc[i] = p->velocity[c->o1*DIMENSION+i] - p->triangles[c->o0].v[i];
  double dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt);
  for (int i = 0; i < DIMENSION; ++i)
    xn[i] = p->position[c->o1*DIMENSION+i] + r*c->n[i] + vlocfree[i]*c->D/vn;
  if (!triangleProjectionIsInside(&p->triangles[c->o0], xn))
    dp = 0;
#if FRICTION_ENABLED
  double t[3], s[3];
  build_basis(c->n,t,s);
  double ct, cs = 0;
  double vt = c->ct + p->triangles[c->o0].vt;
  double vs = c->cs + p->triangles[c->o0].vs;
  
 #if ROTATION_ENABLED

  double res1[3];
  _cross(&p->omega[c->o1*3], c->n, res1);
#endif
  for(int i = 0;i<DIMENSION;i++){
    vt += (p->velocity[c->o1*3+i])*t[i];
    vs += (p->velocity[c->o1*3+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
  }
  const double mu = particleProblemGetMu(p,p->particleMaterial[c->o1],p->triangleMaterial[c->o0]);
  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;
  }

  ct = vt;
  cs = vs;
  if (dp == 0.){
    cs = 0;
    ct = 0;
  }
  cs -= c->cs;
  ct -= c->ct;
  #if ROTATION_ENABLED
  coordAdd(&p->velocity[c->o1 * DIMENSION], -cs*(p->a/(p->a+1))*c->a1, s);
  coordAdd(&p->velocity[c->o1 * DIMENSION], -ct*(p->a/(p->a+1))*c->a1, t);
  for (int i = 0; i<3; i++){
    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;
  }
  #else
  coordAdd(&p->velocity[c->o1 * DIMENSION], -cs*c->a1, s);
  coordAdd(&p->velocity[c->o1 * DIMENSION], -ct*c->a1, t);
  #endif
  c->ct += ct;
  c->cs += cs;
#endif
  dp -= c->dv;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -dp, c->n);
  c->dv += dp;
if(fabs(dp) > tol/dt) return 0;
#if FRICTION_ENABLED
  if(fabs(ct) > tol/dt) return 0;
  if(fabs(cs) > tol/dt) return 0;
#endif
  return 1;
}
#endif



Contact *findContactSorted(Contact *c, Contact *v, size_t *i) {
  while (*i < vectorSize(v) && (v[*i].type < c->type || v[*i].o0 < c->o0 || (v[*i].o0 == c->o0 && v[*i].o1 < c->o1)))
    (*i)++;
  return (*i < vectorSize(v) && v[*i].type == c->type && v[*i].o0 == c->o0 && v[*i].o1 == c->o1) ? &v[*i] : NULL;
}

static void addAlert(double alert, double *rmin, double *rmax) {
  rmin[0] -= alert;
  rmin[1] -= alert;
  rmax[0] += alert;
  rmax[1] += alert;
#if DIMENSION == 3
  rmin[2] -= alert;
  rmax[2] += alert;
#endif
}

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);
  }
#if DIMENSION == 3
  for (size_t i = 0; i < vectorSize(p->triangles); ++i) {
    triangleBoundingBox(&p->triangles[i], amin, amax);
    bbadd(bbmin, bbmax, amin, amax);
  }
#endif
}

static void _particleProblemGenContacts(ParticleProblem *p, const double alert)
{
  clock_t tic = clock();
  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);
  int ntot = 0;
  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);
    for (size_t j = 0; j < vectorSize(found); ++j) {
      ntot++;
      Contact *c= vectorPush(&p->contacts);
      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]))
        vectorPop(p->contacts);
      else if ((cold = findContactSorted(c