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

contact_update_normal instead of contact_init

parent dd59080e
Pipeline #8812 failed with stages
in 5 minutes and 4 seconds
......@@ -115,32 +115,6 @@ static void _cross (const double *a, const double *b, double *c) {
c[2] = a[0] * b[1] - a[1] * b[0];
}
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->o0 = id0;
c->o1 = id1;
for (int k = 0; k < DIMENSION; ++k) {
c->dv[k] = 0;
c->normal[k] = x1[k] - x0[k];
nnorm += c->normal[k] * c->normal[k];
}
nnorm = sqrt(nnorm);
for (int i = 0; i < DIMENSION; ++i)
c->normal[i] /= nnorm;
c->D = fmax(0., nnorm - (p0->r + p1->r));
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;
}
typedef struct {
int material;
int tag;
......@@ -183,35 +157,6 @@ size_t particleProblemAddBoundaryDisk(ParticleProblem *p, const double x[DIMENSI
return particleProblemAddBoundaryDiskTagId(p,x,r,particleProblemGetTagId(p,tag),particleProblemGetMaterialTagId(p,materialTag));
}
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->o0 = disk_id;
c->o1 = particle_id;
for (int i = 0; i < DIMENSION; ++i) {
c->dv[i] = 0;
c->normal[i] = x[i] - d->x[i];
nnorm += c->normal[i] * c->normal[i];
}
nnorm = sqrt(nnorm);
c->D = (nnorm - fabs(particle->r + d->r)) * (d->r < 0 ? -1 : 1);
for (int i = 0; i < DIMENSION; ++i) {
c->normal[i] /= (nnorm == 0 ? 1 : nnorm) * (d->r < 0 ? -1 : 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;
}
struct _PeriodicSegment{
size_t entity_id;
double p[2][DIMENSION];
......@@ -247,45 +192,6 @@ static void segmentBoundingBox(const Segment *s, double *pmin, double *pmax) {
}
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];
}
double nn2 = 0;
double xi = dt/nt2;
for (int i = 0; i < DIMENSION; ++i) {
c->normal[i] = x[i] -(s->p[0][i]-t[i]*xi);
nn2 += c->normal[i] * c->normal[i];
}
const double nnorm = nn2 == 0 ? 1 : sqrt(nn2);
for (int i = 0; i < DIMENSION; ++i) {
c->normal[i] /= nnorm;
}
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;
}
#if DIMENSION == 3
struct _Triangle {
Boundary b;
......@@ -345,43 +251,81 @@ static void triangleBoundingBox(const Triangle *t, double *pmin, double *pmax) {
pmax[i] = fmax(fmax(t->p[0][i], t->p[1][i]), t->p[2][i]);
}
}
#endif
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->dv[i] = 0;
c->normal[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->normal, dd);
if (c->D < 0) {
for (int i = 0; i <3; ++i)
c->normal[i] = -c->normal[i];
c->D = -c->D;
}
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;
static void contact_update_normal(Contact *c, ParticleProblem *p, double dt) {
const double *x1 = &p->position[c->o1*DIMENSION];
const Particle *p1 = &p->particles[c->o1];
double nnorm = 0;
if (c->type == PARTICLE_PARTICLE) {
const double *x0 = &p->position[c->o0*DIMENSION];
const Particle *p0 = &p->particles[c->o0];
for (int k = 0; k < DIMENSION; ++k) {
c->normal[k] = x1[k] - x0[k];
nnorm += c->normal[k] * c->normal[k];
}
nnorm = sqrt(nnorm);
for (int i = 0; i < DIMENSION; ++i)
c->normal[i] /= nnorm;
c->D = fmax(0., nnorm - (p0->r + p1->r));
}
else if (c->type == PARTICLE_DISK) {
const Disk *d = &p->disks[c->o0];
double nnorm = 0;
for (int i = 0; i < DIMENSION; ++i) {
c->normal[i] = x1[i] - d->x[i];
nnorm += c->normal[i] * c->normal[i];
}
nnorm = sqrt(nnorm);
c->D = (nnorm - fabs(p1->r + d->r)) * (d->r < 0 ? -1 : 1);
for (int i = 0; i < DIMENSION; ++i) {
c->normal[i] /= (nnorm == 0 ? 1 : nnorm) * (d->r < 0 ? -1 : 1);
}
}
else if (c->type == PARTICLE_SEGMENT) {
const Segment *s = &p->segments[c->o0];
double t[DIMENSION];
double nt2 = 0;
double dtt = 0;
for (int i = 0; i <DIMENSION; ++i){
t[i] = s->p[1][i] - s->p[0][i];
dtt += t[i] * (s->p[0][i] - x1[i]);
nt2 += t[i]*t[i];
}
double nn2 = 0;
double xi = dtt/nt2;
for (int i = 0; i < DIMENSION; ++i) {
c->normal[i] = x1[i] -(s->p[0][i]-t[i]*xi);
nn2 += c->normal[i] * c->normal[i];
}
const double nnorm = nn2 == 0 ? 1 : sqrt(nn2);
for (int i = 0; i < DIMENSION; ++i) {
c->normal[i] /= nnorm;
}
c->D = nnorm - p1->r;
}
#if DIMENSION == 3
else if (c->type == PARTICLE_TRIANGLE) {
const Triangle *t = &p->triangles[c->o0];
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->normal[i] = N[i] / (nn == 0 ? 1 : nn);
}
double dd[3] = {x1[0] - t->p[0][0],x1[1]- t->p[0][1],x1[2]- t->p[0][2]};
c->D = dot(c->normal, dd);
if (c->D < 0) {
for (int i = 0; i <3; ++i)
c->normal[i] = -c->normal[i];
c->D = -c->D;
}
c->D -= p1->r;
}
#endif
c->type = PARTICLE_TRIANGLE;
c->mu = particleProblemGetMu(p,t->b.material,p->particleMaterial[particle_id]);
return c->D < alert;
}
#endif
#if ROTATION_ENABLED
static void contact_get_omega_pointers(const Contact *c, ParticleProblem *p, double **omega0, double **omega1, double *ir0, double *ir1) {
......@@ -638,20 +582,33 @@ void contact_tree_add_particle(ContactTree *tree, int id, const Contact *old_con
}
else {
int id1;
const Contact *cold;
double *position1;
contact_tree_get_particle_and_position(tree, jj, &id1, &position1);
Contact *c = vectorPush(&p->contacts);
if(!contact_init_particle_particle(c, p,
id, &p->particles[id], position,
id1, &p->particles[id1], position1,
tree->alert)
||(p->ForcedFlag[c->o0]&&p->ForcedFlag[c->o1])){
const Particle *p0 = &p->particles[id];
const Particle *p1 = &p->particles[id1];
c->o0 = id;
c->o1 = id1;
c->imass0 = p->ForcedFlag[id] ? 0. : 1/p0->m;
c->imass1 = p->ForcedFlag[id1] ? 0. : 1/p1->m;
#if ROTATION_ENABLED
c->iinertmom0 = (!p->ForcedFlag[id ]) ? (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 = particleProblemGetMu(p,p->particleMaterial[id],p->particleMaterial[id1]);
contact_update_normal(c,p,0);
if(c->D > tree->alert ||(p->ForcedFlag[c->o0]&&p->ForcedFlag[c->o1])) {
vectorPop(p->contacts);
}
else if ((cold = findContactSorted(c, old_contacts, iold))) {
else {
const Contact *cold = findContactSorted(c, old_contacts, iold);
for (int d = 0; d < DIMENSION; ++d) {
c->dv[d] = cold->dv[d];
c->dv[d] = cold ? cold->dv[d] : 0;
}
contact_apply(c, p);
}
......@@ -662,13 +619,20 @@ void contact_tree_add_particle(ContactTree *tree, int id, const Contact *old_con
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)
diskBoundingBox(&p->disks[id], amin, amax);
else if (type == PARTICLE_SEGMENT)
segmentBoundingBox(&p->segments[id], amin, amax);
Boundary *b;
if (type == PARTICLE_DISK) {
diskBoundingBox(&p->disks[id], amin, amax);
b = &p->disks[id].b;
}
else if (type == PARTICLE_SEGMENT){
segmentBoundingBox(&p->segments[id], amin, amax);
b = &p->segments[id].b;
}
#if DIMENSION == 3
else if (type == PARTICLE_TRIANGLE)
triangleBoundingBox(&p->triangles[id], amin, amax);
else if (type == PARTICLE_TRIANGLE) {
triangleBoundingBox(&p->triangles[id], amin, amax);
b = &p->triangles[id].b;
}
#endif
else printf("invalid type !!!!\n");
addAlert(tree->alert/2, amin, amax);
......@@ -682,24 +646,29 @@ static void contact_tree_gen_boundary_contact(ContactTree *tree, ContactType typ
double *position1;
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);
}
const Particle *particle = &p->particles[id1];
c->o0 = id;
c->o1 = id1;
c->imass0 = 0;
c->imass1 = 1/particle->m;
#if ROTATION_ENABLED
c->iinertmom0 = 0;
c->iinertmom1 = (!p->ForcedFlag[id1]) ? (DIMENSION == 3 ? 2.5 : 2)/(particle->m) : 0;
c->r0 = 0;
c->r1 = particle->r;
#endif
const Contact *cold;
if(!r || p->ForcedFlag[c->o1])
c->type = type;
c->mu = particleProblemGetMu(p,b->material,p->particleMaterial[id1]);
contact_update_normal(c, p, 0);
int r = c->D < tree->alert;
if(c->D>tree->alert || p->ForcedFlag[c->o1])
vectorPop(p->contacts);
else if ((cold = findContactSorted(c, old_contacts, iold))) {
else {
const Contact *cold = findContactSorted(c, old_contacts, iold);
for (int d = 0; d < DIMENSION; ++d) {
c->dv[d] = cold->dv[d];
c->dv[d] = cold ? cold->dv[d] : 0;
}
contact_apply(c, p);
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment