Hello I have a problem to make the object orient to face a position. To get the angle I'm using the code found in thug1 source code, But it seems to not work.

float AngleY(D3DXMATRIX& orient, D3DXVECTOR3& from, D3DXVECTOR3& to)
{
	Vertex tempHeading = *(Vertex*)&(to - from);
	tempHeading.ProjectToPlane(*(Vertex*)&orient.m[Y]);
	if (!tempHeading.Length())
	{
		// If our rot axis is along Y, for example, the
		// target heading is right above us... Can't pick
		// a rotation!
		return (0.0f);
	}
	tempHeading.Normalize();


	// while we have these two vectors, find the angle between them...
	float angCos = D3DXVec3Dot((D3DXVECTOR3*)&orient.m[Z], &tempHeading);

	// Mick:  contrain Dot product to range -1.0f to +1.0f, since FP innacuracies might
	// make it go outside this range
	// (previously this was done only on NGC.  Not sure why it has started failing now
	// but it seems logical that it would fail occasionally, so this check is necessary) 	
	float	ang = angCos;
	if (ang > 1.0f) ang = 1.0f;
	if (ang < -1.0f) ang = -1.0f;
	float retVal = -acosf(ang);

	Vertex temp = CrossProduct((Vertex*)&orient.m[Z], &tempHeading);

	// check to see if the cross product is in the same quatrant as our rot axis...
	// if so, gots to rotate the other way and shit like that...
	int whichAxis;
	float tempMax = temp.GetMaxAxis(&whichAxis);
	if ((tempMax > 0) == (orient.m[Y][whichAxis] > 0))
	{
		return (-retVal);
	}
	return (retVal);
}



//update code:
if (timer >= end)//we should be at the linked location now
{
	if (pos != goal)//if we are not at the right location make us be
	{
		D3DXVECTOR3 relPos = goal - pos;

		sector->bboxMax += relPos;
		sector->bboxMin += relPos;
		for (DWORD i = 0; i < sector->numVertices; i++)
		{
			sector->vertices[i] += relPos;
		}

		pos += relPos;

		update = true;//Now we will update vertexbuffer even if angle is not changed
	}

	//Get the array of links
	CArray* links = link->GetArray(Checksums::Links);
	if (!links)
	{
		RemoveMovingObject(sector);
		_printf("MovingObject final destination\n");
		return update;
	}

	//If 1 link use it, else pick a random one
	if (link->GetNumItems() == 1)
		link = Node::GetNodeStructByIndex((*links)[0]);
	else
		link = Node::GetNodeStructByIndex((*links)[Rnd(link->GetNumItems())]);//Pick a random path
	if (!link)
	{
		RemoveMovingObject(sector);
		_printf("Couldn't find link[%d]...\n", (*links)[0]);
		return update;
	}

	//Get the position of the link
	CStructHeader* _pos;
	if (link->GetStruct(Checksums::Position, &_pos))
	{
		//update the goal target position
		goal = *_pos->pVec;
		//reset timer to zero
		timer = 0;
		//calculate distance between current position and target position
		float distance = D3DXVec3Length(&(pos - goal));
		//calculate end time
		end = distance / speed;
		//Calculate the angle needed to look at a position, taken from thug1src but for some reason not working?
		D3DXVECTOR3 pathHeading = goal - pos;
		goalAngle = D3DXVECTOR3(0, AngleY(orient, pos, goal), 0);

		//if no angle we don't need to update
		if (goalAngle.y)
		{
			(*(Matrix*)&orient).RotateYLocal(goalAngle.y);
			//Use OrthoNormalizeAbout2 because other one was the games original normalize funciton which seems to be bugged?
			(*(Matrix*)&orient).OrthoNormalizeAbout2(Y);

			bboxMax = D3DXVECTOR3(-FLT_MAX, -FLT_MAX, -FLT_MAX);
			bboxMin = D3DXVECTOR3(FLT_MAX, FLT_MAX, FLT_MAX);
			sector->bboxMax = D3DXVECTOR3(FLT_MAX, FLT_MAX, FLT_MAX);
			sector->bboxMin = D3DXVECTOR3(-FLT_MAX, -FLT_MAX, -FLT_MAX);

			for (DWORD i = 0; i < sector->numVertices; i++)
			{
				sector->vertices[i] -= pos;
				D3DXVec3TransformCoord(&sector->vertices[i], &sector->vertices[i], &orient);
				sector->vertices[i] += pos;
				if (bboxMax.x < sector->vertices[i].x)
					bboxMax.x = sector->vertices[i].x;
				if (bboxMin.x > sector->vertices[i].x)
					bboxMin.x = sector->vertices[i].x;

				if (bboxMax.y < sector->vertices[i].y)
					bboxMax.y = sector->vertices[i].y;
				if (bboxMin.y > sector->vertices[i].y)
					bboxMin.y = sector->vertices[i].y;

				if (bboxMax.z < sector->vertices[i].z)
					bboxMax.z = sector->vertices[i].z;
				if (bboxMin.z > sector->vertices[i].z)
					bboxMin.z = sector->vertices[i].z;
			}

			sector->bboxMax = bboxMax;
			sector->bboxMin = bboxMin;

			angle = goalAngle;

			return true;//send state to update vertexbuffer
		}
		//return the stored value since we might wanna update vertexbuffer if position was changed but not the angle
		return update;
	}

	RemoveMovingObject(sector);
	_printf("Couldn't find node -> position[%d]...\n", (*links)[0]);
	return update;
}
else
{
	direction = goal - pos;
	D3DXVec3Normalize(&Velocity, &direction);
	Velocity *= speed * delta;

	if (Velocity)
	{

		sector->bboxMax += Velocity;
		sector->bboxMin += Velocity;
		for (DWORD i = 0; i < sector->numVertices; i++)
		{
			sector->vertices[i] += Velocity;
		}
		pos += Velocity;

		return true;//send state to update vertexbuffer
	}
}

