Commit b24a59e5 authored by Jonathan Lambrechts's avatar Jonathan Lambrechts
Browse files

re-structure contact solver

parent 8a72a9e8
Pipeline #8798 passed with stages
in 5 minutes and 28 seconds
......@@ -42,10 +42,8 @@ assert(np.lib.NumpyVersion(np.__version__) >= "1.17.0")
dir_path = os.path.dirname(os.path.realpath(__file__))
lib2 = np.ctypeslib.load_library("libscontact2",dir_path)
lib2f = np.ctypeslib.load_library("libscontact2f",dir_path)
lib2fwr = np.ctypeslib.load_library("libscontact2fwr",dir_path)
lib3 = np.ctypeslib.load_library("libscontact3",dir_path)
lib3f = np.ctypeslib.load_library("libscontact3f",dir_path)
lib3fwr = np.ctypeslib.load_library("libscontact3fwr",dir_path)
is_64bits = sys.maxsize > 2**32
......@@ -101,16 +99,14 @@ class ParticleProblem :
"""
self._dim = dim
self._friction_enabled = friction_enabled
self._rotation_enabled = False if not friction_enabled else rotation_enabled
if not friction_enabled :
rotation_enabled = False;
self._rotation_enabled = rotation_enabled
if dim == 2 :
if friction_enabled:
self._lib = lib2f if rotation_enabled else lib2fwr
else:
self._lib = lib2
self._lib = lib2 if rotation_enabled else lib2fwr
self._coord_type = c_double*2
elif dim == 3 :
self._lib = (lib3f if rotation_enabled else lib3fwr) if (friction_enabled) else lib3
self._lib = lib3 if rotation_enabled else lib3fwr
self._coord_type = c_double*3
else :
raise ValueError("Dimension should be 2 or 3.")
......@@ -121,8 +117,8 @@ class ParticleProblem :
bndtype =[('material',np.int32),('tag',np.int32)]
self._disktype = np.dtype(bndtype+[('x',np.float64,dim),('v',np.float64,dim),('r',np.float64)])
self._segmenttype = np.dtype(bndtype+[('p',np.float64,(2,dim)),('v',np.float64,(2,dim))])
self._triangletype = np.dtype(bndtype+[('p',np.float64,(3,dim)),('v',np.float64,(3,dim))])
self._segmenttype = np.dtype(bndtype+[('p',np.float64,(2,dim)),('v',np.float64,(dim))])
self._triangletype = np.dtype(bndtype+[('p',np.float64,(3,dim)),('v',np.float64,(dim))])
self._periodicEntitytype = np.dtype([('etag', np.int32),('edim', np.int32),('periodic_transformation', np.float64, dim)])
self._periodicSegmenttype = np.dtype([('entity_id', np.int64), ('p',np.float64,(2,dim))])
self._periodicTriangletype = np.dtype([('entity_id', np.int64),('p', np.float64,(3,dim))])
......@@ -261,7 +257,7 @@ class ParticleProblem :
f += f2
if self._dim == 3:
f+= compute_fn_ft("particle_triangle",self.triangles(),tag)
return f
return -f
def set_boundary_velocity(self, tag, v) :
"""Sets the velocity of a boundary to a given value.
......
......@@ -26,23 +26,17 @@ set(SRC
include_directories(. ../tools)
add_library(scontact3f SHARED ${SRC})
target_compile_definitions(scontact3f PUBLIC -DDIMENSION=3 -DFRICTION_ENABLED=1 -DROTATION_ENABLED=1)
add_library(scontact3 SHARED ${SRC})
target_compile_definitions(scontact3 PUBLIC -DDIMENSION=3 -DROTATION_ENABLED=1)
add_library(scontact2 SHARED ${SRC})
target_compile_definitions(scontact2 PUBLIC "-DDIMENSION=2")
add_library(scontact2f SHARED ${SRC})
target_compile_definitions(scontact2f PUBLIC -DDIMENSION=2 -DFRICTION_ENABLED=1 -DROTATION_ENABLED=1)
target_compile_definitions(scontact2 PUBLIC -DDIMENSION=2 -DROTATION_ENABLED=1)
add_library(scontact2fwr SHARED ${SRC})
target_compile_definitions(scontact2fwr PUBLIC -DDIMENSION=2 -DFRICTION_ENABLED=1)
add_library(scontact3 SHARED ${SRC})
target_compile_definitions(scontact3 PUBLIC "-DDIMENSION=3")
target_compile_definitions(scontact2fwr PUBLIC -DDIMENSION=2)
add_library(scontact3fwr SHARED ${SRC})
target_compile_definitions(scontact3fwr PUBLIC -DDIMENSION=3 -DFRICTION_ENABLED=1)
target_compile_definitions(scontact3fwr PUBLIC -DDIMENSION=3)
......
......@@ -53,12 +53,8 @@ struct _ParticleProblem{
Segment *segments;
PeriodicEntity *periodicEntities;
PeriodicSegment *periodicSegments;
#if FRICTION_ENABLED
#if ROTATION_ENABLED
double *omega;
#endif
double *mu;
#endif
#if DIMENSION == 3
Triangle *triangles;
PeriodicTriangle *periodicTriangles;
......@@ -88,16 +84,15 @@ struct _Particle{
struct _Contact {
size_t o0, o1;
double a0, a1;
double D;
double dv;
double n[DIMENSION];
double ct;
double t[DIMENSION];
#if DIMENSION == 3
double cs;
double s[DIMENSION];
double imass0, imass1;
double mu;
#if ROTATION_ENABLED
double iinertmom0, iinertmom1;
double r0, r1;
#endif
double D;
double dv[DIMENSION];
double basis[DIMENSION][DIMENSION];
ContactType type;
};
......@@ -122,47 +117,50 @@ static void _cross (const double *a, const double *b, double *c) {
static void contactBuildBasis(Contact *c) {
#if DIMENSION==3
c->t[0] = -c->n[2];
c->t[1] = c->n[0];
c->t[2] = -c->n[1];
double ddot = dot(c->n,c->t);
c->basis[1][0] = -c->basis[0][2];
c->basis[1][1] = c->basis[0][0];
c->basis[1][2] = -c->basis[0][1];
double ddot = dot(c->basis[0],c->basis[1]);
double norm = 0;
for(int i = 0;i<3;i++){
c->t[i] -= ddot*c->n[i];
norm += c->t[i]*c->t[i];
c->basis[1][i] -= ddot*c->basis[0][i];
norm += c->basis[1][i]*c->basis[1][i];
}
norm = norm == 0 ? 1 : sqrt(norm);
for(int i = 0;i<3;i++){
c->t[i] /= norm;
c->basis[1][i] /= norm;
}
_cross(c->n, c->t, c->s);
_cross(c->basis[0], c->basis[1], c->basis[2]);
#else
c->t[0] = -c->n[1];
c->t[1] = c->n[0];
c->basis[1][0] = -c->basis[0][1];
c->basis[1][1] = c->basis[0][0];
#endif
}
static int particleInitContact(const ParticleProblem *p, size_t id0, const Particle *p0, const double *x0, size_t id1, const Particle *p1, const double *x1, double alert, Contact *c) {
static int contact_init_particle_particle (Contact *c, const ParticleProblem *p, size_t id0, const Particle *p0, const double *x0, size_t id1, const Particle *p1, const double *x1, double alert) {
double nnorm = 0;
c->dv = 0;
c->ct = 0;
#if DIMENSION == 3
c->cs = 0;
#endif
c->o0 = id0;
c->o1 = id1;
for (int k = 0; k < DIMENSION; ++k) {
c->n[k] = x1[k] - x0[k];
nnorm += c->n[k] * c->n[k];
c->dv[k] = 0;
c->basis[0][k] = x1[k] - x0[k];
nnorm += c->basis[0][k] * c->basis[0][k];
}
nnorm = sqrt(nnorm);
for (int i = 0; i < DIMENSION; ++i)
c->n[i] /= nnorm;
c->basis[0][i] /= nnorm;
contactBuildBasis(c);
c->D = fmax(0., nnorm - (p0->r + p1->r));
c->a0 = (p1->m / (p0->m + p1->m))*(1-p->ForcedFlag[id0])*(!p->ForcedFlag[id1]) + 1 - !p->ForcedFlag[id1];
c->a1 = (p0->m / (p0->m + p1->m))*(1-p->ForcedFlag[id1])*(!p->ForcedFlag[id0]) + 1 - !p->ForcedFlag[id0];
c->imass0 = p->ForcedFlag[id0] ? 0. : 1/p0->m;
c->imass1 = p->ForcedFlag[id1] ? 0. : 1/p1->m;
#if ROTATION_ENABLED
c->iinertmom0 = (!p->ForcedFlag[id0]) ? (DIMENSION == 3 ? 2.5 : 2)/(p0->m) : 0;
c->iinertmom1 = (!p->ForcedFlag[id1]) ? (DIMENSION == 3 ? 2.5 : 2)/(p1->m) : 0;
c->r0 = p0->r;
c->r1 = p1->r;
#endif
c->type = PARTICLE_PARTICLE;
c->mu = 0;
return c->D < alert;
}
......@@ -199,32 +197,42 @@ size_t particleProblemAddBoundaryDiskTagId(ParticleProblem *p, const double x[DI
return vectorSize(p->disks) - 1;
}
static double particleProblemGetMu(const ParticleProblem *p, int mat0, int mat1) {
return p->mu[mat0*particleProblemNMaterial(p)+mat1];
}
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) {
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];
double nnorm = 0;
c->dv = 0;
c->o1 = particle;
c->o0 = id;
c->ct = 0;
#if DIMENSION == 3
c->cs = 0;
#endif
c->o0 = disk_id;
c->o1 = particle_id;
for (int i = 0; i < DIMENSION; ++i) {
c->n[i] = d->x[i] - x[i];
nnorm += c->n[i] * c->n[i];
c->dv[i] = 0;
c->basis[0][i] = x[i] - d->x[i];
nnorm += c->basis[0][i] * c->basis[0][i];
}
nnorm = sqrt(nnorm);
c->D = (nnorm - fabs(p->r + d->r)) * (d->r < 0 ? -1 : 1);
c->D = (nnorm - fabs(particle->r + d->r)) * (d->r < 0 ? -1 : 1);
for (int i = 0; i < DIMENSION; ++i) {
c->n[i] /= (nnorm == 0 ? 1 : nnorm) * (d->r < 0 ? -1 : 1);
c->basis[0][i] /= (nnorm == 0 ? 1 : nnorm) * (d->r < 0 ? -1 : 1);
}
contactBuildBasis(c);
c->a0 = 0;
c->a1 = 1;
c->imass0 = 0;
c->imass1 = 1/particle->m;
#if ROTATION_ENABLED
c->iinertmom0 = 0;
c->iinertmom1 = (!p->ForcedFlag[particle_id]) ? (DIMENSION == 3 ? 2.5 : 2)/(particle->m) : 0;
c->r0 = 0;
c->r1 = particle->r;
#endif
c->type = PARTICLE_DISK;
c->mu = particleProblemGetMu(p,d->b.material,p->particleMaterial[particle_id]);
return c->D < alert;
}
......@@ -243,7 +251,7 @@ static void periodicSegmentBoundingBox(const PeriodicSegment *s, double *pmin, d
struct _Segment{
Boundary b;
double p[2][DIMENSION];
double v[2][DIMENSION];
double v[DIMENSION];
};
......@@ -262,64 +270,43 @@ static void segmentBoundingBox(const Segment *s, double *pmin, double *pmax) {
}
}
static double diskIsInside(const void *pd, const double *x, const double *n, double vnfree, double *vbnd, double dv) {
const Disk *d = (const Disk*)pd;
for (int i = 0; i < DIMENSION; ++i) {
vbnd[i] = d->v[i];
vnfree -= vbnd[i] * n[i];
}
if (dv > fabs(vnfree)) dv = fabs(vnfree);
return dv;
}
static double segmentProjectionIsInside(const void *ps, const double *x, const double *n, double vnfree,double *vbnd, double dv) {
const Segment *s = (const Segment*)ps;
double alpha = 0, beta = 0;
for (int i = 0; i < DIMENSION; ++i) {
const double d = s->p[1][i] - s->p[0][i];
alpha += (x[i] - s->p[0][i]) * d;
beta += (x[i] - s->p[1][i]) * d;
}
if (alpha < 0 || beta > 0) return 0;
double xi = alpha/(alpha-beta);
for (int i = 0; i < DIMENSION; ++i) {
vbnd[i] = (1-xi)*s->v[0][i] + xi*s->v[1][i];
vnfree -= vbnd[i] * n[i];
}
if (dv > fabs(vnfree)) dv = fabs(vnfree);
return dv;
}
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;
#if DIMENSION == 3
c->cs = 0;
#endif
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];
const Particle *particle = &p->particles[particle_id];
c->o0 = segment_id;
c->o1 = particle_id;
double t[DIMENSION];
double nt2 = 0;
double dt = 0;
for (int i = 0; i <DIMENSION; ++i){
c->dv[i] = 0;
t[i] = s->p[1][i] - s->p[0][i];
dt += t[i] * (s->p[0][i] - x[i]);
nt2 += t[i] * t[i];
nt2 += t[i]*t[i];
}
double nn2 = 0;
double xi = dt/nt2;
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];
c->basis[0][i] = x[i] -(s->p[0][i]-t[i]*xi);
nn2 += c->basis[0][i] * c->basis[0][i];
}
const double nnorm = nn2 == 0 ? 1 : sqrt(nn2);
for (int i = 0; i < DIMENSION; ++i) {
c->n[i] /= nnorm;
c->basis[0][i] /= nnorm;
}
contactBuildBasis(c);
c->D = nnorm - p->r;
c->a0 = 0;
c->a1 = 1;
c->D = nnorm - particle->r;
c->imass0 = 0;
c->imass1 = 1/particle->m;
#if ROTATION_ENABLED
c->iinertmom0 = 0;
c->iinertmom1 = (!p->ForcedFlag[particle_id]) ? (DIMENSION == 3 ? 2.5 : 2)/(particle->m) : 0;
c->r0 = 0;
c->r1 = particle->r;
#endif
c->type = PARTICLE_SEGMENT;
c->mu = particleProblemGetMu(p,s->b.material,p->particleMaterial[particle_id]);
return c->D < alert;
}
......@@ -328,7 +315,7 @@ static int segmentInitContact(size_t id, const Segment *s, size_t particle, cons
struct _Triangle {
Boundary b;
double p[3][DIMENSION];
double v[3][DIMENSION];
double v[DIMENSION];
};
struct _PeriodicTriangle {
size_t entity_id;
......@@ -343,9 +330,7 @@ void particleProblemAddBoundaryTriangleTagId(ParticleProblem *p, const double x0
t->p[0][i] = x0[i];
t->p[1][i] = x1[i];
t->p[2][i] = x2[i];
t->v[0][i] = 0.;
t->v[1][i] = 0.;
t->v[2][i] = 0.;
t->v[i] = 0.;
}
}
void particleProblemAddBoundaryTriangle(ParticleProblem *p, const double x0[3], const double x1[3], const double x2[3], const char *tag, const char *material) {
......@@ -386,241 +371,144 @@ static void triangleBoundingBox(const Triangle *t, double *pmin, double *pmax) {
}
}
static double triangleProjectionIsInside(const void *pt, const double *x, const double *n, double vnfree,double *vbnd, double dv) {
const Triangle *t = (const Triangle*)pt;
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 tn[3];
_cross(d0, d1, tn);
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(tn, dp);
for (int i = 0; i < 3; ++i)
xp[i] = x[i] + tn[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] = dot(tn, d)/(tn[0]*tn[0] + tn[1]*tn[1] + tn[2]*tn[2]);
}
if (!((alpha[0] <= 0 && alpha[1] <= 0 && alpha[2]<= 0) || (alpha[0] >= 0 && alpha[1] >= 0 && alpha[2] >= 0))) return 0;
for (int i = 0; i < DIMENSION; ++i) {
vbnd[i] = alpha[0]*t->v[0][i] + alpha[1]*t->v[1][i] + alpha[2]*t->v[2][i];
vnfree -= vbnd[i] * n[i];
}
if (dv > fabs(vnfree)) dv = fabs(vnfree);
return dv;
}
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;
c->cs = 0;
static int contact_init_triangle_particle(Contact *c, const ParticleProblem *p,
size_t triangle_id, size_t particle_id, double *x, double alert) {
const Particle *particle = &p->particles[particle_id];
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->n[i] = N[i] / (nn == 0 ? 1 : 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);
for (int i = 0; i < 3; ++i){
c->dv[i] = 0;
c->basis[0][i] = N[i] / (nn == 0 ? 1 : nn);
}
double dd[3] = {x[0] - t->p[0][0],x[1]- t->p[0][1],x[2]- t->p[0][2]};
c->D = dot(c->basis[0], dd);
if (c->D < 0) {
for (int i = 0; i <3; ++i)
c->n[i] = -c->n[i];
c->basis[0][i] = -c->basis[0][i];
c->D = -c->D;
}
contactBuildBasis(c);
c->D -= p->r;
c->a0 = 0;
c->a1 = 1;
c->D -= particle->r;
c->imass0 = 0;
c->imass1 = 1/particle->m;
#if ROTATION_ENABLED
c->iinertmom0 = 0;
c->iinertmom1 = (!p->ForcedFlag[particle_id]) ? (DIMENSION == 3 ? 2.5 : 2)/(particle->m) : 0;
c->r0 = 0;
c->r1 = particle->r;
#endif
c->type = PARTICLE_TRIANGLE;
c->mu = particleProblemGetMu(p,t->b.material,p->particleMaterial[particle_id]);
return c->D < alert;
}
#endif
#if FRICTION_ENABLED
static double particleProblemGetMu(const ParticleProblem *p, int mat0, int mat1) {
return p->mu[mat0*particleProblemNMaterial(p)+mat1];
#if ROTATION_ENABLED
static void contact_get_omega_pointers(const Contact *c, ParticleProblem *p, double **omega0, double **omega1, double *ir0, double *ir1) {
*omega1 = &p->omega[c->o1*(DIMENSION-1)];
*ir1 = 1/p->particles[c->o1].r;
if (c->type == PARTICLE_PARTICLE) {
*omega0 = &p->omega[c->o0*(DIMENSION-1)];
*ir0 = 1/p->particles[c->o0].r;
}
else {
*omega0 = NULL;
*ir0 = 0;
}
}
#endif
static int contactParticleParticleSolve(Contact *c, ParticleProblem *p, double dt, double tol) {
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 dv = fmax(0., vn - c->D/dt);
dv -= c->dv;
int criterion = fabs(dv) <= tol/dt;
coordAdd(&p->velocity[c->o0*DIMENSION], -dv*c->a0, c->n);
coordAdd(&p->velocity[c->o1*DIMENSION], dv*c->a1, c->n);
c->dv += dv;
#if !FRICTION_ENABLED
return criterion;
#else
dv = c->dv;
const double mu = particleProblemGetMu(p,p->particleMaterial[c->o0],p->particleMaterial[c->o1]);
if(mu<1e-10) return criterion;
double ct = 0;
double vt = c->ct;
#if DIMENSION==3
double cs = 0;
double vs = c->cs;
#endif
for(int i = 0;i<DIMENSION;i++){
vt += (p->velocity[c->o0*DIMENSION+i]-p->velocity[c->o1*DIMENSION+i])*c->t[i];
#if DIMENSION==3
vs += (p->velocity[c->o0*3+i]-p->velocity[c->o1*3+i])*c->s[i];
#endif
static void contact_get_velocity_pointers(const Contact *c, ParticleProblem *p, double **v0, double **v1) {
*v1 = &p->velocity[c->o1*(DIMENSION)];
if (c->type == PARTICLE_PARTICLE) {
*v0 = &p->velocity[c->o0*(DIMENSION)];
}
#if ROTATION_ENABLED && DIMENSION==2
vt += p->omega[c->o0]*p->particles[c->o0].r+p->omega[c->o1]*p->particles[c->o1].r;
#elif ROTATION_ENABLED && DIMENSION==3
double res0[3], res1[3];
_cross(&p->omega[c->o0*3], c->n, res0);
_cross(&p->omega[c->o1*3], c->n, res1);
for(int i = 0;i<DIMENSION;i++){
vt += (res1[i]*p->particles[c->o1].r+res0[i]*p->particles[c->o0].r)*c->t[i];
vs += (res1[i]*p->particles[c->o1].r+res0[i]*p->particles[c->o0].r)*c->s[i];
else if (c->type == PARTICLE_DISK) {
*v0 = p->disks[c->o0].v;
}
else if (c->type == PARTICLE_SEGMENT) {
*v0 = p->segments[c->o0].v;
}
#endif
#if DIMENSION==2
ct = vt*fmin(fabs(vt),3*mu*dv)/(fabs(vt) == 0 ? 1 : fabs(vt));
#else
double norm = sqrt(vt*vt + vs*vs);
ct = vt*fmin(norm,3.5*mu*dv)/(norm == 0 ? 1 : norm);
cs = vs*fmin(norm,3.5*mu*dv)/(norm == 0 ? 1 : norm);
#endif
ct -= c->ct;
#if ROTATION_ENABLED && DIMENSION==2
coordAdd(&p->velocity[c->o0 * DIMENSION], -ct*c->a0/3, c->t);
coordAdd(&p->velocity[c->o1 * DIMENSION], ct*c->a1/3, c->t);
p->omega[c->o0] -= (1/p->particles[c->o0].r)*ct*c->a0*2/3;
p->omega[c->o1] -= (1/p->particles[c->o1].r)*ct*c->a1*2/3;
#elif ROTATION_ENABLED && DIMENSION==3
cs -= c->cs;
coordAdd(&p->velocity[c->o0 * DIMENSION], -ct*c->a0*2/7, c->t);
coordAdd(&p->velocity[c->o1 * DIMENSION], ct*c->a1*2/7, c->t);
coordAdd(&p->velocity[c->o0 * DIMENSION], -cs*c->a0*2/7, c->s);
coordAdd(&p->velocity[c->o1 * DIMENSION], cs*c->a1*2/7, c->s);
for (int i = 0; i<3; i++){
p->omega[c->o0*3 + i] += (1/p->particles[c->o0].r)*c->a0*(cs*c->t[i]-ct*c->s[i])*5/7;
p->omega[c->o1*3 + i] += (1/p->particles[c->o1].r)*c->a1*(cs*c->t[i]-ct*c->s[i])*5/7;
}
c->cs += cs;
#elif DIMENSION==2
coordAdd(&p->velocity[c->o0 * DIMENSION], -ct*c->a0, c->t);
coordAdd(&p->velocity[c->o1 * DIMENSION], ct*c->a1, c->t);
#elif DIMENSION==3
cs -= c->cs;
coordAdd(&p->velocity[c->o0 * DIMENSION], -cs*c->a0, c->s);
coordAdd(&p->velocity[c->o1 * DIMENSION], cs*c->a1, c->s);
c->cs += cs;
#endif
c->ct += ct;
int rfriction = fabs(ct) <= tol/dt;
#if DIMENSION == 3
rfriction &= fabs(cs) <= tol/dt;
#endif
return (criterion && rfriction);
else if (c->type == PARTICLE_TRIANGLE) {
*v0 = p->triangles[c->o0].v;
}
#endif
}
#if FRICTION_ENABLED
static int contactParticleBndFrictionSolve(ParticleProblem *p, Contact *c, Boundary *bnd, double dv, double toldt, double *vbnd) {
const double mu = particleProblemGetMu(p,p->particleMaterial[c