Some per poly collision detection code

I hashed this out over several days and tested it. I do a lot of ugly things in there just to get something produced and working (not worried about speed right now). I haven't been posting any progress so I decided to at least post some code and a screenshot.

The screenshot just shows a collision (the sphere) the center of the poly, and the poly itself that's being collided with.

Some ugly things I do:

• Too many normalize() calls
• Check all bsp leafs instead of only checking the ones I cross

I'm posting code here in the order of abstractdion, i.e, the first functions are the most basic/primitive and the scope of their functionality is smallest.

The last thing I post is a simple way to handle some physics in my physics manager (this is being re-done along with the collision detection mind you).

Code:

---

BOOL        CCWPointInTri(float        P1X,float        P1Y,float        P1Z,
                                          float        P2X,float        P2Y,float        P2Z,
                                          float        P3X,float        P3Y,float        P3Z,
                                          float        NormalX,float        NormalY,float        NormalZ,
                                          float        PointX,float PointY,float        PointZ,float Radius)
{
        Vector3        Point1(P1X,P1Y,P1Z);        Vector3        Point2(P2X,P2Y,P2Z);        Vector3        Point3(P3X,P3Y,P3Z);
        Vector3        Normal(NormalX,NormalY,NormalZ);
        Vector3 Point(PointX,PointY,PointZ);

        Vector3        LocalPoint = Point - Point1; //The point in local coordintes with respect to a vertex

        Vector3        DiffVector        =        Point2 - Point1;
        Vector3        InwardVector(0,0,0);
        InwardVector        =        CrossProduct(&Normal,&DiffVector);
        InwardVector.Normalize();

        if(DotProduct(&InwardVector,&LocalPoint) <= -(Radius + globaltolerance))
        {
                CCWPITDEBUG(        trace << "Returning on first point" << "\n";        )
                return        FALSE;
        }

        LocalPoint        =        Point  - Point2;
        DiffVector        =        Point3 - Point2;
        InwardVector        =        CrossProduct(&Normal,&DiffVector);
        InwardVector.Normalize();


        if(DotProduct(&InwardVector,&LocalPoint) <= -(Radius + globaltolerance))
        {
                CCWPITDEBUG(        trace << "Returning on second point" << "\n";        )
                return        FALSE;
        }

        LocalPoint        =        Point  - Point3;
        DiffVector        =        Point1 - Point3;
        InwardVector        =        CrossProduct(&Normal,&DiffVector);
        InwardVector.Normalize();
       
        if(DotProduct(&InwardVector,&LocalPoint)<= -(Radius + globaltolerance))
        {
                CCWPITDEBUG(        trace << "Returning on third point" << "\n";        )
                return        FALSE;
        }

        return        TRUE;
}

