C++: use refs instead of indexes

This implementation is about 10% faster on my PC. 
But I'm not sure why, so you might want to try it before merging.