TAPsCGMath.cpp

Go to the documentation of this file.

/******************************************************************************
TAPsCGMath.cpp
******************************************************************************/
/******************************************************************************
SUKITTI PUNAK   (07/27/2005)
UPDATE          (10/26/2010)
******************************************************************************/
#include "TAPsCGMath.hpp"
// Using Inclusion Model (i.e. definitions are included in declarations)
//                       (this name.cpp is included in name.hpp)
// Each friend is defined directly inside its declaration.

BEGIN_NAMESPACE_TAPs
//=============================================================================
// Constant Values
//=============================================================================
template <typename T> const Vector3<T> CGMath<T>::ZeroVector( 0, 0, 0 );
template <typename T> const Vector3<T> CGMath<T>::VectorX( 1, 0, 0 );
template <typename T> const Vector3<T> CGMath<T>::VectorY( 0, 1, 0 );
template <typename T> const Vector3<T> CGMath<T>::VectorZ( 0, 0, 1 );
template <typename T> const Matrix3x3<T> CGMath<T>::IdentityMatrix3x3( 1, 0, 0, 0, 1, 0, 0, 0, 1 );
template <typename T> const Matrix4x4<T> CGMath<T>::IdentityMatrix4x4( 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 );
//
template <typename T> Vector3<T>    CGMath<T>::TempVector3( 0, 0, 0 );
template <typename T> Vector3<T>    CGMath<T>::TempVector4( 0, 0, 0, 1 );
template <typename T> Matrix3x3<T>  CGMath<T>::TempMatrix3x3( 1, 0, 0, 0, 1, 0, 0, 0, 1 );
template <typename T> Matrix4x4<T>  CGMath<T>::TempMatrix4x4( 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 );
//-----------------------------------------------------------------------------
#ifndef _WINDOWS_
#endif
//-----------------------------------------------------------------------------

//=========================================================================
// Creating Matrices for transformations
//-------------------------------------------------------------------------
template <typename T>
Matrix4x4<T> CGMath<T>::MatrixTranslation ( Vector3<T> const & translation )
{
    return Matrix4x4<T>(
        1, 0, 0, translation[0],
        0, 1, 0, translation[1],
        0, 0, 1, translation[2],
        0, 0, 0, 1 );
}

template <typename T>
Matrix4x4<T> CGMath<T>::MatrixRotation ( Quaternion<T> const & orientation )
{
    return orientation.GetRotationMatrix4x4();
}

template <typename T>
Matrix4x4<T> CGMath<T>::MatrixScale ( Vector3<T> const & scale )
{
    return Matrix4x4<T>(
        scale[0],  0,      0,      0,
           0,   scale[1],  0,      0,
           0,      0,   scale[2],  0,
           0,      0,      0,      1 );
}
//-------------------------------------------------------------------------
//=========================================================================




//=============================================================================
// Normal Calculation Fns
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
// Normal Calculation Fns
//=============================================================================




//=============================================================================
// Transformation Fns
//-----------------------------------------------------------------------------
// FN:  CreateRotationMatrix3x3 ()
// DESC:    Find a 3x3 rotation matrix from given rotation axis and angle
// I/P:  (Vector3<T>)   rotationAxis        a rotation axis
// I/P:  (T)            rotationAngle       a rotation angle (in degrees)
// O/P:  (Matrix3x3<T>) rotationMat         a 3x3 rotation matrix
// From Mathematatics for 3D Game Programming & Computer Graphics (2nd ed)
// The 3x3 rotation matrix is
//               -                                               -
//              |    c+(1-c)Ax^2   (1-c)AxAy-sAz   (1-c)AxAz+sAy  |
// R_A(zeta) =  |  (1-c)AxAy+sAz     c+(1-c)Ay^2   (1-c)AyAz-sAx  |
//              |  (1-c)AxAz-sAy   (1-c)AyAz+sAx     c+(1-c)Az^2  |
//               -                                               -
template <typename T>
Matrix3x3<T> CGMath<T>::CreateRotationMatrix3x3 (
            Vector3<T> const &  rotationAxis,   // I/P: Rotation Axis
            T                   rotationAngle   // I/P: Rotation Angle (in degs)
)
{
    Vector3<T> A = rotationAxis.GetUnit();
    T rad = rotationAngle * Math<T>::DEG_TO_RAD;
    T c = cos( rad ), s = sin( rad );
    T c_1 = T(1.0) - c;
    T AxAy = A[0] * A[1];
    T AxAz = A[0] * A[2];
    T AyAz = A[1] * A[2];
    T sAx = s * A[0];
    T sAy = s * A[1];
    T sAz = s * A[2];
    //--------------------------------------------------------------------
    return Matrix3x3<T>( 
            c + c_1*A[0]*A[0],  c_1*AxAy - sAz,     c_1*AxAz + sAy,
            c_1*AxAy + sAz,     c + c_1*A[1]*A[1],  c_1*AyAz - sAx,
            c_1*AxAz - sAy,     c_1*AyAz + sAx,     c + c_1*A[2]*A[2] );
}
//-----------------------------------------------------------------------------
template <typename T>
Matrix4x4<T> CGMath<T>::CreateRotationMatrix4x4 (
            Vector3<T> const &  rotationAxis,   // I/P: Rotation Axis
            T                   rotationAngle   // I/P: Rotation Angle (in degs)
)
{
    Vector3<T> A = rotationAxis.GetUnit();
    T rad = rotationAngle * Math<T>::DEG_TO_RAD;
    T c = cos( rad ), s = sin( rad );
    T c_1 = T(1.0) - c;
    T AxAy = A[0] * A[1];
    T AxAz = A[0] * A[2];
    T AyAz = A[1] * A[2];
    T sAx = s * A[0];
    T sAy = s * A[1];
    T sAz = s * A[2];
    //--------------------------------------------------------------------
    return Matrix4x4<T>( 
            c + c_1*A[0]*A[0],  c_1*AxAy - sAz,     c_1*AxAz + sAy,     0,
            c_1*AxAy + sAz,     c + c_1*A[1]*A[1],  c_1*AyAz - sAx,     0,
            c_1*AxAz - sAy,     c_1*AyAz + sAx,     c + c_1*A[2]*A[2],  0,
            0,                  0,                  0,                  1
    );
}
//-----------------------------------------------------------------------------
// FN:  CreateRotationMatrix3x3FromVectorAtoVectorB ()
// DESC:    Find a 3x3 rotation matrix from given rotation axis and angle
// I/P:  (Vector3<T>)   A               starting (vector) point
// I/P:  (Vector3<T>)   B               ending (vector) point
// O/P:  (Matrix3x3<T>) rotationMat     a 3x3 rotation matrix
template <typename T>
Matrix3x3<T> CGMath<T>::CreateRotationMatrix3x3FromVectorAtoVectorB (
            Vector3<T> const &  startingPt,     // I/P: starting point
            Vector3<T> const &  endingPt        // I/P: ending point
)
{
    Vector3<T> rotationAxis( startingPt ^ endingPt );
    T rotationAngle = acos((startingPt*endingPt)/startingPt.Length()/endingPt.Length())
                        * Math<T>::RAD_TO_DEG;
    return CreateRotationMatrix3x3( rotationAxis, rotationAngle );
}
//-----------------------------------------------------------------------------
template <typename T>
Matrix4x4<T> CGMath<T>::CreateRotationMatrix4x4FromVectorAtoVectorB (
            Vector3<T> const &  startingPt,     // I/P: starting point
            Vector3<T> const &  endingPt        // I/P: ending point
)
{
    Vector3<T> rotationAxis( startingPt ^ endingPt );
    T rotationAngle = acos((startingPt*endingPt)/startingPt.Length()/endingPt.Length())
                        * Math<T>::RAD_TO_DEG;
    return CreateRotationMatrix4x4( rotationAxis, rotationAngle );
}
//-----------------------------------------------------------------------------
//=============================================================================

//=============================================================================
// Matrix Fns
//=============================================================================
//-----------------------------------------------------------------------------
// FN:  EigValsVecsOf3x3SymMatrix ()
// DESC:    Find eigen vectors of a 3x3 symmetric matrix.
//          symmetric matrix always has all real eigen values 
//          and its eigen vectors are orthogonal to one another.
//          It also implies the matrix has its inverse matrix.
// First solve for eigen values by using root finding
//   a*x^3 + b*x^2 + c*x + d = 0;
//   The formula is adapted from "Mathematical Handbook of Formulas and Tables"
// Then inverse matrices for finding eigen values
// I/P:  (Matrix3x3<T>) iM                      a 3x3 real symmetric matrix
// O/P:  (T)            lamda1, lamda2, lamda3  eigen values
// O/P:  (Vector3<T>)   v1, v2, v3              eigen vectors
template <typename T>
void CGMath<T>::EigValsVecsOf3x3SymMatrix (
            Matrix3x3<T> & iM,          // I/P: a 3x3 symmetric matrix
            T & l1,                     // O/P: first  eigen value
            T & l2,                     // O/P: second eigen value
            T & l3,                     // O/P: third  eigen value
            Vector3<T> & v1,            // O/P: first  eigen vector
            Vector3<T> & v2,            // O/P: second eigen vector
            Vector3<T> & v3             // O/P: third  eigen vector
)
{
    assert( iM.IsSymmetric() );
    //---------------------------------------------------------------
    // Find all three roots
    // Set up   a*x^3 + b*x^2 + c*x + d = 0
    //T a = 1;
    T b = -iM(0,0) - iM(1,1) - iM(2,2);
    T c =  iM(0,0)*iM(1,1) + iM(0,0)*iM(2,2) 
         - iM(0,1)*iM(0,1) + iM(1,1)*iM(2,2) 
         - iM(0,2)*iM(0,2) - iM(1,2)*iM(1,2);
    T d =  iM(0,2)*iM(0,2)*iM(1,1) 
         - iM(0,0)*iM(1,1)*iM(2,2) 
         - 2*iM(0,1)*iM(1,2)*iM(0,2) 
         + iM(0,1)*iM(0,1)*iM(2,2)
         + iM(0,0)*iM(1,2)*iM(1,2);
    //---------------------------------------------------------------
    // Calculate Discriminant
    //b /= a;  c /= a;  d /= a;
    T Q  = (T(3.0)*c - b*b) / T(9.0);
    T R  = (T(9.0)*b*c - T(27.0)*d - T(2.0)*b*b*b) / T(54.0);
    T Q3 = Q*Q*Q;
    T D  = Q3 + R*R;    // Discriminant
    //std::cout << "D: " << D << "\n";
    //---------------------------------------------------------------
    // All roots are real and unequal if D < 0.
    assert ( D < 0 );
    T zeta_3 = acos( R / sqrt(-Q3) ) / T(3.0);
    T sqrt2_Q = 2 * sqrt( -Q );
    T b_3 = b / T(3.0);
    l1 = sqrt2_Q * cos( zeta_3 ) - b_3;                         // root 1
    l2 = sqrt2_Q * cos( zeta_3 + T(2.0)/T(3.0)*Math<T>::PI ) - b_3; // root 2
    l3 = sqrt2_Q * cos( zeta_3 + T(4.0)/T(3.0)*Math<T>::PI ) - b_3; // root 3
    //---------------------------------------------------------------
    // Sorting the roots (eigen values) by their magnitude
    T temp;
    if ( fabs(l2) > fabs(l1) ) {            // l2 > l1
        temp = l1;  l1 = l2; l2 = temp;
        if ( fabs(l3) > fabs(l1) ) {        // l3 > l1
            temp = l1;  l1 = l3; l3 = temp;
        }
        else if ( fabs(l3) > fabs(l2) ) {   // l3 > l2
            temp = l2;  l2 = l3; l3 = temp;
        }
    }
    else {                                  // l1 > l2
        if ( fabs(l3) > fabs(l2) ) {
            temp = l2;  l2 = l3; l3 = temp; // l3 > l2
        }
    }
    assert( fabs(l1) >= fabs(l2) );
    assert( fabs(l2) >= fabs(l3) );
    //---------------------------------------------------------------
    // Find eigen vectors
    T a1,               b1 = iM(0,1),       c1 = iM(0,2);
    T a2 = iM(0,1),     b2,                 c2 = iM(1,2);
    T a3 = iM(0,2),     b3 = iM(1,2),       c3;
    T lamda[3] = { l1, l2, l3 };
    T divider;
    Vector3<T> v;
    for ( int i = 0; i < 3; ++i ) {
        a1 = iM(0,0) - lamda[i];
        b2 = iM(1,1) - lamda[i];
        c3 = iM(2,2) - lamda[i];
        divider = b2*a1 - b1*a2 - b3*a1 + b1*a3;
        
        //Matrix3x3<T> M( a1, b1, c1, a2, b2, c2, a3, b3, c3 );
        //assert( M.GetDeterminant() != 0 );

        // divider is zero when the determinant of the matrix is zero!
        //assert( divider != 0 );
        //----------------------------------------------------------------
        if ( divider != 0 ) {
        //if ( fabs(divider) > 1E-40 ) {
            v[0] = (b1*c2 - b2*c1 + b3*c1 - b1*c3) / divider;
            v[1] = (a2*c1 - a1*c2 - a3*c1 + a1*c3) / divider;
            v[2] = 1;
            //-------------------------------------------------------
            // make the vector a unit vector
            v.Normalized();
            //if ( divider == 0 )   {
            //  std::cout << divider << ": " << v << std::endl;
            //}
        }
        //----------------------------------------------------------------
        else {          // divider is near ZERO
            //assert( false );
            v.SetXYZ(0,0,0);
        }
        if      ( i == 0 )  v1 = v;     // 1st eigen vector
        else if ( i == 1 )  v2 = v;     // 2nd eigen vector
        else                v3 = v;     // 3rd eigen vector
    }
    //---------------------------------------------------------------
}
//-----------------------------------------------------------------------------
//=============================================================================

//=============================================================================
// Intersection Finding
//=============================================================================
//-----------------------------------------------------------------------------
//*****************************************************************************
// FindRatioOfPointOnLineThatIntersectPlane
// I/P: two vectors defining the line segment
// I/P: a point followed by a point on the plane and its normal
// O/P: the ratio of the point on the line that intersects the plane
//      the ratio is [0-1] from l2 thru l1
// O/P: the projected point on the line that intersects the plane
//*****************************************************************************
template <typename T>
void CGMath<T>::FindRatioOfPointOnLineThatIntersectPlane ( 
                    Vector3<T> const & l1, Vector3<T> const & l2, // I/P: a line
                    Vector3<T> const & a,   // I/P: a point on the plane
                    Vector3<T> const & n,   // I/P: the plane (unit) normal
                    T &            ratio,   // O/P: the ratio
                    Vector3<T> &  projPt,   // O/P: the projected point
                    T tolerance 
)
{
    Vector3<T> l2l1( l2 - l1 );
    ratio  = (n*(a-l1)) / (n*l2l1);
    projPt = l1 + ratio*l2l1;
}
//-----------------------------------------------------------------------------


//*****************************************************************************
// Find the closest distance between two line segments
//   First project p1 and p2 on the line Lq defined by q1 and q2.
//     Let call them pq1 and pq2 respectively.
//   Second project p2 on the plane defined by pq1 and the unit vector of Lq
//     which is the plane normal.
//     Let call the projected point pq3.
//   Third determine d3, which is the square length of p1-pq3.
//     If d3 is not zero, then the line segments are unparallel.
//        Then find the closest points between the line segments.
//          Let call the closest points ps and qs.
//        Then the shortest distance is |ps - qs|.
//     If d3 is zero, then the line segments are parallel.
//        Then find the closest points between the line segments.
//          In this case, there are four 
//        Then the shortest distance is |ps - qs|.
//*****************************************************************************
template <typename T>
void CGMath<T>::FindClosestDistanceBetweenTwoLineSegments ( 
                    Vector3<T> const & p1, Vector3<T> const & p2, // I/P: a line
                    Vector3<T> const & q1, Vector3<T> const & q2, // I/P: a line
                    T & distance,                   
                    Vector3<T> & Ncontact,          
                    T tolerance = Math<T>::ThresholdZero
)
{
    //---------------------------------------------------------------
    T k;
    Vector3<T> pq1, pq2, pq3;
    //---------------------------------------------------------------
    // Find the projected point (pq1) of p1 on the line defined by q2-q1
    FindProjectedPointOnALine( p1, q1, q2, k, pq1 );
    //---------------------------------------------------------------
    // Find the projected point (pq2) of p2 on the line defined by q2-q1
    FindProjectedPointOnALine( p2, q1, q2, k, pq2 );
    //---------------------------------------------------------------
    // Find the projected point (pq3) of p2 on the plane defined by the 
    //   point pq1 and the plane normal defined by (q2-q1)
    FindProjectedPointOnAPlane( p2, p1, (q2-q1).Normalized(), pq3 );
    //---------------------------------------------------------------
    T d3 = (p1 - pq3).SquaredLength();
    //---------------------------------------------------------------
    //---------------------------------------------------------------
    // Unparallel Case
    if ( d3 > tolerance ) {
        //std::cout << "Unparallel Case\n";
        Vector3<T> ps;
        Vector3<T> qs;
        T d1 = (p1 - pq1).SquaredLength();
        T d2 = (p2 - pq2).SquaredLength();
        k = ( d1 - d2 + d3 ) / d3 / static_cast<T>( 2.0 );
        if      ( k >= 1 )  ps = p2;
        else if ( k <= 0 )  ps = p1;
        else                ps = p1 + k*(p2-p1);
        FindProjectedPointOnALine( ps, q1, q2, k );
        if      ( k >= 1 )  qs = q2;
        else if ( k <= 0 )  qs = q1;
        else                qs = q1 + k*(q2-q1);
        Ncontact = qs - ps;
        distance = Ncontact.Length();
        Ncontact.Normalized();
    }
    //---------------------------------------------------------------
    // Parallel Case
    else {
        //std::cout << "Parallel Case\n";
        T k1;
        FindProjectedPointOnALine( p1, q1, q2, k1 );
        if ( 0 <= k1 && k1 <= 1 ) {
            //std::cout << "( 0 <= k1 && k1 <= 1 )\n";
            Vector3<T> q = q1 + k1*(q2-q1);
            Ncontact = q - p1;
            distance = Ncontact.Length();
            Ncontact.Normalized();
            return;
        }
        T k2;
        FindProjectedPointOnALine( p2, q1, q2, k2 );
        if ( 0 <= k2 && k2 <= 1 ) {
            //std::cout << "( 0 <= k2 && k2 <= 1 )\n";
            Vector3<T> q = q1 + k2*(q2-q1);
            Ncontact = q - p2;
            distance = Ncontact.Length();
            Ncontact.Normalized();
            return;
        }
        //std::cout << "k1: " << k1 << " k2: " << k2 << "\n";
        if ( fabs(k1) < fabs(k2) ) {
            //std::cout << "k1 < k2\n";
            T dist = (q1 - p1).SquaredLength();
            distance = (q1 - p2).SquaredLength();
            if ( dist < distance ) {
                //std::cout << "(q1 - p1)\n";
                distance = sqrt( dist );
                Ncontact = (q1 - p1).Normalized();
                return;
            }
            else {
                //std::cout << "(q1 - p2)\n";
                distance = sqrt( distance );
                Ncontact = (q1 - p2).Normalized();
                return;
            }
        }
        else {
            //std::cout << "k2 < k1\n";
            T dist = (q2 - p1).SquaredLength();
            distance = (q2 - p2).SquaredLength();
            if ( dist < distance ) {
                //std::cout << "(q2 - p1)\n";
                distance = sqrt( dist );
                Ncontact = (q2 - p1).Normalized();
                return;
            }
            else {
                //std::cout << "(q2 - p2)\n";
                distance = sqrt( distance );
                Ncontact = (q2 - p2).Normalized();
                return;
            }
        }
    }
    //---------------------------------------------------------------
}
//-----------------------------------------------------------------------------


