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

float3 float3::AnotherPerpendicular(const float3 &hint, const float3 &hint2) const
{
	assume(!this->IsZero());
	assume(hint.IsNormalized());
	assume(hint2.IsNormalized());
	float3 firstPerpendicular = Perpendicular(hint, hint2);
	float3 v = this->Cross(firstPerpendicular);
	return v.Normalized();
}

bool MUST_USE_RESULT float3::AreOrthonormal(const float3 &a, const float3 &b, const float3 &c, float epsilon)
{
	return a.IsPerpendicular(b, epsilon) &&
	       a.IsPerpendicular(c, epsilon) &&
	       b.IsPerpendicular(c, epsilon) &&
	       a.IsNormalized(epsilon*epsilon) &&
	       b.IsNormalized(epsilon*epsilon) &&
	       c.IsNormalized(epsilon*epsilon);
}

float3x4 float3x4::RotateFromTo(const float3 &sourceDirection, const float3 &targetDirection)
{
	assume(sourceDirection.IsNormalized());
	assume(targetDirection.IsNormalized());
	float3x4 r;
	r.SetRotatePart(Quat::RotateFromTo(sourceDirection, targetDirection));
	r.SetTranslatePart(0, 0, 0);
	return r;
}

Quat MUST_USE_RESULT Quat::RotateFromTo(const float3 &sourceDirection, const float3 &targetDirection)
{
	assume(sourceDirection.IsNormalized());
	assume(targetDirection.IsNormalized());
	float angle = sourceDirection.AngleBetweenNorm(targetDirection);
	assume(angle >= 0.f);
	// If sourceDirection == targetDirection, the cross product comes out zero, and normalization would fail. In that case, pick an arbitrary axis.
	float3 axis = sourceDirection.Cross(targetDirection);
	float oldLength = axis.Normalize();
	if (oldLength == 0)
		axis = float3(1, 0, 0);
	return Quat(axis, angle);
}

float4 float4::Perpendicular3(const float3 &hint, const float3 &hint2) const
{
    assume(!this->IsZero3());
    assume(EqualAbs(w, 0));
    assume(hint.IsNormalized());
    assume(hint2.IsNormalized());
    float3 v = this->Cross3(hint).xyz();
    float len = v.Normalize();
    if (len == 0)
        return float4(hint2, 0);
    else
        return float4(v, 0);
}

bool AABB::IntersectRayAABB(const float3 &rayPos, const float3 &rayDir, float &tNear, float &tFar) const
{
	assume(rayDir.IsNormalized());
	tNear = -FLOAT_INF;
	tFar = FLOAT_INF;

	for(int i = 0; i < 3; ++i) // loop for each AABB plane (X,Y,Z)
	{
		if (EqualAbs(rayDir[i], 0.f)) // ray is parallel to plane in question
			if (rayPos[i] < minPoint[i] || rayPos[i] > maxPoint[i]) // early-out if the ray can't possibly enter the box.
				return false;

		// intersection distances to plane.
		float recipDir = 1.f / rayDir[i];
		float t1 = (minPoint[i] - rayPos[i]) * recipDir;
		float t2 = (maxPoint[i] - rayPos[i]) * recipDir;

		if (t1 > t2) Swap(t1, t2); // swap so that t1 is the distance to nearer of the two planes.
		if (t1 > tNear) tNear = t1; // tNear tracks distance to intersect the AABB.
		if (t2 < tFar)
			tFar = t2; // tFar tracks the distance to exit the AABB.
		if (tNear > tFar) // Box is missed since we "exit" before entering it.
			return false;
		if (tFar < 0) // Box is behind the ray.
			return false;
	}
	return true;
}

Quat MUST_USE_RESULT Quat::RotateFromTo(const float3 &sourceDirection, const float3 &targetDirection)
{
	assume(sourceDirection.IsNormalized());
	assume(targetDirection.IsNormalized());
	// If sourceDirection == targetDirection, the cross product comes out zero, and normalization would fail. In that case, pick an arbitrary axis.
	float3 axis = sourceDirection.Cross(targetDirection);
	float oldLength = axis.Normalize();
	if (oldLength != 0.f)
	{
		float halfCosAngle = 0.5f*sourceDirection.Dot(targetDirection);
		float cosHalfAngle = Sqrt(0.5f + halfCosAngle);
		float sinHalfAngle = Sqrt(0.5f - halfCosAngle);
		return Quat(axis.x * sinHalfAngle, axis.y * sinHalfAngle, axis.z * sinHalfAngle, cosHalfAngle);
	}
	else
		return Quat(1.f, 0.f, 0.f, 0.f);

}

void Quat::SetFromAxisAngle(const float3 &axis, float angle)
{
	assume(axis.IsNormalized());
	assume(MATH_NS::IsFinite(angle));
	float cosz = Cos(angle/2.f);
	float sinz = Sin(angle/2.f);
	x = axis.x * sinz;
	y = axis.y * sinz;
	z = axis.z * sinz;
	w = cosz;
}

float float3::AngleBetweenNorm(const float3 &other) const
{
	assume(this->IsNormalized());
	assume(other.IsNormalized());
	float cosa = Dot(other);
	if (cosa >= 1.f)
		return 0.f;
	else if (cosa <= -1.f)
		return pi;
	else
		return acos(cosa);
}

void Capsule::ProjectToAxis(const float3 &direction, float &outMin, float &outMax) const
{
	outMin = Dot(direction, l.a);
	outMax = Dot(direction, l.b);
	if (outMax < outMin)
		Swap(outMin, outMax);

	// The following requires that direction is normalized, otherwise we would have to sub/add 'r * direction.Length()', but
	// don't want to do that for performance reasons.
	assume(direction.IsNormalized());
	outMin -= r;
	outMax += r;
}

void Quat::SetFromAxisAngle(const float3 &axis, float angle)
{
#if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
	SetFromAxisAngle(load_vec3(axis.ptr(), 0.f), angle);
#else
	assume1(axis.IsNormalized(), axis);
	assume1(MATH_NS::IsFinite(angle), angle);
	float sinz, cosz;
	SinCos(angle*0.5f, sinz, cosz);
	x = axis.x * sinz;
	y = axis.y * sinz;
	z = axis.z * sinz;
	w = cosz;
#endif
}

float3 float3::Reflect(const float3 &normal) const
{
	assume2(normal.IsNormalized(), normal.SerializeToCodeString(), normal.Length());
	return 2.f * this->ProjectToNorm(normal) - *this;
}

float4 float4::Reflect3(const float3 &normal) const
{
	assume2(normal.IsNormalized(), normal.SerializeToCodeString(), normal.Length());
	assume(EqualAbs(w, 0));
	return 2.f * this->ProjectToNorm3(normal) - *this;
}

bool Plane::IsInPositiveDirection(const float3 &directionVector) const
{
    assume(directionVector.IsNormalized());
    return normal.Dot(directionVector) >= 0.f;
}

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

	/* 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 float3 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. */

	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.
		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;
}