Random movement and code cleaning

Random movement currently seg faults, cleaned up the heightfield.cpp
code