int BasicPrimitiveTests::IntersectingSegmentAgainstOBB(const LineSegment & segment, const OBB & obb, float & rtn_t1, float & rtn_t2)
{
	
	Eigen::Matrix3f obb_rotation;
	obb.GetRotationMatrix(obb_rotation);
	Eigen::Vector3f segment_A_obb = obb_rotation * (segment.GetPointA() - obb.GetCenter());
	Eigen::Vector3f segment_B_obb = obb_rotation * (segment.GetPointB() - obb.GetCenter());
	
	rtn_t1 = 0.0f;
	rtn_t2 = FLT_MAX;
	Eigen::Vector3f aabb_min = -obb.GetExtents();
	Eigen::Vector3f aabb_max = obb.GetExtents();
	Eigen::Vector3f segment_direction = segment_B_obb - segment_A_obb;
	for (int i = 0; i < 3; ++i)
	{
		if (abs(segment_direction[i]) < EPSILON)
		{
			//Ray is parallel to slab. Not hit if origin not within slab.
			if (segment_A_obb[i] < aabb_min[i] || segment_A_obb[i] > aabb_max[i])
			{
				return 0;
			}
		}
		else
		{
			float one_over_direction = 1.0f / segment_direction[i];
			float t1 = (aabb_min[i] - segment_A_obb[i]) * one_over_direction;
			float t2 = (aabb_max[i] - segment_A_obb[i]) * one_over_direction;
			if (t1 > 1.0f && t2 > 1.0f)
			{
				return 0;
			}
			if (t1 > t2) Swap(t1, t2);
			if (t1 > rtn_t1) rtn_t1 = t1;
			if (t2 > rtn_t2) rtn_t2 = t2;
			if (rtn_t1 > rtn_t2) return 0;
		}
	}
	if (rtn_t1 >= 0.0f && rtn_t1 <= 1.0f)
	{
		if (rtn_t2 >= 0.0f && rtn_t2 <= 1.0f)
		{
			return 2;
		}
		else
		{
			return 1;
		}
	}
	else
	{
		Swap(rtn_t1, rtn_t2);
		if (rtn_t1 >= 0.0f && rtn_t1 <= 1.0f)
		{
			return 1;
		}
		return 0;
	}
	
}
int BasicPrimitiveTests::IntersectingSegmentAgainstSphere(const LineSegment & segment, const BoundingSphere & sphere, float & rtn_t1, float & rtn_t2)
{
	
	Eigen::Vector3f m = segment.GetPointA() - sphere.GetCenter();
	float b = (m).dot(segment.GetPointB() - segment.GetPointA());
	float c = m.dot(m);
	float discriminant = b * b - c;

	int intersection_count = 0;
	float t1 = -b - sqrt(discriminant);
	float t2 = -b + sqrt(discriminant);
	if (t1 >= 0.0f && t1 <= 1.0f && t2 >= 0.0f && t2 <= 1.0f)
	{
		rtn_t1 = t1;
		rtn_t2 = t2;	
		return 2;
	}
	else if (t1 >= 0.0f && t1 <= 1.0f)
	{
		rtn_t1 = t1;
		return 1;
	}
	else if (t2 >= 0.0f && t2 <= 1.0f)
	{
		rtn_t1 = t2;
		return 1;
	}
	
	return 0;
}
int BasicPrimitiveTests::IntersectingSegmentAgainstAABB(const LineSegment & segment, const AABB & aabb)
{
	
	Eigen::Vector3f segment_a_aabb = segment.GetPointA() - aabb.GetCenter();
	Eigen::Vector3f segment_b_aabb = segment.GetPointB() - aabb.GetCenter();
	Eigen::Vector3f segment_m_aabb = (segment_a_aabb + segment_b_aabb) * 0.5f;
	
	return 0;
}
int BasicPrimitiveTests::IntersectingSegmentAgainstOBB(const LineSegment & segment, const OBB & obb)
{
	
	Eigen::Matrix3f obb_rotation;
	obb.GetRotationMatrix(obb_rotation);
	Eigen::Vector3f segment_A_obb = obb_rotation * (segment.GetPointA() - obb.GetCenter());
	Eigen::Vector3f segment_B_obb = obb_rotation * (segment.GetPointB() - obb.GetCenter());
	
	return 0;
}
bool BasicPrimitiveTests::IntersectingSegmentAgainstPlane(const LineSegment & segment, const Plane & plane, float & rtn_t)
{
	/*
		Main Idea:
            -> Subsitutes line equation into plane to find intersection
                -> Tests if intersection is within segment endpoints

        Let:
            -> Line Segment S:
                -> S(t) = A + t * (B-A)
                    -> for 0 <= t <= 1
            -> Plane P:
                -> dot(n, X) = d

        Solve for "t" of intersection:
            -> t = (    d - dot(n, A)    /    dot(n, B-A)    )

        -> Return intersection only if:
            -> 0 <= t <= 1
	*/
	
	rtn_t = (    plane.GetD() - plane.GetNormal().dot(segment.GetPointA())    )    /    plane.GetNormal().dot(segment.GetPointB() - segment.GetPointA());
	return (rtn_t <= 1.0f && rtn_t >= 0.0f);
	
}