broken

scontact/scontact.c

@@ -563,10 +563,13 @@ static void contact_apply(const Contact *c, ParticleProblem *p) {
     }
   }
 }
-static void contact_get_local_free_velocity_normal(Contact *c, ParticleProblem *p, 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_velocity_tangent(Contact *c, ParticleProblem *p, const double *v0, const double *v1, double *v) {
+  for (int d = 1; d < DIMENSION; ++d) {
+    v[d] = 0;
+    for (int i = 0; i<DIMENSION; ++i){
+      v[d] += (v0[i]-v1[i]) * c->basis[d][i];
+    }
   }
 #if DIMENSION==3
     double res0[3], omega0[3], res1[3], omega1[3];
@@ -577,27 +580,26 @@ static void contact_get_local_free_velocity_normal(Contact *c, ParticleProblem *
     _cross(omega0,c->r0,res0);
     _cross(omega1,c->r1,res1);
     for(int d = 0;d<DIMENSION;d++){
-      v[0] += (res0[d] + res1[d]) * c->basis[0][d];
+      for(int i = 0;i<DIMENSION;i++)
+        v[d] += (res0[i] + res1[i]) * c->basis[d][i];
     }
 #else
     double res0[2];
     double res1[2];
     res0[0] = c->type==0 ? -p->omega[p->particles[c->o0].body]*c->r0[1] : 0;
     res0[1] = c->type==0 ? 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];
+    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[0] -= (res0[d]+res1[d]) * c->basis[0][d];
+      v[1] += (res0[d]+res1[d]) * c->basis[1][d];
     }
 #endif
-
 }
-
-static void contact_get_local_velocity_tangent(Contact *c, ParticleProblem *p, const double *v0, const double *v1, double *v) {
-  for (int d = 1; d < DIMENSION; ++d) {
+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] += (v0[i]-v1[i]) * c->basis[d][i];
+      v[d] += (v1[i]-v0[i]) * c->basis[d][i];
     }
   }
 #if DIMENSION==3
@@ -610,17 +612,19 @@ static void contact_get_local_velocity_tangent(Contact *c, ParticleProblem *p, c
     _cross(omega1,c->r1,res1);
     for(int d = 0;d<DIMENSION;d++){
       for(int i = 0;i<DIMENSION;i++)
-        v[d] += (res0[i] + res1[i]) * c->basis[d][i];
+        v[d] += (res1[i] + res0[i]) * c->basis[d][i];
     }
 #else
     double res0[2];
     double res1[2];
-    res0[0] = c->type==0 ? -p->omega[p->particles[c->o0].body]*c->r0[1] : 0;
-    res0[1] = c->type==0 ? 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 d = 0;d<DIMENSION;d++){
-      v[1] += (res0[d]+res1[d]) * c->basis[1][d];
+    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];
+      }
     }
 #endif
 }
@@ -1024,12 +1028,13 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
   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];
   }
   contact_apply(&oldcontact, p);
+
+  //solve the tangeant contact
   if(c->mu<1e-10 || c->dv[0] == 0.){
     for (int d = 1; d < DIMENSION; ++d) {
       c->dv[d] = 0.;
@@ -1051,27 +1056,51 @@ static 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]= {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];
-         }
+    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 (pstick[1]+c->mu*pstick[0] < 0) {
+      p[0] = -v[0]/(wnn-c->mu*wnt);
+      p[1] = -c->mu*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];
+    }
+    else {
+      p[0] = pstick[0];
+      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 dvmax = 0;
   for (int d = 0; d <DIMENSION; ++d) {
-    //if (d>=1) dvold[d] = 0;
     dvmax = fmax(dvmax, fabs(c->dv[d]-dvold[d]));
   }
   contact_apply(c, p);