scontact.c

/*
 * MigFlow - Copyright (C) <2010-2020>
 * <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 "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 _ParticleProblem ParticleProblem;
typedef struct _Body Body;
typedef struct _Contact Contact;
typedef struct _Disk Disk;
typedef struct _Segment Segment;
typedef struct _Triangle Triangle;
typedef struct _PeriodicEntity PeriodicEntity;
typedef struct _PeriodicSegment PeriodicSegment;
typedef struct _PeriodicTriangle PeriodicTriangle;

struct _ParticleProblem{
  Particle *particles;
  Body *bodies;
  Contact *contacts;
  Disk *disks;
  Segment *segments;
  PeriodicEntity *periodicEntities;
  PeriodicSegment *periodicSegments;
  double *mu;
  int n_material;
#if DIMENSION == 3
  Triangle *triangles;
  PeriodicTriangle *periodicTriangles;
#endif
  int use_queue;
};

struct _Particle{
  double r;
  size_t body;
  double x[DIMENSION];
  int material;
};

struct _Body{
  double invertM;
  double invertI;
  // 3D : double invertI[6];
  double theta;
#if DIMENSION==3
  double omega[3];
#else
  double omega;
#endif
  double position[DIMENSION];
  double velocity[DIMENSION];
};

typedef struct {
  int material;
  int tag;
}Boundary;

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

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

struct _Triangle {
  Boundary b;
  double p[3][DIMENSION];
  double v[DIMENSION];
};

#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

struct _Contact {
  // constant over one time step
  size_t o0, o1;
  ContactType type;
  double D0; // initial interpenetration (should be ~0)
  // contact state
  double r0[DIMENSION], r1[DIMENSION]; // vector from body center to contact point
  double reaction[DIMENSION];          // force on o1 expressed in global basis
};

struct _PeriodicEntity{
  int etag;
  int edim;
  double transform[DIMENSION];
};

struct _PeriodicSegment{
  size_t entity_id;
  double p[2][DIMENSION];
};
#if DIMENSION == 3
struct _PeriodicTriangle {
  size_t entity_id;
  double p[3][DIMENSION];
};
#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 */
static void particle_bounding_box(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 double particle_problem_get_mu(const ParticleProblem *p, int mat0, int mat1) {
  return p->mu[mat0*p->n_material+mat1];
}
static void _cross (const double *a, const double *b, double *c) {
#if DIMENSION==2
  c[0] = a[0]*b[1] - a[1]*b[0];
#else
  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];
#endif
}

// fill basis[1] and basis[2] from basis[0]
static void basis_from_normal_vector(double basis[DIMENSION][DIMENSION]) {
#if DIMENSION==3
  basis[1][0] = -basis[0][2];
  basis[1][1] = basis[0][0];
  basis[1][2] = -basis[0][1];
  double ddot = dot(basis[0],basis[1]);
  double norm = 0;
  for(int i = 0;i<3;i++){
    basis[1][i] -= ddot*basis[0][i];
    norm += basis[1][i]*basis[1][i];
  }
  norm = norm == 0 ? 1 : sqrt(norm);
  for(int i = 0;i<3;i++){
    basis[1][i] /= norm;
  }
  _cross(basis[0], basis[1], basis[2]);
#else
  basis[1][0] = -basis[0][1];
  basis[1][1] = basis[0][0];
#endif
}

static void particle_get_position(const ParticleProblem *p, const Particle *particle, double x[DIMENSION]) {
  Body *body = &p->bodies[particle->body];
  const double *xp = particle->x;
  double theta = body->theta;
#if DIMENSION == 2
  x[0] = body->position[0] + cos(theta)*xp[0] - sin(theta)*xp[1];
  x[1] = body->position[1] + sin(theta)*xp[0] + cos(theta)*xp[1];
#else
  // TODO
#endif
}

void particle_problem_get_particles_position(ParticleProblem *p, double *x){
  for(int i = 0;i<vectorSize(p->particles);i++){
    particle_get_position(p, &p->particles[i], x+i*DIMENSION);
  }
}

