/*
* All or portions of this file Copyright (c) Amazon.com, Inc. or its affiliates or
* its licensors.
*
* For complete copyright and license terms please see the LICENSE at the root of this
* distribution (the "License"). All use of this software is governed by the License,
* or, if provided, by the license below or the license accompanying this file. Do not
* remove or modify any license notices. This file is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
*
*/
// Original file Copyright Crytek GMBH or its affiliates, used under license.

// Description : Common intersection-tests


#ifndef CRYINCLUDE_CRYCOMMON_CRY_GEOINTERSECT_H
#define CRYINCLUDE_CRYCOMMON_CRY_GEOINTERSECT_H
#pragma once


#include <Cry_Geo.h>

namespace Intersect {
    inline bool Ray_Plane(const Ray& ray, const Plane& plane, Vec3& output, bool bSingleSidePlane = true)
    {
        float cosine    =   plane.n | ray.direction;

        //REJECTION 1: if "line-direction" is perpendicular to "plane-normal", an intersection is not possible! That means ray is parallel
        //             to the plane
        //REJECTION 2: if bSingleSidePlane == true we deal with single-sided planes. That means
        //             if "line-direction" is pointing in the same direction as "the plane-normal",
        //             an intersection is not possible!
        if ((cosine == 0.0f) ||                                     // normal is orthogonal to vector, cant intersect
            (bSingleSidePlane && (cosine > 0.0f)))  // we are trying to find an intersection in the same direction as the plane normal
        {
            return false;
        }

        float numer     = plane.DistFromPlane(ray.origin);
        float fLength   = -numer / cosine;
        output              =   ray.origin + (ray.direction * fLength);
        //skip, if cutting-point is "behind" ray.origin
        if (fLength < 0.0f)
        {
            return false;
        }

        return true;        //intersection occurred
    }

    inline bool Line_Plane(const Line& line, const Plane& plane, Vec3& output, bool bSingleSidePlane = true)
    {
        float cosine        =   plane.n | line.direction;

        //REJECTION 1: if "line-direction" is perpendicular to "plane-normal", an intersection is not possible! That means ray is parallel
        //             to the plane
        //REJECTION 2: if bSingleSidePlane == true we deal with single-sided planes. That means
        //             if "line-direction" is pointing in the same direction as "the plane-normal",
        //             an intersection is not possible!
        if ((cosine == 0.0f) ||                                     // normal is orthogonal to vector, cant intersect
            (bSingleSidePlane && (cosine > 0.0f)))  // we are trying to find an intersection in the same direction as the plane normal
        {
            return false;
        }

        //an intersection is possible: calculate the exact point!
        float perpdist  = plane | line.pointonline;
        float pd_c          =   -perpdist / cosine;
        output                  =   line.pointonline + (line.direction * pd_c);

        return true;        //intersection occurred
    }

    // Algorithm description:
    // http://softsurfer.com/Archive/algorithm_0104/algorithm_0104B.htm#Line-Plane%20Intersection
    template <typename T>
    inline bool Segment_Plane(const Lineseg_tpl<T>& segment, const Plane_tpl<T>& plane, Vec3_tpl<T>& vOutput, bool bSingleSidePlane = true)
    {
        Vec3_tpl<T> vSegment                                = segment.end - segment.start;
        T                       planeNormalDotSegment       =   plane.n | vSegment;

        //REJECTION 1: if "line-direction" is perpendicular to "plane-normal", an intersection is not possible! That means ray is parallel
        //             to the plane
        //REJECTION 2: if bSingleSidePlane == true we deal with single-sided planes. That means
        //             if "line-direction" is pointing in the same direction as "the plane-normal",
        //             an intersection is not possible!
        if ((planeNormalDotSegment == T(0)) ||                                   // normal is orthogonal to vector, cant intersect
            (bSingleSidePlane && (planeNormalDotSegment > T(0))))  // we are trying to find an intersection in the same direction as the plane normal
        {
            return false;
        }

        // n Dot (segment.start - closest_point_in_plane) = 1 * DistFromPlane(segment.start) * cos(0) = DistFromPlane(segment.start)
        T distanceToStart   = plane.DistFromPlane(segment.start);
        T scale                     = -distanceToStart / planeNormalDotSegment;
        vOutput                     =   segment.start + (vSegment * scale);

        // skip, if segment start and ends in one side of the plane
        if ((scale < T(0)) || (scale > T(1)))
        {
            return false;
        }

        return true;        //intersection occurred
    }

