TwistyPuzzleLib

// points A, B, C are co-linear. Return true iff B is between A and C on this line.

// Compute D1 = A-B, D2=C-B. Then D1 and D2 are parallel vectors.

// They disagree in direction iff |D1+D2|<|D1-D2|

  private boolean isBetween(float ax, float ay, float az,

                            float bx, float by, float bz,

                            float cx, float cy, float cz)

    float d1x = ax-bx;

    float d1y = ay-by;

    float d1z = az-bz;

    float d2x = cx-bx;

    float d2y = cy-by;

    float d2z = cz-bz;

    float sx = d1x+d2x;

    float sy = d1y+d2y;

    float sz = d1z+d2z;

    float dx = d1x-d2x;

    float dy = d1y-d2y;

    float dz = d1z-d2z;

// General algorithm: shoot a half-line from the 'point' and count how many

// sides of the polygon it intersects with. The point is inside iff this number

// is odd. Note that this works also in case of concave polygons.

//

// Arbitrarily take point P on the plane ( we have decided on P=(vert[0]+vert[1])/2 )

// as the other point defining the half-line.

// 'point' and 'P' define a line L1 in 3D. Then for each side the pair of its vertices

// defines a line L2. If L1||L2 return false. Otherwise, the lines are skew so it's

// possible to compute points C1 and C2 on lines L1 and L2 which are closest to the

// other line and check if

//

// a) C1 and P are on the same side of 'point'

//    (which happens iff 'point' is not in between of C1 and P)

// b) C2 is between the two vertices.

//

// Both a) and b) together mean that the half-line intersects with side defined by (p2,d2)

//

// C1 and C2 can be computed in the following way:

// Let n = d1 x d2 - then vector n is perpendicular to both d1 and d2 --> (c1-c2) is

// parallel to n.

// There exist real numbers A,B,C such that

// c1 = p1 + A*d1

// c2 = p2 + B*d2 and

// c2 = c1 + C*n so that

// p1 + A*d1 + C*n = p2 + B*d2  --> (p1-p2) + A*d1 = B*d2 - C*n (*)

// Let n2 = n x d2. Let's multiply both sides of (*) by n2. Then

// (p1-p2)*n2 + A*(d1*n2) = 0 (0 because d1*n2 = n*n2 = 0)

// and from that A = [(p1-p2)*n2]/[d1*n2]

// Similarly     B = [(p2-p1)*n1]/[d2*n1]  where n1 = n x d1.

    float e1x = (vertices[0][0]+vertices[1][0])/2;

    float e1y = (vertices[0][1]+vertices[1][1])/2;

    float e1z = (vertices[0][2]+vertices[1][2])/2;

    float d1x = e1x - point[0];

    float d1y = e1y - point[1];

    float d1z = e1z - point[2];

    float ax = vertices[0][0] - vertices[1][0];

    float ay = vertices[0][1] - vertices[1][1];

    float az = vertices[0][2] - vertices[1][2];

    float normX = d1y*az - d1z*ay;

    float normY = d1z*ax - d1x*az;

    float normZ = d1x*ay - d1y*ax;

    float n1x = d1y*normZ - d1z*normY;

    float n1y = d1z*normX - d1x*normZ;

    float n1z = d1x*normY - d1y*normX;

    float p1x = point[0];

    float p1y = point[1];

    float p1z = point[2];

    int len = vertices.length;

    int numCrossings = 0;

    for(int side=0; side<len; side++)

      float p2x = vertices[side][0];

      float p2y = vertices[side][1];

      float p2z = vertices[side][2];

      int next = side==len-1 ? 0 : side+1;

      float e2x = vertices[next][0];

      float e2y = vertices[next][1];

      float e2z = vertices[next][2];

      float d2x = e2x-p2x;

      float d2y = e2y-p2y;

      float d2z = e2z-p2z;

      float nx = d2y*d1z - d2z*d1y;

      float ny = d2z*d1x - d2x*d1z;

      float nz = d2x*d1y - d2y*d1x;

      float n2x = d2y*nz - d2z*ny;

      float n2y = d2z*nx - d2x*nz;

      float n2z = d2x*ny - d2y*nx;

      float dpx = p1x-p2x;

      float dpy = p1y-p2y;

      float dpz = p1z-p2z;

      float A1 =-dpx*n2x-dpy*n2y-dpz*n2z;

      float B1 = d1x*n2x+d1y*n2y+d1z*n2z;

      float A2 = dpx*n1x+dpy*n1y+dpz*n1z;

      float B2 = d2x*n1x+d2y*n1y+d2z*n1z;

      if( B1==0 || B2==0 ) continue;

      float C1 = A1/B1;

      float C2 = A2/B2;

      float c1x = p1x + C1*d1x;

      float c1y = p1y + C1*d1y;

      float c1z = p1z + C1*d1z;

      float c2x = p2x + C2*d2x;

      float c2y = p2y + C2*d2y;

      float c2z = p2z + C2*d2z;

      if( !isBetween(c1x,c1y,c1z, p1x,p1y,p1z, e1x,e1y,e1z ) &&

           isBetween(p2x,p2y,p2z, c2x,c2y,c2z, e2x,e2y,e2z )  )

        {

        numCrossings++;

        }

    return (numCrossings%2)==1;