直线与三角形相交

bool GeometricTools::intersect(Vector3f ray_o, Vector3f ray_d,
                               Vector3f tri_p0, Vector3f tri_p1, Vector3f tri_p2,
                               Vector3f& res)
{
    float t = 0;

    // for the plane: Xn(x - X0) + Yn(y - Y0) + Zn(z - Z0) = 0 
    // for the line: L(t) = s + td
    // t = (n * p)/(n * d)
    Vector3f p1 = tri_p1 - tri_p0;
    Vector3f p2 = tri_p2 - tri_p0;

    // 注意glm中三角形存储为顺时针,这里需要逆转一下
    Vector3f normal = Cross(p2, p1);
    Vector3f P = tri_p0 - ray_o;

    float fDiv =  Dot(normal, ray_d);
    if (fabs(fDiv) < FloatEPS)
    {
        return false;
    }

    t = Dot(normal, P) / fDiv;
    if (t <= 0)
    {   
        return false;
    }

    res.x = ray_o.x + t * ray_d.x;
    res.y = ray_o.y + t * ray_d.y;
    res.z = ray_o.z + t * ray_d.z;

    // is the point in the triangle?
    // 面积法判断
    // http://www.cnblogs.com/cgwolver/archive/2009/03/26/1257611.html
    Vector3f p01 = tri_p1 - tri_p0;
    Vector3f p02 = tri_p2 - tri_p0;
    Vector3f pr0 = tri_p0 - res;
    Vector3f pr1 = tri_p1 - res;
    Vector3f pr2 = tri_p2 - res;

    float s1  = Magnitude(Cross(p01, p02))/2.0;
    float s21 = Magnitude(Cross(pr0, pr1))/2.0;
    float s22 = Magnitude(Cross(pr0, pr2))/2.0;
    float s23 = Magnitude(Cross(pr1, pr2))/2.0;
    float s2 = s21 + s22 + s23;
    if (fabs(s1 - s2) > FloatEPS)
    {
        return false;
    }

    return true;
}