fix

scontact/scontact.c

@@ -507,10 +507,6 @@ static void contact_get_local_free_velocity_tangent(Contact *c, ParticleProblem
 }
 
 
-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)++;
@@ -836,19 +832,18 @@ static void fifoFree(Fifo *f) {
   free(f->data);
   free(f);
 }
-inline static double contact_avoid(Contact *c, ParticleProblem *p)
+inline static void contact_avoid(Contact *c, ParticleProblem *p, double vnfree)
 {
-  double *x, *n, vnfree, *vbnd, dv;
+  double *x = &p->position[c->o1*DIMENSION];
   if (c->type == PARTICLE_PARTICLE)
-    return dv;
+    return;
   if (c->type == PARTICLE_DISK) {
     const Disk *d = &p->disks[c->o0];;
     for (int i = 0; i < DIMENSION; ++i) {
-      vbnd[i] = d->v[i];
-      vnfree -= vbnd[i] * n[i];
+      vnfree -= d->v[i] * c->basis[0][i];
     }
-    if (dv > fabs(vnfree)) dv = fabs(vnfree);
-    return dv;
+    if (c->dv[0] > fabs(vnfree)) c->dv[0] = fabs(vnfree);
+    return;
   }
   if (c->type == PARTICLE_SEGMENT) {
     const Segment *s = &p->segments[c->o0];
@@ -858,14 +853,17 @@ inline static double contact_avoid(Contact *c, ParticleProblem *p)
       alpha += (x[i] - s->p[0][i]) * d;
       beta  += (x[i] - s->p[1][i]) * d;
     }
-    if (alpha < 0 || beta > 0) return 0;
+    if (alpha < 0 || beta > 0) {
+      c->dv[0] = 0;
+      return;
+    }
     double xi = alpha/(alpha-beta);
     for (int i = 0; i < DIMENSION; ++i) {
-      vbnd[i] = s->v[i];//(1-xi)*s->v[0][i] + xi*s->v[1][i];
-      vnfree -= vbnd[i] * n[i];
+      //vbnd[i] = s->v[i];//(1-xi)*s->v[0][i] + xi*s->v[1][i];
+      vnfree -= s->v[i] * c->basis[0][i];
     }
-    if (dv > fabs(vnfree)) dv = fabs(vnfree);
-    return dv;
+    if (c->dv[0] > fabs(vnfree)) c->dv[0] = fabs(vnfree);
+    return;
   }
 #if DIMENSION == 3
   if (c->type == PARTICLE_TRIANGLE) {
@@ -889,13 +887,16 @@ inline static double contact_avoid(Contact *c, ParticleProblem *p)
       _cross(dx[(i + 1) % 3], dx[(i + 2) %3], d);
       alpha[i] = dot(tn, d)/(tn[0]*tn[0] + tn[1]*tn[1] + tn[2]*tn[2]);
     }
-    if (!((alpha[0] <= 0 && alpha[1] <= 0 &&  alpha[2]<= 0) || (alpha[0] >= 0 && alpha[1] >= 0 && alpha[2] >= 0))) return 0;
+    if (!((alpha[0] <= 0 && alpha[1] <= 0 &&  alpha[2]<= 0) || (alpha[0] >= 0 && alpha[1] >= 0 && alpha[2] >= 0))) {
+      c->dv[0] = 0;
+      return;
+    }
     for (int i = 0; i < DIMENSION; ++i) {
-      vbnd[i] = t->v[i];//alpha[0]*t->v[0][i] + alpha[1]*t->v[1][i] + alpha[2]*t->v[2][i];
-      vnfree -= vbnd[i] * n[i];
+      //vbnd[i] = t->v[i];//alpha[0]*t->v[0][i] + alpha[1]*t->v[1][i] + alpha[2]*t->v[2][i];
+      vnfree -= t->v[i] * c->basis[0][i];
     }
-    if (dv > fabs(vnfree)) dv = fabs(vnfree);
-    return dv;
+    if (c->dv[0] > fabs(vnfree)) c->dv[0] = fabs(vnfree);
+    return;
   }
 #endif
   printf("unknown contact type !\n");
@@ -911,7 +912,7 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
   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);
+  contact_avoid(c, p, v[0]);
   double dvmax = fabs(c->dv[0]-dvold[0]);
   if(c->mu<1e-10 || c->dv[0] == 0.){
     for (int d = 1; d < DIMENSION; ++d) {