// This file contains some code based on the code from Magic Software.
// That code is available under a Free Source License Agreement
// that can be found at http://www.magic-software.com/License/free.pdf
 
#include <gazebo/ode/common.h>
#include <gazebo/ode/odemath.h>
#include <gazebo/ode/collision.h>
#include "config.h"
#include "collision_trimesh_internal.h"

//------------------------------------------------------------------------------
/**
  @brief Finds the shortest distance squared between a point and a triangle.
  
  @param pfSParam  Barycentric coordinate of triangle at point closest to p (u)
  @param pfTParam  Barycentric coordinate of triangle at point closest to p (v)
  @return Shortest distance squared.
  
  The third Barycentric coordinate is implicit, ie. w = 1.0 - u - v
  
  Taken from:
  Magic Software, Inc.
  http://www.magic-software.com
*/
dReal SqrDistancePointTri( const dVector3 p, const dVector3 triOrigin, 
                           const dVector3 triEdge0, const dVector3 triEdge1,
                           dReal* pfSParam, dReal* pfTParam )
{
  dVector3 kDiff;
  Vector3Subtract( triOrigin, p, kDiff );
  dReal fA00 = dCalcVectorDot3( triEdge0, triEdge0 );
  dReal fA01 = dCalcVectorDot3( triEdge0, triEdge1 );
  dReal fA11 = dCalcVectorDot3( triEdge1, triEdge1 );
  dReal fB0 = dCalcVectorDot3( kDiff, triEdge0 );
  dReal fB1 = dCalcVectorDot3( kDiff, triEdge1 );
  dReal fC = dCalcVectorDot3( kDiff, kDiff );
  dReal fDet = dReal(fabs(fA00*fA11-fA01*fA01));
  dReal fS = fA01*fB1-fA11*fB0;
  dReal fT = fA01*fB0-fA00*fB1;
  dReal fSqrDist;

  if ( fS + fT <= fDet )
  {
    if ( fS < REAL(0.0) )
    {
      if ( fT < REAL(0.0) )  // region 4
      {
        if ( fB0 < REAL(0.0) )
        {
          fT = REAL(0.0);
          if ( -fB0 >= fA00 )
          {
            fS = REAL(1.0);
            fSqrDist = fA00+REAL(2.0)*fB0+fC;
          }
          else
          {
            fS = -fB0/fA00;
            fSqrDist = fB0*fS+fC;
          }
        }
        else
        {
          fS = REAL(0.0);
          if ( fB1 >= REAL(0.0) )
          {
            fT = REAL(0.0);
            fSqrDist = fC;
          }
          else if ( -fB1 >= fA11 )
          {
            fT = REAL(1.0);
            fSqrDist = fA11+REAL(2.0)*fB1+fC;
          }
          else
          {
            fT = -fB1/fA11;
            fSqrDist = fB1*fT+fC;
          }
        }
      }
      else  // region 3
      {
        fS = REAL(0.0);
        if ( fB1 >= REAL(0.0) )
        {
          fT = REAL(0.0);
          fSqrDist = fC;
        }
        else if ( -fB1 >= fA11 )
        {
          fT = REAL(1.0);
          fSqrDist = fA11+REAL(2.0)*fB1+fC;
        }
        else
        {
          fT = -fB1/fA11;
          fSqrDist = fB1*fT+fC;
        }
      }
    }
    else if ( fT < REAL(0.0) )  // region 5
    {
      fT = REAL(0.0);
      if ( fB0 >= REAL(0.0) )
      {
        fS = REAL(0.0);
        fSqrDist = fC;
      }
      else if ( -fB0 >= fA00 )
      {
        fS = REAL(1.0);
        fSqrDist = fA00+REAL(2.0)*fB0+fC;
      }
      else
      {
        fS = -fB0/fA00;
        fSqrDist = fB0*fS+fC;
      }
    }
    else  // region 0
    {
      // minimum at interior point
      if (_dequal(fDet, REAL(0.0)))
      {
        fS = REAL(0.0);
        fT = REAL(0.0);
        fSqrDist = dInfinity;
      } 
      else
      {
        dReal fInvDet = REAL(1.0)/fDet;
        fS *= fInvDet;
        fT *= fInvDet;
        fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) +
                   fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC;
      }
    }
  }
  else
  {
    dReal fTmp0, fTmp1, fNumer, fDenom;

    if ( fS < REAL(0.0) )  // region 2
    {
      fTmp0 = fA01 + fB0;
      fTmp1 = fA11 + fB1;
      if ( fTmp1 > fTmp0 )
      {
        fNumer = fTmp1 - fTmp0;
        fDenom = fA00-REAL(2.0)*fA01+fA11;
        if ( fNumer >= fDenom )
        {
          fS = REAL(1.0);
          fT = REAL(0.0);
          fSqrDist = fA00+REAL(2.0)*fB0+fC;
        }
        else
        {
          fS = fNumer/fDenom;
          fT = REAL(1.0) - fS;
          fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) +
                     fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC;
        }
      }
      else
      {
        fS = REAL(0.0);
        if ( fTmp1 <= REAL(0.0) )
        {
          fT = REAL(1.0);
          fSqrDist = fA11+REAL(2.0)*fB1+fC;
        }
        else if ( fB1 >= REAL(0.0) )
        {
          fT = REAL(0.0);
          fSqrDist = fC;
        }
        else
        {
          fT = -fB1/fA11;
          fSqrDist = fB1*fT+fC;
        }
      }
    }
    else if ( fT < REAL(0.0) )  // region 6
    {
      fTmp0 = fA01 + fB1;
      fTmp1 = fA00 + fB0;
      if ( fTmp1 > fTmp0 )
      {
        fNumer = fTmp1 - fTmp0;
        fDenom = fA00-REAL(2.0)*fA01+fA11;
        if ( fNumer >= fDenom )
        {
          fT = REAL(1.0);
          fS = REAL(0.0);
          fSqrDist = fA11+REAL(2.0)*fB1+fC;
        }
        else
        {
          fT = fNumer/fDenom;
          fS = REAL(1.0) - fT;
          fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) +
                     fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC;
        }
      }
      else
      {
        fT = REAL(0.0);
        if ( fTmp1 <= REAL(0.0) )
        {
          fS = REAL(1.0);
          fSqrDist = fA00+REAL(2.0)*fB0+fC;
        }
        else if ( fB0 >= REAL(0.0) )
        {
          fS = REAL(0.0);
          fSqrDist = fC;
        }
        else
        {
          fS = -fB0/fA00;
          fSqrDist = fB0*fS+fC;
        }
      }
    }
    else  // region 1
    {
      fNumer = fA11 + fB1 - fA01 - fB0;
      if ( fNumer <= REAL(0.0) )
      {
        fS = REAL(0.0);
        fT = REAL(1.0);
        fSqrDist = fA11+REAL(2.0)*fB1+fC;
      }
      else
      {
        fDenom = fA00-REAL(2.0)*fA01+fA11;
        if ( fNumer >= fDenom )
        {
          fS = REAL(1.0);
          fT = REAL(0.0);
          fSqrDist = fA00+REAL(2.0)*fB0+fC;
        }
        else
        {
          fS = fNumer/fDenom;
          fT = REAL(1.0) - fS;
          fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) +
                     fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC;
        }
      }
    }
  }

  if ( pfSParam )
      *pfSParam = (float)fS;

  if ( pfTParam )
      *pfTParam = (float)fT;

  return dReal(fabs(fSqrDist));
}

