Got OpenCV ray casting working