Calculating the angular velocity change of a Rigidbody given torque and timestep

Unity Engine
, , ,
1

Hello.

I’m trying to calculate the angular velocity change to a standard 3D Rigidbody when a torque acts on it over a timestep so that it matches the outcome of a Rigidbody being simulated over the same timestep.

I have equivalent code running just fine for velocity, and it matches the results of Rigidbody simulation closely enough to be integrated over approx. 50-100 FixedUpdates with only very small differences in the final velocity…

Finally, let’s assume that I have a good reason for needing to do this, and consequently I’m not looking for a discussion of why I need to do it, just help for where I’m going wrong with the calculation(s) :smiley:

Linked is a super simple git repo showing for the issue(s) I’m having:

The key function is AngularVelocityChangeFromTorque.CalculateRigidBodyAngularVelocityChange

Here it is in full:

//------------------------------------------------------------------------
Vector3 CalculateRigidBodyAngularVelocityChange( Rigidbody rigidbody, Vector3 accumulatedTorqueWorldSpace, float timeStep )
	{
		//
		// context:
		// --------
		//
		// * rotational equivalent of f = ma has "moment of inertia" (MoI) rather than mass
		//		which in 3D is represented as a matrix
		//
		// * there's no matrix divide; the equivalent is multiply by inverse - so to get to angular acceleration
		//		from accumulater torque we need to multiply it by the inverse of the MoI matrix
		//
		// * https://docs.unity3d.com/2022.3/Documentation/ScriptReference/Matrix4x4.html
		// 		* Matrices in Unity are column major; i.e. the position of a transformation matrix is in
		//			the last column, and the first three columns contain x, y, and z-axes.
		// 		* Data is accessed as: row + (column*4).
		// 		* Matrices can be indexed like 2D arrays but note that in an expression like mat[a, b],
		// 			a refers to the row index, while b refers to the column index.
		//
		//
		// assumptions about Unity physics representations (which may be wrong?):
		// ----------------------------------------------------------------------
		// * in local space of a rigidbody, the MoI is a 3x3 matrix with Rigidbody.inertiaTensor in the diagonal
		// where there would be 1, 1, 1 in an identity matrix.
		//
		// * accumulated torque is in world space
		//
		// * Since accumulated torque is in world space, we have to convert the rigidbody's inverse MoI matrix into
		//		world space (i.e. same space as accumulated torque)

		// currently not taking account of manuallyScaledRigidbody.inertiaTensorRotation
		var diagonalMoiTensor = rigidbody.inertiaTensor;
		var moiMatrix         = Matrix4x4.zero;

		// Unity matrix format is columns hold basis axes, brackets are [row, column]
		moiMatrix[ 0, 0 ] = diagonalMoiTensor.x;
		moiMatrix[ 1, 1 ] = diagonalMoiTensor.y;
		moiMatrix[ 2, 2 ] = diagonalMoiTensor.z;
		moiMatrix[ 3, 3 ] = 1f;

		// don't want any scale or translation here so generating a local -> world matrix from just rotation
		var localToWorldRotationMatrix = Matrix4x4.Rotate( rigidbody.transform.rotation );

		// need to include rigidbody.inertiaTensorRotation; but whenever I try to incorporate it I end up wildly off straight away
		var inertiaMatrixInverseWorldSpace = moiMatrix.inverse * localToWorldRotationMatrix;

		// there's no position in inertiaMatrixInverseWorldSpace so it _should_ be just rotation?
		var angularAcceleration  = inertiaMatrixInverseWorldSpace.MultiplyVector( accumulatedTorqueWorldSpace );
		var deltaAngularVelocity = angularAcceleration * timeStep;

		// seems rigidbody.angularVelocity is in Rigidbody local space?
		return rigidbody.transform.localToWorldMatrix.inverse.MultiplyVector( deltaAngularVelocity );
	}

I’d appreciate any help at all that anyone can offer…

Main stuff:

  • is this even close to correct? (seems to be - seems to work fine for uniform inertia tensors…)

  • are my assumptions ok? am I missing something?

  • how should I include rigidbody.inertiaTensorRotation in the calculation - I tried making a matrix from it and multiplying the inertia matrix by it but every permutation I tried with any non-identity value for rigidbody.inertiaTensorRotation seemed to be wildly out…

  • thanks in advance I very appreciate your help!

Alex

2

Here’s the correct calculation to convert from inertiaTensor/inertiaTensorRotation to a 3x3 matrix:

You can find the correct formulas and pseudocode to calculate the angular velocity of a rigid body in this paper:

However, even if you implement perfectly correct and precise calculations, they will match the outcome of a Unity Rigidbody only if the 3x3 inertia tensor is diagonal, that is, when inertiaTensorRotation is an identity rotation. You’ll find large discrepancies when the inertia tensor matrix is not diagonal.

The reason is that PhysX 4, the underlying 3D physics engine in Unity, deliberately uses fake calculations that don’t integrate Euler’s equations. The results look mostly ok, and indeed they are correct when the inertia tensor is diagonal, but produce unrealistic results with objects with non-uniform inertia (mostly anything but primitive shapes).

This was confirmed by PhysX devs themselves:

Hope all this helps!

3 Likes
How soon PhysX 5.0 support will be added to Unity?
3

Thanks so much for that code - will give it a try :heart:

The reason is that PhysX 4, the underlying 3D physics engine in Unity, deliberately uses fake calculations that don’t integrate Euler’s equations. The results look mostly ok , and indeed they are correct when the inertia tensor is diagonal, but produce unrealistic results with objects with non-uniform inertia (mostly anything but primitive shapes).

whaaaaaa!?! that is egg, poo, wee, and egg.

I have been stressing about this for days and it turns out that the underlying physx code is actually “wrong”?!

So, the best I can do is test that the calculation I’m doing matches physX when the inertia tensor is uniform?

4

Yes.

Also yes.

When an object has a non-uniform inertia tensor, its axis of rotation will oscillate even in the absence of external torques or forces. However, in PhysX 4, the rotation axis remains constant regardless of the inertia tensor, unless a force or torque is applied.

Correct Rigidbody dynamics are implemented in PhysX 5, but as of now, Unity won’t be updating to this version:

1 Like
5

thanks so much for the replies! in that case my stuff should all work fine for my purposes - I’ll just make all the tests use uniform tensors.

As it goes I don’t need my manually updated objects to exactly match Unity’s simulation as long as they look correct, I was really just comparing against them as an example of “simulation where the maths are correct” (lol) so I could check my code was close enough…

also, have now included code to handle the inertia tensor rotation using the info in the links you posted & all seems to be working as expected :+1:

cheers, darbs

1 Like