    /// Intersection between two line segments in 2D (ignoring z coordinate). The two parametric
    /// values are set to between 0 and 1 if intersection occurs. If intersection does not occur
    /// their values will indicate the parametric values for intersection of the lines extended
    /// beyond the segment lengths. Parallel lines will result in a negative result, but the parametric
    /// values will both be equal to 0.5
    template<typename F>
    inline bool Lineseg_Lineseg2D(const Lineseg_tpl<F>& lineA, const Lineseg_tpl<F>& lineB, F& outA, F& outB)
    {
        const F Epsilon = (F)0.0000001;

        Vec3_tpl<F> delta = lineB.start - lineA.start;

        Vec3_tpl<F> dirA = lineA.end - lineA.start;
        Vec3_tpl<F> dirB = lineB.end - lineB.start;

        F det = dirA.x * dirB.y - dirA.y * dirB.x;
        F detA = delta.x * dirB.y - delta.y * dirB.x;
        F detB = delta.x * dirA.y - delta.y * dirA.x;

        F absDet = fabs_tpl(det);

        if (absDet >= Epsilon)
        {
            F invDet = (F)1.0 / det;

            F a = detA * invDet;
            F b = detB * invDet;
            outA = a;
            outB = b;

            if ((a > (F)1.0) || (a < (F)0.0) || (b > (F)1.0) || (b < (F)0.0))
            {
                return false;
            }
        }
        else
        {
            outA = outB = (F)0.5;

            return false;
        }

        return true;
    }

    /// Calculates the intersection between a line segment and a polygon, in 2D (i.e.
    /// ignoring z coordinate). The VecContainer should be a container of Vec3 such
    /// that we can traverse it using iterators. intersectionPoint is set to the intersection
    /// point or the end of the segment, if no intersection.
    template<typename VecIterator>
    inline bool Lineseg_Polygon2D(const Lineseg& lineseg, VecIterator polygonBegin, VecIterator polygonEnd, Vec3& intersectionPoint, Vec3* pNormal = NULL, bool bForceNormalOutwards = false)
    {
        intersectionPoint = lineseg.end;
        bool gotIntersection = false;

        float tmin = 1.0f;

        VecIterator iend = polygonEnd;
        VecIterator li, linext;
        Lineseg intersectSegment;
        for (li = polygonBegin; li != iend; ++li)
        {
            linext = li;
            ++linext;
            if (linext == iend)
            {
                linext = polygonBegin;
            }
            Lineseg segmentPoly(*li, *linext);
            float s, t;
            if (Intersect::Lineseg_Lineseg2D(lineseg, segmentPoly, s, t))
            {
                if (s < 0.00001f || s > 0.99999f || t < 0.00001f || t > 0.99999f)
                {
                    continue;
                }
                if (s < tmin)
                {
                    tmin = s;
                    gotIntersection = true;
                    intersectSegment = segmentPoly;
                }
            }
        }

        intersectionPoint = lineseg.start + tmin * (lineseg.end - lineseg.start);

        if (pNormal && gotIntersection)
        {
            Vec3 vPolyseg = intersectSegment.end - intersectSegment.start;
            Vec3 vIntSeg = (lineseg.end - lineseg.start);
            pNormal->x = vPolyseg.y;
            pNormal->y = -vPolyseg.x;
            pNormal->z = 0;
            pNormal->NormalizeSafe();
            // returns the normal towards the start point of the intersecting segment (if it's not forced to be outwards)
            if (!bForceNormalOutwards && vIntSeg.Dot(*pNormal) > 0)
            {
                pNormal->x = -pNormal->x;
                pNormal->y = -pNormal->y;
            }
        }
        return gotIntersection;
    }

    template<typename VecContainer>
    inline bool Lineseg_Polygon2D(const Lineseg& lineseg, const VecContainer& polygon, Vec3& intersectionPoint, Vec3* pNormal = NULL, bool bForceNormalOutwards = false)
    {
        return Lineseg_Polygon2D(lineseg, polygon.begin(), polygon.end(), intersectionPoint, pNormal, bForceNormalOutwards);
    }

    /*
    * calculates intersection between a line and a triangle.
    * IMPORTANT: this is a single-sided intersection test. That means its not enough
    * that the triangle and line overlap, its also important that the triangle
    * is "visible" when you are looking along the line-direction.
    *
    * If you need a double-sided test, you'll have to call this function twice with
    * reversed order of triangle vertices.
    *
    * return values
    * if there is an intertection the functions return "true" and stores the
    * 3d-intersection point in "output". if the function returns "false" the value in
    * "output" is undefined
    *
    */
    inline bool Line_Triangle(const Line& line, const Vec3& v0, const Vec3& v1, const Vec3& v2, Vec3& output)
    {
        const float Epsilon = 0.0000001f;

        Vec3 edgeA = v1 - v0;
        Vec3 edgeB = v2 - v0;

        Vec3 dir = line.direction;

        Vec3 p = dir.Cross(edgeA);
        Vec3 t = line.pointonline - v0;
        Vec3 q = t.Cross(edgeB);

        float dot = edgeB.Dot(p);

        float u = t.Dot(p);
        float v = dir.Dot(q);

        float DotGreaterThanEpsilon = dot - Epsilon;
        float VGreaterEqualThanZero = v;
        float UGreaterEqualThanZero = u;
        float UVLessThanDot = dot - (u + v);
        float ULessThanDot = dot - u;

        float UVGreaterEqualThanZero = (float)fsel(VGreaterEqualThanZero, UGreaterEqualThanZero, VGreaterEqualThanZero);
        float UUVLessThanDot = (float)fsel(UVLessThanDot, ULessThanDot, UVLessThanDot);
        float BothGood = (float)fsel(UVGreaterEqualThanZero, UUVLessThanDot, UVGreaterEqualThanZero);
        float AllGood = (float)fsel(DotGreaterThanEpsilon, BothGood, DotGreaterThanEpsilon);

        if (AllGood < 0.0f)
        {
            return false;
        }

        float dt = edgeA.Dot(q) / dot;

        Vec3 result = (dir * dt) + line.pointonline;
        output = result;

        return true;
    }



