Points on lines where the two lines are the closest together

Question

I'm trying to find the points on two lines where the two lines are the closest. I've implemented the following method (Points and Vectors are as you'd expect, and a Line consists of a Point on the line and a non-normalized direction Vector from that point):

void CDClosestPointsOnTwoLines(Line line1, Line line2, Point* closestPoints)
{
    closestPoints[0] = line1.pointOnLine;
    closestPoints[1] = line2.pointOnLine;

    Vector d1 = line1.direction;
    Vector d2 = line2.direction;

    float a = d1.dot(d1);
    float b = d1.dot(d2);
    float e = d2.dot(d2);

    float d = a*e - b*b;
    if (d != 0) // If the two lines are not parallel.
    {
        Vector r = Vector(line1.pointOnLine) - Vector(line2.pointOnLine);
        float c = d1.dot(r);
        float f = d2.dot(r);

        float s = (b*f - c*e) / d;
        float t = (a*f - b*c) / d;
        closestPoints[0] = line1.positionOnLine(s);
        closestPoints[1] = line2.positionOnLine(t);
    }
    else
    {
        printf("Lines were parallel.\n");
    }
}

I'm using OpenGL to draw three lines that move around the world, the third of which should be the line that most closely connects the other two lines, the two end points of which are calculated using this function.

The problem is that the first point of closestPoints after this function is called will lie on line1, but the second point won't lie on line2, let alone at the closest point on line2! I've checked over the function many times but I can't see where the mistake in my implementation is. I've checked my dot product function, scalar multiplication, subtraction, positionOnLine() etc. etc. So my assumption is that the problem is within this method implementation.

If it helps to find the answer, this is function supposed to be an implementation of section 5.1.8 from 'Real-Time Collision Detection' by Christer Ericson.

Many thanks for any help!

Answer 1

I've tested your code on a little program with OpenGL on a 2D space and the algorithm seems correct.

You says that direction is a non-normalized vector. Review that the method positionOnLine uses a normalized vector to do his calculations. I've used:

Vector2Df positionOnLine( float d )
{
    // where m_direction is a normalized vector
    return m_point + m_direction * d; 
}

More code:

My code is like this:

struct Line
{
    Line( const Vector2Df& p, const Vector2Df& d )
        : m_point( p )
        , m_direction( d )
    {
        m_direction.normalize();
    }

    Vector2Df positionOnLine( float d )
    {
        return m_point + m_direction * d;
    }

    Vector2Df   m_point;
    Vector2Df   m_direction;
};

// Lines creation
Vector2Df v1( 0.f, 0.f );
Vector2Df v2( 100.f, 100.f );

Vector2Df v3( 10.f, 0.f );
Vector2Df v4( 100.f, 110.f );

Line l1( v1, v2 - v1 );
Line l2( v3, v4 - v3 );