//*****************************************************************************
// Find the closest distance between two line segments
//   First project p1 and p2 on the line Lq defined by q1 and q2.
//     Let call them pq1 and pq2 respectively.
//   Second project p2 on the plane defined by pq1 and the unit vector of Lq
//     which is the plane normal.
//     Let call the projected point pq3.
//   Third determine d3, which is the square length of p1-pq3.
//     If d3 is not zero, then the line segments are unparallel.
//        Then find the closest points between the line segments.
//          Let call the closest points ps and qs.
//        Then the shortest distance is |ps - qs|.
//     If d3 is zero, then the line segments are parallel.
//        Then find the closest points between the line segments.
//          In this case, there are four 
//        Then the shortest distance is |ps - qs|.
//*****************************************************************************
template <typename T>
void CGMath<T>::FindClosestDistanceBetweenTwoLineSegments (
                    Vector3<T> const & p1, Vector3<T> const & p2, // I/P: a line 
                    Vector3<T> const & q1, Vector3<T> const & q2, // I/P: a line
                    T & distance,                   
                    Vector3<T> & Ncontact,          
                    Vector3<T> & projPtOnLine_1,    
                    Vector3<T> & projPtOnLine_2,    
                    T tolerance = Math<T>::ThresholdZero
)
{
    //---------------------------------------------------------------
    T k;
    Vector3<T> pq1, pq2, pq3;
    //---------------------------------------------------------------
    // Find the projected point (pq1) of p1 on the line defined by q2-q1
    FindProjectedPointOnALine( p1, q1, q2, k, pq1 );
    //---------------------------------------------------------------
    // Find the projected point (pq2) of p2 on the line defined by q2-q1
    FindProjectedPointOnALine( p2, q1, q2, k, pq2 );
    //---------------------------------------------------------------
    // Find the projected point (pq3) of p2 on the plane defined by the 
    //   point pq1 and the plane normal defined by (q2-q1)
    FindProjectedPointOnAPlane( p2, p1, (q2-q1).Normalized(), pq3 );
    //---------------------------------------------------------------
    T d3 = (p1 - pq3).SquaredLength();
    //---------------------------------------------------------------
    //---------------------------------------------------------------
    // Unparallel Case
    if ( d3 > tolerance ) {
        //std::cout << "Unparallel Case\n";
        T d1 = (p1 - pq1).SquaredLength();
        T d2 = (p2 - pq2).SquaredLength();
        k = ( d1 - d2 + d3 ) / d3 / static_cast<T>( 2.0 );
        if      ( k >= 1 )  projPtOnLine_1 = p2;
        else if ( k <= 0 )  projPtOnLine_1 = p1;
        else                projPtOnLine_1 = p1 + k*(p2-p1);
        FindProjectedPointOnALine( projPtOnLine_1, q1, q2, k );
        if      ( k >= 1 )  projPtOnLine_2 = q2;
        else if ( k <= 0 )  projPtOnLine_2 = q1;
        else                projPtOnLine_2 = q1 + k*(q2-q1);
        Ncontact = projPtOnLine_2 - projPtOnLine_1;
        distance = Ncontact.Length();
        Ncontact.Normalized();
    }
    //---------------------------------------------------------------
    // Parallel Case
    else {
        //std::cout << "Parallel Case\n";
        T k1;
        FindProjectedPointOnALine( p1, q1, q2, k1 );
        if ( 0 <= k1 && k1 <= 1 ) {
            //std::cout << "( 0 <= k1 && k1 <= 1 )\n";
            Vector3<T> q = q1 + k1*(q2-q1);
            Ncontact = q - p1;
            distance = Ncontact.Length();
            Ncontact.Normalized();
            return;
        }
        T k2;
        FindProjectedPointOnALine( p2, q1, q2, k2 );
        if ( 0 <= k2 && k2 <= 1 ) {
            //std::cout << "( 0 <= k2 && k2 <= 1 )\n";
            Vector3<T> q = q1 + k2*(q2-q1);
            Ncontact = q - p2;
            distance = Ncontact.Length();
            Ncontact.Normalized();
            return;
        }
        //std::cout << "k1: " << k1 << " k2: " << k2 << "\n";
        if ( fabs(k1) < fabs(k2) ) {
            //std::cout << "k1 < k2\n";
            T dist = (q1 - p1).SquaredLength();
            distance = (q1 - p2).SquaredLength();
            if ( dist < distance ) {
                //std::cout << "(q1 - p1)\n";
                distance = sqrt( dist );
                Ncontact = (q1 - p1).Normalized();
                return;
            }
            else {
                //std::cout << "(q1 - p2)\n";
                distance = sqrt( distance );
                Ncontact = (q1 - p2).Normalized();
                return;
            }
        }
        else {
            //std::cout << "k2 < k1\n";
            T dist = (q2 - p1).SquaredLength();
            distance = (q2 - p2).SquaredLength();
            if ( dist < distance ) {
                //std::cout << "(q2 - p1)\n";
                distance = sqrt( dist );
                Ncontact = (q2 - p1).Normalized();
                return;
            }
            else {
                //std::cout << "(q2 - p2)\n";
                distance = sqrt( distance );
                Ncontact = (q2 - p2).Normalized();
                return;
            }
        }
    }
    //---------------------------------------------------------------
}
//-----------------------------------------------------------------------------


//*****************************************************************************
// Find the projection point to a plane defined its normal and a point on it
// I/P: a point followed by a point on the plane and its normal
// O/P: the projected point on the plane defined by the line, and the distance
//*****************************************************************************
template <typename T>
void CGMath<T>::FindProjectedPointOnAPlane ( 
                    Vector3<T> const & p,   // I/P: a point
                    Vector3<T> const & a,   // I/P: a point on the plane
                    Vector3<T> const & n,   // I/P: the plane normal
                    Vector3<T> & q,     // O/P: q = the projected pt of p
                    T & distance        // O/P: distance = (p - a)*n
)
{
    //---------------------------------------------------------------
    // plane constant
    //T Kplane = a * n;
    //---------------------------------------------------------------
    //q = p - ( p*n - Kplane ) * n;
    //distance = (q - p).Length();
    //distance = (p - a)*n;
    distance = (p - a)*n;
    q = p - distance*n;
    distance = fabs( distance );
}
//-----------------------------------------------------------------------------
//*****************************************************************************
// Find the projection point to a plane defined its normal and a point on it
// I/P: a point followed by a point on the plane and its normal
// O/P: the projected point on the plane defined by the line
//*****************************************************************************
template <typename T>
void CGMath<T>::FindProjectedPointOnAPlane ( 
                    Vector3<T> const & p,   // I/P: a point
                    Vector3<T> const & a,   // I/P: a point on the plane
                    Vector3<T> const & n,   // I/P: the plane normal
                    Vector3<T> & q      // O/P: q = the projected pt of p
)
{
    q = p - ( (p - a)*n )*n;
}
//-----------------------------------------------------------------------------
//*****************************************************************************
// Find the projection point to a line
// I/P: a point followed by the first and second points defining a line
// O/P: the ratio, the projected point on the line, and the distance
//*****************************************************************************
template <typename T>
void CGMath<T>::FindProjectedPointOnALine ( 
                    Vector3<T> const & p,   // I/P: a point
                    Vector3<T> const & p1, Vector3<T> const & p2, // I/P: a line
                    T & ratio,          // O/P: ratio
                    Vector3<T> & q,     // O/P: q = p1 + ratio*(p1-p2)
                    T & distance        // O/P: distance = |p - q|
)
{
    Vector3<T> p2_p1 = p2 - p1;
    ratio = ((p-p1)*p2_p1) / (p2_p1*p2_p1);
    q = p1 + ratio*(p2 - p1);
    distance = (q - p).Length();
}
//-----------------------------------------------------------------------------
//*****************************************************************************
// Find the projection point to a line
// I/P: a point followed by the first and second points defining a line
// O/P: the ratio and the projected point on the line
//*****************************************************************************
template <typename T>
void CGMath<T>::FindProjectedPointOnALine ( 
                    Vector3<T> const & p,   // I/P: a point
                    Vector3<T> const & p1, Vector3<T> const & p2, // I/P: a line
                    T & ratio,          // O/P: ratio
                    Vector3<T> & q      // O/P: q = p1 + ratio*(p1-p2)
)
{
    Vector3<T> p2_p1 = p2 - p1;
    ratio = ((p-p1)*p2_p1) / (p2_p1*p2_p1);
    q = p1 + ratio*(p2 - p1);
}
//-----------------------------------------------------------------------------
//*****************************************************************************
// Find the projection point to a line
// I/P: a point followed by the first and second points defining a line
// O/P: the ratio
//******************************************************************************
template <typename T>
void CGMath<T>::FindProjectedPointOnALine ( 
                    Vector3<T> const & p,   // I/P: a point
                    Vector3<T> const & p1, Vector3<T> const & p2, // I/P: a line
                    T & ratio           // O/P: ratio
)
{
    Vector3<T> p2_p1 = p2 - p1;
    ratio = ((p-p1)*p2_p1) / (p2_p1*p2_p1);
}
//-----------------------------------------------------------------------------
//*****************************************************************************
// Find LineSegment-LineSegment Intersection
// I/P: two vectors defining the first line segment
// I/P: two vectors defining the second line segment
// O/P: return true then parameter t1 and t2 where 
//        where intersection_pt = p1 + t1(q1-p1) = p2 + t2(q2-p2)
//      return false then parameter t1 and t2 are meaningless

// PROTENTIAL BUGS
//   Have to check that t1 and t2 return values are valids for all cases
//*****************************************************************************
template <typename T>
bool CGMath<T>::FindIntersectionLineSegmentLineSegment (
        Vector3<T> const & p1,      // first line segment
        Vector3<T> const & q1, 
        Vector3<T> const & p2,      // second line segment
        Vector3<T> const & q2, 
        T & t1,     // parameter for first line segment
                        // where intersection_pt = p1 + t1(q1-p1)
        T & t2,     // parameter for second line segment
                        // where intersection_pt = p2 + t2(q2-p2)
        bool & touching,            // Touching instead of Intersecting
        T tolerance
)
{
    touching = false;
    //--------------------------------------------------------------------
    Vector3<T> V1( q1 - p1 );
    Vector3<T> V2( q2 - p2 );
    Vector3<T> N( V1 ^ V2 );
    //--------------------------------------------------------------------
    // Line segments are parallel
    if ( fabs( N.Length() ) < tolerance ) {
        //-----------------------------------------------------------
        // Test if q1,p1,q2 are not on the same line
        if ( fabs( (V1^( q2 - p1 )).Length() ) >= tolerance )   return false;
        //-----------------------------------------------------------
        // Transform to x-axis
        Matrix3x3<T> rotationMat;
        if ( p1.Length() > tolerance ) {
            rotationMat = CreateRotationMatrix3x3FromVectorAtoVectorB(
                        //p1, Vector3<T>(1,0,0) );
                        p1, VectorX );
        }
        else if ( q1.Length() > tolerance ) {
            rotationMat = CreateRotationMatrix3x3FromVectorAtoVectorB(
                        //q1, Vector3<T>(1,0,0) );
                        q1, VectorX );
        }
        else if ( p2.Length() > tolerance ) {
            rotationMat = CreateRotationMatrix3x3FromVectorAtoVectorB(
                        //p2, Vector3<T>(1,0,0) );
                        p2, VectorX );
        }
        else if ( q2.Length() > tolerance ) {
            rotationMat = CreateRotationMatrix3x3FromVectorAtoVectorB(
                        //q2, Vector3<T>(1,0,0) );
                        q2, VectorX );
        }
        else {
            std::cerr << "Line segments are just a point!!!" << std::endl;
            assert( false );
            return false;
        }
        //-----------------------------------------------------------
        T pp1 = ( rotationMat * p1 )[0];
        T pq1 = ( rotationMat * q1 )[0];
        T pp2 = ( rotationMat * p2 )[0];
        T pq2 = ( rotationMat * q2 )[0];
        T A, B, C, D;
        if ( pp1 <= pq1 ) {
            A = pp1;
            B = pq1;
        }
        else {
            A = pq1;
            B = pp1;
        }
        if ( pp2 <= pq2 ) {
            C = pp2;
            D = pq2;
        }
        else {
            C = pq2;
            D = pp2;
        }
        //-----------------------------------------------------------
        //  A-----B  C------D
        //  C------D  A-----B
        if ( B < C || D < A ) {
            return false;
        }
        else {
            // FINISH IT!
            // make ratio
            return true;
        }
    }
    //--------------------------------------------------------------------
    //std::cout << "D1: " << N*p1 << "  D2: " << N*p2 << "\n";
    //std::cout << fabs(N*p1 - N*p2) << " < " << tolerance << "\n";
    //--------------------------------------------------------------------
    /*
    glPushMatrix();
        //glTranslatef( 0, -2, 0 );
        glLineWidth( 5 );
        glBegin( GL_LINES );
            glColor3f( 1, 0, 0 );
            glVertex3fv( &(p1).GetDataFloat() );
            glVertex3fv( &(q1).GetDataFloat() );
            glVertex3fv( &(p2).GetDataFloat() );
            glVertex3fv( &(q2).GetDataFloat() );
        glEnd();
        glBegin( GL_LINES );
            glColor3f( 1, 0, 0 );
            glVertex3fv( &(p2).GetDataFloat() );
            glVertex3fv( &(p2 + p1-p2).GetDataFloat() );
        glEnd();
        glLineWidth( 3 );
        glBegin( GL_LINES );
            glColor3f( 0, 0, 1 );
            glVertex3fv( &(p2).GetDataFloat() );
            glVertex3fv( &(p2 + V2).GetDataFloat() );
        glEnd();
        glLineWidth( 1 );
    glPopMatrix();
    //*/
    //--------------------------------------------------------------------
    // Check if a point p1 of line segment#1 lies on line segment#2
    {
        Vector3<T> p1p2( p1 - p2 );
        if ( fabs( (p1p2 ^ V2).Length() ) < tolerance ) {
            //std::cout << "p1p2\n";
            t1 = 0;
            T length = V2.Length();
            V2.Normalized();
            t2 = (p1p2 * V2) / length;
            //std::cout << "t1: " << t1 << "\n";
            //std::cout << "t2: " << t2 << "\n";
            //-------------------------------------------------------
            if ( 0 <= t2 && t2 <= 1 ) {
                touching = true;
                return true;
            }
            else {
                return false;
            }
        }
    }
    //--------------------------------------------------------------------
    // Check if a point q1 of line segment#1 lies on line segment#2
    {
        Vector3<T> q1p2( q1 - p2 );
        if ( fabs( (q1p2 ^ V2).Length() ) < tolerance ) {
            //std::cout << "q1p2\n";
            t1 = 1;
            T length = V2.Length();
            V2.Normalized();
            t2 = (q1p2 * V2) / length;
            //std::cout << "t1: " << t1 << "\n";
            //std::cout << "t2: " << t2 << "\n";
            //-------------------------------------------------------
            if ( 0 <= t2 && t2 <= 1 ) {
                touching = true;
                return true;
            }
            else {
                return false;
            }
        }
    }
    //--------------------------------------------------------------------
    // Check if a point p2 of line segment#2 lies on line segment#1
    {
        Vector3<T> p2p1( p2 - p1 );
        if ( fabs( (p2p1 ^ V1).Length() ) < tolerance ) {
            //std::cout << "p2p1\n";
            t2 = 0;
            T length = V1.Length();
            V1.Normalized();
            t1 = (p2p1 * V1) / length;
            //std::cout << "t1: " << t1 << "\n";
            //std::cout << "t2: " << t2 << "\n";
            //-------------------------------------------------------
            if ( 0 <= t1 && t1 <= 1 ) {
                touching = true;
                return true;
            }
            else {
                return false;
            }
        }
    }
    //--------------------------------------------------------------------
    // Check if a point q2 of line segment#2 lies on line segment#1
    {
        Vector3<T> q2p1( q2 - p1 );
        if ( fabs( (q2p1 ^ V1).Length() ) < tolerance ) {
            //std::cout << "q2p1\n";
            t2 = 1;
            T length = V1.Length();
            V1.Normalized();
            t1 = (q2p1 * V1) / length;
            //std::cout << "t1: " << t1 << "\n";
            //std::cout << "t2: " << t2 << "\n";
            //-------------------------------------------------------
            if ( 0 <= t1 && t1 <= 1 ) {
                touching = true;
                return true;
            }
            else {
                return false;
            }
        }
    }
    //--------------------------------------------------------------------
    // Check if the line segments are on the same plane level
    N = (V1 ^ ( q2 - p1 )).Normalized();    // Get normal from q1p1 ^ q2p1

    //Vector3<T> N1 = (V1 ^ ( q2 - p1 )).Normalized();
    //Vector3<T> N2 = (V1 ^ ( p2 - p1 )).Normalized();
    //if ( fabs( (N1-N2).Length() ) > tolerance ) {
    //  std::cout << "Line Segments are NOT on the same plane!\n";
    //}
    //else {
    //  std::cout << "Line Segments are on the same plane!\n";
    //}

    //--------------------------------------------------------------------
    //std::cout << "N*p1: " << N*p1 << "\n";
    //std::cout << "N*p2: " << N*p2 << "\n";
    //--------------------------------------------------------------------
    if ( fabs(N*p1 - N*p2) > tolerance ) {
        //std::cout << "Line Segments are NOT on the same plane!\n";
        return false;
    }
    else {
        //std::cout << "Line Segments are on the same plane!\n";
    }
    //--------------------------------------------------------------------
    Vector3<T> K1 = N ^ V1;
    Vector3<T> K2 = N ^ V2;
    t1 = ( K2 * ( p2 - p1 ) ) / ( K2 * V1 );
    t2 = ( K1 * ( p1 - p2 ) ) / ( K1 * V2 );
    //--------------------------------------------------------------------
    //std::cout << "t1: " << t1 << "\n";
    //std::cout << "t2: " << t2 << "\n";
    //--------------------------------------------------------------------
    if ( 0 <= t1 && t1 <= 1 && 0 <= t2 && t2 <= 1 ) {
        if ( t1 == 0 || t1 == 1 || t2 == 0 || t2 == 1 ) {
            touching = true;
        }
        return true;
    }
    else    return false;
    //--------------------------------------------------------------------
    return false;
}
//-----------------------------------------------------------------------------


//*****************************************************************************
// Find Circle-Triangle Intersection
// I/P: a vector defining the circle center, a scalar value defining the sphere radius, 
// and a vector defining the circle normal (defining circle plane)
// I/P: three vectors defining the triangle in counter-clockwise dir
// O/P: three vectors for moving the triangle's points to resolve the intersection
//*****************************************************************************
template <typename T>
bool CGMath<T>::FindIntersectionCircleTriangle (
        // I/P:
        Vector3<T> const & center,  
        T                  radius,  
        Vector3<T> const & normal,  
        Vector3<T> const & p,       
        Vector3<T> const & q,       
        Vector3<T> const & r,       
        // O/P:
        Vector3<T> & outVec_1,      
        Vector3<T> & outVec_2,      
        Vector3<T> & outVec_3,      
        // O/P: (for translating to force feedback)
        Vector3<T> * contactPt,     
        // Threshold for zero value
        T tolerance = Math<T>::ThresholdZero    
)
{
    return false;
}
//-----------------------------------------------------------------------------


