Skip to content

GitLab

Commit 9c743918 authored by Nathan Coppin's avatar Nathan Coppin
Browse files

impulses instead of dv

parent 1c840a8d
Pipeline #9325 failed with stages
in 2 minutes and 25 seconds
Hide whitespace changes
Inline Side-by-side
scontact/scontact.c
View file @ 9c743918
......@@ -103,7 +103,7 @@ struct _Contact {
double iinertmom0, iinertmom1;
double r0[DIMENSION], r1[DIMENSION];
double D;
double dv[DIMENSION];
double p[DIMENSION];
double basis[DIMENSION][DIMENSION];
ContactType type;
double D0;
......@@ -227,7 +227,7 @@ static int contact_init_particle_particle(Contact *c, const ParticleProblem *p,
c->o0 = id0;
c->o1 = id1;
for (int k = 0; k < DIMENSION; ++k) {
c->dv[k] = 0;
c->p[k] = 0;
}
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;
......@@ -303,7 +303,7 @@ 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->dv[i] = 0;
c->p[i] = 0;
}
c->imass0 = 0;
c->imass1 = 1/p->bodies[particle->body].m;
......@@ -387,7 +387,7 @@ 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->dv[i] = 0;
c->p[i] = 0;
}
c->imass0 = 0;
c->imass1 = 1/p->bodies[particle->body].m;
......@@ -472,7 +472,7 @@ 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->dv[i] = 0;
c->p[i] = 0;
c->basis[0][i] = N[i] / (nn == 0 ? 1 : nn);
}
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]};
......@@ -531,35 +531,24 @@ static void contact_get_velocity_pointers(const Contact *c, ParticleProblem *p,
static void contact_apply(const Contact *c, ParticleProblem *p) {
double *v1, *v0;
contact_get_velocity_pointers(c, p, &v0, &v1);
double wnn = c->imass0 + c->imass1;
double wtt = wnn;
double res0[1], res1[1];
_cross(c->r0,c->basis[0],res0);
_cross(c->basis[0],c->r1,res1);
wnn += res0[0]*res0[0]*c->iinertmom0 + res1[0]*res1[0]*c->iinertmom1;
_cross(c->r0,c->basis[1],res0);
_cross(c->basis[1],c->r1,res1);
wtt += res0[0]*res0[0]*c->iinertmom0 + res1[0]*res1[0]*c->iinertmom1;
double *omega0, *omega1;
contact_get_omega_pointers(c, p, &omega0, &omega1);
#if DIMENSION == 2
//printf("r : %f\n",1/ir1);
double result[2];
//printf("omega1 : %f\n",omega1[0]);
result[0] = (c->dv[0]/wnn)*c->basis[0][0] + (c->dv[1]/wtt)*c->basis[1][0];
result[1] = (c->dv[0]/wnn)*c->basis[0][1] + (c->dv[1]/wtt)*c->basis[1][1];
result[0] = c->p[0]*c->basis[0][0] + c->p[1]*c->basis[1][0];
result[1] = c->p[0]*c->basis[0][1] + c->p[1]*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]);
omega0[0] -= c->iinertmom0*(result[1]*c->r0[0] - result[0]*c->r0[1]);
omega1[0] += c->iinertmom1*(result[1]*c->r1[0] - result[0]*c->r1[1]);
#else
//TODO
#endif
coordAdd(v0, -c->dv[0]*c->imass0/wnn, c->basis[0]);
coordAdd(v1, c->dv[0]*c->imass1/wnn, c->basis[0]);
coordAdd(v0, -c->p[0]*c->imass0, c->basis[0]);
coordAdd(v1, c->p[0]*c->imass1, c->basis[0]);
for (int d = 0; d < DIMENSION-1; ++d) {
if (c->dv[d+1] != 0) {
coordAdd(v0, -c->dv[d+1]*c->imass0/wtt, c->basis[d+1]);
coordAdd(v1, c->dv[d+1]*c->imass1/wtt, c->basis[d+1]);
if (c->p[d+1] != 0) {
coordAdd(v0, -c->p[d+1]*c->imass0, c->basis[d+1]);
coordAdd(v1, c->p[d+1]*c->imass1, c->basis[d+1]);
}
}
}
......@@ -591,7 +580,7 @@ static void contact_get_local_velocity_tangent(Contact *c, ParticleProblem *p, c
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;d<DIMENSION;d++){
v[1] += (res0[d]+res1[d]) * c->basis[1][d];
v[1] += (res0[d]-res1[d]) * c->basis[1][d];
}
#endif
}
......@@ -831,7 +820,7 @@ 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->dv[d] = cold->dv[d];
c->p[d] = cold->p[d];
}
contact_apply(c, p);
}
......@@ -882,7 +871,7 @@ 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->dv[d] = cold->dv[d];
c->p[d] = cold->p[d];
}
contact_apply(c, p);
}
......@@ -1020,62 +1009,64 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
}
// allow the pre-existing (from previous time steps) interpenetrations
c->D -= c->D0;
double dvold[DIMENSION] = {0};
for (int d = 0; d < DIMENSION; ++d) {
dvold[d] = c->dv[d];
}
// compute the normal velocity correction
c->dv[0] = fmax(c->dv[0]-c->D/dt, 0.);
if (contact_avoided) c->dv[0] = 0;
// un-apply the old contact
for (int d=0; d<DIMENSION; ++d) {
oldcontact.dv[d] = -oldcontact.dv[d];
oldcontact.p[d] = -oldcontact.p[d];
}
contact_apply(&oldcontact, p);
double pold[DIMENSION] = {0};
for (int d = 0; d < DIMENSION; ++d) {
pold[d] = c->p[d];
}
double wnn = c->imass0+c->imass1;
double wtt = wnn;
double res0[1], res1[1];
_cross(c->r0,c->basis[0],res0);
_cross(c->basis[0],c->r1,res1);
wnn += c->iinertmom0*res0[0]*res0[0]+c->iinertmom1*res1[0]*res1[0];
double rest0[1], rest1[1];
_cross(c->r0,c->basis[1],rest0);
_cross(c->basis[1],c->r1,rest1);
wtt += c->iinertmom0*res0[0]*res0[0]+c->iinertmom1*res1[0]*res1[0];
double wnt = c->iinertmom0*res0[0]*rest0[0]+c->iinertmom1*res1[0]*rest1[0];
double wdet = wtt*wnn-wnt*wnt;
double v[DIMENSION]= {0.}; // local free velocity in the contact basis
double *v0, *v1;
contact_get_velocity_pointers(c, p, &v0, &v1);
contact_get_local_velocity(c, p, v0, v1, v); // v[0] < 0 if particles get closer
v[0] = fmax(c->p[0]*wdet/wtt-c->D/dt, 0.); // for the normal component, the free velocity that has to be canceled
double pstick[2] = {
-(+wtt*v[0]-wnt*v[1])/wdet,
-(-wnt*v[0]+wnn*v[1])/wdet
};
// compute the normal velocity correction
c->p[0] =pstick[0];
if (contact_avoided) c->p[0] = 0;
//solve the tangeant contact
if(c->mu<1e-10 || c->dv[0] == 0.){
if(c->mu<1e-10 || c->p[0] == 0.){
for (int d = 1; d < DIMENSION; ++d) {
c->dv[d] = 0.;
c->p[d] = 0.;
}
}
else {
double wnn = c->imass0+c->imass1;
double wtt = wnn;
double wnt = 0;
double res0[1], res1[1];
_cross(c->r0,c->basis[0],res0);
_cross(c->basis[0],c->r1,res1);
wnn += c->iinertmom0*res0[0]*res0[0]+c->iinertmom1*res1[0]*res1[0];
double rest0[1], rest1[1];
_cross(c->r0,c->basis[1],rest0);
_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];
double *v0, *v1;
contact_get_velocity_pointers(c, p, &v0, &v1);
double v[DIMENSION]= {0.}; // local free velocity in the contact basis
contact_get_local_velocity(c, p, v0, v1, v); // v[0] < 0 if particles get closer
v[0] = -c->dv[0]; // for the normal component, the free velocity that has to be canceled
double wdet = wtt*wnn-wnt*wnt;
double pstick[2] = {
(+wtt*v[0]-wnt*v[1])/wdet,
(-wnt*v[0]+wnn*v[1])/wdet
};
double p[2];
/*if (v[0]>0) {
c->p[0] = 0;
c->p[1] = 0;
}*/
if (pstick[1]+c->mu*pstick[0] < 0) {
p[0] = -v[0]/(wnn-c->mu*wnt);
p[1] = -c->mu*p[0];
c->p[0] = -v[0]/(wnn-c->mu*wnt);
c->p[1] = -c->mu*c->p[0];
}
else if (pstick[1]-c->mu*pstick[0] >0 ) {
p[0] = -v[0]/(wnn+c->mu*wnt);
p[1] = c->mu*p[0];
c->p[0] = -v[0]/(wnn+c->mu*wnt);
c->p[1] = c->mu*c->p[0];
}
else {
p[0] = pstick[0];
p[1] = pstick[1];
c->p[0] = pstick[0];
c->p[1] = pstick[1];
}
#if 0
......@@ -1099,12 +1090,12 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
// apply new contact and check convergence
double dvmax = 0;
double pmax = 0;
for (int d = 0; d <DIMENSION; ++d) {
dvmax = fmax(dvmax, fabs(c->dv[d]-dvold[d]));
pmax = fmax(pmax, fabs(c->p[d]-pold[d]));
}
contact_apply(c, p);
return dvmax < tol/dt;
return pmax*wnn < tol/dt;
}
void swap(int *a, int *b) {
......@@ -1271,18 +1262,16 @@ void particleProblemComputeStressTensor(ParticleProblem *p, const double *nodes,
double *fj = stressTensor + DIMENSION*DIMENSION*inode;
double alpha = 0, d = 0;
if (c->type == PARTICLE_PARTICLE) {
alpha = 1/(1/p->bodies[p->particles[c->o0].body].m+1/p->bodies[p->particles[c->o1].body].m);
d = p->particles[c->o0].r + p->particles[c->o1].r;
}
else {
alpha = p->bodies[p->particles[c->o1].body].m;
d = p->particles[c->o1].r;
}
for (int iD = 0; iD < DIMENSION; iD++){
double fi = c->basis[0][iD]*alpha*c->dv[0];
fi += c->basis[1][iD]*alpha*c->dv[1];
double fi = c->basis[0][iD]*c->p[0];
fi += c->basis[1][iD]*c->p[1];
#if DIMENSION == 3
fi += c->basis[2][iD]*alpha*c->dv[2];
fi += c->basis[2][iD]*c->p[2];
#endif
for (int jD = 0; jD < DIMENSION; jD++){
fj[iD*DIMENSION+jD] += fi*(-d*c->basis[0][jD])/V;
......@@ -1362,7 +1351,7 @@ int particleProblemIterate(ParticleProblem *p, double alert, double dt, double t
for (size_t j = 0; j<vectorSize(p->contacts); ++j){
for (int d = 0; d < DIMENSION; ++d) {
p->contacts[j].dv[d] *= dt;
p->contacts[j].p[d] *= dt;
}
}
......@@ -1409,7 +1398,7 @@ int particleProblemIterate(ParticleProblem *p, double alert, double dt, double t
}
for (size_t j = 0; j<vectorSize(p->contacts); ++j){
for (int d = 0; d < DIMENSION; ++d) {
p->contacts[j].dv[d] /= dt;
p->contacts[j].p[d] /= dt;
}
}
free(oldVelocity);
......@@ -1432,7 +1421,7 @@ int particleProblemIterate(ParticleProblem *p, double alert, double dt, double t
for (size_t j = 0; j<vectorSize(p->contacts); ++j){
for (int d = 0; d < DIMENSION; ++d) {
p->contacts[j].dv[d] /= dt;
p->contacts[j].p[d] /= dt;
}
}
......@@ -1617,14 +1606,14 @@ void particleProblemSetFrictionCoefficient(ParticleProblem *p, double mu, const
p->mu[imat0*n+imat1] = p->mu[imat1*n+imat0] = mu;
}
void particleProblemSetContacts(ParticleProblem *p, size_t n, const size_t *o, const double *v, const double *basis, const int *types) {
void particleProblemSetContacts(ParticleProblem *p, size_t n, const size_t *o, const double *pc, const double *basis, const int *types) {
vectorClear(p->contacts);
vectorPushN(&p->contacts, n);
for (int j = 0; j < n; ++j){
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].dv[d] = v[j*DIMENSION+d];
p->contacts[j].p[d] = pc[j*DIMENSION+d];
}
p->contacts[j].type = types[j];
for (int d = 0; d < DIMENSION; ++d) {
......@@ -1746,13 +1735,13 @@ size_t particleProblemContactN(ParticleProblem *p, int ctype){
return n;
}
void particleProblemContact(ParticleProblem *p, int ctype, size_t *o, double *v){
void particleProblemContact(ParticleProblem *p, int ctype, size_t *o, double *pc){
size_t n = 0;
for (size_t i = 0; i < vectorSize(p->contacts); ++i){
Contact *c = &p->contacts[i];
if (c->type == ctype) {
for (int d = 0; d < DIMENSION; ++d) {
v[DIMENSION*n+d] = p->contacts[i].dv[d];
pc[DIMENSION*n+d] = p->contacts[i].p[d];
}
o[2*n+0] = p->contacts[i].o0;
o[2*n+1] = p->contacts[i].o1;
......
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