CollisionBvhRayCaster.h

// Copyright eeGeo Ltd (2012-2014), All Rights Reserved

#pragma once

#include "Collision.h"
#include "VectorMath.h"
#include "RayCasterResult.h"
#include "CollisionBvh.h"
#include "CollisionBvhIntersectionQueries.h"
#include "RayCasterFirstIntersectionBuilder.h"

#include <vector>
#include <algorithm>
#include <iterator>

namespace Eegeo
{
    namespace Collision
    {
        template <typename TCollisionBvhContainer, typename TRayNodeIntersectionResults >
        class CollisionBvhRayCaster : public Eegeo::NonCopyable
        {
        public:
            typedef typename TCollisionBvhContainer::const_iterator TCollisionBvhIter;
            
            CollisionBvhRayCaster(const TCollisionBvhContainer& collisionBvhs, 
                TRayNodeIntersectionResults& rayNodeIntersectionsScratch, 
                std::function<SkewedRay(const v3&, const v3&, const dv3&, const float)> rayToWorldSpaceTransform,
                const IVerifyRaycastResultDelegate& intersectionValidator)
            : m_collisionBvhs(collisionBvhs)
            , m_rayNodeIntersectionsScratch(rayNodeIntersectionsScratch)
            , m_rayUnflatteningOperation(rayToWorldSpaceTransform)
            , m_intersectionValidator(intersectionValidator)
            {}
            
            inline RayCasterResult FindFirstRayIntersection(const float environmentFlatteningScale, const Eegeo::dv3& rayOrigin, const Eegeo::dv3& rayDirection) const;
            
        private:
            const TCollisionBvhContainer& m_collisionBvhs;
            TRayNodeIntersectionResults& m_rayNodeIntersectionsScratch;
            std::function<SkewedRay(const v3&, const v3&, const dv3&, const float)> m_rayUnflatteningOperation;
            const IVerifyRaycastResultDelegate& m_intersectionValidator;
        };
        
        namespace
        {
            inline bool RayNodeIntersectionResultSortByT(const Eegeo::Collision::RayNodeIntersectionResult& _a, const Eegeo::Collision::RayNodeIntersectionResult& _b)
            {
                return _a.IntersectionParam() < _b.IntersectionParam();
            }
            
            template <typename TCollisionBvhIter, typename TOutIter>
            TOutIter GatherLeafNodesIntersectingRay(
                std::function<SkewedRay(const v3&, const v3&, const dv3&, const float)> rayUnflatteningOperation,
                const float environmentFlatteningScale, 
                const Eegeo::dv3& rayOrigin, 
                const Eegeo::v3& rayDirection, 
                TCollisionBvhIter first, 
                TCollisionBvhIter last, 
                TOutIter outIter)
            {
                for (TCollisionBvhIter iter = first; iter != last; ++iter)
                {
                    const Eegeo::Collision::CollisionBvh& collisionBvh = **iter;
                    
                    v3 meshRelativeRayOrigin = (rayOrigin - collisionBvh.OriginEcef()).ToSingle();
                    
                    const SkewedRay& skewedRay = rayUnflatteningOperation(meshRelativeRayOrigin, rayDirection, collisionBvh.OriginEcef(), environmentFlatteningScale);
                    
                    GetLeafNodesIntersectingSkewedRay(collisionBvh, skewedRay, outIter);
                }
                return outIter;
            }
        }
       
        template <typename TCollisionBvhContainer, typename TRayNodeIntersectionResults>
        inline RayCasterResult
        CollisionBvhRayCaster<TCollisionBvhContainer, TRayNodeIntersectionResults>::FindFirstRayIntersection(
            const float environmentFlatteningScale, const Eegeo::dv3& rayOrigin, const Eegeo::dv3& rayDirection) const
        {
            m_rayNodeIntersectionsScratch.clear();
            GatherLeafNodesIntersectingRay(m_rayUnflatteningOperation, environmentFlatteningScale, rayOrigin, rayDirection.ToSingle(), m_collisionBvhs.begin(), m_collisionBvhs.end(), std::back_inserter(m_rayNodeIntersectionsScratch));
                        
            std::sort(m_rayNodeIntersectionsScratch.begin(), m_rayNodeIntersectionsScratch.end(), RayNodeIntersectionResultSortByT);
            
            RayCasterFirstIntersectionBuilder firstIntersectionBuilder(rayOrigin, rayDirection, environmentFlatteningScale, m_intersectionValidator);
            
            for (const auto& intersection : m_rayNodeIntersectionsScratch)
            {
                bool shouldContinue = firstIntersectionBuilder(intersection);
                if (!shouldContinue)
                {
                    break;
                }
            }
            
            return firstIntersectionBuilder.BuildResult();
        }
    }
}