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 <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{
  int frictionFlag;
  double friction_relaxation;
  Particle *particles;
  Contact *contacts;
  double *position, *velocity, *externalForces, *omega, *mu;
  Disk *disks;
  char **tagname;
  int *diskTag, *segmentTag, *particleTag;
  Segment *segments;
  #if DIMENSION == 3
  Triangle *triangles;
  int *triangleTag;
  #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 In0, In1;
  double D;
  double dv;
  double ct;
  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(size_t id0, Particle *p0, double *x0, size_t id1, Particle *p1, double *x1, double alert, Contact *c) {
  double nnorm = 0;
  c->dv = 0;
  c->ct = 0;
  c->o0 = id0;
  c->o1 = id1;
  for (int k = 0; k < DIMENSION; ++k) {
    c->n[k] = x0[k] - x1[k];
    nnorm += c->n[k] * c->n[k];
  }
  nnorm = sqrt(nnorm);
  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->In0 = c->a0*5/(2*p0->r);
  c->In1 = c->a1*5/(2*p1->r);
  c->type = PARTICLE_PARTICLE;
  return c->D < alert;
}

struct _Disk {
  double x[DIMENSION];
  double v[DIMENSION];
  double r;
  double vt;
};

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, size_t tag) {
  Disk *d = vectorPush(&p->disks);
  *vectorPush(&p->diskTag) = tag;
  d->r = r;
  d->vt = 0;
  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;
  c->ct = 0;
  c->o1 = particle;
  c->o0 = id;
  for (int i = 0; i < DIMENSION; ++i) {
    c->n[i] = d->x[i] - x[i];
    nnorm += c->n[i] * c->n[i];
  }
  nnorm = sqrt(nnorm);
  c->D = (nnorm - fabs(p->r + d->r)) * (d->r < 0 ? -1 : 1);
  if (c->D < 0) c->D = 0;
  for (int i = 0; i < DIMENSION; ++i) {
    c->n[i] /= nnorm * (d->r < 0 ? -1 : 1);
  }
  c->a0 = 0.;
  c->a1 = 1.;
  c->In0 = 0;
  c->In1 = c->a1*5/(2*p->r);
  c->type = PARTICLE_DISK;
  return c->D < alert;
}

struct _Segment{
  double p[2][DIMENSION];
  double v[DIMENSION];
  double vt;
};

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;
  c->ct = 0;
  double t[DIMENSION];
  double nt2 = 0;
  double dt = 0;
  for (int i = 0; i <DIMENSION; ++i){
    t[i] = s->p[1][i] - s->p[0][i];
    dt += t[i] * (s->p[0][i] - x[i]);
    nt2 += t[i] * t[i];
  }
  double nn2 = 0;
  for (int i = 0; i < DIMENSION; ++i) {
    c->n[i] = s->p[0][i] - x[i] - t[i] / nt2 *  dt;
    nn2 += c->n[i] * c->n[i];
  }
  const double nnorm = sqrt(nn2);
  for (int i = 0; i < DIMENSION; ++i) {
    c->n[i] /= nnorm;
  }
  //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]);
  c->D = nnorm - p->r;
  if (c->D < 0 && segmentProjectionIsInside(s, x)) c->D = 0;
  c->a0 = 0.;
  c->a1 = 1.;
  c->In0 = 0;
  c->In1 = c->a1*5/(2*p->r);
  c->type = PARTICLE_SEGMENT;
  return c->D >= 0  && c->D < alert;
}

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

