test_code.h

Go to the documentation of this file.

 
//---------------------------------------------------------------------------------------------------
bool do3DLinesIntersect(const std::vector<Line::FitLine2D>& linesA, const std::vector<Line::FitLine2D>& linesB) {
    std::vector<Vector3D> intersectionPointsA;
    std::vector<Vector3D> intersectionPointsB;
 
    // 1. Check intersections in 2D
    for (size_t i = 0; i < 3; ++i) {
        IntersectionResult res = findIntersection(linesA[i], linesB[i]);
 
        if (!res.intersects && !res.areCoincident) return false;
 
        // Convert 2D intersection points into 3D
        if (i == 0) {  // XY plane
            intersectionPointsA.push_back(Vector3D(res.point.x, res.point.y, 0));
            intersectionPointsB.push_back(Vector3D(res.point.x, res.point.y, 0));
        } else if (i == 1) {  // XZ plane
            intersectionPointsA.push_back(Vector3D(res.point.x, 0, res.point.y));
            intersectionPointsB.push_back(Vector3D(res.point.x, 0, res.point.y));
        } else {  // YZ plane
            intersectionPointsA.push_back(Vector3D(0, res.point.x, res.point.y));
            intersectionPointsB.push_back(Vector3D(0, res.point.x, res.point.y));
        }
    }
 
    // 2. Measure distance between 3D intersection points
    for (size_t i = 0; i < 3; ++i) {
        double dist = distance3D(intersectionPointsA[i], intersectionPointsB[i]);
        if (dist > INTERSECTION_DISTANCE_THRESHOLD) return false;
    }
 
    // 3. Convert FitLine2D to 3D lines (you might already have a method for this)
    Line3D line3DA = convertTo3DLine(linesA);
    Line3D line3DB = convertTo3DLine(linesB);
 
    // 4. Calculate perpendicular distance between 3D lines
    double perpendicularDistance = distanceBetween3DLines(line3DA, line3DB);
    return perpendicularDistance <= PERPENDICULAR_DISTANCE_THRESHOLD;
}
 
double distance3D(const Vector3D& a, const Vector3D& b) {
    return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z));
}
 
double distanceBetween3DLines(const Line3D& lineA, const Line3D& lineB) {
    // This function calculates the shortest distance between two 3D lines. 
    // Please refer to any 3D geometry resource for a formula on how to achieve this.
    // The formula is a bit involved but standard in computational geometry.
 
    // ... [code to compute distance]
 
    return distance;
}
//---------------------------------------------------------------------------------------------------