///////////////////////////////////////////////////////////////////////////////////////////////////
// OpenGL Mathematics Copyright (c) 2005 - 2014 G-Truc Creation (www.g-truc.net)
///////////////////////////////////////////////////////////////////////////////////////////////////
// Created : 2007-04-03
// Updated : 2009-01-20
// Licence : This source is under MIT licence
// File    : glm/gtx/intersect.inl
///////////////////////////////////////////////////////////////////////////////////////////////////

#include "../geometric.hpp"
#include <cfloat>
#include <limits>

namespace glm
{
        template <typename genType>
        GLM_FUNC_QUALIFIER bool intersectRayPlane
        (
                genType const & orig, genType const & dir,
                genType const & planeOrig, genType const & planeNormal,
                typename genType::value_type & intersectionDistance
        )
        {
                typename genType::value_type d = glm::dot(dir, planeNormal);
                typename genType::value_type Epsilon = std::numeric_limits<typename genType::value_type>::epsilon();

                if(d < Epsilon)
                {
                        intersectionDistance = glm::dot(planeOrig - orig, planeNormal) / d;
                        return true;
                }

                return false;
        }

        template <typename genType>
        GLM_FUNC_QUALIFIER bool intersectRayTriangle
        (
                genType const & orig, genType const & dir,
                genType const & v0, genType const & v1, genType const & v2,
                genType & baryPosition
        )
        {
                genType e1 = v1 - v0;
                genType e2 = v2 - v0;

                genType p = glm::cross(dir, e2);

                typename genType::value_type a = glm::dot(e1, p);

                typename genType::value_type Epsilon = std::numeric_limits<typename genType::value_type>::epsilon();
                if(a < Epsilon)
                        return false;

                typename genType::value_type f = typename genType::value_type(1.0f) / a;

                genType s = orig - v0;
                baryPosition.x = f * glm::dot(s, p);
                if(baryPosition.x < typename genType::value_type(0.0f))
                        return false;
                if(baryPosition.x > typename genType::value_type(1.0f))
                        return false;

                genType q = glm::cross(s, e1);
                baryPosition.y = f * glm::dot(dir, q);
                if(baryPosition.y < typename genType::value_type(0.0f))
                        return false;
                if(baryPosition.y + baryPosition.x > typename genType::value_type(1.0f))
                        return false;

                baryPosition.z = f * glm::dot(e2, q);

                return baryPosition.z >= typename genType::value_type(0.0f);
        }

        //template <typename genType>
        //GLM_FUNC_QUALIFIER bool intersectRayTriangle
        //(
        //      genType const & orig, genType const & dir,
        //      genType const & vert0, genType const & vert1, genType const & vert2,
        //      genType & position
        //)
        //{
        //      typename genType::value_type Epsilon = std::numeric_limits<typename genType::value_type>::epsilon();
        //
        //      genType edge1 = vert1 - vert0;
        //      genType edge2 = vert2 - vert0;
        //
        //      genType pvec = cross(dir, edge2);
        //
        //      float det = dot(edge1, pvec);
        //      if(det < Epsilon)
        //              return false;
        //
        //      genType tvec = orig - vert0;
        //
        //      position.y = dot(tvec, pvec);
        //      if (position.y < typename genType::value_type(0) || position.y > det)
        //              return typename genType::value_type(0);
        //
        //      genType qvec = cross(tvec, edge1);
        //
        //      position.z = dot(dir, qvec);
        //      if (position.z < typename genType::value_type(0) || position.y + position.z > det)
        //              return typename genType::value_type(0);
        //
        //      position.x = dot(edge2, qvec);
        //      position *= typename genType::value_type(1) / det;
        //
        //      return typename genType::value_type(1);
        //}