void particleProblemAddBoundaryTriangleTagId(ParticleProblem *p, const double x0[3], const double x1[3], const double x2[3], size_t tag) {
  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] = 
    {{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;
  c->ct = 0;
  double d0[3] = {t->p[1][0] - t->p[0][0], t->p[1][1] - t->p[0][1], t->p[1][2] - t->p[0][2]};
  double d1[3] = {t->p[2][0] - t->p[0][0], t->p[2][1] - t->p[0][1], t->p[2][2] - t->p[0][2]};
  double N[3];
  _cross(d0, d1, N);
  const double nn = sqrt(dot(N, N));
  for (int i = 0; i < 3; ++i)
    c->n[i] = N[i] / nn;
  double dd[3] = {t->p[0][0] - x[0], t->p[0][1] - x[1], t->p[0][2] - x[2]};
  c->D = dot(c->n, dd);
  if (c->D < 0) {
    for (int i = 0; i <3; ++i)
      c->n[i] = -c->n[i];
    c->D = -c->D;
  }
  c->D -= p->r;
  c->a0 = 0.;
  c->a1 = 1.;
  c->In0 = 0;
  c->In1 = c->a1*5/(2*p->r);
  if (c->D < 0)
    c->D = 0;
  c->type = PARTICLE_TRIANGLE;
  return (c->D > 0 || triangleProjectionIsInside(t, x)) && c->D < alert;
}

#endif

static int contactParticleParticleSolve(Contact *c, ParticleProblem *p, double dt, double *dp, double *ct, double tol) {

  #if DIMENSION == 3

  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;
  *dp = fmax(0., vn - c->D/dt) - c->dv;
  *ct = 0 ;
  coordAdd(&p->velocity[c->o0*3], -*dp*c->a0, c->n);
  coordAdd(&p->velocity[c->o1*3], *dp*c->a1, c->n);

  #else 

  double t[2];
  t[0] = c->n[1];
  t[1] = -c->n[0];
  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];
  double vt = c->ct +
    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]+
    (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;
  //printf("vt avant : %.8f c->ct : %.8f \n",vt,c->ct);
  *dp = fmax(0., vn - c->D/dt);
  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");
    }
    else if(vt<(-7./2.)*mu*(*dp)){
      *ct = -p->friction_relaxation*mu*(*dp);//printf("Slip\n");
    }
    else{
      *ct = p->friction_relaxation*(2./7.)*vt;//printf("Stick\n");
    }
    *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;
  }
  *dp -= c->dv;
  //printf("ct : %.8f dp : %.8f\n",*ct,*dp);
  coordAdd(&p->velocity[c->o0 * DIMENSION], -*dp*c->a0, c->n);
  coordAdd(&p->velocity[c->o1 * DIMENSION], *dp*c->a1, c->n);
  #endif  
  if(fabs(*dp) < tol/dt && fabs(*ct) <tol/dt){
    //printf("b : %d b : %d ok\n",c->o0,c->o1);
    return 1;
  }
  else{
    //printf("b : %d b : %d not ok\n",c->o0,c->o1);
    return 0;
  }
}

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 *ct, double *dp, double dt, double tol) {
  double vlocfree[DIMENSION], t[2], vn;
  double vloc[DIMENSION];
  t[0] = c->n[1];
  t[1] = -c->n[0];
  for (int i= 0; i < DIMENSION; ++i)
    vloc[i] = p->velocity[c->o1*DIMENSION + i] - p->disks[c->o0].v[i];
  *dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt);
  
  #if DIMENSION == 2
  
  double vt = c->ct +
  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;
  //printf("vt avant : %.8f\n",vt);
  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");
    }
    else if(vt<(-7./2.)*mu*(*dp)){
      *ct = -p->friction_relaxation*mu*(*dp);//printf("Slip\n");
    }
    else{
      *ct = p->friction_relaxation*(2./7.)*vt;//printf("Stick\n");
    }
    *ct -= c->ct;
    coordAdd(&p->velocity[c->o1 * DIMENSION], -*ct*c->a1, t);
    p->omega[c->o1] -= c->In1*(*ct);
  }
  else{
    *ct  = 0;
  }
  *dp -=c->dv;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -*dp, c->n);
//printf("dis : %d ct : %f dp : %f\n",c->o0,*ct,*dp);
 

  #else

  *dp -=c->dv;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -*dp, c->n);
  *ct = 0;

  #endif
  if(fabs(*dp) < tol/dt &&fabs(*ct) <tol/dt){
    //printf("dis : %d b : %d ok\n",c->o0,c->o1);
    return 1;
  }
  else{
    //printf("dis : %d b : %d not ok\n",c->o0,c->o1);
    return 0;
  }
}

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];
  for (int i= 0; i < DIMENSION; ++i)
    vloc[i] = p->velocity[c->o1*DIMENSION + i] - p->segments[c->o0].v[i];
  *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 DIMENSION == 2
  
  double vt = c->ct +
  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;
  //printf("ball : %d vt avant : %.8f c->ct : %.8f \n",c->o1,vt,c->ct);
  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");
    }
    else if(vt<(-7./2.)*mu*(*dp)){
      *ct = -p->friction_relaxation*mu*(*dp);//printf("Slip\n");
    }
    else{
      *ct = p->friction_relaxation*(2./7.)*vt;//printf("Stick\n");
    }
    *ct -= c->ct;
    coordAdd(&p->velocity[c->o1 * DIMENSION], -*ct*c->a1, t);
    p->omega[c->o1] -= c->In1*(*ct);
  }
  else{
    *ct = 0;
  }
  *dp -=c->dv;
  //printf("seg : %d ct : %f dp : %f\n",c->o0,*ct,*dp);
  coordAdd(&p->velocity[c->o1 * DIMENSION], -*dp, c->n);

  #else
  
  *ct = 0;
  *dp -=c->dv;
  coordAdd(&p->velocity[c->o1 * DIMENSION], -*dp, c->n);
  
  #endif
  if(fabs(*dp) < tol/dt &&fabs(*ct) <tol/dt){
    //printf("seg : %d b : %d ok\n",c->o0,c->o1);
    return 1;
  }
  else{
    //printf("seg : %d b : %d not ok\n",c->o0,c->o1);
    return 0;
  }
}

