Your code is very confusing. I see what you are trying to do (I've implemented the same thing before) but I'm willing to bet money there are bugs, on top of you not being familiar with the algorithm.
In general, it's best to test for a ray/plane intersection to predict the point of collision before penetrating the surface. Once you've got the general idea implemented you will have to work around floating point inaccuracies, else you'll fall through the world with code that appears to have been implemented correctly.
To give you a start, here is some code I wrote to determine if a point lies inside a triangle. It hasn't been rigourously tested yet. Basic idea on how to do this:
1_ Compute your start point and endpoint for the frame (where you currently are and where you'd like to go). I like to extrapolate the length of the move a little bit.
2_ For every item you wish to collide against (in this case a polygon soup of triangles) determine where the ray formed by the start point and end point intersects the current plane (if it doesn't, continue to the next polygon).
3_ Determine whether or not the plane intersection lies inside the current triangle. If the time that you collided with this polygon is less than the currently stored time of intersection, replace the time of intersection with the current one.
I left out a LOT of details.
Code:
//Fixme: investigate to see how vector length plays into uncertainty here
//Normal x DiffVector = InwardVector
//InwardVector Dot Point >= 0 or >= -.0001
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)
{
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);
Point -= Point1; //Translates to local coordinates
Vector3 DiffVector = Point2 - Point1;
Vector3 InwardVector(0,0,0);
InwardVector = CrossProduct(&Normal,&DiffVector);
if(DotProduct(&InwardVector,&Point)< -.0001)
return FALSE;
DiffVector = Point3 - Point2;
InwardVector = CrossProduct(&Normal,&DiffVector);
if(DotProduct(&InwardVector,&Point)< -.0001)
return FALSE;
DiffVector = Point1 - Point3;
InwardVector = CrossProduct(&Normal,&DiffVector);
if(DotProduct(&InwardVector,&Point)< -.0001)
return FALSE;
return TRUE;
}
Just to give you a very basic idea of how that code is supposed to work, here's a picture. The red lines are the unnormalied inward facing vectors (they need not be normalized). If a point that is on the same plane as the triangle is in front of each of these inward facing vectors (the dotproduct between the point and the inward facing vector is greater than 0) then the point in question is inside the triangle.