Hello,

I'm in the midst of developing a simple 3d physics engine, but have been stuck on a problem for quite a while now. I would really appreciate it if someone could have a look at it, because I'm at a loss.
The issue I'm having concerns the collision response code, which seems to fail spectacularly in some cases, generating wrong impulse magnitudes. Specifically, something with the angular part of it is wrong, since it works as expected when only dealing with the linear part.

I'm still not sure if I calculate the inertia tensor in world coordinates the right way, so that could be one possible culprit (though I've tried a few different ways). Impulses seem to be applied correctly to the bodies, so I think the error lies in the calculation of the impulse magnitude denominator.

I've checked it many times with the way the Bullet physics engine does it, with the formulas available at Wikipedia, and some other sources, to no avail.
The bodies simply keep bouncing off eachother (even though there is supposed to be no restitution) or spinning wildly on collisions.

Here is the code that deals with the collision response:

Vector3 originA = contactOrigin - bodyA.Position;
Vector3 originB = contactOrigin - bodyB.Position;
Vector3 velocityA = bodyA.Velocity + Vector3::Cross(bodyA.AngularVelocity, originA);
Vector3 velocityB = bodyB.Velocity + Vector3::Cross(bodyB.AngularVelocity, originB);
Vector3 velocity = velocityA - velocityB;

Matrix3x3 inverseWorldInertiaTensorA = Matrix3x3::Scale(bodyA.InverseInertia) * Matrix3x3::Rotate(bodyA.Orientation);
Matrix3x3 inverseWorldInertiaTensorB = Matrix3x3::Scale(bodyB.InverseInertia) * Matrix3x3::Rotate(bodyB.Orientation);

float normalVelocity = Vector3::Dot(velocity, contactNormal);

if (normalVelocity > 0.f)
{
	float impulseDenominator = 
		bodyA.InverseMass +
		bodyB.InverseMass +
		Vector3::Dot(Vector3::Cross(inverseWorldInertiaTensorA * Vector3::Cross(originA, contactNormal), originA), contactNormal) +
		Vector3::Dot(Vector3::Cross(inverseWorldInertiaTensorB * Vector3::Cross(originB, contactNormal), originB), contactNormal);
	float j = -normalVelocity / impulseDenominator;
	bodyA.Velocity += j * bodyA.InverseMass * contactNormal;
	bodyB.Velocity -= j * bodyB.InverseMass * contactNormal;
	bodyA.AngularVelocity += j * (inverseWorldInertiaTensorA * Vector3::Cross(originA, contactNormal));
	bodyB.AngularVelocity -= j * (inverseWorldInertiaTensorB * Vector3::Cross(originB, contactNormal));
}

Thanks in advance!

Your second line of code has a little too much "A" in it!

6 minutes ago, Dave Roberts said:

Your second line of code has a little too much "A" in it!

Ah, thanks for pointing it out! Sadly, it's not responsible for the problem. I just cleaned up the code a bit before posting it, and that's when I made that mistake.

Hmm.. 

I would start with some asserts in there then .. first off, that your contact normal is normalized.

Second, that your inertia tensor is orthogonal.  [At least I think it should be.. I could be wrong here]

Also.. question - is it valid to orient your inverse of the inertia tensor? Or should you be orienting then inverting?

Just guessing really!

DR

I would need to look at this more carefully, but it looks like you're doing your calculations to determine your change in angular velocity using the normal?  You need to use the tangent.

That part is correct:  Normal cross the vector from centre of mass to impact point is a (normalized) torque

30 minutes ago, Dave Roberts said:

That part is correct:  Normal cross the vector from centre of mass to impact point is a (normalized) torque

Well something's not right there because the j he's calculating just looks like the linear impulse, but then that's being used for both the normal and tangential velocity calculation.  Maybe there's a different way to do this, but the way I remember doing it is to calculate the linear impulse like that and then a separate tangential impulse (use the tangent in place of the normal) and then calculate the delta angular velocity from that.

Your tensors don't look right. You need to use R * I * R^T (or flipped about depending on your notation ordering). The rest more or less looks correct. Be sure to double check it against a known reference code (like Box2D *Lite* or qu3e).

Yeah, Box2D Lite is a good reference.  The code is minimal and easy to understand... as long as you already know what the physics is supposed to be doing. 

Go here:

https://code.google.com/archive/p/box2d/downloads

Thanks a lot for all the responses!

All the contact data should be correct (the contact normal is normalized, the position is where it should be etc.).

I'm using the equation found on Wikipedia:

Apart from the fact that I'm applying the dot product for both bodies seperately (which doesn't seem to make a difference and is also done in other physics libraries), I can't see what I'm doing wrong. I already tried numerous different ways to calculate the tensor, including orienting the non-inverse and also what Randy Gaul suggests (also flipped, see code below), without success.

Matrix3x3 rotationA = Matrix3x3::Rotate(bodyA.Orientation);
Matrix3x3 rotationB = Matrix3x3::Rotate(bodyB.Orientation);
Matrix3x3 inverseInertiaTensorA = Matrix3x3::Scale(bodyA.InverseInertia);
Matrix3x3 inverseInertiaTensorB = Matrix3x3::Scale(bodyB.InverseInertia);

Matrix3x3 inverseWorldInertiaTensorA = rotationA * inverseInertiaTensorA * Matrix3x3::Transpose(rotationA);
Matrix3x3 inverseWorldInertiaTensorB = rotationB * inverseInertiaTensorB * Matrix3x3::Transpose(rotationB);

// Flipped
Matrix3x3 inverseWorldInertiaTensorA = Matrix3x3::Transpose(rotationA) * inverseInertiaTensorA * rotationA;
Matrix3x3 inverseWorldInertiaTensorB = Matrix3x3::Transpose(rotationB) * inverseInertiaTensorB * rotationB;

As for checking it against Box2D, I actually made a functional 2d engine including physics some years ago. The obvious problem is that, in the two dimensional space, inertia, angular velocity and orientation can be represented by simple scalar values, whereas I'm dealing with a matrix, a vector and a quaternion respectively, so it requires different equations. That's what I think is where I'm doing something wrong, as it only fails for the angular part. I also tried calculating the rotation matrix differently, but again to no avail.
Like I said, I checked it against Bullet already. I also tried to figure out how qu3e does it a few days ago, but it seems to do things a bit differently as far as I could see.

One thing that might be worth noting is that it at least seems to work more or less when both bodies have zero rotation, but fails as their rotation changes. Applying impulses to individual bodies works too (using fixed values or using the cursor and ray casting), regardless of rotation.