Commit 0fac4d43 authored by Jonathan Lambrechts's avatar Jonathan Lambrechts
Browse files

segments and disks have bodies

parent 7eb4af5e
Pipeline #9340 failed with stages
in 2 minutes and 21 seconds
......@@ -100,11 +100,11 @@ class ParticleProblem :
self._p = c_void_p(self._lib.particle_problem_new())
if self._p == None :
raise NameError("Cannot create particle problem.")
bndtype =[('material',np.int32),('tag',np.int32)]
bndtype =[('material',np.int32),('tag',np.int32),('body',np.uint64)]
self._particletype = np.dtype([('r',np.float64),('body',np.uint64),('x',np.float64,dim),('material',np.int32)], align=True)
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,(dim))])
self._triangletype = np.dtype(bndtype+[('p',np.float64,(3,dim)),('v',np.float64,(dim))])
self._disktype = np.dtype(bndtype+[('x',np.float64,(1,dim)),('r',np.float64)])
self._segmenttype = np.dtype(bndtype+[('x',np.float64,(2,dim))])
self._triangletype = np.dtype(bndtype+[('x',np.float64,(3,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))])
......@@ -342,6 +342,7 @@ class ParticleProblem :
s = np.ndarray((n_nodes,self._dim**2))
self._lib.particle_problem_compute_stress_tensor(self._p,_np2c(nodes[:,:self._dim]),c_double(radius),c_int(n_nodes),_np2c(s))
return s
def write_vtk(self, odir, i, t) :
"""Writes output files for post-visualization.
......@@ -363,6 +364,8 @@ class ParticleProblem :
("iMass",bodies["inverseM"][:,None]),
("iInertia",bodies["inverseI"][:,None]),
]
sint = np.sin(bodies["theta"])
cost = np.cos(bodies["theta"])
VTK.write(odir+"/bodies",i,t,None,x,data,vtktype="vtp")
x = self.position()
v = self.velocity()
......@@ -383,16 +386,29 @@ class ParticleProblem :
disks = self.disks()
segments = self.segments()
triangles = self.triangles()
x = np.vstack([disks["x"],segments["p"].reshape([-1,self._dim]),triangles["p"].reshape([-1,self._dim])])
def absolute_coord(o):
xb = bodies["position"][o["body"]][:,None,:]
ct = cost[o["body"]][:,None]
st = sint[o["body"]][:,None]
xo = np.empty_like(o["x"])
xo[:,:,0] = xb[:,:,0]+o["x"][:,:,0]*ct-o["x"][:,:,1]*st
xo[:,:,1] = xb[:,:,1]+o["x"][:,:,0]*st+o["x"][:,:,1]*ct
return xo
xdisks = absolute_coord(disks).reshape([-1,self._dim])
xsegs = absolute_coord(segments).reshape([-1,self._dim])
xtris = absolute_coord(triangles).reshape([-1,self._dim])
x = np.vstack([xdisks,xsegs,xtris])
xrel = np.vstack([disks["x"].reshape(-1,self._dim),segments["x"].reshape(-1,self._dim),triangles["x"].reshape(-1,self._dim)])
if self._dim == 2 :
x = np.insert(x,self._dim,0,axis=1)
xrel = np.insert(xrel,self._dim,0,axis=1)
connectivity = np.arange(0,x.shape[0],dtype=np.int32)
types = np.repeat(np.array([1,3,5],dtype=np.int32),[disks.shape[0],segments.shape[0],triangles.shape[0]])
offsets = np.cumsum(np.repeat([1,2,3],[disks.shape[0],segments.shape[0],triangles.shape[0]]),dtype=np.int32)
tags = np.array(np.hstack([disks["tag"],segments["tag"],triangles["tag"]]))
materials = np.array(np.hstack([disks["material"],segments["material"],triangles["material"]]))
tagnames = list(s.encode() for s in self.id2tag)
VTK.write(odir+"/boundaries",i,t,(types,connectivity,offsets),x,cell_data=[("Tag",tags.reshape([-1,1])),("Material",materials.reshape([-1,1]))],field_data=[(b"TagNames",VTK.string_array_encode(tagnames))])
VTK.write(odir+"/boundaries",i,t,(types,connectivity,offsets),x,data=[("RelativePosition",xrel)],cell_data=[("Tag",tags.reshape([-1,1])),("Material",materials.reshape([-1,1]))],field_data=[(b"TagNames",VTK.string_array_encode(tagnames))])
periodicEntity = self.periodic_entity()
periodicSegments = self.periodic_segments()
periodicTriangles = self.periodic_triangles()
......
......@@ -83,19 +83,18 @@ struct _Body{
typedef struct {
int material;
int tag;
size_t body;
}Boundary;
struct _Disk {
Boundary b;
double x[DIMENSION];
double v[DIMENSION];
double r;
};
struct _Segment{
Boundary b;
double p[2][DIMENSION];
double v[DIMENSION];
double x[2][DIMENSION];
};
struct _Triangle {
......@@ -188,13 +187,12 @@ static void basis_from_normal_vector(double basis[DIMENSION][DIMENSION]) {
#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;
static void body_get_point(const Body *b, const double *xlocal, double *xglobal){
double sint = sin(b->theta);
double cost = cos(b->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];
xglobal[0] = b->position[0] + cost*xlocal[0] - sint*xlocal[1];
xglobal[1] = b->position[1] + sint*xlocal[0] + cost*xlocal[1];
#else
// TODO
#endif
......@@ -202,7 +200,8 @@ static void particle_get_position(const ParticleProblem *p, const Particle *part
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);
const Particle *p0 = &p->particles[i];;
body_get_point(&p->bodies[p0->body], p0->x, x+i*DIMENSION);
}
}
......@@ -274,33 +273,56 @@ static int contact_init_particle_particle(Contact *c, const ParticleProblem *p,
return D < alert;
}
static void disk_bounding_box(const Disk *d, double *pmin, double *pmax) {
static void disk_bounding_box(const Body *b, const Disk *d, double *pmin, double *pmax) {
const double r = fabs(d->r);
double x[DIMENSION];
body_get_point(b, d->x, x);
for (int i = 0; i < DIMENSION; ++i) {
pmin[i] = d->x[i] - r;
pmax[i] = d->x[i] + r;
pmin[i] = x[i] - r;
pmax[i] = x[i] + r;
}
}
size_t particle_problem_add_body(ParticleProblem *p, const double x[DIMENSION], double invertM, double invertI){
size_t n = vectorSize(p->bodies);
Body *body = vectorPush(&p->bodies);
body->invertM = invertM;
body->invertI = invertI;
body->theta = 0;
#if DIMENSION == 2
body->omega = 0;
#else
for (int d = 0; d < 3; ++d)
body->omega[d] = 0;
#endif
for (int i = 0; i < DIMENSION; ++i) {
body->position[i] = x[i];
body->velocity[i] = 0;
}
return n;
}
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.body = particle_problem_add_body(p, x, 0, 0);
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.;
d->x[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]) {
static double contact_update_disk_particle(const Contact *c, const ParticleProblem *p, double *x1, double basis[DIMENSION][DIMENSION], double r0[DIMENSION], double r1[DIMENSION]) {
double nnorm = 0;
const Particle *p1 = &p->particles[c->o1];
double x0[DIMENSION];
const Disk *d = &p->disks[c->o0];
body_get_point(&p->bodies[d->b.body], d->x, x0);
for (int i = 0; i < DIMENSION; ++i) {
basis[0][i] = x[i] - d->x[i];
basis[0][i] = x1[i] - x0[i];
nnorm += basis[0][i] * basis[0][i];
}
nnorm = sqrt(nnorm);
......@@ -311,7 +333,7 @@ static double contact_update_disk_particle(const Contact *c, const ParticleProbl
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);
r1[i] = (x0[i]-xbody[i]-basis[0][i]*p1->r);
}
return (nnorm - fabs(p1->r + d->r)) * (d->r < 0 ? -1 : 1);
}
......@@ -349,28 +371,34 @@ static void coordAdd(double *x, double a, const double *y) {
#endif
}
static void segment_bounding_box(const Segment *s, double *pmin, double *pmax) {
static void segment_bounding_box(const ParticleProblem *p, const Segment *s, double *pmin, double *pmax) {
double x[2][DIMENSION];
body_get_point(&p->bodies[s->b.body], s->x[0], x[0]);
body_get_point(&p->bodies[s->b.body], s->x[1], x[1]);
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]);
pmin[i] = fmin(x[0][i], x[1][i]);
pmax[i] = fmax(x[0][i], x[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]) {
static double contact_update_segment_particle(const Contact *c, const ParticleProblem *p, double *x1, double basis[DIMENSION][DIMENSION], double r0[DIMENSION], double r1[DIMENSION], int *contact_avoided) {
const Segment *s = &p->segments[c->o0];
const Particle *p1 = &p->particles[c->o1];
double nt2 = 0;
double dt = 0;
double t[DIMENSION];
double x0[2][DIMENSION];
body_get_point(&p->bodies[s->b.body], s->x[0], x0[0]);
body_get_point(&p->bodies[s->b.body], s->x[1], x0[1]);
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]);
t[i] = x0[1][i] - x0[0][i];
dt += t[i] * (x0[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);
basis[0][i] = x1[i] -(x0[0][i]-t[i]*xi);
nn2 += basis[0][i] * basis[0][i];
}
const double nnorm = nn2 == 0 ? 1 : sqrt(nn2);
......@@ -383,13 +411,13 @@ static double contact_update_segment_particle(const Contact *c, const ParticlePr
r0[i] = 0; // todo change if segment moves
r1[i] = x1[i] - basis[0][i]*p1->r -xbody[i];
}
//*contact_avoided = !(xi >=0 && xi <=1);
*contact_avoided = 0;
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)
......@@ -401,7 +429,8 @@ static int contact_init_segment_particle(Contact *c, ParticleProblem *p, size_t
}
c->type = PARTICLE_SEGMENT;
double basis[DIMENSION][DIMENSION];
double D = contact_update_segment_particle(c, p, x, basis, c->r0, c->r1);
int avoided;
double D = contact_update_segment_particle(c, p, x, basis, c->r0, c->r1, &avoided);
c->D0 = fmin(D, 0);
return D < alert;
}
......@@ -517,10 +546,10 @@ static void contact_get_velocity_pointers(const Contact *c, ParticleProblem *p,
*v0 = p->bodies[p->particles[c->o0].body].velocity;
}
else if (c->type == PARTICLE_DISK) {
*v0 = p->disks[c->o0].v;
*v0 = p->bodies[p->disks[c->o0].b.body].velocity;
}
else if (c->type == PARTICLE_SEGMENT) {
*v0 = p->segments[c->o0].v;
*v0 = p->bodies[p->segments[c->o0].b.body].velocity;
}
#if DIMENSION == 3
else if (c->type == PARTICLE_TRIANGLE) {
......@@ -624,16 +653,17 @@ static void particle_problem_bounding_box(ParticleProblem *p, double *bbmin, dou
}
for (size_t i = 0; i < vectorSize(p->particles); ++i) {
double position[DIMENSION];
particle_get_position(p, &p->particles[i], position);
const Particle *p0 = &p->particles[i];
body_get_point(&p->bodies[p0->body], p0->x, 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);
disk_bounding_box(&p->bodies[p->disks[i].b.body],&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);
segment_bounding_box(p, &p->segments[i], amin, amax);
bbadd(bbmin, bbmax, amin, amax);
}
for(size_t i = 0; i < vectorSize(p->periodicSegments); ++i){
......@@ -744,9 +774,11 @@ static int intcmp(const void *p0, const void *p1) {
static void contact_tree_get_particle_and_position(
const ContactTree *tree, int tree_id, int *p_id, double *p_position) {
const ParticleProblem *p = tree->problem;
if (tree->type[tree_id] == CPARTICLE) {
*p_id = tree->id[tree_id];
particle_get_position(tree->problem, &tree->problem->particles[*p_id], p_position);
const Particle *p0 = &p->particles[*p_id];
body_get_point(&p->bodies[p0->body], p0->x, p_position);
}
else if (tree->type[tree_id] == CGHOST) {
int ghost_id = tree->id[tree_id];
......@@ -763,7 +795,8 @@ void contact_tree_add_particle(ContactTree *tree, int id, const Contact *old_con
*vectorPush(&tree->type) = CPARTICLE;
double amin[DIMENSION], amax[DIMENSION];
double position[DIMENSION];
particle_get_position(p, &p->particles[id], position);
const Particle *p0 = &p->particles[id];
body_get_point(&p->bodies[p0->body],p0->x, position);
particle_bounding_box(&p->particles[id], position, amin, amax);
addAlert(tree->alert/2, amin, amax);
vectorClear(tree->tmp0);
......@@ -801,9 +834,9 @@ static void contact_tree_gen_boundary_contact(ContactTree *tree, ContactType typ
double amin[DIMENSION], amax[DIMENSION];
ParticleProblem *p = tree->problem;
if (type == PARTICLE_DISK)
disk_bounding_box(&p->disks[id], amin, amax);
disk_bounding_box(&p->bodies[p->disks[id].b.body],&p->disks[id], amin, amax);
else if (type == PARTICLE_SEGMENT)
segment_bounding_box(&p->segments[id], amin, amax);
segment_bounding_box(p, &p->segments[id], amin, amax);
#if DIMENSION == 3
else if (type == PARTICLE_TRIANGLE)
triangle_bounding_box(&p->triangles[id], amin, amax);
......@@ -917,17 +950,6 @@ static void fifo_free(Fifo *f) {
free(f);
}
inline static int projection_is_on_segment(const Segment *s, double x[DIMENSION]) {
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;
}
int r = alpha >=0 && beta <=0;
return r;
}
static void body_advance(Body *b, double dt) {
#if DIMENSION == 2
b->theta += dt*b->omega;
......@@ -953,7 +975,7 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
const Particle *p1 = &p->particles[c->o1];
body_advance(&p->bodies[p1->body], dt);
double x1[DIMENSION];
particle_get_position(p, p1, x1);
body_get_point(&p->bodies[p1->body], p1->x, x1);
if (c->type == PARTICLE_PARTICLE) {
const Particle *p0 = &p->particles[c->o0];
imass0 = p->bodies[p0->body].invertM;
......@@ -963,7 +985,7 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
mu = particle_problem_get_mu(p,p0->material,p1->material);
body_advance(&p->bodies[p0->body], dt);
double x0[DIMENSION];
particle_get_position(p, p0, x0);
body_get_point(&p->bodies[p0->body], p0->x, x0);
D = contact_update_particle_particle(c, p, x0, x1, basis, r0, r1);
body_advance(&p->bodies[p0->body], -dt);
}
......@@ -979,8 +1001,7 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
}
if (c->type == PARTICLE_SEGMENT) {
mu = particle_problem_get_mu(p,p->segments[c->o0].b.material,p1->material);
D = contact_update_segment_particle(c, p, x1, basis, r0, r1);
if (!projection_is_on_segment(&p->segments[c->o0], x1)) contact_avoided = 1;
D = contact_update_segment_particle(c, p, x1, basis, r0, r1, &contact_avoided);
}
body_advance(&p->bodies[p1->body], -dt);
}
......@@ -1142,7 +1163,7 @@ void particle_problem_compute_stress_tensor(ParticleProblem *p, const double *no
Cell *stree = cellNew(bbmin, bbmax);
// filter
for (size_t is = 0; is < vectorSize(p->segments); ++is) {
segment_bounding_box(&p->segments[is], amin, amax);
segment_bounding_box(p, &p->segments[is], amin, amax);
cellAdd(stree,amin,amax,is,NULL);
}
#if DIMENSION == 3
......@@ -1163,12 +1184,15 @@ void particle_problem_compute_stress_tensor(ParticleProblem *p, const double *no
cellSearch(stree,amin,amax,&found);
for (size_t ifound = 0; ifound < vectorSize(found); ++ifound) {
const Segment *s = &p->segments[found[ifound]];
double l = hypot(s->p[1][0]-s->p[0][0],s->p[1][1]-s->p[0][1]);
double alpha = (x[0]-s->p[0][0])*(s->p[1][0]-s->p[0][0]) + (x[1]-s->p[0][1])*(s->p[1][1]-s->p[0][1]);
double xs[2][DIMENSION];
body_get_point(&p->bodies[s->b.body], s->x[0], xs[0]);
body_get_point(&p->bodies[s->b.body], s->x[1], xs[1]);
double l = hypot(xs[1][0]-xs[0][0],xs[1][1]-xs[0][1]);
double alpha = (x[0]-xs[0][0])*(xs[1][0]-xs[0][0]) + (x[1]-xs[0][1])*(xs[1][1]-xs[0][1]);
alpha = fmin(1.,fmax(0.,alpha/l));
double dist = hypot(
x[0]-(s->p[0][0]*(1-alpha)+s->p[1][0]*alpha),
x[1]-(s->p[0][1]*(1-alpha)+s->p[1][1]*alpha));
x[0]-(xs[0][0]*(1-alpha)+xs[1][0]*alpha),
x[1]-(xs[0][1]*(1-alpha)+xs[1][1]*alpha));
if (dist < radius*0.999){
close_to_bnd = 1;
break;
......@@ -1297,27 +1321,6 @@ static int particle_problem_solve(ParticleProblem *p, double alert, double dt, d
int particle_problem_iterate(ParticleProblem *p, double alert, double dt, double tol, int maxit,int force_motion, double *externalForces)
{
for (size_t i = 0; i < vectorSize(p->disks); ++i){
for (int j = 0; j < DIMENSION; ++j) {
p->disks[i].x[j] += p->disks[i].v[j] * dt;
}
}
for (size_t i = 0; i < vectorSize(p->segments); ++i) {
for (int j = 0; j < DIMENSION; ++j) {
p->segments[i].p[0][j] += p->segments[i].v[j] * dt;
p->segments[i].p[1][j] += p->segments[i].v[j] * dt;
}
}
#if DIMENSION == 3
for (size_t i = 0; i < vectorSize(p->triangles); ++i) {
for (int j = 0; j < DIMENSION; ++j) {
p->triangles[i].p[0][j] += p->triangles[i].v[j] * dt;
p->triangles[i].p[1][j] += p->triangles[i].v[j] * dt;
p->triangles[i].p[2][j] += p->triangles[i].v[j] * dt;
}
}
#endif
double *oldVelocity = malloc(sizeof(double)*vectorSize(p->bodies)*DIMENSION);
for (size_t j = 0; j < vectorSize(p->bodies); ++j){
for (size_t i = 0; i < DIMENSION; ++i){
......@@ -1354,10 +1357,14 @@ size_t particle_problem_add_boundary_segment(ParticleProblem *p, const double x0
Segment *s = vectorPush(&p->segments);
s->b.tag = tag;
s->b.material = materialTag;
double x[DIMENSION];
for (int d = 0; d < DIMENSION; ++d) {
x[d] = (x0[d]+x1[d])/2;
}
s->b.body = particle_problem_add_body(p, x, 0, 0);
for (int i = 0; i < DIMENSION; ++i) {
s->p[0][i] = x0[i];
s->p[1][i] = x1[i];
s->v[i] = 0.;
s->x[0][i] = x0[i]-x[i];
s->x[1][i] = x1[i]-x[i];
}
return vectorSize(p->segments) - 1;
}
......@@ -1434,28 +1441,6 @@ size_t particle_problem_add_particle(ParticleProblem *p, const double x[DIMENSIO
return n;
}
size_t particle_problem_add_body(ParticleProblem *p, const double x[DIMENSION], double invertM, double invertI){
size_t n = vectorSize(p->bodies);
Body *body = vectorPush(&p->bodies);
body->invertM = invertM;
body->invertI = invertI;
body->theta = 0;
#if DIMENSION == 2
body->omega = 0;
#else
for (int d = 0; d < 3; ++d)
body->omega[d] = 0;
#endif
for (int i = 0; i < DIMENSION; ++i) {
body->position[i] = x[i];
body->velocity[i] = 0;
}
return n;
}
void particle_problem_set_friction_coefficient(ParticleProblem *p, double mu, int mat0, int mat1) {
p->mu[mat0*p->n_material+mat1] = mu;
p->mu[mat1*p->n_material+mat0] = mu;
......
......@@ -53,5 +53,5 @@ while t<tend :
ioutput = int(i/outf) + 1
print(ioutput)
p.write_vtk(odir, ioutput, t)
p.read_vtk(odir, ioutput)
#p.read_vtk(odir, ioutput)
i += 1
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