Sign in to follow this  

Problem implementing sequential impulse mass over 100Kg (solved, it was a miscalculated inverse inertia tensor)

This topic is 824 days old which is more than the 365 day threshold we allow for new replies. Please post a new topic.

If you intended to correct an error in the post then please contact us.

Recommended Posts

Hi, following the advice of some members of he forum I tried to implement the sequential impulse algorithm in a game physics demo, its jus some boxes against the floor, no friction, no collision between boxes, just the floor.

 

It works for small masses below 100 Kg but above that it starts to bounce (the demo have no restitution yet, so this is unespected too).

 

[attachment=29094:help1.gif]
 
If I disable warmstarting this is what I got
 
[attachment=29096:help2.gif]

 

The boxes masses are 100, 120, 140 and 160 (in kilograms), gravity magnitude is  10, dt = 1/240, iterations = 10

 

If I use 100 iterations it mitigates the problem a little, and with 1000 it aparently disapears ¿what could I be doing wrong?

 

I attach the arbiter (contact constraint) code

package com.iigwo.physics_old;

import java.util.ArrayList;
import java.util.List;

import org.joml.Matrix3f;
import org.joml.Vector3f;

public class Arbiter {

	private static final boolean WARMSTARTING = false;
	private static final boolean ACCUMULATE_IMPULSES = false;
	private static final boolean POSITION_CORRECTION = true;

	RigidBody b1, b2;
	final List<Contact> contacts = new ArrayList<>();

	public Arbiter(RigidBody body1, RigidBody body2) {
		this.b1 = body1;
		this.b2 = body2;
	}

	public void update(List<Contact> newContacts) {
		if (WARMSTARTING) {
			for (Contact newContact : newContacts) {
				for (Contact oldContact : contacts) {
					if (newContact.matches(oldContact)) {
						newContact.pn = oldContact.pn;
						break;
					}
				}
			}
		}
		contacts.clear();
		contacts.addAll(newContacts);
	}

	public void preStep(float inverseDt) {

		final float K_ALLOWED_PENETRATION = 0.01f;
		final float K_BIAS_FACTOR = POSITION_CORRECTION ? 0.1f : 0.0f;

		Matrix3f iitiws1 = b1.inverseInertiaTensorInWorldSpace;
		Matrix3f iitiws2 = b2.inverseInertiaTensorInWorldSpace;
		Vector3f vr1 = new Vector3f(), vr2 = new Vector3f(), vr = new Vector3f();

		for (Contact c : contacts) {

			Vector3f n = c.normal;
			Vector3f t1 = new Vector3f();
			Vector3f t2 = new Vector3f();
			Vector3f r1 = new Vector3f(c.position).sub(b1.position);
			Vector3f r2 = new Vector3f(c.position).sub(b2.position);

			vr.set(vr2.set(b2.rotation).cross(r2).add(b2.velocity)).sub(vr1.set(b1.rotation).cross(r1).add(b1.velocity));

			Vector3f vn = new Vector3f(n).mul(vr.dot(n));
			Vector3f vt = new Vector3f(vr).sub(vn);
			if (vt.length() > 0) {
				t1.set(vt).normalize();
				t2.set(t1).cross(n);
			} else {
				System.out.println("ups");
				t1.set(1, 0, 0);
				t2.set(0, 1, 0);
			}

			c.bias = c.bias += -K_BIAS_FACTOR * inverseDt * Math.min(0.0f, c.separation + K_ALLOWED_PENETRATION);

			float kNormal = 0;

			Vector3f r1Xn = new Vector3f(r1).cross(n);
			Vector3f r2Xn = new Vector3f(r2).cross(n);
			if (!b1.haveInfiniteMass) kNormal += b1.inverseMass + n.dot(iitiws1.transform(r1Xn).cross(r1));
			if (!b2.haveInfiniteMass) kNormal += b2.inverseMass + n.dot(iitiws2.transform(r2Xn).cross(r2));
			c.massNormal = 1f / kNormal;

			if (ACCUMULATE_IMPULSES) {
				Vector3f p = new Vector3f().fma(c.pn, n);				
				if (!b1.haveInfiniteMass) {
					b1.velocity.fma(-b1.inverseMass, p);
					b1.rotation.sub(iitiws1.transform(new Vector3f(r1).cross(p)));
				}
				if (!b2.haveInfiniteMass) {
					b2.velocity.fma(+b2.inverseMass, p);
					b2.rotation.add(iitiws2.transform(new Vector3f(r2).cross(p)));
				}
			}
			
		}
	}

