Commit 98163348 authored by Jonathan Lambrechts's avatar Jonathan Lambrechts
Browse files

c->dv in global axis

parent b24a59e5
......@@ -443,12 +443,34 @@ static void contact_get_velocity_pointers(const Contact *c, ParticleProblem *p,
#endif
}
void contact_global_to_local(const Contact *c, const double *global, double *local) {
for (int d = 0; d < DIMENSION; ++d) {
local[d] = 0;
for (int i = 0; i < DIMENSION; ++i) {
local[d] += c->basis[d][i]*global[i];
}
}
}
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];
contact_global_to_local(c,c->dv,dv); // todo rewrite this function in global basis
contact_get_velocity_pointers(c, p, &v0, &v1);
double wnn = c->imass0 + c->imass1;
coordAdd(v0, -c->dv[0]*c->imass0/wnn, c->basis[0]);
coordAdd(v1, c->dv[0]*c->imass1/wnn, c->basis[0]);
coordAdd(v0, -dv[0]*c->imass0/wnn, c->basis[0]);
coordAdd(v1, dv[0]*c->imass1/wnn, c->basis[0]);
double wtt = wnn;
#if ROTATION_ENABLED
wtt += c->iinertmom0 + c->iinertmom1;
......@@ -456,61 +478,24 @@ static void contact_apply(const Contact *c, ParticleProblem *p) {
contact_get_omega_pointers(c, p, &omega0, &omega1, &ir0, &ir1);
#if DIMENSION == 2
if(omega0)
omega0[0] -= ir0*c->dv[1]*c->iinertmom0/wtt;
omega1[0] -= ir1*c->dv[1]*c->iinertmom1/wtt;
omega0[0] -= ir0*dv[1]*c->iinertmom0/wtt;
omega1[0] -= ir1*dv[1]*c->iinertmom1/wtt;
#else
for (int i = 0; i<3; i++){
if(omega0)
omega0[i] += ir0*(c->dv[2]*c->basis[1][i]-c->dv[1]*c->basis[2][i])*c->iinertmom0/wtt;
omega1[i] += ir1*(c->dv[2]*c->basis[1][i]-c->dv[1]*c->basis[2][i])*c->iinertmom1/wtt;
omega0[i] += ir0*(dv[2]*c->basis[1][i]-dv[1]*c->basis[2][i])*c->iinertmom0/wtt;
omega1[i] += ir1*(dv[2]*c->basis[1][i]-dv[1]*c->basis[2][i])*c->iinertmom1/wtt;
}
#endif
#endif
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 (dv[d+1] != 0) {
coordAdd(v0, -dv[d+1]*c->imass0/wtt, c->basis[d+1]);
coordAdd(v1, dv[d+1]*c->imass1/wtt, c->basis[d+1]);
}
}
}
static void contact_get_local_free_velocity_normal(Contact *c, const double *v0, const double *v1, double *v) {
v[0] = c->dv[0];
for (int i = 0; i<DIMENSION; ++i)
v[0] += (v0[i]-v1[i]) * c->basis[0][i];
}
static void contact_get_local_free_velocity_tangent(Contact *c, ParticleProblem *p, const double *v0, const double *v1, double *v) {
for (int d = 1; d < DIMENSION; ++d) {
v[d] = c->dv[d];
for (int i = 0; i<DIMENSION; ++i)
v[d] += (v0[i]-v1[i]) * c->basis[d][i];
}
#if ROTATION_ENABLED
double *omega0, *omega1, ir0, ir1;
contact_get_omega_pointers(c, p, &omega0, &omega1, &ir0, &ir1);
#if DIMENSION==2
if(omega0)
v[1] += omega0[0]*c->r0;
v[1] += omega1[0]*c->r1;
#else
double res0[3]={0.}, res1[3];
if(omega0)
_cross(omega0, c->basis[0], res0);
_cross(omega1, c->basis[0], res1);
for(int i = 0;i<DIMENSION;i++){
v[1] += (res1[i]*c->r1+res0[i]*c->r0)*c->basis[1][i];
v[2] += (res1[i]*c->r1+res0[i]*c->r0)*c->basis[2][i];
}
#endif
#endif
}
typedef double IsInsideFunc(const void*, const double *x, const double *n, double vnfree, double *vbnd, double dv);
const Contact *findContactSorted(Contact *c, const Contact *v, size_t *i) {
while (*i < vectorSize(v) && (v[*i].type < c->type || v[*i].o0 < c->o0 || (v[*i].o0 == c->o0 && v[*i].o1 < c->o1)))
(*i)++;
......@@ -902,24 +887,45 @@ inline static double contact_avoid(Contact *c, ParticleProblem *p)
exit(1);
}
static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol) {
int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol) {
double *v0, *v1;
contact_get_velocity_pointers(c, p, &v0, &v1);
double v[DIMENSION]; // local free velocity in the contact basis
double dvold[DIMENSION] = {0};
coordAdd(dvold,1,c->dv);
contact_get_local_free_velocity_normal(c, v0, v1, v);
c->dv[0] = fmax(0., v[0] - c->D/dt);
contact_avoid(c, p);
double dvmax = fabs(c->dv[0]-dvold[0]);
if(c->mu<1e-10 || c->dv[0] == 0.){
for (int d = 1; d < DIMENSION; ++d) {
c->dv[d] = 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
for (int i = 0; i<DIMENSION; ++i) {
v[i] = (v0[i]-v1[i]+c->dv[i]);
}
#if ROTATION_ENABLED
// todo : here we assume r*n is the from the center of mass which is only true for disc/sphere
double *omega0, *omega1, ir0, ir1;
contact_get_omega_pointers(c, p, &omega0, &omega1, &ir0, &ir1);
#if DIMENSION==2
if(omega0){
v[0] -= omega0[0]*c->r0*c->basis[0][1];
v[1] += omega0[0]*c->r0*c->basis[0][0];
}
v[0] -= omega1[0]*c->r1*c->basis[0][1];
v[1] += omega1[0]*c->r1*c->basis[0][0];
#else
double res0[3]={0.}, res1[3];
if(omega0)
_cross(omega0, c->basis[0], res0);
_cross(omega1, c->basis[0], res1);
for(int i = 0;i<DIMENSION;i++){
v[i] += res1[i]*c->r1+res0[i]*c->r0;
}
#endif
#endif
double vn = 0; // todo : rewrite in global basis for perf
for (int i = 0; i < DIMENSION; ++i) {
vn += c->basis[0][i]*v[i];
}
else {
contact_get_local_free_velocity_tangent(c, p, v0, v1, v);
double dvlocal[DIMENSION] = {fmax(0., vn - c->D/dt),0};
// todo : contact_avoid(c, p);
if(c->mu>1e-10 && dvlocal[0] != 0.){
double wnn = c->imass0+c->imass1;
double wtt = c->imass0+c->imass1;
#if ROTATION_ENABLED
......@@ -937,19 +943,17 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
c->dv[d+1] = v[d+1]*slip;
}
}
for (int d = 1; d <DIMENSION; ++d) {
dvmax = fmax(dvmax, fabs(c->dv[d]-dvold[d]));
}
if (dvmax != 0.) {
coordAdd(c->dv, -1, dvold);
contact_apply(c, p);
coordAdd(c->dv, 1, dvold);
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]);
}
#if ROTATION_ENABLED
double *omega0, *omega1, n0, n1;
contact_get_omega_pointers(c,p,&omega0,&omega1,&n0,&n1);
#endif
return dvmax < tol/dt;
coordAdd(c->dv, -1, dvold);
contact_apply(c, p);
coordAdd(c->dv, 1, dvold);
return dvnorm2 < (tol/dt)*(tol/dt);
}
void swap(int *a, int *b) {
......
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