void particle_problem_get_particles_velocity(ParticleProblem *p, double *v){
  for(int i = 0;i<vectorSize(p->particles);i++){
    const Body *body = &p->bodies[p->particles[i].body];
    double r[3], res[3], omega[3];
    for (int d = 0;d<DIMENSION;d++){
#if DIMENSION==3
      omega[d] = p->bodies[p->particles[i].body].omega[DIMENSION+d];
#else
      omega[d] = 0;
#endif
      r[d] = p->particles[i].x[d];
    }
#if DIMENSION==2
    r[2] = 0; // TODO FIXME !!!! cross does not work in 2D
    omega[2] =  p->bodies[p->particles[i].body].omega;
#endif
    _cross(omega,r,res);
    for(int d = 0;d<DIMENSION;d++){
      v[i*DIMENSION + d] = body->velocity[d] + res[d];
    }
  }
}

static double contact_update_particle_particle(const Contact *c, const ParticleProblem *p,
                                               const double *x0, const double *x1,
                                               double basis[DIMENSION][DIMENSION], double r0[DIMENSION], double r1[DIMENSION]) {
  const Particle *p0 = &p->particles[c->o0];
  const Particle *p1 = &p->particles[c->o1];
  double nnorm = 0;
  for (int k = 0; k < DIMENSION; ++k) {
    basis[0][k] = x1[k] - x0[k];
    nnorm += basis[0][k] * basis[0][k];
  }
  nnorm = sqrt(nnorm);
  for (int i = 0; i < DIMENSION; ++i)
    basis[0][i] /= nnorm;
  basis_from_normal_vector(basis);
  const double *xbody0 = p->bodies[p0->body].position;
  const double *xbody1 = p->bodies[p1->body].position;
  for (int i = 0;i<DIMENSION;i++){
    r0[i] = x0[i]-xbody0[i] + basis[0][i]*p0->r;
    r1[i] = x1[i]-xbody1[i] - basis[0][i]*p1->r;
  }
  return nnorm - (p0->r + p1->r);
}


static int contact_init_particle_particle(Contact *c, const ParticleProblem *p, size_t id0, const double *x0, size_t id1, const double *x1, double alert) {
  const Particle *p0 = &p->particles[id0];
  const Particle *p1 = &p->particles[id1];
  const Body *body0 = &p->bodies[p0->body];
  const Body *body1 = &p->bodies[p1->body];
  if(p0->body==p1->body)
    return 0;
  if(body0->invertM == 0 && body1->invertM == 0 && body0->invertI == 0 && body1->invertI == 0)
    return 0;
  c->o0 = id0;
  c->o1 = id1;
  for (int i = 0; i < DIMENSION; ++i) {
    c->reaction[i] = 0;
    c->r0[i] = 0;
    c->r1[i] = 0;
  }
  c->type = PARTICLE_PARTICLE;
  double basis[DIMENSION][DIMENSION];
  double D = contact_update_particle_particle(c, p, x0, x1, basis, c->r0, c->r1);
  c->D0 = fmin(D, 0);
  return D < alert;
}

