Added libdogleg formula

New pull request, with cleaner history