Using PID should not be necessary for a simple problem like that -

they are hard to tune right and mostly there's a better analytical solution.

Torques ar harder than forces so first a simple example,

Calculate force to reach a target:

Vec3 targetLinvel = (targetPos - body.com) / timestep; // desired linear velocity to move target distance in one step targetLinvel -= body.linearVelocity; // subtract current velocity Vec3 force = 0.3 * targetLinvel * (body.mass / timestep); // soften by some factor <1 to avoid oszillation

This should work and you have only a single tuning factor, which is easy to understand and predict.

Now the same for orientation:

Vec3 axis; float angle; ToAxisAndAngle (&axis, &angle, object_forward, desired_forward); // normalized axis and radians Vec3 targetAngvel = axis * angle / timestep; targetAngvel -= body.angularVelocity; Vec3 torque = targetAngvel / timestep; torque = body.WorldSpaceMatrix.Unrotate (torque); // rotate to body local space... torque.x *= body.InertiaXX; //... so we can apply 3D inertia in the space where it is defined torque.y *= body.InertiaYY; torque.z *= body.InertiaZZ; torque = body.WorldSpaceMatrix.Rotate (torque); // rotate back to world space torque *= 0.3; // avoid oszillation