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);
}
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.