Collision Detection (Point-Polygon)

Code:

// the following is based on code from [url]http://www.ce.chalmers.se/staff/tomasm/raytri/[/url]

// by Tomas Moller, May 2000  

// modified by Ian Rickard, July 2002

static bool intersect_triangle(const Point3f &orig, const Point3f &dir,

        const Point3f &vert0, const Point3f &vert1, const Point3f &vert2,

        bool backface = true, float *t=NULL) {

    Point3f edge1, edge2, tvec, pvec, qvec;

    float det , inv_det;



    // find vectors for two edges sharing vert0

    edge1 = vert1 -vert0;

    edge2 = vert2 -vert0;



    // begin calculating determinant - also used to calculate U parameter 

    pvec.MakeCross(dir, edge2);



    // if determinant is near zero, ray lies in plane of triangle

    det = edge1.Dot(pvec);



    if (det > 0) { // IFR: don't care about the coplanar case

        if (t)

            inv_det = 1.0 / det;



        // calculate distance from vert0 to ray origin

        tvec = orig - vert0;

        

        // calculate U parameter and test bounds

        float u = tvec.Dot(pvec);

        

        if (u < 0.0 || u > det)

            return false;



        // prepare to test V parameter

        qvec.MakeCross(tvec, edge1);



        // calculate V parameter and test bounds

        float v = dir.Dot(qvec);

        

        if (v < 0.0 || u + v > det)

            return false;



    } else {

        if (!backface) return false;

        

        if (t)

            inv_det = 1.0 / det;

        

        tvec = orig - vert0;

        

        float u = tvec.Dot(pvec);

        

        if (u > 0.0 || u < det)

            return false;



        qvec.MakeCross(tvec, edge1);



        float v = dir.Dot(qvec);

        

        if (v > 0.0 || u + v < det)

            return false;

    }



    if(t)

        *t = edge2.Dot(qvec) * inv_det;



    return true;

}

Code:

struct Point3f {

    GLfloat x,y,z;

    

    void set(Float ix, Float iy, Float iz){x=ix;y=iy;z=iz;}

    Point3f(){}

    Point3f(Float ix, Float iy, Float iz){set(ix,iy,iz);}

    Point3f(const Point3f &v){*this = v;}

    Point3f operator +(const Point3f &t) const {return Point3f(x+t.x, y+t.y, z+t.z);}

    Point3f operator -(const Point3f &t) const {return Point3f(x-t.x, y-t.y, z-t.z);}

    Point3f operator +=(const Point3f &t) {return Point3f(x+=t.x, y+=t.y, z+=t.z);}

    Point3f operator -=(const Point3f &t) {return Point3f(x-=t.x, y-=t.y, z-=t.z);}

    Point3f operator* (Float t) const {return Point3f(x*t, y*t, z*t);}

    Point3f operator*= (Float t) {return Point3f(x*=t, y*=t, z*=t);}

    Float Dot(const Point3f &t) const {float result =  x*t.x+y*t.y+z*t.z; return result;}

    void MakeCross(const Point3f &a, const Point3f &b) {x = a.y*b.z-a.z*b.y; y = a.z*b.x-a.x*b.z; z = a.x*b.y-a.y*b.x;}

    void Normalize() {float inv = FastSqrtRecip(x*x+y*y+z*z); x*=inv; y*=inv; z*=inv;}

    operator GLfloat*() {return &x;}

    operator const GLfloat*() const {return &x;}

    Float Length() const {return FastSqrt(x*x+y*y+z*z);}

    Float Length2() const {return x*x+y*y+z*z;}

    Point3f ProjectOntoNorm(const Point3f &onto) {return onto*Dot(onto);}

    Point3f ProjectOnto(const Point3f &onto) {return (onto*Dot(onto))*(__fres(onto.Length2()));}

    Float& operator[](int index) {return (&x)[index];}

};

inline Float Length(const Point3f &i) {return FastSqrt(i.x*i.x+i.y*i.y+i.z*i.z);}