    /*
    * calculates intersection between a ray and a triangle.
    * IMPORTANT: this is a single-sided intersection test. That means its not sufficient
    * that the triangle and rayt overlap, its also important that the triangle
    * is "visible" when you from the origin along the ray-direction.
    *
    * If you need a double-sided test, you'll have to call this function twice with
    * reversed order of triangle vertices.
    *
    * return values
    * if there is an intertection the functions return "true" and stores the
    * 3d-intersection point in "output". if the function returns "false" the value in
    * "output" is undefined
    */
    inline bool Ray_Triangle(const Ray& ray, const Vec3& v0, const Vec3& v1, const Vec3& v2, Vec3& output)
    {
        const float Epsilon = 0.0000001f;

        Vec3 edgeA = v1 - v0;
        Vec3 edgeB = v2 - v0;

        Vec3 dir = ray.direction;

        Vec3 p = dir.Cross(edgeA);
        Vec3 t = ray.origin - v0;
        Vec3 q = t.Cross(edgeB);

        float dot = edgeB.Dot(p);

        float u = t.Dot(p);
        float v = dir.Dot(q);

        float DotGreaterThanEpsilon = dot - Epsilon;
        float VGreaterEqualThanZero = v;
        float UGreaterEqualThanZero = u;
        float UVLessThanDot = dot - (u + v);
        float ULessThanDot = dot - u;

        float UVGreaterEqualThanZero = (float)fsel(VGreaterEqualThanZero, UGreaterEqualThanZero, VGreaterEqualThanZero);
        float UUVLessThanDot = (float)fsel(UVLessThanDot, ULessThanDot, UVLessThanDot);
        float BothGood = (float)fsel(UVGreaterEqualThanZero, UUVLessThanDot, UVGreaterEqualThanZero);
        float AllGood = (float)fsel(DotGreaterThanEpsilon, BothGood, DotGreaterThanEpsilon);

        if (AllGood < 0.0f)
        {
            return false;
        }

        float dt = edgeA.Dot(q) / dot;

        Vec3 result = (dir * dt) + ray.origin;
        output = result;

        float AfterStart = (result - ray.origin).Dot(dir);

        return AfterStart >= 0.0f;
    }



    /*
    * Description:
    *   Calculates intersection between a line-segment and a triangle.
    * Remarks:
    *   IMPORTANT: this is a single-sided intersection test. That means its not sufficient
    *   that the triangle and line-segment overlap, its also important that the triangle
    *   is "visible" when you are looking along the linesegment from "start" to "end".
    * Notes:
    *   If you need a double-sided test, you'll have to call this function twice with
    *   reversed order of triangle vertices.
    *
    * Return value:
    *   If there is an intertection the the functions return "true" and stores the
    *   3d-intersection point in "output". if the function returns "false" the value in
    *   "output" is undefined. If pT is non-zero then if there is an intersection the "t-value"
    *   (from 0-1) is also returned (unmodified if there is no intersection).
    */
    inline bool Lineseg_Triangle(const Lineseg& lineseg, const Vec3& v0, const Vec3& v1, const Vec3& v2, Vec3& output,
        float* outT = 0)
    {
        const float Epsilon = 0.0000001f;

        Vec3 edgeA = v1 - v0;
        Vec3 edgeB = v2 - v0;

        Vec3 dir = lineseg.end - lineseg.start;

        Vec3 p = dir.Cross(edgeA);
        Vec3 t = lineseg.start - v0;
        Vec3 q = t.Cross(edgeB);

        float dot = edgeB.Dot(p);

        float u = t.Dot(p);
        float v = dir.Dot(q);

        float DotGreaterThanEpsilon = dot - Epsilon;
        float VGreaterEqualThanZero = v;
        float UGreaterEqualThanZero = u;
        float UVLessThanDot = dot - (u + v);
        float ULessThanDot = dot - u;

        float UVGreaterEqualThanZero = (float)fsel(VGreaterEqualThanZero, UGreaterEqualThanZero, VGreaterEqualThanZero);
        float UUVLessThanDot = (float)fsel(UVLessThanDot, ULessThanDot, UVLessThanDot);
        float BothGood = (float)fsel(UVGreaterEqualThanZero, UUVLessThanDot, UVGreaterEqualThanZero);
        float AllGood = (float)fsel(DotGreaterThanEpsilon, BothGood, DotGreaterThanEpsilon);

        if (AllGood < 0.0f)
        {
            return false;
        }

        float dt = edgeA.Dot(q) / dot;

        Vec3 result = (dir * dt) + lineseg.start;
        output = result;

        float AfterStart = (result - lineseg.start).Dot(dir);
        float BeforeEnd = -(result - lineseg.end).Dot(dir);
        float Within = (float)fsel(AfterStart, BeforeEnd, AfterStart);

        if (outT)
        {
            *outT = dt;
        }

        return Within >= 0.0f;
    }




