Commit 786cc8d6 authored by Jonathan Lambrechts's avatar Jonathan Lambrechts
Browse files

solve contact in global basis

parent 98163348
......@@ -452,17 +452,6 @@ void contact_global_to_local(const Contact *c, const double *global, double *loc
}
}
void contact_local_to_global(const Contact *c, const double *local, double *global) {
for (int d = 0; d < DIMENSION; ++d) {
global[d] = 0;
for (int i = 0; i < DIMENSION; ++i) {
global[d] += c->basis[i][d]*local[i];
}
}
}
static void contact_apply(const Contact *c, ParticleProblem *p) {
double *v1, *v0;
double dv[DIMENSION];
......@@ -888,13 +877,12 @@ inline static double contact_avoid(Contact *c, ParticleProblem *p)
}
int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol) {
double *v0, *v1;
double dvold[DIMENSION] = {0};
for (int i = 0; i <DIMENSION;++i) {
dvold[i] = c->dv[i];
}
contact_get_velocity_pointers(c, p, &v0, &v1);
double v[DIMENSION]; // local free velocity in the global basis
// local free velocity in the global basis
double v[DIMENSION];
{
double *v0, *v1;
contact_get_velocity_pointers(c, p, &v0, &v1);
for (int i = 0; i<DIMENSION; ++i) {
v[i] = (v0[i]-v1[i]+c->dv[i]);
}
......@@ -919,40 +907,56 @@ int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol) {
}
#endif
#endif
double vn = 0; // todo : rewrite in global basis for perf
}
double dvold[DIMENSION] = {0};
for (int i = 0; i <DIMENSION;++i) {
dvold[i] = c->dv[i];
c->dv[i] = 0;
}
// normal contact solve
double vn = 0;
for (int i = 0; i < DIMENSION; ++i) {
vn += c->basis[0][i]*v[i];
}
double dvlocal[DIMENSION] = {fmax(0., vn - c->D/dt),0};
double dvn = fmax(0., vn - c->D/dt);
for (int i = 0; i < DIMENSION; ++i) {
c->dv[i] += c->basis[0][i]*dvn;
}
// todo : contact_avoid(c, p);
if(c->mu>1e-10 && dvlocal[0] != 0.){
// tangent contact solve
if(c->mu>1e-10 && dvn != 0.){
//remove normal component from free velocity
double vt2 = 0;
for (int i = 0; i < DIMENSION; ++i) {
v[i] -= -c->basis[0][i]*vn;
vt2 += v[i]*v[i];
}
double wnn = c->imass0+c->imass1;
double wtt = c->imass0+c->imass1;
#if ROTATION_ENABLED
wtt += c->iinertmom0 + c->iinertmom1;
#endif
#if DIMENSION == 2
double norm = fabs(v[1]);
#else
double norm = hypot(v[1],v[2]);
#endif
double ptmax = c->mu*fabs(c->dv[0])/wnn;
double pt = norm/wtt;
double ptmax = c->mu*fabs(dvn)/wnn;
double pt = sqrt(vt2)/wtt;
double slip = pt > ptmax ? ptmax/pt : 1;
for (int d = 0; d < DIMENSION-1; ++d) {
c->dv[d+1] = v[d+1]*slip;
for (int d = 0; d < DIMENSION; ++d) {
c->dv[d] += v[d]*slip;
}
}
contact_local_to_global(c,dvlocal, c->dv);
double dvnorm2 = 0;
for (int d = 0; d <DIMENSION; ++d) {
dvnorm2 += (c->dv[d]-dvold[d])*(c->dv[d]-dvold[d]);
}
//apply
coordAdd(c->dv, -1, dvold);
contact_apply(c, p);
coordAdd(c->dv, 1, dvold);
//check convergence
double dvnorm2 = 0;
for (int d = 0; d <DIMENSION; ++d) {
dvnorm2 += (c->dv[d]-dvold[d])*(c->dv[d]-dvold[d]);
}
return dvnorm2 < (tol/dt)*(tol/dt);
}
......
Markdown is supported
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