wip

scontact/scontact.c

@@ -894,8 +894,12 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
   }
   // allow the pre-existing (from previous time steps) interpenetrations
   D -= c->D0;
-  double rold[DIMENSION] = {0};
+  double old_dv[2]= {
+     body1->velocity[0]-body1->omega*c->r1[1]-body0->velocity[0]+body0->omega*c->r0[1],
+     body1->velocity[1]+body1->omega*c->r1[0]-body0->velocity[1]-body0->omega*c->r0[0]
+  };
   // un-apply the old contact
+  double rold[DIMENSION] = {0};
   for (int d = 0; d < DIMENSION; ++d) {
     rold[d] = c->reaction[d];
   }
@@ -911,28 +915,23 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
   }
   double rn0 = c->r0[0]*ny-c->r0[1]*nx; // r0 x n
   double rn1 = -c->r1[0]*ny+c->r1[1]*nx; // -r1 x n
+  double wnn = body0->invertI*rn0*rn0+body1->invertI*rn1*rn1 + body0->invertM + body1->invertM;
   double rt0 = c->r0[0]*nx+c->r0[1]*ny;  // r0 x t
   double rt1 = -c->r1[0]*nx-c->r1[1]*ny; // -r1 x t
-  double wnn = body0->invertI*rn0*rn0+body1->invertI*rn1*rn1 + body0->invertM + body1->invertM;
   double wtt = body0->invertI*rt0*rt0+body1->invertI*rt1*rt1 + body0->invertM + body1->invertM;
   double wnt = body0->invertI*rn0*rt0+body1->invertI*rn1*rt1;
+  double drmax = 0, drmax2;
   double oldrn = rold[0]*nx + rold[1]*ny;
-  double drmax = 0;
+  double oldrt = -rold[0]*ny + rold[1]*nx;
   if (mu < 1e-12) {
-    double vn = fmin(D/dt-dt*(oldrn*wnn), 0.);
-    double pn = -vn/wnn;
-    c->reaction[0] = pn*nx/dt;
-    c->reaction[1] = pn*ny/dt;
-    double dr[DIMENSION];
-    for (int d = 0; d <DIMENSION; ++d) {
-      dr[d] = c->reaction[d]-rold[d];
-    }
-    drmax = fmax(fabs(dr[0]*wnn+dr[1]*wnt),fabs(dr[0]*wnt+dr[1]*wtt));
+    double rn = -D/(dt*dt*wnn)+oldrn+oldrt*wnt/wnn;
+    if (rn <0) rn = 0;
+    c->reaction[0] = rn*nx;
+    c->reaction[1] = rn*ny;
   }
   else {
     double wdet = wnn*wtt-wnt*wnt;
     double v[DIMENSION]= {0.}; // local free velocity in the contact basis
-    double oldrt = -rold[0]*ny + rold[1]*nx;
     double vrot0[2] = {-body0->omega*c->r0[1], body0->omega*c->r0[0]};
     double vrot1[2] = {-body1->omega*c->r1[1], body1->omega*c->r1[0]};
     v[0] = fmin(D/dt-dt*(oldrn*wnn+oldrt*wnt), 0.);
@@ -960,12 +959,6 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
     }
     c->reaction[0] = (pn*nx-pt*ny)/dt;
     c->reaction[1] = (pn*ny+pt*nx)/dt;
-    // apply new contact and check convergence
-    double dr[DIMENSION];
-    for (int d = 0; d <DIMENSION; ++d) {
-      dr[d] = c->reaction[d]-rold[d];
-    }
-    drmax = fmax(fabs(dr[0]*wnn+dr[1]*wnt),fabs(dr[0]*wnt+dr[1]*wtt));
   }
   
   for (int i = 0; i < DIMENSION; ++i) {
@@ -974,7 +967,12 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
   }
   body0->omega -= body0->invertI*(c->r0[0]*(dt*c->reaction[1])-c->r0[1]*(dt*c->reaction[0]));
   body1->omega += body1->invertI*(c->r1[0]*(dt*c->reaction[1])-c->r1[1]*(dt*c->reaction[0]));
-  return drmax*dt*dt < tol;
+  double new_dv[2]= {
+     body1->velocity[0]-body1->omega*c->r1[1]-body0->velocity[0]+body0->omega*c->r0[1],
+     body1->velocity[1]+body1->omega*c->r1[0]-body0->velocity[1]-body0->omega*c->r0[0]
+  };
+  double dv = hypot(new_dv[0]-old_dv[0], new_dv[1]-old_dv[1]);
+  return dv*dt < tol;
 }
 
 void swap(int *a, int *b) {

testcases/bodies/tetris.py

@@ -4,7 +4,7 @@ import os
 import numpy as np
 
 odir = "output"
-friction=True
+friction=False
 os.makedirs(odir,exist_ok=True)
 pieces = np.genfromtxt("pieces.txt")
 pieces_omega = pieces[:,0]