    //----------------------------------------------------------------------------------
    //  Ray_AABB
    //
    //  just ONE intersection point is calculated, and thats the entry point           -
    //   Lineseg and AABB are assumed to be in the same space
    //
    //--- 0x00 = no intersection (output undefined)        --------------------------
    //--- 0x01 = intersection (intersection point in output)              --------------
    //--- 0x02 = start of Lineseg is inside the AABB (ls.start is output)
    //----------------------------------------------------------------------------------
    inline uint8 Ray_AABB(const Ray& ray, const AABB& aabb, Vec3& output1)
    {
        uint8 cflags;
        float cosine;
        Vec3 cut;
        //--------------------------------------------------------------------------------------
        //----         check if "ray.origin" is inside of AABB    ---------------------------
        //--------------------------------------------------------------------------------------
        cflags = (ray.origin.x >= aabb.min.x) << 0;
        cflags |= (ray.origin.x <= aabb.max.x) << 1;
        cflags |= (ray.origin.y >= aabb.min.y) << 2;
        cflags |= (ray.origin.y <= aabb.max.y) << 3;
        cflags |= (ray.origin.z >= aabb.min.z) << 4;
        cflags |= (ray.origin.z <= aabb.max.z) << 5;
        if (cflags == 0x3f)
        {
            output1 = ray.origin;
            return 0x02;
        }

        //--------------------------------------------------------------------------------------
        //----         check intersection with planes           ------------------------------
        //--------------------------------------------------------------------------------------
        for (int i = 0; i < 3; i++)
        {
            if ((ray.direction[i] > 0) && (ray.origin[i] < aabb.min[i]))
            {
                cosine = (-ray.origin[i] + aabb.min[i]) / ray.direction[i];
                cut[i] = aabb.min[i];
                cut[incm3(i)] = ray.origin[incm3(i)] + (ray.direction[incm3(i)] * cosine);
                cut[decm3(i)] = ray.origin[decm3(i)] + (ray.direction[decm3(i)] * cosine);
                if ((cut[incm3(i)] > aabb.min[incm3(i)]) && (cut[incm3(i)] < aabb.max[incm3(i)]) && (cut[decm3(i)] > aabb.min[decm3(i)]) && (cut[decm3(i)] < aabb.max[decm3(i)]))
                {
                    output1 = cut;
                    return 0x01;
                }
            }
            if ((ray.direction[i] < 0) && (ray.origin[i] > aabb.max[i]))
            {
                cosine = (+ray.origin[i] - aabb.max[i]) / ray.direction[i];
                cut[i] = aabb.max[i];
                cut[incm3(i)] = ray.origin[incm3(i)] - (ray.direction[incm3(i)] * cosine);
                cut[decm3(i)] = ray.origin[decm3(i)] - (ray.direction[decm3(i)] * cosine);
                if ((cut[incm3(i)] > aabb.min[incm3(i)]) && (cut[incm3(i)] < aabb.max[incm3(i)]) && (cut[decm3(i)] > aabb.min[decm3(i)]) && (cut[decm3(i)] < aabb.max[decm3(i)]))
                {
                    output1 = cut;
                    return 0x01;
                }
            }
        }
        return 0x00;//no intersection
    }



