minor cleaning

scontact/scontact.c

@@ -936,6 +936,17 @@ inline static int projection_is_on_segment(const Segment *s, double x[DIMENSION]
   return r;
 }
 
+static void body_advance(Body *b, double dt) {
+#if DIMENSION == 2
+  b->theta += dt*b->omega;
+#else
+  //TODO
+#endif
+  for (int d = 0; d < DIMENSION; ++d) {
+    b->position[d] += dt*b->velocity[d];
+  }
+}
+
 
 static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol) {
   // position prediction with the velocity taking into account the previous contact
@@ -948,15 +959,7 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
   double r0[DIMENSION], r1[DIMENSION];
   {
     const Particle *p1 = &p->particles[c->o1];
-    Body *body1 = &p->bodies[p1->body];
-#if DIMENSION == 2
-    body1->theta += dt*body1->omega;
-#else
-    //todo
-#endif
-    for (int d = 0; d < DIMENSION; ++d) {
-      body1->position[d] += dt*body1->velocity[d];
-    }
+    body_advance(&p->bodies[p1->body], dt);
     double x1[DIMENSION];
     particle_get_position(p, p1, x1);
     if (c->type == PARTICLE_PARTICLE) {
@@ -966,26 +969,11 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
       iinertmom0 = p->bodies[p0->body].invertI;
       iinertmom1 = p->bodies[p1->body].invertI;
       mu = particle_problem_get_mu(p,p0->material,p1->material);
+      body_advance(&p->bodies[p0->body], dt);
       double x0[DIMENSION];
-      Body *body0 = &p->bodies[p0->body];
-#if DIMENSION == 2
-      body0->theta += dt*body0->omega;
-#else
-    //todo
-#endif
-      for (int d = 0; d < DIMENSION; ++d) {
-        body0->position[d] += dt*body0->velocity[d];
-      }
       particle_get_position(p, p0, x0);
       D = contact_update_particle_particle(c, p, x0, x1, basis, r0, r1);
-#if DIMENSION == 2
-      body0->theta -= dt*body0->omega;
-#else
-    //todo
-#endif
-      for (int d = 0; d < DIMENSION; ++d) {
-        body0->position[d] -= dt*body0->velocity[d];
-      }
+      body_advance(&p->bodies[p0->body], -dt);
     }
     else {
       imass0 = 0;
@@ -1002,14 +990,7 @@ static int contact_solve(ParticleProblem *p, Contact *c, double dt, double tol)
       D = contact_update_segment_particle(c, p, x1, basis, r0, r1);
       if (!projection_is_on_segment(&p->segments[c->o0], x1)) contact_avoided = 1;
     }
-#if DIMENSION == 2
-    body1->theta -= dt*body1->omega;
-#else
-    //todo
-#endif
-    for (int d = 0; d < DIMENSION; ++d) {
-      body1->position[d] -= dt*body1->velocity[d];
-    }
+    body_advance(&p->bodies[p1->body], -dt);
   }
   // allow the pre-existing (from previous time steps) interpenetrations
   D -= c->D0;