Following up on #3231

Keep "astyled" reformatting

With the introduction of the #if ENABLED(…)

reads better than

Apply `ENABLED` / `DISABLED` macros to files needing only a small
number of changes.

- Also patch up some warnings

Removed the calculation of the inverse matrix since the rotation matrix is orthogonal, therefore inverted == transposed.
Much simpler and mathematically robust.

Check the Readme for instruction how to enable and configure the feature