Boucing with sequential impulse

Please don't post Bullet support questions here, use the above forums instead.
Post Reply
Forinti
Posts: 5
Joined: Sat Sep 14, 2019 9:34 am

Boucing with sequential impulse

Post by Forinti » Sun Sep 15, 2019 7:12 am

Hello everyone,

Above all, sorry for my bad english.
I make a physic engine based on sequential impulses but i have a problem of bouncing, i have debugged this for 2 weeks but I do not find the problem.

In my previous attemps, i try to solve the contact velocity with an impulse and a velocity bias but i have too much bouncing with this.
So i throught that it was the bias that produced this, so i have changed the method for positional correction and it's OK.
But when i add restitution the problem is reappeared so i think i really have a problem in my code...

the simulation :
- I make a narrowphase and broadphase to detect collision
- I update or create or delete the contacts with contact matching
- I integrate force
- I initialize the contacts and warmstart it
- I apply an impulse for each contacts with n itérations
- I integrate the Velocity
- I correct the position
- I clear force

notes :
- the problem is here even where the bodies have not rotation and have not friction.
- the problem is here even without the positional correction.

This is the update code :

Code: Select all

private void broadphase() {

		Body a = null;
		Body b = null;
		Manifold m = null;
		Manifold old = null;

		for (int i = 0; i < bodies.size(); i++) {
			a = bodies.get(i);
			for (int j = i + 1; j < bodies.size(); j++) {
				b = bodies.get(j);
				if (a.invMass == 0f && b.invMass == 0f) {
					continue;
				}
				m = new Manifold(a, b);

				if (m.collide() > 0) {

					old = findManifold(m);
					if (old != null) {
						old.updateContacts(m.contacts, m.numContacts);
					} else {
						contacts.add(m);
					}
				} else {
					contacts.remove(m);
				}
			}
		}
	}

	private Manifold findManifold(Manifold mNew) {
		for (Manifold mOld : contacts) {
			if (mOld.equals(mNew)) {
				return mOld;
			}
		}
		return null;
	}

	public void update(float dt) {
		broadphase();

		integrateForces(dt);

		// Initialize collision
		for (Manifold c : contacts) {
			c.preStep();
		}
		// Solve collisions
		for (int i = 0; i < iterations; i++) {
			for (Manifold c : contacts) {
				c.applyImpulse();
			}
		}
		integrateVelocities(dt);

		for (int i = 0; i <= iterations >> 1; i++) {
			for (Manifold c : contacts) {
				c.solvePositionConstraint();
			}
		}
		clearForces();
	}

	// Semi-Implicit (Symplectic) Euler
	// v = ∫a*dt where a = F/m
	// ang_v = ∫ang_a*dt where ang_a = T/I
	private void integrateForces(float dt) {
		Vec2 acc = new Vec2();
		float angAcc = 0f;
		for (Body b : bodies) {
			if (b.invMass == 0f) {
				continue;
			}
			// linear
			acc.set(b.force).sclLocal(b.invMass);
			acc.y += PhysicSettings.gravity * b.gravityScale;// gravity will
																// always act on
																// the
			// body
			b.velocity.addSclLocal(acc, dt);
			// angular
			angAcc = b.torque * b.invInertia;
			b.angularVelocity += angAcc * dt;
		}
	}

	// Semi-Implicit (Symplectic) Euler
	// x = ∫v*dt
	private void integrateVelocities(float dt) {
		for (Body b : bodies) {
			if (b.invMass == 0f) {
				continue;
			}
			b.position.addSclLocal(b.velocity, dt);
			b.rotation += b.angularVelocity * dt;
		}
	}

	private void clearForces() {
		for (Body b : bodies) {
			b.force.setZero();
			b.torque = 0f;
		}
	}
This is the impulse code :

Code: Select all