    //----------------------------------------------------------------------------------
    //  Ray_OBB
    //
    //  just ONE intersection point is calculated, and thats the entry point           -
    //   Lineseg and OBB are assumed to be in the same space
    //
    //--- 0x00 = no intersection (output undefined)                                 ----
    //--- 0x01 = intersection (intersection point in output)              --------------
    //--- 0x02 = start of Lineseg is inside the OBB (ls.start is output)
    //----------------------------------------------------------------------------------
    inline uint8 Ray_OBB(const Ray& ray, const Vec3& pos, const OBB& obb, Vec3& output1)
    {
        AABB aabb(obb.c - obb.h, obb.c + obb.h);
        Ray aray((ray.origin - pos) * obb.m33, ray.direction * obb.m33);

        uint8 cflags;
        float cosine;
        Vec3 cut;
        //--------------------------------------------------------------------------------------
        //----         check if "aray.origin" is inside of AABB    ---------------------------
        //--------------------------------------------------------------------------------------
        cflags = (aray.origin.x > aabb.min.x) << 0;
        cflags |= (aray.origin.x < aabb.max.x) << 1;
        cflags |= (aray.origin.y > aabb.min.y) << 2;
        cflags |= (aray.origin.y < aabb.max.y) << 3;
        cflags |= (aray.origin.z > aabb.min.z) << 4;
        cflags |= (aray.origin.z < aabb.max.z) << 5;
        if (cflags == 0x3f)
        {
            output1 = aray.origin;
            return 0x02;
        }

        //--------------------------------------------------------------------------------------
        //----         check intersection with planes           ------------------------------
        //--------------------------------------------------------------------------------------
        for (int i = 0; i < 3; i++)
        {
            if ((aray.direction[i] > 0) && (aray.origin[i] < aabb.min[i]))
            {
                cosine = (-aray.origin[i] + aabb.min[i]) / aray.direction[i];
                cut[i] = aabb.min[i];
                cut[incm3(i)] = aray.origin[incm3(i)] + (aray.direction[incm3(i)] * cosine);
                cut[decm3(i)] = aray.origin[decm3(i)] + (aray.direction[decm3(i)] * cosine);
                if ((cut[incm3(i)] > aabb.min[incm3(i)]) && (cut[incm3(i)] < aabb.max[incm3(i)]) && (cut[decm3(i)] > aabb.min[decm3(i)]) && (cut[decm3(i)] < aabb.max[decm3(i)]))
                {
                    output1 = obb.m33 * cut + pos;
                    return 0x01;
                }
            }
            if ((aray.direction[i] < 0) && (aray.origin[i] > aabb.max[i]))
            {
                cosine = (+aray.origin[i] - aabb.max[i]) / aray.direction[i];
                cut[i] = aabb.max[i];
                cut[incm3(i)] = aray.origin[incm3(i)] - (aray.direction[incm3(i)] * cosine);
                cut[decm3(i)] = aray.origin[decm3(i)] - (aray.direction[decm3(i)] * cosine);
                if ((cut[incm3(i)] > aabb.min[incm3(i)]) && (cut[incm3(i)] < aabb.max[incm3(i)]) && (cut[decm3(i)] > aabb.min[decm3(i)]) && (cut[decm3(i)] < aabb.max[decm3(i)]))
                {
                    output1 = obb.m33 * cut + pos;
                    return 0x01;
                }
            }
        }
        return 0x00;//no intersection
    }

    //----------------------------------------------------------------------------------
    //  Lineseg_AABB
    //
    //  just ONE intersection point is calculated, and thats the entry point           -
    //   Lineseg and AABB are assumed to be in the same space
    //
    //--- 0x00 = no intersection (output undefined)        --------------------------
    //--- 0x01 = intersection (intersection point in output)              --------------
    //--- 0x02 = start of Lineseg is inside the AABB (ls.start is output)
    //----------------------------------------------------------------------------------
    inline uint8    Lineseg_AABB(const Lineseg& ls, const AABB& aabb, Vec3& output1)
    {
        uint8 cflags;
        float cosine;
        Vec3 cut;
        Vec3 lnormal = (ls.start - ls.end).GetNormalized();
        //--------------------------------------------------------------------------------------
        //----         check if "ls.start" is inside of AABB    ---------------------------
        //--------------------------------------------------------------------------------------
        cflags = (ls.start.x > aabb.min.x) << 0;
        cflags |= (ls.start.x < aabb.max.x) << 1;
        cflags |= (ls.start.y > aabb.min.y) << 2;
        cflags |= (ls.start.y < aabb.max.y) << 3;
        cflags |= (ls.start.z > aabb.min.z) << 4;
        cflags |= (ls.start.z < aabb.max.z) << 5;
        if (cflags == 0x3f)
        {
            //ls.start is inside of aabb
            output1 = ls.start;
            return 0x02;
        }

        //--------------------------------------------------------------------------------------
        //----         check intersection with x-planes           ------------------------------
        //--------------------------------------------------------------------------------------
        if (lnormal.x)
        {
            if ((ls.start.x < aabb.min.x) && (ls.end.x > aabb.min.x))
            {
                cosine = (-ls.start.x + (+aabb.min.x)) / lnormal.x;
                cut(aabb.min.x, ls.start.y + (lnormal.y * cosine), ls.start.z + (lnormal.z * cosine));
                //check if cut-point is inside YZ-plane border
                if ((cut.y > aabb.min.y) && (cut.y < aabb.max.y) && (cut.z > aabb.min.z) && (cut.z < aabb.max.z))
                {
                    output1 = cut;
                    return 0x01;
                }
            }
            if ((ls.start.x > aabb.max.x) && (ls.end.x < aabb.max.x))
            {
                cosine = (+ls.start.x + (-aabb.max.x)) / lnormal.x;
                cut(aabb.max.x, ls.start.y - (lnormal.y * cosine), ls.start.z - (lnormal.z * cosine));
                //check if cut-point is inside YZ-plane border
                if ((cut.y > aabb.min.y) && (cut.y < aabb.max.y) && (cut.z > aabb.min.z) && (cut.z < aabb.max.z))
                {
                    output1 = cut;
                    return 0x01;
                }
            }
        }
        //--------------------------------------------------------------------------------------
        //----         check intersection with z-planes           ------------------------------
        //--------------------------------------------------------------------------------------
        if (lnormal.z)
        {
            if ((ls.start.z < aabb.min.z) && (ls.end.z > aabb.min.z))
            {
                cosine = (-ls.start.z + (+aabb.min.z)) / lnormal.z;
                cut(ls.start.x + (lnormal.x * cosine), ls.start.y + (lnormal.y * cosine), aabb.min.z);
                //check if cut-point is inside XY-plane border
                if ((cut.x > aabb.min.x) && (cut.x < aabb.max.x) && (cut.y > aabb.min.y) && (cut.y < aabb.max.y))
                {
                    output1 = cut;
                    return 0x01;
                }
            }
            if ((ls.start.z > aabb.max.z) && (ls.end.z < aabb.max.z))
            {
                cosine = (+ls.start.z + (-aabb.max.z)) / lnormal.z;
                cut(ls.start.x - (lnormal.x * cosine), ls.start.y - (lnormal.y * cosine), aabb.max.z);
                //check if cut-point is inside XY-plane border
                if ((cut.x > aabb.min.x) && (cut.x < aabb.max.x) && (cut.y > aabb.min.y) && (cut.y < aabb.max.y))
                {
                    output1 = cut;
                    return 0x01;
                }
            }
        }
        //--------------------------------------------------------------------------------------
        //----         check intersection with y-planes           ------------------------------
        //--------------------------------------------------------------------------------------
        if (lnormal.y)
        {
            if ((ls.start.y < aabb.min.y) && (ls.end.y > aabb.min.y))
            {
                cosine = (-ls.start.y + (+aabb.min.y)) / lnormal.y;
                cut(ls.start.x + (lnormal.x * cosine), aabb.min.y, ls.start.z + (lnormal.z * cosine));
                //check if cut-point is inside XZ-plane border
                if ((cut.x > aabb.min.x) && (cut.x < aabb.max.x) && (cut.z > aabb.min.z) && (cut.z < aabb.max.z))
                {
                    output1 = cut;
                    return 0x01;
                }
            }
            if ((ls.start.y > aabb.max.y) && (ls.end.y < aabb.max.y))
            {
                cosine = (+ls.start.y + (-aabb.max.y)) / lnormal.y;
                cut(ls.start.x - (lnormal.x * cosine), aabb.max.y, ls.start.z - (lnormal.z * cosine));
                //check if cut-point is inside XZ-plane border
                if ((cut.x > aabb.min.x) && (cut.x < aabb.max.x) && (cut.z > aabb.min.z) && (cut.z < aabb.max.z))
                {
                    output1 = cut;
                    return 0x01;
                }
            }
        }
        //no intersection
        return 0x00;
    }



