Whoops.

Completely replaced .cpp file, initially committed the wrong version.