Skip to content

GitLab

Commit 87e7dea6 authored by Jonathan Lambrechts's avatar Jonathan Lambrechts
Browse files

clean #1

parent 7d1db235
Hide whitespace changes
Inline Side-by-side
scontact/scontact.c
View file @ 87e7dea6
......@@ -59,9 +59,7 @@ struct _ParticleProblem{
Segment *segments;
PeriodicEntity *periodicEntities;
PeriodicSegment *periodicSegments;
#if ROTATION_ENABLED
double *omega;
#endif
double *mu;
#if DIMENSION == 3
Triangle *triangles;
......@@ -102,10 +100,8 @@ struct _Contact {
size_t o0, o1;
double imass0, imass1;
double mu;
#if ROTATION_ENABLED
double iinertmom0, iinertmom1;
double r0[DIMENSION], r1[DIMENSION];
#endif
double D;
double dv[DIMENSION];
double basis[DIMENSION][DIMENSION];
......@@ -180,7 +176,6 @@ void getParticlePosition(ParticleProblem *p, double *x){
void getParticleVelocity(ParticleProblem *p, double *v){
for(int i = 0;i<vectorSize(p->particles);i++){
#if ROTATION_ENABLED
double r[3], res[3], omega[3];
for (int d = 0;d<DIMENSION;d++){
#if DIMENSION==3
......@@ -195,13 +190,8 @@ void getParticleVelocity(ParticleProblem *p, double *v){
omega[2] = p->omega[p->particles[i].body*DIMENSION];
#endif
_cross(omega,r,res);
#endif
for(int d = 0;d<DIMENSION;d++){
v[i*DIMENSION + d] = p->velocity[p->particles[i].body*DIMENSION + d]
#if ROTATION_ENABLED
+res[d]
#endif
;
v[i*DIMENSION + d] = p->velocity[p->particles[i].body*DIMENSION + d] +res[d];
}
}
}
......@@ -226,7 +216,6 @@ static int contact_init_particle_particle (Contact *c, const ParticleProblem *p,
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;
#if ROTATION_ENABLED
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];
......@@ -235,7 +224,6 @@ static int contact_init_particle_particle (Contact *c, const ParticleProblem *p,
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;
}
#endif
c->type = PARTICLE_PARTICLE;
c->mu = particleProblemGetMu(p,p->bodyMaterial[p->particles[id0].body],p->bodyMaterial[p->particles[id1].body]);;
return c->D < alert;
......@@ -297,7 +285,6 @@ static int contact_init_disk_particle(Contact *c, ParticleProblem *p, size_t dis
contactBuildBasis(c);
c->imass0 = 0;
c->imass1 = 1/p->bodies[particle->body].m;
#if ROTATION_ENABLED
c->iinertmom0 = 0;
c->iinertmom1 = (!p->ForcedFlag[p->particles[particle_id].body]) ? p->bodies[particle->body].invertI : 0;
......@@ -306,7 +293,6 @@ static int contact_init_disk_particle(Contact *c, ParticleProblem *p, size_t dis
c->r0[i] = 0;
c->r1[i] = (x[i]-xbody[i] -c->basis[0][i]*particle->r);
}
#endif
c->type = PARTICLE_DISK;
c->mu = particleProblemGetMu(p,d->b.material,p->bodyMaterial[p->particles[particle_id].body]);
return c->D < alert;
......@@ -377,7 +363,6 @@ static int contact_init_segment_particle(Contact *c, ParticleProblem *p, size_t
c->D = nnorm - particle->r;
c->imass0 = 0;
c->imass1 = 1/p->bodies[particle->body].m;
#if ROTATION_ENABLED
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];
......@@ -385,7 +370,6 @@ static int contact_init_segment_particle(Contact *c, ParticleProblem *p, size_t
c->r0[i] = 0;
c->r1[i] = x[i]-xbody[i] - c->basis[0][i]*particle->r;
}
#endif
c->type = PARTICLE_SEGMENT;
c->mu = particleProblemGetMu(p,s->b.material,p->bodyMaterial[p->particles[particle_id].body]);
return c->D < alert;
......@@ -478,21 +462,18 @@ static int contact_init_triangle_particle(Contact *c, const ParticleProblem *p,
c->D -= particle->r;
c->imass0 = 0;
c->imass1 = 1/p->bodies[particle->body].m;
#if ROTATION_ENABLED
c->iinertmom0 = 0;
c->iinertmom1 = (!p->ForcedFlag[p->particles[particle_id].body]) ? (DIMENSION == 3 ? 2.5 : 2)/p->bodies[particle->body].m : 0;
for (int i = 0;i<DIMENSION;i++){
c->r0[i] = 0;
c->r1[i] = (particle->x[i] - c->basis[0][i]*particle->r);
}
#endif
c->type = PARTICLE_TRIANGLE;
c->mu = particleProblemGetMu(p,t->b.material,p->bodyMaterial[p->particles[particle_id].body]);
return c->D < alert;
}
#endif
#if ROTATION_ENABLED
static void contact_get_omega_pointers(const Contact *c, ParticleProblem *p, double **omega0, double **omega1) {
*omega1 = &p->omega[p->particles[c->o1].body*(DIMENSION == 2 ? 1 : 3)];
if (c->type == PARTICLE_PARTICLE) {
......@@ -503,7 +484,6 @@ static void contact_get_omega_pointers(const Contact *c, ParticleProblem *p, dou
*omega0 = NULL;
}
}
#endif
static void contact_get_velocity_pointers(const Contact *c, ParticleProblem *p, double **v0, double **v1) {
......@@ -529,7 +509,6 @@ static void contact_apply(const Contact *c, ParticleProblem *p) {
contact_get_velocity_pointers(c, p, &v0, &v1);
double wnn = c->imass0 + c->imass1;
double wtt = wnn;
#if ROTATION_ENABLED
double res0[1], res1[1];
_cross(c->r0,c->basis[0],res0);
_cross(c->basis[0],c->r1,res1);
......@@ -550,7 +529,6 @@ static void contact_apply(const Contact *c, ParticleProblem *p) {
omega1[0] -= c->iinertmom1*(result[0]*c->r1[1] - result[1]*c->r1[0]);
#else
//TODO
#endif
#endif
coordAdd(v0, -c->dv[0]*c->imass0/wnn, c->basis[0]);
coordAdd(v1, c->dv[0]*c->imass1/wnn, c->basis[0]);
......@@ -566,7 +544,6 @@ static void contact_get_local_free_velocity_normal(Contact *c, ParticleProblem *
for (int i = 0; i<DIMENSION; ++i){
v[0] += (v0[i]-v1[i]) * c->basis[0][i];
}
#if ROTATION_ENABLED
#if DIMENSION==3
double res0[3], omega0[3], res1[3], omega1[3];
for (int d = 0;d<DIMENSION;d++){
......@@ -589,7 +566,6 @@ static void contact_get_local_free_velocity_normal(Contact *c, ParticleProblem *
v[0] -= (res0[d]+res1[d]) * c->basis[0][d];
}
#endif
#endif
}
......@@ -600,7 +576,6 @@ static void contact_get_local_free_velocity_tangent(Contact *c, ParticleProblem
v[d] += (v0[i]-v1[i]) * c->basis[d][i];
}
}
#if ROTATION_ENABLED
#if DIMENSION==3
double res0[3], omega0[3], res1[3], omega1[3];
for (int d = 0;d<DIMENSION;d++){
......@@ -624,8 +599,6 @@ static void contact_get_local_free_velocity_tangent(Contact *c, ParticleProblem
v[1] += (res0[d]+res1[d]) * c->basis[1][d];
}
#endif
#endif
}
......@@ -1052,76 +1025,43 @@ inline static void contact_avoid(Contact *c, ParticleProblem *p, double vnfree)
static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol) {
if (c->type == PARTICLE_PARTICLE && p->particles[c->o0].body==p->particles[c->o1].body)
return 1;
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];
const Particle *p1 = &p->particles[c->o1];
double x0[DIMENSION], x1[DIMENSION];
double x0[DIMENSION];
Body *body0 = &p->bodies[p0->body];
Body *body1 = &p->bodies[p1->body];
#if ROTATION_ENABLED
body0->theta += dt*p->omega[p0->body];
body1->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);
particle_get_position(p, p1, x1);
double dv = c->dv[0];
contact_init_particle_particle(c, p, c->o0, p0, x0, c->o1, p1, x1, 0);
c->D -= c->D0;
c->dv[0] = dv;
body0->theta -= dt*p->omega[p0->body];
body1->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];
}
#endif
}
int contact_avoided = 0;
if (c->type == PARTICLE_SEGMENT) {
const Particle *p1 = &p->particles[c->o1];
double x1[DIMENSION];
Body *body1 = &p->bodies[p1->body];
#if ROTATION_ENABLED
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];
}
particle_get_position(p, p1, x1);
double dv = c->dv[0];
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;
c->D -= c->D0;
c->dv[0] = dv;
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];
}
#endif
}
if (c->type == PARTICLE_DISK) {
const Particle *p1 = &p->particles[c->o1];
double x1[DIMENSION];
Body *body1 = &p->bodies[p1->body];
#if ROTATION_ENABLED
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];
}
particle_get_position(p, p1, x1);
double dv = c->dv[0];
contact_init_disk_particle(c, p, c->o0, c->o1, x1, 0);
c->D -= c->D0;
c->dv[0] = dv;
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];
}
#endif
}
c->D -= c->D0;
c->dv[0] = dv;
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 *v0, *v1;
contact_get_velocity_pointers(c, p, &v0, &v1);
......@@ -1156,7 +1096,6 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
double wnn = c->imass0+c->imass1;
double wtt = wnn;
double wnt = 0;
#if ROTATION_ENABLED
double res0[1], res1[1];
_cross(c->r0,c->basis[0],res0);
_cross(c->basis[0],c->r1,res1);
......@@ -1166,7 +1105,6 @@ 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];
#endif
double ptmax = c->mu*fabs(c->dv[0])*wtt/wnn;
if(v[1]<-ptmax){
......@@ -1506,9 +1444,7 @@ int particleProblemIterate(ParticleProblem *p, double alert, double dt, double t
p->position[i] += p->velocity[i] * dt;
}
for (size_t i = 0; i < vectorSize(p->bodies); ++i){
#if ROTATION_ENABLED
p->bodies[i].theta += p->omega[i] * dt;
#endif
}
for (size_t j = 0; j < vectorSize(p->particles); ++j){
......@@ -1572,12 +1508,10 @@ void particleProblemRemoveBodies(ParticleProblem *p, const int *keep_flag) {
vectorRemoveFlag(p->velocity,keep_flag,DIMENSION);
vectorRemoveFlag(p->ForcedFlag,keep_flag,1);
vectorRemoveFlag(p->bodyMaterial,keep_flag,1);
#if ROTATION_ENABLED
#if DIMENSION==2
vectorRemoveFlag(p->omega,keep_flag,1);
#else
vectorRemoveFlag(p->omega,keep_flag,3);
#endif
#endif
int *keepParticles = malloc(sizeof(size_t)*vectorSize(p->particles));
for (int i = 0;i<vectorSize(p->particles);i++){
......@@ -1635,20 +1569,14 @@ void particleProblemAddBodyMaterialTagId(ParticleProblem *p, const double x[DIME
vectorPushN(&p->velocity, DIMENSION);
vectorPush(&p->bodyMaterial);
vectorPush(&p->ForcedFlag);
#if ROTATION_ENABLED && DIMENSION==3
vectorPushN(&p->omega,DIMENSION);
#endif
for (int i = 0; i < DIMENSION; ++i) {
p->position[n * DIMENSION + i] = x[i];
p->velocity[n * DIMENSION + i] = 0;
#if ROTATION_ENABLED && DIMENSION==3
p->omega[n*DIMENSION +i] = 0;
#endif
}
#if ROTATION_ENABLED && DIMENSION==2
vectorPush(&p->omega);
p->omega[n] = 0.0;
#endif
p->bodyMaterial[n] = tag;
p->ForcedFlag[n] = forced;
}
......@@ -1743,9 +1671,7 @@ double *particleProblemPosition(ParticleProblem *p){return &p->position[0];}
double *particleProblemContactForces(ParticleProblem *p){return &p->contactForces[0];}
double *particleProblemMu(ParticleProblem *p){return p->mu;}
int *particleProblemBodyMaterial(ParticleProblem *p) {return p->bodyMaterial;}
#if ROTATION_ENABLED
double *particleProblemOmega(ParticleProblem *p) {return &p->omega[0];}
#endif
void particleProblemClearBodies(ParticleProblem *p) {
vectorClear(p->position);
......@@ -1753,9 +1679,7 @@ void particleProblemClearBodies(ParticleProblem *p) {
vectorClear(p->bodies);
vectorClear(p->contactForces);
vectorClear(p->velocity);
#if ROTATION_ENABLED
vectorClear(p->omega);
#endif
vectorClear(p->bodyMaterial);
vectorClear(p->ForcedFlag);
}
......@@ -1785,9 +1709,7 @@ void particleProblemDelete(ParticleProblem *p) {
vectorFree(p->contactForces);
vectorFree(p->ForcedFlag);
vectorFree(p->mu);
#if ROTATION_ENABLED
vectorFree(p->omega);
#endif
vectorFree(p->mu);
#if DIMENSION == 3
vectorFree(p->periodicTriangles);
......@@ -1820,9 +1742,7 @@ ParticleProblem *particleProblemNew() {
p->position = NULL;
p->ForcedFlag = NULL;
p->velocity = NULL;
#if ROTATION_ENABLED
p->omega = NULL;
#endif
p->mu = NULL;
p->contactForces = NULL;
#if DIMENSION == 3
......
scontact/scontact.h
View file @ 87e7dea6
......@@ -72,9 +72,7 @@ int *particleProblemTriangleMaterial(ParticleProblem *p);
double *particleProblemTriangle(ParticleProblem *p);
double *particleProblemPeriodicTriangle(ParticleProblem *p);
#endif
#if ROTATION_ENABLED
double *particleProblemOmega(ParticleProblem *p);
#endif
#if FRICTION_ENABLED
void particleProblemSetFrictionCoefficient(ParticleProblem *p, double mu, const char *mat0, const char *mat1);
double *particleProblemMu(ParticleProblem *p);
......
testcases/bodies/tetris.py
View file @ 87e7dea6
......@@ -3,7 +3,8 @@ import random
import os
import numpy as np
os.makedirs("output",exist_ok=True)
odir = "output"
os.makedirs(odir,exist_ok=True)
pieces = np.genfromtxt("pieces.txt")
pieces_omega = pieces[:,0]
pieces_coord = pieces[:,1:].reshape([-1,4,2])
......@@ -17,7 +18,7 @@ def add_piece(p, i):
#p.velocity()[-1,1] = -0.1
n_inserted_pieces = 0
p = scontact.ParticleProblem(2,friction_enabled=False,rotation_enabled=True)
p = scontact.ParticleProblem(2,friction_enabled=True,rotation_enabled=True)
a = 1
g = np.array([0,-9.81])
r = 0.05
......@@ -32,12 +33,12 @@ 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")
p.set_friction_coefficient(.2,"Sand","Sand")
p.set_friction_coefficient(.5,"Sand","Steel")
i = 0
p.write_vtk("output",0,0)
p.write_vtk(odir,0,0)
dt = 0.0025
outf=3
outf=15
t = 0
while t<tend :
print(i)
......@@ -49,5 +50,5 @@ while t<tend :
p.iterate(dt,f,tol=1e-8,force_motion=1)
if i %outf == 0 :
ioutput = int(i/outf) + 1
p.write_vtk("output", ioutput, t)
p.write_vtk(odir, ioutput, t)
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