//*****************************************************************************
// Find Sphere-Triangle Intersection
// I/P: one vector defining the sphere center and a scalar value defining the sphere radius
// I/P: three vectors defining the triangle in counter-clockwise dir
//*****************************************************************************
template <typename T>
bool CGMath<T>::FindIntersectionSphereTriangle (
        // I/P:
        Vector3<T> const & center,  
        T                  radius,  
        Vector3<T> const & p,       
        Vector3<T> const & q,       
        Vector3<T> const & r,       
        T tolerance = Math<T>::ThresholdZero    
)
{
    //---------------------------------------------------------------
    // Simplify the computation by 
    // transforming the sphere to the origin
    //      _______
    //     /   |   \
    //     |(0,0,0)| 
    //     \___|___/
    //
    // I.e., the triangle must be translated by the negative of sphere center.
    Vector3<T> pp( p - center );
    Vector3<T> qq( q - center );
    Vector3<T> rr( r - center );
    //---------------------------------------------------------------
    //***************************************************************
    //---------------------------------------------------------------
    // Let P1 is the plane containing the triangle
    //
    //---------------------------------------------------------------
    // Case 1:  Sphere does not intersect with the triangle if sphere's 
    //          closest point to P1 is more than its radius away from P1
    //---------------------------------------------------------------
    // Case 2:  sphere's closest point to P1 is NOT more than its radius 
    //          away from P1
    //   Sphere may intersects with the triangle
    //---------------------------------------------------------------
    //***************************************************************
    //---------------------------------------------------------------
    // Find the normal of the triangle
    Vector3<T> N = (qq - pp) ^ (rr - pp);
    N.Normalized();
    //---------------------------------------------------------------
    // Plane constant tells how far the plane is from the origin.
    T P1 = pp * N;      // Plane Constant
    //---------------------------------------------------------------
    //***************************************************************
    // CASE 1: Sphere center (at origin) is more than its radius away from P1
    //---------------------------------------------------------------
    if ( fabs( P1 ) > radius )  return false;
    //---------------------------------------------------------------
    //***************************************************************
    // CASE 2: Sphere center (at origin) is NOT more than its radius away from P1
    //---------------------------------------------------------------
    // Create a circle centering on the triangle plane from the sphere
    // The circle center is the closest point from the sphere to the triangle plane
    // The circle radius is the radius of circle created by the sphere intersecting the triangle plane
    T           distance;       // the distance of the closest pt to the plane
    Vector3<T>  circleCenter;   // the projected point on the plane
    FindProjectedPointOnAPlane( ZeroVector, pp, N, circleCenter, distance );
    T circleRadius = sqrt( radius*radius - distance*distance );

    if ( FindIfACircleIntersectsATriangleOnTheSamePlane( circleCenter, circleRadius, pp, qq, rr, tolerance ) > 0 ) {
        return true;
    }
    else {
        return false;
    }
}
//-----------------------------------------------------------------------------


//*****************************************************************************
// Find Sphere-Triangle Intersection
// I/P: one vector defining the sphere center and a scalar value defining the sphere radius
// I/P: three vectors defining the triangle in counter-clockwise dir
// O/P: three vectors for moving the triangle's points to resolve the intersection
//*****************************************************************************
template <typename T>
bool CGMath<T>::FindIntersectionSphereTriangle (
        // I/P:
        Vector3<T> const & center,  
        T                  radius,  
        Vector3<T> const & p,       
        Vector3<T> const & q,       
        Vector3<T> const & r,       
        // O/P:
        Vector3<T> & outVec_1,      
        Vector3<T> & outVec_2,      
        Vector3<T> & outVec_3,      
        // O/P: (for translating to force feedback)
        Vector3<T> * contactPt,     
        // Threshold for zero value
        T tolerance = Math<T>::ThresholdZero    
)
{
    //---------------------------------------------------------------
    // Simplify the computation by 
    // transforming the sphere to the origin
    //      _______
    //     /   |   \
    //     |(0,0,0)| 
    //     \___|___/
    //
    // I.e., the triangle must be translated by the negative of sphere center.
    Vector3<T> pp( p - center );
    Vector3<T> qq( q - center );
    Vector3<T> rr( r - center );
    //---------------------------------------------------------------
    //***************************************************************
    //---------------------------------------------------------------
    // Let P1 is the plane containing the triangle
    //
    //---------------------------------------------------------------
    // Case 1:  Sphere does not intersect with the triangle if sphere's 
    //          closest point to P1 is more than its radius away from P1
    //---------------------------------------------------------------
    // Case 2:  sphere's closest point to P1 is NOT more than its radius 
    //          away from P1
    //   Sphere may intersects with the triangle
    //---------------------------------------------------------------
    //***************************************************************
    //---------------------------------------------------------------
    // Find the normal of the triangle
    Vector3<T> N = (qq - pp) ^ (rr - pp);
    N.Normalized();
    //---------------------------------------------------------------
    // Plane constant tells how far the plane is from the origin.
    T P1 = pp * N;      // Plane Constant
    //---------------------------------------------------------------
    //***************************************************************
    // CASE 1: Sphere center (at origin) is more than its radius away from P1
    //---------------------------------------------------------------
    if ( fabs( P1 ) > radius ) {
        if ( contactPt )    (*contactPt).SetXYZ( 0, 0, 0 );
        return false;
    }
    //---------------------------------------------------------------
    //***************************************************************
    // CASE 2: Sphere center (at origin) is NOT more than its radius away from P1
    //---------------------------------------------------------------
    // Create a circle centering on the triangle plane from the sphere
    // The circle center is the closest point from the sphere to the triangle plane
    // The circle radius is the radius of circle created by the sphere intersecting the triangle plane
    //      _______
    //     /   |   \
    //     |(0,0,0)| 
    //     \___|___/
    //         |
    //         |
    //       ------
    //       \ *  /   (*) is the projected point from the origin to the triangle
    //        \  /
    //         \/
    //
    T           distance;       // the distance of the closest pt to the plane
    Vector3<T>  circleCenter;   // the projected point on the plane
    FindProjectedPointOnAPlane( ZeroVector, pp, N, circleCenter, distance );
    T circleRadius = sqrt( radius*radius - distance*distance );

    // Test if the projected point (circleCenter) lies inside the triangle
    if ( FindIfAPointIsInATriangleOnTheSamePlane( circleCenter, pp, qq, rr, tolerance ) )
    {
        // All output vectors are set to the same value.
        Vector3<T> pt( ( circleCenter + CentroidOfTriangle(pp,qq,rr) ).Normalized() );
        outVec_1 = outVec_2 = outVec_3 = pt * (radius - distance);
        if ( contactPt ) {
            *contactPt = pt * radius + center;
        }

        // Old way
        // All output vectors are set to the same value.
        //outVec_1 = outVec_2 = outVec_3 = circleCenter.Normalized() * (radius - distance);
        //if ( contactPt ) {
        //  *contactPt = circleCenter * radius;
        //}
        return true;
    }

    // Test if the projected circle intersects the triangle, while its center (circleCenter) is outside the triangle
    int result = FindIfACircleIntersectsATriangleOnTheSamePlane( circleCenter, circleRadius, pp, qq, rr, tolerance );

    // DEBUG
    // For the collision case from FindIfACircleIntersectsATriangleOnTheSamePlane Fn
    //std::cout << "Result: " << result << "\n";

    switch ( result ) {
        case 0:
            if ( contactPt )    (*contactPt).SetXYZ( 0, 0, 0 );
            return false;
            break;
        case 1:     // See FindIfACircleIntersectsATriangleOnTheSamePlane Fn for detail
            {
                Vector3<T> pt( ( circleCenter + CentroidOfTriangle(pp,qq,rr) ).Normalized() );
                outVec_1 = outVec_2 = pt * (radius - distance);
                outVec_3.SetXYZ( 0, 0, 0 );
                if ( contactPt ) {
                    *contactPt =  pt * radius + center;
                }
            }
            break;
        case 2:     // See FindIfACircleIntersectsATriangleOnTheSamePlane Fn for detail
            {
                Vector3<T> pt( ( circleCenter + CentroidOfTriangle(pp,qq,rr) ).Normalized() );
                outVec_2 = outVec_3 = pt * (radius - distance);
                outVec_1.SetXYZ( 0, 0, 0 );
                if ( contactPt ) {
                    *contactPt =  pt * radius + center;
                }
            }
            break;
        case 3:     // See FindIfACircleIntersectsATriangleOnTheSamePlane Fn for detail
            {
                Vector3<T> pt( ( circleCenter + CentroidOfTriangle(pp,qq,rr) ).Normalized() );
                outVec_1 = outVec_3 = pt * (radius - distance);
                outVec_2.SetXYZ( 0, 0, 0 );
                if ( contactPt ) {
                    *contactPt =  pt * radius + center;
                }
            }
            break;
        case 11:    // See FindIfACircleIntersectsATriangleOnTheSamePlane Fn for detail
        case 21:    // See FindIfACircleIntersectsATriangleOnTheSamePlane Fn for detail
            // Already covered by the test above
            //   "Test if the projected point (circleCenter) lies inside the triangle"
            if ( contactPt )    (*contactPt).SetXYZ( 0, 0, 0 );
            return false;
            break;
    }

    return true;
}
//-----------------------------------------------------------------------------




//*****************************************************************************
// Find Cylinder-Cylinder Intersection
// I/P: four vectors defining two lines with two radii to define two cylinders
// O/P: a closest distance vector
//*****************************************************************************
template <typename T>
bool CGMath<T>::FindIntersectionCylinderCylinder (
    // I/P:
    Vector3<T> const & cA1,         // first  point of the 1st cylinder
    Vector3<T> const & cA2,         // second point of the 1st cylinder
    T                  cAradius,    // radius of the 1st cylinder
    Vector3<T> const & cB1,         // first  point of the 2nd cylinder
    Vector3<T> const & cB2,         // second point of the 2nd cylinder
    T                  cBradius,    // radius of the 2nd cylinder
    // O/P:
    Vector3<T> & distVector,        // the closest distance vector of the two cylinders
    // Threshold for zero value
    T tolerance = Math<T>::ThresholdZero    // thereshold for zero value 
)
{
    //---------------------------------------------------------------
    T k;
    Vector3<T> pq1, pq2, pq3;
    //---------------------------------------------------------------
    // Find the projected point (pq1) of cA1 on the line defined by cB2-cB1
    FindProjectedPointOnALine( cA1, cB1, cB2, k, pq1 );
    //---------------------------------------------------------------
    // Find the projected point (pq2) of cA2 on the line defined by cB2-cB1
    FindProjectedPointOnALine( cA2, cB1, cB2, k, pq2 );
    //---------------------------------------------------------------
    // Find the projected point (pq3) of p2 on the plane defined by the 
    //   point pq1 and the plane normal defined by (cB2-cB1)
    FindProjectedPointOnAPlane( cA2, cA1, (cB2-cB1).Normalized(), pq3 );
    //---------------------------------------------------------------
    T d3 = (cA1 - pq3).SquaredLength();
    //---------------------------------------------------------------
    if ( d3 > cAradius + cBradius ) return false;   // Not intersect
    //---------------------------------------------------------------
    // Unparallel Case
    if ( d3 > tolerance ) {
        Vector3<T> ps;
        Vector3<T> qs;
        T d1 = (cA1 - pq1).SquaredLength();
        T d2 = (cA2 - pq2).SquaredLength();
        k = ( d1 - d2 + d3 ) / d3 / static_cast<T>( 2.0 );
        if      ( k >= 1 )  ps = cA2;
        else if ( k <= 0 )  ps = cA1;
        else                ps = cA1 + k*(cA2-cA1);
        FindProjectedPointOnALine( ps, cB1, cB2, k );
        if      ( k >= 1 )  qs = cB2;
        else if ( k <= 0 )  qs = cB1;
        else                qs = cB1 + k*(cB2-cB1);
        distVector = qs - ps;
        return true;
    }
    //---------------------------------------------------------------
    // Parallel Case
    else {
        T k1;
        FindProjectedPointOnALine( cA1, cB1, cB2, k1 );
        if ( 0 <= k1 && k1 <= 1 ) {
            Vector3<T> q = cB1 + k1*(cB2-cB1);
            distVector = q - cA1;
            return true;
        }
        T k2;
        FindProjectedPointOnALine( cA2, cB1, cB2, k2 );
        if ( 0 <= k2 && k2 <= 1 ) {
            Vector3<T> q = cB1 + k2*(cB2-cB1);
            distVector = q - cA2;
            return true;
        }
        if ( fabs(k1) < fabs(k2) ) {
            T dist = (cB1 - cA1).SquaredLength();
            T distance = (cB1 - cA2).SquaredLength();
            if ( dist < distance ) {
                distVector = cB1 - cA1;
                return true;
            }
            else {
                distVector = cB1 - cA2;
                return true;
            }
        }
        else {
            T dist = (cB2 - cA1).SquaredLength();
            T distance = (cB2 - cA2).SquaredLength();
            if ( dist < distance ) {
                distVector = cB2 - cA1;
                return true;
            }
            else {
                distVector = cB2 - cA2;
                return true;
            }
        }
    }
    //---------------------------------------------------------------
}




//*****************************************************************************
// Find Cylinder-Triangle Intersection
// I/P: two vectors defining a line and a radius defining a cylinder
// I/P: three vectors defining second triangle in counter-clockwise dir
// O/P: a closest distance (not implemented yet)
//*****************************************************************************
template <typename T>
bool CGMath<T>::FindIntersectionCylinderTriangle (
        // I:P:
        Vector3<T> const & c1,      // first  point of cylinder
        Vector3<T> const & c2,      // second point of cylinder
        T                  radius,  // radius of cylinder
        Vector3<T> const & p,       // triangle
        Vector3<T> const & q, 
        Vector3<T> const & r, 
        // Threshold for zero value
        T tolerance = Math<T>::ThresholdZero    
)
{
    //--------------------------------------------------------------------
    // Transform the cylinder to align with y-axis with c1 at the origin
    //
    //      __l2__
    //         |
    //      (0,0,0)
    //      ___|__
    //        l1
    //
    Vector3<T> l2( (c2-c1)/2 );
    T tCylinderHalfLength = l2.Length();
    Vector3<T> vTranslator( l2 + c1 );
    Matrix3x3<T> rotMat = CreateRotationMatrix3x3FromVectorAtoVectorB(
                            l2, VectorY );
    l2.SetXYZ(     0,  tCylinderHalfLength, 0 );
    Vector3<T> l1( 0, -tCylinderHalfLength, 0 );
    //Vector3<T> l1 = rotMat * ( c1 - vTranslator );
    //l2 = rotMat * ( c2 - vTranslator );
    //--------------------------------------------------------------------
    // Transform the triangle with the same transformation for cylinder
    Vector3<T> pp( rotMat * ( p - vTranslator ) );
    Vector3<T> qq( rotMat * ( q - vTranslator ) );
    Vector3<T> rr( rotMat * ( r - vTranslator ) );
    //--------------------------------------------------------------------
    //--------------------------------------------------------------------
    //********************************************************************
    //--------------------------------------------------------------------
    // Let P1 is the plane containing the triangle
    //
    //--------------------------------------------------------------------
    // Case 1:  Cylinder does not intersect with the triangle 
    //          if both vertices of cylinder lie above or below P1
    //--------------------------------------------------------------------
    // Case 2:  Each vertex of cylinder lies on opposite side of P1
    //   Cylinder may intersects with the triangle
    //--------------------------------------------------------------------
    // Case 3:  One vertex of cylinder lies on one side of P1
    //          and the other lies on P1
    //   Cylinder may intersects with the triangle
    //--------------------------------------------------------------------
    // Case 4:  The cylinder is parallel with the plane of the triangle
    //          e.g. both c1 and c2 lies on the plane of the triangle
    //   Cylinder may intersects with the triangle
    //--------------------------------------------------------------------
    //********************************************************************
    // CASE 1: Both vertices of cylinder lie above or below P1
    //--------------------------------------------------------------------
    // Reject if all triangle point lies below y-value of l1
    T lowValue = -tolerance + l1[1];
    if ( pp[1] < lowValue && qq[1] < lowValue && rr[1] < lowValue ) {
    //if ( pp[1] < 0 && qq[1] < 0 && rr[1] < 0 ) {
        //std::cout << "Reject if all triangle point lies below y=0 (y of c1)\n";
        TAPsDebugMessage( "Reject if all triangle point lies below local y of c1\n" );
        return false;
    }
    //--------------------------------------------------------------------
    // Reject if all triangle point lies above y-value of l2
    T highValue = tolerance + l2[1];
    if ( pp[1] > highValue && qq[1] > highValue && rr[1] > highValue ) {
        //std::cout << "Reject if all triangle point lies above y of l2\n";
        TAPsDebugMessage( "Reject if all triangle point lies above local y of c2\n" );
        return false;
    }
    //--------------------------------------------------------------------
    //********************************************************************
    //--------------------------------------------------------------------
    // Find the normal of the triangle
    Vector3<T> N = (qq - pp) ^ (rr - pp);
    N.Normalized();
    //--------------------------------------------------------------------
    // Find where vertices of cylinder lie on which side of P1 plane
    T Kplane = pp * N;      // Plane Constant
    T l1P = N*l1 - Kplane;
    T l2P = N*l2 - Kplane;
    T zero = Math<T>::ZERO;
    //--------------------------------------------------------------------
    //********************************************************************
    // Case 3: c1 lies on the plane, but c2 does not
    //--------------------------------------------------------------------
    if ( fabs( l1P ) <= tolerance && fabs( l2P ) > tolerance ) {
        //l1P = zero;
        //-----------------------------------------------------------
        // Case the point is in the triangle
        bool result = FindIfAPointIsInATriangleOnTheSamePlane( l1, pp, qq, rr, tolerance );
        if ( result ) {
            //std::cout << "Case the point L1 is in the triangle\n";
            TAPsDebugMessage( "Case the point L1 is in the triangle\n" );
            return true;
        }
        //-----------------------------------------------------------
        // Case the point is not in the triangle
        radius += tolerance;
        T ratio, distance;
        Vector3<T> projectedPt;
        FindProjectedPointOnALine( l1, pp, qq, ratio, projectedPt, distance );
        if ( 0 <= ratio && ratio <= 1 && distance <= radius ) {
            //std::cout << "Case1 the point L1 is not in the triangle\n";
            TAPsDebugMessage( "Case1 the point L1 is not in the triangle\n" );
            return true;
        }
        FindProjectedPointOnALine( l1, qq, rr, ratio, projectedPt, distance );
        if ( 0 <= ratio && ratio <= 1 && distance <= radius ) {
            //std::cout << "Case2 the point L1 is not in the triangle\n";
            TAPsDebugMessage( "Case2 the point L1 is not in the triangle\n" );
            return true;
        }
        FindProjectedPointOnALine( l1, rr, pp, ratio, projectedPt, distance );
        if ( 0 <= ratio && ratio <= 1 && distance <= radius ) {
            //std::cout << "Case3 the point L1 is not in the triangle\n";
            TAPsDebugMessage( "Case3 the point L1 is not in the triangle\n" );
            return true;
        }
        //-----------------------------------------------------------
        return false;
    }
    //--------------------------------------------------------------------
    //********************************************************************
    // Case 3: c2 lies on the plane, but c1 does not
    //--------------------------------------------------------------------
    if ( fabs( l2P ) <= tolerance && fabs( l1P ) > tolerance ) {
        //l2P = zero;
        //-----------------------------------------------------------
        // Case the point is in the triangle
        bool result = FindIfAPointIsInATriangleOnTheSamePlane( l2, pp, qq, rr, tolerance );
        if ( result ) {
            //std::cout << "Case the point L2 is in the triangle\n";
            TAPsDebugMessage( "Case the point L2 is in the triangle\n" );
            return true;
        }
        //-----------------------------------------------------------
        // Case the point is not in the triangle
        radius += tolerance;
        T ratio, distance;
        Vector3<T> projectedPt;
        FindProjectedPointOnALine( l2, pp, qq, ratio, projectedPt, distance );
        if ( 0 <= ratio && ratio <= 1 && distance <= radius ) {
            //std::cout << "Case1 the point L2 is not in the triangle\n";
            TAPsDebugMessage( "Case1 the point L2 is not in the triangle\n" );
            return true;
        }
        FindProjectedPointOnALine( l2, qq, rr, ratio, projectedPt, distance );
        if ( 0 <= ratio && ratio <= 1 && distance <= radius ) {
            //std::cout << "Case2 the point L2 is not in the triangle\n";
            TAPsDebugMessage( "Case2 the point L2 is not in the triangle\n" );
            return true;
        }
        FindProjectedPointOnALine( l2, rr, pp, ratio, projectedPt, distance );
        if ( 0 <= ratio && ratio <= 1 && distance <= radius ) {
            //std::cout << "Case3 the point L2 is not in the triangle\n";
            TAPsDebugMessage( "Case3 the point L2 is not in the triangle\n" );
            return true;
        }
        //-----------------------------------------------------------
        return false;
    }
    //--------------------------------------------------------------------
    //********************************************************************
    // Case 4: The cylinder is parallel with the plane of triangle
    //   e.g. both c1 and c2 lies on the plane of the triangle
    //--------------------------------------------------------------------
    if ( fabs(l1P - l2P) <= tolerance ) {
        T distance;
        radius += tolerance;
        //-----------------------------------------------------------
        // Check l1 distance from plane
        Vector3<T> projectedPt1;
        FindProjectedPointOnAPlane( l1, pp, N, projectedPt1, distance );
        //-----------------------------------------------------------
        if ( distance > radius )    return false;
        //-----------------------------------------------------------
        // Case the projected point is in the triangle
        {
            bool result = FindIfAPointIsInATriangleOnTheSamePlane( 
                                projectedPt1, pp, qq, rr, tolerance );
            if ( result ) {
                //std::cout << "Case Parallel: the projected point L1 is in the triangle\n";
                TAPsDebugMessage( "Case Parallel: the projected point L1 is in the triangle\n" );
                return true;
            }
        }
        //-----------------------------------------------------------
        // Check l2 distance from plane
        Vector3<T> projectedPt2;
        FindProjectedPointOnAPlane( l2, pp, N, projectedPt2, distance );
        //-----------------------------------------------------------
        if ( distance > radius )    return false;
        //-----------------------------------------------------------
        // Case the projected point is in the triangle
        {
            bool result = FindIfAPointIsInATriangleOnTheSamePlane( 
                                projectedPt2, pp, qq, rr, tolerance );
            if ( result ) {
                //std::cout << "Case Parallel: the projected point L2 is in the triangle\n";
                TAPsDebugMessage( "Case Parallel: the projected point L2 is in the triangle\n" );
                return true;
            }
        }
        //-----------------------------------------------------------
        // Check if line segment l2-l1 intersects the triangle
        T ratio1, ratio2;
        bool touching;
        if ( FindIntersectionLineSegmentLineSegment( 
                projectedPt1, projectedPt2, pp, qq, ratio1, ratio2, touching, tolerance )
        ) {
            //std::cout << "Case Parallel: line segment L2-L1 intersects the triangle p-q\n";
            TAPsDebugMessage( "Case Parallel: line segment L2-L1 intersects the triangle p-q\n" );
            return true;
        }
        if ( FindIntersectionLineSegmentLineSegment( 
                projectedPt1, projectedPt2, qq, rr, ratio1, ratio2, touching, tolerance )
        ) {
            //std::cout << "Case Parallel: line segment L2-L1 intersects the triangle q-r\n";
            TAPsDebugMessage( "Case Parallel: line segment L2-L1 intersects the triangle q-r\n" );
            return true;
        }
        if ( FindIntersectionLineSegmentLineSegment( 
                projectedPt1, projectedPt2, rr, pp, ratio1, ratio2, touching, tolerance )
        ) {
            //std::cout << "Case Parallel: line segment L2-L1 intersects the triangle r-p\n";
            TAPsDebugMessage( "Case Parallel: line segment L2-L1 intersects the triangle r-p\n" );
            return true;
        }
        //-----------------------------------------------------------
        //std::cout << "Case Parallel: line segment L2-L1 DOESN't intersect the triangle\n";
        TAPsDebugMessage( "Case Parallel: line segment L2-L1 DOESN't intersect the triangle\n" );
        return false;
    }
    //--------------------------------------------------------------------
    // Case the cylinder is not parallel with the plane of triangle
    // and either l1 and l2 of cylinder does not lie on the plane of the triangle
    radius += tolerance;
    //---------------------------------------------------------------
//  if ( ( l1P > radius && l2P > radius ) || ( l1P < -radius && l2P < -radius ) ) {
        //TAPsDebugMessage( "Radius (" << radius << "): " << l1P << " " << l2P << "\n" );
        //TAPsDebugMessage( "Case Not Parallel1: cylinder skeleton does not intersect the triangle plane\n" );
//      return false;
//  }
    /*
    //---------------------------------------------------------------
    if (   ( l1P > tolerance && l2P > tolerance ) 
        || ( l1P < -tolerance && l2P < -tolerance ) ) {
        bool finalResult = false;
        //------------------------------------------------------
        // Project l1 to the triangle plane
        {
            Vector3<T> projectedPt1( pp - l1P*N );
            bool result = FindIfAPointIsInATriangleOnTheSamePlane( 
                                projectedPt1, pp, qq, rr, tolerance );
            //---------------------------------------------
            // Case the projected point is in the triangle
            if ( result ) {
                //std::cout << "Case Not Parallel (--/++): the projected point of l1 is in the triangle\n";
                TAPsDebugMessage( "Case Not Parallel (--/++): the projected point of l1 is in the triangle\n" );
                //return true;
                finalResult = true;
            }
        }
        //------------------------------------------------------
        // Project l2 to the triangle plane
        {
            Vector3<T> projectedPt2( pp - l2P*N );
            bool result = FindIfAPointIsInATriangleOnTheSamePlane( 
                                projectedPt2, pp, qq, rr, tolerance );
            //---------------------------------------------
            // Case the projected point is in the triangle
            if ( result ) {
                //std::cout << "Case Not Parallel (--/++): the projected point of l2 is in the triangle\n";
                TAPsDebugMessage( "Case Not Parallel (--/++): the projected point of l2 is in the triangle\n" );
                //return true;
                finalResult = true;
            }
        }
//      return finalResult;
    }
    //*/
    //--------------------------------------------------------------------
    //********************************************************************
    // CASE 2: Both vertices of cylinder lie above or below P1
    //--------------------------------------------------------------------
    if ( ( l1P < -tolerance && l2P >  tolerance ) 
      || ( l1P >  tolerance && l2P < -tolerance ) ) {
        //------------------------------------------------------
        // Find the point on the line that intersects the triangle plane
        Vector3<T> l2l1( l2-l1 );
        T ratio = (N*(pp-l1)) / (N*l2l1);
        Vector3<T> projectedPt( l1 + ratio*l2l1 );
        //------------------------------------------------------
        bool result = FindIfAPointIsInATriangleOnTheSamePlane( 
                            projectedPt, pp, qq, rr, tolerance );
        //------------------------------------------------------
        // Case the projected point is in the triangle
        if ( result ) {
            //std::cout << "Case Not Parallel: the triangle intersects the cylinder axis\n";
            TAPsDebugMessage( "Case Not Parallel: the triangle intersects the cylinder axis\n" );
            return true;
        }
    }
    //====================================================================
    // Final Test
    //---------------------------------------------------------------
    // Case the projected point is NOT in the triangle
    T distance1, distance2, distance3;
    Vector3<T> contactNormal1, contactNormal2, contactNormal3;
    FindClosestDistanceBetweenTwoLineSegments( 
                l1, l2,         // I/P: a line 
                pp, qq,         // I/P: the pq edge of the triangle
                distance1,      // O/P: distance
                contactNormal1, // O/P: contact normal
                tolerance );
    FindClosestDistanceBetweenTwoLineSegments( 
                l1, l2,         // I/P: a line 
                qq, rr,         // I/P: the pq edge of the triangle
                distance2,      // O/P: distance
                contactNormal2, // O/P: contact normal
                tolerance );
    FindClosestDistanceBetweenTwoLineSegments( 
                l1, l2,         // I/P: a line 
                rr, pp,         // I/P: the pq edge of the triangle
                distance3,      // O/P: distance
                contactNormal3, // O/P: contact normal
                tolerance );
    if ( distance1 < radius || distance2 < radius || distance3 < radius ) {
        //std::cout << distance1 << " " << distance2 << " " << distance3 << "\n";
        //TAPsDebugMessage ( (distance1) + " " 
        //           + (distance2) + " " 
        //           + (distance3) + "\n" );
        //std::cout << "Case Not Parallel: the projected point is Not in the triangle\n"; 
        TAPsDebugMessage( "Case Not Parallel: the projected point is NOT in the triangle, however the cylinder intersects the triangle\n" ); 
        return true;
    }
    //--------------------------------------------------------------------
    //====================================================================
    //--------------------------------------------------------------------
    TAPsDebugMessage( "Reach End of Test for FindIntersectionCylinderTriangle\n" );
    return false;
}
// END: FindIntersectionCylinderTriangle W/O Outputs
//-----------------------------------------------------------------------------




