Add foolishly left out implementation, remove debug draw