Fixed Z shift bug and added constant z offset