m_position.y = terrain->getHeight(m_position.x, m_position.z) + (m_pModel->getRadius() * 2);

float CTerrain::getHeight(float x, float z)
{	
	int cellX = (x - m_pos.x) / MAP_SCALE; // Find cellX which entity point lies within
	int cellZ = -(z - m_pos.z) / MAP_SCALE; // Find cellZ which entity point lies within, negative because the way terrain verts are created

	int currentCell = (( cellZ * MAP_X ) + cellX ); // Find exact cell point lies within


	CVector bottomLeftVert = m_vTerrain[ currentCell ] + m_pos; // Vertices around current cell
	CVector bottomRightVert = m_vTerrain[ currentCell + 1 ] + m_pos;
	CVector topLeftVert = m_vTerrain[ (currentCell + MAP_X) ] + m_pos;
	CVector topRightVert = m_vTerrain[ (currentCell + MAP_X) + 1 ] + m_pos;

	float dx = x - bottomLeftVert.x; // x distance is cell
	float dz = -(z - bottomLeftVert.z); // z distance in cell

	if( dx > dz )
	{
		CVector normal = (topLeftVert - bottomLeftVert).CrossProduct( bottomRightVert - bottomLeftVert ); // Get normal of triangle

		// height = bottomLeftVert + ((dx * triNorm.x) + (dz * triNorm.z)) / -triNorm.y
		float height = bottomLeftVert.y + ( normal.x * dx + normal.z * dz ) / -normal.y;
	}

	else // dz > dx
	{
		CVector normal = (topLeftVert - topRightVert).CrossProduct( bottomRightVert - topRightVert );
     	        float height = topRightVert.y + ( normal.z * dz + normal.x * dx ) / -normal.y;

		return height;
	}

void CTerrain::calculateSurfaceNormals(){	int currentVertex = 0;	// Tri Normal calculations	for( int z = 0; z < MAP_Z-1; z++ )	{		for( int x = 0; x < MAP_X-1; x++ )		{			currentVertex = z * MAP_X + x;			m_vTriNormals[currentVertex * 2] = UTILITY.calculateNormal( &m_vTerrain[currentVertex], &m_vTerrain[currentVertex + 1], &m_vTerrain[currentVertex + MAP_Z]);			m_vTriNormals[(currentVertex * 2) +1] = UTILITY.calculateNormal( &m_vTerrain[currentVertex + MAP_Z + 1], &m_vTerrain[currentVertex + MAP_Z], &m_vTerrain[currentVertex + 1]);		}	}}

CVector CUtility::calculateNormal(CVector *p1, CVector *p2, CVector *p3){	CVector a, b, result;	float length;	a.x = (p1->x - p2->x);	a.y = (p1->y - p2->y);	a.z = (p1->z - p2->z);	b.x= ( p1->x - p3->x );	b.y= ( p1->y - p3->y );	b.z= ( p1->z - p3->z );	result.x = ( a.y * b.z ) - ( b.y * a.z ); 	result.y = ( b.x * a.z ) - ( a.x * b.z );	result.z = ( a.x * b.y ) - ( b.x * a.y );	length = (float)sqrt(result.x * result.x + result.y * result.y + result.z * result.z);	result.x /= length; 	result.y /= length;	result.z /= length;	return result;}

int currentCell = (( cellZ * MAP_X ) + cellX ); // Find exact cell point lies within	int xTriangle = (currentCell * 2);	int zTriangle = (currentCell * 2) + 1;	if( dx > dz )	{		// height = bottomLeftVert + ((dx * triNorm.x) + (dz * triNorm.z)) / -triNorm.y		float height = bottomLeftVert.y + ( (m_vTriNormals[ xTriangle ].x * dx) + (m_vTriNormals[ xTriangle ].z * dz) ) / -m_vTriNormals[ xTriangle ].y;		return height;	}	else // dz > dx	{     	float height = bottomLeftVert.y + ( (m_vTriNormals[ zTriangle ].x * dx) + (m_vTriNormals[ zTriangle ].z * dz) ) / m_vTriNormals[ zTriangle ].y;		return height;	}