private float getMassCoefficient(Vec2 rA, Vec2 rB, Vec2 n) {
		float raXn = rA.cross(n);
		float rbXn = rB.cross(n);
		float kn = bodyA.invMass + bodyB.invMass;
		kn += bodyA.invInertia * raXn * raXn;
		kn += bodyB.invInertia * rbXn * rbXn;
		return kn != 0f ? 1f / kn : 0f;
	}

	private Vec2 getRelativeVelocity(Vec2 rA, Vec2 rB) {
		// v_sum = v + r x ω
		// v_rel = vB_sum - vA_sum
		Vec2 va = rA.crossr(bodyA.angularVelocity).addLocal(bodyA.velocity);
		Vec2 vb = rB.crossr(bodyB.angularVelocity).addLocal(bodyB.velocity);
		return vb.sub(va);
	}

	private Contact findContact(int id) {
		for (int i = 0; i < numContacts; i++) {
			if (contacts[i].id == id) {
				return contacts[i];
			}
		}
		return null;
	}

	public void updateContacts(Contact[] newContacts, int numNewContacts) {
		for (int i = 0; i < numNewContacts; i++) {
			Contact cNew = newContacts[i];
			Contact cOld = findContact(cNew.id);

			if (cOld != null) {
				cNew.pn = cOld.pn;
				cNew.pt = cOld.pt;
			} else { // optional
				cNew.pn = 0f;
				cNew.pt = 0f;
			}
		}
		for (int i = 0; i < numNewContacts; i++) {
			contacts[i] = newContacts[i];
		}
		numContacts = numNewContacts;
	}

	public void preStep() {
		// Compute the friction coefficient
		f = (float) Math.sqrt(bodyA.friction * bodyB.friction);
		// Compute the coefficient of elasticity
		e = Math.min(bodyA.restitution, bodyB.restitution);

		float g = PhysicSettings.gravity * dt;
		float resting = (float) (g * Math.sqrt(bodyA.gravityScale * bodyB.gravityScale)) + MathUtils.EPSILON;

		for (int i = 0; i < numContacts; i++) {
			Contact c = contacts[i];

			// compute COM to contact for body A
			c.position.sub(bodyA.position, c.rA);
			// compute COM to contact for body B
			c.position.sub(bodyB.position, c.rB);

			// Get the relative velocity between A and B
			Vec2 rv = getRelativeVelocity(c.rA, c.rB);

			// Compute the velocity bias
			c.vb = 0f;
			float rvn = rv.dot(c.normal);
			if (rvn < -resting) {
				// apply the coefficient of elasticity
				c.vb += -e * rvn;
			}
			// compute the tangent
			Vec2 remN = c.normal.scl(c.normal.dot(rv));
			c.tangent.set(rv).subLocal(remN).normalizeLocal();

			// compute the sum of effective masses along normal
			c.kn = getMassCoefficient(c.rA, c.rB, c.normal);
			// compute the sum of effective masses along tangent
			c.kt = getMassCoefficient(c.rA, c.rB, c.tangent);
		}
		warmStart();
	}

	private void warmStart() {
		for (int i = 0; i < numContacts; i++) {
			Contact c = contacts[i];

			// warm-start
			Vec2 J = new Vec2();
			J.x = c.normal.x * c.pn + c.tangent.x * c.pt;
			J.y = c.normal.y * c.pn + c.tangent.y * c.pt;
			bodyA.applyImpulse(J.negate(), c.rA);
			bodyB.applyImpulse(J, c.rB);
		}
	}

	public void applyImpulse() {

		// apply friction impulse
		for (int i = 0; i < numContacts; i++) {
			Contact c = contacts[i];

			// Get the relative velocity between A and B
			Vec2 rv = getRelativeVelocity(c.rA, c.rB);

			// Get the relative velocity along the normal
			// v_rel . n
			float rvt = rv.dot(c.tangent);

			// Calculate normal impulse scalar relative to A's normal
			float j = c.kt * -rvt;

			// Coulomb's Law : -f * lambdaN <= lambdaT <= f * lambdaN
			float maxJ = f * c.pn;

			// clamp the accumulated tangential impulse
			float pt0 = c.pt;
			c.pt = MathUtils.clamp(pt0 + j, -maxJ, maxJ);
			j = c.pt - pt0;

			// Apply tangent impulse
			Vec2 jt = c.tangent.scl(j);
			bodyA.applyImpulse(jt.negate(), c.rA);
			bodyB.applyImpulse(jt, c.rB);
		}

		// apply normal impulse
		for (int i = 0; i < numContacts; i++) {
			Contact c = contacts[i];

			// Get the relative velocity between A and B
			Vec2 rv = getRelativeVelocity(c.rA, c.rB);

			// Get the relative velocity along the normal
			// v_rel . n
			float rvn = rv.dot(c.normal);

			// Calculate normal impulse scalar relative to A's normal
			float j = -c.kn * (rvn - c.vb);

			// accumulate impulses
			float pn0 = c.pn;
			c.pn = Math.max(pn0 + j, 0f);
			j = c.pn - pn0;

			// Apply normal impulse
			Vec2 jn = c.normal.scl(j);
			bodyA.applyImpulse(jn.negate(), c.rA);
			bodyB.applyImpulse(jn, c.rB);
		}
	}
What is wrong ?
thank you in advance for the time you're going to devote to me.

Forinti
Posts: 5
Joined: Sat Sep 14, 2019 9:34 am

Re: Boucing with sequential impulse

Post by Forinti » Sat Oct 05, 2019 9:01 am

Hello everyone,

I still haven't found out where the problem lies, despite a lot of tests:

- Change the way you maintain contact between images (by id vs by distance)
- Put a different color on contacts that persit
- Manage a single contact by collision
- Changing the way to preserve body peers
- Changing time-steps to a variable time-step
- And various mathematical tests