static void disk_bounding_box(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 particle_problem_add_boundary_disk(ParticleProblem *p, const double x[DIMENSION], double r, int tag, int materialTag) {
  Disk *d = vectorPush(&p->disks);
  d->b.tag = tag;
  d->b.material = materialTag;
  d->r = r;
  for (int i = 0; i < DIMENSION; ++i) {
    d->x[i] = x[i];
    d->v[i] = 0.;
  }
  return vectorSize(p->disks) - 1;
}

static double contact_update_disk_particle(const Contact *c, const ParticleProblem *p, double *x, double basis[DIMENSION][DIMENSION], double r0[DIMENSION], double r1[DIMENSION]) {
  double nnorm = 0;
  const Particle *p1 = &p->particles[c->o1];
  const Disk *d = &p->disks[c->o0];
  for (int i = 0; i < DIMENSION; ++i) {
    basis[0][i] = x[i] - d->x[i];
    nnorm += basis[0][i] * basis[0][i];
  }
  nnorm = sqrt(nnorm);
  for (int i = 0; i < DIMENSION; ++i) {
    basis[0][i] /= (nnorm == 0 ? 1 : nnorm) * (d->r < 0 ? -1 : 1);
  }
  basis_from_normal_vector(basis);
  const double *xbody = p->bodies[p1->body].position;
  for (int i = 0;i<DIMENSION;i++){
    r0[i] = 0; // todo change when disks move
    r1[i] = (x[i]-xbody[i] -basis[0][i]*p1->r);
  }
  return (nnorm - fabs(p1->r + d->r)) * (d->r < 0 ? -1 : 1);
}

static int contact_init_disk_particle(Contact *c, ParticleProblem *p, size_t disk_id, size_t particle_id, double *x, double alert) {
  const Disk *d = &p->disks[disk_id];
  const Particle *particle = &p->particles[particle_id];
  const Body *body = &p->bodies[particle->body];
  if (body->invertI == 0 && body->invertM == 0)
    return 0;
  c->o0 = disk_id;
  c->o1 = particle_id;
  for (int i = 0; i < DIMENSION; ++i) {
    c->reaction[i] = 0;
    c->r0[i] = 0;
    c->r1[i] = 0;
  }
  c->type = PARTICLE_DISK;
  double basis[DIMENSION][DIMENSION];
  double D = contact_update_disk_particle(c, p, x, basis, c->r0, c->r1);
  c->D0 = fmin(D, 0);
  return D < alert;
}

static void periodic_segment_bounding_box(const PeriodicSegment *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 void coordAdd(double *x, double a, const double *y) {
  x[0] += a * y[0];
  x[1] += a * y[1];
#if DIMENSION == 3
  x[2] += a * y[2];
#endif
}

static void segment_bounding_box(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 double contact_update_segment_particle(const Contact *c, const ParticleProblem *p, double *x1, double basis[DIMENSION][DIMENSION], double r0[DIMENSION], double r1[DIMENSION]) {
  const Segment *s = &p->segments[c->o0];
  const Particle *p1 = &p->particles[c->o1];
  double nt2 = 0;
  double dt = 0;
  double t[DIMENSION];
  for (int i = 0; i <DIMENSION; ++i){
    t[i] = s->p[1][i] - s->p[0][i];
    dt += t[i] * (s->p[0][i] - x1[i]);
    nt2 += t[i]*t[i];
  }
  double nn2 = 0;
  double xi = dt/nt2;
  for (int i = 0; i < DIMENSION; ++i) {
    basis[0][i] = x1[i] -(s->p[0][i]-t[i]*xi);
    nn2 += basis[0][i] * basis[0][i];
  }
  const double nnorm = nn2 == 0 ? 1 : sqrt(nn2);
  for (int i = 0; i < DIMENSION; ++i) {
    basis[0][i] /= nnorm;
  }
  basis_from_normal_vector(basis);
  const double *xbody = p->bodies[p1->body].position;
  for (int i = 0;i<DIMENSION;i++){
    r0[i] = 0; // todo change if segment moves
    r1[i] = x1[i] - basis[0][i]*p1->r -xbody[i];
  }
  return nnorm - p1->r;
}

static int contact_init_segment_particle(Contact *c, ParticleProblem *p, size_t segment_id, size_t particle_id, double *x, double alert) {
  const Segment *s = &p->segments[segment_id];
  double xcheck[3];
  particle_get_position(p, &p->particles[particle_id], xcheck);
  const Particle *particle = &p->particles[particle_id];
  const Body *body= &p->bodies[particle->body];
  if (body->invertI == 0 && body->invertM == 0)
    return 0;
  c->o0 = segment_id;
  c->o1 = particle_id;
  for (int i = 0; i < DIMENSION; ++i) {
    c->reaction[i] = 0;
    c->r0[i] = 0;
    c->r1[i] = 0;
  }
  c->type = PARTICLE_SEGMENT;
  double basis[DIMENSION][DIMENSION];
  double D = contact_update_segment_particle(c, p, x, basis, c->r0, c->r1);
  c->D0 = fmin(D, 0);
  return D < alert;
}


#if DIMENSION == 3
void particle_problem_add_boundary_triangle(ParticleProblem *p, const double x0[3], const double x1[3], const double x2[3], int tag, int materialTag) {
  Triangle *t = vectorPush(&p->triangles);
  t->b.tag = tag;
  t->b.material = 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.;
  }
}

size_t particle_problem_add_boundary_periodic_triangle(ParticleProblem  *p, const double x0[DIMENSION], const double x1[DIMENSION], const double x2[DIMENSION], const int etag){
  PeriodicTriangle *t = vectorPush(&p->periodicTriangles);
  t->entity_id = -1;
  for(size_t ie = 0; ie < vectorSize(p->periodicEntities); ++ie){
    PeriodicEntity *pe = &p->periodicEntities[ie];
    if(etag == pe->etag){
      t->entity_id = ie;
      break;
    }
  }
  if (t->entity_id == -1) printf("Entity not found in periodic triangles !\n");
  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];
  }
  return vectorSize(p->periodicTriangles) - 1;
}