    //----------------------------------------------------------------------------------
    //  Lineseg_OBB
    //
    //  just ONE intersection point is calculated, and thats the entry point           -
    //  Lineseg and OBB are assumed to be in the same space
    //
    //--- 0x00 = no intersection (output undefined)           --------------------------
    //--- 0x01 = intersection (intersection point in output)              --------------
    //--- 0x02 = start of Lineseg is inside the OBB (ls.start is output)
    //----------------------------------------------------------------------------------
    inline uint8    Lineseg_OBB(const Lineseg& lseg, const Vec3& pos, const OBB& obb, Vec3& output1)
    {
        AABB aabb(obb.c - obb.h, obb.c + obb.h);
        Lineseg ls((lseg.start - pos) * obb.m33, (lseg.end - pos) * obb.m33);

        uint8 cflags;
        float cosine;
        Vec3 cut;
        Vec3 lnormal = (ls.start - ls.end).GetNormalized();
        //--------------------------------------------------------------------------------------
        //----         check if "ls.start" is inside of AABB    ---------------------------
        //--------------------------------------------------------------------------------------
        cflags = (ls.start.x > aabb.min.x) << 0;
        cflags |= (ls.start.x < aabb.max.x) << 1;
        cflags |= (ls.start.y > aabb.min.y) << 2;
        cflags |= (ls.start.y < aabb.max.y) << 3;
        cflags |= (ls.start.z > aabb.min.z) << 4;
        cflags |= (ls.start.z < aabb.max.z) << 5;
        if (cflags == 0x3f)
        {
            //ls.start is inside of aabb
            output1 = obb.m33 * ls.start + pos;
            return 0x02;
        }

        //--------------------------------------------------------------------------------------
        //----         check intersection with x-planes           ------------------------------
        //--------------------------------------------------------------------------------------
        if (lnormal.x)
        {
            if ((ls.start.x < aabb.min.x) && (ls.end.x > aabb.min.x))
            {
                cosine = (-ls.start.x + (+aabb.min.x)) / lnormal.x;
                cut(aabb.min.x, ls.start.y + (lnormal.y * cosine), ls.start.z + (lnormal.z * cosine));
                //check if cut-point is inside YZ-plane border
                if ((cut.y > aabb.min.y) && (cut.y < aabb.max.y) && (cut.z > aabb.min.z) && (cut.z < aabb.max.z))
                {
                    output1 = obb.m33 * cut + pos;
                    return 0x01;
                }
            }
            if ((ls.start.x > aabb.max.x) && (ls.end.x < aabb.max.x))
            {
                cosine = (+ls.start.x + (-aabb.max.x)) / lnormal.x;
                cut(aabb.max.x, ls.start.y - (lnormal.y * cosine), ls.start.z - (lnormal.z * cosine));
                //check if cut-point is inside YZ-plane border
                if ((cut.y > aabb.min.y) && (cut.y < aabb.max.y) && (cut.z > aabb.min.z) && (cut.z < aabb.max.z))
                {
                    output1 = obb.m33 * cut + pos;
                    return 0x01;
                }
            }
        }
        //--------------------------------------------------------------------------------------
        //----         check intersection with z-planes           ------------------------------
        //--------------------------------------------------------------------------------------
        if (lnormal.z)
        {
            if ((ls.start.z < aabb.min.z) && (ls.end.z > aabb.min.z))
            {
                cosine = (-ls.start.z + (+aabb.min.z)) / lnormal.z;
                cut(ls.start.x + (lnormal.x * cosine), ls.start.y + (lnormal.y * cosine), aabb.min.z);
                //check if cut-point is inside XY-plane border
                if ((cut.x > aabb.min.x) && (cut.x < aabb.max.x) && (cut.y > aabb.min.y) && (cut.y < aabb.max.y))
                {
                    output1 = obb.m33 * cut + pos;
                    return 0x01;
                }
            }
            if ((ls.start.z > aabb.max.z) && (ls.end.z < aabb.max.z))
            {
                cosine = (+ls.start.z + (-aabb.max.z)) / lnormal.z;
                cut(ls.start.x - (lnormal.x * cosine), ls.start.y - (lnormal.y * cosine), aabb.max.z);
                //check if cut-point is inside XY-plane border
                if ((cut.x > aabb.min.x) && (cut.x < aabb.max.x) && (cut.y > aabb.min.y) && (cut.y < aabb.max.y))
                {
                    output1 = obb.m33 * cut + pos;
                    return 0x01;
                }
            }
        }
        //--------------------------------------------------------------------------------------
        //----         check intersection with y-planes           ------------------------------
        //--------------------------------------------------------------------------------------
        if (lnormal.y)
        {
            if ((ls.start.y < aabb.min.y) && (ls.end.y > aabb.min.y))
            {
                cosine = (-ls.start.y + (+aabb.min.y)) / lnormal.y;
                cut(ls.start.x + (lnormal.x * cosine), aabb.min.y, ls.start.z + (lnormal.z * cosine));
                //check if cut-point is inside XZ-plane border
                if ((cut.x > aabb.min.x) && (cut.x < aabb.max.x) && (cut.z > aabb.min.z) && (cut.z < aabb.max.z))
                {
                    output1 = obb.m33 * cut + pos;
                    return 0x01;
                }
            }
            if ((ls.start.y > aabb.max.y) && (ls.end.y < aabb.max.y))
            {
                cosine = (+ls.start.y + (-aabb.max.y)) / lnormal.y;
                cut(ls.start.x - (lnormal.x * cosine), aabb.max.y, ls.start.z - (lnormal.z * cosine));
                //check if cut-point is inside XZ-plane border
                if ((cut.x > aabb.min.x) && (cut.x < aabb.max.x) && (cut.z > aabb.min.z) && (cut.z < aabb.max.z))
                {
                    output1 = obb.m33 * cut + pos;
                    return 0x01;
                }
            }
        }
        //no intersection
        return 0x00;
    }