BOOL        CWPointInTri(float        P1X,float        P1Y,float        P1Z,
                                          float        P2X,float        P2Y,float        P2Z,
                                          float        P3X,float        P3Y,float        P3Z,
                                          float        NormalX,float        NormalY,float        NormalZ,
                                          float        PointX,float PointY,float        PointZ,float Radius)
{
        Vector3        Point1(P1X,P1Y,P1Z);        Vector3        Point2(P2X,P2Y,P2Z);        Vector3        Point3(P3X,P3Y,P3Z);
        Vector3        Normal(-NormalX,-NormalY,-NormalZ);
        Vector3 Point(PointX,PointY,PointZ);

        Vector3        LocalPoint = Point - Point1; //The point in local coordintes with respect to a vertex

        Vector3        DiffVector        =        Point2 - Point1;
        Vector3        InwardVector(0,0,0);
        InwardVector        =        CrossProduct(&Normal,&DiffVector);
        InwardVector.Normalize();

        if(DotProduct(&InwardVector,&LocalPoint) <= -(Radius + globaltolerance))
        {
                CCWPITDEBUG(        trace << "\nReturning on first point" << "\n";        )
                CCWPITDEBUG(        trace << "DP InVec, LocalPoint: " << DotProduct(&InwardVector,&LocalPoint) << "\n";)
                CCWPITDEBUG(        trace << "GlobalTolerance: " << globaltolerance << "\n"; )
                CCWPITDEBUG(        trace << "DP Diff, In: " << DotProduct(&DiffVector,&InwardVector) << "\n"; )
               
                CCWPITDEBUG(        trace << "LocalPointlength: " << LocalPoint.GetLength() << "\n"; )        //small? small == bad?
                CCWPITDEBUG(        trace << "DiffVector(edge) length: " << DiffVector.GetLength() << "\n"; )  //small? small == bad?

                //construct a temporary plane and see if it would work any better
                float        PlaneD = DotProduct(&InwardVector,&Point1); //point1 + point3 are both on plane
               
                CCWPITDEBUG(trace << "PlaneD: " << PlaneD << "\n"; )
                float        Dist = DotProduct(&InwardVector,&Point) - PlaneD;
                CCWPITDEBUG(trace << "Dist: " << Dist << "\n"; )
                return        FALSE;
        }

        LocalPoint        =        Point  - Point2;
        DiffVector        =        Point3 - Point2;
        InwardVector        =        CrossProduct(&Normal,&DiffVector);
        InwardVector.Normalize();


        if(DotProduct(&InwardVector,&LocalPoint) <= -(Radius+globaltolerance))
        {
                CCWPITDEBUG(        trace << "\nReturning on second point" << "\n";        )
                CCWPITDEBUG(        trace << "DP InVec, LocalPoint: " << DotProduct(&InwardVector,&LocalPoint) << "\n";)
                CCWPITDEBUG(        trace << "GlobalTolerance: " << globaltolerance << "\n"; )
                CCWPITDEBUG(        trace << "DP Diff, In: " << DotProduct(&DiffVector,&InwardVector) << "\n"; )
               
                CCWPITDEBUG(        trace << "LocalPointlength: " << LocalPoint.GetLength() << "\n"; )        //small? small == bad?
                CCWPITDEBUG(        trace << "DiffVector(edge) length: " << DiffVector.GetLength() << "\n"; )  //small? small == bad?

                //construct a temporary plane and see if it would work any better
                float        PlaneD = DotProduct(&InwardVector,&Point1); //point1 + point3 are both on plane
               
                CCWPITDEBUG(trace << "PlaneD: " << PlaneD << "\n"; )
                float        Dist = DotProduct(&InwardVector,&Point) - PlaneD;
                CCWPITDEBUG(trace << "Dist: " << Dist << "\n"; )
                return        FALSE;
        }

        LocalPoint        =        Point  - Point3;
        DiffVector        =        Point1 - Point3;
        InwardVector        =        CrossProduct(&Normal,&DiffVector);
        InwardVector.Normalize();
       
        if(DotProduct(&InwardVector,&LocalPoint) <= -(Radius + globaltolerance))
        {
                CCWPITDEBUG(        trace << "\nReturning on third point" << "\n";        )
                CCWPITDEBUG(        trace << "DP InVec, LocalPoint: " << DotProduct(&InwardVector,&LocalPoint) << "\n";)
                CCWPITDEBUG(        trace << "GlobalTolerance: " << globaltolerance << "\n"; )
                CCWPITDEBUG(        trace << "DP Diff, In: " << DotProduct(&DiffVector,&InwardVector) << "\n"; )
               
                CCWPITDEBUG(        trace << "LocalPointlength: " << LocalPoint.GetLength() << "\n"; )        //small? small == bad?
                CCWPITDEBUG(        trace << "DiffVector(edge) length: " << DiffVector.GetLength() << "\n"; )  //small? small == bad?

                //construct a temporary plane and see if it would work any better
                float        PlaneD = DotProduct(&InwardVector,&Point1); //point1 + point3 are both on plane
               
                CCWPITDEBUG(trace << "PlaneD: " << PlaneD << "\n"; )
                float        Dist = DotProduct(&InwardVector,&Point) - PlaneD;
                CCWPITDEBUG(trace << "Dist: " << Dist << "\n"; )
                return        FALSE;
        }

        return        TRUE;
}
       

//Same thing, different version
BOOL        CWPointInTri(float P1X,float P1Y,float P1Z,
                                        float P2X,float P2Y,float P2Z,
                                        float P3X,float P3Y,float P3Z,
                                        float NormalX,float NormalY,float NormalZ,
                                        float PointX, float PointY, float PointZ)
{
        Vector3        P1(P1X,P1Y,P1Z);
        Vector3        P2(P2X,P2Y,P2Z);
        Vector3        P3(P3X,P3Y,P3Z);

        Vector3        Point(PointX,PointY,PointZ);
        Vector3        Normal(NormalX,NormalY,NormalZ);

        Vector3        Edge1 = P1 - P2;
        Vector3        Edge2 = P2 - P3;
        Vector3        Edge3 = P3 - P1;

        Vector3        PminusP2 = Point - P2;
        Vector3        PminusP3 = Point - P3;
        Vector3        PminusP1 = Point - P1;

        if(DotProduct(&Normal,&CrossProduct(&Edge1,&PminusP2)) >= globaltolerance)
        if(DotProduct(&Normal,&CrossProduct(&Edge2,&PminusP3)) >= globaltolerance)
        if(DotProduct(&Normal,&CrossProduct(&Edge3,&PminusP1)) >= globaltolerance)
                return true;

        return false;
}

---

Code:

---


