bool Frustum::collision(const CollisionObject * pCollisionObject, bool calculateNormal)
{
	calculateNormal;

	if(pCollisionObject->getCollisionType()==CollisionType_Sphere)
	{
		Sphere * pSphere = (Sphere*) pCollisionObject;

		float x = pSphere->getX();
		float y = pSphere->getY();
		float z = pSphere->getZ();
		float r = pSphere->getR();
		
		for(int i=0;i<6;i++)
		{
			if(mPlanes[i].a * x + mPlanes[i].b * y + mPlanes[i].c * z + mPlanes[i].d <= -r)
			{
				return false;
			}
		}
		
		return true;
	}
	else if(pCollisionObject->getCollisionType()==CollisionType_AABB)
	{
		AABB * pAABB = (AABB*) pCollisionObject;
		
		for(unsigned int j=0;j<6;j++)
		{
			unsigned int cornersInside = 8;

			for(unsigned int i=0;i<8;i++)
			{
				D3DXVECTOR3 corner = pAABB->getCorner(i);

				/*
				if(mPlanes[j].a * corner.x + mPlanes[j].b * corner.y + mPlanes[j].c * corner.z + mPlanes[j].d <= 0)
				{
					cornersInside--;
				}
				*/

				if(D3DXPlaneDotNormal(&mPlanes[j], &corner) +  mPlanes[j].d <= 0)
				{
					cornersInside--;
				}
			}

			if(cornersInside==0)
			{
				return false;
			}
		}
		
		return true;
	}
	if(pCollisionObject->getCollisionType()==CollisionType_OBB)
	{
		OBB * pOBB = (OBB*)pCollisionObject;

		for(unsigned int j=0;j<6;j++)
		{
			unsigned int cornersInside = 8;

			for(unsigned int i=0; i<8; i++)
			{
				D3DXVECTOR3 corner = pOBB->getCorner(i);

				if(D3DXPlaneDotNormal(&mPlanes[j], &corner) +  mPlanes[j].d <= 0)
				{
					cornersInside--;
				}				
			}

			if(cornersInside==0)
			{
				return false;
			}
		}
		
		return true;
	}

	return false;
}
bool OBB::collision(const CollisionObject * pCollisionObject, bool calculateNormal)
{
	if(pCollisionObject->getCollisionType()==CollisionType_OBB)
	{
		OBB * pOBB = (OBB*) pCollisionObject;
		
		for(unsigned int aIndex=0; aIndex<3; aIndex++)
	    {
			  D3DXVECTOR3 axis = mAxis[aIndex];

              float min1 = 1000000.0f;
              float max1 = -1000000.0f;
              float min2 = 1000000.0f;
              float max2 = -1000000.0f;
              
              for(unsigned int cIndex=0; cIndex<8; cIndex++)
              {
				  float pos = D3DXVec3Dot(&axis, &mCorners[cIndex]);// axis.x * mCorners[cIndex].x + axis.y * mCorners[cIndex].y + axis.z * mCorners[cIndex].z;    
                     
                     if(pos < min1)
                     {
                          min1 = pos;  
                     }
                     else if(pos > max1)     
                     {
                          max1 = pos;
                     }                   
              }
              
              for(unsigned int cIndex=0; cIndex<8; cIndex++)
              {
					 D3DXVECTOR3 corner = pOBB->getCorner(cIndex);

                     float pos = D3DXVec3Dot(&axis, &corner);//axis.x * corner.x + axis.y * corner.y  + axis.z * corner.z;    
                     
                     if(pos < min2)
                     {
                          min2 = pos;  
                     }
                     else if(pos > max2)     
                     {
                          max2 = pos;
                     }                   
              }
              
              if(max1 < min2 ||  min1 > max2)
              {
                     return false;       
              }          
        }

		for(unsigned int aIndex=0; aIndex<3; aIndex++)
	    {
			  D3DXVECTOR3 axis = pOBB->getAxis(aIndex);

              float min1 = 1000000.0f;
              float max1 = -1000000.0f;
              float min2 = 1000000.0f;
              float max2 = -1000000.0f;
              
              for(unsigned int cIndex=0; cIndex<8; cIndex++)
              {
                     float pos = D3DXVec3Dot(&axis, &mCorners[cIndex]);//axis.x * mCorners[cIndex].x + axis.y * mCorners[cIndex].y + axis.z * mCorners[cIndex].z;    
                     
                     if(pos < min1)
                     {
                          min1 = pos;  
                     }
                     else if(pos > max1)     
                     {
                          max1 = pos;
                     }                   
              }
              
              for(unsigned int cIndex=0; cIndex<8; cIndex++)
              {
					 D3DXVECTOR3 corner = pOBB->getCorner(cIndex);

                     float pos = D3DXVec3Dot(&axis, &corner);//axis.x * corner.x + axis.y * corner.y  + axis.z * corner.z;    
                     
                     if(pos < min2)
                     {
                          min2 = pos;  
                     }
                     else if(pos > max2)     
                     {
                          max2 = pos;
                     }                   
              }
              
              if(max1 < min2 ||  min1 > max2)
              {
                     return false;       
              }          
        }
        
		if(calculateNormal)
		{
			setCollisionNormal(pOBB->getCenter() - mCenter);
			pOBB->setCollisionNormal(mCenter - pOBB->getCenter());
		}

        return true; 

	}
	else if(pCollisionObject->getCollisionType()==CollisionType_AABB)
	{
		AABB * pAABB = (AABB*) pCollisionObject;
		
		D3DXVECTOR3 aabbAxis[3];

		aabbAxis[0] = D3DXVECTOR3(1,0,0);
		aabbAxis[1] = D3DXVECTOR3(0,1,0);
		aabbAxis[2] = D3DXVECTOR3(0,0,1);
		
		for(int aIndex=0;aIndex<3;aIndex++)
		{
              float min1 = 1000000;
              float max1 = -1000000;
              float min2 = 1000000;
              float max2 = -1000000;
              
              for(int cIndex=0;cIndex<8;cIndex++)
              {
					 //D3DXVECTOR3 corner = pAABB->getCorner(cIndex);

                     //float pos = mAxis[aIndex].x * mCorners[cIndex].x + mAxis[aIndex].y * mCorners[cIndex].y + mAxis[aIndex].z * mCorners[cIndex].z;
					 float pos = D3DXVec3Dot(&mAxis[aIndex], &mCorners[cIndex]);
                     
                     if(pos < min1)
                     {
                          min1 = pos;  
                     }
                     else if(pos > max1)     
                     {
                          max1 = pos;
                     }                   
              }
              
              for(int cIndex=0;cIndex<8;cIndex++)
              {
					 D3DXVECTOR3 corner = pAABB->getCorner(cIndex);

                     //float pos = mAxis[aIndex].x * aabbCorners[cIndex].x + mAxis[aIndex].y * aabbCorners[cIndex].y  + mAxis[aIndex].z * aabbCorners[cIndex].z;
				     float pos = D3DXVec3Dot(&mAxis[aIndex], &corner);
                     
                     if(pos < min2)
                     {
                          min2 = pos;  
                     }
                     else if(pos > max2)     
                     {
                          max2 = pos;
                     }                   
              }
              
              if(max1 < min2 || max2 < min1)
              {
                     return false;       
              }           
        }

		for(int aIndex=0;aIndex<3;aIndex++)
		{
              float min1 = 1000000;
              float max1 = -1000000;
              float min2 = 1000000;
              float max2 = -1000000;
            
              for(int cIndex=0;cIndex<8;cIndex++)
              {
					 //D3DXVECTOR3 corner = pAABB->getCorner(cIndex);

                     //float pos = aabbAxis[aIndex].x * mCorners[cIndex].x + aabbAxis[aIndex].y * mCorners[cIndex].y + aabbAxis[aIndex].z * mCorners[cIndex].z;
					 float pos = D3DXVec3Dot(&aabbAxis[aIndex], &mCorners[cIndex]);
                     
                     if(pos < min1)
                     {
                          min1 = pos;  
                     }
                     else if(pos > max1)     
                     {
                          max1 = pos;
                     }                   
              }
              
              for(int cIndex=0;cIndex<8;cIndex++)
              {
					 D3DXVECTOR3 corner = pAABB->getCorner(cIndex);

                     //float pos = aabbAxis[aIndex].x * aabbCorners[cIndex].x + aabbAxis[aIndex].y * aabbCorners[cIndex].y  + aabbAxis[aIndex].z * aabbCorners[cIndex].z;  
				     float pos = D3DXVec3Dot(&aabbAxis[aIndex], &corner);
                     
                     if(pos < min2)
                     {
                          min2 = pos;  
                     }
                     else if(pos > max2)     
                     {
                          max2 = pos;
                     }                   
              }
              
              if(max1 < min2 || max2 < min1)
              {
                     return false;       
              }           
        }

		if(calculateNormal)
		{
			setCollisionNormal(pAABB->getCenter() - mCenter);
			pAABB->setCollisionNormal(mCenter - pAABB->getCenter());
		}
        
        return true;

	}
	else if(pCollisionObject->getCollisionType()==CollisionType_Sphere)
	{
		Sphere * pSphere = (Sphere*) pCollisionObject;

		for(int aIndex=0;aIndex<3;aIndex++)
	    {
              float min1 = 1000000;
			  float max1 = -1000000;
			  float min2 = 1000000;
              float max2 = -1000000;
              
              for(int cIndex=0;cIndex<8;cIndex++)
              {
                     float pos = mAxis[aIndex].x * mCorners[cIndex].x + mAxis[aIndex].y * mCorners[cIndex].y + mAxis[aIndex].z * mCorners[cIndex].z;    
                     
                     if(pos < min1)
                     {
                          min1 = pos;  
                     }
                     else if(pos > max1)     
                     {
                          max1 = pos;
                     }
              }
              
			  min2 = mAxis[aIndex].x * pSphere->getX() + mAxis[aIndex].y * pSphere->getY() + mAxis[aIndex].z * pSphere->getZ() - pSphere->getR();
			  max2 = mAxis[aIndex].x * pSphere->getX() + mAxis[aIndex].y * pSphere->getY() + mAxis[aIndex].z * pSphere->getZ() + pSphere->getR();
              
              if(max1 < min2 ||  min1 > max2)
              {
                     return false;       
              }          
        }
		
		D3DXVECTOR3 obbToSphere = D3DXVECTOR3(pSphere->getX(),pSphere->getY(),pSphere->getZ());
		obbToSphere -= mCenter;

		float length = obbToSphere.x * obbToSphere.x + obbToSphere.y * obbToSphere.y + obbToSphere.z * obbToSphere.z;
		length = sqrt(length);

		obbToSphere /= length;

		float min1 = 1000000;
		float max1 = -1000000;
		float min2 = 1000000;
        float max2 = -1000000;

		for(int cIndex=0;cIndex<8;cIndex++)
        {
             float pos = obbToSphere.x * mCorners[cIndex].x + obbToSphere.y * mCorners[cIndex].y + obbToSphere.z * mCorners[cIndex].z;    
                     
             if(pos < min1)
             {
                   min1 = pos;  
             }
             else if(pos > max1)     
             {
                   max1 = pos;
             }
        }
              
	    min2 = obbToSphere.x * pSphere->getX() + obbToSphere.y * pSphere->getY() + obbToSphere.z * pSphere->getZ() - pSphere->getR();
	    max2 = obbToSphere.x * pSphere->getX() + obbToSphere.y * pSphere->getY() + obbToSphere.z * pSphere->getZ() + pSphere->getR();
              
        if(max1 < min2 ||  min1 > max2)
        {
              return false;       
        }

		if(calculateNormal)
		{
			D3DXVECTOR3 sphereCenter = D3DXVECTOR3(pSphere->getX(), pSphere->getY(), pSphere->getZ());

			setCollisionNormal(sphereCenter - mCenter);
			pSphere->setCollisionNormal(mCenter - sphereCenter);
		}
        
        return true;
	}

	return false;
}