    //----------------------------------------------------------------------------------
    //--- 0x00 = no intersection                               --------------------------
    //--- 0x01 = not possible   --
    //--- 0x02 = not possible  --
    //--- 0x03 = two intersection, lineseg has ENTRY and EXIT point  --
    //----------------------------------------------------------------------------------

    inline unsigned char Line_Sphere(const Line& line, const Sphere& s, Vec3& i0, Vec3& i1)
    {
        Vec3 end = line.pointonline + line.direction;

        float a = line.direction | line.direction;
        float b = (line.direction | (line.pointonline - s.center)) * 2.0f;
        float c = ((line.pointonline - s.center) | (line.pointonline - s.center)) - (s.radius * s.radius);

        float desc = (b * b) - (4 * a * c);

        unsigned char intersection = 0;
        if (desc >= 0.0f)
        {
            float lamba0 = (-b - sqrt_tpl(desc)) / (2.0f * a);
            //_stprintf(d3dApp.token,"lamba0: %20.12f",lamba0);
            //d3dApp.m_pFont->DrawText( 2, d3dApp.PrintY, D3DCOLOR_ARGB(255,255,255,0), d3dApp.token ); d3dApp.PrintY+=20;
            i0 = line.pointonline + ((end - line.pointonline) * lamba0);
            intersection = 1;

            float lamba1 = (-b + sqrt_tpl(desc)) / (2.0f * a);
            //_stprintf(d3dApp.token,"lamba1: %20.12f",lamba1);
            //d3dApp.m_pFont->DrawText( 2, d3dApp.PrintY, D3DCOLOR_ARGB(255,255,255,0), d3dApp.token ); d3dApp.PrintY+=20;
            i1 = line.pointonline + ((end - line.pointonline) * lamba1);
            intersection |= 2;
        }

        return intersection;
    }



