line intersect

gistfile1.txt

pcl::PointXYZ lineIntersect(const std::vector<float>& P, const std::vector<float>& Q){
    Eigen::Vector3f DP(P[3], P[4], P[5]);
    Eigen::Vector3f DQ(Q[3], Q[4], Q[5]);
    Eigen::Vector3f P0(P[0], P[1], P[2]);
    Eigen::Vector3f Q0(Q[0], Q[1], Q[2]);
    Eigen::Vector3f PQ = Q0 - P0;

    float a = DP.dot(DP);
    float b = DP.dot(DQ);
    float c = DQ.dot(DQ);
    float d = DP.dot(PQ);
    float e = DQ.dot(PQ);

    float DD = a * c - b * b;
    float tt = (b * e - c * d) / DD;
    float uu = (a * e - b * d) / DD;

    Eigen::Vector3f Pi = P0 + tt * DP;
    Eigen::Vector3f Qi = Q0 + uu * DQ;
    float l2 = sqrt(pow(Pi(0) - Qi(0), 2) + pow(Pi(1) - Qi(1), 2) + pow(Pi(2) - Qi(2), 2));

    std::cout << l2 << std::endl;

    return pcl::PointXYZ(Pi(0), Pi(1), Pi(2));
}