c->dv in global axis

scontact/scontact.c

@@ -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) {