Logo Questions Linux Laravel Mysql Ubuntu Git Menu
 

3D Ray-Quad intersection test in java

In 3D space I am trying to determine if a ray/line intersects a square and if so, the x and y position on the square that it intersects.

I have a ray represented by two points:

R1 = (Rx1, Ry1, Rz1) and 
R2 = (Rx2, Ry2, Rz2)

And the square is represented by four vertices:

S1 = (Sx1, Sy1, Sz1), 
S2 = (Sx2, Sy2, Sz2), 
S3 = (Sx3, Sy3, Sz3) and 
S4 = (Sx4, Sy4, Sz4).

I’ve found lots of algebraic equations for this online but none seem to fit this problem exactly. Ideally I would like the answer in Java code, but an equation that I can easily convert to code will do also.

All help will be appreciated.

like image 242
Smalesy Avatar asked Jan 14 '14 13:01

Smalesy


1 Answers

Here is an overview of the solution:

  1. Compute the plane equation of the square (assuming the four points are coplanar),

  2. Do a ray / plane intersection, this gives you either nothing (ray parallel to the square and I ignore the case where the ray is embedded in the plane) or a point,

  3. Once you have the intersection point, project it on a local 2D basis in the plane of the square, this will give the 2D coordinates (u, v) of the point on the plane,

  4. Check whether the 2D coordinates (u, v) are within the square (assuming the four points form a parallelogram and you chose two adjacent edges for the local 2D basis), if yes then there is intersection (and you have the u/v coordinates).

Now with actual equations, assuming the four square vertices are placed as follows:

   S1 +------+ S2
      |      |
      |      |
   S3 +------+ S4
  1. The normal to the plane is: n = (S2 - S1) x (S3 - S1)

    A point M belongs to this plane iff it satisfies this equation: n . ( M - S1 ) = 0

  2. A point M belongs to the ray iff it can be written: M = R1 + t * dR with dR = R2 - R1

    Compute the ray / plane intersection (equate the two previous equations):

    n . ( M - S1 ) = 0 = n . ( R1 + t * dR - S1 ) = n . (R1 - S1) + t * n . dR

    If n . dR is 0 then the plane is parallel to the ray, and there is no intersection (again, ignoring the case where the ray is embedded in the plane).

    Else t = -n . (R1 - S1) / n . dR and plugging this result into the previous equation M = R1 + t * dR gives the 3D coordinates of the point of intersection M.

  3. Project the vector M - S1 onto the two vectors S2 - S1 and S3 - S1 (the square edges starting from S1), this gives two numbers (u, v):

    u = (M - S1) . (S2 - S1)

    v = (M - S1) . (S3 - S1)

  4. If 0 <= u <= |S2 - S1|^2 and 0 <= v <= |S3 - S1|^2, then the point of intersection M lies inside the square, else it's outside.

And finally a sample Java implementation of the previous equations (optimized for reading ease...):

public class Test {
    static class Vector3 {
        public float x, y, z;

        public Vector3(float x, float y, float z) {
            this.x = x;
            this.y = y;
            this.z = z;
        }

        public Vector3 add(Vector3 other) {
            return new Vector3(x + other.x, y + other.y, z + other.z);
        }

        public Vector3 sub(Vector3 other) {
            return new Vector3(x - other.x, y - other.y, z - other.z);
        }

        public Vector3 scale(float f) {
            return new Vector3(x * f, y * f, z * f);
        }

        public Vector3 cross(Vector3 other) {
            return new Vector3(y * other.z - z * other.y,
                               z - other.x - x * other.z,
                               x - other.y - y * other.x);
        }

        public float dot(Vector3 other) {
            return x * other.x + y * other.y + z * other.z;
        }
    }

    public static boolean intersectRayWithSquare(Vector3 R1, Vector3 R2,
                                                 Vector3 S1, Vector3 S2, Vector3 S3) {
        // 1.
        Vector3 dS21 = S2.sub(S1);
        Vector3 dS31 = S3.sub(S1);
        Vector3 n = dS21.cross(dS31);

        // 2.
        Vector3 dR = R1.sub(R2);

        float ndotdR = n.dot(dR);

        if (Math.abs(ndotdR) < 1e-6f) { // Choose your tolerance
            return false;
        }

        float t = -n.dot(R1.sub(S1)) / ndotdR;
        Vector3 M = R1.add(dR.scale(t));

        // 3.
        Vector3 dMS1 = M.sub(S1);
        float u = dMS1.dot(dS21);
        float v = dMS1.dot(dS31);

        // 4.
        return (u >= 0.0f && u <= dS21.dot(dS21)
             && v >= 0.0f && v <= dS31.dot(dS31));
    }

    public static void main(String... args) {
        Vector3 R1 = new Vector3(0.0f, 0.0f, -1.0f);
        Vector3 R2 = new Vector3(0.0f, 0.0f,  1.0f);

        Vector3 S1 = new Vector3(-1.0f, 1.0f, 0.0f);
        Vector3 S2 = new Vector3( 1.0f, 1.0f, 0.0f);
        Vector3 S3 = new Vector3(-1.0f,-1.0f, 0.0f);

        boolean b = intersectRayWithSquare(R1, R2, S1, S2, S3);
        assert b;

        R1 = new Vector3(1.5f, 1.5f, -1.0f);
        R2 = new Vector3(1.5f, 1.5f,  1.0f);

        b = intersectRayWithSquare(R1, R2, S1, S2, S3);
        assert !b;
    }
}
like image 192
user3146587 Avatar answered Sep 18 '22 04:09

user3146587