Commit ab828ab3 by Jonathan Lambrechts

### seems ok in 2D

parent b73b598c
Pipeline #9273 failed with stages
in 2 minutes and 28 seconds
 ... ... @@ -99,8 +99,6 @@ class ParticleProblem : """ self._dim = dim self._friction_enabled = friction_enabled if not friction_enabled : rotation_enabled = False; self._rotation_enabled = rotation_enabled if dim == 2 : self._lib = lib2 if rotation_enabled else lib2fwr ... ... @@ -143,6 +141,10 @@ class ParticleProblem : """Returns the list of body masses.""" return self._get_matrix("Body", 2)[:, 1][:,None] def invert_inertia(self): """Returns the list of body masses.""" return self._get_matrix("Body", 2)[:, 0][:,None] def position(self): return self._get_matrix("Position", self._dim) ... ... @@ -162,7 +164,7 @@ class ParticleProblem : def omega(self) : d = 1 if self._dim==2 else 3 if self._friction_enabled and self._rotation_enabled : if self._rotation_enabled : return(self._get_matrix("Omega", d)) else : return np.zeros((self.n_bodies(),d)) ... ... @@ -605,7 +607,7 @@ class ParticleProblem : x = np.sum(positions*masses.reshape((-1,1)),axis=0)/np.sum(masses) I = np.sum(masses.reshape((-1,1))*radii.reshape((-1,1))**2/2 + np.sum((positions-x)**2,axis=1)*masses.reshape((-1,1))) self._lib.particleProblemAddBody(self._p, self._coord_type(*x), c_double(np.sum(masses)), c_double(I),material.encode(),c_int(forced)) self._lib.particleProblemAddBody(self._p, self._coord_type(*x), c_double(np.sum(masses)), c_double(1/I),material.encode(),c_int(forced)) for i in range(positions.shape[0]): self.add_particle(positions[i,:]-x,radii[i],idd) ... ...
 ... ... @@ -88,7 +88,7 @@ struct _Particle{ }; struct _Body{ double I; double invertI; double m; }; ... ... @@ -209,8 +209,8 @@ static int contact_init_particle_particle (Contact *c, const ParticleProblem *p, 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]) ? 1/(p->bodies[p0->body].I) : 0; c->iinertmom1 = (!p->ForcedFlag[p1->body]) ? 1/(p->bodies[p1->body].I) : 0; c->iinertmom0 = (!p->ForcedFlag[p0->body]) ? p->bodies[p0->body].invertI : 0; c->iinertmom1 = (!p->ForcedFlag[p1->body]) ? p->bodies[p1->body].invertI : 0; for (int i = 0;ir1[i] = (p1->x[i] - c->basis[0][i]*p1->r); c->r0[i] = (p0->x[i] + c->basis[0][i]*p0->r); ... ... @@ -279,7 +279,7 @@ static int contact_init_disk_particle(Contact *c, ParticleProblem *p, size_t dis c->imass1 = 1/p->bodies[particle->body].m; #if ROTATION_ENABLED c->iinertmom0 = 0; c->iinertmom1 = (!p->ForcedFlag[p->particles[particle_id].body]) ? 1/p->bodies[particle->body].I : 0; c->iinertmom1 = (!p->ForcedFlag[p->particles[particle_id].body]) ? p->bodies[particle->body].invertI : 0; for (int i = 0;ir0[i] = 0; ... ... @@ -356,7 +356,7 @@ static int contact_init_segment_particle(Contact *c, ParticleProblem *p, size_t c->imass1 = 1/p->bodies[particle->body].m; #if ROTATION_ENABLED c->iinertmom0 = 0; c->iinertmom1 = (!p->ForcedFlag[p->particles[particle_id].body]) ? 1/p->bodies[particle->body].I: 0; c->iinertmom1 = (!p->ForcedFlag[p->particles[particle_id].body]) ? p->bodies[particle->body].invertI: 0; for (int i = 0;ir0[i] = 0; c->r1[i] = (particle->x[i] - c->basis[0][i]*particle->r); ... ... @@ -523,7 +523,7 @@ static void contact_apply(const Contact *c, ParticleProblem *p) { result[1] = (c->dv[0]/wnn)*c->basis[0][1] + (c->dv[1]/wtt)*c->basis[1][1]; if(omega0) omega0[0] += c->iinertmom0*(result[0]*c->r0[1] - result[1]*c->r0[0]); omega1[0] += c->iinertmom1*(result[0]*c->r1[1] - result[1]*c->r1[0]); omega1[0] -= c->iinertmom1*(result[0]*c->r1[1] - result[1]*c->r1[0]); #else //TODO #endif ... ... @@ -562,7 +562,7 @@ static void contact_get_local_free_velocity_normal(Contact *c, ParticleProblem * res1[0] = -p->omega[p->particles[c->o1].body]*c->r1[1]; res1[1] = p->omega[p->particles[c->o1].body]*c->r1[0]; for(int d = 0;dbasis[0][d]; v[0] -= (res0[d]+res1[d]) * c->basis[0][d]; } #endif #endif ... ... @@ -597,7 +597,7 @@ static void contact_get_local_free_velocity_tangent(Contact *c, ParticleProblem res1[0] = -p->omega[p->particles[c->o1].body]*c->r1[1]; res1[1] = p->omega[p->particles[c->o1].body]*c->r1[0]; for(int d = 0;dbasis[1][d]; v[1] -= (res0[d]+res1[d]) * c->basis[1][d]; } #endif #endif ... ... @@ -950,12 +950,10 @@ 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]; printf("pos abs : %f\n",x[i] + p->position[body*DIMENSION+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; } if (alpha < 0 || beta > 0) { c->dv[0] = 0; return; } ... ... @@ -1024,7 +1022,6 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol) } } else { contact_get_local_free_velocity_tangent(c, p, v0, v1, v); double wnn = c->imass0+c->imass1; double wtt = wnn; ... ... @@ -1500,11 +1497,11 @@ void particleProblemAddParticle(ParticleProblem *p, const double x[DIMENSION], d } } void particleProblemAddBodyMaterialTagId(ParticleProblem *p, const double x[DIMENSION], double m, double I, int tag, int forced){ void particleProblemAddBodyMaterialTagId(ParticleProblem *p, const double x[DIMENSION], double m, double invertI, int tag, int forced){ size_t n = vectorSize(p->bodies); Body *body = vectorPush(&p->bodies); body->m = m; body->I = I; body->invertI = invertI; vectorPushN(&p->position, DIMENSION); vectorPushN(&p->velocity, DIMENSION); vectorPush(&p->bodyMaterial); ... ...