Commit 535161da authored by Jonathan Lambrechts's avatar Jonathan Lambrechts
Browse files

wip

parent a5478694
Pipeline #9352 failed with stages
in 1 minute
......@@ -894,8 +894,12 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
}
// allow the pre-existing (from previous time steps) interpenetrations
D -= c->D0;
double rold[DIMENSION] = {0};
double old_dv[2]= {
body1->velocity[0]-body1->omega*c->r1[1]-body0->velocity[0]+body0->omega*c->r0[1],
body1->velocity[1]+body1->omega*c->r1[0]-body0->velocity[1]-body0->omega*c->r0[0]
};
// un-apply the old contact
double rold[DIMENSION] = {0};
for (int d = 0; d < DIMENSION; ++d) {
rold[d] = c->reaction[d];
}
......@@ -911,28 +915,23 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
}
double rn0 = c->r0[0]*ny-c->r0[1]*nx; // r0 x n
double rn1 = -c->r1[0]*ny+c->r1[1]*nx; // -r1 x n
double wnn = body0->invertI*rn0*rn0+body1->invertI*rn1*rn1 + body0->invertM + body1->invertM;
double rt0 = c->r0[0]*nx+c->r0[1]*ny; // r0 x t
double rt1 = -c->r1[0]*nx-c->r1[1]*ny; // -r1 x t
double wnn = body0->invertI*rn0*rn0+body1->invertI*rn1*rn1 + body0->invertM + body1->invertM;
double wtt = body0->invertI*rt0*rt0+body1->invertI*rt1*rt1 + body0->invertM + body1->invertM;
double wnt = body0->invertI*rn0*rt0+body1->invertI*rn1*rt1;
double drmax = 0, drmax2;
double oldrn = rold[0]*nx + rold[1]*ny;
double drmax = 0;
double oldrt = -rold[0]*ny + rold[1]*nx;
if (mu < 1e-12) {
double vn = fmin(D/dt-dt*(oldrn*wnn), 0.);
double pn = -vn/wnn;
c->reaction[0] = pn*nx/dt;
c->reaction[1] = pn*ny/dt;
double dr[DIMENSION];
for (int d = 0; d <DIMENSION; ++d) {
dr[d] = c->reaction[d]-rold[d];
}
drmax = fmax(fabs(dr[0]*wnn+dr[1]*wnt),fabs(dr[0]*wnt+dr[1]*wtt));
double rn = -D/(dt*dt*wnn)+oldrn+oldrt*wnt/wnn;
if (rn <0) rn = 0;
c->reaction[0] = rn*nx;
c->reaction[1] = rn*ny;
}
else {
double wdet = wnn*wtt-wnt*wnt;
double v[DIMENSION]= {0.}; // local free velocity in the contact basis
double oldrt = -rold[0]*ny + rold[1]*nx;
double vrot0[2] = {-body0->omega*c->r0[1], body0->omega*c->r0[0]};
double vrot1[2] = {-body1->omega*c->r1[1], body1->omega*c->r1[0]};
v[0] = fmin(D/dt-dt*(oldrn*wnn+oldrt*wnt), 0.);
......@@ -960,12 +959,6 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
}
c->reaction[0] = (pn*nx-pt*ny)/dt;
c->reaction[1] = (pn*ny+pt*nx)/dt;
// apply new contact and check convergence
double dr[DIMENSION];
for (int d = 0; d <DIMENSION; ++d) {
dr[d] = c->reaction[d]-rold[d];
}
drmax = fmax(fabs(dr[0]*wnn+dr[1]*wnt),fabs(dr[0]*wnt+dr[1]*wtt));
}
for (int i = 0; i < DIMENSION; ++i) {
......@@ -974,7 +967,12 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
}
body0->omega -= body0->invertI*(c->r0[0]*(dt*c->reaction[1])-c->r0[1]*(dt*c->reaction[0]));
body1->omega += body1->invertI*(c->r1[0]*(dt*c->reaction[1])-c->r1[1]*(dt*c->reaction[0]));
return drmax*dt*dt < tol;
double new_dv[2]= {
body1->velocity[0]-body1->omega*c->r1[1]-body0->velocity[0]+body0->omega*c->r0[1],
body1->velocity[1]+body1->omega*c->r1[0]-body0->velocity[1]-body0->omega*c->r0[0]
};
double dv = hypot(new_dv[0]-old_dv[0], new_dv[1]-old_dv[1]);
return dv*dt < tol;
}
void swap(int *a, int *b) {
......
......@@ -4,7 +4,7 @@ import os
import numpy as np
odir = "output"
friction=True
friction=False
os.makedirs(odir,exist_ok=True)
pieces = np.genfromtxt("pieces.txt")
pieces_omega = pieces[:,0]
......
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