2018-09-07 14:11:04 +00:00
|
|
|
/*
|
|
|
|
Bullet Continuous Collision Detection and Physics Library
|
2022-01-06 22:37:49 +00:00
|
|
|
Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
|
2018-09-07 14:11:04 +00:00
|
|
|
|
|
|
|
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"
|
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
#include <string.h> //for memset
|
2018-09-07 14:11:04 +00:00
|
|
|
|
2019-06-11 11:18:05 +00:00
|
|
|
#include <cmath>
|
|
|
|
|
2018-09-07 14:11:04 +00:00
|
|
|
const int kNoMerge = -1;
|
|
|
|
|
|
|
|
bool btBatchedConstraints::s_debugDrawBatches = false;
|
|
|
|
|
|
|
|
struct btBatchedConstraintInfo
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
int constraintIndex;
|
|
|
|
int numConstraintRows;
|
|
|
|
int bodyIds[2];
|
2018-09-07 14:11:04 +00:00
|
|
|
};
|
|
|
|
|
|
|
|
struct btBatchInfo
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
int numConstraints;
|
|
|
|
int mergeIndex;
|
2018-09-07 14:11:04 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
btBatchInfo() : numConstraints(0), mergeIndex(kNoMerge) {}
|
2018-09-07 14:11:04 +00:00
|
|
|
};
|
|
|
|
|
|
|
|
bool btBatchedConstraints::validate(btConstraintArray* constraints, const btAlignedObjectArray<btSolverBody>& bodies) const
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
//
|
|
|
|
// 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;
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
static void debugDrawSingleBatch(const btBatchedConstraints* bc,
|
|
|
|
btConstraintArray* constraints,
|
|
|
|
const btAlignedObjectArray<btSolverBody>& bodies,
|
|
|
|
int iBatch,
|
|
|
|
const btVector3& color,
|
|
|
|
const btVector3& offset)
|
2018-09-07 14:11:04 +00:00
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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);
|
|
|
|
}
|
|
|
|
}
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
static void debugDrawPhase(const btBatchedConstraints* bc,
|
|
|
|
btConstraintArray* constraints,
|
|
|
|
const btAlignedObjectArray<btSolverBody>& bodies,
|
|
|
|
int iPhase,
|
|
|
|
const btVector3& color0,
|
|
|
|
const btVector3& color1,
|
|
|
|
const btVector3& offset)
|
2018-09-07 14:11:04 +00:00
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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);
|
|
|
|
}
|
|
|
|
}
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
static void debugDrawAllBatches(const btBatchedConstraints* bc,
|
|
|
|
btConstraintArray* constraints,
|
|
|
|
const btAlignedObjectArray<btSolverBody>& bodies)
|
2018-09-07 14:11:04 +00:00
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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);
|
|
|
|
}
|
|
|
|
}
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
static void initBatchedBodyDynamicFlags(btAlignedObjectArray<bool>* outBodyDynamicFlags, const btAlignedObjectArray<btSolverBody>& bodies)
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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));
|
|
|
|
}
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
static int runLengthEncodeConstraintInfo(btBatchedConstraintInfo* outConInfos, int numConstraints)
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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;
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
struct ReadSolverConstraintsLoop : public btIParallelForBody
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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;
|
|
|
|
}
|
|
|
|
}
|
2018-09-07 14:11:04 +00:00
|
|
|
};
|
|
|
|
|
|
|
|
static int initBatchedConstraintInfo(btBatchedConstraintInfo* outConInfos, btConstraintArray* constraints)
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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;
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
static void expandConstraintRowsInPlace(int* constraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows)
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
static void expandConstraintRows(int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows)
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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;
|
|
|
|
}
|
|
|
|
}
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
struct ExpandConstraintRowsLoop : public btIParallelForBody
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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);
|
|
|
|
}
|
2018-09-07 14:11:04 +00:00
|
|
|
};
|
|
|
|
|
|
|
|
static void expandConstraintRowsMt(int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows)
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
BT_PROFILE("expandConstraintRowsMt");
|
|
|
|
ExpandConstraintRowsLoop loop(destConstraintBatchIds, srcConstraintBatchIds, conInfos, numConstraintRows);
|
|
|
|
int grainSize = 600;
|
|
|
|
btParallelFor(0, numConstraints, grainSize, loop);
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
static void initBatchedConstraintInfoArray(btAlignedObjectArray<btBatchedConstraintInfo>* outConInfos, btConstraintArray* constraints)
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
BT_PROFILE("initBatchedConstraintInfoArray");
|
|
|
|
btAlignedObjectArray<btBatchedConstraintInfo>& conInfos = *outConInfos;
|
|
|
|
int numConstraints = constraints->size();
|
|
|
|
conInfos.resizeNoInitialize(numConstraints);
|
2018-09-07 14:11:04 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
int newSize = initBatchedConstraintInfo(&outConInfos->at(0), constraints);
|
|
|
|
conInfos.resizeNoInitialize(newSize);
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
static void mergeSmallBatches(btBatchInfo* batches, int iBeginBatch, int iEndBatch, int minBatchSize, int maxBatchSize)
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
static void updateConstraintBatchIdsForMerges(int* constraintBatchIds, int numConstraints, const btBatchInfo* batches, int numBatches)
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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;
|
|
|
|
}
|
|
|
|
}
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
struct UpdateConstraintBatchIdsForMergesLoop : public btIParallelForBody
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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);
|
|
|
|
}
|
2018-09-07 14:11:04 +00:00
|
|
|
};
|
|
|
|
|
|
|
|
static void updateConstraintBatchIdsForMergesMt(int* constraintBatchIds, int numConstraints, const btBatchInfo* batches, int numBatches)
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
BT_PROFILE("updateConstraintBatchIdsForMergesMt");
|
|
|
|
UpdateConstraintBatchIdsForMergesLoop loop(constraintBatchIds, batches, numBatches);
|
|
|
|
int grainSize = 800;
|
|
|
|
btParallelFor(0, numConstraints, grainSize, loop);
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
inline bool BatchCompare(const btBatchedConstraints::Range& a, const btBatchedConstraints::Range& b)
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
int lenA = a.end - a.begin;
|
|
|
|
int lenB = b.end - b.begin;
|
|
|
|
return lenA > lenB;
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
static void writeOutConstraintIndicesForRangeOfBatches(btBatchedConstraints* bc,
|
2019-01-03 13:26:51 +00:00
|
|
|
const int* constraintBatchIds,
|
|
|
|
int numConstraints,
|
|
|
|
int* constraintIdPerBatch,
|
|
|
|
int batchBegin,
|
|
|
|
int batchEnd)
|
2018-09-07 14:11:04 +00:00
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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;
|
|
|
|
}
|
|
|
|
}
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
struct WriteOutConstraintIndicesLoop : public btIParallelForBody
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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);
|
|
|
|
}
|
2018-09-07 14:11:04 +00:00
|
|
|
};
|
|
|
|
|
|
|
|
static void writeOutConstraintIndicesMt(btBatchedConstraints* bc,
|
2019-01-03 13:26:51 +00:00
|
|
|
const int* constraintBatchIds,
|
|
|
|
int numConstraints,
|
|
|
|
int* constraintIdPerBatch,
|
|
|
|
int maxNumBatchesPerPhase,
|
|
|
|
int numPhases)
|
2018-09-07 14:11:04 +00:00
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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;
|
|
|
|
}
|
|
|
|
}
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
static void writeGrainSizes(btBatchedConstraints* bc)
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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;
|
2019-06-11 11:18:05 +00:00
|
|
|
float grainSize = std::floor((0.25f * numBatches / float(numThreads)) + 0.0f);
|
2019-01-03 13:26:51 +00:00
|
|
|
bc->m_phaseGrainSize[iPhase] = btMax(1, int(grainSize));
|
|
|
|
}
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
static void writeOutBatches(btBatchedConstraints* bc,
|
2019-01-03 13:26:51 +00:00
|
|
|
const int* constraintBatchIds,
|
|
|
|
int numConstraints,
|
|
|
|
const btBatchInfo* batches,
|
|
|
|
int* batchWork,
|
|
|
|
int maxNumBatchesPerPhase,
|
|
|
|
int numPhases)
|
2018-09-07 14:11:04 +00:00
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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);
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
//
|
|
|
|
// 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
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
struct Chunk
|
|
|
|
{
|
|
|
|
void** ptr;
|
|
|
|
size_t size;
|
|
|
|
};
|
|
|
|
Chunk m_chunks[N];
|
|
|
|
int m_numChunks;
|
|
|
|
|
2018-09-07 14:11:04 +00:00
|
|
|
public:
|
2019-01-03 13:26:51 +00:00
|
|
|
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;
|
|
|
|
}
|
|
|
|
}
|
2018-09-07 14:11:04 +00:00
|
|
|
};
|
|
|
|
|
|
|
|
static btVector3 findMaxDynamicConstraintExtent(
|
2019-01-03 13:26:51 +00:00
|
|
|
btVector3* bodyPositions,
|
|
|
|
bool* bodyDynamicFlags,
|
|
|
|
btBatchedConstraintInfo* conInfos,
|
|
|
|
int numConstraints,
|
|
|
|
int numBodies)
|
2018-09-07 14:11:04 +00:00
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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;
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
struct btIntVec3
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
int m_ints[3];
|
2018-09-07 14:11:04 +00:00
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
SIMD_FORCE_INLINE const int& operator[](int i) const { return m_ints[i]; }
|
|
|
|
SIMD_FORCE_INLINE int& operator[](int i) { return m_ints[i]; }
|
2018-09-07 14:11:04 +00:00
|
|
|
};
|
|
|
|
|
|
|
|
struct AssignConstraintsToGridBatchesParams
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
bool* bodyDynamicFlags;
|
|
|
|
btIntVec3* bodyGridCoords;
|
|
|
|
int numBodies;
|
|
|
|
btBatchedConstraintInfo* conInfos;
|
|
|
|
int* constraintBatchIds;
|
|
|
|
btIntVec3 gridChunkDim;
|
|
|
|
int maxNumBatchesPerPhase;
|
|
|
|
int numPhases;
|
|
|
|
int phaseMask;
|
|
|
|
|
|
|
|
AssignConstraintsToGridBatchesParams()
|
|
|
|
{
|
|
|
|
memset(this, 0, sizeof(*this));
|
|
|
|
}
|
2018-09-07 14:11:04 +00:00
|
|
|
};
|
|
|
|
|
|
|
|
static void assignConstraintsToGridBatches(const AssignConstraintsToGridBatchesParams& params, int iConBegin, int iConEnd)
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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;
|
|
|
|
}
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
struct AssignConstraintsToGridBatchesLoop : public btIParallelForBody
|
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
const AssignConstraintsToGridBatchesParams* m_params;
|
|
|
|
|
|
|
|
AssignConstraintsToGridBatchesLoop(const AssignConstraintsToGridBatchesParams& params)
|
|
|
|
{
|
|
|
|
m_params = ¶ms;
|
|
|
|
}
|
|
|
|
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
|
|
|
|
{
|
|
|
|
assignConstraintsToGridBatches(*m_params, iBegin, iEnd);
|
|
|
|
}
|
2018-09-07 14:11:04 +00:00
|
|
|
};
|
|
|
|
|
|
|
|
//
|
|
|
|
// 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(
|
2019-01-03 13:26:51 +00:00
|
|
|
btBatchedConstraints* batchedConstraints,
|
|
|
|
btAlignedObjectArray<char>* scratchMemory,
|
|
|
|
btConstraintArray* constraints,
|
|
|
|
const btAlignedObjectArray<btSolverBody>& bodies,
|
|
|
|
int minBatchSize,
|
|
|
|
int maxBatchSize,
|
|
|
|
bool use2DGrid)
|
2018-09-07 14:11:04 +00:00
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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
|
2021-09-29 13:47:08 +00:00
|
|
|
if (static_cast<size_t>(scratchMemory->capacity()) < scratchSize)
|
2019-01-03 13:26:51 +00:00
|
|
|
{
|
|
|
|
// 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;
|
|
|
|
|
2020-01-08 17:05:43 +00:00
|
|
|
gridExtent.setMax(btVector3(btScalar(1), btScalar(1), btScalar(1)));
|
|
|
|
|
2019-01-03 13:26:51 +00:00
|
|
|
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));
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
static void setupSingleBatch(
|
2019-01-03 13:26:51 +00:00
|
|
|
btBatchedConstraints* bc,
|
|
|
|
int numConstraints)
|
2018-09-07 14:11:04 +00:00
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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);
|
|
|
|
}
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
void btBatchedConstraints::setup(
|
2019-01-03 13:26:51 +00:00
|
|
|
btConstraintArray* constraints,
|
|
|
|
const btAlignedObjectArray<btSolverBody>& bodies,
|
|
|
|
BatchingMethod batchingMethod,
|
|
|
|
int minBatchSize,
|
|
|
|
int maxBatchSize,
|
|
|
|
btAlignedObjectArray<char>* scratchMemory)
|
2018-09-07 14:11:04 +00:00
|
|
|
{
|
2019-01-03 13:26:51 +00:00
|
|
|
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());
|
|
|
|
}
|
2018-09-07 14:11:04 +00:00
|
|
|
}
|