seems ok in 2D

python/scontact.py

@@ -99,8 +99,6 @@ class ParticleProblem :
         """
         self._dim = dim
         self._friction_enabled = friction_enabled
-        if not friction_enabled :
-            rotation_enabled = False;
         self._rotation_enabled = rotation_enabled
         if dim == 2 :
             self._lib = lib2 if rotation_enabled else lib2fwr
@@ -143,6 +141,10 @@ class ParticleProblem :
         """Returns the list of body masses."""
         return self._get_matrix("Body", 2)[:, 1][:,None]
 
+    def invert_inertia(self):
+        """Returns the list of body masses."""
+        return self._get_matrix("Body", 2)[:, 0][:,None]
+
     def position(self):
         return self._get_matrix("Position", self._dim)
 
@@ -162,7 +164,7 @@ class ParticleProblem :
 
     def omega(self) : 
         d = 1 if self._dim==2 else 3
-        if self._friction_enabled and self._rotation_enabled :
+        if self._rotation_enabled :
             return(self._get_matrix("Omega", d))
         else :
             return np.zeros((self.n_bodies(),d))
@@ -605,7 +607,7 @@ class ParticleProblem :
 
         x = np.sum(positions*masses.reshape((-1,1)),axis=0)/np.sum(masses)
         I = np.sum(masses.reshape((-1,1))*radii.reshape((-1,1))**2/2 + np.sum((positions-x)**2,axis=1)*masses.reshape((-1,1)))
-        self._lib.particleProblemAddBody(self._p, self._coord_type(*x), c_double(np.sum(masses)), c_double(I),material.encode(),c_int(forced))
+        self._lib.particleProblemAddBody(self._p, self._coord_type(*x), c_double(np.sum(masses)), c_double(1/I),material.encode(),c_int(forced))
         for i in range(positions.shape[0]):
           self.add_particle(positions[i,:]-x,radii[i],idd)
 

scontact/scontact.c

@@ -88,7 +88,7 @@ struct _Particle{
 };
 
 struct _Body{
-  double I;
+  double invertI;
   double m;
 };
 
@@ -209,8 +209,8 @@ static int contact_init_particle_particle (Contact *c, const ParticleProblem *p,
   c->imass0 = p->ForcedFlag[p0->body] ? 0. : 1/p->bodies[p0->body].m;
   c->imass1 = p->ForcedFlag[p1->body] ? 0. : 1/p->bodies[p1->body].m;
 #if ROTATION_ENABLED
-  c->iinertmom0 = (!p->ForcedFlag[p0->body]) ? 1/(p->bodies[p0->body].I) : 0;
-  c->iinertmom1 = (!p->ForcedFlag[p1->body]) ? 1/(p->bodies[p1->body].I) : 0;
+  c->iinertmom0 = (!p->ForcedFlag[p0->body]) ? p->bodies[p0->body].invertI : 0;
+  c->iinertmom1 = (!p->ForcedFlag[p1->body]) ? p->bodies[p1->body].invertI : 0;
   for (int i = 0;i<DIMENSION;i++){
     c->r1[i] = (p1->x[i] - c->basis[0][i]*p1->r);
     c->r0[i] =  (p0->x[i] + c->basis[0][i]*p0->r);
@@ -279,7 +279,7 @@ static int contact_init_disk_particle(Contact *c, ParticleProblem *p, size_t dis
   c->imass1 = 1/p->bodies[particle->body].m;
 #if ROTATION_ENABLED
   c->iinertmom0 = 0;
-  c->iinertmom1 = (!p->ForcedFlag[p->particles[particle_id].body]) ? 1/p->bodies[particle->body].I : 0;
+  c->iinertmom1 = (!p->ForcedFlag[p->particles[particle_id].body]) ? p->bodies[particle->body].invertI : 0;
 
   for (int i = 0;i<DIMENSION;i++){
     c->r0[i] = 0;
@@ -356,7 +356,7 @@ static int contact_init_segment_particle(Contact *c, ParticleProblem *p, size_t
   c->imass1 = 1/p->bodies[particle->body].m;
 #if ROTATION_ENABLED
   c->iinertmom0 = 0;
-  c->iinertmom1 = (!p->ForcedFlag[p->particles[particle_id].body]) ? 1/p->bodies[particle->body].I: 0;
+  c->iinertmom1 = (!p->ForcedFlag[p->particles[particle_id].body]) ? p->bodies[particle->body].invertI: 0;
   for (int i = 0;i<DIMENSION;i++){
     c->r0[i] = 0;
     c->r1[i] = (particle->x[i] - c->basis[0][i]*particle->r);
@@ -523,7 +523,7 @@ static void contact_apply(const Contact *c, ParticleProblem *p) {
   result[1] = (c->dv[0]/wnn)*c->basis[0][1] + (c->dv[1]/wtt)*c->basis[1][1];
   if(omega0)
     omega0[0] += c->iinertmom0*(result[0]*c->r0[1] - result[1]*c->r0[0]);
-  omega1[0] +=  c->iinertmom1*(result[0]*c->r1[1] - result[1]*c->r1[0]);
+  omega1[0] -= c->iinertmom1*(result[0]*c->r1[1] - result[1]*c->r1[0]);
 #else
 //TODO
 #endif
@@ -562,7 +562,7 @@ static void contact_get_local_free_velocity_normal(Contact *c, ParticleProblem *
     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[0] -= (res0[d]+res1[d]) * c->basis[0][d];
     }
 #endif
 #endif
@@ -597,7 +597,7 @@ static void contact_get_local_free_velocity_tangent(Contact *c, ParticleProblem
     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];
+      v[1] -= (res0[d]+res1[d]) * c->basis[1][d];
     }
 #endif
 #endif
@@ -950,12 +950,10 @@ inline static void contact_avoid(Contact *c, ParticleProblem *p, double vnfree)
     double alpha = 0, beta = 0;
     for (int i = 0; i < DIMENSION; ++i) {
       const double d = s->p[1][i] - s->p[0][i];
-      printf("pos abs : %f\n",x[i] + p->position[body*DIMENSION+i]);
       alpha += (x[i] + p->position[body*DIMENSION+i] - s->p[0][i]) * d;
       beta  += (x[i] + p->position[body*DIMENSION+i] - s->p[1][i]) * d;
     }
     if (alpha < 0 || beta > 0) {
-      
       c->dv[0] = 0;
       return;
     }
@@ -1024,7 +1022,6 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
     }
   }
   else {
-    
     contact_get_local_free_velocity_tangent(c, p, v0, v1, v);
     double wnn = c->imass0+c->imass1;
     double wtt = wnn;
@@ -1500,11 +1497,11 @@ void particleProblemAddParticle(ParticleProblem *p, const double x[DIMENSION], d
   }
 }
 
-void particleProblemAddBodyMaterialTagId(ParticleProblem *p, const double x[DIMENSION], double m, double I, int tag, int forced){
+void particleProblemAddBodyMaterialTagId(ParticleProblem *p, const double x[DIMENSION], double m, double invertI, int tag, int forced){
   size_t n = vectorSize(p->bodies);
   Body *body = vectorPush(&p->bodies);
   body->m = m;
-  body->I = I;
+  body->invertI = invertI;
   vectorPushN(&p->position, DIMENSION);
   vectorPushN(&p->velocity, DIMENSION);
   vectorPush(&p->bodyMaterial);

testcases/bodies/test.py

@@ -6,7 +6,7 @@ os.makedirs("output",exist_ok=True)
 
 p = scontact.ParticleProblem(2,friction_enabled=True,rotation_enabled=True)
 a = 1
-g = np.array([0,-9.81])
+g = np.array([0,0])
 r = 0.05
 rho = 1000
 p.add_boundary_segment([-a,-a],[-a,a],"bnd",material="Steel")
@@ -21,7 +21,13 @@ y = np.arange(-10*r,a-2*r,3*r)
     #p.add_body(np.array([[x[i],y[j]]]),R,np.pi*R**2*rho,material="Sand")
 
 
-p.add_body(np.array([[0,5*r],[1.5*r,6.5*r],[3*r,8*r]]),R,np.pi*R**2*rho,material="Sand",forced=0)
+D = r*2**0.5
+#p.add_body(np.array([[0,5*r],[D,5*r+D],[2*D,5*r+2*D]]),R,np.pi*R**2*rho,material="Sand",forced=0)
+p.add_body(np.array([[0,5*r],[0,7*r],[0,9*r]]),R,np.pi*R**2*rho,material="Sand",forced=0)
+V = 4
+T = (4*r+1)/V
+#p.invert_inertia()[0,0] = 10
+#p.mass()[0,0] = 1
 #p.add_body(np.array([[1.5*r,9*r],[3*r,9*r]]),R,np.pi*R**2*rho,material="Sand",forced=0)
 #p.add_body(np.array([[0,0*r],[1.5*r,0*r]]),R,np.pi*R**2*rho,material="Sand",forced=0)
 #p.add_body(np.array([[0,-2*r],[1.5*r,-2*r]]),R,np.pi*R**2*rho,material="Sand",forced=0)
@@ -32,19 +38,19 @@ p.add_body(np.array([[0,5*r],[1.5*r,6.5*r],[3*r,8*r]]),R,np.pi*R**2*rho,material
 #p.add_body(np.array([[0,4*r]]),R,np.pi*R**2*rho,material="Sand",forced=1)
 #p.add_body(np.array([[0,-4*r]]),R,np.pi*R**2*rho,material="Sand",forced=1)
 #p.add_body(np.array([[0,9*r]]),R,np.pi*R**Sand*2,rho="material",forced=0)
-
-tend=0.592
+tend=30
 p.set_friction_coefficient(.2,"Sand","Sand")
 p.set_friction_coefficient(.5,"Sand","Steel")
 i = 0
 f = g*(np.pi*p.r()**2*rho)
 p.write_vtk("output",0,0)
-dt = 0.001
-outf=1
+dt = 0.01
+outf=5
 t = 0
+p.velocity()[0,1] = -V
+p.omega()[0,0] = 2*np.pi/(T*1.5)
 while t<tend :
-    print(i)
-    print(p.omega())
+    print("Ec = ", (p.velocity()[0,1]**2*p.mass()[0,0] + p.omega()[0,0]**2/p.invert_inertia())/2)
     t += dt
     p.iterate(dt,f,tol=1e-8,force_motion=1)
     if i %outf == 0 :

validation/inclined2d/inclined2d.py

@@ -80,3 +80,4 @@ class Inclined2d(unittest.TestCase) :
         print(vit-ref)
         error =  max(abs(ref - np.array(vit))/ref)
         self.assertLess(error,.01/100., "error is too large in inclined2d")
+        self.assertLess(p.omega()[0,0], 0, "Omega should be negative")