CGeometry.h

Go to the documentation of this file.

//==============================================================================
/*
    Software License Agreement (BSD License)
    Copyright (c) 2003-2016, CHAI3D.
    (www.chai3d.org)

    All rights reserved.

    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions
    are met:

    * Redistributions of source code must retain the above copyright
    notice, this list of conditions and the following disclaimer.

    * Redistributions in binary form must reproduce the above
    copyright notice, this list of conditions and the following
    disclaimer in the documentation and/or other materials provided
    with the distribution.

    * Neither the name of CHAI3D nor the names of its contributors may
    be used to endorse or promote products derived from this software
    without specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
    COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
    POSSIBILITY OF SUCH DAMAGE. 

    \author    <http://www.chai3d.org>
    \author    Francois Conti
    \version   3.2.0 $Rev: 2171 $
*/
//==============================================================================

//------------------------------------------------------------------------------
#ifndef CGeometryH
#define CGeometryH
//------------------------------------------------------------------------------
#include "math/CPolySolver.h"
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
namespace chai3d {
//------------------------------------------------------------------------------

//==============================================================================
//==============================================================================

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------


//==============================================================================
//==============================================================================
inline double cTriangleArea(const cVector3d& a_vertex0,
                            const cVector3d& a_vertex1,
                            const cVector3d& a_vertex2)
{
    cVector3d u, v;
    a_vertex1.subr(a_vertex0, u);
    a_vertex2.subr(a_vertex0, v);
    return (0.5 * (cCross(u,v).length()));
}


//==============================================================================
//==============================================================================
inline cVector3d cProjectPointOnPlane(const cVector3d& a_point,
                                      const cVector3d& a_planePoint,
                                      const cVector3d& a_planeNormal)
{
    cVector3d n = a_planeNormal;

    // compute a projection matrix
    cMatrix3d projectionMatrix;
    projectionMatrix.set(
       (n(1) * n(1)) + (n(2) * n(2)), -(n(0) * n(1) ),-(n(0) * n(2)),
      -(n(1) * n(0)), (n(0) * n(0)) + (n(2) * n(2) ),-(n(1) * n(2)),
      -(n(2) * n(0)),-(n(2) * n(1)), (n(0) * n(0) ) + (n(1) * n(1))
      );

    // project point on plane and return projected point.
    cVector3d point;
    a_point.subr(a_planePoint, point);
    projectionMatrix.mul(point);
    point.add(a_planePoint);

    // return result
    return (point);
}


//==============================================================================
//==============================================================================
inline cVector3d cProjectPointOnPlane(const cVector3d& a_point,
                                      const cVector3d& a_planePoint0,
                                      const cVector3d& a_planePoint1,
                                      const cVector3d& a_planePoint2)
{
    // create two vectors from the three input points lying in the projection plane.
    cVector3d v01, v02;
    a_planePoint1.subr(a_planePoint0, v01);
    a_planePoint2.subr(a_planePoint0, v02);

    // compute the normal vector of the plane
    cVector3d n;
    v01.crossr(v02, n);
    n.normalize();

    // return projected point
    return (cProjectPointOnPlane(a_point,a_planePoint0,n));
}


//==============================================================================
//==============================================================================
inline void cProjectPointOnPlane(const cVector3d& a_point,
                                 const cVector3d& a_planePoint0,
                                 const cVector3d& a_planePoint1,
                                 const cVector3d& a_planePoint2,
                                 double& a_r01,
                                 double& a_r02)
{
    cVector3d v01 = cSub(a_planePoint1, a_planePoint0);
    cVector3d v02 = cSub(a_planePoint2, a_planePoint0);
    cVector3d point = cSub(a_point, a_planePoint0);

    // matrix
    double m00 = v01(0) * v01(0) + v01(1) * v01(1) + v01(2) * v01(2);
    double m01 = v01(0) * v02(0) + v01(1) * v02(1) + v01(2) * v02(2);
    double m10 = m01;
    double m11 = v02(0) * v02(0) + v02(1) * v02(1) + v02(2) * v02(2);
    double det = m00 * m11 - m10 * m01;

    // vector
    double vm0 = v01(0) * point(0) + v01(1) * point(1) + v01(2) * point(2);
    double vm1 = v02(0) * point(0) + v02(1) * point(1) + v02(2) * point(2);

    // inverse
    a_r01 = (1.0 / det) * ( m11 * vm0 - m01 * vm1);
    a_r02 = (1.0 / det) * (-m10 * vm0 + m00 * vm1);
}


//==============================================================================
//==============================================================================
inline cVector3d cProjectPointOnLine(const cVector3d& a_point,
                                     const cVector3d& a_pointOnLine,
                                     const cVector3d& a_directionOfLine)
{
    // temp variable
    cVector3d point, result;

    // compute norm of line direction
    double lengthDirSq = a_directionOfLine.lengthsq();

    if (lengthDirSq > 0.0)
    {
        a_point.subr(a_pointOnLine, point);

        a_directionOfLine.mulr( (point.dot(a_directionOfLine) / (lengthDirSq)),
                                result);

        result.add(a_pointOnLine);
        return (result);
    }
    else
    {
        return (a_pointOnLine);
    }
}


//==============================================================================
//==============================================================================
inline cVector3d cProjectPointOnSegment(const cVector3d& a_point,
                                        const cVector3d& a_segmentPointA,
                                        const cVector3d& a_segmentPointB)
{
    // if both points are equal
    if (a_segmentPointA.equals(a_segmentPointB))
    {
        return (a_segmentPointA);
    }

    // compute line
    cVector3d segmentAB;
    a_segmentPointB.subr(a_segmentPointA, segmentAB);

    // project tool onto segment
    cVector3d projection = cProjectPointOnLine(a_point, a_segmentPointA, segmentAB);

    double distanceAB = segmentAB.lengthsq();
    double distanceAP = cDistanceSq(projection, a_segmentPointA);
    double distanceBP = cDistanceSq(projection, a_segmentPointB);

    if (distanceAP > distanceAB)
    {
        return(a_segmentPointB);
    }
    else
    if (distanceBP > distanceAB)
    {
        return(a_segmentPointA);
    }
    else
    {
        return(projection);
    }
}


//==============================================================================
//==============================================================================
inline cVector3d cProjectPointOnDiskXY (const cVector3d& a_point,
                                        const double& a_height,
                                        const double& a_radius)
{
    cVector3d center(0.0, 0.0, a_height);
    cVector3d pointProj(a_point);

    pointProj(2)  = a_height;

    cVector3d ray = cSub (pointProj, center);

    if (ray.length() < a_radius) return (pointProj);
    else
    {
        ray.normalize();
        return center + a_radius * ray;
    }
}


//==============================================================================
//==============================================================================
inline cVector3d cProjectPointOnTriangle (const cVector3d& a_point,
                                          const cVector3d& a_vertex0,
                                          const cVector3d& a_vertex1,
                                          const cVector3d& a_vertex2)
{
    cVector3d result;

    // project point on plane
    double r01, r02;
    cProjectPointOnPlane(a_point, a_vertex0, a_vertex1, a_vertex2, r01, r02);

    // check if point in located inside triangle
    if ((r01 >= 0.0) && (r02 >= 0.0) && (r01 <= 1.0) && (r02 <= 1.0) && ((r01 + r02) <= 1.0))
    {
        result = a_vertex0 + r01 * (a_vertex1 - a_vertex0) + r02 * (a_vertex2 - a_vertex0);
    }
    else
    {
        // project point on edge 01
        cVector3d p01 = cProjectPointOnSegment(a_point, a_vertex0, a_vertex1);

        // project point on edge 02
        cVector3d p02 = cProjectPointOnSegment(a_point, a_vertex0, a_vertex2);

        // project point on edge 12
        cVector3d p12 = cProjectPointOnSegment(a_point, a_vertex1, a_vertex2);

        // check for nearest point
        double d01 = cDistanceSq(a_point, p01);
        double d02 = cDistanceSq(a_point, p02);
        double d12 = cDistanceSq(a_point, p12);

        if (d01 < d02)
        {
            if (d01 < d12)
            {
                result = p01;
            }
            else
            {
                result = p12;
            }
        }
        else
        {
            if (d02 < d12)
            {
                result = p02;
            }
            else
            {
                result = p12;
            }
        }
    }

    return (result);
}


//==============================================================================
//==============================================================================
inline cVector3d cProject(const cVector3d& a_vector0, 
                          const cVector3d& a_vector1)
{
    cVector3d result;

    // compute projection
    double lengthSq = a_vector1.lengthsq();
    a_vector1.mulr( (a_vector0.dot(a_vector1) / (lengthSq)), result);

    // return result
    return (result);
}


//==============================================================================
//==============================================================================
inline double cDistanceToLine(const cVector3d& a_point, 
                              const cVector3d& a_linePoint1,
                              const cVector3d& a_linePoint2)
{
    cVector3d point = cProjectPointOnLine(a_point, a_linePoint1, cSub(a_linePoint2, a_linePoint1));
    return (cDistance(a_point, point));
}


//==============================================================================
//==============================================================================
inline cVector3d cComputeSurfaceNormal(const cVector3d& a_surfacePoint0,
                                       const cVector3d& a_surfacePoint1, 
                                       const cVector3d& a_surfacePoint2)
{
    cVector3d v01, v02, result;

    // compute surface normal
    a_surfacePoint1.subr(a_surfacePoint0, v01);
    a_surfacePoint2.subr(a_surfacePoint0, v02);
    v01.normalize();
    v02.normalize();
    v01.crossr(v02, result);
    result.normalize();

    // return result
    return (result);
}


//==============================================================================
//==============================================================================
inline bool cBoxContains(const cVector3d& a_point,
                         const cVector3d& a_boxMin,
                         const cVector3d& a_boxMax)
{
    if ((a_point(0)  >= a_boxMin(0) ) && (a_point(0)  <= a_boxMax(0)) &&
        (a_point(1)  >= a_boxMin(1) ) && (a_point(1)  <= a_boxMax(1)) &&
        (a_point(2)  >= a_boxMin(2) ) && (a_point(2)  <= a_boxMax(2)))
    {
        return (true);
    }
    else
    {
        return (false);
    }
}


//==============================================================================
//==============================================================================
inline int cIntersectionSegmentPlane(const cVector3d& a_segmentPointA,
                                     const cVector3d& a_segmentPointB,
                                     const cVector3d& a_planePos,
                                     const cVector3d& a_planeNormal,
                                     cVector3d& a_collisionPoint,
                                     cVector3d& a_collisionNormal)
{
    cVector3d d = a_segmentPointB - a_segmentPointA;
    cVector3d p = a_segmentPointA;
    double length = d.length();

    // sanity check
    if (cZero(length)) 
    { 
        return (0);
    }

    // normalize d
    d.mul(1.0 / length);

    // compute intersection between segment and disk plane
    double c[2];
    double s[1];

    c[0] = a_planeNormal(0)*p(0) - a_planeNormal(0)*a_planePos(0) +
           a_planeNormal(1)*p(1) - a_planeNormal(1)*a_planePos(1) +
           a_planeNormal(2)*p(2) - a_planeNormal(2)*a_planePos(2);
    c[1] = a_planeNormal(0)*d(0) + a_planeNormal(1)*d(1) + a_planeNormal(2)*d(2);

    int num = cSolveLinear(c, s);

    if (num == 0)
    {
        return (0);
    }
    else
    {
        if ((s[0] >= 0.0) && (s[0] <= length))
        {
            a_collisionPoint = p + s[0] * d;
            if (cDot(a_planeNormal, cSub(a_segmentPointA, a_collisionPoint)) >= 0.0)
            {
                a_collisionNormal = a_planeNormal;
            }
            else
            {
                a_collisionNormal =-a_planeNormal;
            }
            return (1);
        }
        else
        {
            return (0);
        }
    }
}


//==============================================================================
//==============================================================================
inline int cIntersectionSegmentDisk(const cVector3d& a_segmentPointA,
                                    const cVector3d& a_segmentPointB,
                                    const cVector3d& a_diskPos,
                                    const cVector3d& a_diskNormal,
                                    const double& a_diskRadius,
                                    cVector3d& a_collisionPoint,
                                    cVector3d& a_collisionNormal)
{
    // compute intersection between segment and disk plane
    if (cIntersectionSegmentPlane(a_segmentPointA,
                                  a_segmentPointB,
                                  a_diskPos,
                                  a_diskNormal,
                                  a_collisionPoint,
                                  a_collisionNormal) == 0)
    {
        return (0);
    }

    // check if intersection point within disk
    if (cDistance(a_collisionPoint, a_diskPos) <= a_diskRadius)
    {
        return (1);
    }
    else
    {
        return (0);
    }
}


//==============================================================================
//==============================================================================
inline int cIntersectionSegmentSphere(const cVector3d& a_segmentPointA,
                                      const cVector3d& a_segmentPointB,
                                      const cVector3d& a_spherePos,
                                      const double& a_sphereRadius,
                                      cVector3d& a_collisionPoint0,
                                      cVector3d& a_collisionNormal0,
                                      cVector3d& a_collisionPoint1,
                                      cVector3d& a_collisionNormal1)
{
    // temp variables
    cVector3d AB, CA;
    a_segmentPointB.subr(a_segmentPointA, AB);
    a_segmentPointA.subr(a_spherePos, CA);
    double radiusSq = a_sphereRadius * a_sphereRadius;

    double a = AB.lengthsq();
    double b = 2.0 * cDot(AB, CA);
    double c = CA.lengthsq() - radiusSq;

    // invalid segment
    if (a == 0)
    {
        return (0);
    }

    double d = b*b - 4*a*c;

    // segment ray is located outside of sphere
    if (d < 0)
    {
        return (0);
    }

    // segment ray intersects sphere
    d = sqrt(d);
    double e = 2.0 * a;

    // compute both solutions
    double u0 = (-b + d) / e;
    double u1 = (-b - d) / e;

    // check if the solutions are located along the segment AB
    bool valid_u0 = cContains(u0, 0.0, 1.0);
    bool valid_u1 = cContains(u1, 0.0, 1.0);

    // two intersection points are located along segment AB
    if (valid_u0 && valid_u1)
    {
        if (u0 > u1) { cSwap(u0, u1); }

        // compute point 0
        AB.mulr(u0, a_collisionPoint0);
        a_collisionPoint0.add(a_segmentPointA);

        a_collisionPoint0.subr(a_spherePos, a_collisionNormal0);
        a_collisionNormal0.normalize();

        // compute point 1
        AB.mulr(u1, a_collisionPoint1);
        a_collisionPoint1.add(a_segmentPointA);

        a_collisionPoint1.subr(a_spherePos, a_collisionNormal1);
        a_collisionNormal1.normalize();

        return (2);
    }

    // one intersection point is located along segment AB
    else if (valid_u0)
    {
        // compute point 0
        AB.mulr(u0, a_collisionPoint0);
        a_collisionPoint0.add(a_segmentPointA);

        a_collisionPoint0.subr(a_spherePos, a_collisionNormal0);
        a_collisionNormal0.normalize();

        // check dot product to see if the intial segment point is located
        // inside the sphere.
        double dotProduct = cDot(AB, a_collisionNormal0);
        if (dotProduct < 0.0)
        {
            return (1);
        }
        else
        {
            return (0);
        }
    }

    // one intersection point is located along segment AB
    else if (valid_u1)
    {
        // compute point 0
        AB.mulr(u1, a_collisionPoint0);
        a_collisionPoint0.add(a_segmentPointA);

        a_collisionPoint0.subr(a_spherePos, a_collisionNormal0);
        a_collisionNormal0.normalize();

        // check dot product to see if the intial segment point is located
        // inside the sphere.
        double dotProduct = cDot(AB, a_collisionNormal0);
        if (dotProduct < 0.0)
        {
            return (1);
        }
        else
        {
            return (0);
        }
    }

    // both points are located outside of the segment AB
    else
    {
        return (0);
    }
}


//==============================================================================
//==============================================================================
inline int cIntersectionSegmentEllipsoid(const cVector3d& a_segmentPointA,
    const cVector3d& a_segmentPointB,
    const cVector3d& a_pos,
    const double& a_radiusX,
    const double& a_radiusY,
    const double& a_radiusZ,
    cVector3d& a_collisionPoint0,
    cVector3d& a_collisionNormal0,
    cVector3d& a_collisionPoint1,
    cVector3d& a_collisionNormal1)
{
    // scale to sphere
    double radius = cMin(a_radiusX, cMin(a_radiusY, a_radiusZ));
    double scaleX = a_radiusX / radius;
    double scaleY = a_radiusY / radius;
    double scaleZ = a_radiusZ / radius;

    cVector3d segmentPointA = a_segmentPointA;
    segmentPointA.mul(1.0 / scaleX, 1.0 / scaleY, 1.0 / scaleZ);
    cVector3d segmentPointB = a_segmentPointB;
    segmentPointB.mul(1.0 / scaleX, 1.0 / scaleY, 1.0 / scaleZ);

    // temp variables
    cVector3d AB, CA;
    segmentPointB.subr(segmentPointA, AB);
    segmentPointA.subr(a_pos, CA);
    double radiusSq = radius * radius;

    double a = AB.lengthsq();
    double b = 2.0 * cDot(AB, CA);
    double c = CA.lengthsq() - radiusSq;

    // invalid segment
    if (a == 0)
    {
        return (0);
    }

    double d = b*b - 4 * a*c;

    // segment ray is located outside of sphere
    if (d < 0)
    {
        return (0);
    }

    // segment ray intersects sphere
    d = sqrt(d);
    double e = 2.0 * a;

    // compute both solutions
    double u0 = (-b + d) / e;
    double u1 = (-b - d) / e;

    // check if the solutions are located along the segment AB
    bool valid_u0 = cContains(u0, 0.0, 1.0);
    bool valid_u1 = cContains(u1, 0.0, 1.0);

    // two intersection points are located along segment AB
    if (valid_u0 && valid_u1)
    {
        if (u0 > u1) { cSwap(u0, u1); }

        // compute point 0
        AB.mulr(u0, a_collisionPoint0);
        a_collisionPoint0.add(segmentPointA);
        a_collisionPoint0.subr(a_pos, a_collisionNormal0);
        a_collisionNormal0.normalize();
        a_collisionNormal0.mul(1.0 / scaleX, 1.0 / scaleY, 1.0 / scaleZ);
        a_collisionPoint0.mul(scaleX, scaleY, scaleZ);

        // compute point 1
        AB.mulr(u1, a_collisionPoint1);
        a_collisionPoint1.add(segmentPointA);
        a_collisionPoint1.subr(a_pos, a_collisionNormal1);
        a_collisionNormal1.normalize();
        a_collisionNormal1.mul(1.0 / scaleX, 1.0 / scaleY, 1.0 / scaleZ);
        a_collisionPoint1.mul(scaleX, scaleY, scaleZ);

        return (2);
    }

    // one intersection point is located along segment AB
    else if (valid_u0)
    {
        // compute point 0
        AB.mulr(u0, a_collisionPoint0);
        a_collisionPoint0.add(segmentPointA);
        a_collisionPoint0.subr(a_pos, a_collisionNormal0);
        a_collisionNormal0.normalize();
        a_collisionNormal0.mul(1.0 / scaleX, 1.0 / scaleY, 1.0 / scaleZ);
        a_collisionPoint0.mul(scaleX, scaleY, scaleZ);

        // check dot product to see if the intial segment point is located
        // inside the sphere.
        double dotProduct = cDot(AB, a_collisionNormal0);
        if (dotProduct < 0.0)
        {
            return (1);
        }
        else
        {
            return (0);
        }
    }

    // one intersection point is located along segment AB
    else if (valid_u1)
    {
        // compute point 0
        AB.mulr(u1, a_collisionPoint0);
        a_collisionPoint0.add(segmentPointA);
        a_collisionPoint0.subr(a_pos, a_collisionNormal0);
        a_collisionNormal0.normalize();
        a_collisionNormal0.mul(1.0 / scaleX, 1.0 / scaleY, 1.0 / scaleZ);
        a_collisionPoint0.mul(scaleX, scaleY, scaleZ);

        // check dot product to see if the intial segment point is located
        // inside the sphere.
        double dotProduct = cDot(AB, a_collisionNormal0);
        if (dotProduct < 0.0)
        {
            return (1);
        }
        else
        {
            return (0);
        }
    }

    // both points are located outside of the segment AB
    else
    {
        return (0);
    }
}


//==============================================================================
//==============================================================================
inline int cIntersectionSegmentToplessCylinder(const cVector3d& a_segmentPointA,
                                               const cVector3d& a_segmentPointB,
                                               const cVector3d& a_cylinderPointA,
                                               const cVector3d& a_cylinderPointB,
                                               const double& a_cylinderRadius,
                                               cVector3d& a_collisionPoint0,
                                               cVector3d& a_collisionNormal0,
                                               double& a_collisionPointRelPosAB0,
                                               cVector3d& a_collisionPoint1,
                                               cVector3d& a_collisionNormal1,
                                               double& a_collisionPointRelPosAB1)
{
    // compute some initial values
    cVector3d cylinderAB = a_cylinderPointB - a_cylinderPointA;
    double lengthCylinderAB = cylinderAB.length();

    // sanity check
    if (lengthCylinderAB == 0.0) { return (0); }

    // compute more initial values
    cVector3d cylinderDir = cNormalize(cylinderAB);
    cVector3d RC = a_segmentPointA - a_cylinderPointA;
    cVector3d segmentAB = a_segmentPointB - a_segmentPointA;
    if (segmentAB.lengthsq() == 0) { return (0); }

    cVector3d segmentDir = cNormalize(segmentAB);
    cVector3d n = cCross(segmentDir, cylinderDir);

    // segment is parallel to cylinder
    double length = n.length();
    if (length == 0.0)
    {
        return (0);
    }

    n.normalize();
    double d = fabs (cDot (RC,n));

    if (d <= a_cylinderRadius)
    {
        cVector3d O = cCross(RC, cylinderDir);
        double t = -cDot(O,n) / length;
        O = cCross(n, cylinderDir);
        O.normalize();
        double s = fabs(sqrt(a_cylinderRadius*a_cylinderRadius - d*d) / cDot(segmentDir,O));
        double u0 = t - s;
        double u1 = t + s;

        // reorder solutions
        if (u0 > u1) { cSwap (u0,u1); }

        bool valid_u0 = true;
        bool valid_u1 = true;

        // check if solutions along segment
        double lengthAB = segmentAB.length();
        if (!cContains(u0, 0.0, lengthAB)) { valid_u0 = false; }
        if (!cContains(u1, 0.0, lengthAB)) { valid_u1 = false; }

        // check if solutions lay along cylinder
        cVector3d P0, P1;
        cVector3d cylinderDirNeg = cNegate(cylinderDir);
        
        if (valid_u0)
        {
            P0 = a_segmentPointA + u0 * segmentDir;
            cVector3d P0A = cSub(P0, a_cylinderPointA);
            cVector3d P0B = cSub(P0, a_cylinderPointB);

            double cosAngleA = cCosAngle(cylinderDir, P0A);
            double cosAngleB = cCosAngle(cylinderDirNeg, P0B);

            if ((cosAngleA <= 0.0) || (cosAngleB <= 0.0))
            {
                valid_u0 = false;
            }
            else
            {
                a_collisionPointRelPosAB0 = cosAngleA * (P0A.length() / lengthCylinderAB);
            }
        }

        if (valid_u1)
        {
            P1 = a_segmentPointA + u1 * segmentDir;
            cVector3d P1A = cSub(P1, a_cylinderPointA);
            cVector3d P1B = cSub(P1, a_cylinderPointB);

            double cosAngleA = cCosAngle(cylinderDir, P1A);
            double cosAngleB = cCosAngle(cylinderDirNeg, P1B);

            if ((cosAngleA <= 0.0) || (cosAngleB <= 0.0))
            {
                valid_u1 = false;
            }
            else
            {
                a_collisionPointRelPosAB1 = cosAngleA * (P1A.length() / lengthCylinderAB);
            }
        }

        if (valid_u0 && valid_u1)
        {
            a_collisionPoint0 = P0;
            a_collisionNormal0 = cNormalize(cCross(cylinderDir, cCross(cSub(P0, a_cylinderPointA), cylinderDir)));
            a_collisionPoint1 = P1;
            a_collisionNormal1 = cNormalize(cCross(cylinderDir, cCross(cSub(P1, a_cylinderPointA), cylinderDir)));
            return (2);
        }
        else if (valid_u0)
        {
            a_collisionPoint0 = P0;
            a_collisionNormal0 = cNormalize(cCross(cylinderDir, cCross(cSub(P0, a_cylinderPointA), cylinderDir)));

            // check dot product to see if the intial segment point is located inside the cylinder.
            double dotProduct = cDot(segmentAB, a_collisionNormal0);
            if (dotProduct < 0.0)
            {
                return (1);
            }
            else
            {
                return (0);
            }
        }
        else if (valid_u1)
        {
            a_collisionPoint0 = P1;
            a_collisionNormal0 = cNormalize(cCross(cylinderDir, cCross(cSub(P1, a_cylinderPointA), cylinderDir)));

            // check dot product to see if the intial segment point is located inside the cylinder.
            double dotProduct = cDot(segmentAB, a_collisionNormal0);
            if (dotProduct < 0.0)
            {
                return (1);
            }
            else
            {
                return (0);
            }
        }
    }

    return (0);
}


//==============================================================================
//==============================================================================
inline int cIntersectionSegmentCylinder(const cVector3d& a_segmentPointA,
                                        const cVector3d& a_segmentPointB,
                                        const double& a_baseRadius,
                                        const double& a_topRadius,
                                        const double& a_height,
                                        cVector3d& a_collisionPoint,
                                        cVector3d& a_collisionNormal)
{
    // initialization
    cVector3d p = a_segmentPointA;
    cVector3d d = a_segmentPointB - a_segmentPointA;
    double length = d.length();

    // sanity check
    if (cZero(length))
    {
        return (0);
    }
    d.mul(1.0 / length);

    // compute collision with cylinder side
    double A = a_baseRadius;
    double B = (a_topRadius - a_baseRadius) / a_height;

    double c[3];
    double s[2];
    
    c[0] =-2.0*A*B*p(2) - cSqr(B*p(2)) - cSqr(A) + cSqr(p(0)) + cSqr(p(1));
    c[1] = 2.0*p(0)*d(0) + 2.0*p(1)*d(1) - 2.0*A*B*d(2) - 2.0*cSqr(B)*p(2)*d(2);
    c[2] = cSqr(d(0)) + cSqr(d(1)) - cSqr(B*d(2));

    int numCol = cSolveQuadric(c, s);

    // compute collision with top cap
    cVector3d collisionPointTop, collisionNormalTop;

    int numColTop = cIntersectionSegmentDisk(a_segmentPointA,
                                             a_segmentPointB,
                                             cVector3d(0.0, 0.0, a_height),
                                             cVector3d(0.0, 0.0, 1.0),
                                             a_topRadius,
                                             collisionPointTop,
                                             collisionNormalTop);

    // compute collision with base cap
    cVector3d collisionPointBase, collisionNormalBase;

    int numColBase = cIntersectionSegmentDisk(a_segmentPointA,
                                              a_segmentPointB,
                                              cVector3d(0.0, 0.0, 0.0),
                                              cVector3d(0.0, 0.0,-1.0),
                                              a_baseRadius,
                                              collisionPointBase,
                                              collisionNormalBase);

    // evaluate solutions
    bool hit = false;
    double distance = C_LARGE;

    // check top cap
    if (numColTop > 0)
    {
        hit = true;
        distance = cDistance(a_segmentPointA, collisionPointTop);
        a_collisionPoint = collisionPointTop;
        a_collisionNormal = cVector3d(0.0, 0.0, 1.0);
    }

    // check base cap
    if (numColBase > 0)
    {
        hit = true;
        double dist = cDistance(a_segmentPointA, collisionPointBase);
        if (dist < distance)
        {
            a_collisionPoint = collisionPointBase;
            a_collisionNormal = cVector3d(0.0, 0.0,-1.0);
            distance = dist;
        }
    }

    // check cylinder solution 1
    if (numCol > 0)
    {
        if ((s[0] >= 0.0) && (s[0] <= length) && (s[0] < distance))
        {
            cVector3d collisionPoint = p + d * s[0];
            if ((collisionPoint(2) > 0.0) && (collisionPoint(2) < a_height))
            {
                cVector3d normal(collisionPoint(0), collisionPoint(1), 0.0);
                normal.normalize();
                normal(2) = (a_baseRadius - a_topRadius) / a_height;
                a_collisionNormal = normal;
                a_collisionPoint = collisionPoint;
                distance = s[0];
                hit = true;
            }
        }
    }

    // check cylinder solution 2
    if (numCol > 1)
    {
        if ((s[1] >= 0.0) && (s[1] <= length) && (s[1] < distance))
        {
            cVector3d collisionPoint = p + d * s[1];
            if ((collisionPoint(2) > 0.0) && (collisionPoint(2) < a_height))
            {
                cVector3d normal(collisionPoint(0), collisionPoint(1), 0.0);
                normal.normalize();
                normal(2) = (a_baseRadius - a_topRadius) / a_height; 
                a_collisionNormal = normal;
                a_collisionPoint = collisionPoint;
                hit = true;
            }
        }
    }

    if (hit)
    {
        return (1);
    }
    else
    {
        return (0);
    }
}


//==============================================================================
//==============================================================================
inline int cIntersectionSegmentBox(const cVector3d& a_segmentPointA,
                                   const cVector3d& a_segmentPointB,
                                   const cVector3d& a_boxMin,
                                   const cVector3d& a_boxMax,
                                   cVector3d& a_collisionPoint,
                                   cVector3d& a_collisionNormal)
{
    double tmin = 0.0;
    double tmax = C_LARGE;

    cVector3d d = a_segmentPointB - a_segmentPointA;
    double distance = d.length();
    if (distance == 0)
    {
        return(0);
    }
    d.normalize();

    // for all three slabs
    double normalDir = 1.0;
    int normalAxis = 0;

    for (int i = 0; i<3; i++)
    {
        if (fabs(d(i)) < C_SMALL)
        {
            // ray is parallel to slab. No hit if origin not within slab
            if (a_segmentPointA(i) < a_boxMin(i) || a_segmentPointA(i) > a_boxMax(i))
            {
                return (0);
            }
        }
        else
        {
            // compute intersection t value of ray with near and far plane of slab
            double ood = 1.0 / d(i);
            double t1 = (a_boxMin(i) - a_segmentPointA(i)) * ood;
            double t2 = (a_boxMax(i) - a_segmentPointA(i)) * ood;
            int n = 1;

            // make t1 be intersection with near plane, t2 with far plane
            if (t1 > t2)
            {
                cSwap(t1, t2);
                n = -1;
            }

            // compute the intersection of slab intersection intervals
            if (t1 > tmin)
            {
                tmin = t1;
                normalAxis = i;
                normalDir = -n;
            }

            if (t2 < tmax)
            {
                tmax = t2;
            }

            // exit with no collision as soon as slab intersection becomes empty
            if (tmin > tmax)
            {
                return (0);
            }
        }
    }

    if (tmin > distance)
    {
        return (0);
    }

    // compute collision point and surface normal
    a_collisionPoint = a_segmentPointA + cMul(tmin, d);
    a_collisionNormal.zero();
    a_collisionNormal(normalAxis) = normalDir;

    return (1);
}


//==============================================================================
//==============================================================================
inline int cIntersectionSegmentTorus(const cVector3d& a_segmentPointA,
                                     const cVector3d& a_segmentPointB,
                                     const double& a_innerRadius, 
                                     const double& a_outerRadius,
                                     cVector3d& a_collisionPoint,
                                     cVector3d& a_collisionNormal)
{
    cVector3d collisionPoint, collisionNormal;

    // compute boundary box
    double width = a_outerRadius + a_innerRadius;
    cVector3d boxMin(-width,-width,-a_innerRadius);
    cVector3d boxMax( width, width, a_innerRadius);

    // check collision with boundary box
    if (cIntersectionSegmentBox(a_segmentPointA,
                                a_segmentPointB,
                                boxMin,
                                boxMax,
                                collisionPoint,
                                collisionNormal) == 0)
    {
        return (0);
    }

    cVector3d p = a_segmentPointA;
    cVector3d d = cNormalize(a_segmentPointB - a_segmentPointA);

    double dd = cDot(d, d);
    double pd = cDot(p, d);
    double pp = cDot(p, p);
    double r2 = cSqr(a_innerRadius);
    double R2 = cSqr(a_outerRadius);

    double c[5];
    double s[4];

    c[4] = cSqr(dd);
    c[3] = 4.0 * dd * pd;
    c[2] = 4.0 * cSqr(pd) + 2.0 * dd * (pp - r2 - R2) + 4.0 * R2 * cSqr(d(2));
    c[1] = 4.0 * pd * (pp - r2 - R2) + 8.0 * R2 * p(2) * d(2);
    c[0] = cSqr(pp - r2 - R2) + 4.0 * R2 * cSqr(p(2)) - 4.0 * R2 * r2;

    int num = cSolveQuartic(c, s);

    if (num == 0)
    {
        // no solutions found
        return (0);
    }

    // search for nearest solution (sollision point)
    double sol = C_LARGE;
    for (int i=0; i<num; i++)
    {
        if (s[i] < sol)
        {
            sol = s[i];
        }
    }

    // compute point
    a_collisionPoint = p + sol * d;

    // compute surface normal
    cVector3d pt = cMul(a_outerRadius, cNormalize(cVector3d(a_collisionPoint(0), a_collisionPoint(1), 0.0)));
    a_collisionNormal = cNormalize(a_collisionPoint - pt);

    // return one collision point detected
    return (1);
}


//==============================================================================
//==============================================================================
inline bool cIntersectionSegmentTriangle(const cVector3d& a_segmentPointA,
                                         const cVector3d& a_segmentPointB,
                                         const cVector3d& a_triangleVertex0,
                                         const cVector3d& a_triangleVertex1,
                                         const cVector3d& a_triangleVertex2,
                                         const bool a_reportFrontSideCollision,
                                         const bool a_reportBackSideCollision,
                                         cVector3d& a_collisionPoint,
                                         cVector3d& a_collisionNormal,
                                         double& a_collisionPosVertex01,
                                         double& a_collisionPosVertex02
                                         )
{
    // This value controls how close rays can be to parallel to the triangle
    // surface before we discard them
    const double C_INTERSECT_EPSILON = 10e-14f;

    // compute a ray and check its length
    cVector3d rayDir;
    a_segmentPointB.subr(a_segmentPointA, rayDir);
    double segmentLengthSquare = rayDir.lengthsq();
    if (segmentLengthSquare == 0.0) { return (false); }

    // Compute the triangle's normal
    cVector3d t_E0, t_E1, t_N;
    a_triangleVertex1.subr(a_triangleVertex0, t_E0);
    a_triangleVertex2.subr(a_triangleVertex0, t_E1);
    t_E0.crossr(t_E1, t_N);

    // If the ray is parallel to the triangle (perpendicular to the
    // normal), there's no collision
    double dotProduct = t_N.dot(rayDir);
    if (fabs(dotProduct)<10E-15f) return (false);

    if ((dotProduct < 0.0) && (a_reportFrontSideCollision == false)) return (false);
    if ((dotProduct > 0.0) && (a_reportBackSideCollision == false)) return (false);

    double t_T = cDot(t_N, cSub(a_triangleVertex0, a_segmentPointA)) / cDot(t_N, rayDir);

    if (t_T + C_INTERSECT_EPSILON < 0) return (false);

    cVector3d t_Q = cSub(cAdd(a_segmentPointA, cMul(t_T, rayDir)), a_triangleVertex0);
    double t_Q0 = cDot(t_E0,t_Q);
    double t_Q1 = cDot(t_E1,t_Q);
    double t_E00 = cDot(t_E0,t_E0);
    double t_E01 = cDot(t_E0,t_E1);
    double t_E11 = cDot(t_E1,t_E1);
    double t_D = (t_E00 * t_E11) - (t_E01 * t_E01);

    if ((t_D > -C_INTERSECT_EPSILON) && (t_D < C_INTERSECT_EPSILON)) return(false);

    double t_S0 = ((t_E11 * t_Q0) - (t_E01 * t_Q1)) / t_D;
    double t_S1 = ((t_E00 * t_Q1) - (t_E01 * t_Q0)) / t_D;

    // Collision has occurred. It is reported.
    if (
      (t_S0 >= 0.0 - C_INTERSECT_EPSILON) &&
      (t_S1 >= 0.0 - C_INTERSECT_EPSILON) &&
      ((t_S0 + t_S1) <= 1.0 + C_INTERSECT_EPSILON)
      )
    {
        cVector3d t_I = cAdd(a_triangleVertex0, cMul(t_S0,t_E0), cMul(t_S1, t_E1));

        // Square distance between ray origin and collision point.
        double distanceSquare = a_segmentPointA.distancesq(t_I);

        // check if collision occurred within segment. If yes, report collision
        if (distanceSquare <= segmentLengthSquare)
        {
            a_collisionPoint = cAdd(a_segmentPointA, cMul(t_T, rayDir));
            t_N.normalizer(a_collisionNormal);
            if (cCosAngle(a_collisionNormal, rayDir) > 0.0)
            {
                a_collisionNormal.negate();
            }
            
            a_collisionPosVertex01 = t_S0;
            a_collisionPosVertex02 = t_S1;

            return (true);
        }
    }

    // no collision occurred
    return (false);
}


//------------------------------------------------------------------------------
}       // namespace chai3d
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
#endif  // CGeometryH
//------------------------------------------------------------------------------