Code:

// Get current orientationMatrix basis0 = entity.Orientation();// Compute desired orientationVector3 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);elseaxis /= Sqrt(len2);Vector3 delta = axis * angle;...

Code:

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 velocityomega -= GetAngularVelocity();// Compute acceleration needed to reach new velocity in t seconds.Vector3 alpha = omega / t;// Compute torque needed to achieve accelerationMatrix I = GetWorldInertiaTensor();Vector3 tau = I * alpha;// Don't exceed our limitif (tau.Length() > maxTorque)tau = tau.Normalized() * maxTorque;// Go!ApplyTorque(tau);}