Removed redundant factorizing of inertia matrix