//*****************************************************************************
// Find Cylinder-Triangle Intersection
// I/P: two vectors defining a line and a radius defining a cylinder
// I/P: three vectors defining second triangle in counter-clockwise dir
// O/P: a closest distance (not implemented yet)
//*****************************************************************************
template <typename T>
bool CGMath<T>::FindIntersectionCylinderTriangle (
        // I:P:
        Vector3<T> const & c1,      // first  point of cylinder
        Vector3<T> const & c2,      // second point of cylinder
        T                  radius,  // radius of cylinder
        Vector3<T> const & p,       // triangle
        Vector3<T> const & q, 
        Vector3<T> const & r, 
        // O/P:
        Vector3<T> & outVec_1,      
        Vector3<T> & outVec_2,      
        Vector3<T> & outVec_3,      
        // O/P: (for translating to force feedback)
        Vector3<T> * contactPt,     
        // Threshold for zero value
        T tolerance = Math<T>::ThresholdZero    
)
{
    //--------------------------------------------------------------------
    // Transform the cylinder to align with y-axis with c1 at the origin
    //
    //      __l2__
    //         |
    //      (0,0,0)
    //      ___|__
    //        l1
    //
    Vector3<T> l2( (c2-c1)/2 );
    T tCylinderHalfLength = l2.Length();
    Vector3<T> vTranslator( l2 + c1 );  // translate the cylinder's center pt at half height to the origin
    Matrix3x3<T> rotMat = CreateRotationMatrix3x3FromVectorAtoVectorB(
                            l2, VectorY );
    l2.SetXYZ(     0,  tCylinderHalfLength, 0 );
    Vector3<T> l1( 0, -tCylinderHalfLength, 0 );
    //Vector3<T> l1 = rotMat * ( c1 - vTranslator );
    //l2 = rotMat * ( c2 - vTranslator );
    //--------------------------------------------------------------------
    // Transform the triangle with the same transformation for cylinder
    Vector3<T> pp( rotMat * ( p - vTranslator ) );
    Vector3<T> qq( rotMat * ( q - vTranslator ) );
    Vector3<T> rr( rotMat * ( r - vTranslator ) );
    //--------------------------------------------------------------------
    //--------------------------------------------------------------------
    //********************************************************************
    //--------------------------------------------------------------------
    // Let P1 is the plane containing the triangle
    //
    //--------------------------------------------------------------------
    // Case 1:  Cylinder does not intersect with the triangle 
    //          if both vertices of cylinder lie above or below P1
    //--------------------------------------------------------------------
    // Case 2:  Each vertex of cylinder lies on opposite side of P1
    //   Cylinder may intersects with the triangle
    //--------------------------------------------------------------------
    // Case 3:  One vertex of cylinder lies on one side of P1
    //          and the other lies on P1
    //   Cylinder may intersects with the triangle
    //--------------------------------------------------------------------
    // Case 4:  The cylinder is parallel with the plane of the triangle
    //          e.g. both c1 and c2 lies on the plane of the triangle
    //   Cylinder may intersects with the triangle
    //--------------------------------------------------------------------
    //********************************************************************
    // CASE 1: Both vertices of cylinder lie above or below P1
    //--------------------------------------------------------------------
    // Reject if all triangle point lies below y-value of l1
    T lowValue = -tolerance + l1[1];
    if ( pp[1] < lowValue && qq[1] < lowValue && rr[1] < lowValue ) {
    //if ( pp[1] < 0 && qq[1] < 0 && rr[1] < 0 ) {
        //std::cout << "Reject if all triangle point lies below y=0 (y of c1)\n";
        TAPsDebugMessage( "Reject if all triangle point lies below local y of c1\n" );
        if ( contactPt )    (*contactPt).SetXYZ( 0, 0, 0 );
        return false;
    }
    //--------------------------------------------------------------------
    // Reject if all triangle point lies above y-value of l2
    T highValue = tolerance + l2[1];
    if ( pp[1] > highValue && qq[1] > highValue && rr[1] > highValue ) {
        //std::cout << "Reject if all triangle point lies above y of l2\n";
        TAPsDebugMessage( "Reject if all triangle point lies above local y of c2\n" );
        if ( contactPt )    (*contactPt).SetXYZ( 0, 0, 0 );
        return false;
    }
    //--------------------------------------------------------------------
    //********************************************************************
    //--------------------------------------------------------------------
    // Find the normal of the triangle
    Vector3<T> N = (qq - pp) ^ (rr - pp);
    N.Normalized();
    //--------------------------------------------------------------------
    // Find where vertices of cylinder lie on which side of P1 plane
    T Kplane = pp * N;      // Plane Constant
    T l1P = N*l1 - Kplane;
    T l2P = N*l2 - Kplane;
    T zero = Math<T>::ZERO;
    //--------------------------------------------------------------------
    //********************************************************************
    // Case 3: c1 lies on the plane, but c2 does not
    //--------------------------------------------------------------------
    if ( fabs( l1P ) <= tolerance && fabs( l2P ) > tolerance ) {
        //l1P = zero;
        //-----------------------------------------------------------
        // Case the point is in the triangle
        bool result = FindIfAPointIsInATriangleOnTheSamePlane( l1, pp, qq, rr, tolerance );
        if ( result ) {

            TAPsDebugMessage( "Case the point L1 is in the triangle\n" );

            // Move the triangle's point if it lies between l2 and l1.
            radius += tolerance;
            T ratio, distance;
            Vector3<T> projectedPt;

            rotMat.Inversed();

            FindProjectedPointOnALine( pp, l2, l1, ratio, projectedPt, distance );
            if( 0 <= ratio && ratio <= 1 ) {
                outVec_1 = (rotMat * ( l1 - projectedPt )) + vTranslator;
            }
            FindProjectedPointOnALine( qq, l2, l1, ratio, projectedPt, distance );
            if( 0 <= ratio && ratio <= 1 ) {
                outVec_2 = (rotMat * ( l1 - projectedPt )) + vTranslator;
            }
            FindProjectedPointOnALine( rr, l2, l1, ratio, projectedPt, distance );
            if( 0 <= ratio && ratio <= 1 ) {
                outVec_3 = (rotMat * ( l1 - projectedPt )) + vTranslator;
            }

            if ( contactPt ) {
                //std::cout << "Cyn CD# c1 01\n";
                *contactPt = c1;
            }
            return true;
        }
        //-----------------------------------------------------------
        // Case the point is not in the triangle
        // But the point is less than the cylinder radius from the triangle
        radius += tolerance;
        T ratio, distance;
        Vector3<T> projectedPt;
        FindProjectedPointOnALine( l1, pp, qq, ratio, projectedPt, distance );
        if ( 0 <= ratio && ratio <= 1 && distance <= radius ) {

            TAPsDebugMessage( "Case1 the point L1 is not in the triangle\n" );

            Vector3<T> out( (projectedPt - l1).Normalized() * (radius - distance) );
            out = (rotMat.Inversed() * out) + vTranslator;
            outVec_1 = outVec_2 = outVec_3 = out;

            if ( contactPt ) {
                //std::cout << "Cyn CD# c1 02\n";
                *contactPt = (rotMat.Inversed() * Vector3<T>( 0, 0, projectedPt[2] )) + vTranslator;
            }

            return true;
        }
        FindProjectedPointOnALine( l1, qq, rr, ratio, projectedPt, distance );
        if ( 0 <= ratio && ratio <= 1 && distance <= radius ) {

            TAPsDebugMessage( "Case2 the point L1 is not in the triangle\n" );

            Vector3<T> out( (projectedPt - l1).Normalized() * (radius - distance) );
            out = (rotMat.Inversed() * out) + vTranslator;
            outVec_1 = outVec_2 = outVec_3 = out;

            if ( contactPt ) {
                //std::cout << "Cyn CD# c1 03\n";
                *contactPt = (rotMat.Inversed() * Vector3<T>( 0, 0, projectedPt[2] )) + vTranslator;
            }

            return true;
        }
        FindProjectedPointOnALine( l1, rr, pp, ratio, projectedPt, distance );
        if ( 0 <= ratio && ratio <= 1 && distance <= radius ) {

            TAPsDebugMessage( "Case3 the point L1 is not in the triangle\n" );

            Vector3<T> out( (projectedPt - l1).Normalized() * (radius - distance) );
            out = (rotMat.Inversed() * out) + vTranslator;
            outVec_1 = outVec_2 = outVec_3 = out;

            if ( contactPt ) {
                //std::cout << "Cyn CD# c1 04\n";
                *contactPt = (rotMat.Inversed() * Vector3<T>( 0, 0, projectedPt[2] )) + vTranslator;
            }

            return true;
        }
        //-----------------------------------------------------------
        if ( contactPt )    (*contactPt).SetXYZ( 0, 0, 0 );
        return false;
    }
    //--------------------------------------------------------------------
    //********************************************************************
    // Case 3: c2 lies on the plane, but c1 does not
    //--------------------------------------------------------------------
    if ( fabs( l2P ) <= tolerance && fabs( l1P ) > tolerance ) {
        //l2P = zero;
        //-----------------------------------------------------------
        // Case the point is in the triangle
        bool result = FindIfAPointIsInATriangleOnTheSamePlane( l2, pp, qq, rr, tolerance );
        if ( result ) {

            TAPsDebugMessage( "Case the point L2 is in the triangle\n" );

            // Move the triangle's point if it lies between l2 and l1.
            radius += tolerance;
            T ratio, distance;
            Vector3<T> projectedPt;

            FindProjectedPointOnALine( pp, l2, l1, ratio, projectedPt, distance );
            if( 0 <= ratio && ratio <= 1 ) {
                outVec_1 = (rotMat.Inversed() * ( l2 - projectedPt )) + vTranslator;
            }
            FindProjectedPointOnALine( qq, l2, l1, ratio, projectedPt, distance );
            if( 0 <= ratio && ratio <= 1 ) {
                outVec_2 = (rotMat.Inversed() * ( l2 - projectedPt )) + vTranslator;
            }
            FindProjectedPointOnALine( rr, l2, l1, ratio, projectedPt, distance );
            if( 0 <= ratio && ratio <= 1 ) {
                outVec_3 = (rotMat.Inversed() * ( l2 - projectedPt )) + vTranslator;
            }

            if ( contactPt ) {
                //std::cout << "Cyn CD# c2 01\n";
                *contactPt = c2;
            }
            return true;
        }
        //-----------------------------------------------------------
        // Case the point is not in the triangle
        radius += tolerance;
        T ratio, distance;
        Vector3<T> projectedPt;
        FindProjectedPointOnALine( l2, pp, qq, ratio, projectedPt, distance );
        if ( 0 <= ratio && ratio <= 1 && distance <= radius ) {

            TAPsDebugMessage( "Case1 the point L2 is not in the triangle\n" );

            Vector3<T> out( (projectedPt - l2).Normalized() * (radius - distance) );
            out = (rotMat.Inversed() * out) + vTranslator;
            outVec_1 = outVec_2 = outVec_3 = out;

            if ( contactPt ) {
                //std::cout << "Cyn CD# c2 02\n";
                *contactPt = (rotMat.Inversed() * Vector3<T>( 0, 0, projectedPt[2] )) + vTranslator;
            }

            return true;
        }
        FindProjectedPointOnALine( l2, qq, rr, ratio, projectedPt, distance );
        if ( 0 <= ratio && ratio <= 1 && distance <= radius ) {

            TAPsDebugMessage( "Case2 the point L2 is not in the triangle\n" );

            Vector3<T> out( (projectedPt - l2).Normalized() * (radius - distance) );
            out = (rotMat.Inversed() * out) + vTranslator;
            outVec_1 = outVec_2 = outVec_3 = out;

            if ( contactPt ) {
                //std::cout << "Cyn CD# c2 03\n";
                *contactPt = (rotMat.Inversed() * Vector3<T>( 0, 0, projectedPt[2] )) + vTranslator;
            }

            return true;
        }
        FindProjectedPointOnALine( l2, rr, pp, ratio, projectedPt, distance );
        if ( 0 <= ratio && ratio <= 1 && distance <= radius ) {

            TAPsDebugMessage( "Case3 the point L2 is not in the triangle\n" );

            Vector3<T> out( (projectedPt - l2).Normalized() * (radius - distance) );
            out = (rotMat.Inversed() * out) + vTranslator;
            outVec_1 = outVec_2 = outVec_3 = out;

            if ( contactPt ) {
                //std::cout << "Cyn CD# c2 04\n";
                *contactPt = (rotMat.Inversed() * Vector3<T>( 0, 0, projectedPt[2] )) + vTranslator;
            }

            return true;
        }
        //-----------------------------------------------------------
        if ( contactPt )    (*contactPt).SetXYZ( 0, 0, 0 );
        return false;
    }
    //--------------------------------------------------------------------
    //********************************************************************
    // Case 4: The cylinder is parallel with the plane of triangle
    //   e.g. both c1 and c2 lies on the plane of the triangle
    //--------------------------------------------------------------------
    if ( fabs(l1P - l2P) <= tolerance ) {
        T distance;
        radius += tolerance;
        //-----------------------------------------------------------
        // Check l1 distance from plane
        Vector3<T> projectedPt1;
        FindProjectedPointOnAPlane( l1, pp, N, projectedPt1, distance );
        //-----------------------------------------------------------
        if ( distance > radius ) {
            if ( contactPt )    (*contactPt).SetXYZ( 0, 0, 0 );
            return false;
        }
        //-----------------------------------------------------------
        // Case the projected point is in the triangle
        {
            bool result = FindIfAPointIsInATriangleOnTheSamePlane( 
                                projectedPt1, pp, qq, rr, tolerance );
            if ( result ) {
                TAPsDebugMessage( "Case Parallel: the projected point L1 is in the triangle\n" );

                Vector3<T> out( (projectedPt1 - l1).Normalized() * (radius - distance) );
                out = (rotMat.Inversed() * out) + vTranslator;
                outVec_1 = outVec_2 = outVec_3 = out;

                if ( contactPt ) {
                    //std::cout << "Cyn CD# (c1&c2 parallel to the tri) c1\n";
                    *contactPt = c1;
                }
                return true;
            }
        }
        //-----------------------------------------------------------
        // Check l2 distance from plane
        Vector3<T> projectedPt2;
        FindProjectedPointOnAPlane( l2, pp, N, projectedPt2, distance );
        //-----------------------------------------------------------
        if ( distance > radius ) {
            if ( contactPt )    (*contactPt).SetXYZ( 0, 0, 0 );
            return false;
        }
        //-----------------------------------------------------------
        // Case the projected point is in the triangle
        {
            bool result = FindIfAPointIsInATriangleOnTheSamePlane( 
                                projectedPt2, pp, qq, rr, tolerance );
            if ( result ) {
                TAPsDebugMessage( "Case Parallel: the projected point L2 is in the triangle\n" );

                Vector3<T> out( (projectedPt2 - l2).Normalized() * (radius - distance) );
                out = (rotMat.Inversed() * out) + vTranslator;
                outVec_1 = outVec_2 = outVec_3 = out;

                if ( contactPt ) {
                    //std::cout << "Cyn CD# (c1&c2 parallel to the tri) c2\n";
                    *contactPt = c2;
                }
                return true;
            }
        }
        //-----------------------------------------------------------
        // Check if line segment l2-l1 intersects the triangle
        T ratio1, ratio2;
        bool touching;
        if ( FindIntersectionLineSegmentLineSegment( 
                projectedPt1, projectedPt2, pp, qq, ratio1, ratio2, touching, tolerance )
        ) {
            TAPsDebugMessage( "Case Parallel: line segment L2-L1 intersects the triangle p-q\n" );

            // same as      (projectedPt2 - l2)
            Vector3<T> out( (projectedPt1 - l1).Normalized() * (radius - distance) );
            out = (rotMat.Inversed() * out) + vTranslator;
            outVec_1 = outVec_2 = outVec_3 = out;

            if ( contactPt ) {
                //std::cout << "Cyn CD# (c1&c2 parallel to the tri) (c1+c2)/2 -- 01\n";
                *contactPt = ( c1 + c2 ) * 0.5;
            }
            return true;
        }
        if ( FindIntersectionLineSegmentLineSegment( 
                projectedPt1, projectedPt2, qq, rr, ratio1, ratio2, touching, tolerance )
        ) {
            TAPsDebugMessage( "Case Parallel: line segment L2-L1 intersects the triangle q-r\n" );

            // same as      (projectedPt2 - l2)
            Vector3<T> out( (projectedPt1 - l1).Normalized() * (radius - distance) );
            out = (rotMat.Inversed() * out) + vTranslator;
            outVec_1 = outVec_2 = outVec_3 = out;

            if ( contactPt ) {
                //std::cout << "Cyn CD# (c1&c2 parallel to the tri) (c1+c2)/2 -- 02\n";
                *contactPt = ( c1 + c2 ) * 0.5;
            }
            return true;
        }
        if ( FindIntersectionLineSegmentLineSegment( 
                projectedPt1, projectedPt2, rr, pp, ratio1, ratio2, touching, tolerance )
        ) {
            TAPsDebugMessage( "Case Parallel: line segment L2-L1 intersects the triangle r-p\n" );

            // same as      (projectedPt2 - l2)
            Vector3<T> out( (projectedPt1 - l1).Normalized() * (radius - distance) );
            out = (rotMat.Inversed() * out) + vTranslator;
            outVec_1 = outVec_2 = outVec_3 = out;

            if ( contactPt ) {
                //std::cout << "Cyn CD# (c1&c2 parallel to the tri) (c1+c2)/2 -- 03\n";
                *contactPt = ( c1 + c2 ) * 0.5;
            }
            return true;
        }
        //-----------------------------------------------------------
        //std::cout << "Case Parallel: line segment L2-L1 DOESN't intersect the triangle\n";
        TAPsDebugMessage( "Case Parallel: line segment L2-L1 DOESN't intersect the triangle\n" );
        if ( contactPt )    (*contactPt).SetXYZ( 0, 0, 0 );
        return false;
    }
    //--------------------------------------------------------------------
    // Case the cylinder is not parallel with the plane of triangle
    // and either l1 and l2 of cylinder does not lie on the plane of the triangle
    radius += tolerance;
    //---------------------------------------------------------------
//  if ( ( l1P > radius && l2P > radius ) || ( l1P < -radius && l2P < -radius ) ) {
        //TAPsDebugMessage( "Radius (" << radius << "): " << l1P << " " << l2P << "\n" );
        //TAPsDebugMessage( "Case Not Parallel1: cylinder skeleton does not intersect the triangle plane\n" );
//      return false;
//  }
    /*
    //---------------------------------------------------------------
    if (   ( l1P > tolerance && l2P > tolerance ) 
        || ( l1P < -tolerance && l2P < -tolerance ) ) {
        bool finalResult = false;
        //------------------------------------------------------
        // Project l1 to the triangle plane
        {
            Vector3<T> projectedPt1( pp - l1P*N );
            bool result = FindIfAPointIsInATriangleOnTheSamePlane( 
                                projectedPt1, pp, qq, rr, tolerance );
            //---------------------------------------------
            // Case the projected point is in the triangle
            if ( result ) {
                //std::cout << "Case Not Parallel (--/++): the projected point of l1 is in the triangle\n";
                TAPsDebugMessage( "Case Not Parallel (--/++): the projected point of l1 is in the triangle\n" );
                //return true;
                finalResult = true;
            }
        }
        //------------------------------------------------------
        // Project l2 to the triangle plane
        {
            Vector3<T> projectedPt2( pp - l2P*N );
            bool result = FindIfAPointIsInATriangleOnTheSamePlane( 
                                projectedPt2, pp, qq, rr, tolerance );
            //---------------------------------------------
            // Case the projected point is in the triangle
            if ( result ) {
                //std::cout << "Case Not Parallel (--/++): the projected point of l2 is in the triangle\n";
                TAPsDebugMessage( "Case Not Parallel (--/++): the projected point of l2 is in the triangle\n" );
                //return true;
                finalResult = true;
            }
        }
//      return finalResult;
    }
    //*/
    //--------------------------------------------------------------------
    //********************************************************************
    // CASE 2: Both vertices of cylinder lie above or below P1
    //--------------------------------------------------------------------
    if ( ( l1P < -tolerance && l2P >  tolerance ) 
      || ( l1P >  tolerance && l2P < -tolerance ) ) {
        //------------------------------------------------------
        // Find the point on the line that intersects the triangle plane
        Vector3<T> l2l1( l2-l1 );
        T ratio = (N*(pp-l1)) / (N*l2l1);
        Vector3<T> projectedPt( l1 + ratio*l2l1 );
        //------------------------------------------------------
        bool result = FindIfAPointIsInATriangleOnTheSamePlane( 
                            projectedPt, pp, qq, rr, tolerance );
        //------------------------------------------------------
        // Case the projected point is in the triangle
        if ( result ) {

            TAPsDebugMessage( "Case Not Parallel: the triangle intersects the cylinder axis\n" );

            // The out vectors for THIS CASE IS IGNORED!!!
            // To implement it the ratio should be converted to length. 
            // The length should be compared with the cylinder radius.
            // If the length is less than the radius, then move the triangle out 
            // from the cylinder top or bottom part.
            // Otherwise, move the triangle out from the cylinder axis with 
            // the move direction is parallel to the cylinder axis.

            // Move the triangle out with the opposite direction of its normal
            outVec_1 = N * radius;
            outVec_1 = outVec_2 = outVec_3 = (rotMat.Inversed() * outVec_1);

            if ( contactPt ) {
                //std::cout << "Cyn CD# Triangle intersect the cylinder's axis\n";
                *contactPt = (rotMat * projectedPt) + vTranslator;  // rotMat is already inversed
            }
            return true;
        }
    }
    //====================================================================
    // Final Test
    //---------------------------------------------------------------
    // Case the projected point is NOT in the triangle
    T distance1, distance2, distance3;
    Vector3<T> contactNormal1, contactNormal2, contactNormal3;
    Vector3<T> projPtOnCylAxis, projPtOnATriEdge;
    bool result = false;

    outVec_1 = outVec_2 = outVec_3 = ZeroVector;

    FindClosestDistanceBetweenTwoLineSegments( 
                l1, l2,         // I/P: a line 
                pp, qq,         // I/P: the pq edge of the triangle
                distance1,      // O/P: distance
                contactNormal1, // O/P: contact normal
                projPtOnCylAxis, 
                projPtOnATriEdge, 
                tolerance );
    distance1 = radius - distance1;

    if ( contactPt )    (*contactPt).SetXYZ( 0, 0, 0 );     // clear the contact point

    rotMat.Inversed();      // inverse the rotation matrix

    if ( distance1 >= 0 ) {

        TAPsDebugMessage( "CASE: distance1 >= 0" );

        //Vector3<T> dir( projPtOnATriEdge - projPtOnCylAxis );
        Vector3<T> dir( projPtOnCylAxis - projPtOnATriEdge );
        //dir = ((rotMat * dir) + vTranslator).Normalized() * distance1;
        dir = dir.Normalized() * distance1;
        outVec_1 = dir;
        outVec_2 = dir;

        if ( contactPt ) {
            //std::cout << "Cyn CD# Final Test -- 01\n";
            *contactPt += (rotMat * projPtOnCylAxis) + vTranslator;
        }
        result = true;
    }

    FindClosestDistanceBetweenTwoLineSegments( 
                l1, l2,         // I/P: a line 
                qq, rr,         // I/P: the pq edge of the triangle
                distance2,      // O/P: distance
                contactNormal2, // O/P: contact normal
                projPtOnCylAxis, 
                projPtOnATriEdge, 
                tolerance );
    distance2 = radius - distance2;
    if ( distance2 >= 0 ) {

        TAPsDebugMessage( "CASE: distance2 >= 0" );

        //Vector3<T> dir( projPtOnATriEdge - projPtOnCylAxis );
        Vector3<T> dir( projPtOnCylAxis - projPtOnATriEdge );
        //dir = ((rotMat * dir) + vTranslator).Normalized() * distance2;
        dir = dir.Normalized() * distance2;
        outVec_2 = dir;
        outVec_3 = dir;

        if ( contactPt ) {
            //std::cout << "Cyn CD# Final Test -- 02\n";
            *contactPt += (rotMat * projPtOnCylAxis) + vTranslator;
        }
        result = true;
    }

    FindClosestDistanceBetweenTwoLineSegments( 
                l1, l2,         // I/P: a line 
                rr, pp,         // I/P: the pq edge of the triangle
                distance3,      // O/P: distance
                contactNormal3, // O/P: contact normal
                projPtOnCylAxis, 
                projPtOnATriEdge, 
                tolerance );
    distance3 = radius - distance3;
    if ( distance3 >= 0 ) {

        TAPsDebugMessage( "CASE: distance3 >= 0" );

        //Vector3<T> dir( projPtOnATriEdge - projPtOnCylAxis );
        Vector3<T> dir( projPtOnCylAxis - projPtOnATriEdge );
        //dir = ((rotMat * dir) + vTranslator).Normalized() * distance3;
        dir = dir.Normalized() * distance3;
        outVec_1 = dir;
        outVec_3 = dir;

        if ( contactPt ) {
            //std::cout << "Cyn CD# Final Test -- 03\n";
            *contactPt += (rotMat * projPtOnCylAxis) + vTranslator;
        }
        result = true;
    }

    if ( distance1 >= 0 && distance3 >= 0 ) {
        outVec_1 *= 0.5;
        if ( contactPt ) {
            *contactPt *= 0.5;
        }
    }
    if ( distance1 >= 0 && distance2 >= 0 ) {
        outVec_2 *= 0.5;
        if ( contactPt ) {
            *contactPt *= 0.5;
        }
    }
    if ( distance2 >= 0 && distance3 >= 0 ) {
        outVec_3 *= 0.5;
        if ( contactPt ) {
            *contactPt *= 0.5;
        }
    }

    return result;
}
// END: FindIntersectionCylinderTriangle W/ Outputs
//-----------------------------------------------------------------------------




