godot/thirdparty/bullet/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp

/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/


#include "btBatchedConstraints.h"

#include "LinearMath/btIDebugDraw.h"
#include "LinearMath/btMinMax.h"
#include "LinearMath/btStackAlloc.h"
#include "LinearMath/btQuickprof.h"

#include <string.h> //for memset

const int kNoMerge = -1;

bool btBatchedConstraints::s_debugDrawBatches = false;


struct btBatchedConstraintInfo
{
    int constraintIndex;
    int numConstraintRows;
    int bodyIds[2];
};


struct btBatchInfo
{
    int numConstraints;
    int mergeIndex;

    btBatchInfo() : numConstraints(0), mergeIndex(kNoMerge) {}
};


bool btBatchedConstraints::validate(btConstraintArray* constraints, const btAlignedObjectArray<btSolverBody>& bodies) const
{
    //
    // validate: for debugging only. Verify coloring of bodies, that no body is touched by more than one batch in any given phase
    //
    int errors = 0;
    const int kUnassignedBatch = -1;

    btAlignedObjectArray<int> bodyBatchId;
    for (int iPhase = 0; iPhase < m_phases.size(); ++iPhase)
    {
        bodyBatchId.resizeNoInitialize(0);
        bodyBatchId.resize( bodies.size(), kUnassignedBatch );
        const Range& phase = m_phases[iPhase];
        for (int iBatch = phase.begin; iBatch < phase.end; ++iBatch)
        {
            const Range& batch = m_batches[iBatch];
            for (int iiCons = batch.begin; iiCons < batch.end; ++iiCons)
            {
                int iCons = m_constraintIndices[iiCons];
                const btSolverConstraint& cons = constraints->at(iCons);
                const btSolverBody& bodyA = bodies[cons.m_solverBodyIdA];
                const btSolverBody& bodyB = bodies[cons.m_solverBodyIdB];
                if (! bodyA.internalGetInvMass().isZero())
                {
                    int thisBodyBatchId = bodyBatchId[cons.m_solverBodyIdA];
                    if (thisBodyBatchId == kUnassignedBatch)
                    {
                        bodyBatchId[cons.m_solverBodyIdA] = iBatch;
                    }
                    else if (thisBodyBatchId != iBatch)
                    {
                        btAssert( !"dynamic body is used in 2 different batches in the same phase" );
                        errors++;
                    }
                }
                if (! bodyB.internalGetInvMass().isZero())
                {
                    int thisBodyBatchId = bodyBatchId[cons.m_solverBodyIdB];
                    if (thisBodyBatchId == kUnassignedBatch)
                    {
                        bodyBatchId[cons.m_solverBodyIdB] = iBatch;
                    }
                    else if (thisBodyBatchId != iBatch)
                    {
                        btAssert( !"dynamic body is used in 2 different batches in the same phase" );
                        errors++;
                    }
                }
            }
        }
    }
    return errors == 0;
}


static void debugDrawSingleBatch( const btBatchedConstraints* bc,
    btConstraintArray* constraints,
    const btAlignedObjectArray<btSolverBody>& bodies,
    int iBatch,
    const btVector3& color,
    const btVector3& offset
    )
{
    if (bc && bc->m_debugDrawer && iBatch < bc->m_batches.size())
    {
        const btBatchedConstraints::Range& b = bc->m_batches[iBatch];
        for (int iiCon = b.begin; iiCon < b.end; ++iiCon)
        {
            int iCon = bc->m_constraintIndices[iiCon];
            const btSolverConstraint& con = constraints->at(iCon);
            int iBody0 = con.m_solverBodyIdA;
            int iBody1 = con.m_solverBodyIdB;
            btVector3 pos0 = bodies[iBody0].getWorldTransform().getOrigin() + offset;
            btVector3 pos1 = bodies[iBody1].getWorldTransform().getOrigin() + offset;
            bc->m_debugDrawer->drawLine(pos0, pos1, color);
        }
    }
}


static void debugDrawPhase( const btBatchedConstraints* bc,
    btConstraintArray* constraints,
    const btAlignedObjectArray<btSolverBody>& bodies,
    int iPhase,
    const btVector3& color0,
    const btVector3& color1,
    const btVector3& offset
    )
{
    BT_PROFILE( "debugDrawPhase" );
    if ( bc && bc->m_debugDrawer && iPhase < bc->m_phases.size() )
    {
        const btBatchedConstraints::Range& phase = bc->m_phases[iPhase];
        for (int iBatch = phase.begin; iBatch < phase.end; ++iBatch)
        {
            float tt = float(iBatch - phase.begin) / float(btMax(1, phase.end - phase.begin - 1));
            btVector3 col = lerp(color0, color1, tt);
            debugDrawSingleBatch(bc, constraints, bodies, iBatch, col, offset);
        }
    }
}


