text: // Geometric Tools, LLC
  // Copyright (c) 1998-2014
  // Distributed under the Boost Software License, Version 1.0.
  // http://www.boost.org/LICENSE_1_0.txt
  // http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
  //
  // File Version: 5.0.3 (2011/10/26)
  
  #include "Wm5MathematicsPCH.h"
  #include "Wm5IntrSphere3Sphere3.h"
  
  namespace Wm5
  {
  //----------------------------------------------------------------------------
  template <typename Real>
  IntrSphere3Sphere3<Real>::IntrSphere3Sphere3 (const Sphere3<Real>& rkSphere0,
      const Sphere3<Real>& rkSphere1)
      :
      mSphere0(&rkSphere0),
      mSphere1(&rkSphere1)
  {
  }
  //----------------------------------------------------------------------------
  template <typename Real>
  const Sphere3<Real>& IntrSphere3Sphere3<Real>::GetSphere0 () const
  {
      return *mSphere0;
  }
  //----------------------------------------------------------------------------
  template <typename Real>
  const Sphere3<Real>& IntrSphere3Sphere3<Real>::GetSphere1 () const
  {
      return *mSphere1;
  }
  //----------------------------------------------------------------------------
  template <typename Real>
  bool IntrSphere3Sphere3<Real>::Test ()
  {
      Vector3<Real> diff = mSphere1->Center - mSphere0->Center;
      Real rSum = mSphere0->Radius + mSphere1->Radius;
      return diff.SquaredLength() <= rSum*rSum;
  }
  //----------------------------------------------------------------------------
  template <typename Real>
  bool IntrSphere3Sphere3<Real>::Find ()
  {
      // Plane of intersection must have N as its normal.
      Vector3<Real> C1mC0 = mSphere1->Center - mSphere0->Center;
      Real sqrLen = C1mC0.SquaredLength();
      Real r0 = mSphere0->Radius, r1 = mSphere1->Radius;
  
      Real rSum = r0 + r1;
      Real rSumSqr = rSum*rSum;
      if (sqrLen > rSumSqr)
      {
          // Spheres are disjoint/separated.
          mIntersectionType = IT_EMPTY;
          return false;
      }
      if (sqrLen == rSumSqr)
      {
          // Spheres are just touching.  The caller must call
          // GetIntersectionType() to determine what type of intersection has
          // occurred.  In this case, GetContactPoint() should be called, not
          // GetCircle().  The circle parameters are set just in case the caller
          // does not test for intersection type.
          C1mC0.Normalize();
          mContactPoint = mSphere0->Center + r0*C1mC0;
          mCircle.Normal = C1mC0;
          mCircle.Center = mContactPoint;
          mCircle.Radius = (Real)0;
          mIntersectionType = IT_POINT;
          return true;
      }
  
      Real rDif = r0 - r1;
      Real rDifSqr = rDif*rDif;
      if (sqrLen < rDifSqr)
      {
          // One sphere is strictly contained in the other.  The caller must
          // call GetIntersectionType() to determine what type of intersection
          // has occurred.  In this case, neither GetCircle() nor
          // GetContactPoint() should be called.  The circle and contact
          // parameters are set just in case the caller does not test for
          // intersection type, but the choices are arbitrary.
          C1mC0.Normalize();
          mContactPoint = ((Real)0.5)*(mSphere0->Center + mSphere1->Center);
          mCircle.Normal = C1mC0;
          mCircle.Center = mContactPoint;
          mCircle.Radius = (Real)0;
          mIntersectionType = (rDif <= (Real)0 ? IT_SPHERE0 : IT_SPHERE1);
          return true;
      }
      if (sqrLen == rDifSqr)
      {
          // One sphere is contained in the other sphere but with a single point
          // of contact.  The caller must call GetIntersectionType() to
          // determine what type of intersection has occurred.  In this case,
          // GetContactPoint() should be called.  The circle parameters are set
          // just in case the caller does not test for intersection type.
          C1mC0.Normalize();
          mCircle.Radius = (Real)0;
          mCircle.Normal = C1mC0;
          if (rDif <= (Real)0)
          {
              mContactPoint = mSphere1->Center + r1*C1mC0;
              mIntersectionType = IT_SPHERE0_POINT;
          }
          else
          {
              mContactPoint = mSphere0->Center + r0*C1mC0;
              mIntersectionType = IT_SPHERE1_POINT;
          }
          mCircle.Center = mContactPoint;
          return true;
      }
  
      // Compute t for which the circle of intersection has center
      // K = C0 + t*(C1 - C0).
      Real t = ((Real)0.5)*((Real)1 + rDif*rSum/sqrLen);
  
      // Center and radius of circle of intersection.
      mCircle.Center = mSphere0->Center + t*C1mC0;
      mCircle.Radius = Math<Real>::Sqrt(Math<Real>::FAbs(r0*r0 - t*t*sqrLen));
  
      // Compute N, U, and V for plane of circle.
      C1mC0.Normalize();
      mCircle.Normal = C1mC0;
      Vector3<Real>::GenerateComplementBasis(mCircle.Direction0,
          mCircle.Direction1, mCircle.Normal);
  
      // The intersection is a circle.
      mIntersectionType = IT_CIRCLE;
      return true;
  }
  //----------------------------------------------------------------------------
  template <typename Real>
  bool IntrSphere3Sphere3<Real>::Test (Real tmax,
      const Vector3<Real>& velocity0, const Vector3<Real>& velocity1)
  {
      Vector3<Real> relVelocity = velocity1 - velocity0;
      Real a = relVelocity.SquaredLength();
      Vector3<Real> CDiff = mSphere1->Center - mSphere0->Center;
      Real c = CDiff.SquaredLength();
      Real rSum = mSphere0->Radius + mSphere1->Radius;
      Real rSumSqr = rSum*rSum;
  
      if (a > (Real)0)
      {
          Real b = CDiff.Dot(relVelocity);
          if (b <= (Real)0)
          {
              if (-tmax*a <= b)
              {
                  return a*c - b*b <= a*rSumSqr;
              }
              else
              {
                  return tmax*(tmax*a + ((Real)2)*b) + c <= rSumSqr;
              }
          }
      }
  
      return c <= rSumSqr;
  }
  //----------------------------------------------------------------------------
  template <typename Real>
  bool IntrSphere3Sphere3<Real>::Find (Real tmax,
      const Vector3<Real>& velocity0, const Vector3<Real>& velocity1)
  {
      Vector3<Real> relVelocity = velocity1 - velocity0;
      Real a = relVelocity.SquaredLength();
      Vector3<Real> CDiff = mSphere1->Center - mSphere0->Center;
      Real c = CDiff.SquaredLength();
      Real rSum = mSphere0->Radius + mSphere1->Radius;
      Real rSumSqr = rSum*rSum;
  
      if (a > (Real)0)
      {
          Real b = CDiff.Dot(relVelocity);
          if (b <= (Real)0)
          {
              if (-tmax*a <= b || tmax*(tmax*a + ((Real)2.0)*b) + c <= rSumSqr)
              {
                  Real cdiff = c - rSumSqr;
                  Real discr = b*b - a*cdiff;
                  if (discr >= (Real)0)
                  {
                      if (cdiff <= (Real)0)
                      {
                          // The spheres are initially intersecting.  Estimate a
                          // point of contact by using the midpoint of the line
                          // segment connecting the sphere centers.
                          mContactTime = (Real)0;
                          mContactPoint = ((Real)0.5)*(mSphere0->Center +
                              mSphere1->Center);
                      }
                      else
                      {
                          // The first time of contact is in [0,tmax].
                          mContactTime = -(b + Math<Real>::Sqrt(discr))/a;
                          if (mContactTime < (Real)0)
                          {
                              mContactTime = (Real)0;
                          }
                          else if (mContactTime > tmax)
                          {
                              mContactTime = tmax;
                          }
  
                          Vector3<Real> newCDiff = CDiff +
                              mContactTime*relVelocity;
  
                          mContactPoint = mSphere0->Center +
                              mContactTime*velocity0 +
                              (mSphere0->Radius/rSum)*newCDiff;
                      }
                      return true;
                  }
              }
              return false;
          }
      }
  
      if (c <= rSumSqr)
      {
          // The spheres are initially intersecting.  Estimate a point of
          // contact by using the midpoint of the line segment connecting the
          // sphere centers.
          mContactTime = (Real)0;
          mContactPoint = ((Real)0.5)*(mSphere0->Center + mSphere1->Center);
          return true;
      }
  
      return false;
  }
  //----------------------------------------------------------------------------
  template <typename Real>
  const Circle3<Real>& IntrSphere3Sphere3<Real>::GetCircle () const
  {
      return mCircle;
  }
  //----------------------------------------------------------------------------
  template <typename Real>
  const Vector3<Real>& IntrSphere3Sphere3<Real>::GetContactPoint () const
  {
      return mContactPoint;
  }
  //----------------------------------------------------------------------------
  
  //----------------------------------------------------------------------------
  // Explicit instantiation.
  //----------------------------------------------------------------------------
  template WM5_MATHEMATICS_ITEM
  class IntrSphere3Sphere3<float>;
  
  template WM5_MATHEMATICS_ITEM
  class IntrSphere3Sphere3<double>;
  //----------------------------------------------------------------------------
  }
  