//*****************************************************************************
// Find Triangle-Triangle Intersection
// I/P: three vectors defining first triangle in counter-clockwise dir
// I/P: three vectors defining second triangle in counter-clockwise dir
// O/P: a closest distance
//*****************************************************************************
template <typename T>
bool CGMath<T>::FindIntersectionTriangleTriangle (
        Vector3<T> const & p1,      // first triangle
        Vector3<T> const & q1, 
        Vector3<T> const & r1, 
        Vector3<T> const & p2,      // second triangle
        Vector3<T> const & q2, 
        Vector3<T> const & r2, 
        T tolerance = Math<T>::ThresholdZero
)
{
    T zero = Math<T>::ZERO;
    //--------------------------------------------------------------------
    // Find the normal of triangle#2
    Vector3<T> N2 = (q2 - p2) ^ (r2 - p2);
    N2.Normalized();
    //--------------------------------------------------------------------
    // Find where vertices of triangle#1 lie on which side of P2
    T p1P2 = N2 * (p1 - p2);
    T q1P2 = N2 * (q1 - p2);
    T r1P2 = N2 * (r1 - p2);
    if ( fabs( p1P2 ) < tolerance ) p1P2 = zero;
    if ( fabs( q1P2 ) < tolerance ) q1P2 = zero;
    if ( fabs( r1P2 ) < tolerance ) r1P2 = zero;
    if ( p1P2 > zero && q1P2 > zero && r1P2 > zero )    return false;
    if ( p1P2 < zero && q1P2 < zero && r1P2 < zero )    return false;
    //--------------------------------------------------------------------
    // Find the normal of triangle#1
    Vector3<T> N1 = (q1 - p1) ^ (r1 - p1);
    N1.Normalized();
    //--------------------------------------------------------------------
    // Find where vertices of triangle#2 lie on which side of P1
    T p2P1 = N1 * (p2 - p1);
    T q2P1 = N1 * (q2 - p1);
    T r2P1 = N1 * (r2 - p1);
    if ( fabs( p2P1 ) < tolerance ) p2P1 = zero;
    if ( fabs( q2P1 ) < tolerance ) q2P1 = zero;
    if ( fabs( r2P1 ) < tolerance ) r2P1 = zero;
    //--------------------------------------------------------------------
    Vector3<T> const * V2[3];
    int caseNo = 0;
    // Case p2P1 > zero
    if      ( p2P1 > zero ) {
        if      ( q2P1 > zero ) {
            if      ( r2P1 > zero ) {
                // No Intersection: +p2P1, +q2P1, +r2P1
                return false;
            }
            else if ( r2P1 < zero ) {
                // Case 1: +p2P1, +q2P1, -r2P1
                caseNo = 1;
                V2[0] = &p2;
                V2[1] = &q2;
                V2[2] = &r2;
            }
            else {
                // Case 2: +p2P1, +q2P1, 0r2P1
                caseNo = 2;
                V2[0] = &r2;
                V2[1] = &p2;
                V2[2] = &q2;
            }
        }
        else if ( q2P1 < zero ) {
            if      ( r2P1 > zero ) {
                // Case 1: +p2P1, -q2P1, +r2P1
                caseNo = 1;
                V2[0] = &r2;
                V2[1] = &p2;
                V2[2] = &q2;
            }
            else if ( r2P1 < zero ) {
                // Case 1: +p2P1, -q2P1, -r2P1
                caseNo = 1;
                V2[0] = &q2;
                V2[1] = &r2;
                V2[2] = &p2;
            }
            else {
                // Case 3: +p2P1, -q2P1, 0r2P1
                caseNo = 3;
                V2[0] = &r2;
                V2[1] = &p2;
                V2[2] = &q2;
            }
        }
        else {
            if      ( r2P1 > zero ) {
                // Case 2: +p2P1, 0q2P1, +r2P1
                caseNo = 2;
                V2[0] = &q2;
                V2[1] = &r2;
                V2[2] = &p2;
            }
            else if ( r2P1 < zero ) {
                // Case 3: +p2P1, 0q2P1, -r2P1
                caseNo = 3;
                V2[0] = &q2;
                V2[1] = &r2;
                V2[2] = &p2;
            }
            else {
                // Case 4: +p2P1, 0q2P1, 0r2P1
                caseNo = 4;
                V2[0] = &q2;
                V2[1] = &r2;
                V2[2] = &p2;
            }
        }
    }
    //--------------------------------------------------------------------
    // Case p2P1 < zero
    else if ( p2P1 < zero ) {
        if      ( q2P1 < zero ) {
            if      ( r2P1 < zero ) {
                // No Intersection: -p2P1, -q2P1, -r2P1
                return false;
            }
            else if ( r2P1 > zero ) {
                // Case 1: -p2P1, -q2P1, +r2P1
                caseNo = 1;
                V2[0] = &p2;
                V2[1] = &q2;
                V2[2] = &r2;
            }
            else {
                // Case 2: -p2P1, -q2P1, 0r2P1
                caseNo = 2;
                V2[0] = &r2;
                V2[1] = &p2;
                V2[2] = &q2;
            }
        }
        else if ( q2P1 > zero ) {
            if      ( r2P1 < zero ) {
                // Case 1: -p2P1, +q2P1, -r2P1
                caseNo = 1;
                V2[0] = &r2;
                V2[1] = &p2;
                V2[2] = &q2;
            }
            else if ( r2P1 > zero ) {
                // Case 1: -p2P1, +q2P1, +r2P1
                caseNo = 1;
                V2[0] = &q2;
                V2[1] = &r2;
                V2[2] = &p2;
            }
            else {
                // Case 3: -p2P1, +q2P1, 0r2P1
                caseNo = 3;
                V2[0] = &r2;
                V2[1] = &p2;
                V2[2] = &q2;
            }
        }
        else {
            if      ( r2P1 < zero ) {
                // Case 2: -p2P1, 0q2P1, -r2P1
                caseNo = 2;
                V2[0] = &q2;
                V2[1] = &r2;
                V2[2] = &p2;
            }
            else if ( r2P1 > zero ) {
                // Case 3: -p2P1, 0q2P1, +r2P1
                caseNo = 3;
                V2[0] = &q2;
                V2[1] = &r2;
                V2[2] = &p2;
            }
            else {
                // Case 4: -p2P1, 0q2P1, 0r2P1
                caseNo = 4;
                V2[0] = &q2;
                V2[1] = &r2;
                V2[2] = &p2;
            }
        }
    }
    //--------------------------------------------------------------------
    // Case p2P1 = zero
    else {
        if      ( q2P1 < zero ) {
            if      ( r2P1 < zero ) {
                // Case 2: 0p2P1, -q2P1, -r2P1
                caseNo = 2;
                V2[0] = &p2;
                V2[1] = &q2;
                V2[2] = &r2;
            }
            else if ( r2P1 > zero ) {
                // Case 3: 0p2P1, -q2P1, +r2P1
                caseNo = 3;
                V2[0] = &p2;
                V2[1] = &q2;
                V2[2] = &r2;
            }
            else {
                // Case 4: 0p2P1, -q2P1, 0r2P1
                caseNo = 4;
                V2[0] = &r2;
                V2[1] = &p2;
                V2[2] = &q2;
            }
        }
        else if ( q2P1 > zero ) {
            if      ( r2P1 < zero ) {
                // Case 3: 0p2P1, +q2P1, -r2P1
                caseNo = 3;
                V2[0] = &p2;
                V2[1] = &q2;
                V2[2] = &r2;
            }
            else if ( r2P1 > zero ) {
                // Case 2: 0p2P1, +q2P1, +r2P1
                caseNo = 2;
                V2[0] = &p2;
                V2[1] = &q2;
                V2[2] = &r2;
            }
            else {
                // Case 4: 0p2P1, +q2P1, 0r2P1
                caseNo = 4;
                V2[0] = &r2;
                V2[1] = &p2;
                V2[2] = &q2;
            }
        }
        else {
            if      ( r2P1 < zero ) {
                // Case 4: 0p2P1, 0q2P1, -r2P1
                caseNo = 4;
                V2[0] = &p2;
                V2[1] = &q2;
                V2[2] = &r2;
            }
            else if ( r2P1 > zero ) {
                // Case 4: 0p2P1, 0q2P1, +r2P1
                caseNo = 4;
                V2[0] = &p2;
                V2[1] = &q2;
                V2[2] = &r2;
            }
            else {
                // Case 5: 0p2P1, 0q2P1, 0r2P1
                caseNo = 5;
            }
        }
    }
    //--------------------------------------------------------------------
    // Case 1:  Two vertices of triangle#2 lie on one side of P1
    //          and the other lies on the other side of P1
    //          //Two vertices of triangle#1 lie on one side of P2
    //          //and the other lies on the other side of P2
    //  Triangle#2 may intersects with triangle#1
    //--------------------------------------------------------------------
    // Case 2:  Two vertices of triangle#2 lie on one side of P1
    //          and the other lies on P1
    //          //Two vertices of triangle#1 lie on one side of P2
    //          //and the other lies on the other side of P2
    //  A vertex of triangle#2 may touches interior/boundary of triangle#1
    //--------------------------------------------------------------------
    // Case 3:  One vertex of triangle#2 lies on one side of P1, 
    //          another one lies on the other side of P1, 
    //          and the last one lies on P1
    //          //Two vertices of triangle#1 lie on one side of P2
    //          //and the other lies on the other side of P2
    //  A vertex of triangle#2 may touches interior/boundary of triangle#1
    //--------------------------------------------------------------------
    // Case 4:  One vertex of triangle#2 lies on one side of P1
    //          and the other two lie on P1
    //          //Two vertices of triangle#1 lie on one side of P2
    //          //and the other lies on the other side of P2
    //  An edge of Triangle#2 may intersects or touches triangle#1
    //--------------------------------------------------------------------
    // Case 5:  All three vertices of triangle#2 lies on P1
    //          Also all three vertices of triangle#1 (should) lies on P2
    //  Triangle#2 and triangle#1 are coplanar
    //  Triangle#2 may intersects with triangle#1
    //--------------------------------------------------------------------
    //Vector3<T> D( N1 ^ N2 );      // The direction of intersection line
    T t2[2];            // 
    T D1;               // with N1 define plane1 -- D1 = N1*(any_pt_on_P1)
    Vector3<T> proj2[2];
    Vector3<T> diff[2];
    Vector3<T> intersectionPt[4];
    bool result1, result2, result3;
    T para1, para2;
    bool touching;
    int count;
    switch ( caseNo ) {
        //===========================================================
        // *V2[0] and *V2[1] lie on one side of P1 plane
        // *V2[2] lies on the other side of P1 plane
        case 1:
            //std::cout << "Case1\n";
            //--------------------------------------------------
            // Project onto P1 plane
            D1 = N1 * p1;
            diff[0] = *V2[2] - *V2[0];
            diff[1] = *V2[2] - *V2[1];
            t2[0] = ( D1 - N1*(*V2[0]) ) / ( N1*diff[0] );
            t2[1] = ( D1 - N1*(*V2[1]) ) / ( N1*diff[1] );
            proj2[0] = *V2[0] + t2[0]*diff[0];
            proj2[1] = *V2[1] + t2[1]*diff[1];
            //--------------------------------------------------
            // Test if points are inside Triangle#1
            result1 = FindIfAPointIsInATriangleOnTheSamePlane( proj2[0], p1, q1, r1, tolerance );
            result2 = FindIfAPointIsInATriangleOnTheSamePlane( proj2[1], p1, q1, r1, tolerance );
            //glPointSize( 9 );
            //glBegin( GL_POINTS );
            //  if ( result1 )  {
            //      glColor3f( 0, 0, 0 );
            //  }
            //  else {
            //      glColor3f( 0.5, 0.5, 1 );
            //  }
            //  glVertex3fv( &(proj2[0].GetDataFloat()) );
            //  if ( result2 )  {
            //      glColor3f( 0, 0, 0 );
            //  }
            //  else {
            //      glColor3f( 0.5, 0.5, 1 );
            //  }
            //  glVertex3fv( &(proj2[1].GetDataFloat()) );
            //glEnd();
            //glPointSize( 1 );
            //--------------------------------------------------
            // At least one of the projected points is inside Triangle#1
            if ( result1 || result2 ) {
                //std::cout << "RESULT1 & 2: " << result1 << "\t" << result2 << "\n";
                return true;
            }
            //--------------------------------------------------
            // Both projected points are outside Triangle#1
            else {
                count = 0;
                result1 = FindIntersectionLineSegmentLineSegment( 
                    proj2[0], proj2[1], p1, q1, para1, para2, touching, tolerance );
                result2 = FindIntersectionLineSegmentLineSegment( 
                    proj2[0], proj2[1], q1, r1, para1, para2, touching, tolerance );
                result3 = FindIntersectionLineSegmentLineSegment( 
                    proj2[0], proj2[1], r1, q1, para1, para2, touching, tolerance );
                if ( result1 ) ++count;
                if ( result2 ) ++count;
                if ( result3 ) ++count;
                if ( count >= 2 ) return true;
                //-----------------------------------------
                // Case one point of p1, coplanar with P2
                else {
                    if ( p1P2 == zero ) {
                        result1 = FindIfAPointIsInATriangleOnTheSamePlane( p1, p2, q2, r2, tolerance );
                        if ( result1 )  return true;
                    }
                    if ( q1P2 == zero ) {
                        result1 = FindIfAPointIsInATriangleOnTheSamePlane( q1, p2, q2, r2, tolerance );
                        if ( result1 )  return true;
                    }
                    if ( r1P2 == zero ) {
                        result1 = FindIfAPointIsInATriangleOnTheSamePlane( r1, p2, q2, r2, tolerance );
                        if ( result1 )  return true;
                    }
                }
            }
            //--------------------------------------------------
            return false;
            break;
        //===========================================================
        // *V2[2] is on the P1 plane
        // *V2[0] and *V2[1] are on one side of P1 plane
        case 2:
            //std::cout << "Case2\n";
            //--------------------------------------------------
            result1 = FindIfAPointIsInATriangleOnTheSamePlane( *V2[0], p1, q1, r1, tolerance );
            //glPointSize( 9 );
            //glBegin( GL_POINTS );
            //  if ( result1 )  {
            //      glColor3f( 0, 0, 0 );
            //  }
            //  else {
            //      glColor3f( 0.5, 0.5, 1 );
            //  }
            //  glVertex3fv( &((*V2[0]).GetDataFloat()) );
            //glEnd();
            //glPointSize( 1 );
            if ( result1 )  return true;
            else            return false;
            //--------------------------------------------------
            break;
        //===========================================================
        // *V2[0] is on the P1 plane
        // *V2[1] lies on one side of P1 plane
        // *V2[2] lies on the other side of P1 plane
        case 3:
            //std::cout << "Case3\n";
            //--------------------------------------------------
            // Project onto P1 plane
            D1 = N1 * p1;
            diff[0] = *V2[2] - *V2[1];
            t2[0] = ( D1 - N1*(*V2[1]) ) / ( N1*diff[0] );
            proj2[0] = *V2[1] + t2[0]*diff[0];
            //--------------------------------------------------
            // Test if points are inside Triangle#1
            result1 = FindIfAPointIsInATriangleOnTheSamePlane( *V2[0], p1, q1, r1, tolerance );
            result2 = FindIfAPointIsInATriangleOnTheSamePlane( proj2[0], p1, q1, r1, tolerance );
            //glPointSize( 9 );
            //glBegin( GL_POINTS );
            //  if ( result1 )  {
            //      glColor3f( 0, 0, 0 );
            //  }
            //  else {
            //      glColor3f( 0.5, 0.5, 1 );
            //  }
            //  glVertex3fv( &((*V2[0]).GetDataFloat()) );
            //  if ( result2 )  {
            //      glColor3f( 0, 0, 0 );
            //  }
            //  else {
            //      glColor3f( 0.5, 0.5, 1 );
            //  }
            //  glVertex3fv( &(proj2[0].GetDataFloat()) );

            //  glColor3f( 1, 0, 0 );
            //  glVertex3fv( &((*V2[1]).GetDataFloat()) );
            //  glVertex3fv( &((*V2[2]).GetDataFloat()) );
            //glEnd();
            //glPointSize( 1 );
            //--------------------------------------------------
            // At least one of the projected points is inside Triangle#1
            if ( result1 || result2 ) {
                //std::cout << "RESULT1 & 2: " << result1 << "\t" << result2 << "\n";
                return true;
            }
            //--------------------------------------------------
            // Both projected points are outside Triangle#1
            else {
                count = 0;
                result1 = FindIntersectionLineSegmentLineSegment( 
                    *V2[0], proj2[0], p1, q1, para1, para2, touching, tolerance );
                result2 = FindIntersectionLineSegmentLineSegment( 
                    *V2[0], proj2[0], q1, r1, para1, para2, touching, tolerance );
                result3 = FindIntersectionLineSegmentLineSegment( 
                    *V2[0], proj2[0], r1, q1, para1, para2, touching, tolerance );
                if ( result1 ) ++count;
                if ( result2 ) ++count;
                if ( result3 ) ++count;
                if ( count >= 2 ) return true;
                //-----------------------------------------
                // Case one point of p1, coplanar with P2
                else {
                    if ( p1P2 == zero ) {
                        result1 = FindIfAPointIsInATriangleOnTheSamePlane( p1, p2, q2, r2, tolerance );
                        if ( result1 )  return true;
                    }
                    if ( q1P2 == zero ) {
                        result1 = FindIfAPointIsInATriangleOnTheSamePlane( q1, p2, q2, r2, tolerance );
                        if ( result1 )  return true;
                    }
                    if ( r1P2 == zero ) {
                        result1 = FindIfAPointIsInATriangleOnTheSamePlane( r1, p2, q2, r2, tolerance );
                        if ( result1 )  return true;
                    }
                }
            }
            //--------------------------------------------------
            return false;
            break;
        //===========================================================
        // *V2[0] and *V2[1] lie on P1 plane
        // *V2[2] lies on one side of P1 plane
        case 4:
            //std::cout << "Case4\n";
            //--------------------------------------------------
            // Test if points are inside Triangle#1
            result1 = FindIfAPointIsInATriangleOnTheSamePlane( *V2[0], p1, q1, r1, tolerance );
            result2 = FindIfAPointIsInATriangleOnTheSamePlane( *V2[1], p1, q1, r1, tolerance );
            //glPointSize( 9 );
            //glBegin( GL_POINTS );
            //  if ( result1 )  {
            //      glColor3f( 0, 0, 0 );
            //  }
            //  else {
            //      glColor3f( 0.5, 0.5, 1 );
            //  }
            //  glVertex3fv( &((*V2[0]).GetDataFloat()) );
            //  if ( result2 )  {
            //      glColor3f( 0, 0, 0 );
            //  }
            //  else {
            //      glColor3f( 0.5, 0.5, 1 );
            //  }
            //  glVertex3fv( &((*V2[1]).GetDataFloat()) );
            //glEnd();
            //glPointSize( 1 );
            //--------------------------------------------------
            // At least one of the projected points is inside Triangle#1
            if ( result1 || result2 ) {
                return true;
            }
            //--------------------------------------------------
            // Both projected points are outside Triangle#1
            else {
                count = 0;
                result1 = FindIntersectionLineSegmentLineSegment( 
                    *V2[0], *V2[1], p1, q1, para1, para2, touching, tolerance );
                result2 = FindIntersectionLineSegmentLineSegment( 
                    *V2[0], *V2[1], q1, r1, para1, para2, touching, tolerance );
                result3 = FindIntersectionLineSegmentLineSegment( 
                    *V2[0], *V2[1], r1, q1, para1, para2, touching, tolerance );
                if ( result1 ) ++count;
                if ( result2 ) ++count;
                if ( result3 ) ++count;
                if ( count >= 2 ) return true;
            }
            //--------------------------------------------------
            break;
        //===========================================================
        // *V2[0], *V2[1], and *V2[2] lie on P1 plane
        // The two triangles are coplanar
        case 5:
            //std::cout << "Case5\n";
            // SHOULD USE TWO DIMENSION INTERSECTION TEST FOR EFFICIENCY
            //--------------------------------------------------
            // Test if any point of Triangle#2 is inside Triangle#1
            result1 = FindIfAPointIsInATriangleOnTheSamePlane( p2, p1, q1, r1, tolerance );
            result2 = FindIfAPointIsInATriangleOnTheSamePlane( q2, p1, q1, r1, tolerance );
            result3 = FindIfAPointIsInATriangleOnTheSamePlane( r2, p1, q1, r1, tolerance );
            if ( result1 || result2 || result3 )    return true;
            else {
                //-----------------------------------------
                // Test if any point of Triangle#1 is inside Triangle#2
                result1 = FindIfAPointIsInATriangleOnTheSamePlane( p1, p2, q2, r2, tolerance );
                result2 = FindIfAPointIsInATriangleOnTheSamePlane( q1, p2, q2, r2, tolerance );
                result3 = FindIfAPointIsInATriangleOnTheSamePlane( r1, p2, q2, r2, tolerance );
                if ( result1 || result2 || result3 )    return true;
                else {
                    //--------------------------------
                    // Test if any edge of Triangle#2 intersects any edge of Triangle#1
                    //---------------------------
                    // Edge pq of Triangle#2
                    result1 = FindIntersectionLineSegmentLineSegment( 
                        p2, q2, p1, q1, para1, para2, touching, tolerance );
                    if ( result1 )  return true;
                    result2 = FindIntersectionLineSegmentLineSegment( 
                        p2, q2, q1, r1, para1, para2, touching, tolerance );
                    if ( result2 )  return true;
                    result3 = FindIntersectionLineSegmentLineSegment( 
                        p2, q2, r1, q1, para1, para2, touching, tolerance );
                    if ( result3 )  return true;
                    //---------------------------
                    // Edge qr of Triangle#2
                    result1 = FindIntersectionLineSegmentLineSegment( 
                        q2, r2, p1, q1, para1, para2, touching, tolerance );
                    if ( result1 )  return true;
                    result2 = FindIntersectionLineSegmentLineSegment( 
                        q2, r2, q1, r1, para1, para2, touching, tolerance );
                    if ( result2 )  return true;
                    result3 = FindIntersectionLineSegmentLineSegment( 
                        q2, r2, r1, q1, para1, para2, touching, tolerance );
                    if ( result3 )  return true;
                    //---------------------------
                    // Edge rp of Triangle#2
                    result1 = FindIntersectionLineSegmentLineSegment( 
                        r2, p2, p1, q1, para1, para2, touching, tolerance );
                    if ( result1 )  return true;
                    result2 = FindIntersectionLineSegmentLineSegment( 
                        r2, p2, q1, r1, para1, para2, touching, tolerance );
                    if ( result2 )  return true;
                    result3 = FindIntersectionLineSegmentLineSegment( 
                        r2, p2, r1, q1, para1, para2, touching, tolerance );
                    if ( result3 )  return true;
                }
            }
            //--------------------------------------------------
            return false;
            break;
        //===========================================================
        default:
            std::cout << "Shouldn't reach this case!" << std::endl;
            assert( false );
            break;
    }
    return false;
}
//-----------------------------------------------------------------------------

