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