[All] Fixed multiple bodies merge in same collection during single ti…

…me step and:

1- added corresponding test
2- saved previous scene scale when doing reload
3- added new examples
4- usual cleaning that makes the commit diff hard to read...

Sleeping and Merging Bodies/MergingBodiesProj/src/mergingBodies/BodyPairContact.java

@@ -13,12 +13,14 @@ public class BodyPairContact {
 
 	public ArrayList<Contact> contactList = new ArrayList<Contact>();
 
-	public ArrayList<Double> relativeVelHistory = new ArrayList<Double>();
+	public ArrayList<Double> relativeVelocityHist = new ArrayList<Double>();
 
 	boolean updatedThisTimeStep = false;
 
 	boolean merged = false;
 
+	ArrayList<Boolean> contactStateHist = new ArrayList<Boolean>();
+
 	public BodyPairContact(RigidBody body1, RigidBody body2) {
 		this.body1 = body1;
 		this.body2 = body2;
@@ -56,6 +58,45 @@ public boolean isIn(ArrayList<BodyPairContact> bodyPairContacts) {
 		return false;
 	}
 
+	/**
+	 * Check if relative velocity has been strictly decreasing over CollisionProcessor.sleepAccum time steps.
+	 * @return true or false
+	 */
+	public boolean isRelativeVelocityDecreasing() {
+
+		double epsilon = 5e-4;
+		double threshold = CollisionProcessor.sleepingThreshold.getValue();
+
+		if ((relativeVelocityHist.size() == CollisionProcessor.sleepAccum.getValue())) {
+			double previousValue = 0; 
+			double currentValue = 0;
+			for (Double relativeVelocity : relativeVelocityHist) {
+				currentValue = relativeVelocity;
+				if (relativeVelocity > threshold || currentValue > previousValue + epsilon ) 
+					return false;
+				previousValue = relativeVelocity;
+			}
+		} else {
+			return false;
+		}
+
+		return true;
+	}
+
+	/**
+	 * Check if contacts have been stable over CollisionProcessor.sleepAccum time steps.
+	 * @return true or false
+	 */
+	public boolean areContactsStable() {
+
+		if ((contactStateHist.size() == CollisionProcessor.sleepAccum.getValue()))
+			for (boolean isStable : contactStateHist)
+				if (!isStable)
+					return false;
+
+		return true;
+	}
+
 	/**
 	 * Given a RigidBody sB in a BodyPairContact, return the adjacent RigidBody in the BodyPairContact
 	 * @param body

Sleeping and Merging Bodies/MergingBodiesProj/src/mergingBodies/CollisionProcessor.java

@@ -101,7 +101,7 @@ public void processCollisions( double dt ) {
 
 			if (RigidBodySystem.enableMerging.getValue() || RigidBodySystem.enableSleeping.getValue())
 				for (Contact contact : contacts) 
-					storeInBodyContacts(contact);
+					storeInBodyPairContacts(contact);
 
 			updateContactMap();
 
@@ -372,10 +372,10 @@ protected void prune(ArrayList<Contact> contacts) {
 	}
 
 	/**
-	 * Store contact in bodyContacts, and update relative velocity history.
+	 * Store contact in bodyPairContacts, and update relative velocity history.
 	 * @param contact
 	 */
-	private void storeInBodyContacts(Contact contact) {
+	private void storeInBodyPairContacts(Contact contact) {
 
 		if (contact.body1.pinned && contact.body2.pinned) return;
 
@@ -384,15 +384,21 @@ private void storeInBodyContacts(Contact contact) {
 
 		if (bpc != null) { // if it exists
 			if (!bpc.updatedThisTimeStep) { // only once per time step
-				bpc.relativeVelHistory.add(contact.getRelativeMetric());
-				if (bpc.relativeVelHistory.size() > CollisionProcessor.sleepAccum.getValue()) {
-					bpc.relativeVelHistory.remove(0);
-				}
-				bpc.updatedThisTimeStep = true;
+				bpc.relativeVelocityHist.add(contact.getRelativeMetric());
+				if (bpc.relativeVelocityHist.size() > CollisionProcessor.sleepAccum.getValue())
+					bpc.relativeVelocityHist.remove(0);
+
+//				if (bpc.contactStateHist.size() == 0)
+//					bpc.contactStateHist.add(true);
+//				
+//				if (bpc.contactStateHist.size() > CollisionProcessor.sleepAccum.getValue())
+//					bpc.contactStateHist.remove(0);
+
+				bpc.updatedThisTimeStep = true;				
 			}
 		} else { // body contact did not exist in previous list
 			bpc = new BodyPairContact(contact.body1, contact.body2);
-			bpc.relativeVelHistory.add(contact.getRelativeMetric());
+			bpc.relativeVelocityHist.add(contact.getRelativeMetric());
 			bpc.updatedThisTimeStep = true;
 			bodyContacts.add(bpc);
 		}
@@ -557,7 +563,7 @@ private void processCollision( RigidBody body1, Block b1, RigidBody body2, Block
 			normal.normalize();
 
 			Contact contact = null;
-			contact = new Contact( body1, body2, contactW, normal, b1, b2, distance);
+			contact = new Contact(body1, body2, contactW, normal, b1, b2, distance);
 
 			// set normals in body coordinates
 			contact.normalB1.set(normal);
@@ -573,7 +579,7 @@ private void processCollision( RigidBody body1, Block b1, RigidBody body2, Block
 
 	public DoubleParameter restitution = new DoubleParameter( "restitution (bounce)", 0, 0, 1 );
 	public DoubleParameter friction = new DoubleParameter("Coulomb friction coefficient", 0.8, 0, 2 );
-	public IntParameter iterations = new IntParameter("iterations for GS solve", 200, 1, 500);
+	public IntParameter iterations = new IntParameter("iterations for GS solve", 500, 1, 1000);
 	private BooleanParameter shuffle = new BooleanParameter( "shuffle", false);
 	private BooleanParameter warmStart = new BooleanParameter( "warm start", true);
 	public static DoubleParameter feedbackStiffness = new DoubleParameter("feedback coefficient", 0, 0,50  );

Sleeping and Merging Bodies/MergingBodiesProj/src/mergingBodies/Contact.java

@@ -4,6 +4,7 @@
 import com.jogamp.opengl.GL2;
 import com.jogamp.opengl.GLAutoDrawable;
 
+import mintools.parameters.DoubleParameter;
 import no.uib.cipr.matrix.DenseVector;
 
 import java.util.ArrayList;
@@ -213,6 +214,15 @@ public double getRelativeMetric() {
 		double k = 0.5*relativeVelocity.lengthSquared() + 0.5*relativeAngularVelocity*relativeAngularVelocity;
 		return k;
 	}
+
+	/**
+	 * Getter for lambda
+	 * @return metric
+	 */
+	public Vector2d getLambda() {
+		Vector2d l = new Vector2d(lambda);
+		return l;
+	}
 
 	/**
 	 * Draws the contact points
@@ -261,13 +271,14 @@ public void drawContactForce( GLAutoDrawable drawable ) {
 		gl.glColor4f(1, 0, 0, 1);
 		gl.glBegin( GL.GL_LINES );
 
-		double scale = 0.05;
+		double scale = forceVizScale.getValue();
 		gl.glVertex2d(contactW.x + scale*contactForceB1.x, contactW.y+scale*contactForceB1.y);
 		gl.glVertex2d(contactW.x + scale*contactForceB2.x, contactW.y+scale*contactForceB2.y);
 
 		gl.glEnd();
 	}
 
+	static DoubleParameter forceVizScale = new DoubleParameter("force viz scale", 0.05, 0.01, 1);
 
 	/**
 	 * Draws the connections between bodies to visualize the 
@@ -282,7 +293,7 @@ public void drawInternalContactForce( GLAutoDrawable drawable ) {
 		gl.glLineWidth(2);
 		gl.glBegin( GL.GL_LINES );
 
-		double scale = 0.05;
+		double scale = forceVizScale.getValue();
 		gl.glVertex2d(contactW.x + scale*contactForceB1.x, contactW.y+scale*contactForceB1.y);
 		gl.glVertex2d(contactW.x + scale*contactForceB2.x, contactW.y+scale*contactForceB2.y);
 

Sleeping and Merging Bodies/MergingBodiesProj/src/mergingBodies/LCPApp.java

@@ -73,9 +73,8 @@ public LCPApp() {
 
     public void setUp() {
         system.mouseSpring = mouseSpring;
-        systemDir = "datalcp/unstableStackTest.png";
+        systemDir = "datalcp/jamTest.png";
         loadSystem(systemDir); 
-        // good default scene
         T.getBackingMatrix().setIdentity();
         ev = new EasyViewer( "2D Rigid Body Collision Processing", this, new Dimension(540,480), new Dimension(640,480) );
         ev.addInteractor(this); 
@@ -465,9 +464,13 @@ else if ( e.getKeyCode() == KeyEvent.VK_S ) {
                     stepped = true;
                 } 
                 else if ( e.getKeyCode() == KeyEvent.VK_R ) {                    
-                    //systemReset(); 
-
+                	double px = posx.getValue();
+                    double py = posy.getValue();
+                    double s = scale.getValue();
                 	loadSystem(system.name);
+                	posx.setValue(px);
+                    posy.setValue(py);
+                    scale.setValue(s);
                 } 
                 else if ( e.getKeyCode() == KeyEvent.VK_A ) {
                 	scale.setValue( imageWidth / windowWidth );

Sleeping and Merging Bodies/MergingBodiesProj/src/mergingBodies/RigidBodySystem.java

@@ -135,9 +135,10 @@ public void advanceTime( double dt ) {
 		}
 
 		if (enableMerging.getValue()) {
-			if(enableUnmerging.getValue())
+			if(enableUnmerging.getValue()) {
 				unmergeBodies(dt);
-			checkIndex();
+				checkIndex();
+			}
 		}
 
 		if (generateBody) {
@@ -437,40 +438,28 @@ private void buildNeighborBody(RigidBody b, ArrayList<RigidBody> subBodies, Arra
 	 */
 	public void mergeBodies() {
 		LinkedList<BodyPairContact> removalQueue = new LinkedList<BodyPairContact>();
+
 		for (BodyPairContact bpc:collisionProcessor.bodyContacts) {
-			boolean mergeCondition = false;
-			double threshold = CollisionProcessor.sleepingThreshold.getValue();
-
-			double epsilon = 0.0005;
-			if ((bpc.relativeVelHistory.size() == CollisionProcessor.sleepAccum.getValue())) {
-
-				mergeCondition = true;
-				double prevValue = 0; double currentValue = 0;
-				for (Double relVel : bpc.relativeVelHistory) {
-					currentValue = relVel;
-					if (relVel > threshold || currentValue > prevValue + epsilon ) {
-						mergeCondition = false; break;
-					}
-					prevValue = relVel;
-				}
-			}
+
+			boolean mergeCondition = (bpc.isRelativeVelocityDecreasing());// || bpc.areContactsStable());
 
 			if (!bpc.updatedThisTimeStep) mergeCondition = false;
 			if (bpc.body1.pinned || bpc.body2.pinned) mergeCondition = false;
-			if (bpc.body1.merged || bpc.body2.merged) mergeCondition = false;
-			if(bpc.body1.state == ObjectState.SLEEPING && bpc.body2.state == ObjectState.SLEEPING) mergeCondition = true;
+			if (bpc.body1.merged && bpc.body2.merged) mergeCondition = false;
+			if (bpc.body1.state == ObjectState.SLEEPING && bpc.body2.state == ObjectState.SLEEPING) mergeCondition = true;
 
 			if (mergeCondition) {
 				bpc.merged = true;
-				//if they are both not collections...make a new collection!
 				if(!bpc.body1.isInCollection() && !bpc.body2.isInCollection()) {
-					bodies.remove(bpc.body1); bodies.remove(bpc.body2);
-					RigidCollection col = new RigidCollection(bpc.body1, bpc.body2);
-					col.addInternalContact(bpc);
-					bodies.add(col);
+					//both are not collections: make a new collection
+					bodies.remove(bpc.body1); 
+					bodies.remove(bpc.body2);
+					RigidCollection collection = new RigidCollection(bpc.body1, bpc.body2);
+					collection.addInternalContact(bpc);
+					bodies.add(collection);
 				}
 				else if (bpc.body1.isInCollection() && bpc.body2.isInCollection()) {
-					// if they are BOTH collections... think about what to do
+					//both are collections:
 					//take all the bodies in the least massive one and add them to the collection of the most massive
 					if (bpc.body1.parent.massLinear > bpc.body2.parent.massLinear) {
 						bpc.body1.merged = true;
@@ -488,14 +477,14 @@ else if (bpc.body1.isInCollection() && bpc.body2.isInCollection()) {
 					}
 				}
 				else if (bpc.body1.isInCollection()) {
-					//body1 is in a collection... body2 isnt
+					//body1 is in a collection, body2 is not
 					bodies.remove(bpc.body2);
 					bpc.body1.parent.addBody(bpc.body2);
 					bpc.body1.parent.addInternalContact(bpc);
 					bpc.body1.parent.addIncompleteContacts(bpc.body2, removalQueue);
 				}
 				else if (bpc.body2.isInCollection()) {
-					//body2 is in a collection... body1 isnt
+					//body2 is in a collection, body1 is not
 					bodies.remove(bpc.body1);
 					bpc.body2.parent.addBody(bpc.body1);
 					bpc.body2.parent.addInternalContact(bpc);
@@ -609,10 +598,10 @@ public void reset() {
 	private void checkIndex() {
 		int i = 0;
 		nbCollections = 0;
-		for(RigidBody b: bodies) {
-			b.index = i;
+		for(RigidBody body: bodies) {
+			body.index = i;
 			i++;
-			if(b instanceof RigidCollection)
+			if(body instanceof RigidCollection)
 				nbCollections++;
 		}
 	}
@@ -820,9 +809,9 @@ private void displayCollectionBV(RigidCollection b, GLAutoDrawable drawable) {
 	private BooleanParameter drawContactGraph = new BooleanParameter( "draw contact graph", true );
 	private BooleanParameter drawSpeedCOM = new BooleanParameter( "draw speed COM", false );
 	private BooleanParameter processCollisions = new BooleanParameter( "process collisions", true );
-	public static BooleanParameter enableMerging = new BooleanParameter( "enable merging", true);
-	public static BooleanParameter enableUnmerging = new BooleanParameter( "enable unmerging", true);
-	public static BooleanParameter enableUpdateContactsInCollections = new BooleanParameter( "enable update contact in collection", true);
+	public static BooleanParameter enableMerging = new BooleanParameter( "enable merging", false);
+	public static BooleanParameter enableUnmerging = new BooleanParameter( "enable unmerging", false);
+	public static BooleanParameter enableUpdateContactsInCollections = new BooleanParameter( "enable update contact in collection", false);
 	public static BooleanParameter enableSleeping = new BooleanParameter( "enable sleeping", false);
 	public BooleanParameter drawIndex = new BooleanParameter( "dawIndex", false );
 
@@ -853,6 +842,7 @@ public JPanel getControls() {
 		vfpv.add( drawContactGraph.getControls() );
 		vfpv.add( drawSpeedCOM.getControls() );
 		vfpv.add( drawIndex.getControls() );
+		vfpv.add( Contact.forceVizScale.getSliderControls(true) ); // Gross?
 
 		CollapsiblePanel cp = new CollapsiblePanel(vfpv.getPanel());
 		cp.collapse();