How to compute torque needed to rotate to a point

[Jim Offerman]

I've been trying to write a function that computes how much torque must be applied to rotate a rigidbody to face a certain point:

Code: Select all

void RigidBody::RotateToPoint(const Vector3& point, float maxTorque)
{
	const Entity& entity = GetEntity();
	
	// Angular distance between current orientation and desired orientation.
	Vector3 localDir = entity.PointToLocal(point).Normalized();
	float x = atan2f(localDir.y, localDir.z);
	float y = 0.f; // atan2f(localDir.z, localDir.x);
	float z = 0.f; // atan2f(localDir.y, localDir.z);
	Vector3 delta = entity.VectorToWorld(Vector3(x, y, z));

	// Compute the angular velocity we need to reach the point in t seconds.
	float t = Time_GetDeltaTime();
	Vector3 omega = delta / t;

	// Subtract current velocity
	omega -= GetAngularVelocity();

	// Compute acceleration needed to reach new velocity in t seconds.
	Vector3 alpha = omega / t;

	// Compute torque needed to achieve acceleration
	Matrix I = GetWorldInertiaTensor();
	Vector3 tau = I * alpha;

	// Don't exceed our limit
	if (tau.Length() > maxTorque)
		tau = tau.Normalized() * maxTorque;

	// Go!
	ApplyTorque(tau);
}

(RigidBody is a thin wrapper around btRigidBody that hooks it into my engine's component system and takes care of right-handed <-> left-handed conversions)

The above code works as long as I restrict my calculations to a single axis. Unfortunately, it completely breaks down when I try the general version.

I suspect the problem is in how I compute the angular distance that the body must travel. Could any of you help me out with that?

thanks,
Jim.

[Erwin Coumans]

Checkout calculateVelocity in src/LinearMath/btTransformUtil.cpp

Thanks,
Erwin

[Jim Offerman]

Thanks for the tip! I've copied the delta axis/angle calculations from the calculateDiffAxisAngle() call in calculateVelocity:

Code: Select all

		// Get current orientation
		Matrix basis0 = entity.Orientation();
		
		// Compute desired orientation
		Vector3 az = (point - pos).Normalized();
		Vector3 ax = Cross(entity.Up(), az).Normalized();
		if (ax.LengthSquared() < FLT_EPSILON * FLT_EPSILON)
			ax = Vector3(1.f, 0.f, 0.f);
		Vector3 ay = Cross(az, ax).Normalized();
		Matrix basis1(ax.x, ay.x, az.x, 0.f,
					  ax.y, ay.y, az.y, 0.f,
					  ax.z, ay.z, az.z, 0.f,
					  0.f,  0.f,  0.f,  1.f);
		
		// Code copied from calculateDiffAxisAngle()
		Quaternion dorn = basis1 * basis0.Inverse();
		dorn.Normalize();
		
		float angle = dorn.Angle();
		Vector3 axis(dorn.x, dorn.y, 0.f/*dorn.z*/);
		float len2 = axis.LengthSquared();
		if (len2 < FLT_EPSILON * FLT_EPSILON)
			axis = Vector3(1.f, 0.f, 0.f);
		else
			axis /= Sqrt(len2);
		
		Vector3 delta = axis * angle;
		...

Unfortunately, this makes my function even more unstable
Anything else I could be doing wrong?

[Jim Offerman]

Works now!

I finally settled on using the local-space heading and elevation angles to compute delta, which produces stable results for me. I still get some overshoot when the angles between the current heading and target are >90 degrees, but for frame-to-frame course correction it works very well.