Can you help me resolve a bug in mesh collision code?

Question

I have entities which walk along a 3D mesh. I move the units along their x,z axis, and every frame I update their y-position according to the quad or triangle they are standing on, using Barycentric coordinates. They do walk along the mesh, however every time they enter a new quad or triangle, they "bob" up and down on the y-axis.

Height testing is done by finding the "bottom-left" vertex of a "sqaure" of four vertices made by converting the unit's x,z position into a 1d index into an array of vertices.

They move smoothly until they enter a new part of the mesh, where it looks like their y-coordinate shifts once very noticeably and then keep moving smoothly. It happens both when testing against the square made by the four vertices around the unit, and with testing against triangles of mesh.

Here is the code to find y-coordinate on a square:

float px = batchTranslation[ i ].Value.x;
float pz = batchTranslation[ i ].Value.z;

int index = ( int ) ( ( math.floor( pz / MAP_SCALE ) ) * MAP_SIZE + ( math.floor( px / MAP_SCALE ) ) );

int bL = index;
int bR = index + 1;
int tL = index + MAP_SIZE;

float3 p1 = vertices[ bL ];
float3 p2 = vertices[ bR ];
float3 p3 = vertices[ tL ];

float3 normal = math.cross( ( p2 - p1 ) , ( p3 - p1 ) );

float py = ( normal.x * ( p1.x - px ) + normal.z * ( p1.z - pz ) + normal.y * p1.y ) / normal.y + HALF_HEIGHT;

batchTranslation[ i ] = new Translation { Value = new float3( px , py , pz ) };

Here is the code for finding the y-coordinate on the triangles of the mesh:

float px = batchTranslation[ i ].Value.x;
float pz = batchTranslation[ i ].Value.z;

int index = ( int ) ( ( math.floor( pz / MAP_SCALE ) ) * MAP_SIZE + ( math.floor( px / MAP_SCALE ) ) );

int bL = index;
int bR = index + 1;
int tL = index + MAP_SIZE;
int tR = index + MAP_SIZE + 1;

float3 V1 = vertices[ bL ];
float3 V2 = vertices[ bR ];
float3 V3 = vertices[ tL ];
float3 V4 = vertices[ tR ];

float py = batchTranslation[ i ].Value.y;

if ( IsInside( V1.x , V1.z , V2.x , V2.z , V3.x , V3.z , px , pz ) )
    py = CalcY( V1 , V2 , V3 , px , pz );
else if ( IsInside( V1.x , V1.z , V2.x , V2.z , V4.x , V4.z , px , pz ) )
    py = CalcY( V1 , V2 , V4 , px , pz );

batchTranslation[ i ] = new Translation { Value = new float3( px , py , pz ) };

private float Area( float x1 , float z1 , float x2 , float z2 , float x3 , float z3 )
{
    return math.abs( ( x1 * ( z2 - z3 ) + x2 * ( z3 - z1 ) + x3 * ( z1 - z2 ) ) / 2.0f );
}

private bool IsInside( float x1 , float y1 , float x2 , float y2 , float x3 , float y3 , float x , float y )
{
    float A = Area( x1 , y1 , x2 , y2 , x3 , y3 ); // Calculate area of triangle ABC
    float A1 = Area( x , y , x2 , y2 , x3 , y3 ); // Calculate area of triangle PBC
    float A2 = Area( x1 , y1 , x , y , x3 , y3 ); // Calculate area of triangle PAC
    float A3 = Area( x1 , y1 , x2 , y2 , x , y ); // Calculate area of triangle PAB
    return ( A == A1 + A2 + A3 ); // Check if sum of A1, A2 and A3 is same as A
}

// Returns y coordinate of point on triangle given point x and z coordinates
private float CalcY( float3 p1 , float3 p2 , float3 p3 , float x , float z )
{
    // determinant
    float det = ( p2.z - p3.z ) * ( p1.x - p3.x ) + ( p3.x - p2.x ) * ( p1.z - p3.z );

    float l1 = ( ( p2.z - p3.z ) * ( x - p3.x ) + ( p3.x - p2.x ) * ( z - p3.z ) ) / det;
    float l2 = ( ( p3.z - p1.z ) * ( x - p3.x ) + ( p1.x - p3.x ) * ( z - p3.z ) ) / det;
    float l3 = 1.0f - l1 - l2;

    return l1 * p1.y + l2 * p2.y + l3 * p3.y;
}
```

Answer 1

Turns out I was using the wrong vertices it should be:

IsInside( V1.x , V1.z , V2.x , V2.z , V4.x , V4.z , px , pz )

And :

py = CalcY( V1 , V2 , V4 , px , pz );

Or:

py = CalcY( V1 , V3 , V4 , px , pz );

Also, use doubles instead of floats for accuracy.