Finding the intersect location of two Rays

Question

I have two rays. Each ray has a start location vector (Vector3D) and a direction vector (Vector3D), but continue on to infinity. They are both on the same plane, but in a 3D environment. The Rays are interdependent, which means that they might not mirror each other perfectly. From this i need to calculate the location at which these rays intersect in the 3D environment and output it as a vector. In essence: a rangefinder.

How should i go about doing this? Is there a better way of doing it than using the C# Ray structure, is it even possible?

I am a pretty new coder (read: bad) but any answer is appreciated, I would enjoy it if an explanation was included.

Crude image of the rays

Answer 1

Two lines in 3D space only intersect if they are on the same plane. The probability that two random lines in space intersect is really small.

When you want to find out if two rays intersect, if you are looking for an exact intersection point, the chance is that you are not going to be able to calculate it due to floating point errors.

The next best thing is to find the shortest distance between two rays. Then if that distance is smaller than a certain threshold (defined by you) we could say the rays are intersecting.

---

Finding shortest distance

Here are two rays in 3D space, with the blue vector representing the shortest distance.

Let's take a frame from that gif:

Legend:

• p1 is ray1.Position
• p2 is ray2.Position
• d1 is ray1.Direction
• d2 is ray2.Direction
• d3 is the cross product d1 x d2

The cross product of the rays directions will be perpendicular to both rays, so it's the shortest direction from ray to ray. If lines are parallel, the cross product will be zero, but for now lets only deal with non-parallel lines.

From the photo, we get the equation:

• p1 + a*d1 + c*d3 = p2 + b*d2

Rearranged so the variables are on the left:

• a*d1 - b*d2 + c*d3 = p2 - p1

Since each of the know values (d1, d2, d3, p1 and p2) has three components (x,y,z), this is a system of three linear equations with 3 variables.

• a*d1.X - b*d2.X + c*d3.X = p2.X - p1.X
• a*d1.Y - b*d2.Y + c*d3.Y = p2.Y - p1.Y
• a*d1.Z - b*d2.Z + c*d3.Z = p2.Z - p1.Z

Using Gaussian elimination, we get the values of a, b and c.

If both a and b are positive, the position of the intersection will be

• Vector3 position = ray1.Position + a*ray1.Direction;
• Vector3 direction = c * d3; //direction.Length() is the distance

You could return these values as a Ray for convenience.

If either a or b are negative, this means that the calculated shortest distance would be behind one (or both) of the rays, so a different method should be used to find the shortest distance. This method is the same for lines that are parallel (cross product d1 x d2 is zero).

Now the calculation becomes finding which ray (positive direction) is closest to the position (p1 or p2) of the other ray. For this we use dot product (projection of a vector onto another vector)

Legend:

• dP = p2 - p1

Before calculating dot product of d1 dot dP, make sure that d1 (or d2) are normalized (Vector3.Normalize()) - dot product is meant to work on unit vectors.

Now it's a matter of finding which is the shortest distance, based on the projection factor (result of dot) on ray1 (lets call it a2) and projection factor on ray2 (lets call it b2).

If both a2 and b2 are negative (negative side of the ray), then the shortest distance is from position to position. If one is in the negative direction then the other one is the shortest. Else it's the shorter of the two.

---

Working code:

public Ray FindShortestDistance(Ray ray1, Ray ray2)
{
    if (ray1.Position == ray2.Position) // same position - that is the point of intersection
        return new Ray(ray1.Position, Vector3.Zero);

    var d3 = Vector3.Cross(ray1.Direction, ray2.Direction);

    if (d3 != Vector3.Zero) // lines askew (non - parallel)
    {
        //d3 is a cross product of ray1.Direction (d1) and ray2.Direction(d2)
        //    that means d3 is perpendicular to both d1 and d2 (since it's not zero - we checked that)    
        //
        //If we would look at our lines from the direction where they seem parallel 
        //    (such projection must always exist for lines that are askew)
        //    we would see something like this
        //
        //   p1   a*d1
        //   +----------->x------
        //                |
        //                | c*d3
        //       p2  b*d2 v 
        //       +------->x----
        //
        //p1 and p2 are positions ray1.Position and ray2.Position - x marks the points of intersection.
        //    a, b and c are factors we multiply the direction vectors with (d1, d2, d3)
        //
        //From the illustration we can the shortest distance equation
        //    p1 + a*d1 + c*d3 = p2 + b*d2
        //
        //If we rearrange it so we have a b and c on the left:
        //    a*d1 - b*d2 + c*d3 = p2 - p1
        //
        //And since all of the know variables (d1, d2, d3, p2 and p1) have 3 coordinates (x,y,z)
        //    now we have a set of 3 linear equations with 3 variables.
        //   
        //    a * d1.X - b * d2.X + c * d3.X = p2.X - p1.X
        //    a * d1.Y - b * d2.Y + c * d3.Y = p2.Y - p1.Y
        //    a * d1.Z - b * d2.Z + c * d3.Z = p2.Z - p1.Z
        //
        //If we use matrices, it would be
        //    [d1.X  -d2.X  d3.X ]   [ a ]   [p2.X - p1.X]
        //    [d1.Y  -d2.Y  d3.Y ] * [ a ] = [p2.Y - p1.Y]
        //    [d1.Z  -d2.Z  d3.Z ]   [ a ]   [p2.Z - p1.Z]
        //
        //Or in short notation
        //
        //   [d1.X  -d2.X  d3.X | p2.X - p1.X]
        //   [d1.Y  -d2.Y  d3.Y | p2.Y - p1.Y]
        //   [d1.Z  -d2.Z  d3.Z | p2.Z - p1.Z]
        //
        //After Gaussian elimination, the last column will contain values a b and c

        float[] matrix = new float[12];

        matrix[0] = ray1.Direction.X;
        matrix[1] = -ray2.Direction.X;
        matrix[2] = d3.X;
        matrix[3] = ray2.Position.X - ray1.Position.X;

        matrix[4] = ray1.Direction.Y;
        matrix[5] = -ray2.Direction.Y;
        matrix[6] = d3.Y;
        matrix[7] = ray2.Position.Y - ray1.Position.Y;

        matrix[8] = ray1.Direction.Z;
        matrix[9] = -ray2.Direction.Z;
        matrix[10] = d3.Z;
        matrix[11] = ray2.Position.Z - ray1.Position.Z;

        var result = Solve(matrix, 3, 4);

        float a = result[3];
        float b = result[7];
        float c = result[11];

        if (a >= 0 && b >= 0) // normal shortest distance (between positive parts of the ray)
        {
            Vector3 position = ray1.Position + a * ray1.Direction;
            Vector3 direction = d3 * c;

            return new Ray(position, direction);
        }
        //else will fall through below:
        //    the shortest distance was between a negative part of a ray (or both rays)
        //    this means the shortest distance is between one of the ray positions and another ray 
        //    (or between the two positions)
    }

    //We're looking for the distance between a point and a ray, so we use dot products now
    //Projecting the difference between positions (dP) onto the direction vectors will
    //   give us the position of the shortest distance ray.
    //The magnitude of the shortest distance ray is the the difference between its 
    //    position and the other rays position

    ray1.Direction.Normalize(); //needed for dot product - it works with unit vectors
    ray2.Direction.Normalize();

    Vector3 dP = ray2.Position - ray1.Position;

    //shortest distance ray position would be ray1.Position + a2 * ray1.Direction
    //                                     or ray2.Position + b2 * ray2.Direction (if b2 < a2)
    //                                     or just distance between points if both (a and b) < 0
    //if either a or b (but not both) are negative, then the shortest is with the other one
    float a2 = Vector3.Dot(ray1.Direction, dP);
    float b2 = Vector3.Dot(ray2.Direction, -dP);

    if (a2 < 0 && b2 < 0)
        return new Ray(ray1.Position, dP);


    Vector3 p3a = ray1.Position + a2 * ray1.Direction;
    Vector3 d3a = ray2.Position - p3a;

    Vector3 p3b = ray1.Position;
    Vector3 d3b = ray2.Position + b2 * ray2.Direction - p3b;

    if (b2 < 0)
        return new Ray(p3a, d3a);

    if (a2 < 0)
        return new Ray(p3b, d3b);

    if (d3a.Length() <= d3b.Length())
        return new Ray(p3a, d3a);

    return new Ray(p3b, d3b);
}

//Solves a set of linear equations using Gaussian elimination
float[] Solve(float[] matrix, int rows, int cols)
{
    for (int i = 0; i < cols - 1; i++)
        for (int j = i; j < rows; j++)
            if (matrix[i + j * cols] != 0)
            {
                if (i != j)
                    for (int k = i; k < cols; k++)
                    {
                        float temp = matrix[k + j * cols];
                        matrix[k + j * cols] = matrix[k + i * cols];
                        matrix[k + i * cols] = temp;
                    }

                j = i;

                for (int v = 0; v < rows; v++)
                    if (v == j)
                        continue;
                    else
                    {
                        float factor = matrix[i + v * cols] / matrix[i + j * cols];
                        matrix[i + v * cols] = 0;

                        for (int u = i + 1; u < cols; u++)
                        {
                            matrix[u + v * cols] -= factor * matrix[u + j * cols];
                            matrix[u + j * cols] /= matrix[i + j * cols];
                        }
                        matrix[i + j * cols] = 1;
                    }
                break;
            }

    return matrix;
}

---