//------------------------------------------------------------------------------
/**
  @brief Finds the shortest distance squared between two line segments.
  @param pfSegP0  t value for seg1 where the shortest distance between
                  the segments exists.
  @param pfSegP0  t value for seg2 where the shortest distance between
                  the segments exists.
  @return Shortest distance squared.
  
  Taken from:
  Magic Software, Inc.
  http://www.magic-software.com
*/
dReal SqrDistanceSegments( const dVector3 seg1Origin, const dVector3 seg1Direction, 
                           const dVector3 seg2Origin, const dVector3 seg2Direction,
                           dReal* pfSegP0, dReal* pfSegP1 )
{
  const dReal gs_fTolerance = 1e-05f;
  dVector3 kDiff, kNegDiff, seg1NegDirection;
  Vector3Subtract( seg1Origin, seg2Origin, kDiff );
  Vector3Negate( kDiff, kNegDiff );
  dReal fA00 = dCalcVectorDot3( seg1Direction, seg1Direction );
  Vector3Negate( seg1Direction, seg1NegDirection );
  dReal fA01 = dCalcVectorDot3( seg1NegDirection, seg2Direction );
  dReal fA11 = dCalcVectorDot3( seg2Direction, seg2Direction );
  dReal fB0 = dCalcVectorDot3( kDiff, seg1Direction );
  dReal fC = dCalcVectorDot3( kDiff, kDiff );
  dReal fDet = dReal(fabs(fA00*fA11-fA01*fA01));
  dReal fB1, fS, fT, fSqrDist, fTmp;

  if ( fDet >= gs_fTolerance )
  {
    // line segments are not parallel
    fB1 = dCalcVectorDot3( kNegDiff, seg2Direction );
    fS = fA01*fB1-fA11*fB0;
    fT = fA01*fB0-fA00*fB1;
        
    if ( fS >= REAL(0.0) )
    {
      if ( fS <= fDet )
      {
        if ( fT >= REAL(0.0) )
        {
          if ( fT <= fDet )  // region 0 (interior)
          {
            // minimum at two interior points of 3D lines
            dReal fInvDet = REAL(1.0)/fDet;
            fS *= fInvDet;
            fT *= fInvDet;
            fSqrDist = fS*(fA00*fS+fA01*fT+REAL(2.0)*fB0) +
                       fT*(fA01*fS+fA11*fT+REAL(2.0)*fB1)+fC;
          }
          else  // region 3 (side)
          {
            fT = REAL(1.0);
            fTmp = fA01+fB0;
            if ( fTmp >= REAL(0.0) )
            {
              fS = REAL(0.0);
              fSqrDist = fA11+REAL(2.0)*fB1+fC;
            }
            else if ( -fTmp >= fA00 )
            {
              fS = REAL(1.0);
              fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB1+fTmp);
            }
            else
            {
              fS = -fTmp/fA00;
              fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC;
            }
          }
        }
        else  // region 7 (side)
        {
          fT = REAL(0.0);
          if ( fB0 >= REAL(0.0) )
          {
            fS = REAL(0.0);
            fSqrDist = fC;
          }
          else if ( -fB0 >= fA00 )
          {
            fS = REAL(1.0);
            fSqrDist = fA00+REAL(2.0)*fB0+fC;
          }
          else
          {
            fS = -fB0/fA00;
            fSqrDist = fB0*fS+fC;
          }
        }
      }
      else
      {
        if ( fT >= REAL(0.0) )
        {
          if ( fT <= fDet )  // region 1 (side)
          {
            fS = REAL(1.0);
            fTmp = fA01+fB1;
            if ( fTmp >= REAL(0.0) )
            {
              fT = REAL(0.0);
              fSqrDist = fA00+REAL(2.0)*fB0+fC;
            }
            else if ( -fTmp >= fA11 )
            {
              fT = REAL(1.0);
              fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp);
            }
            else
            {
              fT = -fTmp/fA11;
              fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC;
            }
          }
          else  // region 2 (corner)
          {
            fTmp = fA01+fB0;
            if ( -fTmp <= fA00 )
            {
              fT = REAL(1.0);
              if ( fTmp >= REAL(0.0) )
              {
                fS = REAL(0.0);
                fSqrDist = fA11+REAL(2.0)*fB1+fC;
              }
              else
              {
                fS = -fTmp/fA00;
                fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC;
              }
            }
            else
            {
              fS = REAL(1.0);
              fTmp = fA01+fB1;
              if ( fTmp >= REAL(0.0) )
              {
                fT = REAL(0.0);
                fSqrDist = fA00+REAL(2.0)*fB0+fC;
              }
              else if ( -fTmp >= fA11 )
              {
                fT = REAL(1.0);
                fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp);
              }
              else
              {
                fT = -fTmp/fA11;
                fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC;
              }
            }
          }
        }
        else  // region 8 (corner)
        {
          if ( -fB0 < fA00 )
          { 
            fT = REAL(0.0);
            if ( fB0 >= REAL(0.0) )
            {
              fS = REAL(0.0);
              fSqrDist = fC;
            }
            else
            {
              fS = -fB0/fA00;
              fSqrDist = fB0*fS+fC;
            }
          }
          else
          {
            fS = REAL(1.0);
            fTmp = fA01+fB1;
            if ( fTmp >= REAL(0.0) )
            {
              fT = REAL(0.0);
              fSqrDist = fA00+REAL(2.0)*fB0+fC;
            }
            else if ( -fTmp >= fA11 )
            {
              fT = REAL(1.0);
              fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB0+fTmp);
            }
            else
            {
              fT = -fTmp/fA11;
              fSqrDist = fTmp*fT+fA00+REAL(2.0)*fB0+fC;
            }
          }
        }
      }
    }
    else 
    {
      if ( fT >= REAL(0.0) )
      {
        if ( fT <= fDet )  // region 5 (side)
        {
          fS = REAL(0.0);
          if ( fB1 >= REAL(0.0) )
          {
            fT = REAL(0.0);
            fSqrDist = fC;
          }
          else if ( -fB1 >= fA11 )
          {
            fT = REAL(1.0);
            fSqrDist = fA11+REAL(2.0)*fB1+fC;
          }
          else
          {
            fT = -fB1/fA11;
            fSqrDist = fB1*fT+fC;
          }
        }
        else  // region 4 (corner)
        {
          fTmp = fA01+fB0;
          if ( fTmp < REAL(0.0) )
          {
            fT = REAL(1.0);
            if ( -fTmp >= fA00 )
            {
              fS = REAL(1.0);
              fSqrDist = fA00+fA11+fC+REAL(2.0)*(fB1+fTmp);
            }
            else
            {
              fS = -fTmp/fA00;
              fSqrDist = fTmp*fS+fA11+REAL(2.0)*fB1+fC;
            }
          }
          else
          {
            fS = REAL(0.0);
            if ( fB1 >= REAL(0.0) )
            {
              fT = REAL(0.0);
              fSqrDist = fC;
            }
            else if ( -fB1 >= fA11 )
            {
              fT = REAL(1.0);
              fSqrDist = fA11+REAL(2.0)*fB1+fC;
            }
            else
            {
              fT = -fB1/fA11;
              fSqrDist = fB1*fT+fC;
            }
          }
        }
      }
      else   // region 6 (corner)
      {
        if ( fB0 < REAL(0.0) )
        {
          fT = REAL(0.0);
          if ( -fB0 >= fA00 )
          {
            fS = REAL(1.0);
            fSqrDist = fA00+REAL(2.0)*fB0+fC;
          }
          else
          {
            fS = -fB0/fA00;
            fSqrDist = fB0*fS+fC;
          }
        }
        else
        {
          fS = REAL(0.0);
          if ( fB1 >= REAL(0.0) )
          {
            fT = REAL(0.0);
            fSqrDist = fC;
          }
          else if ( -fB1 >= fA11 )
          {
            fT = REAL(1.0);
            fSqrDist = fA11+REAL(2.0)*fB1+fC;
          }
          else
          {
            fT = -fB1/fA11;
            fSqrDist = fB1*fT+fC;
          }
        }
      }
    }
  }
  else
  {
    // line segments are parallel
    if ( fA01 > REAL(0.0) )
    {
      // direction vectors form an obtuse angle
      if ( fB0 >= REAL(0.0) )
      {
        fS = REAL(0.0);
        fT = REAL(0.0);
        fSqrDist = fC;
      }
      else if ( -fB0 <= fA00 )
      {
        fS = -fB0/fA00;
        fT = REAL(0.0);
        fSqrDist = fB0*fS+fC;
      }
      else
      {
        //fB1 = -kDiff % seg2.m;
        fB1 = dCalcVectorDot3( kNegDiff, seg2Direction );
        fS = REAL(1.0);
        fTmp = fA00+fB0;
        if ( -fTmp >= fA01 )
        {
          fT = REAL(1.0);
          fSqrDist = fA00+fA11+fC+REAL(2.0)*(fA01+fB0+fB1);
        }
        else
        {
          fT = -fTmp/fA01;
          fSqrDist = fA00+REAL(2.0)*fB0+fC+fT*(fA11*fT+REAL(2.0)*(fA01+fB1));
        }
      }
    }
    else
    {
      // direction vectors form an acute angle
      if ( -fB0 >= fA00 )
      {
        fS = REAL(1.0);
        fT = REAL(0.0);
        fSqrDist = fA00+REAL(2.0)*fB0+fC;
      }
      else if ( fB0 <= REAL(0.0) )
      {
        fS = -fB0/fA00;
        fT = REAL(0.0);
        fSqrDist = fB0*fS+fC;
      }
      else
      {
        fB1 = dCalcVectorDot3( kNegDiff, seg2Direction );
        fS = REAL(0.0);
        if ( fB0 >= -fA01 )
        {
          fT = REAL(1.0);
          fSqrDist = fA11+REAL(2.0)*fB1+fC;
        }
        else
        {
          fT = -fB0/fA01;
          fSqrDist = fC+fT*(REAL(2.0)*fB1+fA11*fT);
        }
      }
    }
  }
    
  if ( pfSegP0 )
    *pfSegP0 = fS;
  
  if ( pfSegP1 )
    *pfSegP1 = fT;
    
  return dReal(fabs(fSqrDist));
}

