How to find the intersection of two lines in a 3D space using jmonkeyengine3 (or possibly another library)?

Question

At first I tried something like this:

  Ray ray1 = new Ray();
  Vector3f originRay1 = new Vector3f(0, 0, 1);
  ray1.setOrigin(originRay1);
  Vector3f directionRay1 = new Vector3f(0, 0, -1);
  ray1.setDirection(directionRay1 );

  Ray ray2 = new Ray();
  Vector3f originRay2 = new Vector3f(1, 0, 0);
  Vector3f directionRay2 = new Vector3f(-1, 0, 0);
  ray2.setDirection(directionRay2 );
  ray2.setOrigin(originRay2);

  CollisionResults results= new CollisionResults();
  int collide = ray1.collideWith(ray2, results);

But that throws an UnsupportedCollisionException, so it's not something that can be calculated using two Ray objects.

To be honest I don't know what I expected when I tried this. There can't be any line collision algorithm without taking into account a delta / margin of error of some sort or a different kind of result altogether by, for example, returning the shortest vector between the two lines. Or at least such an algorithm wouldn't make much sense to me!

In any case I also looked into classes Line and LineSegment but they don't implement the Collidable interface. Then I looked for some candidate that does implement Collidable and resembles a line, but I don't see a clear candidate.

I'd rather do it using jme3 or the JDK libraries if possible, but I'm open to read other suggestions.

As mentioned before I would have to take into account the precision somehow. For example, there would be an intersection if the distance between the lines is below a 'delta' that I pass as parameter and then it returns the point on one of the lines if the shortest distance between the lines is less than that delta.

Answer 1

A safe way is to compute the shortest distance between the two lines.

This is easily done by taking the cross product of the direction vectors of the two lines, which gives the direction of the common perpendicular, normalizing it then computing the scalar product of this vector with any vector from a point of the first line to a point of the second.

Let the vector equations be

A1 + t1 D1
A2 + t2 D2

Then the distance:

d12 = |(A2 - A1).(D1 x D2)| / |D1 x D2|

If the lines are given by points PQ and RS,

d = |(P - R).((Q - P) x (S - R))| / |(Q - P) x (S - R)|

Answer 2

I think it could be useful to someone else if I posted here my Java implementation, which is nothing more than the translation of this c implementation as suggested in the comments above and some unit tests.

I also believe the result is more readable than the original, it throws exceptions when the arguments are invalid instead of returning a null kind of result and the scope of variables is reduced.

A couple of observations about the code:

1. I understood the epsilon EPS in the original version as the minimum distance between the coordinates of two points in order to define a line. I already use such a constant with the long, explicit name NUMBERS_SHOULD_BE_DIFFERENT_DELTA which is the minimum distance needed between two points so that loss of precision in calculations don't have an adverse effect in the result. I believe in general another such delta is needed in applications with geometry calculations when comparing whether points are almost equal. Hence the long name to differentiate them.

2. LineSegment3D class, not included here, is just a thin wrapper for jme3's LineSegment in case you are wondering why I'm using jme3 and not jme3's LineSegment.

Not relevant to the question, but the reason for this is that I prefer to distinguish between the semantics of vectors and points (jme3 only uses vectors everywhere).

import static com.google.common.base.Preconditions.checkNotNull;
import static com.google.common.base.Preconditions.checkArgument;
import static java.lang.Math.abs;
import javax.vecmath.Point3d;