//*****************************************************************************
// Find if a circle intersects a Triangle
// (Assume the circle center lies on the triangle plane)
// I/P: a vector (center) and a scalar (radius) defining a circle on the triangle plane
// I/P: three vectors defining a triangle in counter-clockwise direction
// O/P: a closest distance (not implemented yet)
// REMARK: This fn is used by FindIntersectionSphereTriangle fn
//*****************************************************************************
template <typename T>
int CGMath<T>::FindIfACircleIntersectsATriangleOnTheSamePlane (
        Vector3<T> const & circleCenter,        
        T          const & circleRadius,        
        Vector3<T> const & p,                   
        Vector3<T> const & q,                   
        Vector3<T> const & r,                   
        T tolerance = Math<T>::ThresholdZero    
)
{
    //                \      /
    //                 \    /
    //                  \  /
    //                   \/
    //                   /\r
    //                  /  \
    // qResult := true /    \  pResult := true if line(p,pt) doesn't intersect line(q,r)
    //                / Tri  \
    // _____________ /________\_________________
    //              /p         \q
    //             /            \
    //            /              \
    //           /                \
    //          / rResult := true if line(r,pt) doesn't intersect line(p,q)
    //
    // qResult := true if line(q,pt) doesn't interscet line(r,p)
    //--------------------------------------------------------------------
    // CASE: the circle intersect a line of triangle
    //  - Find the closest point
    //  - The closest point must be within the circle radius from the circle center
    //  - Find if the point lies on the line of triangle (within the circle radius)
    Vector3<T> projPt;
    T ratio, distance;
    {
        FindProjectedPointOnALine( circleCenter, p, q, ratio, projPt, distance );
        if ( distance <= circleRadius ) {
            Vector3<T> pq( p-q );
            Vector3<T> ptp( (projPt-p) );
            if ( (ptp^pq).Length() < tolerance ) {
                T LEN = pq.Length() + sqrt( circleRadius*circleRadius - distance*distance );
                T len1 = ptp.Length();
                T len2 = (projPt-q).Length();
                if ( len1 <= LEN && len2 <= LEN ) {
                    return 1;
                }
                else {
                    return 0;
                }
            }
        }
    }
    {
        FindProjectedPointOnALine( circleCenter, q, r, ratio, projPt, distance );
        if ( distance <= circleRadius ) {
            Vector3<T> qr( q-r );
            Vector3<T> ptq( (projPt-q) );
            if ( (ptq^qr).Length() < tolerance ) {
                T LEN = qr.Length() + sqrt( circleRadius*circleRadius - distance*distance );
                T len1 = ptq.Length();
                T len2 = (projPt-r).Length();
                if ( len1 <= LEN && len2 <= LEN ) {
                    return 2;
                }
                else {
                    return 0;
                }
            }
        }
    }
    {
        FindProjectedPointOnALine( circleCenter, r, p, ratio, projPt, distance );
        if ( distance <= circleRadius ) {
            Vector3<T> rp( r-p );
            Vector3<T> ptr( (projPt-r) );
            if ( (ptr^rp).Length() < tolerance ) {
                T LEN = rp.Length() + sqrt( circleRadius*circleRadius - distance*distance );
                T len1 = ptr.Length();
                T len2 = (projPt-p).Length();
                if ( len1 <= LEN && len2 <= LEN ) {
                    return 3;
                }
                else {
                    return 0;
                }
            }
        }
    }
    //--------------------------------------------------------------------
    // CASE: the circle is completely inside or outside of the triangle
    //
    // Remark: The first case above made a complete sweep of the circle 
    //         inside and outside around the triangle.
    //
    //         center triangle is the original triangle
    //         outer  triangle is the triangle from sweeping the circle around outside the triangle
    //         inner  triangle is the triangle from sweeping the circle around inside  the triangle
    //
    //                 /\
    //                /  \
    //               / /\ \
    //              / /  \ \
    //             / / /\ \ \
    //            / / /__\ \ \
    //           / /________\ \ 
    //          /______________\
    //
    //         So if the circle is within circle radius from the triangle, 
    //         then the first case will return true.
    //
    // Therefore, the computation below will be similar to compute with the inner triangle.
    T t1, t2;
    bool pTouching, qTouching, rTouching;
    bool pResult = FindIntersectionLineSegmentLineSegment( p, circleCenter, q, r, t1, t2, pTouching, tolerance );
    bool qResult = FindIntersectionLineSegmentLineSegment( q, circleCenter, r, p, t1, t2, qTouching, tolerance );
    bool rResult = FindIntersectionLineSegmentLineSegment( r, circleCenter, p, q, t1, t2, rTouching, tolerance );
    //--------------------------------------------------------------------
    // Check Touchings first followed by Results
    if ( pTouching || qTouching || pTouching ) {
        //std::cout << "Touching: ";
        //std::cout << pTouching << "\t" << qTouching << "\t" << rTouching << "\n";
        return 11;
    }
    if ( pResult || qResult || rResult ) {
        //std::cout << "Not Touching: ";
        //std::cout << pResult << "\t" << qResult << "\t" << rResult << "\n";
        return 0;
    }
    //--------------------------------------------------------------------
    T lengthPtoQ = ( p - q ).Length();
    T lengthPtoR = ( p - r ).Length();
    T lengthQtoR = ( q - r ).Length();
    T lengthPointToP = (circleCenter - p).Length();
    T lengthPointToQ = (circleCenter - q).Length();
    T lengthPointToR = (circleCenter - r).Length();
    //--------------------------------------------------------------------
    if ( lengthPointToP > lengthPtoR && lengthPointToP > lengthPtoQ ) {
        return 0;
    }
    if ( lengthPointToQ > lengthPtoQ && lengthPointToQ > lengthQtoR ) {
        return 0;
    }
    if ( lengthPointToR > lengthPtoR && lengthPointToR > lengthQtoR ) {
        return 0;
    }
    //--------------------------------------------------------------------
    return 21;
}
//-----------------------------------------------------------------------------