	public void applyImpulse() {

		Matrix3f iitiws1 = b1.inverseInertiaTensorInWorldSpace;
		Matrix3f iitiws2 = b2.inverseInertiaTensorInWorldSpace;

		Vector3f vr1 = new Vector3f(), vr2 = new Vector3f(), vr = new Vector3f();

		for (Contact c : contacts) {

			Vector3f r1 = new Vector3f(c.position).sub(b1.position);
			Vector3f r2 = new Vector3f(c.position).sub(b2.position);

			Vector3f n = c.normal;
			
			vr.set(vr2.set(b2.rotation).cross(r2).add(b2.velocity)).sub(vr1.set(b1.rotation).cross(r1).add(b1.velocity));

			float vn = vr.dot(n);
			float dpn = c.massNormal * (-vn + c.bias);
			if (ACCUMULATE_IMPULSES) {
				float pn0 = c.pn;
				c.pn = Math.max(pn0 + dpn, 0);
				dpn = c.pn - pn0;
			} else dpn = Math.max(dpn, 0);
			Vector3f pn = new Vector3f(n).mul(dpn);

			if (!b1.haveInfiniteMass) {
				b1.velocity.fma(-b1.inverseMass, pn);
				b1.rotation.sub(iitiws1.transform(new Vector3f(r1).cross(pn)));
			}
			if (!b2.haveInfiniteMass) {
				b2.velocity.fma(+b2.inverseMass, pn);
				b2.rotation.add(iitiws2.transform(new Vector3f(r2).cross(pn)));
			}
		}
	}

	public static final float clamp(float f, float min, float max) {
		if (f < min) return min;
		if (f > max) return max;
		return f;
	}

	public boolean haveContacts() {
		return !contacts.isEmpty();
	}
}

Share this post


Link to post
Share on other sites

Did you disable position correction? I can be everything from wrong contact points, to wrong Jacobians, to wrong inertia. Whatever this is it is unrelated to the mass!

 

Is z your up axis?

And use temporaries! How do you expect that anyone can read this: vr.set(vr2.set(b2.rotation).cross(r2).add(b2.velocity)).sub(vr1.set(b1.rotation).cross(r1).add(b1.velocity));

Edited by Dirk Gregorius

Share this post


Link to post
Share on other sites

From my experience you have a bug somewhere. Even if the density is to low (say 0.05) then you wouldn't get these results.

 

Warm Starting should be once before iterations (one time per frame), not per iteration.

 

Are your distance (position constraint) negative (meaning penetration) and positive (meaning separation)?

 

This looks strange:

c.bias = c.bias += -K_BIAS_FACTOR * inverseDt * Math.min(0.0f, c.separation + K_ALLOWED_PENETRATION);

 You're fine with:

c.bias = -frequency * coefficient * Min(0.0f, c.separation + slop);

240 Hz? Try 60 Hz for now (1/60) with 5 iterations.

 

Check if the solver converges (e.g. (Ju >= 0 <-> vn >= 0)).

Edited by Irlan Robson

Share this post


Link to post
Share on other sites

 

This looks strange:

c.bias = c.bias += -K_BIAS_FACTOR * inverseDt * Math.min(0.0f, c.separation + K_ALLOWED_PENETRATION);

 You're fine with:

c.bias = -frequency * coefficient * Min(0.0f, c.separation + slop);

240 Hz? Try 60 Hz for now (1/60) with 5 iterations.

 

Oh, sorry, was a typo

 

 

Did you disable position correction? I can be everything from wrong contact points, to wrong Jacobians, to wrong inertia. Whatever this is it is unrelated to the mass!

 

It was the inertia tensor!!! I forgot to invert it! Thanks Dirk, it works now.

 

The demo works now with 5 iterations at 60Hz

Share this post


Link to post
Share on other sites

This topic is 824 days old which is more than the 365 day threshold we allow for new replies. Please post a new topic.

If you intended to correct an error in the post then please contact us.

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account

Sign up for a new account in our community. It's easy!

Register a new account

Sign in

Already have an account? Sign in here.

Sign In Now

Sign in to follow this