        template <typename genType>
        GLM_FUNC_QUALIFIER bool intersectLineTriangle
        (
                genType const & orig, genType const & dir,
                genType const & vert0, genType const & vert1, genType const & vert2,
                genType & position
        )
        {
                typename genType::value_type Epsilon = std::numeric_limits<typename genType::value_type>::epsilon();

                genType edge1 = vert1 - vert0;
                genType edge2 = vert2 - vert0;

                genType pvec = cross(dir, edge2);

                float det = dot(edge1, pvec);

                if (det > -Epsilon && det < Epsilon)
                        return false;
                float inv_det = typename genType::value_type(1) / det;

                genType tvec = orig - vert0;

                position.y = dot(tvec, pvec) * inv_det;
                if (position.y < typename genType::value_type(0) || position.y > typename genType::value_type(1))
                        return false;

                genType qvec = cross(tvec, edge1);

                position.z = dot(dir, qvec) * inv_det;
                if (position.z < typename genType::value_type(0) || position.y + position.z > typename genType::value_type(1))
                        return false;

                position.x = dot(edge2, qvec) * inv_det;

                return true;
        }

        template <typename genType>
        GLM_FUNC_QUALIFIER bool intersectRaySphere
        (
                genType const & rayStarting, genType const & rayNormalizedDirection,
                genType const & sphereCenter, const typename genType::value_type sphereRadiusSquered,
                typename genType::value_type & intersectionDistance
        )
        {
                typename genType::value_type Epsilon = std::numeric_limits<typename genType::value_type>::epsilon();
                genType diff = sphereCenter - rayStarting;
                typename genType::value_type t0 = dot(diff, rayNormalizedDirection);
                typename genType::value_type dSquared = dot(diff, diff) - t0 * t0;
                if( dSquared > sphereRadiusSquered )
                {
                        return false;
                }
                typename genType::value_type t1 = sqrt( sphereRadiusSquered - dSquared );
                intersectionDistance = t0 > t1 + Epsilon ? t0 - t1 : t0 + t1;
                return intersectionDistance > Epsilon;
        }

        template <typename genType>
        GLM_FUNC_QUALIFIER bool intersectRaySphere
        (
                genType const & rayStarting, genType const & rayNormalizedDirection,
                genType const & sphereCenter, const typename genType::value_type sphereRadius,
                genType & intersectionPosition, genType & intersectionNormal
        )
        {
                typename genType::value_type distance;
                if( intersectRaySphere( rayStarting, rayNormalizedDirection, sphereCenter, sphereRadius * sphereRadius, distance ) )
                {
                        intersectionPosition = rayStarting + rayNormalizedDirection * distance;
                        intersectionNormal = (intersectionPosition - sphereCenter) / sphereRadius;
                        return true;
                }
                return false;
        }

        template <typename genType>
        GLM_FUNC_QUALIFIER bool intersectLineSphere
        (
                genType const & point0, genType const & point1,
                genType const & sphereCenter, typename genType::value_type sphereRadius,
                genType & intersectionPoint1, genType & intersectionNormal1, 
                genType & intersectionPoint2, genType & intersectionNormal2
        )
        {
                typename genType::value_type Epsilon = std::numeric_limits<typename genType::value_type>::epsilon();
                genType dir = normalize(point1 - point0);
                genType diff = sphereCenter - point0;
                typename genType::value_type t0 = dot(diff, dir);
                typename genType::value_type dSquared = dot(diff, diff) - t0 * t0;
                if( dSquared > sphereRadius * sphereRadius )
                {
                        return false;
                }
                typename genType::value_type t1 = sqrt( sphereRadius * sphereRadius - dSquared );
                if( t0 < t1 + Epsilon )
                        t1 = -t1;
                intersectionPoint1 = point0 + dir * (t0 - t1);
                intersectionNormal1 = (intersectionPoint1 - sphereCenter) / sphereRadius;
                intersectionPoint2 = point0 + dir * (t0 + t1);
                intersectionNormal2 = (intersectionPoint2 - sphereCenter) / sphereRadius;
                return true;
        }
}//namespace glm