static void debugDrawAllBatches( const btBatchedConstraints* bc,
    btConstraintArray* constraints,
    const btAlignedObjectArray<btSolverBody>& bodies
    )
{
    BT_PROFILE( "debugDrawAllBatches" );
    if ( bc && bc->m_debugDrawer && bc->m_phases.size() > 0 )
    {
        btVector3 bboxMin(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
        btVector3 bboxMax = -bboxMin;
        for (int iBody = 0; iBody < bodies.size(); ++iBody)
        {
            const btVector3& pos = bodies[iBody].getWorldTransform().getOrigin();
            bboxMin.setMin(pos);
            bboxMax.setMax(pos);
        }
        btVector3 bboxExtent = bboxMax - bboxMin;
        btVector3 offsetBase = btVector3( 0, bboxExtent.y()*1.1f, 0 );
        btVector3 offsetStep = btVector3( 0, 0, bboxExtent.z()*1.1f );
        int numPhases = bc->m_phases.size();
        for (int iPhase = 0; iPhase < numPhases; ++iPhase)
        {
            float b = float(iPhase)/float(numPhases-1);
            btVector3 color0 = btVector3(1,0,b);
            btVector3 color1 = btVector3(0,1,b);
            btVector3 offset = offsetBase + offsetStep*(float(iPhase) - float(numPhases-1)*0.5);
            debugDrawPhase(bc, constraints, bodies, iPhase, color0, color1, offset);
        }
    }
}


static void initBatchedBodyDynamicFlags(btAlignedObjectArray<bool>* outBodyDynamicFlags, const btAlignedObjectArray<btSolverBody>& bodies)
{
    BT_PROFILE("initBatchedBodyDynamicFlags");
    btAlignedObjectArray<bool>& bodyDynamicFlags = *outBodyDynamicFlags;
    bodyDynamicFlags.resizeNoInitialize(bodies.size());
    for (int i = 0; i < bodies.size(); ++i)
    {
        const btSolverBody& body = bodies[ i ];
        bodyDynamicFlags[i] = ( body.internalGetInvMass().x() > btScalar( 0 ) );
    }
}


static int runLengthEncodeConstraintInfo(btBatchedConstraintInfo* outConInfos, int numConstraints)
{
    BT_PROFILE("runLengthEncodeConstraintInfo");
    // detect and run-length encode constraint rows that repeat the same bodies
    int iDest = 0;
    int iSrc = 0;
    while (iSrc < numConstraints)
    {
        const btBatchedConstraintInfo& srcConInfo = outConInfos[iSrc];
        btBatchedConstraintInfo& conInfo = outConInfos[iDest];
        conInfo.constraintIndex = iSrc;
        conInfo.bodyIds[0] = srcConInfo.bodyIds[0];
        conInfo.bodyIds[1] = srcConInfo.bodyIds[1];
        while (iSrc < numConstraints && outConInfos[iSrc].bodyIds[0] == srcConInfo.bodyIds[0] && outConInfos[iSrc].bodyIds[1] == srcConInfo.bodyIds[1])
        {
            ++iSrc;
        }
        conInfo.numConstraintRows = iSrc - conInfo.constraintIndex;
        ++iDest;
    }
    return iDest;
}


struct ReadSolverConstraintsLoop : public btIParallelForBody
{
    btBatchedConstraintInfo* m_outConInfos;
    btConstraintArray* m_constraints;

    ReadSolverConstraintsLoop( btBatchedConstraintInfo* outConInfos, btConstraintArray* constraints )
    {
        m_outConInfos = outConInfos;
        m_constraints = constraints;
    }
    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
    {
        for (int i = iBegin; i < iEnd; ++i)
        {
            btBatchedConstraintInfo& conInfo = m_outConInfos[i];
            const btSolverConstraint& con = m_constraints->at( i );
            conInfo.bodyIds[0] = con.m_solverBodyIdA;
            conInfo.bodyIds[1] = con.m_solverBodyIdB;
            conInfo.constraintIndex = i;
            conInfo.numConstraintRows = 1;
        }
    }
};


static int initBatchedConstraintInfo(btBatchedConstraintInfo* outConInfos, btConstraintArray* constraints)
{
    BT_PROFILE("initBatchedConstraintInfo");
    int numConstraints = constraints->size();
    bool inParallel = true;
    if (inParallel)
    {
        ReadSolverConstraintsLoop loop(outConInfos, constraints);
        int grainSize = 1200;
        btParallelFor(0, numConstraints, grainSize, loop);
    }
    else
    {
        for (int i = 0; i < numConstraints; ++i)
        {
            btBatchedConstraintInfo& conInfo = outConInfos[i];
            const btSolverConstraint& con = constraints->at( i );
            conInfo.bodyIds[0] = con.m_solverBodyIdA;
            conInfo.bodyIds[1] = con.m_solverBodyIdB;
            conInfo.constraintIndex = i;
            conInfo.numConstraintRows = 1;
        }
    }
    bool useRunLengthEncoding = true;
    if (useRunLengthEncoding)
    {
        numConstraints = runLengthEncodeConstraintInfo(outConInfos, numConstraints);
    }
    return numConstraints;
}


static void expandConstraintRowsInPlace(int* constraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows)
{
    BT_PROFILE("expandConstraintRowsInPlace");
    if (numConstraintRows > numConstraints)
    {
        // we walk the array in reverse to avoid overwriteing
        for (int iCon = numConstraints - 1; iCon >= 0; --iCon)
        {
            const btBatchedConstraintInfo& conInfo = conInfos[iCon];
            int iBatch = constraintBatchIds[iCon];
            for (int i = conInfo.numConstraintRows - 1; i >= 0; --i)
            {
                int iDest = conInfo.constraintIndex + i;
                btAssert(iDest >= iCon);
                btAssert(iDest >= 0 && iDest < numConstraintRows);
                constraintBatchIds[iDest] = iBatch;
            }
        }
    }
}


static void expandConstraintRows(int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows)
{
    BT_PROFILE("expandConstraintRows");
    for ( int iCon = 0; iCon < numConstraints; ++iCon )
    {
        const btBatchedConstraintInfo& conInfo = conInfos[ iCon ];
        int iBatch = srcConstraintBatchIds[ iCon ];
        for ( int i = 0; i < conInfo.numConstraintRows; ++i )
        {
            int iDest = conInfo.constraintIndex + i;
            btAssert( iDest >= iCon );
            btAssert( iDest >= 0 && iDest < numConstraintRows );
            destConstraintBatchIds[ iDest ] = iBatch;
        }
    }
}


struct ExpandConstraintRowsLoop : public btIParallelForBody
{
    int* m_destConstraintBatchIds;
    const int* m_srcConstraintBatchIds;
    const btBatchedConstraintInfo* m_conInfos;
    int m_numConstraintRows;

    ExpandConstraintRowsLoop( int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraintRows)
    {
        m_destConstraintBatchIds = destConstraintBatchIds;
        m_srcConstraintBatchIds = srcConstraintBatchIds;
        m_conInfos = conInfos;
        m_numConstraintRows = numConstraintRows;
    }
    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
    {
        expandConstraintRows(m_destConstraintBatchIds, m_srcConstraintBatchIds + iBegin, m_conInfos + iBegin, iEnd - iBegin, m_numConstraintRows);
    }
};


static void expandConstraintRowsMt(int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows)
{
    BT_PROFILE("expandConstraintRowsMt");
    ExpandConstraintRowsLoop loop(destConstraintBatchIds, srcConstraintBatchIds, conInfos, numConstraintRows);
    int grainSize = 600;
    btParallelFor(0, numConstraints, grainSize, loop);
}


static void initBatchedConstraintInfoArray(btAlignedObjectArray<btBatchedConstraintInfo>* outConInfos, btConstraintArray* constraints)
{
    BT_PROFILE("initBatchedConstraintInfoArray");
    btAlignedObjectArray<btBatchedConstraintInfo>& conInfos = *outConInfos;
    int numConstraints = constraints->size();
    conInfos.resizeNoInitialize(numConstraints);

    int newSize = initBatchedConstraintInfo(&outConInfos->at(0), constraints);
    conInfos.resizeNoInitialize(newSize);
}


static void mergeSmallBatches(btBatchInfo* batches, int iBeginBatch, int iEndBatch, int minBatchSize, int maxBatchSize)
{
    BT_PROFILE("mergeSmallBatches");
    for ( int iBatch = iEndBatch - 1; iBatch >= iBeginBatch; --iBatch )
    {
        btBatchInfo& batch = batches[ iBatch ];
        if ( batch.mergeIndex == kNoMerge && batch.numConstraints > 0 && batch.numConstraints < minBatchSize )
        {
            for ( int iDestBatch = iBatch - 1; iDestBatch >= iBeginBatch; --iDestBatch )
            {
                btBatchInfo& destBatch = batches[ iDestBatch ];
                if ( destBatch.mergeIndex == kNoMerge && ( destBatch.numConstraints + batch.numConstraints ) < maxBatchSize )
                {
                    destBatch.numConstraints += batch.numConstraints;
                    batch.numConstraints = 0;
                    batch.mergeIndex = iDestBatch;
                    break;
                }
            }
        }
    }
    // flatten mergeIndexes
    // e.g. in case where A was merged into B and then B was merged into C, we need A to point to C instead of B
    // Note: loop goes forward through batches because batches always merge from higher indexes to lower,
    //     so by going from low to high it reduces the amount of trail-following
    for ( int iBatch = iBeginBatch; iBatch < iEndBatch; ++iBatch )
    {
        btBatchInfo& batch = batches[ iBatch ];
        if ( batch.mergeIndex != kNoMerge )
        {
            int iMergeDest = batches[ batch.mergeIndex ].mergeIndex;
            // follow trail of merges to the end
            while ( iMergeDest != kNoMerge )
            {
                int iNext = batches[ iMergeDest ].mergeIndex;
                if ( iNext == kNoMerge )
                {
                    batch.mergeIndex = iMergeDest;
                    break;
                }
                iMergeDest = iNext;
            }
        }
    }
}


static void updateConstraintBatchIdsForMerges(int* constraintBatchIds, int numConstraints, const btBatchInfo* batches, int numBatches)
{
    BT_PROFILE("updateConstraintBatchIdsForMerges");
    // update batchIds to account for merges
    for (int i = 0; i < numConstraints; ++i)
    {
        int iBatch = constraintBatchIds[i];
        btAssert(iBatch < numBatches);
        // if this constraint references a batch that was merged into another batch
        if (batches[iBatch].mergeIndex != kNoMerge)
        {
            // update batchId
            constraintBatchIds[i] = batches[iBatch].mergeIndex;
        }
    }
}


struct UpdateConstraintBatchIdsForMergesLoop : public btIParallelForBody
{
    int* m_constraintBatchIds;
    const btBatchInfo* m_batches;
    int m_numBatches;

    UpdateConstraintBatchIdsForMergesLoop( int* constraintBatchIds, const btBatchInfo* batches, int numBatches )
    {
        m_constraintBatchIds = constraintBatchIds;
        m_batches = batches;
        m_numBatches = numBatches;
    }
    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
    {
        BT_PROFILE( "UpdateConstraintBatchIdsForMergesLoop" );
        updateConstraintBatchIdsForMerges( m_constraintBatchIds + iBegin, iEnd - iBegin, m_batches, m_numBatches );
    }
};


static void updateConstraintBatchIdsForMergesMt(int* constraintBatchIds, int numConstraints, const btBatchInfo* batches, int numBatches)
{
    BT_PROFILE( "updateConstraintBatchIdsForMergesMt" );
    UpdateConstraintBatchIdsForMergesLoop loop(constraintBatchIds, batches, numBatches);
    int grainSize = 800;
    btParallelFor(0, numConstraints, grainSize, loop);
}


inline bool BatchCompare(const btBatchedConstraints::Range& a, const btBatchedConstraints::Range& b)
{
    int lenA = a.end - a.begin;
    int lenB = b.end - b.begin;
    return lenA > lenB;
}


static void writeOutConstraintIndicesForRangeOfBatches(btBatchedConstraints* bc,
    const int* constraintBatchIds,
    int numConstraints,
    int* constraintIdPerBatch,
    int batchBegin,
    int batchEnd
    )
{
    BT_PROFILE("writeOutConstraintIndicesForRangeOfBatches");
    for ( int iCon = 0; iCon < numConstraints; ++iCon )
    {
        int iBatch = constraintBatchIds[ iCon ];
        if (iBatch >= batchBegin && iBatch < batchEnd)
        {
            int iDestCon = constraintIdPerBatch[ iBatch ];
            constraintIdPerBatch[ iBatch ] = iDestCon + 1;
            bc->m_constraintIndices[ iDestCon ] = iCon;
        }
    }
}


struct WriteOutConstraintIndicesLoop : public btIParallelForBody
{
    btBatchedConstraints* m_batchedConstraints;
    const int* m_constraintBatchIds;
    int m_numConstraints;
    int* m_constraintIdPerBatch;
    int m_maxNumBatchesPerPhase;

    WriteOutConstraintIndicesLoop( btBatchedConstraints* bc, const int* constraintBatchIds, int numConstraints, int* constraintIdPerBatch, int maxNumBatchesPerPhase )
    {
        m_batchedConstraints = bc;
        m_constraintBatchIds = constraintBatchIds;
        m_numConstraints = numConstraints;
        m_constraintIdPerBatch = constraintIdPerBatch;
        m_maxNumBatchesPerPhase = maxNumBatchesPerPhase;
    }
    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
    {
        BT_PROFILE( "WriteOutConstraintIndicesLoop" );
        int batchBegin = iBegin * m_maxNumBatchesPerPhase;
        int batchEnd = iEnd * m_maxNumBatchesPerPhase;
        writeOutConstraintIndicesForRangeOfBatches(m_batchedConstraints,
            m_constraintBatchIds,
            m_numConstraints,
            m_constraintIdPerBatch,
            batchBegin,
            batchEnd
        );
    }
};


static void writeOutConstraintIndicesMt(btBatchedConstraints* bc,
    const int* constraintBatchIds,
    int numConstraints,
    int* constraintIdPerBatch,
    int maxNumBatchesPerPhase,
    int numPhases
    )
{
    BT_PROFILE("writeOutConstraintIndicesMt");
    bool inParallel = true;
    if (inParallel)
    {
        WriteOutConstraintIndicesLoop loop( bc, constraintBatchIds, numConstraints, constraintIdPerBatch, maxNumBatchesPerPhase );
        btParallelFor( 0, numPhases, 1, loop );
    }
    else
    {
        for ( int iCon = 0; iCon < numConstraints; ++iCon )
        {
            int iBatch = constraintBatchIds[ iCon ];
            int iDestCon = constraintIdPerBatch[ iBatch ];
            constraintIdPerBatch[ iBatch ] = iDestCon + 1;
            bc->m_constraintIndices[ iDestCon ] = iCon;
        }
    }
}


static void writeGrainSizes(btBatchedConstraints* bc)
{
    typedef btBatchedConstraints::Range Range;
    int numPhases = bc->m_phases.size();
    bc->m_phaseGrainSize.resizeNoInitialize(numPhases);
    int numThreads = btGetTaskScheduler()->getNumThreads();
    for (int iPhase = 0; iPhase < numPhases; ++iPhase)
    {
        const Range& phase = bc->m_phases[ iPhase ];
        int numBatches = phase.end - phase.begin;
        float grainSize = floor((0.25f*numBatches / float(numThreads)) + 0.0f);
        bc->m_phaseGrainSize[ iPhase ] = btMax(1, int(grainSize));
    }
}


static void writeOutBatches(btBatchedConstraints* bc,
    const int* constraintBatchIds,
    int numConstraints,
    const btBatchInfo* batches,
    int* batchWork,
    int maxNumBatchesPerPhase,
    int numPhases
)
{
    BT_PROFILE("writeOutBatches");
    typedef btBatchedConstraints::Range Range;
    bc->m_constraintIndices.reserve( numConstraints );
    bc->m_batches.resizeNoInitialize( 0 );
    bc->m_phases.resizeNoInitialize( 0 );

    //int maxNumBatches = numPhases * maxNumBatchesPerPhase;
    {
        int* constraintIdPerBatch = batchWork;  // for each batch, keep an index into the next available slot in the m_constraintIndices array
        int iConstraint = 0;
        for (int iPhase = 0; iPhase < numPhases; ++iPhase)
        {
            int curPhaseBegin = bc->m_batches.size();
            int iBegin = iPhase * maxNumBatchesPerPhase;
            int iEnd = iBegin + maxNumBatchesPerPhase;
            for ( int i = iBegin; i < iEnd; ++i )
            {
                const btBatchInfo& batch = batches[ i ];
                int curBatchBegin = iConstraint;
                constraintIdPerBatch[ i ] = curBatchBegin;  // record the start of each batch in m_constraintIndices array
                int numConstraints = batch.numConstraints;
                iConstraint += numConstraints;
                if ( numConstraints > 0 )
                {
                    bc->m_batches.push_back( Range( curBatchBegin, iConstraint ) );
                }
            }
            // if any batches were emitted this phase,
            if ( bc->m_batches.size() > curPhaseBegin )
            {
                // output phase
                bc->m_phases.push_back( Range( curPhaseBegin, bc->m_batches.size() ) );
            }
        }

        btAssert(iConstraint == numConstraints);
        bc->m_constraintIndices.resizeNoInitialize( numConstraints );
        writeOutConstraintIndicesMt( bc, constraintBatchIds, numConstraints, constraintIdPerBatch, maxNumBatchesPerPhase, numPhases );
    }
    // for each phase
    for (int iPhase = 0; iPhase < bc->m_phases.size(); ++iPhase)
    {
        // sort the batches from largest to smallest (can be helpful to some task schedulers)
        const Range& curBatches = bc->m_phases[iPhase];
        bc->m_batches.quickSortInternal(BatchCompare, curBatches.begin, curBatches.end-1);
    }
    bc->m_phaseOrder.resize(bc->m_phases.size());
    for (int i = 0; i < bc->m_phases.size(); ++i)
    {
        bc->m_phaseOrder[i] = i;
    }
    writeGrainSizes(bc);
}


//
// PreallocatedMemoryHelper -- helper object for allocating a number of chunks of memory in a single contiguous block.
//                             It is generally more efficient to do a single larger allocation than many smaller allocations.
//
// Example Usage:
//
//  btVector3* bodyPositions = NULL;
//  btBatchedConstraintInfo* conInfos = NULL;
//  {
//    PreallocatedMemoryHelper<8> memHelper;
//    memHelper.addChunk( (void**) &bodyPositions, sizeof( btVector3 ) * bodies.size() );
//    memHelper.addChunk( (void**) &conInfos, sizeof( btBatchedConstraintInfo ) * numConstraints );
//    void* memPtr = malloc( memHelper.getSizeToAllocate() );  // allocate the memory
//    memHelper.setChunkPointers( memPtr );  // update pointers to chunks
//  }
template <int N>
class PreallocatedMemoryHelper
{
    struct Chunk
    {
        void** ptr;
        size_t size;
    };
    Chunk m_chunks[N];
    int m_numChunks;
public:
    PreallocatedMemoryHelper() {m_numChunks=0;}
    void addChunk( void** ptr, size_t sz )
    {
        btAssert( m_numChunks < N );
        if ( m_numChunks < N )
        {
            Chunk& chunk = m_chunks[ m_numChunks ];
            chunk.ptr = ptr;
            chunk.size = sz;
            m_numChunks++;
        }
    }
    size_t getSizeToAllocate() const
    {
        size_t totalSize = 0;
        for (int i = 0; i < m_numChunks; ++i)
        {
            totalSize += m_chunks[i].size;
        }
        return totalSize;
    }
    void setChunkPointers(void* mem) const
    {
        size_t totalSize = 0;
        for (int i = 0; i < m_numChunks; ++i)
        {
            const Chunk& chunk = m_chunks[ i ];
            char* chunkPtr = static_cast<char*>(mem) + totalSize;
            *chunk.ptr = chunkPtr;
            totalSize += chunk.size;
        }
    }
};



static btVector3 findMaxDynamicConstraintExtent(
    btVector3* bodyPositions,
    bool* bodyDynamicFlags,
    btBatchedConstraintInfo* conInfos,
    int numConstraints,
    int numBodies
    )
{
    BT_PROFILE("findMaxDynamicConstraintExtent");
    btVector3 consExtent = btVector3(1,1,1) * 0.001;
    for (int iCon = 0; iCon < numConstraints; ++iCon)
    {
        const btBatchedConstraintInfo& con = conInfos[ iCon ];
        int iBody0 = con.bodyIds[0];
        int iBody1 = con.bodyIds[1];
        btAssert(iBody0 >= 0 && iBody0 < numBodies);
        btAssert(iBody1 >= 0 && iBody1 < numBodies);
        // is it a dynamic constraint?
        if (bodyDynamicFlags[iBody0] && bodyDynamicFlags[iBody1])
        {
            btVector3 delta = bodyPositions[iBody1] - bodyPositions[iBody0];
            consExtent.setMax(delta.absolute());
        }
    }
    return consExtent;
}


struct btIntVec3
{
    int m_ints[ 3 ];

    SIMD_FORCE_INLINE const int& operator[](int i) const {return m_ints[i];}
    SIMD_FORCE_INLINE int&       operator[](int i)       {return m_ints[i];}
};


struct AssignConstraintsToGridBatchesParams
{
    bool* bodyDynamicFlags;
    btIntVec3* bodyGridCoords;
    int numBodies;
    btBatchedConstraintInfo* conInfos;
    int* constraintBatchIds;
    btIntVec3 gridChunkDim;
    int maxNumBatchesPerPhase;
    int numPhases;
    int phaseMask;

    AssignConstraintsToGridBatchesParams()
    {
        memset(this, 0, sizeof(*this));
    }
};


static void assignConstraintsToGridBatches(const AssignConstraintsToGridBatchesParams& params, int iConBegin, int iConEnd)
{
    BT_PROFILE("assignConstraintsToGridBatches");
    // (can be done in parallel)
    for ( int iCon = iConBegin; iCon < iConEnd; ++iCon )
    {
        const btBatchedConstraintInfo& con = params.conInfos[ iCon ];
        int iBody0 = con.bodyIds[ 0 ];
        int iBody1 = con.bodyIds[ 1 ];
        int iPhase = iCon; //iBody0; // pseudorandom choice to distribute evenly amongst phases
        iPhase &= params.phaseMask;
        int gridCoord[ 3 ];
        // is it a dynamic constraint?
        if ( params.bodyDynamicFlags[ iBody0 ] && params.bodyDynamicFlags[ iBody1 ] )
        {
            const btIntVec3& body0Coords = params.bodyGridCoords[iBody0];
            const btIntVec3& body1Coords = params.bodyGridCoords[iBody1];
            // for each dimension x,y,z,
            for (int i = 0; i < 3; ++i)
            {
                int coordMin = btMin(body0Coords.m_ints[i], body1Coords.m_ints[i]);
                int coordMax = btMax(body0Coords.m_ints[i], body1Coords.m_ints[i]);
                if (coordMin != coordMax)
                {
                    btAssert( coordMax == coordMin + 1 );
                    if ((coordMin&1) == 0)
                    {
                        iPhase &= ~(1 << i); // force bit off
                    }
                    else
                    {
                        iPhase |= (1 << i); // force bit on
                        iPhase &= params.phaseMask;
                    }
                }
                gridCoord[ i ] = coordMin;
            }
        }
        else
        {
            if ( !params.bodyDynamicFlags[ iBody0 ] )
            {
                iBody0 = con.bodyIds[ 1 ];
            }
            btAssert(params.bodyDynamicFlags[ iBody0 ]);
            const btIntVec3& body0Coords = params.bodyGridCoords[iBody0];
            // for each dimension x,y,z,
            for ( int i = 0; i < 3; ++i )
            {
                gridCoord[ i ] = body0Coords.m_ints[ i ];
            }
        }
        // calculate chunk coordinates
        int chunkCoord[ 3 ];
        btIntVec3 gridChunkDim = params.gridChunkDim;
        // for each dimension x,y,z,
        for ( int i = 0; i < 3; ++i )
        {
            int coordOffset = ( iPhase >> i ) & 1;
            chunkCoord[ i ] = (gridCoord[ i ] - coordOffset)/2;
            btClamp( chunkCoord[ i ], 0, gridChunkDim[ i ] - 1);
            btAssert( chunkCoord[ i ] < gridChunkDim[ i ] );
        }
        int iBatch = iPhase * params.maxNumBatchesPerPhase + chunkCoord[ 0 ] + chunkCoord[ 1 ] * gridChunkDim[ 0 ] + chunkCoord[ 2 ] * gridChunkDim[ 0 ] * gridChunkDim[ 1 ];
        btAssert(iBatch >= 0 && iBatch < params.maxNumBatchesPerPhase*params.numPhases);
        params.constraintBatchIds[ iCon ] = iBatch;
    }
}


struct AssignConstraintsToGridBatchesLoop : public btIParallelForBody
{
    const AssignConstraintsToGridBatchesParams* m_params;

    AssignConstraintsToGridBatchesLoop( const AssignConstraintsToGridBatchesParams& params )
    {
        m_params = &params;
    }
    void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE
    {
        assignConstraintsToGridBatches(*m_params, iBegin, iEnd);
    }
};


//
// setupSpatialGridBatchesMt -- generate batches using a uniform 3D grid
//
/*

Bodies are treated as 3D points at their center of mass. We only consider dynamic bodies at this stage,
because only dynamic bodies are mutated when a constraint is solved, thus subject to race conditions.

1. Compute a bounding box around all dynamic bodies
2. Compute the maximum extent of all dynamic constraints. Each dynamic constraint is treated as a line segment, and we need the size of
   box that will fully enclose any single dynamic constraint

3. Establish the cell size of our grid, the cell size in each dimension must be at least as large as the dynamic constraints max-extent,
   so that no dynamic constraint can span more than 2 cells of our grid on any axis of the grid. The cell size should be adjusted
   larger in order to keep the total number of cells from being excessively high

Key idea: Given that each constraint spans 1 or 2 grid cells in each dimension, we can handle all constraints by processing
          in chunks of 2x2x2 cells with 8 different 1-cell offsets ((0,0,0),(0,0,1),(0,1,0),(0,1,1),(1,0,0)...).
          For each of the 8 offsets, we create a phase, and for each 2x2x2 chunk with dynamic constraints becomes a batch in that phase.

4. Once the grid is established, we can calculate for each constraint which phase and batch it belongs in.

5. Do a merge small batches on the batches of each phase separately, to try to even out the sizes of batches

Optionally, we can "collapse" one dimension of our 3D grid to turn it into a 2D grid, which reduces the number of phases
to 4. With fewer phases, there are more constraints per phase and this makes it easier to create batches of a useful size.
*/
//
static void setupSpatialGridBatchesMt(
    btBatchedConstraints* batchedConstraints,
    btAlignedObjectArray<char>* scratchMemory,
    btConstraintArray* constraints,
    const btAlignedObjectArray<btSolverBody>& bodies,
    int minBatchSize,
    int maxBatchSize,
    bool use2DGrid
)
{
    BT_PROFILE("setupSpatialGridBatchesMt");
    const int numPhases = 8;
    int numConstraints = constraints->size();
    int numConstraintRows = constraints->size();

    const int maxGridChunkCount = 128;
    int allocNumBatchesPerPhase = maxGridChunkCount;
    int minNumBatchesPerPhase = 16;
    int allocNumBatches = allocNumBatchesPerPhase * numPhases;

    btVector3* bodyPositions = NULL;
    bool* bodyDynamicFlags = NULL;
    btIntVec3* bodyGridCoords = NULL;
    btBatchInfo* batches = NULL;
    int* batchWork = NULL;
    btBatchedConstraintInfo* conInfos = NULL;
    int* constraintBatchIds = NULL;
    int* constraintRowBatchIds = NULL;
    {
        PreallocatedMemoryHelper<10> memHelper;
        memHelper.addChunk( (void**) &bodyPositions, sizeof( btVector3 ) * bodies.size() );
        memHelper.addChunk( (void**) &bodyDynamicFlags, sizeof( bool ) * bodies.size() );
        memHelper.addChunk( (void**) &bodyGridCoords, sizeof( btIntVec3 ) * bodies.size() );
        memHelper.addChunk( (void**) &batches, sizeof( btBatchInfo )* allocNumBatches );
        memHelper.addChunk( (void**) &batchWork, sizeof( int )* allocNumBatches );
        memHelper.addChunk( (void**) &conInfos, sizeof( btBatchedConstraintInfo ) * numConstraints );
        memHelper.addChunk( (void**) &constraintBatchIds, sizeof( int ) * numConstraints );
        memHelper.addChunk( (void**) &constraintRowBatchIds, sizeof( int ) * numConstraintRows );
        size_t scratchSize = memHelper.getSizeToAllocate();
        // if we need to reallocate
        if (scratchMemory->capacity() < scratchSize)
        {
            // allocate 6.25% extra to avoid repeated reallocs
            scratchMemory->reserve( scratchSize + scratchSize/16 );
        }
        scratchMemory->resizeNoInitialize( scratchSize );
        char* memPtr = &scratchMemory->at(0);
        memHelper.setChunkPointers( memPtr );
    }

    numConstraints = initBatchedConstraintInfo(conInfos, constraints);

    // compute bounding box around all dynamic bodies
    // (could be done in parallel)
    btVector3 bboxMin(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
    btVector3 bboxMax = -bboxMin;
    //int dynamicBodyCount = 0;
    for (int i = 0; i < bodies.size(); ++i)
    {
        const btSolverBody& body = bodies[i];
        btVector3 bodyPos = body.getWorldTransform().getOrigin();
        bool isDynamic = ( body.internalGetInvMass().x() > btScalar( 0 ) );
        bodyPositions[i] = bodyPos;
        bodyDynamicFlags[i] = isDynamic;
        if (isDynamic)
        {
            //dynamicBodyCount++;
            bboxMin.setMin(bodyPos);
            bboxMax.setMax(bodyPos);
        }
    }

    // find max extent of all dynamic constraints
    // (could be done in parallel)
    btVector3 consExtent = findMaxDynamicConstraintExtent(bodyPositions, bodyDynamicFlags, conInfos, numConstraints, bodies.size());

    btVector3 gridExtent = bboxMax - bboxMin;

    btVector3 gridCellSize = consExtent;
    int gridDim[3];
    gridDim[ 0 ] = int( 1.0 + gridExtent.x() / gridCellSize.x() );
    gridDim[ 1 ] = int( 1.0 + gridExtent.y() / gridCellSize.y() );
    gridDim[ 2 ] = int( 1.0 + gridExtent.z() / gridCellSize.z() );

    // if we can collapse an axis, it will cut our number of phases in half which could be more efficient
    int phaseMask = 7;
    bool collapseAxis = use2DGrid;
    if ( collapseAxis )
    {
        // pick the smallest axis to collapse, leaving us with the greatest number of cells in our grid
        int iAxisToCollapse = 0;
        int axisDim = gridDim[iAxisToCollapse];
        //for each dimension
        for ( int i = 0; i < 3; ++i )
        {
            if (gridDim[i] < axisDim)
            {
                iAxisToCollapse = i;
                axisDim = gridDim[i];
            }
        }
        // collapse it
        gridCellSize[iAxisToCollapse] = gridExtent[iAxisToCollapse] * 2.0f;
        phaseMask &= ~(1 << iAxisToCollapse);
    }

    int numGridChunks = 0;
    btIntVec3 gridChunkDim;  // each chunk is 2x2x2 group of cells
    while (true)
    {
        gridDim[0] = int( 1.0 + gridExtent.x() / gridCellSize.x() );
        gridDim[1] = int( 1.0 + gridExtent.y() / gridCellSize.y() );
        gridDim[2] = int( 1.0 + gridExtent.z() / gridCellSize.z() );
        gridChunkDim[ 0 ] = btMax( 1, ( gridDim[ 0 ] + 0 ) / 2 );
        gridChunkDim[ 1 ] = btMax( 1, ( gridDim[ 1 ] + 0 ) / 2 );
        gridChunkDim[ 2 ] = btMax( 1, ( gridDim[ 2 ] + 0 ) / 2 );
        numGridChunks = gridChunkDim[ 0 ] * gridChunkDim[ 1 ] * gridChunkDim[ 2 ];
        float nChunks = float(gridChunkDim[0]) * float(gridChunkDim[1]) * float(gridChunkDim[2]);  // suceptible to integer overflow
        if ( numGridChunks <= maxGridChunkCount && nChunks <= maxGridChunkCount )
        {
            break;
        }
        gridCellSize *= 1.25; // should roughly cut numCells in half
    }
    btAssert(numGridChunks <= maxGridChunkCount );
    int maxNumBatchesPerPhase = numGridChunks;

    // for each dynamic body, compute grid coords
    btVector3 invGridCellSize = btVector3(1,1,1)/gridCellSize;
    // (can be done in parallel)
    for (int iBody = 0; iBody < bodies.size(); ++iBody)
    {
        btIntVec3& coords = bodyGridCoords[iBody];
        if (bodyDynamicFlags[iBody])
        {
            btVector3 v = ( bodyPositions[ iBody ] - bboxMin )*invGridCellSize;
            coords.m_ints[0] = int(v.x());
            coords.m_ints[1] = int(v.y());
            coords.m_ints[2] = int(v.z());
            btAssert(coords.m_ints[0] >= 0 && coords.m_ints[0] < gridDim[0]);
            btAssert(coords.m_ints[1] >= 0 && coords.m_ints[1] < gridDim[1]);
            btAssert(coords.m_ints[2] >= 0 && coords.m_ints[2] < gridDim[2]);
        }
        else
        {
            coords.m_ints[0] = -1;
            coords.m_ints[1] = -1;
            coords.m_ints[2] = -1;
        }
    }

    for (int iPhase = 0; iPhase < numPhases; ++iPhase)
    {
        int batchBegin = iPhase * maxNumBatchesPerPhase;
        int batchEnd = batchBegin + maxNumBatchesPerPhase;
        for ( int iBatch = batchBegin; iBatch < batchEnd; ++iBatch )
        {
            btBatchInfo& batch = batches[ iBatch ];
            batch = btBatchInfo();
        }
    }

    {
        AssignConstraintsToGridBatchesParams params;
        params.bodyDynamicFlags = bodyDynamicFlags;
        params.bodyGridCoords = bodyGridCoords;
        params.numBodies = bodies.size();
        params.conInfos = conInfos;
        params.constraintBatchIds = constraintBatchIds;
        params.gridChunkDim = gridChunkDim;
        params.maxNumBatchesPerPhase = maxNumBatchesPerPhase;
        params.numPhases = numPhases;
        params.phaseMask = phaseMask;
        bool inParallel = true;
        if (inParallel)
        {
            AssignConstraintsToGridBatchesLoop loop(params);
            int grainSize = 250;
            btParallelFor(0, numConstraints, grainSize, loop);
        }
        else
        {
            assignConstraintsToGridBatches( params, 0, numConstraints );
        }
    }
    for ( int iCon = 0; iCon < numConstraints; ++iCon )
    {
        const btBatchedConstraintInfo& con = conInfos[ iCon ];
        int iBatch = constraintBatchIds[ iCon ];
        btBatchInfo& batch = batches[iBatch];
        batch.numConstraints += con.numConstraintRows;
    }

    for (int iPhase = 0; iPhase < numPhases; ++iPhase)
    {
        // if phase is legit,
        if (iPhase == (iPhase&phaseMask))
        {
            int iBeginBatch = iPhase * maxNumBatchesPerPhase;
            int iEndBatch = iBeginBatch + maxNumBatchesPerPhase;
            mergeSmallBatches( batches, iBeginBatch, iEndBatch, minBatchSize, maxBatchSize );
        }
    }
    // all constraints have been assigned a batchId
    updateConstraintBatchIdsForMergesMt(constraintBatchIds, numConstraints, batches, maxNumBatchesPerPhase*numPhases);

    if (numConstraintRows > numConstraints)
    {
        expandConstraintRowsMt(&constraintRowBatchIds[0], &constraintBatchIds[0], &conInfos[0], numConstraints, numConstraintRows);
    }
    else
    {
        constraintRowBatchIds = constraintBatchIds;
    }

    writeOutBatches(batchedConstraints, constraintRowBatchIds, numConstraintRows, batches, batchWork, maxNumBatchesPerPhase, numPhases);
    btAssert(batchedConstraints->validate(constraints, bodies));
}


static void setupSingleBatch(
    btBatchedConstraints* bc,
    int numConstraints
)
{
    BT_PROFILE("setupSingleBatch");
    typedef btBatchedConstraints::Range Range;

    bc->m_constraintIndices.resize( numConstraints );
    for ( int i = 0; i < numConstraints; ++i )
    {
        bc->m_constraintIndices[ i ] = i;
    }

    bc->m_batches.resizeNoInitialize( 0 );
    bc->m_phases.resizeNoInitialize( 0 );
    bc->m_phaseOrder.resizeNoInitialize( 0 );
    bc->m_phaseGrainSize.resizeNoInitialize( 0 );

    if (numConstraints > 0)
    {
        bc->m_batches.push_back( Range( 0, numConstraints ) );
        bc->m_phases.push_back( Range( 0, 1 ) );
        bc->m_phaseOrder.push_back(0);
        bc->m_phaseGrainSize.push_back(1);
    }
}


void btBatchedConstraints::setup(
    btConstraintArray* constraints,
    const btAlignedObjectArray<btSolverBody>& bodies,
    BatchingMethod batchingMethod,
    int minBatchSize,
    int maxBatchSize,
    btAlignedObjectArray<char>* scratchMemory
    )
{
    if (constraints->size() >= minBatchSize*4)
    {
        bool use2DGrid = batchingMethod == BATCHING_METHOD_SPATIAL_GRID_2D;
        setupSpatialGridBatchesMt( this, scratchMemory, constraints, bodies, minBatchSize, maxBatchSize, use2DGrid );
        if (s_debugDrawBatches)
        {
            debugDrawAllBatches( this, constraints, bodies );
        }
    }
    else
    {
        setupSingleBatch( this, constraints->size() );
    }
}