    //----------------------------------------------------------------------------------
    //--- 0x00 = no intersection                               --------------------------
    //--- 0x01 = not possible   --
    //--- 0x02 = one intersection, lineseg has just an EXIT point but no ENTRY point (ls.start is inside the sphere)  --
    //--- 0x03 = two intersection, lineseg has ENTRY and EXIT point  --
    //----------------------------------------------------------------------------------

    inline unsigned char Ray_Sphere(const Ray& ray, const Sphere& s, Vec3& i0, Vec3& i1)
    {
        Vec3 end = ray.origin + ray.direction;
        float a = ray.direction | ray.direction;
        float b = (ray.direction | (ray.origin - s.center)) * 2.0f;
        float c = ((ray.origin - s.center) | (ray.origin - s.center)) - (s.radius * s.radius);

        float desc = (b * b) - (4 * a * c);

        unsigned char intersection = 0;
        if (desc >= 0.0f)
        {
            float lamba0 = (-b - sqrt_tpl(desc)) / (2.0f * a);
            //  _stprintf(d3dApp.token,"lamba0: %20.12f",lamba0);
            //  d3dApp.m_pFont->DrawText( 2, d3dApp.PrintY, D3DCOLOR_ARGB(255,255,255,0), d3dApp.token );   d3dApp.PrintY+=20;
            if (lamba0 > 0.0f)
            {
                i0 = ray.origin + ((end - ray.origin) * lamba0);
                intersection = 1;
            }

            float lamba1 = (-b + sqrt_tpl(desc)) / (2.0f * a);
            //  _stprintf(d3dApp.token,"lamba1: %20.12f",lamba1);
            //  d3dApp.m_pFont->DrawText( 2, d3dApp.PrintY, D3DCOLOR_ARGB(255,255,255,0), d3dApp.token );   d3dApp.PrintY+=20;
            if (lamba1 > 0.0f)
            {
                i1 = ray.origin + ((end - ray.origin) * lamba1);
                intersection |= 2;
            }
        }
        return intersection;
    }

    inline bool Ray_SphereFirst(const Ray& ray, const Sphere& s, Vec3& intPoint)
    {
        Vec3 p2;
        unsigned char res = Ray_Sphere(ray, s, intPoint, p2);
        if (res == 2)
        {
            intPoint = p2;
        }
        if (res > 1)
        {
            return true;
        }
        return false;
    }



    //----------------------------------------------------------------------------------
    //--- 0x00 = no intersection                               --------------------------
    //--- 0x01 = one intersection, lineseg has just an ENTRY point but no EXIT point (ls.end is inside the sphere)  --
    //--- 0x02 = one intersection, lineseg has just an EXIT point but no ENTRY point (ls.start is inside the sphere)  --
    //--- 0x03 = two intersection, lineseg has ENTRY and EXIT point  --
    //----------------------------------------------------------------------------------
    inline unsigned char Lineseg_Sphere(const Lineseg& ls, const Sphere& s, Vec3& i0, Vec3& i1)
    {
        Vec3 dir = (ls.end - ls.start);

        float a = dir | dir;
        if (a == 0.0f)
        {
            return 0;
        }

        float b = (dir | (ls.start - s.center)) * 2.0f;
        float c = ((ls.start - s.center) | (ls.start - s.center)) - (s.radius * s.radius);
        float desc = (b * b) - (4 * a * c);

        unsigned char intersection = 0;
        if (desc >= 0.0f)
        {
            float lamba0 = (-b - sqrt_tpl(desc)) / (2.0f * a);
            if (lamba0 > 0.0f)
            {
                i0 = ls.start + ((ls.end - ls.start) * lamba0);
                //skip, if 1st cutting-point is "in front" of ls.end
                if (((i0 - ls.end) | dir) > 0)
                {
                    return 0;
                }
                intersection = 0x01;
            }

            float lamba1 = (-b + sqrt_tpl(desc)) / (2.0f * a);
            if (lamba1 > 0.0f)
            {
                i1 = ls.start + ((ls.end - ls.start) * lamba1);
                //skip, if 2nd cutting-point is "in front" of ls.end (=ls.end is inside sphere)
                if (((i1 - ls.end) | dir) > 0)
                {
                    return intersection;
                }
                intersection |= 0x02;
            }
        }
        return intersection;
    }


    inline bool Lineseg_SphereFirst(const Lineseg& lineseg, const Sphere& s, Vec3& intPoint)
    {
        Vec3 p2;
        uint8 res = Lineseg_Sphere(lineseg, s, intPoint, p2);
        if (res == 2)
        {
            intPoint = p2;
        }
        if (res > 1)
        {
            return true;
        }
        return false;
    }
}; //CIntersect














#endif // CRYINCLUDE_CRYCOMMON_CRY_GEOINTERSECT_H