//------------------------------------------------------------------------------
/**
  @brief Finds the shortest distance squared between a line segment and 
         a triangle.
         
  @param pfSegP   t value for the line segment where the shortest distance between
                  the segment and the triangle occurs.
                  So the point along the segment that is the shortest distance
                  away from the triangle can be obtained by (seg.end - seg.start) * t.
  @param pfTriP0  Barycentric coordinate of triangle at point closest to seg (u)
  @param pfTriP1  Barycentric coordinate of triangle at point closest to seg (v)
  @return Shortest distance squared.
  
  The third Barycentric coordinate is implicit, ie. w = 1.0 - u - v
         
  Taken from:
  Magic Software, Inc.
  http://www.magic-software.com
*/
dReal SqrDistanceSegTri( const dVector3 segOrigin, const dVector3 segEnd, 
                         const dVector3 triOrigin, 
                         const dVector3 triEdge0, const dVector3 triEdge1,
                         dReal* pfSegP, dReal* pfTriP0, dReal* pfTriP1 )
{
  const dReal gs_fTolerance = 1e-06f;
  dVector3 segDirection, segNegDirection, kDiff, kNegDiff;
  Vector3Subtract( segEnd, segOrigin, segDirection );
  Vector3Negate( segDirection, segNegDirection );
  Vector3Subtract( triOrigin, segOrigin, kDiff );
  Vector3Negate( kDiff, kNegDiff );
  dReal fA00 = dCalcVectorDot3( segDirection, segDirection );
  dReal fA01 = dCalcVectorDot3( segNegDirection, triEdge0 );
  dReal fA02 = dCalcVectorDot3( segNegDirection, triEdge1 );
  dReal fA11 = dCalcVectorDot3( triEdge0, triEdge0 );
  dReal fA12 = dCalcVectorDot3( triEdge0, triEdge1 );
  dReal fA22 = dCalcVectorDot3( triEdge1, triEdge1 );
  dReal fB0  = dCalcVectorDot3( kNegDiff, segDirection );
  dReal fB1  = dCalcVectorDot3( kDiff, triEdge0 );
  dReal fB2  = dCalcVectorDot3( kDiff, triEdge1 );

  dVector3 kTriSegOrigin, kTriSegDirection, kPt;
  dReal fSqrDist, fSqrDist0, fR, fS, fT, fR0, fS0, fT0;

  // Set up for a relative error test on the angle between ray direction
  // and triangle normal to determine parallel/nonparallel status.
  dVector3 kN;
  dCalcVectorCross3( kN, triEdge0, triEdge1 );
  dReal fNSqrLen = dCalcVectorDot3( kN, kN );
  dReal fDot = dCalcVectorDot3( segDirection, kN );
  bool bNotParallel = (fDot*fDot >= gs_fTolerance*fA00*fNSqrLen);

  if ( bNotParallel )
  {
    dReal fCof00 = fA11*fA22-fA12*fA12;
    dReal fCof01 = fA02*fA12-fA01*fA22;
    dReal fCof02 = fA01*fA12-fA02*fA11;
    dReal fCof11 = fA00*fA22-fA02*fA02;
    dReal fCof12 = fA02*fA01-fA00*fA12;
    dReal fCof22 = fA00*fA11-fA01*fA01;
    dReal fInvDet = REAL(1.0)/(fA00*fCof00+fA01*fCof01+fA02*fCof02);
    dReal fRhs0 = -fB0*fInvDet;
    dReal fRhs1 = -fB1*fInvDet;
    dReal fRhs2 = -fB2*fInvDet;

    fR = fCof00*fRhs0+fCof01*fRhs1+fCof02*fRhs2;
    fS = fCof01*fRhs0+fCof11*fRhs1+fCof12*fRhs2;
    fT = fCof02*fRhs0+fCof12*fRhs1+fCof22*fRhs2;

    if ( fR < REAL(0.0) )
    {
      if ( fS+fT <= REAL(1.0) )
      {
        if ( fS < REAL(0.0) )
        {
          if ( fT < REAL(0.0) )  // region 4m
          {
            // min on face s=0 or t=0 or r=0
            Vector3Copy( triOrigin, kTriSegOrigin );
            Vector3Copy( triEdge1, kTriSegDirection );
            fSqrDist = SqrDistanceSegments( segOrigin, segDirection, 
                                            kTriSegOrigin, kTriSegDirection, 
                                            &fR, &fT );
            fS = REAL(0.0);
            Vector3Copy( triOrigin, kTriSegOrigin );
            Vector3Copy( triEdge0, kTriSegDirection );
            fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, 
                                             kTriSegOrigin, kTriSegDirection, 
                                             &fR0, &fS0 );
            fT0 = REAL(0.0);
            if ( fSqrDist0 < fSqrDist )
            {
              fSqrDist = fSqrDist0;
              fR = fR0;
              fS = fS0;
              fT = fT0;
            }
            fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, 
                                             &fS0, &fT0 );
            fR0 = REAL(0.0);
            if ( fSqrDist0 < fSqrDist )
            {
              fSqrDist = fSqrDist0;
              fR = fR0;
              fS = fS0;
              fT = fT0;
            }
          }
          else  // region 3m
          {
            // min on face s=0 or r=0
            Vector3Copy( triOrigin, kTriSegOrigin );
            Vector3Copy( triEdge1, kTriSegDirection );
            fSqrDist = SqrDistanceSegments( segOrigin, segDirection, 
                                            kTriSegOrigin, kTriSegDirection,
                                            &fR,&fT );
            fS = REAL(0.0);
            fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
                                             &fS0, &fT0 );
            fR0 = REAL(0.0);
            if ( fSqrDist0 < fSqrDist )
            {
              fSqrDist = fSqrDist0;
              fR = fR0;
              fS = fS0;
              fT = fT0;
            }
          }
        }
        else if ( fT < REAL(0.0) )  // region 5m
        {
          // min on face t=0 or r=0
          Vector3Copy( triOrigin, kTriSegOrigin );
          Vector3Copy( triEdge0, kTriSegDirection );
          fSqrDist = SqrDistanceSegments( segOrigin, segDirection, 
                                          kTriSegOrigin, kTriSegDirection, 
                                          &fR, &fS );
          fT = REAL(0.0);
          fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
                                           &fS0, &fT0 );
          fR0 = REAL(0.0);
          if ( fSqrDist0 < fSqrDist )
          {
            fSqrDist = fSqrDist0;
            fR = fR0;
            fS = fS0;
            fT = fT0;
          }
        }
        else  // region 0m
        {
          // min on face r=0
          fSqrDist = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, 
                                          &fS, &fT );
          fR = REAL(0.0);
        }
      }
      else
      {
        if ( fS < REAL(0.0) )  // region 2m
        {
          // min on face s=0 or s+t=1 or r=0
          Vector3Copy( triOrigin, kTriSegOrigin );
          Vector3Copy( triEdge1, kTriSegDirection );
          fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
                                          kTriSegOrigin, kTriSegDirection,
                                          &fR, &fT );
          fS = REAL(0.0);
          Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
          Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
          fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, 
                                           kTriSegOrigin, kTriSegDirection,
                                           &fR0, &fT0 );
          fS0 = REAL(1.0) - fT0;
          if ( fSqrDist0 < fSqrDist )
          {
            fSqrDist = fSqrDist0;
            fR = fR0;
            fS = fS0;
            fT = fT0;
          }
          fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
                                           &fS0, &fT0 );
          fR0 = REAL(0.0);
          if ( fSqrDist0 < fSqrDist )
          {
            fSqrDist = fSqrDist0;
            fR = fR0;
            fS = fS0;
            fT = fT0;
          }
        }
        else if ( fT < REAL(0.0) )  // region 6m
        {
          // min on face t=0 or s+t=1 or r=0
          Vector3Copy( triOrigin, kTriSegOrigin );
          Vector3Copy( triEdge0, kTriSegDirection );
          fSqrDist = SqrDistanceSegments( segOrigin, segDirection, 
                                          kTriSegOrigin, kTriSegDirection,
                                          &fR, &fS );
          fT = REAL(0.0);
          Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
          Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
          fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
                                           kTriSegOrigin, kTriSegDirection,
                                           &fR0, &fT0 );
          fS0 = REAL(1.0) - fT0;
          if ( fSqrDist0 < fSqrDist )
          {
            fSqrDist = fSqrDist0;
            fR = fR0;
            fS = fS0;
            fT = fT0;
          }
          fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
                                           &fS0, &fT0 );
          fR0 = REAL(0.0);
          if ( fSqrDist0 < fSqrDist )
          {
            fSqrDist = fSqrDist0;
            fR = fR0;
            fS = fS0;
            fT = fT0;
          }
        }
        else  // region 1m
        {
          // min on face s+t=1 or r=0
          Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
          Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
          fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
                                          kTriSegOrigin, kTriSegDirection,
                                          &fR, &fT );
          fS = REAL(1.0) - fT;
          fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1,
                                           &fS0, &fT0 );
          fR0 = REAL(0.0);
          if ( fSqrDist0 < fSqrDist )
          {
            fSqrDist = fSqrDist0;
            fR = fR0;
            fS = fS0;
            fT = fT0;
          }
        }
      }
    }
    else if ( fR <= REAL(1.0) )
    {
      if ( fS+fT <= REAL(1.0) )
      {
        if ( fS < REAL(0.0) )
        {
          if ( fT < REAL(0.0) )  // region 4
          {
            // min on face s=0 or t=0
            Vector3Copy( triOrigin, kTriSegOrigin );
            Vector3Copy( triEdge1, kTriSegDirection );
            fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
                                            kTriSegOrigin, kTriSegDirection,
                                            &fR, &fT );
            fS = REAL(0.0);
            Vector3Copy( triOrigin, kTriSegOrigin );
            Vector3Copy( triEdge0, kTriSegDirection );
            fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection, 
                                             kTriSegOrigin, kTriSegDirection,
                                             &fR0, &fS0 );
            fT0 = REAL(0.0);
            if ( fSqrDist0 < fSqrDist )
            {
              fSqrDist = fSqrDist0;
              fR = fR0;
              fS = fS0;
              fT = fT0;
            }
          }
          else  // region 3
          {
            // min on face s=0
            Vector3Copy( triOrigin, kTriSegOrigin );
            Vector3Copy( triEdge1, kTriSegDirection );
            fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
                                            kTriSegOrigin, kTriSegDirection,
                                            &fR, &fT );
            fS = REAL(0.0);
          }
        }
        else if ( fT < REAL(0.0) )  // region 5
        {
          // min on face t=0
          Vector3Copy( triOrigin, kTriSegOrigin );
          Vector3Copy( triEdge0, kTriSegDirection );
          fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
                                          kTriSegOrigin, kTriSegDirection,
                                          &fR, &fS );
          fT = REAL(0.0);
        }
        else  // region 0
        {
          // global minimum is interior, done
          fSqrDist = REAL(0.0);
        }
      }
      else
      {
        if ( fS < REAL(0.0) )  // region 2
        {
          // min on face s=0 or s+t=1
          Vector3Copy( triOrigin, kTriSegOrigin );
          Vector3Copy( triEdge1, kTriSegDirection );
          fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
                                          kTriSegOrigin, kTriSegDirection,
                                          &fR, &fT );
          fS = REAL(0.0);
          Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
          Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
          fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
                                           kTriSegOrigin, kTriSegDirection,
                                           &fR0, &fT0 );
          fS0 = REAL(1.0) - fT0;
          if ( fSqrDist0 < fSqrDist )
          {
            fSqrDist = fSqrDist0;
            fR = fR0;
            fS = fS0;
            fT = fT0;
          }
        }
        else if ( fT < REAL(0.0) )  // region 6
        {
          // min on face t=0 or s+t=1
          Vector3Copy( triOrigin, kTriSegOrigin );
          Vector3Copy( triEdge0, kTriSegDirection );
          fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
                                          kTriSegOrigin, kTriSegDirection,
                                          &fR, &fS );
          fT = REAL(0.0);
          Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
          Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
          fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
                                           kTriSegOrigin, kTriSegDirection,
                                           &fR0, &fT0 );
          fS0 = REAL(1.0) - fT0;
          if ( fSqrDist0 < fSqrDist )
          {
            fSqrDist = fSqrDist0;
            fR = fR0;
            fS = fS0;
            fT = fT0;
          }
        }
        else  // region 1
        {
          // min on face s+t=1
          Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
          Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
          fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
                                          kTriSegOrigin, kTriSegDirection,
                                          &fR, &fT );
          fS = REAL(1.0) - fT;
        }
      }
    }
    else  // fR > 1
    {
      if ( fS+fT <= REAL(1.0) )
      {
        if ( fS < REAL(0.0) )
        {
          if ( fT < REAL(0.0) )  // region 4p
          {
            // min on face s=0 or t=0 or r=1
            Vector3Copy( triOrigin, kTriSegOrigin );
            Vector3Copy( triEdge1, kTriSegDirection );
            fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
                                            kTriSegOrigin, kTriSegDirection,
                                            &fR, &fT );
            fS = REAL(0.0);
            Vector3Copy( triOrigin, kTriSegOrigin );
            Vector3Copy( triEdge0, kTriSegDirection );
            fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
                                             kTriSegOrigin, kTriSegDirection,
                                             &fR0, &fS0 );
            fT0 = REAL(0.0);
            if ( fSqrDist0 < fSqrDist )
            {
              fSqrDist = fSqrDist0;
              fR = fR0;
              fS = fS0;
              fT = fT0;
            }
            Vector3Add( segOrigin, segDirection, kPt );
            fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
                                             &fS0, &fT0 );
            fR0 = REAL(1.0);
            if ( fSqrDist0 < fSqrDist )
            {
              fSqrDist = fSqrDist0;
              fR = fR0;
              fS = fS0;
              fT = fT0;
            }
          }
          else  // region 3p
          {
            // min on face s=0 or r=1
            Vector3Copy( triOrigin, kTriSegOrigin );
            Vector3Copy( triEdge1, kTriSegDirection );
            fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
                                            kTriSegOrigin, kTriSegDirection,
                                            &fR, &fT );
            fS = REAL(0.0);
            Vector3Add( segOrigin, segDirection, kPt );
            fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
                                             &fS0, &fT0 );
            fR0 = REAL(1.0);
            if ( fSqrDist0 < fSqrDist )
            {
              fSqrDist = fSqrDist0;
              fR = fR0;
              fS = fS0;
              fT = fT0;
            }
          }
        }
        else if ( fT < REAL(0.0) )  // region 5p
        {
          // min on face t=0 or r=1
          Vector3Copy( triOrigin, kTriSegOrigin );
          Vector3Copy( triEdge0, kTriSegDirection );
          fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
                                          kTriSegOrigin, kTriSegDirection,
                                          &fR, &fS );
          fT = REAL(0.0);
          Vector3Add( segOrigin, segDirection, kPt );
          fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
                                           &fS0, &fT0 );
          fR0 = REAL(1.0);
          if ( fSqrDist0 < fSqrDist )
          {
            fSqrDist = fSqrDist0;
            fR = fR0;
            fS = fS0;
            fT = fT0;
          }
        }
        else  // region 0p
        {
          // min face on r=1
          Vector3Add( segOrigin, segDirection, kPt );
          fSqrDist = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
                                          &fS, &fT );
          fR = REAL(1.0);
        }
      }
      else
      {
        if ( fS < REAL(0.0) )  // region 2p
        {
          // min on face s=0 or s+t=1 or r=1
          Vector3Copy( triOrigin, kTriSegOrigin );
          Vector3Copy( triEdge1, kTriSegDirection );
          fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
                                          kTriSegOrigin, kTriSegDirection,
                                          &fR, &fT );
          fS = REAL(0.0);
          Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
          Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
          fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
                                           kTriSegOrigin, kTriSegDirection,
                                           &fR0, &fT0 );
          fS0 = REAL(1.0) - fT0;
          if ( fSqrDist0 < fSqrDist )
          {
            fSqrDist = fSqrDist0;
            fR = fR0;
            fS = fS0;
            fT = fT0;
          }
          Vector3Add( segOrigin, segDirection, kPt );
          fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
                                           &fS0, &fT0 );
          fR0 = REAL(1.0);
          if ( fSqrDist0 < fSqrDist )
          {
            fSqrDist = fSqrDist0;
            fR = fR0;
            fS = fS0;
            fT = fT0;
          }
        }
        else if ( fT < REAL(0.0) )  // region 6p
        {
          // min on face t=0 or s+t=1 or r=1
          Vector3Copy( triOrigin, kTriSegOrigin );
          Vector3Copy( triEdge0, kTriSegDirection );
          fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
                                          kTriSegOrigin, kTriSegDirection,
                                          &fR, &fS );
          fT = REAL(0.0);
          Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
          Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
          fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
                                           kTriSegOrigin, kTriSegDirection,
                                           &fR0, &fT0 );
          fS0 = REAL(1.0) - fT0;
          if ( fSqrDist0 < fSqrDist )
          {
            fSqrDist = fSqrDist0;
            fR = fR0;
            fS = fS0;
            fT = fT0;
          }
          Vector3Add( segOrigin, segDirection, kPt );
          fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1,
                                           &fS0, &fT0 );
          fR0 = REAL(1.0);
          if ( fSqrDist0 < fSqrDist )
          {
            fSqrDist = fSqrDist0;
            fR = fR0;
            fS = fS0;
            fT = fT0;
          }
        }
        else  // region 1p
        {
          // min on face s+t=1 or r=1
          Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
          Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
          fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
                                          kTriSegOrigin, kTriSegDirection,
                                          &fR, &fT );
          fS = REAL(1.0) - fT;
          Vector3Add( segOrigin, segDirection, kPt );
          fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, 
                                           &fS0, &fT0 );
          fR0 = REAL(1.0);
          if ( fSqrDist0 < fSqrDist )
          {
            fSqrDist = fSqrDist0;
            fR = fR0;
            fS = fS0;
            fT = fT0;
          }
        }
      }
    }
  }
  else
  {
    // segment and triangle are parallel
    Vector3Copy( triOrigin, kTriSegOrigin );
    Vector3Copy( triEdge0, kTriSegDirection );
    fSqrDist = SqrDistanceSegments( segOrigin, segDirection,
                                    kTriSegOrigin, kTriSegDirection, &fR, &fS );
    fT = REAL(0.0);

    Vector3Copy( triEdge1, kTriSegDirection );
    fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
                                     kTriSegOrigin, kTriSegDirection,
                                     &fR0, &fT0 );
    fS0 = REAL(0.0);
    if ( fSqrDist0 < fSqrDist )
    {
      fSqrDist = fSqrDist0;
      fR = fR0;
      fS = fS0;
      fT = fT0;
    }

    Vector3Add( triOrigin, triEdge0, kTriSegOrigin );
    Vector3Subtract( triEdge1, triEdge0, kTriSegDirection );
    fSqrDist0 = SqrDistanceSegments( segOrigin, segDirection,
                                     kTriSegOrigin, kTriSegDirection, &fR0, &fT0 );
    fS0 = REAL(1.0) - fT0;
    if ( fSqrDist0 < fSqrDist )
    {
      fSqrDist = fSqrDist0;
      fR = fR0;
      fS = fS0;
      fT = fT0;
    }

    fSqrDist0 = SqrDistancePointTri( segOrigin, triOrigin, triEdge0, triEdge1, 
                                     &fS0, &fT0 );
    fR0 = REAL(0.0);
    if ( fSqrDist0 < fSqrDist )
    {
      fSqrDist = fSqrDist0;
      fR = fR0;
      fS = fS0;
      fT = fT0;
    }

    Vector3Add( segOrigin, segDirection, kPt );
    fSqrDist0 = SqrDistancePointTri( kPt, triOrigin, triEdge0, triEdge1, 
                                     &fS0, &fT0 );
    fR0 = REAL(1.0);
    if ( fSqrDist0 < fSqrDist )
    {
      fSqrDist = fSqrDist0;
      fR = fR0;
      fS = fS0;
      fT = fT0;
    }
  }

  if ( pfSegP )
    *pfSegP = fR;

  if ( pfTriP0 )
    *pfTriP0 = fS;

  if ( pfTriP1 )
    *pfTriP1 = fT;

  return fSqrDist;
}