//*****************************************************************************
// Find if a Point is in a Triangle
// (Assume the point lies on the triangle plane)
// I/P: a vector defining a point on the triangle plane
// I/P: three vectors defining a triangle in counter-clockwise dir
// O/P: a closest distance (not implemented yet)
// REMARK: This fn is used by FindIntersectionTriangleTriangle fn
//*****************************************************************************
template <typename T>
bool CGMath<T>::FindIfAPointIsInATriangleOnTheSamePlane (
        Vector3<T> const & pt,      // a point
        Vector3<T> const & p,       // a triangle
        Vector3<T> const & q, 
        Vector3<T> const & r, 
        T tolerance = Math<T>::ThresholdZero
)
{
    // Test if the vector formed by the cross product of vector (tri_pt1 - tri_pt0) and (pt - tri_pt0) 
    // and the vector formed by the cross product of vector (tri_pt1 - tri_pt0) and (tri_pt2 - tri_pt0) 
    // has the same direction.
    //
    //            * 1
    //           / \
    //          /   \
    //         /     \
    //        /   *   \
    //       /    pt   \
    //      /           \
    //   2 * -----------* 0
    //
    // The test has to be for all the three combinations of the triangle points.
    // If all test result in the same direction then the point is in the triangle.
    //*
    if ( ( ( (q-p)^(pt-p) ) * ( (q-p)^(r-p) ) ) < 0 )   return false;
    if ( ( ( (r-q)^(pt-q) ) * ( (r-q)^(p-q) ) ) < 0 )   return false;
    if ( ( ( (p-r)^(pt-r) ) * ( (p-r)^(q-r) ) ) < 0 )   return false;
    return true;
    //*/

    /*

    //                \      /
    //                 \    /
    //                  \  /
    //                   \/
    //                   /\r
    //                  /  \
    // qResult := true /    \  pResult := true if line(p,pt) doesn't intersect line(q,r)
    //                / Tri  \
    // _____________ /________\_________________
    //              /p         \q
    //             /            \
    //            /              \
    //           /                \
    //          / rResult := true if line(r,pt) doesn't intersect line(p,q)
    //
    // qResult := true if line(q,pt) doesn't interscet line(r,p)
    //--------------------------------------------------------------------
    // Case the point lies on a line of triangle
    {
        Vector3<T> pq( p-q );
        Vector3<T> ptp( (pt-p) );
        if ( (ptp^pq).Length() < tolerance ) {
            T LEN = pq.Length();
            T len1 = ptp.Length();
            T len2 = (pt-q).Length();
            if ( len1 <= LEN && len2 <= LEN )   return true;
            else                                return false;
        }
    }
    {
        Vector3<T> qr( q-r );
        Vector3<T> ptq( (pt-q) );
        if ( (ptq^qr).Length() < tolerance ) {
            T LEN = qr.Length();
            T len1 = ptq.Length();
            T len2 = (pt-r).Length();
            if ( len1 <= LEN && len2 <= LEN )   return true;
            else                                return false;
        }
    }
    {
        Vector3<T> rp( r-p );
        Vector3<T> ptr( (pt-r) );
        if ( (ptr^rp).Length() < tolerance ) {
            T LEN = rp.Length();
            T len1 = ptr.Length();
            T len2 = (pt-p).Length();
            if ( len1 <= LEN && len2 <= LEN )   return true;
            else                                return false;
        }
    }
    //--------------------------------------------------------------------
    // CASE: the point is inside or outside of the triangle
    T t1, t2;
    bool pTouching, qTouching, rTouching;
    bool pResult = FindIntersectionLineSegmentLineSegment( p, pt, q, r, t1, t2, pTouching, tolerance );
    bool qResult = FindIntersectionLineSegmentLineSegment( q, pt, r, p, t1, t2, qTouching, tolerance );
    bool rResult = FindIntersectionLineSegmentLineSegment( r, pt, p, q, t1, t2, rTouching, tolerance );
    //--------------------------------------------------------------------
    // Check Touchings first followed by Results
    if ( pTouching || qTouching || pTouching ) {
        //std::cout << "Touching: ";
        //std::cout << pTouching << "\t" << qTouching << "\t" << rTouching << "\n";
        return true;
    }
    if ( pResult || qResult || rResult ) {
        //std::cout << "Not Touching: ";
        //std::cout << pResult << "\t" << qResult << "\t" << rResult << "\n";
        return false;
    }
    //--------------------------------------------------------------------
    T lengthPtoQ = ( p - q ).Length();
    T lengthPtoR = ( p - r ).Length();
    T lengthQtoR = ( q - r ).Length();
    T lengthPointToP = (pt - p).Length();
    T lengthPointToQ = (pt - q).Length();
    T lengthPointToR = (pt - r).Length();
    //--------------------------------------------------------------------
    if ( lengthPointToP > lengthPtoR && lengthPointToP > lengthPtoQ ) {
        return false;
    }
    if ( lengthPointToQ > lengthPtoQ && lengthPointToQ > lengthQtoR ) {
        return false;
    }
    if ( lengthPointToR > lengthPtoR && lengthPointToR > lengthQtoR ) {
        return false;
    }
    //--------------------------------------------------------------------
    return true;
    //*/
}
//-----------------------------------------------------------------------------

//*****************************************************************************
// Find the centroid of a Triangle
//*****************************************************************************
template <typename T>
Vector3<T> CGMath<T>::CentroidOfTriangle (
        Vector3<T> const & p,       
        Vector3<T> const & q,       
        Vector3<T> const & r        
)
{
    return Vector3<T>( ( p + q + r ) / T(3) );
}
//-----------------------------------------------------------------------------

//*****************************************************************************
// Find if a Point is in a Triangle
// (No assumption whether the point lies on the triangle plane)
// I/P: a vector defining a point on the triangle plane
// I/P: three vectors defining a triangle in counter-clockwise dir
// O/P: a closest distance (not implemented yet)
// REMARK: This fn is used by FindIntersectionCylinderTriangle fn
//*****************************************************************************
template <typename T>
bool CGMath<T>::FindIfAPointIsInATriangle (
        Vector3<T> const & pt,      // a point
        Vector3<T> const & p,       // a triangle
        Vector3<T> const & q, 
        Vector3<T> const & r, 
        T tolerance = Math<T>::ThresholdZero
)
{
    //                \      /
    //                 \    /
    //                  \  /
    //                   \/
    //                   /\r
    //                  /  \
    // qResult := true /    \  pResult := true
    //                / Tri  \
    // _____________ /________\_________________
    //              /p         \q
    //             /            \
    //            /              \
    //           /                \
    //          / rResult := true  \
    //
    //--------------------------------------------------------------------
    // Find the normal of triangle#1
    Vector3<T> N = (q - p) ^ (r - p);
    N.Normalized();
    //--------------------------------------------------------------------
    // The point is not on the triangle if it doesn't lie on the triangle plane
    T Kplane = p * N;       // Plane Constant
    T ptP = N*pt - Kplane;
    if ( fabs( ptP ) > tolerance ) return false;

    FindIfAPointIsInATriangleOnTheSamePlane( pt, p, q, r, tolerance );
    // The code below were replaced by the above statement!
    /*
    //--------------------------------------------------------------------
    // Case the point lies on a line of triangle
    {
        Vector3<T> pq( p-q );
        Vector3<T> ptp( (pt-p) );
        if ( (ptp^pq).Length() < tolerance ) {
            T LEN = pq.Length();
            T len1 = ptp.Length();
            T len2 = (pt-q).Length();
            if ( len1 <= LEN && len2 <= LEN )   return true;
            else                                return false;
        }
    }
    {
        Vector3<T> qr( q-r );
        Vector3<T> ptq( (pt-q) );
        if ( (ptq^qr).Length() < tolerance ) {
            T LEN = qr.Length();
            T len1 = ptq.Length();
            T len2 = (pt-r).Length();
            if ( len1 <= LEN && len2 <= LEN )   return true;
            else                                return false;
        }
    }
    {
        Vector3<T> rp( r-p );
        Vector3<T> ptr( (pt-r) );
        if ( (ptr^rp).Length() < tolerance ) {
            T LEN = rp.Length();
            T len1 = ptr.Length();
            T len2 = (pt-p).Length();
            if ( len1 <= LEN && len2 <= LEN )   return true;
            else                                return false;
        }
    }
    T t1, t2;
    //--------------------------------------------------------------------
    bool pTouching, qTouching, rTouching;
    bool pResult = FindIntersectionLineSegmentLineSegment( p, pt, q, r, t1, t2, pTouching, tolerance );
    bool qResult = FindIntersectionLineSegmentLineSegment( q, pt, r, p, t1, t2, qTouching, tolerance );
    bool rResult = FindIntersectionLineSegmentLineSegment( r, pt, p, q, t1, t2, rTouching, tolerance );
    //--------------------------------------------------------------------
    //glPushMatrix();
    //  glBegin( GL_LINES );
    //      glColor3f( 0.15, 0.25, 0.4 );
    //      glVertex3fv( &p.GetDataFloat() );
    //      glVertex3fv( &pt.GetDataFloat() );
    //      glVertex3fv( &q.GetDataFloat() );
    //      glVertex3fv( &pt.GetDataFloat() );
    //      glVertex3fv( &r.GetDataFloat() );
    //      glVertex3fv( &pt.GetDataFloat() );
    //  glEnd();
    //glPopMatrix();
    //--------------------------------------------------------------------
    // Check Touchings first followed by Results
    if ( pTouching || qTouching || rTouching )  return true;
    if ( pResult || qResult || rResult )        return false;
    //--------------------------------------------------------------------
    T lengthPtoQ = ( p - q ).Length();
    T lengthPtoR = ( p - r ).Length();
    T lengthQtoR = ( q - r ).Length();
    T lengthPointToP = (pt - p).Length();
    T lengthPointToQ = (pt - q).Length();
    T lengthPointToR = (pt - r).Length();
    //--------------------------------------------------------------------
    if ( lengthPointToP > lengthPtoR && lengthPointToP > lengthPtoQ ) {
        return false;
    }
    if ( lengthPointToQ > lengthPtoQ && lengthPointToQ > lengthQtoR ) {
        return false;
    }
    if ( lengthPointToR > lengthPtoR && lengthPointToR > lengthQtoR ) {
        return false;
    }
    //--------------------------------------------------------------------
    return true;
    //*/
}
//-----------------------------------------------------------------------------
//*****************************************************************************
// Find Line Segment - Triangle Intersection
// I/P: two vectors defining the line segment
// I/P: three vectors defining the triangle in counter-clockwise dir
// O/P: the ratio of the point on the line that intersects the plane
// O/P: the projected point on the line that intersects the plane
// Return true if the line segment intersects the triangle.
// Otherwise, return false.
//*****************************************************************************
template <typename T>
bool CGMath<T>::FindIntersectionLineSegmentTriangle (
        Vector3<T> const & p1, Vector3<T> const & p2,   // I/P: a line segment
        Vector3<T> const & t1,      // I/P: the triangle
        Vector3<T> const & t2, 
        Vector3<T> const & t3, 
        T &            ratio,       // O/P: the ratio
        Vector3<T> &  projPt,       // O/P: the projected point
        T tolerance 
)
{
    //---------------------------------------------------------------
    // Find the normal of the triangle
    Vector3<T> N = (t2 - t1) ^ (t3 - t1);
    N.Normalized();
    //---------------------------------------------------------------
    // Find the point on the line segment that intersects the plane
    FindRatioOfPointOnLineThatIntersectPlane( 
        p1, p2, t1, N, ratio, projPt, tolerance );
    if ( ratio < 0.0 || 1.0 < ratio )   return false;
    //---------------------------------------------------------------
    // Find whether the projected point intersects the triangle
    return FindIfAPointIsInATriangleOnTheSamePlane( 
                projPt, t1, t2, t3, tolerance );
    //---------------------------------------------------------------
}
//-----------------------------------------------------------------------------
//*****************************************************************************
// Find Line Segment - Triangle Intersection
// I/P: two vectors defining the line segment
// I/P: three vectors defining the triangle in counter-clockwise dir
// O/P: the ratio of the point on the line that intersects the plane
// O/P: the projected point on the line that intersects the plane
// O/P: the intersection angle (in degrees) of the line with the plane's normal
// Return true if the line segment intersects the triangle.
// Otherwise, return false.
//*****************************************************************************
template <typename T>
bool CGMath<T>::FindIntersectionLineSegmentTriangle (
        Vector3<T> const & p1, Vector3<T> const & p2,   // I/P: a line segment
        Vector3<T> const & t1,      // I/P: the triangle
        Vector3<T> const & t2, 
        Vector3<T> const & t3, 
        T &           ratio,                // O/P: the ratio
        T &           intersectionAngle,    // O/P: the intersection angle (in degrees) of the line with the plane's normal
        Vector3<T> &  projPt,               // O/P: the projected point
        T tolerance 
)
{
    //---------------------------------------------------------------
    // Find the normal of the triangle
    Vector3<T> N = (t2 - t1) ^ (t3 - t1);
    N.Normalized();
    //---------------------------------------------------------------
    // Find the point on the line segment that intersects the plane
    FindRatioOfPointOnLineThatIntersectPlane( 
        p1, p2, t1, N, ratio, projPt, tolerance );
    if ( ratio < 0.0 || 1.0 < ratio )   return false;

    intersectionAngle = acos( N * (p2-p1).Normalized() ) * Math<T>::RAD_TO_DEG;
    //---------------------------------------------------------------
    // Find whether the projected point intersects the triangle
    return FindIfAPointIsInATriangleOnTheSamePlane( 
                projPt, t1, t2, t3, tolerance );
    //---------------------------------------------------------------
}
//-----------------------------------------------------------------------------
//=============================================================================

//=============================================================================
// Spline Fns
//=============================================================================
//*****************************************************************************
// CatmullRomSplineSegment Fn
// --------------------------
// I/P: 4 points p0, p1, p2, p3 and t in ranges [0,1]
// O/P: Catmull-Rom Spline Curve Pt 
//*****************************************************************************
//-----------------------------------------------------------------------------
template <typename T>
Vector3<T> CGMath<T>::CatmullRomSplinePt ( 
            Vector3<T> const & p0, Vector3<T> const & p1, 
            Vector3<T> const & p2, Vector3<T> const & p3, 
            T t
)
{
    return Vector3<T>( 0.5 * ( 
                (2*p1) +
                (-p0 + p2) * t +
                (2*p0 - 5*p1 + 4*p2 - p3) * t*t +
                (-p0 + 3*p1 - 3*p2 + p3 ) * t*t*t ) );
}
//-----------------------------------------------------------------------------
//=============================================================================

//=============================================================================
// Subdivision Fns
//=============================================================================
//*****************************************************************************
// SubdivideUniPolyInBBForm Fn (using de Casteljau algorithm)
// ---------------------------
    // I/P: P control points and degree
    //      # of points is degree + 1
    // I/P: t is parameter value in range [0-1]
    // O/P: left and right subdivision, each with degree + 1 points
    //      with the last point of the left subdivision is the same as 
    //      the first pt of the right subdivision
//*****************************************************************************
//-----------------------------------------------------------------------------
template <typename T>
void CGMath<T>::SubdivideUniPolyInBBForm (
                    Vector3<T> const P[],   // I/P: points on curve
                    int degree,             // I/P: degree
                    T   t,                  // I/P: parameter value
                    Vector3<T> L[],         // O/P: subdivision points
                    Vector3<T> R[]          // O/P: subdivision points
)
{
    L[0] = P[0];
    R[degree] = P[degree];
    Vector3<T> * Q = new Vector3<T>[degree];    // temporary storage
    for ( int i = 0; i < degree; ++i ) {
        Q[i] = (1-t)*P[i] + t*P[i+1];
    }
    L[1] = Q[0];
    int d = degree - 1;
    R[d] = Q[d];
    for ( ; d > 0; --d ) {
        for ( int i = 0; i < d; ++i ) {
            Q[i] = (1-t)*Q[i] + t*Q[i+1];
        }
        L[degree-d+1] = Q[0];
        R[d] = Q[d];
    }
    R[0] = Q[0];
    delete [] Q;
}
//-----------------------------------------------------------------------------
//=============================================================================

//=============================================================================
// Constraint Fns
//=============================================================================
//*****************************************************************************
// SolveAForConstCubicBezierCurveLength Fn
// ---------------------------------------
// Find unknown A that make the cubic Bezier curve defined by 4 ctrl pts 
// have the curve length equals to the LENGTH contraint with number of 
// subdivision constraint (number of subdivision)
//*****************************************************************************
//-----------------------------------------------------------------------------
template <typename T>
T CGMath<T>::SolveAForConstCubicBezierCurveLength ( 
                const Vector3<T> p0,    // I/P: ctrl pt #1
                const Vector3<T> p3,    // I/P: ctrl pt #4
                const Vector3<T> t0,    // I/P: tangent at pt #1
                const Vector3<T> t3,    // I/P: tangent at pt #4
                int   iNumSubdivision,  // I/P: #subdivision constraint
                T     tLength,          // I/P: length constraint
                T     tolerance,        // I/P: tolerance
                Vector3<T> * pPtOnCurve // O/P: ptr to points on curve
)
{
    if ( (p0 - p3).Length() >= tLength ) return 0;
    //---------------------------------------------------------------
    // Subdivide Bezier curve "iNumSubdivision" times
    int iT = static_cast<int>( pow( 2, iNumSubdivision ) );
    int iTotalPts = 4*iT - iT + 1;
    //std::cout << "iTotalPts: " << iTotalPts << "\n";
    Vector3<T> * P = new Vector3<T>[iTotalPts];
    Vector3<T> * Q = new Vector3<T>[iTotalPts];
    Vector3<T> * R; // = new Vector3<T>[iTotalPts];
    //---------------------------------------------------------------
    T tUpper = 1000;    // upper range limit
    T tLower = 0;       // lower range limit
    T A = (tUpper + tLower) / static_cast<T>( 2.0 );
    T tError;
    T tCurveLength = 0;
    P[0] = p0;
    //---------------------------------------------------------------
    // Constraint the curve length
    //---------------------------------------------------------------
    while ( true ) {
        P[1] = p0 + A*t0;
        P[2] = p3 + A*t3;
        P[3] = p3;
        //----------------------------------------------------------------
        for ( int i = 0; i < iNumSubdivision; ++i ) {
            for ( int c = 0; c < pow(2,i)*3; c+=3 ) {
                SubdivideUniPolyInBBForm( &P[c], 3, 0.5, &Q[c*2], &Q[c*2+3] );
            }
            // Swap
            R = P;
            P = Q;
            Q = R;
        }
        //----------------------------------------------------------------
        tCurveLength = 0;
        for ( int i = 1; i < iTotalPts; ++i ) {
            tCurveLength += ( P[i] - P[i-1] ).Length();
        }
        //----------------------------------------------------------------
        //std::cout << "CurveLength = " << tCurveLength << "\n";
        tError = tCurveLength - tLength;
        if      ( tError > tolerance ) {
            tUpper = A;
            A = static_cast<T>( (tUpper + tLower) / 2.0 );
        }
        else if ( tError < -tolerance ) {
            tLower = A;
            A = static_cast<T>( (tUpper + tLower) / 2.0 );
        }
        else {
            if ( pPtOnCurve ) {
                for ( int i = 0; i < iTotalPts; ++i ) {
                    pPtOnCurve[i] = P[i];
                }
            }
            delete [] P;
            delete [] Q;
            //std::cout << "tCurveLength: " << tCurveLength << std::endl;
            return A;
        }
        //std::cout << "upper: " << tUpper 
        //  << "  A: " << A 
        //  << "  lower: " << tLower
        //  << "\n";
        /*
        if ( A == 0 ) {
            delete [] P;
            delete [] Q;
            return A;
        }
        //*/
    }
    //---------------------------------------------------------------
    //return A;
}
//-----------------------------------------------------------------------------
//=============================================================================

