opencv 求两条线的交叉点

#include <opencv2\highgui\highgui.hpp>
#include <opencv2\opencv.hpp>
using namespace std;
using namespace cv;

Point2f CrossPoint(vector<Point2f> dots)
{
    Point2f pt;
    double X1 = dots[3].x - dots[0].x;//b1
    double Y1 = dots[3].y - dots[0].y;//a1
    double X2 = dots[2].x - dots[1].x;//b2
    double Y2 = dots[2].y - dots[1].y;//a2

    double X21 = dots[1].x - dots[0].x;
    double Y21 = dots[1].y - dots[0].y;

    double D = Y1*X2 - Y2*X1;// a1b2-a2b1
                             // 
    if (D == 0) return 0;
    pt.x = (X1*X2*Y21 + Y1*X2*dots[0].x - Y2*X1*dots[1].x) / D;
    pt.y = -(Y1*Y2*X21 + X1*Y2*dots[0].y - X2*Y1*dots[1].y) / D;
    if ((abs(pt.x - dots[0].x - X1 / 2) <= abs(X1 / 2)) &&
        (abs(pt.y - dots[0].y - Y1 / 2) <= abs(Y1 / 2)) &&
        (abs(pt.x - dots[1].x - X2 / 2) <= abs(X2 / 2)) &&
        (abs(pt.y - dots[1].y - Y2 / 2) <= abs(Y2 / 2))) {
        return pt;
    }
    return 0;
}

int main()
{
    Mat srcImage(Size(800, 800), CV_8UC3, Scalar(0));

    Point2f a(200, 300);
    Point2f b(600, 300);
    Point2f c(200, 600);
    Point2f d(600, 600);

    circle(srcImage, a, 6, Scalar(0, 255, 0), -1, 8, 0);
    circle(srcImage, b, 6, Scalar(0, 255, 0), -1, 8, 0);
    circle(srcImage, c, 6, Scalar(0, 255, 0), -1, 8, 0);
    circle(srcImage, d, 6, Scalar(0, 255, 0), -1, 8, 0);

    line(srcImage, a, d, Scalar(0, 0, 255), 2);
    line(srcImage, b, c, Scalar(0, 0, 255), 2);

    vector<Point2f> dots;
    dots.push_back(a);
    dots.push_back(c);
    dots.push_back(b);
    dots.push_back(d);



    Point2f jx = CrossPoint(dots);

    circle(srcImage, jx, 10, Scalar(0, 255, 255), 2, 8, 0);

    imshow("srcImage", srcImage);
    waitKey(0);
    return 0;
}