bool Plane::Equals(const Plane &other, float epsilon) const
{
return IsParallel(other, epsilon) && EqualAbs(d, other.d, epsilon);
}
bool Plane::Contains(const Circle &circle, float epsilon) const
{
return Contains(circle.pos, epsilon) && (EqualAbs(Abs(Dot(normal, circle.normal)), 1.f) || circle.r <= epsilon);
}
bool Plane::SetEquals(const Plane &plane, float epsilon) const
{
return (normal.Equals(plane.normal) && EqualAbs(d, plane.d, epsilon)) ||
(normal.Equals(-plane.normal) && EqualAbs(-d, plane.d, epsilon));
}
bool Quat::IsNormalized(float epsilonSq) const
{
return EqualAbs(LengthSq(), 1.f, epsilonSq);
}
bool Quat::Equals(const Quat &rhs, float epsilon) const
{
return EqualAbs(x, rhs.x, epsilon) && EqualAbs(y, rhs.y, epsilon) && EqualAbs(z, rhs.z, epsilon) && EqualAbs(w, rhs.w, epsilon);
}
float4 float3x4::MulDir(const float4 &directionVector) const
{
assume(EqualAbs(directionVector.w, 0.f));
return this->TransformDir(directionVector);
}
/** Computes the (s,t) coordinates of the smallest sphere that passes through three points (0,0,0), ab and ac.
@param ab The first point to fit the sphere through.
@param ac The second point to fit the sphere through. The third point is hardcoded to (0,0,0). When fitting a sphere
through three points a, b and c, pass in b-a as the parameter ab, and c-a as the parameter ac (i.e. translate
the coordinate system center to lie at a).
@param s [out] Outputs the s-coordinate of the sphere center (in the 2D barycentric UV convention)
@param t [out] Outputs the t-coordinate of the sphere center (in the 2D barycentric UV convention) To
compute the actual point, calculate the expression origin + s*ab + t*ac.
@note The returned sphere is one that passes through the three points (0,0,0), ab and ac. It is NOT necessarily the
smallest sphere that encloses these three points!
@return True if the function succeeded. False on failure. This function fails if the points (0,0,0), ab and ac
are collinear, in which case there does not exist a sphere that passes through the three given points. */
bool FitSphereThroughPoints(const vec &ab, const vec &ac, float &s, float &t)
{
/* The task is to compute the minimal radius sphere through the three points
a, b and c. (Note that this is not necessarily the minimal radius sphere enclosing
the points a, b and c!)
Denote by p the sphere center position, and r the sphere radius. If the sphere
is to run through the points a, b and c, then the center point of the sphere
must be equidistant to these points, i.e.
|| p - a || == || p - b || == || p - c ||,
or
a^2 - 2ap + p^2 == b^2 - 2bp + p^2 == c^2 - 2cp + p^2.
Subtracting pairwise, we get
(b-a)p == (b^2 - a^2)/2 and (1)
(c-a)p == (c^2 - a^2)/2. (2)
Additionally, the center point of the sphere must lie on the same plane as the triangle
defined by the points a, b and c. Therefore, the point p can be represented as a 2D
barycentric coordinates (s,t) as follows:
p == a + s*(b-a) + t*(c-a). (3)
Now, without loss of generality, assume that the point a lies at origin (translate the origin
of the coordinate system to be centered at the point a), i.e. make the substitutions
A = (0,0,0), B = b-a, C = c-a, and we have:and we have:
BP == B^2/2, (1')
CP == C^2/2 and (2')
P == s*B + t*C. (3') */
const float BB = Dot(ab,ab);
const float CC = Dot(ac,ac);
const float BC = Dot(ab,ac);
/* Substitute (3') into (1') and (2'), to obtain a matrix equation
( B^2 BC ) * (s) = (B^2 / 2)
( BC C^2 ) (t) (C^2 / 2)
which equals
(s) = ( B^2 BC )^-1 * (B^2 / 2)
(t) ( BC C^2 ) (C^2 / 2)
Use the formula for inverting a 2x2 matrix, and we have
(s) = 1 / (2 * B^2 * C^2 - (BC)^2) * ( C^2 -BC ) * (B^2)
(t) ( -BC B^2 ) (C^2)
*/
float denom = BB*CC - BC*BC;
if (EqualAbs(denom, 0.f, 5e-3f))
return false;
denom = 0.5f / denom; // == 1 / (2 * B^2 * C^2 - (BC)^2)
s = (CC * BB - BC * CC) * denom;
t = (CC * BB - BC * BB) * denom;
return true;
}
float4 float3x4::MulPos(const float4 &pointVector) const
{
assume(!EqualAbs(pointVector.w, 0.f));
return this->Transform(pointVector);
}
bool float3x4::HasUniformScale(float epsilon) const
{
float3 scale = ExtractScale();
return EqualAbs(scale.x, scale.y, epsilon) && EqualAbs(scale.x, scale.z, epsilon);
}
bool float3x4::IsSymmetric(float epsilon) const
{
return EqualAbs(v[0][1], v[1][0], epsilon) &&
EqualAbs(v[0][2], v[2][0], epsilon) &&
EqualAbs(v[1][2], v[2][1], epsilon);
}
bool float3x4::IsUpperTriangular(float epsilon) const
{
return EqualAbs(v[1][0], 0.f, epsilon)
&& EqualAbs(v[2][0], 0.f, epsilon)
&& EqualAbs(v[2][1], 0.f, epsilon);
}
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;
}
