Get intersections of skew lines at closest distance

Here’s another useful math snippet to return the intersections for two given lines (skew lines). The returned intersections are those if the two line were getting closer and closer together to finally intersects.

 
////////////////////////////////////////////////////////////////
///
/// Finds the closest intersection point on skew lines. 
/// It returns the t values for each infinite inputed lines.
///
/// @param s1 [IN]  a point on line 1
/// @param v1 [IN]  direction of line 1
/// @param s2 [IN]  a point on line 2
/// @param v2 [IN]  direction of line 2
/// @param p1 [OUT] intersection point on line 1
/// @param p2 [OUT] intersection point on line 2
///
/// @return the time values vector 
///       (only X and Y components or valid)
///
/// NOTE: 
///  The closest distance could be obtrains by doing (p2 - p1).Len()
///
const Vector3 GetSkewLineIntersections(
    const Vector3& s1, const Vector3& v1, 
    const Vector3& s2, const Vector3& v2, Vector3* p1, Vector3* p2)
{
    float d = 0.0f, v1dotv2 = 0.0f, v1p2 = 0.0f, v2p2 = 0.0f;
    Vector3 r(0,0,0), t(0,0,0);
    Matrix m = Matrix::I;
 
    // Precomputed values
    v1dotv2 = Dot3(v1, v2);
    v1p2 = v1.LenSquared();
    v2p2 = v2.LenSquared();
 
    // Solving matrix
    m[0][0] = -v2p2; // col 0, row 0
    m[1][0] = -v1dotv2; // col 0, row 1
    m[0][1] = v1dotv2; // col 1, row 0
    m[1][1] = v1p2; // col 1, row 1
 
    // Projected vector
    r.x = Dot3((s2 - s1), v1);
    r.y = Dot3((s2 - s1), v2);
 
    // precomputed value
    d = 1.0f / (v1dotv2 * v1dotv2 - v1p2 * v2p2);
 
    // Compute time values
    t = d * Mul3x3(m, r);
 
    // Compute intersected points on each lines
    if ( p1 ) { *p1 = s1 + t.x * v1; }
    if ( p2 ) { *p2 = s2 + t.y * v2; }
 
    return t;
}
This entry was posted in C++, Coding, Snippet and tagged , , . Bookmark the permalink.

Leave a Reply

Your email address will not be published. Required fields are marked *