HitInfo        TraceSphereToBSPFaces(BSP *pBSP, Vector3        &Start,Vector3        &End,float        Radius, float        Epsilon)
{
        Vector3        FinalHitPoint = End;
        float        FinalHitFrac = 1.0f;
        Vector3        FinalTempHitPoint1;
        Vector3        FinalHitNormal(0,0,0);
       
        Vector3        EndMinusStart = End - Start;

        int                HitTri        =        -1;
        tBSPFace        *pHitFace = NULL;
        Vector3        P1,P2,P3;//temp verts on triangle

//        std::vector<int>        LeafIndices;
//        FindLeavesTouchedByMove(pBSP,Start,End,Radius,LeafIndices);

        //Iterate through every leaf
        for(int leaf = 0; leaf < pBSP->mNumLeafs; leaf++)
//        for(int leaf = 0; leaf < LeafIndices.size(); leaf++)
        {       
        //        tBSPLeaf        *pLeaf        =        &pBSP->mpLeafs[LeafIndices[leaf]];
                tBSPLeaf        *pLeaf  =  &pBSP->mpLeafs[leaf];
                for(int face = 0; face < pLeaf->facelist.size(); face++)
                {
                        tBSPFace        *pFace = &pBSP->mpFaces[pLeaf->facelist[face]];
                        float        StartD = DotProduct(&pFace->vNormal,&Start) - pFace->planeD - TestBall.mRadius;
                                if(StartD > 0)
                                {
                                        float        EndD = DotProduct(&pFace->vNormal,&End) - pFace->planeD - TestBall.mRadius;
                                        if(EndD < 0)
                                        {
                                                float        AbsStart = fabs(StartD);
                                                float        AbsEnd        = fabs(EndD);
                                                float        Total        = AbsStart + AbsEnd;
                                                float        tempFrac        = (AbsStart - Epsilon)        /        Total;
                                               
                                               
                                                if(tempFrac < 0.0f)        //FIXME: should I just return?
                                                {
                                                        tempFrac = 0.0f;
                                                }
                                                else        if(tempFrac >= 1.0f)//FIXME: better handling here?
                                                {
                                                        tempFrac = 1.0f;
                                                }
                                               
                                                if(tempFrac < FinalHitFrac)
                                                {
                                                        Vector3        tempHitPoint = Start + EndMinusStart * tempFrac;
                                                        for(int tri = 0; tri < pFace->numMeshVerts / 3; tri++)
                                                        {
                                                                pBSP->GetBSPVertexesFromTri(leaf,face,tri,&P1,&P2,&P3);
                                                                {
                                                                        if(CWPointInTri(P1.x,P1.y,P1.z,P2.x,P2.y,P2.z,P3.x,P3.y,P3.z,
                                                                                pFace->vNormal.x,pFace->vNormal.y,pFace->vNormal.z,tempHitPoint.x,tempHitPoint.y,tempHitPoint.z,TestBall.mRadius))
                                                                        {
                                                                                FinalHitPoint        =        Start + (End - Start) * tempFrac;//tempHitPoint;
                                                                                FinalHitFrac        =        tempFrac;
                                                                                FinalHitNormal  =  pFace->vNormal;
                                                                               
                                                                                break;
                                                                        }
                                                                }
                                                        }
                                                }
                                        }
                                }
                        }
                }
               
               
                HitInfo        RetVal;
                RetVal.HitPoint                                =        FinalHitPoint;
                RetVal.InterpValue                        =        FinalHitFrac;
                RetVal.HitNormal                        =        FinalHitNormal;
                return        RetVal;
}

---

Code:

---

       
void        PhysicsManager::PHYSICS_RUNFRAMES()
{
        double        numframes = mTimeBuffer        *        mFPS;
       
        while(numframes >= 1.0f)
        {
                RENDER_PHYSICS_FRAME();
                numframes--;
                mTimeBuffer        -=        mSecondsPerFrame;
        }
}
       
        /*
                Renders/runs a single physics frame (each frame advances mSecondsPerFrame in time)
        */
void        PhysicsManager::RENDER_PHYSICS_FRAME()
{       
        RigidBody        *A,*B;
        std::vector<RigidBody*>::iterator        ptr_outside;
       
        //Add weight and predict positions for all rigid bodies

        for(ptr_outside        =        mvRigidBodies.begin(); ptr_outside != mvRigidBodies.end(); ptr_outside++)
        {
                RigidBody &        A_ref = **(ptr_outside);
       
                A_ref.mLinearMomentum += Vector3(0,-30,0) * mSecondsPerFrame;
                A_ref.PredictNewPosAndOrientation(mSecondsPerFrame);       
        }

        for(ptr_outside        =        mvRigidBodies.begin(); ptr_outside != mvRigidBodies.end(); ptr_outside++)
        {
                RigidBody &        A_ref = **(ptr_outside);
               
                //Check to see if the object hits the world
                HitInfo        temp = TraceSphereToBSPFaces(gpBSP,A_ref.mPosition,A_ref.mPredictedPosition,A_ref.mRadius,5.0f);
                       
                if(temp.InterpValue        < 1.0f)        //it collided with world
                {
                        A_ref.mPosition                        = temp.HitPoint;
                        A_ref.mLinearVelocity        -= temp.HitNormal * DotProduct(&temp.HitNormal,&A_ref.mLinearVelocity) * 1.1;
                        A_ref.mLinearMomentum = A_ref.mLinearVelocity * A_ref.mMass;
                }
                else
                        A_ref.mPosition = A_ref.mPredictedPosition;
        }
}

---