It works! So happy :attn: :attn: !
I won't test it in too many directions, because I can't calculate that much in my head to verify if its right or wrong.

Also, at first, all I will use this for will be directly on an axis, and first as an infinite ground plane.

Next up: Ray-Plane intersection tests!