solve contact in global basis

scontact/scontact.c

@@ -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);
 }