//
// Intersection functions
//
bool
Contains(const Point3D &point) const
{
Check_Object(this); Check_Object(&point);
Vector3D diff;
diff.Subtract(center, point);
return radius*radius - diff.GetLengthSquared() > -SMALL;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Sphere&
Sphere::Union(
const Sphere& sphere1,
const Sphere& sphere2
)
{
Check_Object(this);
Check_Object(&sphere1);
Check_Object(&sphere2);
//
//--------------------------------------------------
// Calculate the length between the sphere midpoints
//--------------------------------------------------
//
Vector3D dist;
dist.Subtract(sphere1.center, sphere2.center);
Scalar len = dist.GetLength();
//
//------------------------------------------------------
// If the sphere is contained in the old sphere, move on
//------------------------------------------------------
//
if (len + sphere1.radius <= sphere2.radius)
{
*this = sphere2;
return *this;
}
//
//----------------------------------------------------------
// If the new sphere contains the old sphere, use it instead
//----------------------------------------------------------
//
if (len + sphere2.radius <= sphere1.radius)
{
*this = sphere1;
return *this;
}
//
//------------------------------
// Calculate the new centerpoint
//------------------------------
//
len += sphere1.radius + sphere2.radius;
UnitVector3D direction;
direction.Normalize(dist);
len *= 0.5f;
center.AddScaled(
sphere2.center,
direction,
len - sphere2.radius
);
radius = len;
return *this;
}
bool
Intersects(const Sphere &sphere) const
{
Check_Object(this); Check_Object(&sphere);
Scalar r = radius + sphere.radius;
Vector3D temp;
temp.Subtract(center, sphere.center);
return r*r - temp.GetLengthSquared() >= -SMALL;
}
//
//#############################################################################
//#############################################################################
//
Scalar
Ray3D::GetDistanceTo(
const Sphere &sphere,
Scalar *penetration
) const
{
Scalar
b,c;
Vector3D
temp;
//
//-------------------------------------------------------------------------
// Set up to solve a quadratic equation for the intersection of the ray and
// sphere. The solution is based on finding the closest point on the line
// to the sphere, and then calculating the interval between the entry and
// exit points of the ray
//-------------------------------------------------------------------------
//
temp.Subtract(origin,sphere.center);
b = 2.0f * (direction * temp);
c = temp.GetLengthSquared() - sphere.radius*sphere.radius;
//
//--------------------------------------------------------------------------
// Compute the squared interval to use for the solution. If it is negative,
// then the ray misses the sphere
//--------------------------------------------------------------------------
//
*penetration = b*b - 4.0f*c;
if (*penetration<SMALL)
return 0.0f;
//
//-------------------------------------------------------------------------
// Otherwise, find the linear distance along the line of the entry point by
// subtracting half the interval between entry and exit points from the
// distance to the closest point on the sphere
//-------------------------------------------------------------------------
//
else
{
*penetration = Sqrt(*penetration);
return -0.5f*(b+*penetration);
}
}
//
//#############################################################################
//#############################################################################
//
UnitQuaternion&
UnitQuaternion::Subtract(
const UnitVector3D &end,
const UnitVector3D &start
)
{
Check_Pointer(this);
Check_Object(&start);
Check_Object(&end);
Vector3D
axis;
SinCosPair
delta;
delta.cosine = start*end;
//
//----------------------------------------------------------------------
// See if the vectors point in the same direction. If so, return a null
// rotation
//----------------------------------------------------------------------
//
if (Close_Enough(delta.cosine, 1.0f))
{
x = 0.0f;
y = 0.0f;
z = 0.0f;
w = 1.0f;
}
//
//-------------------------------------------------------------------------
// See if the vectors directly oppose each other. If so, pick the smallest
// axis coordinate and generate a vector along it. Project this onto the
// base vector and subtract it out, leaving a perpendicular projection.
// Extend that out to unit length, then set the angle to PI
//-------------------------------------------------------------------------
//
else if (Close_Enough(delta.cosine, -1.0f))
{
//
//---------------------------
// Pick out the smallest axis
//---------------------------
//
int
smallest=0;
Scalar
value=2.0f;
for (int i=X_Axis; i<=Z_Axis; ++i)
{
if (Abs(start[i]) < value)
{
smallest = i;
value = Abs(start[i]);
}
}
//
//----------------------------------------
// Set up a vector along the selected axis
//----------------------------------------
//
axis.x = 0.0f;
axis.y = 0.0f;
axis.z = 0.0f;
axis[smallest] = 1.0f;
//
//-------------------------------------------------------------------
// If the value on that axis wasn't zero, subtract out the projection
//-------------------------------------------------------------------
//
if (!Small_Enough(value))
{
Vector3D t;
t.Multiply(start, start*axis);
axis.Subtract(axis, t);
axis.Normalize(axis);
}
//
//----------------------
// Convert to quaternion
//----------------------
//
x = axis.x;
y = axis.y;
z = axis.z;
w = 0.0f;
}
//
//--------------------------------------------------
// Otherwise, generate the cross product and unitize
//--------------------------------------------------
//
else
{
axis.Cross(start, end);
delta.sine = axis.GetLength();
axis /= delta.sine;
//
//---------------------------------------------------------------
// Now compute sine and cosine of half the angle and generate the
// quaternion
//---------------------------------------------------------------
//
delta.sine = Sqrt((1.0f - delta.cosine)*0.5f);
x = axis.x * delta.sine;
y = axis.y * delta.sine;
z = axis.z * delta.sine;
w = Sqrt((1.0f + delta.cosine)*0.5f);
}
return *this;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Scalar
Line3D::GetDistanceTo(
const Sphere &sphere,
Scalar *penetration
) const
{
Check_Object(this);
Check_Object(&sphere);
Check_Pointer(penetration);
//
//-------------------------------------------------------------------
// Determine if ray intersects bounding sphere of object. If sphere
// is (X-C)*(X-C) = R^2 and ray is X = t*D+L for t >= 0, then
// intersection is obtained by plugging X into sphere equation to
// get quadratic: (D*D)t^2 + 2*(D*(L-C))t + (L-C)*(L-C) = 0
// Define a = D*D = 1.0f, b = 2*(D*(L-C)), and c = (L-C)*(L-C).
//-------------------------------------------------------------------
//
Vector3D diff;
diff.Subtract(origin, sphere.center);
Scalar b = (direction*diff) * 2.0f;
Scalar c = (diff*diff) - sphere.radius*sphere.radius;
//
//-------------------------------------------------------------------------
// If penetration is negative, we couldn't hit the sphere at all. If it is
// really small, it touches at only one place
//-------------------------------------------------------------------------
//
*penetration = b*b - 4.0f*c;
if (*penetration < -SMALL)
{
return -1.0f;
}
b *= -0.5f;
if (*penetration<SMALL)
{
*penetration = 0.0f;
Min_Clamp(b, 0.0f);
return (b > length) ? -1.0f : b;
}
//
//-------------------------------------------------------------
// We know we hit the sphere, so figure out where it first hits
//-------------------------------------------------------------
//
*penetration = 0.5f * Sqrt(*penetration);
if (b + *penetration < -SMALL)
{
return -1.0f;
}
b -= *penetration;
if (b > length)
{
return -1.0f;
}
Min_Clamp(b, 0.0f);
return b;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Scalar
Line3D::GetDistanceTo(
const OBB& box,
int *first_axis
)
{
Check_Object(this);
Check_Object(&box);
Check_Pointer(first_axis);
//
//------------------------------------------------------------------------
// Get the vector from the line to the centerpoint of the OBB. All planes
// will be generated relative to this
//------------------------------------------------------------------------
//
Point3D center;
center = box.localToParent;
Vector3D delta;
delta.Subtract(center, origin);
//
//--------------------------------------------------
// Set up the loop to examine each of the three axes
//--------------------------------------------------
//
Scalar enters = -100.0f - length;
Scalar leaves = length + 100.0f;
for (int axis=X_Axis; axis <= Z_Axis; ++axis)
{
UnitVector3D
normal(
box.localToParent(axis, X_Axis),
box.localToParent(axis, Y_Axis),
box.localToParent(axis, Z_Axis)
);
//
//----------------------------------------------------------------------
// Now, we have to calculate how far the line moves along the normal per
// unit traveled down the line. If it is perpendicular to the normal,
// then it will hit or miss based solely upon the origin location
//----------------------------------------------------------------------
//
Scalar drift = direction * normal;
Scalar distance;
if (Small_Enough(drift))
{
distance = delta * normal;
if (Fabs(distance) > box.axisExtents[axis])
return -1.0f;
else
continue;
}
//
//--------------------------------------------------------------------
// We know the line is not parallel, so we will now calculate how long
// the line will stay inside the box. We also will calculate how far
// from the origin to the centerplane of the OBB
//--------------------------------------------------------------------
//
drift = 1.0f / drift;
Scalar span = box.axisExtents[axis] * Fabs(drift);
distance = (delta * normal) * drift;
//
//--------------------------------------------------------------------
// Now adjust where the line can enter and leave the OBB, and if it is
// no longer possible to hit, stop checking
//--------------------------------------------------------------------
//
Scalar enter = distance - span;
Scalar leave = distance + span;
if (enter > enters)
{
*first_axis = axis;
enters = enter;
}
if (leave < leaves)
leaves = leave;
if (enters > leaves)
return -1.0f;
}
//
//-------------------------------------------------------------------------
// If we got here, then the line in theory can hit the OBB, so now we check
// to make sure it hits it within the allowed span of the line
//-------------------------------------------------------------------------
//
if (leaves < 0.0f || enters > length)
return -1.0f;
Min_Clamp(enters, 0.0f);
return enters;
}
