Thank you hermann!
Your solution was much easier than anything I was going to come up with.
This is what I ended up with. No matter the orientation of the tool I just read the z position of the second point.
First Point
PR[20:SCRATCH] = LPOS
UFRAME[6]=PR[20]
Second point
UFRAME_NUM=6
PR[20:SCRATCH] = LPOS
R[200]=PR[20,3]
UFRAME_NUM=0