This is quite frustrating because I don't know where to look anymore...

It's probably a mistake on my part to think that but the fact of warm-start contacts that wanting to converge towards an anti-normal value (restitution) is it a problem and does not insert too much energy into the system ?
Because the frame's coherence seems to me to be a little stunned, right ?

Thanks in advance

irlanrobson
Posts: 16
Joined: Sun Feb 08, 2015 3:21 pm
Location: Brasi­lia, DF

Re: Boucing with sequential impulse

Post by irlanrobson » Tue Oct 15, 2019 3:42 pm

Your math seems to be correct. There is probably a small problem in your code. Maybe a sign problem or something like this.

Comparing your engine with Box2D may help you to identify the issue:

https://github.com/erincatto/box2d-lite

https://github.com/erincatto/Box2D

Forinti
Posts: 5
Joined: Sat Sep 14, 2019 9:34 am

Re: Boucing with sequential impulse

Post by Forinti » Sun Nov 03, 2019 7:56 am

Hello Irlan,

I just saw your answer, thank you.
I can not stop comparing but something must escape me.

I will rewrite a test engine that only manages circles with the minimum possible to facilitate debugging and get to the point.
In the meantime I'm attaching a video of the problem, if anyone has an idea ...

Note : in the video I forgot to translate this [Sans warmstarting = Without warmstarting] and [Avec warmstarting = With warmstarting].

https://www.youtube.com/watch?v=dgSTr9z4Omc

irlanrobson
Posts: 16
Joined: Sun Feb 08, 2015 3:21 pm
Location: Brasi­lia, DF

Re: Boucing with sequential impulse

Post by irlanrobson » Mon Nov 04, 2019 4:02 pm

Well, one problem in the code you posted is that it solves the tangent and normal constraints independently. I don't think this is correct.

The friction forces depend on the normal forces. Therefore, you should solve a tangent constraint followed by a normal constraint or vice-versa since constraint order is important. Recall constraints solved by last are prioritized.

However, I believe you can find many resources in the literature dealing with decoupled friction.

Forinti
Posts: 5
Joined: Sat Sep 14, 2019 9:34 am

Re: Boucing with sequential impulse

Post by Forinti » Wed Nov 06, 2019 7:35 pm

It's interesting, I'll look at this more deeply.
I had just left a little aside the problem described in this post to implement the friction.

For the bouncing problem, from the last answer I made an extremely lightweight physics engine that only handles circles and resolves only the linear velocity constraint.
And I have exactly the same problem. (I can post the code if needed)
The simplest solution for me would be to no longer use the warmstarting, but I really do not want to remove this feature ...

I want to clarify that it is only with stacking, a body falling only on the floor poses no problem in any case.
Without warmstarting everything is good, with or without restitution.
With warmstarting it is extremely stable without restitution but very unstable with the restitution (even explode).

I come to doubt that my code has errors and asks me this very simple question:
Is it possible to have a stable stack with elastic bodies and warmstarting without adding another feature to the engine?

thank you in advance

irlanrobson
Posts: 16
Joined: Sun Feb 08, 2015 3:21 pm
Location: Brasi­lia, DF

Re: Boucing with sequential impulse

Post by irlanrobson » Thu Nov 07, 2019 2:07 pm

Without warm-starting the solver can take lots of iterations to converge. For this reason you should consider using warm-starting.

FYI one of the main reasons for a simulation to blow up is a bad choice of integrator. Semi-impicit Euler is stable for non-stiff equations. If your simulation is exploding and it is using this type of integrator there is probably an error hidden somewhere in your code. I would not call a bug an instability problem.
Is it possible to have a stable stack with elastic bodies and warmstarting without adding another feature to the engine?
Yes, a vertical stack of spheres with varying restitution should be stable and come to rest. I have no problems at this configuration.

Forinti
Posts: 5
Joined: Sat Sep 14, 2019 9:34 am

Re: Boucing with sequential impulse

Post by Forinti » Sun Nov 10, 2019 10:01 am

It bothers me a lot but I give up the warmstarting, I can not find the problem and I do tests that make no sense.
I'm posting the light engine code or doing tests in case something gets in your way ...
Anyway thanks for your help Irlan.

Code: Select all

package physic;

public class Manifold {

	public static final class Pair {
		public final Circle circleA;
		public final Circle circleB;

		public Pair(Circle circleA, Circle circleB) {
			this.circleA = circleA;
			this.circleB = circleB;
		}

		@Override
		public int hashCode() {
			final int prime = 31;
			int result = 1;
			result = prime * result + circleA.id;
			result = prime * result + circleB.id;
			return result;
		}

		@Override
		public boolean equals(Object obj) {
			if (this == obj)
				return true;
			if (obj == null)
				return false;
			if (getClass() != obj.getClass())
				return false;
			Pair other = (Pair) obj;

			if ((circleA.id == other.circleA.id) && (circleB.id == other.circleB.id))
				return true;

			return false;
		}
	}