Additional functions taken from thug1source aswell

class Matrix
{
    static Matrix& CreateRotateYMatrix(Matrix& rotY, float angle)
	{


		float	s = sinf(angle);
		float	c = cosf(angle);

		rotY.m[RIGHT][X] = c;
		rotY.m[RIGHT][Y] = 0.0f;
		rotY.m[RIGHT][Z] = -s;
		rotY.m[RIGHT][W] = 0.0f;

		rotY.m[UP][X] = 0.0f;
		rotY.m[UP][Y] = 1.0f;
		rotY.m[UP][Z] = 0.0f;
		rotY.m[UP][W] = 0.0f;

		rotY.m[AT][X] = s;
		rotY.m[AT][Y] = 0.0f;
		rotY.m[AT][Z] = c;
		rotY.m[AT][W] = 0.0f;

		rotY.m[POS][X] = 0.0f;
		rotY.m[POS][Y] = 0.0f;
		rotY.m[POS][Z] = 0.0f;
		rotY.m[POS][W] = 1.0f;

		return rotY;
	}
	
	Matrix& OrthoNormalizeAbout2(int r0)
	{
		int r1, r2;
		r1 = r0 + 1;
		if (r1 == 3)
		{
			r1 = 0;
		}
		r2 = r1 + 1;
		if (r2 == 3)
		{
			r2 = 0;
		}
		// Now regarding Rows r0,r1,r2
		// r0 = r1 x r2	   (implied)
		// r1 = r2 x r0	   (calculate this)
		// r2 = r0 x r1	   (and this)
		//
		// We need to recalculate rows r1 and r2 using the above cross produces
		// however if r0 is close to r2, then the calculation of r1 will be off
		// so it's better to calulate r2 and then r1 
		// the first pair to do will be whichever has the smaller dot product

		if (fabsf(D3DXVec3Dot((D3DXVECTOR3*)(m[r2]), (D3DXVECTOR3*)(m[r0]))) < fabsf(D3DXVec3Dot((D3DXVECTOR3*)(m[r0]), (D3DXVECTOR3*)(m[r1]))))
		{
			*(Vertex*)(m[r1]) = CrossProduct((Vertex*)(m[r2]), (Vertex*)(m[r0]));
			(*(Vertex*)(m[r1])).Normalize();
			*(Vertex*)(m[r2]) = CrossProduct((Vertex*)(m[r0]), (Vertex*)(m[r1]));
			(*(Vertex*)(m[r2])).Normalize();
		}
		else
		{
			*(Vertex*)(m[r2]) = CrossProduct((Vertex*)(m[r0]), (Vertex*)(m[r1]));
			(*(Vertex*)(m[r2])).Normalize();
			*(Vertex*)(m[r1]) = CrossProduct((Vertex*)(m[r2]), (Vertex*)(m[r0]));
			(*(Vertex*)(m[r1])).Normalize();
		}

		return *this;

	}
	
	inline Matrix& RotateYLocal(const float angle)
	{

		CreateRotateYMatrix(*this, angle) * *this;

		return *this;
	}
}

This is now fixed, it was because I was rotating on the already rotated vertices.