Fix bug leading to stack smashing
Fixes recastnavigation/recastnavigation#687
To address this, I basically reverted recastnavigation/recastnavigation@e063ba6
& recastnavigation/recastnavigation@adcd4f4 which seem to be the most likely culprit for the issue. Contrary to the commit message in e063ba6, which claims that the changes made are an optimization, reverting the changes had no measurable impact on performance.