#if DIMENSION == 3
static int contactParticleTriangleSolve(Contact *c, const double *v, const double *x, double r, Triangle *t, double dt, double tol, double *dp, double *ct) {
  double vlocfree[DIMENSION], vn, xn[DIMENSION], vloc[DIMENSION];
  for (int i= 0; i < DIMENSION; ++i)
    vloc[i] = v[i] - t->v[i];
  *dp = contactParticleBndSolve(c, vloc, vlocfree, &vn, dt) - c->dv;
  *ct = 0;
  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;
  if(fabs(*dp) < tol/dt){
    return 1;
  }
  else{
    return 0;
  }
}
#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 _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])
  - 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]));
}

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];
  }
  for (int i = 0; i < N * nElements; ++i) {
    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];
  for (int i = 0; i < nElements; ++i) {
    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) {
      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);
      }
    //}
  }
  vectorFree(found);
  cellFree(tree);
}

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;
  #if DIMENSION == 3
  if (amin[2] > bmax[2] || amax[2] < bmin[2]) return 0;
  #endif
  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;
  }

  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))) {
          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;
        }*/
      }
    }
    start = end;
  }
  clock_t tic3 = clock();
  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);
  vectorFree(cc);
}

static void _particleProblemGenContacts(ParticleProblem *p, const double alert)
{
  //_particleProblemGenContacts2(p,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(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))) {
        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);
        c->dv = cold->dv;
        c->ct = cold->ct;
	#if DIMENSION == 2
  	int index = p->particleTag[c->o0]*vectorSize(p->mu) + p->particleTag[c->o1];
  	if(p->frictionFlag && p->mu[index]!=0){
    	  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
      }
    }
  }
  //printf("time old %i\n", clock()-tic);
  //exit(0);
  // 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);
    for (size_t j = 0; j < vectorSize(found); ++j) {
      Contact *c = vectorPush(&p->contacts);
      if(!diskInitContact(i, &p->disks[i], found[j], &p->particles[found[j]], &p->position[found[j] * DIMENSION], alert, c))
        vectorPop(p->contacts);
      else if ((cold = findContactSorted(c, oldContacts, &iold))) {
        double t[2];
        t[0] = c->n[1];
        t[1] = - c->n[0];
        coordAdd(&p->velocity[c->o1 * DIMENSION], -cold->dv, c->n);
        c->dv = cold->dv;
        c->ct = cold->ct;
	#if DIMENSION == 2
  	int index = (p->particleTag[c->o1]+1)*vectorSize(p->mu) -1;
  	if(p->frictionFlag && p->mu[index]!=0){
    	  coordAdd(&p->velocity[c->o1 * DIMENSION], -c->ct*c->a1, t);
   	   p->omega[c->o1] -= c->In1*(c->ct);
  	}
	#endif
      }
    }
  }
  // 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);
    for (size_t j = 0; j < vectorSize(found); ++j) {
      Contact *c = vectorPush(&p->contacts);
      if (!segmentInitContact(i, &p->segments[i], found[j], &p->particles[found[j]], &p->position[found[j] * DIMENSION], alert, c))
        vectorPop(p->contacts);
      else if ((cold = findContactSorted(c, oldContacts, &iold))) {
        double t[2];
        t[0] = c->n[1];
        t[1] = - c->n[0];
        coordAdd(&p->velocity[c->o1 * DIMENSION], -cold->dv, c->n);
        c->dv = cold->dv;
	c->ct = cold->ct;
	#if DIMENSION == 2
  	int index = (p->particleTag[c->o1]+1)*vectorSize(p->mu) -1;
  	if(p->frictionFlag && p->mu[index]!=0){
    	  coordAdd(&p->velocity[c->o1 * DIMENSION], -c->ct*c->a1, t);
   	   p->omega[c->o1] -= c->In1*(c->ct);
  	}
	#endif
      }
    }
  }
  // Triangles 
  #if DIMENSION == 3
  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) {
      Contact *c =  vectorPush(&p->contacts);
      if (!triangleInitContact(i, &p->triangles[i], found[j], &p->particles[found[j]], &p->position[found[j] * DIMENSION], alert, c))
        vectorPop(p->contacts);
      else if ((cold = findContactSorted(c, oldContacts, &iold))) {
        coordAdd(&p->velocity[c->o1 * DIMENSION], -cold->dv, c->n);
        c->dv = cold->dv;
        c->ct = 0;
      }
    }
  }
  #endif
  vectorFree(oldContacts);
  vectorFree(found);
  cellFree(tree);
}

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

static void _particleProblemSolveContacts(ParticleProblem *p, double dt, double tol) {
  int converged = 0;
  while (! converged) {
    converged = 1;
    for (int ic = vectorSize(p->contacts)-1; ic >= 0;  --ic) {
      Contact *c = &p->contacts[ic];