Commit 91aefae0 authored by Jonathan Lambrechts's avatar Jonathan Lambrechts
Browse files

fix

parent c1f51bdb
Pipeline #9328 failed with stages
in 2 minutes and 24 seconds
......@@ -101,10 +101,11 @@ struct _Contact {
double imass0, imass1;
double mu;
double iinertmom0, iinertmom1;
double r0[DIMENSION], r1[DIMENSION];
double r0[DIMENSION], r1[DIMENSION]; // vector from body center to contact point
double D;
double p[DIMENSION];
double basis[DIMENSION][DIMENSION];
double p[DIMENSION]; // impulsion on o1 expressed in local basis
double basis[DIMENSION][DIMENSION]; // basis[0] contact direction from o0 to o1
// basis[1] = rot(basis[0],pi/2) (in 2D)
ContactType type;
double D0;
};
......@@ -375,7 +376,7 @@ void contact_update_segment_particle(Contact *c, const ParticleProblem *p, doubl
const double *xbody = &p->position[p1->body*DIMENSION];
for (int i = 0;i<DIMENSION;i++){
c->r0[i] = 0; // todo change if segment moves
c->r1[i] = x1[i]-xbody[i] - c->basis[0][i]*p1->r;
c->r1[i] = x1[i] - c->basis[0][i]*p1->r -xbody[i];
}
}
......@@ -533,24 +534,22 @@ static void contact_apply(const Contact *c, ParticleProblem *p) {
contact_get_velocity_pointers(c, p, &v0, &v1);
double *omega0, *omega1;
contact_get_omega_pointers(c, p, &omega0, &omega1);
double pxyz[3];
for (int i = 0; i < DIMENSION; ++i) {
pxyz[i] = 0;
for (int j = 0; j < DIMENSION; ++j) {
pxyz[i] += c->p[j]*c->basis[j][i];
}
v0[i] -= c->imass0*pxyz[i];
v1[i] += c->imass1*pxyz[i];
}
#if DIMENSION == 2
double result[2];
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[1]*c->r0[0] - result[0]*c->r0[1]);
omega1[0] += c->iinertmom1*(result[1]*c->r1[0] - result[0]*c->r1[1]);
omega0[0] -= c->iinertmom0*(c->r0[0]*pxyz[1] - c->r0[1]*pxyz[0]);
omega1[0] += c->iinertmom1*(c->r1[0]*pxyz[1] - c->r1[1]*pxyz[0]);
#else
//TODO
#endif
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->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]);
}
}
}
static void contact_get_local_velocity_tangent(Contact *c, ParticleProblem *p, const double *v0, const double *v1, double *v) {
......@@ -584,37 +583,34 @@ static void contact_get_local_velocity_tangent(Contact *c, ParticleProblem *p, c
}
#endif
}
static void contact_get_local_velocity(Contact *c, ParticleProblem *p, const double *v0, const double *v1, double *v) {
for (int d = 0; d < DIMENSION; ++d) {
v[d] = 0;
for (int i = 0; i<DIMENSION; ++i){
v[d] += (v1[i]-v0[i]) * c->basis[d][i];
static void contact_get_local_velocity(Contact *c, ParticleProblem *p, double *v) {
// v1 - v0 in local basis
double *v0, *v1;
contact_get_velocity_pointers(c, p, &v0, &v1);
#if DIMENSION==2
double vrot0[2];
double vrot1[2];
vrot0[0] = c->type==PARTICLE_PARTICLE ? -p->omega[p->particles[c->o0].body]*c->r0[1] : 0;
vrot0[1] = c->type==PARTICLE_PARTICLE ? p->omega[p->particles[c->o0].body]*c->r0[0] : 0;
vrot1[0] = -p->omega[p->particles[c->o1].body]*c->r1[1];
vrot1[1] = p->omega[p->particles[c->o1].body]*c->r1[0];
for(int d0 = 0; d0< DIMENSION; ++d0) {
for(int d = 0; d<DIMENSION; d++){
v[d0] += (v1[d]+vrot1[d]-v0[d]-vrot0[d]) * c->basis[d0][d];
}
}
#if DIMENSION==3
double res0[3], omega0[3], res1[3], omega1[3];
for (int d = 0;d<DIMENSION;d++){
omega0[d] = c->type == 0 ? p->omega[p->particles[c->o0].body*DIMENSION+d] : 0;
omega1[d] = p->omega[p->particles[c->o1].body*DIMENSION+d];
}
_cross(omega0,c->r0,res0);
_cross(omega1,c->r1,res1);
for(int d = 0;d<DIMENSION;d++){
for(int i = 0;i<DIMENSION;i++)
v[d] += (res1[i] + res0[i]) * c->basis[d][i];
}
#else
double res0[2];
double res1[2];
res0[0] = c->type==PARTICLE_PARTICLE ? -p->omega[p->particles[c->o0].body]*c->r0[1] : 0;
res0[1] = c->type==PARTICLE_PARTICLE ? p->omega[p->particles[c->o0].body]*c->r0[0] : 0;
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 d0 = 0; d0< DIMENSION; ++d0) {
for(int d = 0; d<DIMENSION; d++){
v[d0] += (res1[d]-res0[d]) * c->basis[d0][d];
}
}
double res0[3], omega0[3], res1[3], omega1[3];
for (int d = 0;d<DIMENSION;d++){
omega0[d] = c->type == 0 ? p->omega[p->particles[c->o0].body*DIMENSION+d] : 0;
omega1[d] = p->omega[p->particles[c->o1].body*DIMENSION+d];
}
_cross(omega0,c->r0,res0);
_cross(omega1,c->r1,res1);
for(int d = 0;d<DIMENSION;d++){
for(int i = 0;i<DIMENSION;i++)
v[d] += (v1[i]+res1[i] -v0[i]-res0[i]) * c->basis[d][i];
}
#endif
}
......@@ -966,12 +962,10 @@ inline static int projection_is_on_segment(const Segment *s, double x[DIMENSION]
static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol) {
int contact_avoided = 0;
Contact oldcontact = *c;
// position prediction with the velocity taking into account the previous contact
// the initialize the contact with those positions
// and revert to the position in n
// then initialize the contact with those positions and revert to the position in n
{
const Particle *p1 = &p->particles[c->o1];
p->bodies[p1->body].theta += dt*p->omega[p1->body];
......@@ -1018,44 +1012,28 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
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 rn0, rn1, rt0, rt1;
_cross(c->r0,c->basis[0],&rn0);
_cross(c->basis[0],c->r1,&rn1);
_cross(c->r0,c->basis[1],&rt0);
_cross(c->basis[1],c->r1,&rt1);
double wnn = c->iinertmom0*rn0*rn0+c->iinertmom1*rn1*rn1 + c->imass0 + c->imass1;
double wtt = c->iinertmom0*rt0*rt0+c->iinertmom1*rt1*rt1 + c->imass0 + c->imass1;
double wnt = c->iinertmom0*rn0*rt0+c->iinertmom1*rn1*rt1;
double wdet = wnn*wtt-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]*wnn-c->D/dt, 0.); // for the normal component, the free velocity that has to be canceled
contact_get_local_velocity(c, p, v); // v[0] < 0 if particles get closer
v[0] = fmin(c->D/dt-c->p[0]*wnn-c->p[1]*wnt, 0.);
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->p[0] == 0.){
for (int d = 1; d < DIMENSION; ++d) {
c->p[d] = 0.;
}
if (contact_avoided || v[0] >0) {
c->p[0] = 0;
c->p[1] = 0;
}
else {
if (v[0]>0) {
c->p[0] = 0;
c->p[1] = 0;
}
if (pstick[1]+c->mu*pstick[0] < 0) {
c->p[0] = -v[0]/(wnn-c->mu*wnt);
c->p[1] = -c->mu*c->p[0];
......@@ -1068,34 +1046,21 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
c->p[0] = pstick[0];
c->p[1] = pstick[1];
}
#if 0
double v[DIMENSION]= {0.}; // local free velocity in the contact basis
contact_get_local_velocity_tangent(c, p, v0, v1, v);
double ptmax = c->mu*fabs(c->dv[0])*wtt/wnn;
if(v[1]<-ptmax){
c->dv[0] /= (1-c->mu*wnt/wnn);
c->dv[1] = -c->mu*fabs(c->dv[0]);
}
else if (v[1]>ptmax){
c->dv[0] /= (1+c->mu*wnt/wnn);
c->dv[1] = c->mu*fabs(c->dv[0]);
}
else{
c->dv[1] = v[1];
}
#endif
}
// apply new contact and check convergence
double pmax = 0;
double dvmax = 0;
#if DIMENSION == 2
double dp[DIMENSION];
for (int d = 0; d <DIMENSION; ++d) {
pmax = fmax(pmax, fabs(c->p[d]-pold[d]));
dp[d] = c->p[d]-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);
return pmax*wnn < tol/dt;
return dvmax < tol/dt;
}
void swap(int *a, int *b) {
......
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