Fixed missing z when transforming point; Added missing return value for...

Fixed missing z when transforming point; Added missing return value for exception cases; Added usage of const refs