[ODE] patch - position-correcting constraints

[Adam D. Moss]

Hi.

This EXPERIMENTAL patch attempts to resolve penetrations by
altering body positions and orientations instead of altering
their velocities.

This has the effect of almost entirely eliminating the fake-
bounce and explosions caused by interpenetrating geoms.

Someone might be interested in testing or improving it.  It
only affects QuickStep.

Three warnings:
1) This method seems much more sensitive to redundant
constraints/contacts - perhaps someone who does contact-merging
can comment on how much contact-merging helps.  A lower step
size also helps (more microsteps don't help).
2) Bodies need higher thresholds to autodisable.  I'm not sure
why, since there should be less velocity in the system now
rather than more.
3) The patch isn't particularly clean, and does have room for
optimisation.  Not really meant to permanantly go into anyone's tree.

--Adam
-- 
Adam D. Moss   -   adam at gimp.org
-------------- next part --------------
Index: ../src/quickstep.cpp
===================================================================
--- ../src/quickstep.cpp	(revision 4097)
+++ ../src/quickstep.cpp	(working copy)
@@ -50,6 +50,12 @@
 #define WARM_STARTING 1
 
 
+// XX adam - try to mostly fulfill constraints by affecting body
+// position rather than body velocity, to somewhat eliminate undesirable
+// fake-bounce.
+#define POSITIONAL_CORRECTION 1
+
+
 // for the SOR method:
 // uncomment the following line to determine a new constraint-solving
 // order for each iteration. however, the qsort per iteration is expensive,
@@ -559,6 +565,12 @@
 #endif
 	}
 
+        // ADAM: blank clvel,cavel;
+	for (i=0; i<nb; i++) {
+          dSetZero (body[i]->clvel,3);
+          dSetZero (body[i]->cavel,3);
+        }
+
 	// add the gravity force to all bodies
 	for (i=0; i<nb; i++) {
 		if ((body[i]->flags & dxBodyNoGravity)==0) {
@@ -664,9 +676,10 @@
 		// put v/h + invM*fe into tmp1
 		for (i=0; i<nb; i++) {
 			dReal body_invMass = body[i]->invMass;
-			for (j=0; j<3; j++) tmp1[i*6+j] = body[i]->facc[j] * body_invMass + body[i]->lvel[j] * stepsize1;
+			for (j=0; j<3; j++) tmp1[i*6+j] = body[i]->facc[j] * body_invMass +
+                                              (body[i]->lvel[j] + 0*body[i]->clvel[j]) * stepsize1;
 			dMULTIPLY0_331 (tmp1 + i*6 + 3,invI + i*12,body[i]->tacc);
-			for (j=0; j<3; j++) tmp1[i*6+3+j] += body[i]->avel[j] * stepsize1;
+			for (j=0; j<3; j++) tmp1[i*6+3+j] += (body[i]->avel[j]+0*body[i]->cavel[j]) * stepsize1;
 		}
 
 		// put J*tmp1 into rhs
@@ -705,10 +718,12 @@
 		// note that the SOR method overwrites rhs and J at this point, so
 		// they should not be used again.
 		
-		// add stepsize * cforce to the body velocity
+		// add stepsize * cforce to the [corrective] body velocity
 		for (i=0; i<nb; i++) {
-			for (j=0; j<3; j++) body[i]->lvel[j] += stepsize * cforce[i*6+j];
-			for (j=0; j<3; j++) body[i]->avel[j] += stepsize * cforce[i*6+3+j];
+                  //for (j=0; j<3; j++) body[i]->lvel[j] += stepsize * cforce[i*6+j];
+                  for (j=0; j<3; j++) body[i]->clvel[j] += stepsize * cforce[i*6+j];
+                  //for (j=0; j<3; j++) body[i]->avel[j] += stepsize * cforce[i*6+3+j];
+                  for (j=0; j<3; j++) body[i]->cavel[j] += stepsize * cforce[i*6+3+j];
 		}
 
 		// if joint feedback is requested, compute the constraint force.
@@ -755,7 +770,7 @@
 	for (i=0; i<nb; i++) {
 		dReal body_invMass = body[i]->invMass;
 		for (j=0; j<3; j++) body[i]->lvel[j] += stepsize * body_invMass * body[i]->facc[j];
-		for (j=0; j<3; j++) body[i]->tacc[j] *= stepsize;	
+		for (j=0; j<3; j++) body[i]->tacc[j] *= stepsize;
 		dMULTIPLYADD0_331 (body[i]->avel,invI + i*12,body[i]->tacc);
 	}
 
@@ -776,7 +791,22 @@
 	// update the position and orientation from the new linear/angular velocity
 	// (over the given timestep)
 	IFTIMING (dTimerNow ("update position");)
-	for (i=0; i<nb; i++) dxStepBody (body[i],stepsize);
+	for (i=0; i<nb; i++) {
+          // add constraint-correcting velocity for the purposes of the body update
+          for (j=0; j<3; j++) body[i]->lvel[j] += body[i]->clvel[j];
+          for (j=0; j<3; j++) body[i]->avel[j] += body[i]->cavel[j];
+          dxStepBody (body[i],stepsize);
+#ifdef POSITIONAL_CORRECTION
+          // remove most of the constraint-correcting velocity again.
+          // why not all?  because that leads to enormous jitter.  not sure why.
+#define CONSTRAINT_LINSCALE_MAGIC_NUMBER (0.80)
+#define CONSTRAINT_ANGSCALE_MAGIC_NUMBER (1.00)
+          for (j=0; j<3; j++) body[i]->lvel[j] -= body[i]->clvel[j] *
+                                CONSTRAINT_LINSCALE_MAGIC_NUMBER;// * world->global_erp;
+          for (j=0; j<3; j++) body[i]->avel[j] -= body[i]->cavel[j] *
+                                CONSTRAINT_ANGSCALE_MAGIC_NUMBER;// * world->global_erp;
+#endif
+        }
 
 	IFTIMING (dTimerNow ("tidy up");)
 
Index: ../src/objects.h
===================================================================
--- ../src/objects.h	(revision 4000)
+++ ../src/objects.h	(working copy)
@@ -98,6 +98,7 @@
   dQuaternion q;		// orientation quaternion
   dMatrix3 R;			// rotation matrix, always corresponds to q
   dVector3 lvel,avel;		// linear and angular velocity of POR
+  dVector3 clvel,cavel;		// corrective velocity of POR
   dVector3 facc,tacc;		// force and torque accumulators
   dVector3 finite_rot_axis;	// finite rotation axis, unit length or 0=none