Commit 2cb5bf0a authored by Jonathan Lambrechts's avatar Jonathan Lambrechts
Browse files

wip

parent cfbff5e4
......@@ -196,15 +196,11 @@ void getParticleVelocity(ParticleProblem *p, double *v){
}
}
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) {
if(p0->body==p1->body)
return 0;
static void contact_update_particle_particle(Contact *c, const ParticleProblem *p, const double *x0, const double *x1) {
const Particle *p0 = &p->particles[c->o0];
const Particle *p1 = &p->particles[c->o1];
double nnorm = 0;
c->o0 = id0;
c->o1 = id1;
for (int k = 0; k < DIMENSION; ++k) {
c->dv[k] = 0;
c->basis[0][k] = x1[k] - x0[k];
nnorm += c->basis[0][k] * c->basis[0][k];
}
......@@ -214,18 +210,32 @@ static int contact_init_particle_particle (Contact *c, const ParticleProblem *p,
contactBuildBasis(c);
//c->D = fmax(0., nnorm - (p0->r + p1->r));
c->D = nnorm - (p0->r + p1->r);
c->imass0 = p->ForcedFlag[p0->body] ? 0. : 1/p->bodies[p0->body].m;
c->imass1 = p->ForcedFlag[p1->body] ? 0. : 1/p->bodies[p1->body].m;
c->iinertmom0 = (!p->ForcedFlag[p0->body]) ? p->bodies[p0->body].invertI : 0;
c->iinertmom1 = (!p->ForcedFlag[p1->body]) ? p->bodies[p1->body].invertI : 0;
const double *xbody0 = &p->position[p0->body*DIMENSION];
const double *xbody1 = &p->position[p1->body*DIMENSION];
for (int i = 0;i<DIMENSION;i++){
c->r1[i] = x1[i]-xbody1[i] - c->basis[0][i]*p1->r;
c->r0[i] = x0[i]-xbody0[i] + c->basis[0][i]*p0->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];
if(p0->body==p1->body)
return 0;
c->o0 = id0;
c->o1 = id1;
for (int k = 0; k < DIMENSION; ++k) {
c->dv[k] = 0;
}
c->imass0 = p->ForcedFlag[p0->body] ? 0. : 1/p->bodies[p0->body].m;
c->imass1 = p->ForcedFlag[p1->body] ? 0. : 1/p->bodies[p1->body].m;
c->iinertmom0 = (!p->ForcedFlag[p0->body]) ? p->bodies[p0->body].invertI : 0;
c->iinertmom1 = (!p->ForcedFlag[p1->body]) ? p->bodies[p1->body].invertI : 0;
c->type = PARTICLE_PARTICLE;
c->mu = particleProblemGetMu(p,p->bodyMaterial[p->particles[id0].body],p->bodyMaterial[p->particles[id1].body]);;
contact_update_particle_particle(c, p, x0, x1);
return c->D < alert;
}
......@@ -266,35 +276,42 @@ 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];
static void contact_update_disk_particle(Contact *c, const ParticleProblem *p, double *x) {
double nnorm = 0;
c->o0 = disk_id;
c->o1 = particle_id;
const Particle *p1 = &p->particles[c->o1];
const Disk *d = &p->disks[c->o0];
for (int i = 0; i < DIMENSION; ++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(particle->r + d->r)) * (d->r < 0 ? -1 : 1);
c->D = (nnorm - fabs(p1->r + d->r)) * (d->r < 0 ? -1 : 1);
for (int i = 0; i < DIMENSION; ++i) {
c->basis[0][i] /= (nnorm == 0 ? 1 : nnorm) * (d->r < 0 ? -1 : 1);
}
contactBuildBasis(c);
const double *xbody = &p->position[p1->body*DIMENSION];
for (int i = 0;i<DIMENSION;i++){
c->r0[i] = 0; // todo change when disks move
c->r1[i] = (x[i]-xbody[i] -c->basis[0][i]*p1->r);
}
}
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];
c->o0 = disk_id;
c->o1 = particle_id;
for (int i = 0; i < DIMENSION; ++i) {
c->dv[i] = 0;
}
c->imass0 = 0;
c->imass1 = 1/p->bodies[particle->body].m;
c->iinertmom0 = 0;
c->iinertmom1 = (!p->ForcedFlag[p->particles[particle_id].body]) ? p->bodies[particle->body].invertI : 0;
const double *xbody = &p->position[particle->body*DIMENSION];
for (int i = 0;i<DIMENSION;i++){
c->r0[i] = 0;
c->r1[i] = (x[i]-xbody[i] -c->basis[0][i]*particle->r);
}
c->type = PARTICLE_DISK;
c->mu = particleProblemGetMu(p,d->b.material,p->bodyMaterial[p->particles[particle_id].body]);
contact_update_disk_particle(c, p, x);
return c->D < alert;
}
......@@ -332,27 +349,21 @@ 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];
double xcheck[3];
particle_get_position(p, &p->particles[particle_id], xcheck);
const Particle *particle = &p->particles[particle_id];
c->o0 = segment_id;
c->o1 = particle_id;
double t[DIMENSION];
void contact_update_segment_particle(Contact *c, const ParticleProblem *p, double *x1) {
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){
c->dv[i] = 0;
t[i] = s->p[1][i] - s->p[0][i];
dt += t[i] * (s->p[0][i] - x[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) {
c->basis[0][i] = x[i] -(s->p[0][i]-t[i]*xi);
c->basis[0][i] = x1[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);
......@@ -360,18 +371,31 @@ static int contact_init_segment_particle(Contact *c, ParticleProblem *p, size_t
c->basis[0][i] /= nnorm;
}
contactBuildBasis(c);
c->D = nnorm - particle->r;
c->D = nnorm - p1->r;
const double *xbody = &p->position[p1->body*DIMENSION];
for (int i = 0;i<DIMENSION;i++){
c->r0[i] = 0; // todo change if segment moves
c->r1[i] = x1[i]-xbody[i] - c->basis[0][i]*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];
c->o0 = segment_id;
c->o1 = particle_id;
for (int i = 0; i <DIMENSION; ++i){
c->dv[i] = 0;
}
c->imass0 = 0;
c->imass1 = 1/p->bodies[particle->body].m;
c->iinertmom0 = 0;
c->iinertmom1 = (!p->ForcedFlag[p->particles[particle_id].body]) ? p->bodies[particle->body].invertI: 0;
const double *xbody = &p->position[particle->body*DIMENSION];
for (int i = 0;i<DIMENSION;i++){
c->r0[i] = 0;
c->r1[i] = x[i]-xbody[i] - c->basis[0][i]*particle->r;
}
c->type = PARTICLE_SEGMENT;
c->mu = particleProblemGetMu(p,s->b.material,p->bodyMaterial[p->particles[particle_id].body]);
contact_update_segment_particle(c, p, x);
return c->D < alert;
}
......@@ -569,9 +593,9 @@ static void contact_get_local_free_velocity_normal(Contact *c, ParticleProblem *
}
static void contact_get_local_free_velocity_tangent(Contact *c, ParticleProblem *p, const double *v0, const double *v1, double *v) {
static void contact_get_local_velocity_tangent(Contact *c, ParticleProblem *p, const double *v0, const double *v1, double *v) {
for (int d = 1; d < DIMENSION; ++d) {
v[d] = c->dv[d];
v[d] = 0;
for (int i = 0; i<DIMENSION; ++i){
v[d] += (v0[i]-v1[i]) * c->basis[d][i];
}
......@@ -796,10 +820,7 @@ void contact_tree_add_particle(ContactTree *tree, int id, const Contact *old_con
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, &p->particles[id], position,
id1, &p->particles[id1], position1,
tree->alert)
if(!contact_init_particle_particle(c, p, id, position, id1, position1, tree->alert)
||(p->ForcedFlag[p->particles[c->o0].body]&&p->ForcedFlag[p->particles[c->o1].body])){
vectorPop(p->contacts);
}
......@@ -949,150 +970,72 @@ inline static int projection_is_on_segment(const Segment *s, double x[DIMENSION]
return r;
}
inline static void contact_avoid(Contact *c, ParticleProblem *p, double vnfree)
{
double x[DIMENSION];
particle_get_position(p, &p->particles[c->o1], x);
int body = p->particles[c->o1].body;
if (c->type == PARTICLE_PARTICLE)
return;
if (c->type == PARTICLE_DISK) {
const Disk *d = &p->disks[c->o0];;
for (int i = 0; i < DIMENSION; ++i) {
vnfree -= d->v[i] * c->basis[0][i];
}
if (c->dv[0] > fabs(vnfree)) c->dv[0] = fabs(vnfree);
return;
}
if (c->type == PARTICLE_SEGMENT) {
const Segment *s = &p->segments[c->o0];
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) {
c->dv[0] = 0;
return;
}
//double xi = alpha/(alpha-beta);
for (int i = 0; i < DIMENSION; ++i) {
//vbnd[i] = s->v[i];//(1-xi)*s->v[0][i] + xi*s->v[1][i];
vnfree -= s->v[i] * c->basis[0][i];
}
if (c->dv[0] > fabs(vnfree)) c->dv[0] = fabs(vnfree);
return;
}
#if DIMENSION == 3
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 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))) {
c->dv[0] = 0;
return;
}
for (int i = 0; i < DIMENSION; ++i) {
//vbnd[i] = t->v[i];//alpha[0]*t->v[0][i] + alpha[1]*t->v[1][i] + alpha[2]*t->v[2][i];
vnfree -= t->v[i] * c->basis[0][i];
}
if (c->dv[0] > fabs(vnfree)) c->dv[0] = fabs(vnfree);
return;
}
#endif
printf("unknown contact type !\n");
exit(1);
}
static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol) {
const Particle *p1 = &p->particles[c->o1];
Body *body1 = &p->bodies[p1->body];
body1->theta += dt*p->omega[p1->body];
for (int d = 0; d < DIMENSION; ++d) {
p->position[p1->body*DIMENSION+d] += dt*p->velocity[p1->body*DIMENSION+d];
}
double x1[DIMENSION];
particle_get_position(p, p1, x1);
double dv = c->dv[0];
if (c->type == PARTICLE_PARTICLE) {
const Particle *p0 = &p->particles[c->o0];
double x0[DIMENSION];
Body *body0 = &p->bodies[p0->body];
body0->theta += dt*p->omega[p0->body];
int contact_avoided = 0;
Contact oldcontact = *c;
// position prediction with the velocity taking into account the previous contact
// the initialize the contact with those positions
// and revert to the position in n
{
const Particle *p1 = &p->particles[c->o1];
p->bodies[p1->body].theta += dt*p->omega[p1->body];
for (int d = 0; d < DIMENSION; ++d) {
p->position[p0->body*DIMENSION+d] += dt*p->velocity[p0->body*DIMENSION+d];
p->position[p1->body*DIMENSION+d] += dt*p->velocity[p1->body*DIMENSION+d];
}
particle_get_position(p, p0, x0);
contact_init_particle_particle(c, p, c->o0, p0, x0, c->o1, p1, x1, 0);
body0->theta -= dt*p->omega[p0->body];
double x1[DIMENSION];
particle_get_position(p, p1, x1);
if (c->type == PARTICLE_PARTICLE) {
const Particle *p0 = &p->particles[c->o0];
double x0[DIMENSION];
Body *body0 = &p->bodies[p0->body];
body0->theta += dt*p->omega[p0->body];
for (int d = 0; d < DIMENSION; ++d) {
p->position[p0->body*DIMENSION+d] += dt*p->velocity[p0->body*DIMENSION+d];
}
particle_get_position(p, p0, x0);
contact_update_particle_particle(c, p, x0, x1);
body0->theta -= dt*p->omega[p0->body];
for (int d = 0; d < DIMENSION; ++d) {
p->position[p0->body*DIMENSION+d] -= dt*p->velocity[p0->body*DIMENSION+d];
}
}
if (c->type == PARTICLE_SEGMENT) {
contact_update_segment_particle(c, p, x1);
if (!projection_is_on_segment(&p->segments[c->o0], x1)) contact_avoided = 1;
}
if (c->type == PARTICLE_DISK) {
contact_update_disk_particle(c, p, x1);
}
p->bodies[p1->body].theta -= dt*p->omega[p1->body];
for (int d = 0; d < DIMENSION; ++d) {
p->position[p0->body*DIMENSION+d] -= dt*p->velocity[p0->body*DIMENSION+d];
p->position[p1->body*DIMENSION+d] -= dt*p->velocity[p1->body*DIMENSION+d];
}
}
int contact_avoided = 0;
if (c->type == PARTICLE_SEGMENT) {
contact_init_segment_particle(c, p, c->o0, c->o1, x1, 0);
if (!projection_is_on_segment(&p->segments[c->o0], x1)) contact_avoided = 1;
}
if (c->type == PARTICLE_DISK) {
contact_init_disk_particle(c, p, c->o0, c->o1, x1, 0);
}
// allow the pre-existing (from previous time steps) interpenetrations
c->D -= c->D0;
c->dv[0] = dv;
body1->theta -= dt*p->omega[p1->body];
double dvold[DIMENSION] = {0};
for (int d = 0; d < DIMENSION; ++d) {
p->position[p1->body*DIMENSION+d] -= dt*p->velocity[p1->body*DIMENSION+d];
dvold[d] = c->dv[d];
}
double *v0, *v1;
contact_get_velocity_pointers(c, p, &v0, &v1);
double dvold[DIMENSION] = {0};
coordAdd(dvold,1,c->dv);
//contact_get_local_free_velocity_normal(c, p, v0, v1, v);
double incdv0 = -c->D/dt;
double newdv0 = fmax(c->dv[0] + incdv0, 0.);
if (contact_avoided) newdv0 = 0;
double dvmax = fabs(c->dv[0]-newdv0);
c->dv[0] = newdv0;
//c->dv[0] = fmax(0., v[0] - c->D/dt);
//contact_avoid(c, p, v[0]);
//double dvmax = fabs(c->dv[0]-dvold[0]);
// compute the normal velocity correction
c->dv[0] = fmax(c->dv[0]-c->D/dt, 0.);
if (contact_avoided) c->dv[0] = 0;
// un-apply the old contact
for (int d=0; d<DIMENSION; ++d) {
oldcontact.dv[d] = -oldcontact.dv[d];
}
contact_apply(&oldcontact, p);
if(c->mu<1e-10 || c->dv[0] == 0.){
for (int d = 1; d < DIMENSION; ++d) {
c->dv[d] = 0.;
}
}
else {
double v[DIMENSION]= {0.}; // local free velocity in the contact basis
for (int i=0; i < DIMENSION; ++i) {
for (int j=0; j < DIMENSION; ++j) {
//v[i] += (v0[j]-v1[j])*c->basis[i][j];
}
//printf("v[%d] : %f\n",i,v[i]);
}
contact_get_local_free_velocity_tangent(c, p, v0, v1, v);
//if(p->particles[c->o0].body == 31 && p->particles[c->o1].body == 21)
//printf("c->o0 : %ld c->o1 : %ld v[0] : %f v[1] : %f\n",c->o0,c->o1,v[0],v[1]);
double wnn = c->imass0+c->imass1;
double wtt = wnn;
double wnt = 0;
......@@ -1105,29 +1048,33 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
_cross(c->basis[1],c->r1,rest1);
wtt += c->iinertmom0*res0[0]*res0[0]+c->iinertmom1*res1[0]*res1[0];
wnt += c->iinertmom0*res0[0]*rest0[0]+c->iinertmom1*res1[0]*rest1[0];
double ptmax = c->mu*fabs(c->dv[0])*wtt/wnn;
if(v[1]<-ptmax){
c->dv[0] /= (1-c->mu*wnt/wnn);
c->dv[1] = -c->mu*fabs(c->dv[0]);
}
else if (v[1]>ptmax){
c->dv[0] /= (1+c->mu*wnt/wnn);
c->dv[1] = c->mu*fabs(c->dv[0]);
}
else{
c->dv[1] = v[1];
}
}
for (int d = 1; d <DIMENSION; ++d) {
double *v0, *v1;
contact_get_velocity_pointers(c, p, &v0, &v1);
double v[DIMENSION]= {0.}; // local free velocity in the contact basis
contact_get_local_velocity_tangent(c, p, v0, v1, v);
double ptmax = c->mu*fabs(c->dv[0])*wtt/wnn;
if(v[1]<-ptmax){
c->dv[0] /= (1-c->mu*wnt/wnn);
c->dv[1] = -c->mu*fabs(c->dv[0]);
}
else if (v[1]>ptmax){
c->dv[0] /= (1+c->mu*wnt/wnn);
c->dv[1] = c->mu*fabs(c->dv[0]);
}
else{
c->dv[1] = v[1];
}
}
double dvmax = 0;
for (int d = 0; d <DIMENSION; ++d) {
//if (d>=1) dvold[d] = 0;
dvmax = fmax(dvmax, fabs(c->dv[d]-dvold[d]));
}
if (dvmax != 0.) {
//if(c->type == 2) printf("dv[0] : %f dv[1] : %f\n",c->dv[0],c->dv[1]);
coordAdd(c->dv, -1, dvold);
contact_apply(c, p);
coordAdd(c->dv, 1, dvold);
}
contact_apply(c, p);
return dvmax < tol/dt;
}
......
......@@ -4,6 +4,7 @@ import os
import numpy as np
odir = "output"
friction=True
os.makedirs(odir,exist_ok=True)
pieces = np.genfromtxt("pieces.txt")
pieces_omega = pieces[:,0]
......@@ -18,7 +19,7 @@ def add_piece(p, i):
#p.velocity()[-1,1] = -0.1
n_inserted_pieces = 0
p = scontact.ParticleProblem(2,friction_enabled=True,rotation_enabled=True)
p = scontact.ParticleProblem(2,friction_enabled=friction,rotation_enabled=True)
a = 1
g = np.array([0,-9.81])
r = 0.05
......@@ -33,8 +34,9 @@ p.add_boundary_segment([a,-a],[-a,-a],"bnd",material="Steel")
p.add_boundary_disk([-a,a],0,"bnd","Steel")
p.add_boundary_disk([a,a],0,"bnd","Steel")
tend=30
p.set_friction_coefficient(.2,"Sand","Sand")
p.set_friction_coefficient(.5,"Sand","Steel")
if friction:
p.set_friction_coefficient(.2,"Sand","Sand")
p.set_friction_coefficient(.5,"Sand","Steel")
i = 0
p.write_vtk(odir,0,0)
dt = 0.0025
......
Supports Markdown
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