C++ vec::IsNormalized方法代码示例

int Sphere::IntersectLine(const vec &linePos, const vec &lineDir, const vec &sphereCenter,
                          float sphereRadius, float &t1, float &t2)
{
	assume2(lineDir.IsNormalized(), lineDir, lineDir.LengthSq());
	assume1(sphereRadius >= 0.f, sphereRadius);

	/* A line is represented explicitly by the set { linePos + t * lineDir }, where t is an arbitrary float.
	  A sphere is represented implictly by the set of vectors that satisfy ||v - sphereCenter|| == sphereRadius.
	  To solve which points on the line are also points on the sphere, substitute v <- linePos + t * lineDir
	  to obtain:

	    || linePos + t * lineDir - sphereCenter || == sphereRadius, and squaring both sides we get
	    || linePos + t * lineDir - sphereCenter ||^2 == sphereRadius^2, or rearranging:
	    || (linePos - sphereCenter) + t * lineDir ||^2 == sphereRadius^2. */

	// This equation represents the set of points which lie both on the line and the sphere. There is only one
	// unknown variable, t, for which we solve to get the actual points of intersection.

	// Compute variables from the above equation:
	const vec a = linePos - sphereCenter;
	const float radSq = sphereRadius * sphereRadius;

	/* so now the equation looks like

	    || a + t * lineDir ||^2 == radSq.

	  Since ||x||^2 == <x,x> (i.e. the square of a vector norm equals the dot product with itself), we get
	
	    <a + t * lineDir, a + t * lineDir> == radSq,
	
	  and using the identity <a+b, a+b> == <a,a> + 2*<a,b> + <b,b> (which holds for dot product when a and b are reals),
	  we have

	    <a,a> + 2 * <a, t * lineDir> + <t * lineDir, t * lineDir> == radSq, or		
	    <a,a> - radSq + 2 * <a, lineDir> * t + <lineDir, lineDir> * t^2 == 0, or

	    C + Bt + At^2 == 0, where

	    C = <a,a> - radSq,
	    B = 2 * <a, lineDir>, and
	    A = <lineDir, lineDir> == 1, since we assumed lineDir is normalized. */

	// Warning! If Dot(a,a) is large (distance between line pos and sphere center) and sphere radius very small,
	// catastrophic cancellation can occur here!
	const float C = Dot(a,a) - radSq;
	const float B = 2.f * Dot(a, lineDir);

	/* The equation A + Bt + Ct^2 == 0 is a second degree equation on t, which is easily solvable using the
	  known formula, and we obtain

	    t = [-B +/- Sqrt(B^2 - 4AC)] / 2A. */

	float D = B*B - 4.f * C; // D = B^2 - 4AC.
	if (D < 0.f) // There is no solution to the square root, so the ray doesn't intersect the sphere.
	{
		// Output a degenerate enter-exit range so that batch processing code may use min of t1's and max of t2's to
		// compute the nearest enter and farthest exit without requiring branching on the return value of this function.
		t1 = FLOAT_INF;
		t2 = -FLOAT_INF;
		return 0;
	}

	if (D < 1e-4f) // The expression inside Sqrt is ~ 0. The line is tangent to the sphere, and we have one solution.
	{
		t1 = t2 = -B * 0.5f;
		return 1;
	}

	// The Sqrt expression is strictly positive, so we get two different solutions for t.
	D = Sqrt(D);
	t1 = (-B - D) * 0.5f;
	t2 = (-B + D) * 0.5f;
	return 2;
}

bool AABB::IntersectLineAABB_CPP(const vec &linePos, const vec &lineDir, float &tNear, float &tFar) const
{
	assume2(lineDir.IsNormalized(), lineDir, lineDir.LengthSq());
	assume2(tNear <= tFar && "AABB::IntersectLineAABB: User gave a degenerate line as input for the intersection test!", tNear, tFar);
	// The user should have inputted values for tNear and tFar to specify the desired subrange [tNear, tFar] of the line
	// for this intersection test.
	// For a Line-AABB test, pass in
	//    tNear = -FLOAT_INF;
	//    tFar = FLOAT_INF;
	// For a Ray-AABB test, pass in
	//    tNear = 0.f;
	//    tFar = FLOAT_INF;
	// For a LineSegment-AABB test, pass in
	//    tNear = 0.f;
	//    tFar = LineSegment.Length();

	// Test each cardinal plane (X, Y and Z) in turn.
	if (!EqualAbs(lineDir.x, 0.f))
	{
		float recipDir = RecipFast(lineDir.x);
		float t1 = (minPoint.x - linePos.x) * recipDir;
		float t2 = (maxPoint.x - linePos.x) * recipDir;

		// tNear tracks distance to intersect (enter) the AABB.
		// tFar tracks the distance to exit the AABB.
		if (t1 < t2)
			tNear = Max(t1, tNear), tFar = Min(t2, tFar);
		else // Swap t1 and t2.
			tNear = Max(t2, tNear), tFar = Min(t1, tFar);

		if (tNear > tFar)
			return false; // Box is missed since we "exit" before entering it.
	}
	else if (linePos.x < minPoint.x || linePos.x > maxPoint.x)
		return false; // The ray can't possibly enter the box, abort.

	if (!EqualAbs(lineDir.y, 0.f))
	{
		float recipDir = RecipFast(lineDir.y);
		float t1 = (minPoint.y - linePos.y) * recipDir;
		float t2 = (maxPoint.y - linePos.y) * recipDir;

		if (t1 < t2)
			tNear = Max(t1, tNear), tFar = Min(t2, tFar);
		else // Swap t1 and t2.
			tNear = Max(t2, tNear), tFar = Min(t1, tFar);

		if (tNear > tFar)
			return false; // Box is missed since we "exit" before entering it.
	}
	else if (linePos.y < minPoint.y || linePos.y > maxPoint.y)
		return false; // The ray can't possibly enter the box, abort.

	if (!EqualAbs(lineDir.z, 0.f)) // ray is parallel to plane in question
	{
		float recipDir = RecipFast(lineDir.z);
		float t1 = (minPoint.z - linePos.z) * recipDir;
		float t2 = (maxPoint.z - linePos.z) * recipDir;

		if (t1 < t2)
			tNear = Max(t1, tNear), tFar = Min(t2, tFar);
		else // Swap t1 and t2.
			tNear = Max(t2, tNear), tFar = Min(t1, tFar);
	}
	else if (linePos.z < minPoint.z || linePos.z > maxPoint.z)
		return false; // The ray can't possibly enter the box, abort.

	return tNear <= tFar;
}