//...

   /**
    * Calculate the line segment that is the shortest route between the two lines
    * determined by the segments.
    * 
    * Even though we are passing segments as arguments the result is the intersection of the lines in which 
    * the segments are contained, not the intersection of the segments themselves.
    * 
    */
   public static LineSegment3D lineToLineIntersection(LineSegment3D segmentA, LineSegment3D segmentB) { 
      checkNotNull(segmentA, "Segment cannot be null.");
      checkNotNull(segmentB, "Segment cannot be null.");

      Point3d p1 = segmentA.getPoints().getValue0();
      Point3d p2 = segmentA.getPoints().getValue1();
      Point3d p3 = segmentB.getPoints().getValue0();
      Point3d p4 = segmentB.getPoints().getValue1();

      Point3d p43 = new Point3d(p4.x - p3.x, p4.y - p3.y, p4.z - p3.z);      
      checkArgument(!(abs(p43.x) < NUMBERS_SHOULD_BE_DIFFERENT_DELTA && 
                        abs(p43.y) < NUMBERS_SHOULD_BE_DIFFERENT_DELTA && 
                        abs(p43.z) < NUMBERS_SHOULD_BE_DIFFERENT_DELTA), MSG_INVALID_POINTS_FOR_INTERSECTION_CALCULATION);
      Point3d p21 = new Point3d(p2.x - p1.x, p2.y - p1.y, p2.z - p1.z);
      checkArgument(!(abs(p21.x) < NUMBERS_SHOULD_BE_DIFFERENT_DELTA && 
                        abs(p21.y) < NUMBERS_SHOULD_BE_DIFFERENT_DELTA && 
                        abs(p21.z) < NUMBERS_SHOULD_BE_DIFFERENT_DELTA), MSG_INVALID_POINTS_FOR_INTERSECTION_CALCULATION);
      Point3d p13 = new Point3d(p1.x - p3.x, p1.y - p3.y, p1.z - p3.z);
      double d1343 = p13.x * p43.x + p13.y * p43.y + p13.z * p43.z;
      double d4321 = p43.x * p21.x + p43.y * p21.y + p43.z * p21.z;
      double d4343 = p43.x * p43.x + p43.y * p43.y + p43.z * p43.z;
      double d2121 = p21.x * p21.x + p21.y * p21.y + p21.z * p21.z;
      double denom = d2121 * d4343 - d4321 * d4321;
      checkArgument(abs(denom) >= NUMBERS_SHOULD_BE_DIFFERENT_DELTA, MSG_INVALID_POINTS_FOR_INTERSECTION_CALCULATION);
      double d1321 = p13.x * p21.x + p13.y * p21.y + p13.z * p21.z;
      double numer = d1343 * d4321 - d1321 * d4343;

      double mua = numer / denom;
      double mub = (d1343 + d4321 * mua) / d4343;

      return new LineSegment3D(
            new Point3d(p1.x+mua*p21.x, p1.y+mua*p21.y, p1.z+mua*p21.z), 
            new Point3d(p3.x+mub*p43.x, p3.y+mub*p43.y, p3.z+mub*p43.z));
   }

A couple of JUnit 4 test cases. Notice that I'm also using a custom method to test if two points are similar enough to be considered the same:

   @Test
   public void testLineToLineIntersection_LineAlongZAxis_LineAlongXAxis() {
      LineSegment3D segmentA = new LineSegment3D(new Point3d(1, 0, 0), new Point3d(3, 0, 0));
      LineSegment3D segmentB = new LineSegment3D(new Point3d(0, 0, -1), new Point3d(0, 0, 5));
      LineSegment3D segment = GeometryUtil.lineToLineIntersection(segmentA, segmentB);

      Point3d expected = new Point3d(0, 0, 0);
      Pair<Point3d, Point3d> segmentPoints = segment.getPoints();

      Assert.assertTrue( GeometryUtil.almostEqual(segmentPoints.getValue0(), expected));
      Assert.assertTrue( GeometryUtil.almostEqual(segmentPoints.getValue1(), expected));
   }

   @Test
   public void testLineToLineIntersection_LineAlongZAxis_LineParallelXAxis_DoNotCross() {
      LineSegment3D segmentA = new LineSegment3D(new Point3d(1, 0, 0), new Point3d(3, 0, 0));
      LineSegment3D segmentB = new LineSegment3D(new Point3d(0, 1, -1), new Point3d(0, 1, 5));
      LineSegment3D segment = GeometryUtil.lineToLineIntersection(segmentA, segmentB);

      Pair<Point3d, Point3d> segmentPoints = segment.getPoints();

      Point3d expectedFrom = new Point3d(0, 0, 0);
      Point3d expectedTo = new Point3d(0, 1, 0);

      Assert.assertTrue( GeometryUtil.almostEqual(segmentPoints.getValue0(), expectedFrom));
      Assert.assertTrue( GeometryUtil.almostEqual(segmentPoints.getValue1(), expectedTo));
   }

   //I created this test by using 
   //https://technology.cpm.org/general/3dgraph/
   //it's pretty easy to create four points and play around until one can ensure that the lines approximately intersect
   //The calculations for creating intersecting examples are quite easy too, this just saved a little more time and it's good enough for me
   @Test
   public void testLineToLineIntersection_RandomLinesAlmostIntersect() {
      LineSegment3D segmentA = new LineSegment3D(new Point3d(-3, -2, 4), new Point3d(1, 3, 2));
      LineSegment3D segmentB = new LineSegment3D(new Point3d(-1, -2, 1), new Point3d(-1, 4, 6));
      LineSegment3D segment = GeometryUtil.lineToLineIntersection(segmentA, segmentB);

      Pair<Point3d, Point3d> segmentPoints = segment.getPoints();

      double distance = segmentPoints.getValue0().distance(segmentPoints.getValue1());

      Assert.assertTrue( distance < 0.1);
   }