environment refactoring

switched over to using vectors for position and orientation
replaced switches on heading with rotation matrices
replaced environment storage with torus2