static void periodic_triangle_bounding_box(const PeriodicTriangle *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 triangle_bounding_box(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 int contact_init_triangle_particle(Contact *c, const ParticleProblem *p,
    size_t triangle_id, size_t particle_id, double *x, double alert) {
  double basis[DIMENSION][DIMENSION];
  const Particle *particle = &p->particles[particle_id];
  const Body *body = &p->bodies[particle->body];
  if (body->invertI == 0 && body->invertM == 0)
    return 0;
  const Triangle *t = &p->triangles[triangle_id];
  c->o0 = triangle_id;
  c->o1 = particle_id;
  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->reaction[i] = 0;
    basis[0][i] = N[i] / (nn == 0 ? 1 : nn);
    c->r0[i] = 0;
    c->r1[i] = 0;
  }
  double dd[3] = {x[0]+particle->x[0] - t->p[0][0],x[1] + particle->x[1]- t->p[0][1],x[2] + particle->x[2]- t->p[0][2]};
  double D = dot(basis[0], dd);
  if (D < 0) {
    for (int i = 0; i <3; ++i)
      basis[0][i] = -basis[0][i];
    D = -D;
  }
  basis_from_normal_vector(basis);
  D -= particle->r;
  for (int i = 0;i<DIMENSION;i++){
    c->r0[i] = 0;
    c->r1[i] = (particle->x[i] - basis[0][i]*particle->r);
  }
  c->type = PARTICLE_TRIANGLE;
  c->D0 = fmin(D, 0);
  return D < alert;
}
#endif

static void contact_get_omega_pointers(const Contact *c, ParticleProblem *p, double **omega0, double **omega1) {
#if DIMENSION == 2
  *omega1 = &p->bodies[p->particles[c->o1].body].omega;
#else
  *omega1 = p->bodies[p->particles[c->o1].body].omega;
#endif
  if (c->type == PARTICLE_PARTICLE) {
#if DIMENSION == 2
    *omega0 = &p->bodies[p->particles[c->o0].body].omega;
#else
    *omega0 = p->bodies[p->particles[c->o0].body].omega;
#endif
  }
  else {
    *omega0 = NULL;
  }
}


static void contact_get_velocity_pointers(const Contact *c, ParticleProblem *p, double **v0, double **v1) {
  *v1 = p->bodies[p->particles[c->o1].body].velocity;
  if (c->type == PARTICLE_PARTICLE) {
    *v0 = p->bodies[p->particles[c->o0].body].velocity;
  }
  else if (c->type == PARTICLE_DISK) {
    *v0 = p->disks[c->o0].v;
  }
  else if (c->type == PARTICLE_SEGMENT) {
    *v0 = p->segments[c->o0].v;
  }
#if DIMENSION == 3
  else if (c->type == PARTICLE_TRIANGLE) {
    *v0 = p->triangles[c->o0].v;
  }
#endif
}

static void contact_apply(const Contact *c, ParticleProblem *p, double dt) {
  double *v1, *v0;
  contact_get_velocity_pointers(c, p, &v0, &v1);
  double *omega0, *omega1;
  contact_get_omega_pointers(c, p, &omega0, &omega1);
  double imass1 = p->bodies[p->particles[c->o1].body].invertM;
  for (int i = 0; i < DIMENSION; ++i) {
    v1[i] += imass1*(dt*c->reaction[i]);
  }
  if(c->type == PARTICLE_PARTICLE) {
    double imass0 = p->bodies[p->particles[c->o0].body].invertM;
    for (int i = 0; i < DIMENSION; ++i) {
      v0[i] -= imass0*(dt*c->reaction[i]);
    }
  }
#if DIMENSION == 2
  if(omega0) {
    double iinertmom0 = p->bodies[p->particles[c->o0].body].invertI;
    omega0[0] -= iinertmom0*(c->r0[0]*(dt*c->reaction[1]) - c->r0[1]*(dt*c->reaction[0]));
  }
  double iinertmom1 = p->bodies[p->particles[c->o1].body].invertI;
  omega1[0] += iinertmom1*(c->r1[0]*(dt*c->reaction[1]) - c->r1[1]*(dt*c->reaction[0]));
#else
//TODO
#endif
}

static void contact_get_local_velocity(Contact *c, ParticleProblem *p, double *v, double basis[DIMENSION][DIMENSION]) {
  // v1 - v0 in local basis
  double *v0, *v1;
  contact_get_velocity_pointers(c, p, &v0, &v1);
  double *omega0, *omega1;
  contact_get_omega_pointers(c, p, &omega0, &omega1);
#if DIMENSION==2
  double vrot0[2] = {0};
  double vrot1[2];
  if (omega0) {
    vrot0[0] = -(*omega0)*c->r0[1];
    vrot0[1] = (*omega0)*c->r0[0];
  }
  vrot1[0] = -(*omega1)*c->r1[1];
  vrot1[1] = (*omega1)*c->r1[0];
  for(int d0 = 0; d0< DIMENSION; ++d0) {
    for(int d = 0; d<DIMENSION; d++){
      v[d0] += (v1[d]+vrot1[d]-v0[d]-vrot0[d]) * basis[d0][d];
    }
  }
#else
  double res0[3]={0}, res1[3];
  if(omega0)
    _cross(omega0,c->r0,res0);
  _cross(omega1,c->r1,res1);
  for(int d = 0;d<DIMENSION;d++){
    for(int i = 0;i<DIMENSION;i++)
      v[d] += (v1[i]+res1[i] -v0[i]-res0[i]) * basis[d][i];
  }
#endif
}


const Contact *findContactSorted(Contact *c, const 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 particle_problem_bounding_box(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) {
    double position[DIMENSION];
    particle_get_position(p, &p->particles[i], position);
    particle_bounding_box(&p->particles[i], position, amin, amax);
    bbadd(bbmin, bbmax, amin, amax);
  }
  for (size_t i = 0; i < vectorSize(p->disks); ++i) {
    disk_bounding_box(&p->disks[i], amin, amax);
    bbadd(bbmin, bbmax, amin, amax);
  }
  for (size_t i = 0; i < vectorSize(p->segments); ++i) {
    segment_bounding_box(&p->segments[i], amin, amax);
    bbadd(bbmin, bbmax, amin, amax);
  }
  for(size_t i = 0; i < vectorSize(p->periodicSegments); ++i){
    periodic_segment_bounding_box(&p->periodicSegments[i], amin, amax);
    bbadd(bbmin, bbmax, amin, amax);
  }
#if DIMENSION == 3
  for (size_t i = 0; i < vectorSize(p->triangles); ++i) {
    triangle_bounding_box(&p->triangles[i], amin, amax);
    bbadd(bbmin, bbmax, amin, amax);
  }
  for(size_t i = 0; i < vectorSize(p->periodicTriangles); ++i){
    periodic_triangle_bounding_box(&p->periodicTriangles[i], amin, amax);
    bbadd(bbmin, bbmax, amin, amax);
  }
#endif
}

typedef enum {CPARTICLE=0, CGHOST, CPERIODIC} ContactTreeType;
typedef struct {
  Cell *tree;
  int *id;
  ContactTreeType *type;
  int *tmp0;
  int *ghost_id;
  ParticleProblem *problem;
  double *ghost_position;
  double *periodic_translate;
  int *periodic_tag;
  double alert;
} ContactTree;

ContactTree *contact_tree_create(ParticleProblem *p, double alert) {
  double bbmin[DIMENSION], bbmax[DIMENSION];
  particle_problem_bounding_box(p, bbmin, bbmax);
  ContactTree *tree = malloc(sizeof(ContactTree));
  tree->tree = cellNew(bbmin, bbmax);
  tree->tmp0 = NULL;
  tree->id = NULL;
  tree->type = NULL;
  tree->ghost_position = NULL;
  tree->ghost_id = NULL;
  tree->periodic_translate = NULL;
  tree->periodic_tag = NULL;
  tree->alert = alert;
  tree->problem = p;
  return tree;
}

void contact_tree_free(ContactTree *tree) {
  vectorFree(tree->tmp0);
  vectorFree(tree->id);
  vectorFree(tree->type);
  vectorFree(tree->ghost_position);
  vectorFree(tree->ghost_id);
  vectorFree(tree->periodic_translate);
  vectorFree(tree->periodic_tag);
  cellFree(tree->tree);
  free(tree);
}

void contact_tree_add_periodic_segment(ContactTree *tree, PeriodicSegment *segment) {
  int i = vectorSize(tree->id);
  *vectorPush(&tree->id) = vectorSize(tree->periodic_tag);
  *vectorPush(&tree->type) = CPERIODIC;
  for (int d = 0; d < DIMENSION; ++d) {
    *vectorPush(&tree->periodic_translate) = (tree->problem->periodicEntities[segment->entity_id]).transform[d];
  }
  *vectorPush(&tree->periodic_tag) = segment->entity_id;
  double amin[DIMENSION], amax[DIMENSION];
  periodic_segment_bounding_box(segment, amin, amax);
  addAlert(tree->alert/2, amin, amax);
  cellAdd(tree->tree, amin, amax, i, NULL);
}
#if DIMENSION==3
void contact_tree_add_periodic_triangle(ContactTree *tree, PeriodicTriangle *triangle) {
  int i = vectorSize(tree->id);
  *vectorPush(&tree->id) = vectorSize(tree->periodic_tag);
  *vectorPush(&tree->type) = CPERIODIC;
  for (int d = 0; d < DIMENSION; ++d) {
    *vectorPush(&tree->periodic_translate) = (tree->problem->periodicEntities[triangle->entity_id]).transform[d];
  }
  *vectorPush(&tree->periodic_tag) = triangle->entity_id;
  double amin[DIMENSION], amax[DIMENSION];
  periodic_triangle_bounding_box(triangle, amin, amax);
  addAlert(tree->alert/2, amin, amax);
  cellAdd(tree->tree, amin, amax, i, NULL);
}
#endif

void contact_tree_add_ghost_particle(ContactTree *tree, int id, const double *translation) {
  int i = vectorSize(tree->id);
  *vectorPush(&tree->id) = vectorSize(tree->ghost_id);
  *vectorPush(&tree->type) = CGHOST;
  *vectorPush(&tree->ghost_id) = id;
  for (int d = 0; d < DIMENSION; ++d) {
    *vectorPush(&tree->ghost_position) = tree->problem->particles[id].x[d]+translation[d];
  }
  double amin[DIMENSION], amax[DIMENSION];
  particle_bounding_box(&tree->problem->particles[id], tree->ghost_position+vectorSize(tree->ghost_position)-DIMENSION, amin, amax);
  addAlert(tree->alert/2, amin, amax);
  cellAdd(tree->tree, amin, amax, i, NULL);
}

static int intcmp(const void *p0, const void *p1) {
  return *(int*)p0 - *(int*)p1;
}

static void contact_tree_get_particle_and_position(
    const ContactTree *tree, int tree_id, int *p_id, double *p_position) {
  if (tree->type[tree_id] == CPARTICLE) {
    *p_id = tree->id[tree_id];
    particle_get_position(tree->problem, &tree->problem->particles[*p_id], p_position);
  }
  else if (tree->type[tree_id] == CGHOST) {
    int ghost_id = tree->id[tree_id];
    *p_id = tree->ghost_id[ghost_id];
    for (int i = 0; i < DIMENSION; ++i)
    p_position[i] = tree->ghost_position[ghost_id*DIMENSION+i];
  }
}

void contact_tree_add_particle(ContactTree *tree, int id, const Contact *old_contacts, size_t *iold) {
  ParticleProblem *p = tree->problem;
  int i = vectorSize(tree->id);
  *vectorPush(&tree->id) = id;
  *vectorPush(&tree->type) = CPARTICLE;
  double amin[DIMENSION], amax[DIMENSION];
  double position[DIMENSION];
  particle_get_position(p, &p->particles[id], position);
  particle_bounding_box(&p->particles[id], position, amin, amax);
  addAlert(tree->alert/2, amin, amax);
  vectorClear(tree->tmp0);
  cellAdd(tree->tree, amin, amax, i, &tree->tmp0);
  int last_periodic_tag = -1;
  for (size_t j = 0; j < vectorSize(tree->tmp0); ++j) {
    int jj = tree->tmp0[j];
    if (tree->type[jj] == CPERIODIC) {
      if (last_periodic_tag == tree->periodic_tag[tree->id[jj]]) continue;
      last_periodic_tag = tree->periodic_tag[tree->id[jj]];
      const double *t = &tree->periodic_translate[tree->id[jj]*DIMENSION];
      contact_tree_add_ghost_particle(tree, id, t);
    }
    else {
      int id1;
      const Contact *cold;
      double position1[DIMENSION];
      contact_tree_get_particle_and_position(tree, jj, &id1, position1);
      Contact *c = vectorPush(&p->contacts);
      if(!contact_init_particle_particle(c, p, id, position, id1, position1, tree->alert)){
        vectorPop(p->contacts);
      }
      else {
        if ((cold = findContactSorted(c, old_contacts, iold))) {
          for (int d = 0; d < DIMENSION; ++d) {
            c->reaction[d] = cold->reaction[d];
          }
        }
      }
    }
  }
}

static void contact_tree_gen_boundary_contact(ContactTree *tree, ContactType type, int id, const Contact *old_contacts, size_t *iold) {
  double amin[DIMENSION], amax[DIMENSION];
  ParticleProblem *p = tree->problem;
  if (type == PARTICLE_DISK)
      disk_bounding_box(&p->disks[id], amin, amax);
  else if (type == PARTICLE_SEGMENT)
      segment_bounding_box(&p->segments[id], amin, amax);
#if DIMENSION == 3
  else if (type == PARTICLE_TRIANGLE)
      triangle_bounding_box(&p->triangles[id], amin, amax);
#endif
  else printf("invalid type !!!!\n");
  addAlert(tree->alert/2, amin, amax);
  vectorClear(tree->tmp0);
  cellSearch(tree->tree, amin, amax, &tree->tmp0);
  for (size_t j = 0; j < vectorSize(tree->tmp0); ++j) {
    int jj = tree->tmp0[j];
    if (tree->type[jj] == CPERIODIC)
      continue;
    int id1;
    double position1[DIMENSION];
    contact_tree_get_particle_and_position(tree, jj, &id1, position1);
    Contact *c = vectorPush(&p->contacts);
    int r = 0;
    if (type == PARTICLE_DISK){
      r = contact_init_disk_particle(c, p, id, id1, position1, tree->alert);
    }
    else if (type == PARTICLE_SEGMENT) {
      r = contact_init_segment_particle(c, p, id, id1, position1, tree->alert);
    }
#if DIMENSION == 3
    else if (type == PARTICLE_TRIANGLE) {
      r = contact_init_triangle_particle(c, p, id, id1, position1, tree->alert);
    }
#endif
    const Contact *cold;
    if(!r)
      vectorPop(p->contacts);
    else {
      if ((cold = findContactSorted(c, old_contacts, iold))) {
        for (int d = 0; d < DIMENSION; ++d) {
          c->reaction[d] = cold->reaction[d];
        }
      }
    }
  }
}

static void particle_problem_gen_contacts(ParticleProblem *p, const double alert)
{
  clock_t tic = clock