SAPDivision.cpp

/* Copyright (C) 2013 Wildfire Games.
 * This file is part of 0 A.D.
 *
 * 0 A.D. is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 2 of the License, or
 * (at your option) any later version.
 *
 * 0 A.D. is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with 0 A.D.  If not, see <http://www.gnu.org/licenses/>.
 */
//#include "precompiled.h"
#include "SAPDivision.h"




    SAPDivision::SAPDivision()
    {
        m_XAxis.reserve(1024);
    }
    SAPDivision::~SAPDivision()
    {
        Destroy();
    }


    SAPDivision::SAPDivision(const SAPDivision& rhs)
    {
        this->operator=(rhs);
    }
    SAPDivision& SAPDivision::operator=(const SAPDivision& rhs)
    {
        if (this != &rhs)
        {
            Clear();

            if (int count = rhs.m_XAxis.size())
            {
                m_XAxis.resize(count);
                memcpy(&m_XAxis[0], &rhs.m_XAxis[0], sizeof(SAPNode) * count);
                m_LiveHandles.reserve(count);
                for (int i = 0; i < count; ++i)
                {
                    SAPHandle* handle = AllocHandle();
                    handle->index = i;
                    handle->entity = m_XAxis[i].handle->entity; // grab ptr from other Handle
                    m_XAxis[i].handle = handle; // set the new handle
                    m_LiveHandles.push_back(handle);
                }
            }
        }
        return *this;
    }


    SAPHandle* SAPDivision::AllocHandle()
    {
        SAPHandle* handle;
        if (!m_FreeHandles.empty()) // try to reuse a handle in freehandles
        {
            handle = m_FreeHandles.back();
            m_FreeHandles.pop_back();
        }
        else handle = new SAPHandle;
        m_LiveHandles.push_back(handle);
        return handle;
    }


    void SAPDivision::Clear()
    {
        for (size_t i = 0; i < m_LiveHandles.size(); ++i)
            m_FreeHandles.push_back(m_LiveHandles[i]); // push all live handles to freed
        m_XAxis.clear();
        m_LiveHandles.clear();
        m_FreeHandles.clear();
    }


    void SAPDivision::Destroy()
    {
        for (size_t i = 0; i < m_LiveHandles.size(); ++i)
            delete m_LiveHandles[i];
        for (size_t i = 0; i < m_FreeHandles.size(); ++i)
            delete m_FreeHandles[i];
        m_XAxis.clear();
        m_LiveHandles.clear();
        m_FreeHandles.clear();
    }


    // fixes axis indices starting from the specified index (inclusive)
    // fix_axis(Axis, 0, Axis.size() - 1) iterates and fixes the entire axis
    static void axis_fix_indices(SAPNode* axis, int fromIndex, int toIndex)
    {
        for (int i = fromIndex; i <= toIndex; ++i)
            axis[i].handle->index = i;
    }
    static int axis_insert_node(std::vector<SAPNode>& axis, const SAPNode& node)
    {
        int imin = 0;
        if(int imax = axis.size())
        {
            SAPNode* data = &axis[0];
            float targetXMin = node.x_min;
            while(imin < imax) // binary-search
            {
                const int imid = (imin + imax) >> 1;
                if(data[imid].x_min < targetXMin)
                    imin = imid + 1;
                else
                    imax = imid;
            }
        }
        axis.insert(axis.begin() + imin, node);
        return imin;
    }
    SAPHandle* SAPDivision::Insert(CEntityHandle* entity, const Vector2& toMin, const Vector2& toMax)
    {
        SAPHandle* handle = AllocHandle();

        SAPNode node; // create new SAPNode:
        node.x_min = toMin.x;
        node.x_max = toMax.x;
        node.z_min = toMin.y;
        node.z_max = toMax.y;
        node.x_pos = (toMin.x + toMax.x) / 2;
        node.z_pos = (toMin.y + toMax.y) / 2;
        node.handle = handle;

        int index = axis_insert_node(m_XAxis, node);
        axis_fix_indices(&m_XAxis[0], index + 1, m_XAxis.size() - 1);

        handle->index = index;
        handle->entity = entity;
        return handle;
    }

    static void erase_first(std::vector<SAPHandle*>& vec, const SAPHandle* value)
    {
        if (const int count = vec.size()) {
            SAPHandle** data = &vec[0];
            for (int i = 0; i < count; ++i)
                if (data[i] == value)
                    { vec.erase(vec.begin() + i); return; }
        }
    }
    void SAPDivision::Remove(SAPHandle* handle)
    {
        m_XAxis.erase(m_XAxis.begin() + handle->index);
        axis_fix_indices(&m_XAxis[0], handle->index, m_XAxis.size() - 1);

        // remove from live handles and throw it into freelist, so we can reuse it cheaply
        erase_first(m_LiveHandles, handle);
        m_FreeHandles.push_back(handle);
    }


    static bool swap_handle(SAPNode* pOld, SAPNode* pNew)
    {
        SAPNode temp = *pOld;
        intptr_t size = (intptr_t)pNew - (intptr_t)pOld; // get the difference
        bool forward;
        if (size > 0)   forward = true;                 // is the target forward in axis?
        else            forward = false, size = -size;  // the target is backward in axis.

        if (size == sizeof(SAPNode)) // only 1 element swap
        {
            *pOld = *pNew;
            *pNew = temp;
            return false; // no change to axis order
        }
        if (forward) memmove(pOld, pOld + 1, size); // move the whole block backwards
        else         memmove(pOld + 1, pOld, size); // move the whole block forward
        *pNew = temp; // write the handle
        return true; // elements were shifted, axis is invalidated :(
    }
    void SAPDivision::Move(SAPHandle* handle, const Vector2& pos)
    {
        const int iOld  = handle->index;        // old (current) axis index
        SAPNode* xAxis  = &m_XAxis[0];
        SAPNode* node   = &xAxis[iOld];

        float dX = pos.x - node->x_pos; // delta movement in X axis
        if (dX != 0.0f) // move in XAXIS
        {
            const int iLastX = m_XAxis.size() - 1;      // last valid index
            const float xMin = node->x_min + dX;        // new MIN value in X axis
            int iNew         = iOld;                    // new axis index

            if (dX > 0.0f)
                 while (iNew != iLastX && xAxis[iNew + 1].x_min < xMin) ++iNew; // forward in X axis
            else while (iNew != 0 && xAxis[iNew - 1].x_min > xMin) --iNew;      // backwards in X axis

            if (iNew != iOld) // did anything change?
            {
                if (swap_handle(xAxis + iOld, xAxis + iNew))
                {
                    axis_fix_indices(xAxis, iOld, iNew);    // update a range of indices
                }
                else
                {
                    handle->index = iNew;                   // update the swapped value indices:
                    xAxis[iOld].handle->index = iOld;
                }
                node = &xAxis[iNew]; // make sure to update the pointer after we swapped
            }
            node->x_min += dX;
            node->x_max += dX;
            node->x_pos = pos.x; // update position
        }

        float dZ = pos.y - node->z_pos; // delta movement in Z axis
        if (dZ != 0.0f) // move in ZAXIS
        {
            node->z_min += dZ;
            node->z_max += dZ;
            node->z_pos = pos.y;
        }
    }


    static inline void insert_inrange(SAPRangeQuery& inrange, SAPHandle* handle, float sqrange)
    {
        if (inrange.count == SAPRangeQuery::MAX_COUNT)
        {
            assert(0 && "SAPRangeQuery::MAX_COUNT[2048] exceeded! Increase capacity.");
            return;
        }
        SAPRangeQuery::Result& r = inrange.items[inrange.count++];
        r.sqrange = sqrange;
        r.handle = handle;
    }

    void SAPDivision::GetInBoundingBox(SAPRangeQuery& inrange, const Vector2& min, const Vector2& max)
    {

    }

    int SAPDivision::GetInEntityRange(SAPRangeQuery& inrange, SAPHandle* handle, float range)
    {
        inrange.clear();

        SAPNode* xAxis  = &m_XAxis[0];
        SAPNode* xEnd   = &xAxis[m_XAxis.size()];
        SAPNode* left   = &xAxis[handle->index - 1];
        SAPNode* right  = &xAxis[handle->index + 1];

        const float sqRange = range * range;
        const float outRange = 2 * range;
        const float x_pos   = xAxis[handle->index].x_pos;
        const float z_pos   = xAxis[handle->index].z_pos;

        int iterations = 0;

        for (; left >= xAxis; --left) // scan LEFT in X axis
        {
            ++iterations;
            const float dx = x_pos - left->x_max;
            if (dx > range)
                break; // not in range and there will no longer be any in X axis

            float dz;
            if (left->z_pos > z_pos)    dz = left->z_min - z_pos; // target is UP in Z axis
            else                        dz = z_pos - left->z_max; // target is DOWN in Z axis
            if (dz > range)
                continue; // negative match, continue scanning X axis

            const float sqlen = dx*dx + dz*dz;
            if (sqlen <= sqRange) // is it in range?
            {
                insert_inrange(inrange, left->handle, sqlen); // add it to the list
            }
        }

        for (; right != xEnd; ++right) // scan RIGHT in X axis
        {
            ++iterations;
            const float dx = right->x_min - x_pos;
            if (dx > range)
                break; // not in range and there will no longer be any in X axis

            float dz;
            if (right->z_pos > z_pos)   dz = right->z_min - z_pos; // target is UP in Z axis
            else                        dz = z_pos - right->z_max; // target is DOWN in Z axis
            if (dz > range)
                continue; // negative match, continue scanning X axis

            const float sqlen = dx*dx + dz*dz;
            if (sqlen <= sqRange) // is it in range?
            {
                insert_inrange(inrange, right->handle, sqlen); // add it to the list
            }
        }
        return iterations;
    }