Ray intersect Box problem

  

void DXEngine::SelectObject(int x, int y)
{
	m_pkDevice->GetViewport(&m_pkViewPort);
	m_pkDevice->GetTransform(D3DTS_WORLD, &m_kWorldMatrix);
	m_pkDevice->GetTransform(D3DTS_PROJECTION, &m_kProjMatrix);
	m_pkDevice->GetTransform(D3DTS_VIEW, &m_kViewMatrix);

	D3DXVECTOR3 Out[2];
	D3DXVECTOR3 kMouse(x,y,0);
	D3DXVec3Unproject(&Out[0], &kMouse, &m_pkViewPort, &m_kProjMatrix, &m_kViewMatrix, &m_kWorldMatrix);

	kMouse.z = 100;
	D3DXVec3Unproject(&Out[1], &kMouse, &m_pkViewPort, &m_kProjMatrix, &m_kViewMatrix, &m_kWorldMatrix);

	for(int i=0; i<m_vkCloneList.size(); i++)
	{
		CLONE *pkClone = m_vkCloneList[i];

		Object* pkObject = m_pkObjectManager->GetObj(
			m_vkObjectList[pkClone->iObject]->strFileName);

		if(pkObject)
		{
			BoundingBox kBoundingBox = pkObject->GetBoundingBox();  
			
			D3DXVECTOR3 kDir = Out[0] - Out[1];
			D3DXVec3Normalize(&kDir, &kDir);
			
			float coord[3]	 = {0,0,0};
			float dir[3]	 = {kDir.x,kDir.y,kDir.z};
			float origin[3] = {kMouse.x,kMouse.y,100};

			//if(kBoundingBox.RayIntersect(Out[1], Out[0]) == true)

			if(kBoundingBox.HitBoundingBox(origin, dir, coord) == true)
			{
				MessageBox(m_khWndRender, "Picked!", "Test", MB_OK);
				return;
			}
		}
	}
}

  

  

inline float
SquareMagnitude (const D3DXVECTOR3& v)
{
   return v.x*v.x + v.y*v.y + v.z*v.z;
}

inline float
Magnitude (const D3DXVECTOR3& v)
{
   return (float) sqrt(SquareMagnitude(v));
}


bool BoundingBox::RayIntersect(D3DXVECTOR3 Vec1, D3DXVECTOR3 Vec2)
{
	//line direction

	D3DXVECTOR3 l = Vec2-Vec1;
	D3DXVec3Normalize(&l, &l);

	D3DXVECTOR3 mid = Vec2+(Vec1-Vec2)/2; //midpoint of the line 


	// segment

	float hl = Magnitude(Vec2-Vec1)/2; //segment half-length


	D3DXVECTOR3 Pivot(x_center, y_center, z_center);

	D3DXVECTOR3 T = Pivot - mid;

	//do any of the principal axes

	//form a separating axis?


	if( fabs(T.x) > x_half_size + hl*fabs(l.x) )
		return false;

	if( fabs(T.y) > y_half_size + hl*fabs(l.y) )
		return false;

	if( fabs(T.z) > z_half_size + hl*fabs(l.z) )
		return false;

	float r = (float) (y_half_size*fabs(l.z) + z_half_size*fabs(l.y));

	if( fabs(T.y*l.z - T.z*l.y) > r )
		return false;

	r = (float) (x_half_size*fabs(l.z) + z_half_size*fabs(l.x));

	if( fabs(T.z*l.x - T.x*l.z) > r )
		return false;

	r = (float) (x_half_size*fabs(l.y) + y_half_size*fabs(l.x));

	if( fabs(T.x*l.y - T.y*l.x) > r )
		return false;

	return true;
}

#define RIGHT	0
#define LEFT	1
#define MIDDLE	2
#define NUMDIM  3

bool BoundingBox::HitBoundingBox( float origin[3], float dir[3], float coord[3])
{
	float minB[3] = {x_center-x_half_size,y_center-y_half_size,z_center-z_half_size};
	float maxB[3] = {x_center+x_half_size,y_center+y_half_size,z_center+z_half_size};

	bool inside = true;
	char quadrant[NUMDIM];
	register int i;
	int whichPlane;
	float maxT[NUMDIM];
	float candidatePlane[NUMDIM];

	/* Find candidate planes; this loop can be avoided if
   	rays cast all from the eye(assume perpsective view) */
	for (i=0; i<NUMDIM; i++)
		if(origin[i] < minB[i]) {
			quadrant[i] = LEFT;
			candidatePlane[i] = minB[i];
			inside = false;
		}else if (origin[i] > maxB[i]) {
			quadrant[i] = RIGHT;
			candidatePlane[i] = maxB[i];
			inside = false;
		}else	{
			quadrant[i] = MIDDLE;
		}

	/* Ray origin inside bounding box */
	if(inside)	{
		coord = origin;
		return true;
	}

	/* Calculate T distances to candidate planes */
	for (i = 0; i < NUMDIM; i++)
		if (quadrant[i] != MIDDLE && dir[i] !=0.)
			maxT[i] = (candidatePlane[i]-origin[i]) / dir[i];
		else
			maxT[i] = -1.;

	/* Get largest of the maxT''s for final choice of intersection */
	whichPlane = 0;
	for (i = 1; i < NUMDIM; i++)
		if (maxT[whichPlane] < maxT[i])
			whichPlane = i;

	/* Check final candidate actually inside box */
	if (maxT[whichPlane] < 0.) return false;
	for (i = 0; i < NUMDIM; i++)
		if (whichPlane != i) {
			coord[i] = origin[i] + maxT[whichPlane] *dir[i];
			if (coord[i] < minB[i] || coord[i] > maxB[i])
				return false;
		} else {
			coord[i] = candidatePlane[i];
		}
	return true;				/* ray hits box */
}