Commit 3b471958 authored by Jonathan Lambrechts's avatar Jonathan Lambrechts
Browse files

theta

parent ab828ab3
Pipeline #9274 failed with stages
in 2 minutes and 24 seconds
......@@ -139,11 +139,11 @@ class ParticleProblem :
def mass(self):
"""Returns the list of body masses."""
return self._get_matrix("Body", 2)[:, 1][:,None]
return self._get_matrix("Body", 3)[:, 1][:,None]
def invert_inertia(self):
"""Returns the list of body masses."""
return self._get_matrix("Body", 2)[:, 0][:,None]
return self._get_matrix("Body", 3)[:, 0][:,None]
def position(self):
return self._get_matrix("Position", self._dim)
......@@ -177,14 +177,16 @@ class ParticleProblem :
if self.dim() == 3 :
self._saved_triangles = np.copy(self.triangles())
if self._friction_enabled and self._rotation_enabled :
if self._rotation_enabled :
self._saved_omega = np.copy(self.omega())
self._saved_theta = np.copy(self.theta())
def restore_state(self) :
self.velocity()[:] = self._saved_velocity
self.position()[:] = self._saved_position
if self._friction_enabled and self._rotation_enabled :
if self._rotation_enabled :
self.omega()[:] = self._saved_omega
self.theta()[:] = _self.saved_theta
self.segments()[:] = self._saved_segments
self.disks()[:] = self._saved_disk
if self.dim() == 3 :
......
......@@ -42,6 +42,10 @@ typedef struct _PeriodicEntity PeriodicEntity;
typedef struct _PeriodicSegment PeriodicSegment;
typedef struct _PeriodicTriangle PeriodicTriangle;
// m-> invertM
// retirer forcedFlag
// passer tout en _ plutôt que cammelCase (sauf les types) dans les nouvelles fonctions
struct _ParticleProblem{
Particle *particles;
Body *bodies;
......@@ -89,7 +93,9 @@ struct _Particle{
struct _Body{
double invertI;
// 3D : double invertI[6];
double m;
double theta;
};
struct _Contact {
......@@ -114,8 +120,8 @@ struct _PeriodicEntity{
static void particleBoundingBox(const Particle *p, const double x[DIMENSION], double pmin[DIMENSION], double pmax[DIMENSION]){
for (int i = 0; i < DIMENSION; ++i) {
pmin[i] = x[i] + p->x[i] - p->r;
pmax[i] = x[i] + p->x[i] + p->r;
pmin[i] = x[i] - p->r;
pmax[i] = x[i] + p->r;
}
}
static double particleProblemGetMu(const ParticleProblem *p, int mat0, int mat1) {
......@@ -153,15 +159,25 @@ static void contactBuildBasis(Contact *c) {
#endif
}
void *getParticlePosition(ParticleProblem *p, double *x){
static void particle_get_position(const ParticleProblem *p, const Particle *particle, double x[DIMENSION]) {
size_t body = particle->body;
const double *xp = particle->x;
double theta = p->bodies[body].theta;
#if DIMENSION == 2
x[0] = p->position[body*DIMENSION+0] + cos(theta)*xp[0] - sin(theta)*xp[1];
x[1] = p->position[body*DIMENSION+1] + sin(theta)*xp[0] + cos(theta)*xp[1];
#else
// TODO
#endif
}
void getParticlePosition(ParticleProblem *p, double *x){
for(int i = 0;i<vectorSize(p->particles);i++){
for(int d = 0;d<DIMENSION;d++){
x[i*DIMENSION + d] = p->position[p->particles[i].body*DIMENSION + d] + p->particles[i].x[d];
}
particle_get_position(p, &p->particles[i], x+i*DIMENSION);
}
}
void *getParticleVelocity(ParticleProblem *p, double *v){
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];
......@@ -198,7 +214,7 @@ static int contact_init_particle_particle (Contact *c, const ParticleProblem *p,
c->o1 = id1;
for (int k = 0; k < DIMENSION; ++k) {
c->dv[k] = 0;
c->basis[0][k] = x1[k] + p1->x[k] - x0[k] - p0->x[k];
c->basis[0][k] = x1[k] - x0[k];
nnorm += c->basis[0][k] * c->basis[0][k];
}
nnorm = sqrt(nnorm);
......@@ -211,9 +227,11 @@ static int contact_init_particle_particle (Contact *c, const ParticleProblem *p,
#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];
const double *xbody1 = &p->position[p1->body*DIMENSION];
for (int i = 0;i<DIMENSION;i++){
c->r1[i] = (p1->x[i] - c->basis[0][i]*p1->r);
c->r0[i] = (p0->x[i] + c->basis[0][i]*p0->r);
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;
......@@ -266,7 +284,7 @@ static int contact_init_disk_particle(Contact *c, ParticleProblem *p, size_t dis
c->o1 = particle_id;
for (int i = 0; i < DIMENSION; ++i) {
c->dv[i] = 0;
c->basis[0][i] = x[i] + particle->x[i]- d->x[i];
c->basis[0][i] = x[i] - d->x[i];
nnorm += c->basis[0][i] * c->basis[0][i];
}
nnorm = sqrt(nnorm);
......@@ -281,9 +299,10 @@ static int contact_init_disk_particle(Contact *c, ParticleProblem *p, size_t dis
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] = (particle->x[i] - c->basis[0][i]*particle->r);
c->r1[i] = (x[i]-xbody[i] -c->basis[0][i]*particle->r);
}
#endif
c->type = PARTICLE_DISK;
......@@ -328,6 +347,8 @@ 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;
......@@ -337,13 +358,13 @@ static int contact_init_segment_particle(Contact *c, ParticleProblem *p, size_t
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] - particle->x[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->basis[0][i] = x[i] + particle->x[i] -(s->p[0][i]-t[i]*xi);
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);
......@@ -357,9 +378,10 @@ static int contact_init_segment_particle(Contact *c, ParticleProblem *p, size_t
#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];
for (int i = 0;i<DIMENSION;i++){
c->r0[i] = 0;
c->r1[i] = (particle->x[i] - c->basis[0][i]*particle->r);
c->r1[i] = x[i]-xbody[i] - c->basis[0][i]*particle->r;
}
#endif
c->type = PARTICLE_SEGMENT;
......@@ -639,7 +661,9 @@ static void _particleProblemBoundingBox(ParticleProblem *p, double *bbmin, doubl
bbmax[i] = -DBL_MAX;
}
for (size_t i = 0; i < vectorSize(p->particles); ++i) {
particleBoundingBox(&p->particles[i], &p->position[p->particles[i].body * DIMENSION], amin, amax);
double position[DIMENSION];
particle_get_position(p, &p->particles[i], position);
particleBoundingBox(&p->particles[i], position, amin, amax);
bbadd(bbmin, bbmax, amin, amax);
}
for (size_t i = 0; i < vectorSize(p->disks); ++i) {
......@@ -757,15 +781,16 @@ 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 ContactTree *tree, int tree_id, int *p_id, double *p_position) {
if (tree->type[tree_id] == CPARTICLE) {
*p_id = tree->id[tree_id];
*p_position = &tree->problem->position[tree->problem->particles[(*p_id)].body*DIMENSION];
particle_get_position(tree->problem, &tree->problem->particles[*p_id], p_position);
}
else if (tree->type[tree_id] == CGHOST) {
int ghost_id = tree->id[tree_id];
*p_id = tree->ghost_id[ghost_id];
*p_position = &tree->ghost_position[ghost_id*DIMENSION];
for (int i = 0; i < DIMENSION; ++i)
p_position[i] = tree->ghost_position[ghost_id*DIMENSION+i];
}
}
......@@ -775,7 +800,8 @@ void contact_tree_add_particle(ContactTree *tree, int id, const Contact *old_con
*vectorPush(&tree->id) = id;
*vectorPush(&tree->type) = CPARTICLE;
double amin[DIMENSION], amax[DIMENSION];
const double *position = &p->position[p->particles[id].body*DIMENSION];
double position[DIMENSION];
particle_get_position(p, &p->particles[id], position);
particleBoundingBox(&p->particles[id], position, amin, amax);
addAlert(tree->alert/2, amin, amax);
vectorClear(tree->tmp0);
......@@ -792,8 +818,8 @@ 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);
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,
......@@ -832,8 +858,8 @@ static void contact_tree_gen_boundary_contact(ContactTree *tree, ContactType typ
if (tree->type[jj] == CPERIODIC)
continue;
int id1;
double *position1;
contact_tree_get_particle_and_position(tree, jj, &id1, &position1);
double position1[DIMENSION];
contact_tree_get_particle_and_position(tree, jj, &id1, position1);
Contact *c = vectorPush(&p->contacts);
int r = 0;
if (type == PARTICLE_DISK){
......@@ -933,7 +959,8 @@ static void fifoFree(Fifo *f) {
inline static void contact_avoid(Contact *c, ParticleProblem *p, double vnfree)
{
double *x = p->particles[c->o1].x;
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;
......@@ -950,8 +977,8 @@ inline static void contact_avoid(Contact *c, ParticleProblem *p, double vnfree)
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] + p->position[body*DIMENSION+i] - s->p[0][i]) * d;
beta += (x[i] + p->position[body*DIMENSION+i] - s->p[1][i]) * d;
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;
......@@ -973,10 +1000,10 @@ inline static void contact_avoid(Contact *c, ParticleProblem *p, double vnfree)
double tn[3];
_cross(d0, d1, tn);
double xp[3];
double dp[3] = {t->p[0][0] - x[0] - p->position[body*DIMENSION+0], t->p[0][1] - x[1]- p->position[body*DIMENSION+1], t->p[0][2] - x[2]- p->position[body*DIMENSION+2]};
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] + p->position[body*DIMENSION+i] + tn[i] * nd;
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]},
......@@ -1147,10 +1174,11 @@ void particleProblemComputeStressTensor(ParticleProblem *p, const double *nodes,
int *found = NULL;
for (size_t ic = 0; ic < vectorSize(p->contacts); ++ic) {
const Contact *c = p->contacts + ic;
double *xp = p->position + p->particles[c->o1].body*DIMENSION;
double xp[DIMENSION];
particle_get_position(p, &p->particles[c->o1], xp);
double x[DIMENSION];
for (int d = 0; d < DIMENSION; ++d) {
x[d] = xp[d] + p->particles[c->o1].x[d] - c->basis[0][d]*p->particles[c->o1].r;
x[d] = xp[d]- c->basis[0][d]*p->particles[c->o1].r;
}
cellAdd(tree,x,x,ic,NULL);
}
......@@ -1215,9 +1243,10 @@ void particleProblemComputeStressTensor(ParticleProblem *p, const double *nodes,
for (size_t ifound = 0; ifound < vectorSize(found); ++ifound) {
const Contact *c = p->contacts+found[ifound];
double x[DIMENSION];
double *xp = p->position + p->particles[c->o1].body*DIMENSION;
double xp[DIMENSION];
particle_get_position(p, &p->particles[c->o1], xp);
for (int d = 0; d < DIMENSION; ++d) {
x[d] = xp[d] + p->particles[c->o1].x[d] - c->basis[0][d]*p->particles[c->o1].r;
x[d] = xp[d] - c->basis[0][d]*p->particles[c->o1].r;
}
double d2 = 0;
for (int d = 0; d < DIMENSION; ++d) d2 += (x[d]-nodes[inode*DIMENSION+d])*(x[d]-nodes[inode*DIMENSION+d]);
......@@ -1369,19 +1398,10 @@ int particleProblemIterate(ParticleProblem *p, double alert, double dt, double t
for (size_t i = 0; i < vectorSize(p->position); ++i){
p->position[i] += p->velocity[i] * dt;
}
#if ROTATION_ENABLED
for (size_t i = 0; i < vectorSize(p->particles); ++i){
#if DIMENSION==2
double xold = p->particles[i].x[0];
double yold = p->particles[i].x[1];
p->particles[i].x[0] = xold*cos(p->omega[p->particles[i].body]*dt) - yold*sin(p->omega[p->particles[i].body]*dt);
p->particles[i].x[1] = xold*sin(p->omega[p->particles[i].body]*dt) + yold*cos(p->omega[p->particles[i].body]*dt);
#else
//TODO
p->bodies[i].theta += p->omega[i] * dt;
#endif
}
#endif
for (size_t j = 0; j < vectorSize(p->particles); ++j){
for (size_t i = 0; i < DIMENSION; ++i){
......@@ -1502,6 +1522,7 @@ void particleProblemAddBodyMaterialTagId(ParticleProblem *p, const double x[DIME
Body *body = vectorPush(&p->bodies);
body->m = m;
body->invertI = invertI;
body->theta = 0;
vectorPushN(&p->position, DIMENSION);
vectorPushN(&p->velocity, DIMENSION);
vectorPush(&p->bodyMaterial);
......
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