//=============================================================================
// Constraint Fns
//=============================================================================
//*****************************************************************************
// TransformALinkToWorldCoordinates Fn
// -----------------------------------
// vector v = p1 - p0 will be transfromed to align with x-axis of the world 
// coordinates.
// O/P: theta and phi angles for z-rotation and x'-rotation respectively
//*****************************************************************************
//-----------------------------------------------------------------------------
template <typename T>
Vector3<T> CGMath<T>::TransformALinkToWorldXAxis ( 
                const Vector3<T> V, // I/P: V
                T & Cy,             // O/P: cosine(theta) of y-rotation
                T & Sy,             // O/P: sine(theta) of y-rotation
                T & Cz,             // O/P: cosine(phi) of z-rotation
                T & Sz              // O/P: sine(phi) of z-rotation
                // return   Rz * Ry * V;
                // where Rz = [  Cz -Sz  0  ]
                //            [  Sz  Cz  0  ]
                //            [  0   0   1  ]
                // and   Ry = [  Cy  0   Sy ]
                //            [  0   1   0  ]
                //            [ -Sy  0   Cy ]
)
{
    //===============================================================
    // RECORD angles in Cy, Sy, Cz, and Sz
    // since it takes care of all quadrants by keeping them separately
    // instead of keeping just theta and phi angles
    //---------------------------------------------------------------

    //===============================================================
    // Variables
    //---------------------------------------------------------------
    //Real c, s;
    Real D;
    Matrix3x3<Real> Ry, Rz;
    Vector3<T> OP = V;
    //===============================================================
    // ROTATE r to x-axis
    //---------------------------------------------------------------
    // Rotate OP to xy-plane
    D = sqrt( OP[0]*OP[0] + OP[2]*OP[2] );
    if ( D > 0 ) {
        Cy = OP[0] / D;
        Sy = OP[2] / D;
    }
    else {
        std::cout << "ERROR: D length is zero!\n";
        exit(-1);
    }
    Ry = Matrix3x3<Real>(
             Cy,  0,  Sy,
             0,   1,  0,
            -Sy,  0,  Cy
    );
    OP = Ry * OP;
    // Rotate it again to x-axis
    D = sqrt( OP[0]*OP[0] + OP[1]*OP[1] );
    if ( D > 0 ) {
        Cz =  OP[0] / D;
        Sz = -OP[1] / D;
    }
    else {
        std::cout << "ERROR: D length is zero!\n";
        exit(-1);
    }
    Rz = Matrix3x3<Real>(
            Cz, -Sz, 0,
            Sz,  Cz, 0,
            0,   0,  1
    );
    OP = Rz * OP;
    //*
    std::cout << "V: " << V << " with length of " << V.Length() << "\n";
    std::cout << "OP: " << OP << "\n";
    //*/
    //---------------------------------------------------------------
    return OP;
}
//-----------------------------------------------------------------------------




//=============================================================================
// Helper Function for Collision Detection and Response
//-----------------------------------------------------------------------------
template <typename T>
bool CGMath<T>::CD_SphereAtOrigin_vs_Point (
        T   sphere_radius,                  
        Vector3<T> const & point,           
        Vector3<T> & shortestDistanceOut    
)
{
    //---------------------------------------------------------------
    // The sphere has a radius and centers at the origin
    T   ptRadius = point.Length();
    //-----------------------------------------------------
    // If the point is within the sphere radius
    if ( ptRadius <= sphere_radius ) {

        // Find the shortest distance from the point to the sphere surface
        T difRadius = sphere_radius - ptRadius;
        // If the point is at the origin (degenerate case)
        if ( difRadius >= sphere_radius ) {
            shortestDistanceOut.SetXYZ( 0, sphere_radius, 0 );
        }
        else {
            shortestDistanceOut = point.GetUnit() * difRadius;
        }

        return true;
    }
    //---------------------------------------------------------------
    return false;
}
// END: TEST_SphereAtOrigin_vs_Point(...)
//-----------------------------------------------------------------------------


//-----------------------------------------------------------------------------
template <typename T>
bool CGMath<T>::CD_CylinderAtOrigin_vs_Point (
        T   cylinder_radius,                
        T   cylinder_height,                
        Vector3<T> const & point,           
        Vector3<T> & shortestDistanceOut    
)
{
    //---------------------------------------------------------------
    // In the local coordinate of the bounding cylinder, 
    // the cylinder axis is parallel to z-axis 
    // where it is centered at the origin.
    // The cylinder has radius and height.
    // Its height is measured from -z and +z axis
    // Its center is situated between height/2 in -z and +z axis.
    T   halfHeight = cylinder_height / T(2.0);
    //-----------------------------------------------------
    // If the point is within the cylinder height
    if ( -halfHeight <= point[2] && point[2] <= halfHeight ) {
        // If the point is within the cylinder radius
        T   ptRadius = sqrt( point[0]*point[0] + point[1]*point[1] );
        T difRadius = cylinder_radius - ptRadius;
        if ( difRadius >= 0 ) {
            // Find the shortest distance from the point to the cylinder surface
            T difHeight = halfHeight - fabs( point[2] );
            if ( difRadius <= difHeight ) {
                // If the point is on the z-axis (degenerate case)
                // I.e., point[0] and point[1] are zeroes (same as ptRadius == cylinder_radius)
                if ( ptRadius == cylinder_radius ) {
                    shortestDistanceOut.SetXYZ( 0, cylinder_radius, 0 );
                }
                // If the point is NOT on the z-axis
                else {
                    Vector2<T> difDirection = Vector2<T>( point[0], point[1] );
                    difDirection.Normalized();
                    difDirection *= difRadius;
                    shortestDistanceOut.SetXYZ( difDirection[0], difDirection[1], 0 );
                }
            }
            else {
                shortestDistanceOut.SetXYZ( 0, 0, point[2] >= 0 ? difHeight : -difHeight );
            }

            return true;
        }
    }
    //---------------------------------------------------------------
    return false;
}
// END: TEST_CylinderAtOrigin_vs_Point(...)
//-----------------------------------------------------------------------------


//-----------------------------------------------------------------------------
template <typename T>
bool CGMath<T>::CD_CylinderAtOrigin_vs_Sphere (
        T   cylinder_radius,                
        T   cylinder_height,                
        Vector3<T> const & sphereCenter,    
        T                  sphereRadius     
)
{
    //---------------------------------------------------------------
    // In the local coordinate of the bounding cylinder, 
    // the cylinder axis is parallel to z-axis 
    // where it is centered at the origin.
    // The cylinder has radius and height.
    // Its height is measured from -z and +z axis
    // Its center is situated between height/2 in -z and +z axis.
    T   halfHeight = cylinder_height / T(2.0);
    //-----------------------------------------------------
    // If the point is within the cylinder height
    if ( -halfHeight <= sphereCenter[2] && sphereCenter[2] <= halfHeight ) {
        // If the point is within the cylinder radius
        T   ptRadius = sqrt( sphereCenter[0]*sphereCenter[0] + sphereCenter[1]*sphereCenter[1] );
        T difRadius = cylinder_radius + sphereRadius - ptRadius;
        if ( difRadius >= 0 ) {
            return true;
        }
    }
    // If the point is NOT within the cylinder height
    else {
        // If the sphere center is within (halfHeight + sphereRadius)
        //
        //      sphere center o---------*--+ sphere boundary
        //                    |         |
        //                    |         h
        //                    |         |
        //                    |<--len-->*-----.-----*  top of cylinder
        //                    +         |           |
        //                              |           |
        //                    |<--proj_len--->|     |
        //                              |           |
        //
        //    len = sqrt( sphere_radius - h^2 )
        //    proj_len = sqrt( sphere_center[0]^2 + sphere_center[1]^2 )
        //
        //    If len is greater than or equal to ( proj_len - cylinder_radius ), 
        //    the sphere intersects or touches the cylinder.
        //
        if ( halfHeight < sphereCenter[2] && sphereCenter[2] <= halfHeight + sphereRadius ) {           // Top
            T height = sphereCenter[2] - halfHeight;
            T ls = sphereRadius*sphereRadius - height*height;
            if ( ls < 0 )   return false;   // the height is too far
            T len = sqrt( ls );
            T proj_len = sqrt( sphereCenter[0]*sphereCenter[0] + sphereCenter[1]*sphereCenter[1] );
            T dif = len - ( proj_len - cylinder_radius );
            if ( dif >= 0 ) {
                return true;
            }
        }
        else if ( -halfHeight > sphereCenter[2] && sphereCenter[2] >= -halfHeight - sphereRadius ) {        // Bottom
            T height = -sphereCenter[2] - halfHeight;
            T ls = sphereRadius*sphereRadius - height*height;
            if ( ls < 0 )   return false;   // the height is too far
            T len = sqrt( ls );
            T proj_len = sqrt( sphereCenter[0]*sphereCenter[0] + sphereCenter[1]*sphereCenter[1] );
            T dif = len - ( proj_len - cylinder_radius );
            if ( dif >= 0 ) {
                return true;
            }
        }
    }
    //---------------------------------------------------------------
    return false;
}
// END: TEST_CylinderAtOrigin_vs_Sphere(...)
//-----------------------------------------------------------------------------

    
//-----------------------------------------------------------------------------
template <typename T>
bool CGMath<T>::CD_CylinderAtOrigin_vs_Sphere (
        T   cylinder_radius,                
        T   cylinder_height,                
        Vector3<T> const & sphereCenter,    
        T                  sphereRadius,    
        Vector3<T> & shortestDistanceOut    
)
{
    //---------------------------------------------------------------
    // In the local coordinate of the bounding cylinder, 
    // the cylinder axis is parallel to z-axis 
    // where it is centered at the origin.
    // The cylinder has radius and height.
    // Its height is measured from -z and +z axis
    // Its center is situated between height/2 in -z and +z axis.
    T   halfHeight = cylinder_height / 2.0;
    //-----------------------------------------------------
    // If the point is within the cylinder height
    if ( -halfHeight <= sphereCenter[2] && sphereCenter[2] <= halfHeight ) {
        // If the point is within the cylinder radius
        T   ptRadius = sqrt( sphereCenter[0]*sphereCenter[0] + sphereCenter[1]*sphereCenter[1] );
        T difRadius = cylinder_radius + sphereRadius - ptRadius;
        if ( difRadius >= 0 ) {

            // Find the shortest distance from the point to the cylinder surface
            T difHeight = halfHeight - fabs( sphereCenter[2] );
            // If the sphere center is outside the cylinder
            if ( difRadius - sphereRadius <= difHeight ) {
                // If the point is on the z-axis (degenerate case)
                // I.e., point[0] and point[1] are zeroes (same as ptRadius == cylinder_radius)
                if ( ptRadius == cylinder_radius ) {
                    shortestDistanceOut.SetXYZ( 0, cylinder_radius, 0 );
                }
                // If the point is NOT on the z-axis
                else {
                    Vector2<T> difDirection = Vector2<T>( sphereCenter[0], sphereCenter[1] );
                    difDirection.Normalized();
                    difDirection *= difRadius;
                    shortestDistanceOut.SetXYZ( difDirection[0], difDirection[1], 0 );
                }
            }
            else {
                shortestDistanceOut.SetXYZ( 0, 0, sphereCenter[2] >= 0 ? difHeight : -difHeight );
            }

            return true;
        }
    }
    // If the point is NOT within the cylinder height
    else {
        // If the sphere center is within (halfHeight + sphereRadius)
        //
        //      sphere center o---------*--+ sphere boundary
        //                    |         |
        //                    |         h
        //                    |         |
        //                    |<--len-->*-----.-----*  top of cylinder
        //                    +         |           |
        //                              |           |
        //                    |<--proj_len--->|     |
        //                              |           |
        //
        //    len = sqrt( sphere_radius - h^2 )
        //    proj_len = sqrt( sphere_center[0]^2 + sphere_center[1]^2 )
        //
        //    If len is greater than or equal to ( proj_len - cylinder_radius ), 
        //    the sphere intersects or touches the cylinder.
        //
        if ( halfHeight < sphereCenter[2] && sphereCenter[2] <= halfHeight + sphereRadius ) {           // Top
            T height = sphereCenter[2] - halfHeight;
            T ls = sphereRadius*sphereRadius - height*height;
            if ( ls < 0 )   return false;   // the height is too far
            T len = sqrt( ls );
            T proj_len = sqrt( sphereCenter[0]*sphereCenter[0] + sphereCenter[1]*sphereCenter[1] );
            T dif = len - ( proj_len - cylinder_radius );
            if ( dif >= 0 ) {

                // Find the shortest length
                Vector2<T> difDirection = Vector2<T>( sphereCenter[0], sphereCenter[1] );
                difDirection.Normalized();
                difDirection *= dif;
                shortestDistanceOut.SetXYZ( difDirection[0], difDirection[1], 0 );

                return true;
            }
        }
        else if ( -halfHeight > sphereCenter[2] && sphereCenter[2] >= -halfHeight - sphereRadius ) {        // Bottom
            T height = -sphereCenter[2] - halfHeight;
            T ls = sphereRadius*sphereRadius - height*height;
            if ( ls < 0 )   return false;   // the height is too far
            T len = sqrt( ls );
            T proj_len = sqrt( sphereCenter[0]*sphereCenter[0] + sphereCenter[1]*sphereCenter[1] );
            T dif = len - ( proj_len - cylinder_radius );
            if ( dif >= 0 ) {

                // Find the shortest length
                Vector2<T> difDirection = Vector2<T>( sphereCenter[0], sphereCenter[1] );
                difDirection.Normalized();
                difDirection *= dif;
                shortestDistanceOut.SetXYZ( difDirection[0], difDirection[1], 0 );

                return true;
            }
        }
    }
    //---------------------------------------------------------------
    return false;
}
// END: TEST_CylinderAtOrigin_vs_Sphere(...)
//-----------------------------------------------------------------------------
// Helper Function for Collision Detection and Response
//=============================================================================




//=============================================================================
// Sphere-Sphere Intersection Test
//-----------------------------------------------------------------------------
template <typename T>
T CGMath<T>::IntersectionDistance_Sphere_vs_Sphere (
        Vector3<T> const & centerA,         
        T radiusA,                          
        Vector3<T> const & centerB,         
        T radiusB,                          
        Vector3<T> & shortestDistanceOut    
)
{
    // Use centerB - centerA, so that shortestDistanceOut *= length; can be used 
    // instead of shortestDistanceOut *= -length;
    shortestDistanceOut = centerB - centerA;
    T length = shortestDistanceOut.Length() - ( radiusA + radiusB );
    shortestDistanceOut.Normalized();
    shortestDistanceOut *= length;
    return length;
}
//-----------------------------------------------------------------------------
template <typename T>
T CGMath<T>::IntersectionDistance_Sphere_vs_Sphere (
        Vector3<T> const & centerA,         
        T radiusA,                          
        Vector3<T> const & centerB,         
        T radiusB,                          
        Matrix4x4<T> const & transformB,    
        Vector3<T> & shortestDistanceOut    
)
{
    // Use centerB - centerA, so that shortestDistanceOut *= length; can be used 
    // instead of shortestDistanceOut *= -length;
    shortestDistanceOut = transformB * Vector4<T>(centerB).GetVector3() - centerA;
    T length = shortestDistanceOut.Length() - ( radiusA + radiusB );
    shortestDistanceOut.Normalized();
    shortestDistanceOut *= length;
    return length;
}
//-----------------------------------------------------------------------------
template <typename T>
T CGMath<T>::IntersectionDistance_Sphere_vs_Sphere (
        Vector3<T> const & centerA,         
        T radiusA,                          
        Matrix4x4<T> const & transformA,    
        Vector3<T> const & centerB,         
        T radiusB,                          
        Vector3<T> & shortestDistanceOut    
)
{
    // Use centerB - centerA, so that shortestDistanceOut *= length; can be used 
    // instead of shortestDistanceOut *= -length;
    shortestDistanceOut = centerB - transformA * Vector4<T>(centerA).GetVector3();
    T length = shortestDistanceOut.Length() - ( radiusA + radiusB );
    shortestDistanceOut.Normalized();
    shortestDistanceOut *= length;
    return length;
}
//-----------------------------------------------------------------------------
template <typename T>
T CGMath<T>::IntersectionDistance_Sphere_vs_Sphere (
        Vector3<T> const & centerA,         
        T radiusA,                          
        Matrix4x4<T> const & transformA,    
        Vector3<T> const & centerB,         
        T radiusB,                          
        Matrix4x4<T> const & transformB,    
        Vector3<T> & shortestDistanceOut    
)
{
    // Use centerB - centerA, so that shortestDistanceOut *= length; can be used 
    // instead of shortestDistanceOut *= -length;
    shortestDistanceOut = transformB * Vector4<T>(centerB).GetVector3() - transformA * Vector4<T>(centerA).GetVector3();
    T length = shortestDistanceOut.Length() - ( radiusA + radiusB );
    shortestDistanceOut.Normalized();
    shortestDistanceOut *= length;
    return length;
}
//-----------------------------------------------------------------------------
// Sphere-Sphere Intersection Test
//=============================================================================




//=============================================================================
// Interpolation Functions
//-----------------------------------------------------------------------------
template <typename T>
T CGMath<T>::LinearInterpolation (
        T val1,     
        T val2,     
        T percent   
)
{
    return  val1 + percent*(val2-val1);
}
//-----------------------------------------------------------------------------
template <typename T>
Vector2<T> CGMath<T>::LinearInterpolation (
        Vector2<T> val1,    
        Vector2<T> val2,    
        T percent           
)
{
    return  Vector2<T>( 
        val1[0] + percent*(val2[0]-val1[0]), 
        val1[1] + percent*(val2[1]-val1[1]) 
    );
}
//-----------------------------------------------------------------------------
template <typename T>
Vector3<T> CGMath<T>::LinearInterpolation (
        Vector3<T> val1,    
        Vector3<T> val2,    
        T percent           
)
{
    return  Vector3<T>( 
        val1[0] + percent*(val2[0]-val1[0]), 
        val1[1] + percent*(val2[1]-val1[1]), 
        val1[2] + percent*(val2[2]-val1[2]) 
    );
}
//-----------------------------------------------------------------------------
template <typename T>
Vector4<T> CGMath<T>::LinearInterpolation (
        Vector4<T> val1,    
        Vector4<T> val2,    
        T percent           
)
{
    return  Vector4<T>( 
        val1[0] + percent*(val2[0]-val1[0]), 
        val1[1] + percent*(val2[1]-val1[1]), 
        val1[2] + percent*(val2[2]-val1[2]), 
        val1[3] + percent*(val2[3]-val1[3]) 
    );
}
//-----------------------------------------------------------------------------
// Interpolation Functions
//=============================================================================




//=============================================================================
END_NAMESPACE_TAPs
//34567890123456789012345678901234567890123456789012345678901234567890123456789
//--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----