Skip to content

Instantly share code, notes, and snippets.

# soulslicer/gist:26c4d6d36407e37c21cdf033c532a9ca

Created September 3, 2020 18:39
Show Gist options
• Save soulslicer/26c4d6d36407e37c21cdf033c532a9ca to your computer and use it in GitHub Desktop.
line intersect
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters. Learn more about bidirectional Unicode characters
 pcl::PointXYZ lineIntersect(const std::vector& P, const std::vector& 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)); }
to join this conversation on GitHub. Already have an account? Sign in to comment