+static inline bool
+fgdRayTriangle(SGVec3d& x, const SGVec3d& point, const SGVec3d& dir,
+ const SGVec3d v[3])
+{
+ double eps = 1e-4;
+ // Method based on the observation that we are looking for a
+ // point x that can be expressed in terms of the triangle points
+ // x = p_0 + \mu_1*(p_1 - p_0) + \mu_2*(p_2 - p_0)
+ // with 0 <= \mu_1, \mu_2 and \mu_1 + \mu_2 <= 1.
+ // OTOH it could be expressed in terms of the ray
+ // x = point + \lambda*dir
+ // Now we can compute \mu_i and \lambda.
+ // define
+ SGVec3d d1 = v[1] - v[0];
+ SGVec3d d2 = v[2] - v[0];
+ SGVec3d b = point - v[0];
+
+ // the vector in normal direction, but not normalized
+ SGVec3d d1crossd2 = cross(d1, d2);
+
+ double denom = -dot(dir, d1crossd2);
+ double signDenom = copysign(1, denom);
+ // return if paralell ??? FIXME what if paralell and in plane?
+ // may be we are ok below than anyway??
+ // if (SGMiscd::abs(denom) <= SGLimitsd::min())
+ // return false;
+
+ // Now \lambda would read
+ // lambda = 1/denom*dot(b, d1crossd2);
+ // To avoid an expensive division we multiply by |denom|
+ double lambdaDenom = signDenom*dot(b, d1crossd2);
+ if (lambdaDenom < 0)
+ return false;
+ // For line segment we would test against
+ // if (1 < lambda)
+ // return false;
+ // with the original lambda. The multiplied test would read
+ // if (absDenom < lambdaDenom)
+ // return false;
+
+ double absDenom = fabs(denom);
+ double absDenomEps = absDenom*eps;
+
+ SGVec3d bcrossr = cross(b, dir);
+ // double mu1 = 1/denom*dot(d2, bcrossr);
+ double mu1 = signDenom*dot(d2, bcrossr);
+ if (mu1 < -absDenomEps)
+ return false;
+ // double mu2 = -1/denom*dot(d1, bcrossr);
+ // if (mu2 < -eps)
+ // return false;
+ double mmu2 = signDenom*dot(d1, bcrossr);
+ if (mmu2 > absDenomEps)
+ return false;
+
+ if (mu1 - mmu2 > absDenom + absDenomEps)
+ return false;
+
+ x = point;
+ // if we have survived here it could only happen with denom == 0
+ // that the point is already in plane. Then return the origin ...
+ if (SGLimitsd::min() < absDenom)
+ x += (lambdaDenom/absDenom)*dir;
+
+ return true;
+}
+