	public Circle circleA;
	public Circle circleB;
	public Contact contact = new Contact();

	public Manifold(Pair pair) {
		this.circleA = pair.circleA;
		this.circleB = pair.circleB;
	}

	public boolean collide() {
		Circle a = circleA;
		Circle b = circleB;

		Vec2 relPos = b.position.sub(a.position);

		double distSqr = relPos.lenSqr();
		double radiusSum = a.radius + b.radius;

		if (distSqr > (radiusSum * radiusSum))
			return false;

		double distance = Math.sqrt(distSqr);

		if (distance == 0d) {

			contact.normal.set(1d, 0d);
			contact.position.set(a.position);

		} else {
			contact.normal.set(relPos).sclLocal(1d / distance);
			contact.position.set(contact.normal).sclLocal(a.radius).addLocal(a.position);
		}
		return true;
	}

	public void updateContact(Contact newContact) {
		if (Settings.warmstarting) {
			Vec2 relPos = newContact.position.sub(contact.position);
			if (relPos.len() <= Settings.contactMatchingDistance) {
				newContact.accImpulses = contact.accImpulses;
			}
		}
		contact = newContact;
	}

	private double getRelativeVelocityAlongNormal() {
		return circleB.velocity.sub(circleA.velocity).dot(contact.normal);
	}

	public void preStep() {
		contact.normalMass = circleA.invMass + circleB.invMass;
		contact.normalMass = contact.normalMass > 0d ? 1d / contact.normalMass : 0d;

		double resting = (Settings.frequency * Settings.gravity) + Settings.epsilon;
		double rvn = getRelativeVelocityAlongNormal();
		contact.velocityBias = rvn < -resting ? -Settings.restitution * rvn : 0d;

		warmStart();
	}

	private void warmStart() {
		Vec2 J = new Vec2();
		J.x = contact.normal.x * contact.accImpulses;
		J.y = contact.normal.y * contact.accImpulses;

		circleA.velocity.x -= J.x * circleA.invMass;
		circleA.velocity.y -= J.y * circleA.invMass;

		circleB.velocity.x += J.x * circleB.invMass;
		circleB.velocity.y += J.y * circleB.invMass;
	}

	public void applyImpulses() {
		double j = contact.normalMass * (-getRelativeVelocityAlongNormal() + contact.velocityBias);

		double pn0 = contact.accImpulses;
		contact.accImpulses = Math.max(pn0 + j, 0d);
		j = contact.accImpulses - pn0;

		Vec2 jn = contact.normal.scl(j);

		circleA.velocity.x -= jn.x * circleA.invMass;
		circleA.velocity.y -= jn.y * circleA.invMass;

		circleB.velocity.x += jn.x * circleB.invMass;
		circleB.velocity.y += jn.y * circleB.invMass;
	}

}

Code: Select all

package physic;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import physic.Manifold.Pair;

public class World {

	public final List<Circle> circles = new ArrayList<>();
	public final Map<Pair, Manifold> contacts = new HashMap<>();

	public Circle addCircle(double radius, double x, double y, boolean isStatic) {
		Circle c = new Circle(radius, x, y, isStatic);
		circles.add(c);
		return c;
	}

	public void update() {
		broadphase();
		integrateGravity();
		preStep();
		for (int i = 0; i < Settings.solverIterations; i++)
			applyImpulses();
		integrateVelocities();
	}

	private void broadphase() {
		for (int i = 0; i < circles.size(); i++) {
			for (int j = i + 1; j < circles.size(); j++) {
				Circle circleA = circles.get(i);
				Circle circleB = circles.get(j);

				if (circleA.invMass == 0f && circleB.invMass == 0f)
					continue;

				Pair pair = new Pair(circleA, circleB);
				Manifold newManifold = new Manifold(pair);

				if (newManifold.collide()) {

					if (contacts.containsKey(pair)) {
						contacts.get(pair).updateContact(newManifold.contact);
					} else {
						contacts.put(pair, newManifold);
					}

				} else {
					contacts.remove(pair);
				}
			}
		}
	}

	private void integrateGravity() {
		for (Circle c : circles) {
			if (c.invMass == 0f)
				continue;
			c.velocity.y += Settings.frequency * Settings.gravity;
		}
	}

	private void preStep() {
		for (Manifold m : contacts.values())
			m.preStep();
	}

	private void applyImpulses() {
		for (Manifold m : contacts.values())
			m.applyImpulses();
	}

	private void integrateVelocities() {
		for (Circle c : circles) {
			if (c.invMass == 0f)
				continue;
			c.position.x += Settings.frequency * c.velocity.x;
			c.position.y += Settings.frequency * c.velocity.y;
		}
	}

}

Post Reply