Skip to content

GitLab

Commit 46d080d0 authored by Jonathan Lambrechts's avatar Jonathan Lambrechts
Browse files

impulse->reaction (force)

parent 4386f59a
Pipeline #9335 failed with stages
in 2 minutes and 20 seconds
Hide whitespace changes
Inline Side-by-side
python/scontact.py
View file @ 46d080d0
......@@ -109,7 +109,7 @@ class ParticleProblem :
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))])
self._bodytype = np.dtype([('inverseI', np.float64),('inverseM', np.float64), ('theta', np.float64),('omega',np.float64),('position',np.float64,dim),('velocity',np.float64,dim)], align=True)
self._contacttype = np.dtype([('o', np.uint64,(2,)), ('type', np.int32), ('space',np.float64),('r', np.float64,(2,self._dim)) , ('impulse', np.float64,(self._dim,))],align=True)
self._contacttype = np.dtype([('o', np.uint64,(2,)), ('type', np.int32), ('space',np.float64),('r', np.float64,(2,self._dim)) , ('reaction', np.float64,(self._dim,))],align=True)
self.material2id = {}
self.tag2id = {}
self.id2tag = []
......@@ -328,8 +328,7 @@ class ParticleProblem :
return self._lib.particle_problem_iterate(self._p, c_double(np.max(self.r()) if self.r().size != 0 else tol), c_double(dt), c_double(tol), c_int(-1),c_int(force_motion),_np2c(forces))
def contacts(self) :
"""Gives the contact forces. Warning : during the resolution of the contacts,
the considered quantities are impulses, while forces are written and read.
"""Gives the contact forces.
"""
return self._get_array("contact", self._contacttype)
......@@ -414,7 +413,7 @@ class ParticleProblem :
contacts = self.contacts()
for tid,name in enumerate(("particle_particle","particle_disk","particle_segment","particle_triangle")) :
o = contacts[contacts["type"]==tid]["o"]
v = contacts[contacts["type"]==tid]["impulse"]
v = contacts[contacts["type"]==tid]["reaction"]
nameb = name.encode()
fdata.append((nameb, v))
fdata.append((nameb+b"_idx",o))
......
scontact/scontact.c
View file @ 46d080d0
......@@ -117,7 +117,7 @@ struct _Contact {
double D0; // initial interpenetration (should be ~0)
// contact state
double r0[DIMENSION], r1[DIMENSION]; // vector from body center to contact point
double impulse[DIMENSION]; // impulse on o1 expressed in global basis
double reaction[DIMENSION]; // force on o1 expressed in global basis
};
struct _PeriodicEntity{
......@@ -269,7 +269,9 @@ static int contact_init_particle_particle(Contact *c, const ParticleProblem *p,
c->o0 = id0;
c->o1 = id1;
for (int i = 0; i < DIMENSION; ++i) {
c->impulse[i] = 0;
c->reaction[i] = 0;
c->r0[i] = 0;
c->r1[i] = 0;
}
c->type = PARTICLE_PARTICLE;
double basis[DIMENSION][DIMENSION];
......@@ -329,7 +331,9 @@ static int contact_init_disk_particle(Contact *c, ParticleProblem *p, size_t dis
c->o0 = disk_id;
c->o1 = particle_id;
for (int i = 0; i < DIMENSION; ++i) {
c->impulse[i] = 0;
c->reaction[i] = 0;
c->r0[i] = 0;
c->r1[i] = 0;
}
c->type = PARTICLE_DISK;
double basis[DIMENSION][DIMENSION];
......@@ -402,7 +406,9 @@ static int contact_init_segment_particle(Contact *c, ParticleProblem *p, size_t
c->o0 = segment_id;
c->o1 = particle_id;
for (int i = 0; i < DIMENSION; ++i) {
c->impulse[i] = 0;
c->reaction[i] = 0;
c->r0[i] = 0;
c->r1[i] = 0;
}
c->type = PARTICLE_SEGMENT;
double basis[DIMENSION][DIMENSION];
......@@ -476,8 +482,10 @@ static int contact_init_triangle_particle(Contact *c, const ParticleProblem *p,
_cross(d0, d1, N);
const double nn = sqrt(dot(N, N));
for (int i = 0; i < 3; ++i){
c->impulse[i] = 0;
c->reaction[i] = 0;
basis[0][i] = N[i] / (nn == 0 ? 1 : nn);
c->r0[i] = 0;
c->r1[i] = 0;
}
double dd[3] = {x[0]+particle->x[0] - t->p[0][0],x[1] + particle->x[1]- t->p[0][1],x[2] + particle->x[2]- t->p[0][2]};
double D = dot(basis[0], dd);
......@@ -535,28 +543,28 @@ static void contact_get_velocity_pointers(const Contact *c, ParticleProblem *p,
#endif
}
static void contact_apply(const Contact *c, ParticleProblem *p) {
static void contact_apply(const Contact *c, ParticleProblem *p, double dt) {
double *v1, *v0;
contact_get_velocity_pointers(c, p, &v0, &v1);
double *omega0, *omega1;
contact_get_omega_pointers(c, p, &omega0, &omega1);
double imass1 = p->bodies[p->particles[c->o1].body].invertM;
for (int i = 0; i < DIMENSION; ++i) {
v1[i] += imass1*c->impulse[i];
v1[i] += imass1*(dt*c->reaction[i]);
}
if(c->type == PARTICLE_PARTICLE) {
double imass0 = p->bodies[p->particles[c->o0].body].invertM;
for (int i = 0; i < DIMENSION; ++i) {
v0[i] -= imass0*c->impulse[i];
v0[i] -= imass0*(dt*c->reaction[i]);
}
}
#if DIMENSION == 2
if(omega0) {
double iinertmom0 = p->bodies[p->particles[c->o0].body].invertI;
omega0[0] -= iinertmom0*(c->r0[0]*c->impulse[1] - c->r0[1]*c->impulse[0]);
omega0[0] -= iinertmom0*(c->r0[0]*(dt*c->reaction[1]) - c->r0[1]*(dt*c->reaction[0]));
}
double iinertmom1 = p->bodies[p->particles[c->o1].body].invertI;
omega1[0] += iinertmom1*(c->r1[0]*c->impulse[1] - c->r1[1]*c->impulse[0]);
omega1[0] += iinertmom1*(c->r1[0]*(dt*c->reaction[1]) - c->r1[1]*(dt*c->reaction[0]));
#else
//TODO
#endif
......@@ -795,11 +803,10 @@ void contact_tree_add_particle(ContactTree *tree, int id, const Contact *old_con
else {
if ((cold = findContactSorted(c, old_contacts, iold))) {
for (int d = 0; d < DIMENSION; ++d) {
c->impulse[d] = cold->impulse[d];
c->reaction[d] = cold->reaction[d];
c->r0[d] = cold->r0[d];
c->r1[d] = cold->r1[d];
}
contact_apply(c, p);
}
}
}
......@@ -847,11 +854,10 @@ static void contact_tree_gen_boundary_contact(ContactTree *tree, ContactType typ
else {
if ((cold = findContactSorted(c, old_contacts, iold))) {
for (int d = 0; d < DIMENSION; ++d) {
c->impulse[d] = cold->impulse[d];
c->reaction[d] = cold->reaction[d];
c->r0[d] = cold->r0[d];
c->r1[d] = cold->r1[d];
}
contact_apply(c, p);
}
}
}
......@@ -1020,12 +1026,9 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
double pold[DIMENSION] = {0};
// un-apply the old contact
for (int d = 0; d < DIMENSION; ++d) {
pold[d] = c->impulse[d];
}
for (int d=0; d<DIMENSION; ++d) {
c->impulse[d] = -c->impulse[d];
pold[d] = c->reaction[d]*dt;
}
contact_apply(c, p);
contact_apply(c, p, -dt);
for (int d=0; d<DIMENSION; ++d) {
c->r0[d] = r0[d];
c->r1[d] = r1[d];
......@@ -1064,21 +1067,21 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
pt = pstick[1];
}
}
c->impulse[0] = pn*basis[0][0] + pt*basis[1][0];
c->impulse[1] = pn*basis[0][1] + pt*basis[1][1];
c->reaction[0] = (pn*basis[0][0] + pt*basis[1][0])/dt;
c->reaction[1] = (pn*basis[0][1] + pt*basis[1][1])/dt;
// apply new contact and check convergence
double dvmax = 0;
#if DIMENSION == 2
double dp[DIMENSION];
for (int d = 0; d <DIMENSION; ++d) {
dp[d] = c->impulse[d]-pold[d];
dp[d] = c->reaction[d]*dt-pold[d];
}
dvmax = fmax(fabs(dp[0]*wnn+dp[1]*wnt),fabs(dp[0]*wnt+dp[1]*wtt));
#else
//TOD0
#endif
contact_apply(c, p);
contact_apply(c, p, dt);
return dvmax < tol/dt;
}
......@@ -1250,7 +1253,7 @@ void particle_problem_compute_stress_tensor(ParticleProblem *p, const double *no
d = p->particles[c->o1].r;
}
for (int iD = 0; iD < DIMENSION; iD++){
double fi = c->impulse[iD];
double fi = c->reaction[iD];
for (int jD = 0; jD < DIMENSION; jD++){
// TODO check if p->ri- p->rj is correct
fj[iD*DIMENSION+jD] += fi*(c->r1[jD]-c->r0[jD])/V;
......@@ -1319,6 +1322,9 @@ static int particle_problem_solve_contact_queue(ParticleProblem *p, double dt, d
static int particle_problem_solve(ParticleProblem *p, double alert, double dt, double tol, int maxit)
{
particle_problem_gen_contacts(p, alert);
for (size_t i = 0; i < vectorSize(p->contacts); ++i) {
contact_apply(&p->contacts[i], p, dt);
}
if (p->use_queue)
return particle_problem_solve_contact_queue(p,dt,tol);
else
......@@ -1328,12 +1334,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 j = 0; j<vectorSize(p->contacts); ++j){
for (int d = 0; d < DIMENSION; ++d) {
p->contacts[j].impulse[d] *= dt;
}
}
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;
......@@ -1375,11 +1375,6 @@ int particle_problem_iterate(ParticleProblem *p, double alert, double dt, double
body->velocity[i] = oldVelocity[j*DIMENSION + i];
}
}
for (size_t j = 0; j<vectorSize(p->contacts); ++j){
for (int d = 0; d < DIMENSION; ++d) {
p->contacts[j].impulse[d] /= dt;
}
}
free(oldVelocity);
return 0;
}
......@@ -1397,13 +1392,6 @@ int particle_problem_iterate(ParticleProblem *p, double alert, double dt, double
}
free(oldVelocity);
for (size_t j = 0; j<vectorSize(p->contacts); ++j){
for (int d = 0; d < DIMENSION; ++d) {
p->contacts[j].impulse[d] /= dt;
}
}
return 1;
}
......@@ -1525,7 +1513,7 @@ void particle_problem_set_contacts(ParticleProblem *p, size_t n, const size_t *o
p->contacts[j].o0 = o[j*2+0];
p->contacts[j].o1 = o[j*2+1];
for(int d = 0; d < DIMENSION; ++d) {
p->contacts[j].impulse[d] = pc[j*DIMENSION+d];
p->contacts[j].reaction[d] = pc[j*DIMENSION+d];
}
p->contacts[j].type = types[j];
}
......
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