line-line intersection in 3d for openframeworks

gistfile1.cpp

ofVec3f utils::lineLineIntersection(ofVec3f p1, ofVec3f p2, ofVec3f p3, ofVec3f p4) {
    ofVec3f pa, pb;
    utils::lineLineIntersectSegment(p1,p2,p3,p4,pa,pb);
    return (pa+pb)/2.f;
}

// from the bourkster.
bool utils::lineLineIntersectSegment(ofVec3f p1, ofVec3f p2, ofVec3f p3, ofVec3f p4, ofVec3f &pa, ofVec3f &pb) {


   float mua = 0;
   float mub = 0;

   ofVec3f p13,p43,p21;

   float d1343,d4321,d1321,d4343,d2121;
   float numer,denom;
   p13 = p1 - p3;
   p43 = p4 - p3;


   if (ABS(p43.x) < FLT_EPSILON && ABS(p43.y) < FLT_EPSILON && ABS(p43.z) < FLT_EPSILON)
      return false;

   p21 = p2 - p1;

   if (ABS(p21.x) < FLT_EPSILON && ABS(p21.y) < FLT_EPSILON && ABS(p21.z) < FLT_EPSILON)
      return false;

   d1343 = p13.x * p43.x + p13.y * p43.y + p13.z * p43.z;
   d4321 = p43.x * p21.x + p43.y * p21.y + p43.z * p21.z;
   d1321 = p13.x * p21.x + p13.y * p21.y + p13.z * p21.z;
   d4343 = p43.x * p43.x + p43.y * p43.y + p43.z * p43.z;
   d2121 = p21.x * p21.x + p21.y * p21.y + p21.z * p21.z;

   denom = d2121 * d4343 - d4321 * d4321;
   if (ABS(denom) < FLT_EPSILON)
      return false;
   numer = d1343 * d4321 - d1321 * d4343;

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

   pa = p1 + p21 * mua;
   pb = p3 + p43 * mub;

   return true;
}