App/solver/Solver.cpp - main - BirdLeon/ROBLOX2016

App/solver/Solver.cpp
1 754 строки70 KB
PatoFlamejanteTV
full source code
19 дек 2024, 19:11
19 дек 2024, 19:1105db15d
#include "stdafx.h"

#include "solver/Solver.h"
#include "solver/SolverKernel.h"
#include "solver/SolverSerializer.h"
#include "solver/SolverProfiler.h"

#include "G3D/Matrix3.h"
#include "G3D/Vector3.h"
#include "v8world/RotateJoint.h"
#include "v8world/Primitive.h"
#include "v8kernel/Body.h"
#include "v8kernel/ContactConnector.h"

#include "RbxAssert.h"
#include "FastLog.h"
#include "g3d/g3dmath.h"
#include "rbx/ArrayDynamic.h"

#include "boost/functional/hash.hpp"

#include "rbx/Profiler.h"

FASTFLAGVARIABLE(PGSSolverFileDump, false)
DYNAMIC_FASTFLAGVARIABLE(PGSSolverSimIslandsEnabled, false)
DYNAMIC_FASTFLAGVARIABLE(PGSSolverUsesIslandizableCode, false)
DYNAMIC_FASTFLAGVARIABLE(PGSSolverIntegrateOnlyPositionsEnabled, false)
FASTFLAG(PhysicsAnalyzerEnabled)

namespace RBX
{

class ContactManifold 
{
public:
    ContactManifold(): referenceCount( 0 ) { }
    ~ContactManifold();

    ArrayDynamic< ConstraintCollision* > collisions;
    int referenceCount;

private:
    ContactManifold( const ContactManifold& );
    ContactManifold& operator=( const ContactManifold& );
};

// Given a pointer to Body, return an index to it's position in the body list
// If it doesn't exists in the body list, add it to the end.
// Those SimBodies that aren't in the indexation are necessarily anchored.
static RBX_SIMD_INLINE int getBodyIndex( 
    ArrayDynamic< SimBody* >& _anchoredBodyList,
    BodyIndexation& _allBodyIndexation,
    Body* _body )
{
    SimBody* simBody = NULL;
    if( _body != NULL )
    {
        simBody = _body->getRootSimBody();
    }
    auto it = _allBodyIndexation.find( simBody );

    int index = 0;
    // A new body that we haven't seen previously. Add it to the list of anchored bodies and add its index
    if( it == NULL )
    {
        // We can assert that it is anchored...
        index = (int)_allBodyIndexation.size();
        _allBodyIndexation[ simBody ] = index;
        _anchoredBodyList.push_back( simBody );
    }
    else
    {
        index = *it;
    }
    return index;
}

//
// From the constraint list extract information and lay it out in arrays:
// - indices of constrained body pairs
// - a list of anchored bodies interacting constrained/colliding with at least one simulated body
// - an indexation of SimBodies
// - constraint block size and offset to the constraint blocks
// - total size of the diagonal symmetric blocks
//
static boost::uint32_t gatherConstraintPairData( ArrayBase< BodyPairIndices >& _constrainedSimBodyPairs,
                                               ArrayBase< boost::uint8_t >& _dimensions, 
                                               ArrayBase< boost::uint32_t >& _offsets,
                                               ArrayDynamic< SimBody* >& _anchoredBodyList,
                                               BodyIndexation& _bodyIndexation,
                                               boost::uint32_t& _totalBlockSize,
                                               boost::uint32_t& _collisionCount,
                                               const ArrayBase< Constraint* >& _constraints )
{
    RBXPROFILER_SCOPE("Physics", "gatherConstraintPairData");

    boost::uint32_t totalDimension = 0;
    _totalBlockSize = 0;
    _collisionCount = 0;
    for( size_t i = 0; i < _constraints.size(); i++ )
    {
        boost::uint8_t dimension = _constraints[ i ]->getDimension();
        _dimensions[ i ] = dimension;
        _offsets[ i ] = totalDimension;
        totalDimension += dimension;
        _totalBlockSize += ( dimension * ( dimension + 1 ) ) / 2;

        int indexA = getBodyIndex( _anchoredBodyList, _bodyIndexation, _constraints[ i ]->getBodyA() );
        int indexB = getBodyIndex( _anchoredBodyList, _bodyIndexation, _constraints[ i ]->getBodyB() );
        _constrainedSimBodyPairs[ i ] = BodyPairIndices( indexA, indexB );
        if( _constraints[ i ]->getType() == Constraint::Types_Collision )
        {
            _collisionCount++;
        }
    }

    return totalDimension;
}

static RBX_SIMD_INLINE void integrateVelocities( SolverBodyDynamicProperties& _variableData, const SolverBodyMassAndInertia& _massAndInertia, const Vector3& _externalImpulse, const Vector3& _externalAngularImpulse, float _dt, const SolverConfig& _config )
{
    // Integrate linear velocity
    _variableData.integratedLinearVelocity = _variableData.linearVelocity + _massAndInertia.massInvVelStage * _externalImpulse;
    RBXASSERT_VERY_FAST( !RBX::Math::isNanInfVector3( _variableData.integratedLinearVelocity ) );

    // Integrate angular velocity
    SymmetricMatrix invInertia;
    invInertia.diagonals = _massAndInertia.inertiaDiagonal;
    invInertia.offDiagonals = _massAndInertia.inertiaOffDiagonal;
    _variableData.integratedAngularVelocity = expf( -_config.angularDamping * _dt ) * _variableData.angularVelocity + invInertia * _externalAngularImpulse;
    RBXASSERT_VERY_FAST( !RBX::Math::isNanInfVector3( _variableData.integratedAngularVelocity ) );
}

static inline float fudgeMassForPosStage( float massInv, const SolverConfig& _config )
{
    return powf( massInv, _config.stabilizationMassReductionPower );
}

//
// Init internal structures for sim bodies, and integrate velocities
//
static void integrateVelocitiesAndInitSimulatedObjects( ArrayDynamic< SolverBodyStaticProperties >& _bodyStaticData, 
                                                       ArrayDynamic< SolverBodyDynamicProperties >& _bodyVariableData, 
                                                       ArrayDynamic< SolverBodyMassAndInertia >& _massAndInertia,
                                                       ArrayDynamic< float >& _effectiveMassMultipliers,
                                                       const ArrayBase< SimBody* >& _simBodies, float _dt, const SolverConfig& _config )
{
    RBXPROFILER_SCOPE("Physics", "integrateVelocitiesAndInitSimulatedObjects");

    for( size_t i = 0; i < _simBodies.size(); i++ )
    {
        SimBody* body = _simBodies[ i ];
        body->updateIfDirty();

        SolverBodyDynamicProperties variableData;
        variableData.angularVelocity = body->getPV().velocity.rotational;
        variableData.linearVelocity = body->getPV().velocity.linear;
        variableData.orientation = body->getPV().position.rotation;
        variableData.position = body->getPV().position.translation;
        Vector3 externalImpulse = body->getForce() * _dt + body->getImpulse();
        Vector3 externalAngularImpulse = body->getTorque() * _dt + body->getRotationallmpulse();

        Matrix3 inertiaInv = body->getInverseInertiaInWorld();
        SolverBodyStaticProperties staticData;
        SolverBodyMassAndInertia massAndInertia;
        staticData.isStatic = false;
        massAndInertia.inertiaDiagonal = Vector3( inertiaInv[0][0], inertiaInv[1][1], inertiaInv[2][2] );
        massAndInertia.inertiaOffDiagonal = Vector3( inertiaInv[0][1], inertiaInv[0][2], inertiaInv[1][2] );
        massAndInertia.massInvVelStage = body->getMassRecip();
        float massInversePosStage = fudgeMassForPosStage( massAndInertia.massInvVelStage, _config );
        if( massAndInertia.massInvVelStage > 0.0f )
        {
            massAndInertia.posToVelMassRatio = massInversePosStage / massAndInertia.massInvVelStage;
        }
        else
        {
            massAndInertia.posToVelMassRatio = 0.0f;
        }
        staticData.bodyUID = body->getUID();
        
        staticData.guid = body->getBody()->getGuidIndex();
        _bodyStaticData.push_back( staticData );
        _massAndInertia.push_back( massAndInertia );

        integrateVelocities( variableData, massAndInertia, externalImpulse, externalAngularImpulse, _dt, _config );
        _bodyVariableData.push_back( variableData );

        _effectiveMassMultipliers.push_back( 1.0f );
    }

    if( DFFlag::PGSSolverIntegrateOnlyPositionsEnabled && _config.integrateOnlyPositions )
    {
        for( auto b : _bodyVariableData )
        {
            b.linearVelocity = Vector3(0.0f);
            b.angularVelocity = Vector3(0.0f);
            b.integratedLinearVelocity = Vector3(0.0f);
            b.integratedAngularVelocity = Vector3(0.0f);
        }
    }
}

//
// Initialize internal structures for anchored bodies that are being interacted with
//
void PGSSolver::initAnchoredObjects( 
    ArrayDynamic< SolverBodyDynamicProperties >& _dynamicProps,
    ArrayDynamic< SolverBodyStaticProperties >& _staticProps,
    ArrayDynamic< SolverBodyMassAndInertia >& _massAndInertia,
    ArrayDynamic< float >& _effectiveMassMultipliers,
    VirtualDisplacementArray& _velStageVirDisplacements,
    VirtualDisplacementArray& _posStageVirDisplacements,
    int offsetToAnchoredObjects,
    const ArrayBase< SimBody* >& _anchoredBodyList,
    const SolverConfig& _config ) const
{
    RBXPROFILER_SCOPE("Physics", "initAnchoredObjects");

    for( int i = 0; i < (int)_anchoredBodyList.size(); i++ )
    {
        auto body = _anchoredBodyList[ i ];
        if( body != NULL )
        {
            body->updateIfDirty();
        }

        SolverBodyDynamicProperties dynamicProps;
        SolverBodyStaticProperties staticProps;
        SolverBodyMassAndInertia massAndInertia;
        VirtualDisplacement velStageVirD;
        VirtualDisplacement posStageVirD;
        staticProps.isStatic = true;
        boost::uint64_t uid = body != NULL ? body->getUID() : 0;
        auto bodyCacheIt = bodyCache.find( uid );
        if( ( body != NULL ) && ( bodyCacheIt != bodyCache.end() ) && _config.virtualMassesEnabled )
        {
            const SolverBodyCache& bodyCache = bodyCacheIt->second;
            dynamicProps.linearVelocity = bodyCache.linearVelocity;
            dynamicProps.angularVelocity = bodyCache.angularVelocity;
            dynamicProps.integratedLinearVelocity = bodyCache.integratedLinearVelocity;
            dynamicProps.integratedAngularVelocity = bodyCache.integratedAngularVelocity;

            velStageVirD = VirtualDisplacement( simd::load3( &bodyCache.virDVelStageLin.x ), simd::load3( &bodyCache.virDVelStageAng.x ) );
            posStageVirD = VirtualDisplacement( simd::load3( &bodyCache.virDPosStageLin.x ), simd::load3( &bodyCache.virDPosStageAng.x ) );

            Matrix3 inertiaInv = body->getInverseInertiaInWorld();
            massAndInertia.inertiaDiagonal = Vector3( inertiaInv[0][0], inertiaInv[1][1], inertiaInv[2][2] );
            massAndInertia.inertiaOffDiagonal = Vector3( inertiaInv[0][1], inertiaInv[0][2], inertiaInv[1][2] );
            massAndInertia.massInvVelStage = body->getMassRecip();
            float massInversePosStage = fudgeMassForPosStage(massAndInertia.massInvVelStage, _config);
            if( massAndInertia.massInvVelStage > 0.0f )
            {
                massAndInertia.posToVelMassRatio = massInversePosStage / massAndInertia.massInvVelStage;
            }
            else
            {
                massAndInertia.posToVelMassRatio = 0.0f;
            }
        }
        else
        {
            Vector3 zero( 0.0f, 0.0f, 0.0f );

            if( body != NULL )
            {
                // Conveyor belt
                dynamicProps.linearVelocity = body->getBodyConst()->getPvFast().velocity.linear;
                dynamicProps.angularVelocity = body->getBodyConst()->getPvFast().velocity.rotational;
            }
            else
            {
                dynamicProps.linearVelocity = zero;
                dynamicProps.angularVelocity = zero;
            }
            dynamicProps.integratedLinearVelocity = dynamicProps.linearVelocity;
            dynamicProps.integratedAngularVelocity = dynamicProps.integratedAngularVelocity;

            velStageVirD.reset();
            posStageVirD.reset();

            massAndInertia.inertiaDiagonal = zero;
            massAndInertia.inertiaOffDiagonal = zero;
            massAndInertia.massInvVelStage = 0.0f;
            massAndInertia.posToVelMassRatio = 0.0f;
        }
        
        if( body != NULL )
        {
            dynamicProps.orientation = body->getPV().position.rotation;
            dynamicProps.position = body->getPV().position.translation;
        }
        else
        {
            dynamicProps.orientation = dynamicProps.orientation.identity();
            dynamicProps.position = Vector3(0.0f, 0.0f, 0.0f);
        }

        staticProps.guid = 0;
        staticProps.bodyUID = uid;
        if( body != NULL )
        {
            staticProps.guid = body->getBody()->getGuidIndex();
        }

        _dynamicProps.push_back(dynamicProps);
        _staticProps.push_back(staticProps);
        _massAndInertia.push_back(massAndInertia);
        _effectiveMassMultipliers.push_back( 0.0f );
        _velStageVirDisplacements[ offsetToAnchoredObjects + i ] = velStageVirD;
        _posStageVirDisplacements[ offsetToAnchoredObjects + i ] = posStageVirD;
    }
}

static RBX_SIMD_INLINE void integratePositions( SolverBodyDynamicProperties& _dynamicProps, const SolverBodyStaticProperties& _staticProps, 
                                                   const VirtualDisplacementPOD& _velStageVirD, const VirtualDisplacementPOD& _posStageVirD, float _dt )
{
    RBXASSERT_VERY_FAST(!RBX::Math::isNanInfVector3(_velStageVirD.lin));
    RBXASSERT_VERY_FAST(!RBX::Math::isNanInfVector3(_velStageVirD.ang));
    RBXASSERT_VERY_FAST(!RBX::Math::isNanInfVector3(_posStageVirD.lin));
    RBXASSERT_VERY_FAST(!RBX::Math::isNanInfVector3(_posStageVirD.ang));

    // Applying constraint impulses
    _dynamicProps.linearVelocity = _dynamicProps.integratedLinearVelocity + _velStageVirD.lin;
    _dynamicProps.angularVelocity = _dynamicProps.integratedAngularVelocity + _velStageVirD.ang;

    // Applying error correction impulse
    _dynamicProps.position += _posStageVirD.lin;
    Matrix3 changeInOrientation = Matrix3::fillRotation( _posStageVirD.ang );
    _dynamicProps.orientation = changeInOrientation * _dynamicProps.orientation;

    // Integrate positions
    _dynamicProps.position += _dynamicProps.linearVelocity * _dt;

    // Integrate orientation
    Vector3 infinitessimalRotation = _dynamicProps.angularVelocity * _dt;
    changeInOrientation = Matrix3::fillRotation( infinitessimalRotation );
    _dynamicProps.orientation = changeInOrientation * _dynamicProps.orientation;

    _dynamicProps.orientation.orthonormalize();
    RBXASSERT_VERY_FAST( !RBX::Math::isNanInfVector3( _dynamicProps.position ) );
    RBXASSERT_VERY_FAST( !RBX::Math::isNanInfVector3( _dynamicProps.orientation.row(0) ) );
    RBXASSERT_VERY_FAST( !RBX::Math::isNanInfVector3( _dynamicProps.orientation.row(1) ) );
    RBXASSERT_VERY_FAST( !RBX::Math::isNanInfVector3( _dynamicProps.orientation.row(2) ) );
}

void PGSSolver::integratePositionsAndUpdateSimBodies( 
    SimBody* const * _simBodies, 
    SolverBodyDynamicProperties* const _bodyVariableData,
    const SolverBodyStaticProperties* const _bodyStaticData, 
    size_t _simBodyCount,
    const VirtualDisplacementArray& _velocityDeltas,
    const VirtualDisplacementArray& _positionDeltas,
    float _dt )
{
    RBXPROFILER_SCOPE("Physics", "integratePositionsAndUpdateSimBodies");

    for( int index = 0; index < (int)_simBodyCount; index++ )
    {
        integratePositions( _bodyVariableData[ index ], _bodyStaticData[ index ], _velocityDeltas[ index ], _positionDeltas[ index ], _dt );

        SolverBodyCache& cache = bodyCache[ _simBodies[ index ]->getUID() ];
        cache.virDPosStageLin = _positionDeltas[ index ].lin;
        cache.virDPosStageAng = _positionDeltas[ index ].ang;
        cache.virDVelStageLin = _velocityDeltas[ index ].lin;
        cache.virDVelStageAng = _velocityDeltas[ index ].ang;
        cache.linearVelocity = _simBodies[ index ]->getPV().velocity.linear; //  _bodyVariableData[ index ].linearVelocity;
        cache.angularVelocity = _simBodies[ index ]->getPV().velocity.rotational; // _bodyVariableData[ index ].angularVelocity;
        cache.integratedLinearVelocity =  _bodyVariableData[ index ].integratedLinearVelocity;
        cache.integratedAngularVelocity =  _bodyVariableData[ index ].integratedAngularVelocity;
        cache.simBodyDebug = _simBodies[ index ];

        _simBodies[ index ]->updateFromSolver( 
            _bodyVariableData[ index ].position, _bodyVariableData[ index ].orientation, 
            _bodyVariableData[ index ].linearVelocity, _bodyVariableData[ index ].angularVelocity );
    }
}

static RBX_SIMD_INLINE void integratePositionsIgnoreVelocities( SolverBodyDynamicProperties& _dynamicProps, const SolverBodyStaticProperties& _staticProps, 
                                               const VirtualDisplacementPOD& _posStageVirD, float _dt )
{
    RBXASSERT_VERY_FAST(!RBX::Math::isNanInfVector3(_posStageVirD.lin));
    RBXASSERT_VERY_FAST(!RBX::Math::isNanInfVector3(_posStageVirD.ang));

    // Applying error correction impulse
    _dynamicProps.position += _posStageVirD.lin;
    Matrix3 changeInOrientation = Matrix3::fillRotation( _posStageVirD.ang );
    _dynamicProps.orientation = changeInOrientation * _dynamicProps.orientation;

    _dynamicProps.orientation.orthonormalize();
    RBXASSERT_VERY_FAST( !RBX::Math::isNanInfVector3( _dynamicProps.position ) );
    RBXASSERT_VERY_FAST( !RBX::Math::isNanInfVector3( _dynamicProps.orientation.row(0) ) );
    RBXASSERT_VERY_FAST( !RBX::Math::isNanInfVector3( _dynamicProps.orientation.row(1) ) );
    RBXASSERT_VERY_FAST( !RBX::Math::isNanInfVector3( _dynamicProps.orientation.row(2) ) );
}

void PGSSolver::integratePositionsIgnoreVelocitiesAndUpdateSimBodies( 
    SimBody* const * _simBodies, 
    SolverBodyDynamicProperties* const _bodyVariableData,
    const SolverBodyStaticProperties* const _bodyStaticData, 
    size_t _simBodyCount,
    const VirtualDisplacementArray& _positionDeltas,
    float _dt )
{
    RBXPROFILER_SCOPE("Physics", "integratePositionsIgnoreVelocitiesAndUpdateSimBodies");

    for( int index = 0; index < (int)_simBodyCount; index++ )
    {
        integratePositionsIgnoreVelocities( _bodyVariableData[ index ], _bodyStaticData[ index ], _positionDeltas[ index ], _dt );

        _simBodies[ index ]->updateFromSolver( _bodyVariableData[ index ].position, _bodyVariableData[ index ].orientation, Vector3(0.0f), Vector3(0.0f) );
    }
}

static const int sampleCount = 240;
PGSSolver::PGSSolver(): constraintUIDGenerator( 0 ),
    inconsistentConstraintDetectorEnabled( false ),
    physicsAnalyzerBreakOnIssue( true ),
    gatherCollisionsProfiler( sampleCount, "  PGS Gather collisions & constraints: %.3fms" ),
    islandSplitProfiler( sampleCount, "  PGS Split into islands: %.3fms"),
    integrateVelocitiesProfiler( sampleCount, "  PGS Integrate velocities: %.3fms" ),
    initAnchoredBodiesProfiler( sampleCount, "  PGS Init anchored bodies: %.3fms" ),
    buildEquationsProfiler( sampleCount, "  PGS Build equations: %.3fms" ),
    computeEffectiveMassesProfiler( sampleCount, "  PGS Compute effective masses: %.3fms" ),
    preconditioningProfiler( sampleCount, "  PGS Preconditioning: %.3fms" ),
    multiplyEffectiveMassMultipliersProfiler( sampleCount, "  PGS Pre-multiply mass multipliers: %.3fms" ),
    initVirDProfiler( sampleCount, "  PGS Init Virtual Displacements: %.3fms" ),
    kernelProfiler( sampleCount, "  PGS Kernel: %.3fms" ),
    integratePositionsProfiler( sampleCount, "  PGS Integrate positions: %.3fms" ),
    writeCacheProfiler( sampleCount, "  PGS Write constraint cache: %.3fms" ),
    solverProfiler( sampleCount, "PGS Solver total: %.2fms" ),
    dumpLogSwitch( false ),
    userId( 0 )
{
}


void PGSSolver::detectInconsistentConstraints( 
    VirtualDisplacementArray& _positionDeltas,
    ArrayBase< ConstraintVariables >& _positionStage,
    const ArrayBase< ConstraintJacobianPair >& _jacobians,
    const ArrayBase< ConstraintJacobianPair >& _preconditionedJacobiansPosStage,
    const ArrayBase< EffectiveMassPair >& _effectiveMassesPosStage,
    const ArrayBase< float >& _sorPos,
    const ArrayBase< boost::uint8_t >& _dimensions,
    const ArrayBase< boost::uint32_t >& _offsets,
    const ArrayBase< BodyPairIndices >& _simBodyPairs,
    const ArrayBase< Constraint* >& _constraints,
    size_t _collisionCount,
    const SolverConfig& _solverConfig )
{
    ArrayDynamic< float > impulseDeltas( _positionStage.size() );
    impulseDeltas.assign(_positionStage.size(), 0.0f);
    ArrayDynamic< float > impulseDeltaDeltas( _positionStage.size() );
    impulseDeltaDeltas.assign(_positionStage.size(), 0.0f);

    ArrayDynamic< float > displacements( _positionStage.size() );
    ArrayDynamic< float > residuals( _positionStage.size() );
    ArrayDynamic< float > deltaResiduals( _positionStage.size() );

    _positionDeltas.reset();

//#define ENABLE_INCONSISTENT_CONSTRAINT_DETECTOR_DEBUG

#ifdef ENABLE_INCONSISTENT_CONSTRAINT_DETECTOR_DEBUG
    std::stringstream out;
    out << "Progression: ";
#endif

    static int minSteadyIterations = 5;
    int divergingIterations = 0;
    size_t divergingConstraints = 0;
    size_t convergingConstraints = 0;

    double oldEnergy = 0.0f;
    
    double energy = 0.0f;
    double dEnergy = 0.0f;
    double d2Energy = 0.0f;
    int iteration = 0;
    for( ; ; iteration++ )
    {
        // Run the PGS for a few iterations
        PGSSolveKernelComputeErrors( impulseDeltas, impulseDeltaDeltas, 
            _positionStage, 
            _positionDeltas,
            _constraints.size(), _collisionCount, _positionDeltas.getSize(), 
            _dimensions.data(), _simBodyPairs.data(),
            _jacobians.data(),
            _preconditionedJacobiansPosStage.data(), 
            _effectiveMassesPosStage.data(), _solverConfig );

        oldEnergy = energy;
        energy = 0.0f;
        dEnergy = 0.0f;
        d2Energy = 0.0f;

        // Project the impulses onto displacements
        for( size_t i = 0; i < residuals.size(); i++ )
        {
            simd::v4f d = _jacobians[i].dot(_effectiveMassesPosStage[i]);
            simd::storeSingle( &displacements[i], simd::splat(_positionStage[i].impulse) * d );
            simd::storeSingle( &residuals[i], simd::splat(impulseDeltas[i]) * d );
            simd::storeSingle( &deltaResiduals[i], simd::splat(impulseDeltaDeltas[i]) * d );
            energy += 0.5f * _positionStage[i].impulse * displacements[i];
            dEnergy += _positionStage[i].impulse * residuals[i];
            d2Energy += _positionStage[i].impulse * deltaResiduals[i];
        }

        // Test the convergence of the constraints
        divergingConstraints = 0;
        convergingConstraints = 0;
        for( size_t i = 0; i < _constraints.size(); i++ )
        {
            boost::uint32_t offset = _offsets[ i ];
            switch( _constraints[ i ]->testPGSConvergence(displacements.data() + offset, residuals.data() + offset, deltaResiduals.data() + offset, _solverConfig) )
            {
            case Constraint::Convergence_Converges:
                convergingConstraints++;
                break;
            case Constraint::Convergence_Diverges:
                divergingConstraints++;
                break;
            default:
                break;
            }
        }

        if( convergingConstraints == _constraints.size() )
        {
#ifdef ENABLE_INCONSISTENT_CONSTRAINT_DETECTOR_DEBUG
            out << "Converged! ";
#endif
            break;
        }
        if( divergingConstraints + convergingConstraints == _constraints.size() )
        {
            divergingIterations++;
            if( divergingIterations == minSteadyIterations )
            {
#ifdef ENABLE_INCONSISTENT_CONSTRAINT_DETECTOR_DEBUG
                out << "Diverged! ";
#endif
                break;
            }
        }
        else
        {
            divergingIterations = 0;
        }
        if( ( iteration+1 ) * _solverConfig.pgsIterations >= _solverConfig.inconsistentConstraintMaxIterations )
        {
#ifdef ENABLE_INCONSISTENT_CONSTRAINT_DETECTOR_DEBUG
            out << "Undetermined! ";
#endif
            break;
        }
    }

#ifdef ENABLE_INCONSISTENT_CONSTRAINT_DETECTOR_DEBUG
    if( divergingConstraints + convergingConstraints != _constraints.size() )
    {
        if( divergingConstraints >= convergingConstraints / 2 )
        {
            out << "Probably diverging";
        }
        else
        {
            out << "Probably converging";
        }
    }
#endif

#ifdef ENABLE_INCONSISTENT_CONSTRAINT_DETECTOR_DEBUG
    float dEnergyAverage = ( energy - oldEnergy ) / _solverConfig.pgsIterations;
    unsigned iterations = ( iteration + 1 ) * _solverConfig.pgsIterations;
    out << "Iterations: " << iterations << ". ";
    out << "E = " << energy << ", dE = " << dEnergy << ", gamma = " << 100.0 * dEnergy / dEnergyAverage - 100.0 << "%%, " << 100.0 * d2Energy / dEnergyAverage << "%% ";
    RBX::StandardOut::singleton()->printf(RBX::MESSAGE_OUTPUT, out.str().c_str() );
#endif

    serializer & residuals & deltaResiduals;

    if( convergingConstraints != _constraints.size() )
    {
        inconsistentBodies.push_back( ArrayDynamic< boost::uint64_t >() );
    }

    boost::unordered_set< boost::uint64_t > alreadyRegisteredBodies;
    for( size_t i = 0; i < _constraints.size(); i++ )
    {
        boost::uint32_t offset = _offsets[ i ];
        Constraint::Convergence convergence = _constraints[ i ]->testPGSConvergence(displacements.data() + offset, residuals.data() + offset, deltaResiduals.data() + offset, _solverConfig);

        if( convergence == Constraint::Convergence_Converges )
        {
            continue;
        }

        if( simBodies.find( _constraints[i]->getBodyA()->getRootSimBody() ) != simBodies.end() 
            && alreadyRegisteredBodies.find( _constraints[i]->getBodyA()->getUID() ) == alreadyRegisteredBodies.end() )
        {
            alreadyRegisteredBodies.insert( _constraints[i]->getBodyA()->getUID() );
            inconsistentBodies.back().push_back( _constraints[i]->getBodyA()->getUID() );
        }
        if( _constraints[i]->getBodyB() && simBodies.find( _constraints[i]->getBodyB()->getRootSimBody() ) != simBodies.end()
            && alreadyRegisteredBodies.find( _constraints[i]->getBodyB()->getUID() ) == alreadyRegisteredBodies.end() )
        {
            alreadyRegisteredBodies.insert( _constraints[i]->getBodyB()->getUID() );
            inconsistentBodies.back().push_back( _constraints[i]->getBodyB()->getUID() );
        }

        InconsistentBodyPair pair;
        pair.convergence = convergence;
        pair.bodyA = _constraints[i]->getBodyA();
        pair.bodyB = _constraints[i]->getBodyB();
        pair.bodyPair.first = _constraints[i]->getBodyA()->getUID();
        pair.bodyPair.second = _constraints[i]->getBodyB()->getUID();
        inconsistentBodyPairs.push_back( pair );
    }
}

template< class T >
class MultiIndexList
{
public:
    MultiIndexList( size_t listCount, size_t storageSize )
    {
        next.assign( storageSize, -1 );
        first.assign( listCount, -1 );
        storage.reserve( storageSize );
    }

    void addToList( size_t list, T value )
    {
        boost::int32_t element = ( boost::int32_t )storage.size();
        storage.push_back( value );
        if( first[ list ] == -1 )
        {
            first[ list ] = element;
            return;
        }
        next[ element ] = first[ list ];
        first[ list ] = element;
    }

    size_t getFirst( size_t list ) const
    {
        return first[ list ];
    }

    size_t getNext( size_t element ) const
    {
        return next[ element ];
    }

    bool isValid( boost::int32_t index ) const
    {
        return index != -1;
    }

    T getValue( boost::int32_t index ) const
    {
        RBXASSERT_VERY_FAST( index != -1 );
        return storage[ index ];
    }

private:
    ArrayDynamic< boost::int32_t > first;
    ArrayDynamic< boost::int32_t > next;
    ArrayDynamic< T > storage;
};

static void addAdjacentBodies( ArrayDynamic< boost::uint32_t >& _islandSimBodies, 
                              ArrayDynamic< boost::uint32_t >& _islandConstraints,
                              ArrayBase< boost::uint8_t >& _visitedBodies,
                              ArrayBase< boost::uint8_t >& _visitedConstraints,
                              ArrayDynamic< boost::int32_t >& _simBodyRecursionStack,
                              boost::int32_t _rootBodyIndex, 
                              const ArrayBase< std::pair< boost::int32_t, boost::int32_t > >& _bodyIndexPairs,
                              const MultiIndexList< boost::uint32_t >& _adjacencyLists )
{
    // Push the root body index onto the stack
    for( _simBodyRecursionStack.push_back( _rootBodyIndex ); _simBodyRecursionStack.size() > 0;)
    {
        boost::int32_t bodyIndex = _simBodyRecursionStack.back();
        _simBodyRecursionStack.pop_back();

        // Iterate over the list of constraints involving this body
        for( size_t it = _adjacencyLists.getFirst( bodyIndex ); 
            _adjacencyLists.isValid( it ); 
            it = _adjacencyLists.getNext( it ) )
        {
            size_t cIndex = _adjacencyLists.getValue( it );

            // Ignore if this constraint has been visited
            if( _visitedConstraints[ cIndex ] )
            {
                continue;
            }
            _visitedConstraints[ cIndex ] = 1;
            _islandConstraints.push_back( cIndex );

            // Push the second body of the constraint onto the stack
            {
                boost::int32_t index = _bodyIndexPairs[ cIndex ].first;
                if( index != -1 && !_visitedBodies[ index ] )
                {
                    _visitedBodies[ index ] = 1;
                    _islandSimBodies.push_back( index );
                    _simBodyRecursionStack.push_back( index );
                }
            }

            {
                boost::int32_t index = _bodyIndexPairs[ cIndex ].second;
                if( index != -1 && !_visitedBodies[ index ] )
                {
                    _visitedBodies[ index ] = 1;
                    _islandSimBodies.push_back( index );
                    _simBodyRecursionStack.push_back( index );
                }
            }
        }
    }
}

static void breakIntoIslands( ArrayDynamic< SimBody* >& _islandSimBodies, 
                             ArrayDynamic< Constraint* >& _islandConstraints,
                             ArrayDynamic< std::pair< boost::uint32_t, boost::uint32_t > >& _islandSizes,
                            const boost::unordered_set< SimBody* >& _simBodies, 
                            const ArrayBase< Constraint* >& _constraints )
{
    RBXPROFILER_SCOPE("Physics", "breakIntoIslands");
    // Index the bodies
    DenseHashMap< SimBody*, boost::int32_t > bodyIndexation( NULL, G3D::ceilPow2( 2 * _simBodies.size() ) );
    ArrayDynamic< SimBody* > simBodyList( _simBodies.size(), ArrayNoInit() );
    boost::int32_t index = 0;
    for( auto b : _simBodies )
    {
        bodyIndexation[ b ] = index;
        simBodyList[ index ] = b;
        index++;
    }

    // Form the adjacency map
    MultiIndexList< boost::uint32_t > adjacencyLists( _simBodies.size(), 2 * _constraints.size() );
    ArrayDynamic< std::pair< boost::int32_t, boost::int32_t > > bodyIndexPairs( _constraints.size(), ArrayNoInit() );
    for( boost::uint32_t i = 0; i < ( boost::uint32_t )_constraints.size(); i++ )
    {
        auto c = _constraints[ i ];
        std::pair< boost::int32_t, boost::int32_t > pair( -1, -1 );
        boost::int32_t* indexA = bodyIndexation.find( c->getBodyA()->getRootSimBody() );
        if( indexA != NULL )
        {
            adjacencyLists.addToList( *indexA, i );
            pair.first = *indexA;
        }

        if( c->getBodyB() != NULL )
        {
            boost::int32_t* indexB = bodyIndexation.find( c->getBodyB()->getRootSimBody() );
            if( indexB != NULL )
            {
                adjacencyLists.addToList( *indexB, i );
                pair.second = *indexB;
            }
        }
        bodyIndexPairs[ i ] = pair;
    }

    // Reserve for the worst case scenario
    _islandSimBodies.reserve( _simBodies.size() );
    _islandConstraints.reserve( _constraints.size() );
    _islandSizes.reserve( _simBodies.size() );

    // Temporary buffers
    // These are all the memory allocations necessary
    ArrayDynamic< boost::uint8_t > visitedBodies;
    visitedBodies.assign( _simBodies.size(), 0 );
    ArrayDynamic< boost::uint8_t > visitedConstraints;
    visitedConstraints.assign( _constraints.size(), 0 );
    ArrayDynamic< boost::uint32_t > indexedIslandConstraints;
    indexedIslandConstraints.reserve( _constraints.size() );
    ArrayDynamic< boost::uint32_t > indexedIslandBodies;
    indexedIslandBodies.reserve( _simBodies.size() );
    ArrayDynamic< boost::int32_t > tempBuffer;
    tempBuffer.reserve( _simBodies.size() );
    
    for( boost::uint32_t bodyIndex = 0; bodyIndex < simBodyList.size(); bodyIndex++ )
    {
        if( !visitedBodies[ bodyIndex ] )
        {
            visitedBodies[ bodyIndex ] = 1;

            indexedIslandBodies.push_back( bodyIndex );
            addAdjacentBodies( indexedIslandBodies, indexedIslandConstraints, visitedBodies, visitedConstraints, tempBuffer, bodyIndex, bodyIndexPairs, adjacencyLists );

            // Store the island size
            _islandSizes.push_back( std::pair< boost::uint32_t, boost::uint32_t >( indexedIslandBodies.size(), indexedIslandConstraints.size() ) );

            // Sort and add constraints
            std::sort( indexedIslandConstraints.begin(), indexedIslandConstraints.end() );
            for( auto p : indexedIslandConstraints )
            {
                _islandConstraints.push_back( _constraints[ p ] );
            }

            // Add the bodies
            for( auto p : indexedIslandBodies )
            {
                _islandSimBodies.push_back( simBodyList[ p ] );
            }

            // Cleanup
            indexedIslandBodies.clear();
            indexedIslandConstraints.clear();
        }
    }
}

void PGSSolver::solve( const std::vector< ContactConnector* >& _contactConnectors, float _dt, boost::uint64_t debugTime, bool _throttled )
{
    inconsistentBodyPairs.clear();
    inconsistentBodies.clear();

    if( FFlag::PhysicsAnalyzerEnabled && inconsistentConstraintDetectorEnabled )
    {
        static SolverConfig solverConfigInconsitencyDetector( SolverConfig::Type_InconsistencyDetector );
        PGSSolver::solveInternal(_contactConnectors, _dt, debugTime, _throttled, solverConfigInconsitencyDetector);
    }

    // Solve only if there are no inconsistent constraints detected
    if( !physicsAnalyzerBreakOnIssue || ( inconsistentBodyPairs.size() == 0 ) )
    {
        if( !DFFlag::PGSSolverUsesIslandizableCode )
        {
            solveLegacy(_contactConnectors, _dt, debugTime, _throttled);
        }
        else
        {
            static SolverConfig solverConfigDefault( SolverConfig::Type_Default );
            solverConfigDefault.useSimIslands = DFFlag::PGSSolverSimIslandsEnabled;
            PGSSolver::solveInternal(_contactConnectors, _dt, debugTime, _throttled, solverConfigDefault);
        }
    }
}

void PGSSolver::solvePositions( const std::vector< ContactConnector* >& _contactConnectors )
{
    static SolverConfig solverConfig( SolverConfig::Type_PositionalCorrection );

    PGSSolver::solveInternal(_contactConnectors, 0.0f, 0, false, solverConfig);
}

void PGSSolver::solveInternal( const std::vector< ContactConnector* >& _contactConnectors, float _dt, boost::uint64_t debugTime, bool _throttled, const SolverConfig& _solverConfig )
{
    RBXPROFILER_SCOPE("Physics", "PGSSolver::solve");

    // To enable add '"FFlagPGSSolverFileDump": "True"' in ClientSettings/ClientAppSettings.json
    // Press Ctrl-F8 to start recording, Ctrl-Shift-F8 to stop
    
    serializer.update( dumpLogSwitch, userId, debugTime );
    SolverConfig solverConfig( _solverConfig );

    solverProfiler.start();
    gatherCollisionsProfiler.start();

    // All objects will be indexed in this map. Uses DenseHashMap.
    const boost::unordered_set< SimBody* >& selectedSimBodies = _throttled ? highPrioritySimBodies : simBodies;

    // Form manifolds and match against existing ones
    ArrayDynamic< ContactManifold* > activeManifolds;
    activeManifolds.reserve( contactManifolds.size() );
    size_t collisionCount = addContactConnectors( activeManifolds, _contactConnectors, selectedSimBodies );

    //
    // Gather collisions and constraints into a single array
    //
    ArrayDynamic< Constraint* > constraints;
    constraints.reserve( pureConstraintSet.size() + collisionCount );

    // Iterate over the (ordered map) of constraints - linear in number of elements.
    {
        RBXPROFILER_SCOPE("Physics", "fillConstraintSet");

        if( _throttled )
        {
            // If we are throttling, ignore any constraints that aren't involved with high priority bodies
            for( const auto& it : pureConstraintSet )
            {
                if( highPrioritySimBodies.find( it.second->getBodyA()->getRootSimBody() ) == highPrioritySimBodies.end() 
                    && ( ( it.second->getBodyB() == NULL ) || highPrioritySimBodies.find( it.second->getBodyB()->getRootSimBody() ) == highPrioritySimBodies.end() ) )
                {
                    continue;
                }
                if( !it.second->isBroken() )
                {
                    constraints.push_back( it.second );
                }
            }
        }
        else
        {
            for( const auto& it : pureConstraintSet )
            {
                if( !it.second->isBroken() )
                {
                    constraints.push_back( it.second );
                }
            }
        }
    
        for( const auto manifold : activeManifolds )
        {
            constraints.insert( constraints.end(), manifold->collisions.cbegin(), manifold->collisions.cend() );
        }
    }
    gatherCollisionsProfiler.end();

    serializer & solverConfig.pgsIterations;

    serializer.serializeConstraints(constraints);

    if( solverConfig.useSimIslands )
    {
        // Break bodies and constraints into islands
        islandSplitProfiler.start();
        ArrayDynamic< SimBody* > islandBodies;
        ArrayDynamic< Constraint* > islandConstraints;
        ArrayDynamic< std::pair< boost::uint32_t, boost::uint32_t > > islandSizes;
        breakIntoIslands( islandBodies, islandConstraints, islandSizes, selectedSimBodies, constraints );
        islandSplitProfiler.end();

        if( solverConfig.inconsistentConstraintDetectorEnabled )
        {
            inconsistentBodies.reserve( islandSizes.size() );
        }

        // Dispatch the solve tasks
        size_t constraintIndex = 0;
        size_t simBodyIndex = 0;
        for( size_t i = 0; i < islandSizes.size(); i++ )
        {
            ArrayRef< SimBody* > simBodiesRef( islandBodies.begin() + simBodyIndex, islandSizes[ i ].first );
            simBodyIndex += islandSizes[ i ].first;
            ArrayRef< Constraint* > constraintsRef( islandConstraints.begin() + constraintIndex, islandSizes[ i ].second );
            constraintIndex += islandSizes[ i ].second;
            solveIsland( constraintsRef, simBodiesRef,  _dt, solverConfig );
        }
    }
    else
    {
        ArrayDynamic< SimBody* > simBodyArray;
        simBodyArray.reserve( selectedSimBodies.size() );
        for( auto b : selectedSimBodies )
        {
            simBodyArray.push_back( b );
        }
        solveIsland( constraints, simBodyArray, _dt, solverConfig );
    }

    solverProfiler.end();

    // Print profiling data
    gatherCollisionsProfiler.printStats();
    islandSplitProfiler.printStats();
    integrateVelocitiesProfiler.printStats();
    initAnchoredBodiesProfiler.printStats();
    buildEquationsProfiler.printStats();
    computeEffectiveMassesProfiler.printStats();
    preconditioningProfiler.printStats();
    multiplyEffectiveMassMultipliersProfiler.printStats();
    initVirDProfiler.printStats();
    kernelProfiler.printStats();
    writeCacheProfiler.printStats();
    integratePositionsProfiler.printStats();
    solverProfiler.printStats();
}

void PGSSolver::solveIsland( const ArrayDynamic< Constraint* >& _constraints, 
                            const ArrayDynamic< SimBody* >& _simBodies, 
                            float _dt, 
                            const SolverConfig& solverConfig )
{
    // Align to cache lines
    int defaultAlignment = 64;

    BodyIndexation bodyIndexation( (SimBody*)NULL, G3D::ceilPow2( 2 * _simBodies.size() ) );
    ArrayDynamic< SimBody* > simBodyList;
    {
        RBXPROFILER_SCOPE("Physics", "generateBodyIndices");

        simBodyList.reserve( _simBodies.size() );
        for( auto body : _simBodies )
        {
            bodyIndexation[ body ] = (int)simBodyList.size();
            simBodyList.push_back( body );
        }
    }

    //
    // Gather sim body indices for each constraint
    //
    ArrayDynamic< boost::uint8_t > dimensions( _constraints.size(), ArrayNoInit() );
    ArrayDynamic< boost::uint32_t > offsets( _constraints.size(), ArrayNoInit() );
    ArrayDynamic< BodyPairIndices > simBodyPairs( _constraints.size(), ArrayNoInit() );

    // Listing anchored bodies interacting with sim bodies
    ArrayDynamic< SimBody* > anchoredBodyList;
    boost::uint32_t totalBlockSize;
    boost::uint32_t collisionCount = 0;
    auto totalDimension = gatherConstraintPairData(simBodyPairs, dimensions, offsets, anchoredBodyList, bodyIndexation, totalBlockSize, collisionCount, _constraints );

    //
    // Init sim bodies, integrate velocities
    //
    integrateVelocitiesProfiler.start();
    ArrayDynamic< SolverBodyDynamicProperties > bodyVariableData;
    ArrayDynamic< SolverBodyStaticProperties > bodyStaticData;
    ArrayDynamic< SolverBodyMassAndInertia > massAndInertia;
    ArrayDynamic< float > effectiveMassMultipliers;
    bodyVariableData.reserve( bodyIndexation.size() );
    bodyStaticData.reserve( bodyIndexation.size() );
    massAndInertia.reserve( bodyIndexation.size() );
    effectiveMassMultipliers.reserve( bodyIndexation.size() );
    integrateVelocitiesAndInitSimulatedObjects( bodyStaticData, bodyVariableData, massAndInertia, effectiveMassMultipliers, simBodyList, _dt, solverConfig );
    integrateVelocitiesProfiler.end();

    initAnchoredBodiesProfiler.start();
    VirtualDisplacementArray velocityDeltasSIMD( bodyIndexation.size(), defaultAlignment );
    VirtualDisplacementArray positionDeltasSIMD( bodyIndexation.size(), defaultAlignment );
    velocityDeltasSIMD.reset();
    positionDeltasSIMD.reset();
    initAnchoredObjects(bodyVariableData, bodyStaticData, massAndInertia, effectiveMassMultipliers, velocityDeltasSIMD, positionDeltasSIMD, (int)simBodyList.size(), anchoredBodyList, solverConfig);

    initAnchoredBodiesProfiler.end();

    serializer.serializeForces(simBodyList, bodyStaticData.size());
    serializer.tag("Bodies");
    serializer & simBodyPairs & bodyVariableData & bodyStaticData & massAndInertia;

    //
    // Build constraint equations: setup the Jacobians and the reaction vectors, restore cached solutions
    //
    buildEquationsProfiler.start();
    ArrayDynamic< ConstraintJacobianPair > jacobians( totalDimension, ArrayNoInit(), defaultAlignment );
    ArrayDynamic< ConstraintVariables > velocityStage( totalDimension, ArrayNoInit(), defaultAlignment );
    ArrayDynamic< ConstraintVariables > positionStage( totalDimension, ArrayNoInit(), defaultAlignment );
    ArrayDynamic< float > sorVel( totalDimension, ArrayNoInit(), defaultAlignment );
    ArrayDynamic< float > sorPos( totalDimension, ArrayNoInit(), defaultAlignment );
    ArrayDynamic< boost::uint8_t > useBlock( totalDimension, ArrayNoInit() );
    {
        RBXPROFILER_SCOPE("Physics", "buildConstraintEquations");

        for( size_t i = 0; i < _constraints.size(); i++ )
        {
            const auto& bodyA = bodyVariableData[ simBodyPairs[i].first ];
            const auto& bodyB = bodyVariableData[ simBodyPairs[i].second ];
            boost::uint32_t currentOffset = offsets[ i ];
            _constraints[ i ]->restoreCacheAndBuildEquation( 
                jacobians.data() + currentOffset, 
                velocityStage.data() + currentOffset, 
                positionStage.data() + currentOffset,
                sorVel.data() + currentOffset,
                sorPos.data() + currentOffset,
                useBlock.data() + currentOffset,
                bodyA, bodyB, solverConfig, _dt );
        }
        if( !solverConfig.velocityStageSOREnabled )
        {
            sorVel.assign(sorVel.size(),1.0f);
        }
        if( !solverConfig.positionStageSOREnabled )
        {
            sorPos.assign(sorPos.size(),1.0f);
        }
    }
    buildEquationsProfiler.end();

    serializer.tag("Variables");
    serializer & velocityStage & positionStage;

    //
    // Compute effective masses: M^-1 * J^t
    //
    computeEffectiveMassesProfiler.start();
    ArrayDynamic< EffectiveMassPair > effectiveMassesVel( totalDimension, ArrayNoInit(), defaultAlignment );
    ArrayDynamic< EffectiveMassPair > effectiveMassesPos( totalDimension, ArrayNoInit(), defaultAlignment );
    PGSComputeEffectiveMasses(effectiveMassesVel.data(), effectiveMassesPos.data(), _constraints.size(), dimensions.data(), jacobians.data(), simBodyPairs.data(), massAndInertia.data(), solverConfig);
    computeEffectiveMassesProfiler.end();

    //
    // Pre-condition the Jacobians and reaction vectors
    //
    preconditioningProfiler.start();
    ArrayDynamic< ConstraintJacobianPair > preconditionedJacobiansVelStage( totalDimension, ArrayNoInit(), defaultAlignment );
    ArrayDynamic< ConstraintJacobianPair > preconditionedJacobiansPosStage( totalDimension, ArrayNoInit(), defaultAlignment );
    PGSPreconditionConstraintEquations(preconditionedJacobiansVelStage.data(), preconditionedJacobiansPosStage.data(), 
        velocityStage.data(), positionStage.data(), _constraints.size(), dimensions.data(), useBlock.data(), 
        sorVel.data(), sorPos.data(), jacobians.data(), effectiveMassesVel.data(), effectiveMassesPos.data() );
    preconditioningProfiler.end();

    //
    // Collapse the masses of sleeping simulated objects to infinity
    // This has to be done after pre-conditioning
    //
    multiplyEffectiveMassMultipliersProfiler.start();
    PGSApplyEffectiveMassMultipliers( effectiveMassesVel.data(), effectiveMassesPos.data(), _constraints.size(), dimensions.data(), effectiveMassMultipliers.data(),
        simBodyPairs.data(), solverConfig );
    multiplyEffectiveMassMultipliersProfiler.end();

    serializer.tag("Jacobians");
    serializer & jacobians & preconditionedJacobiansVelStage & preconditionedJacobiansPosStage & effectiveMassesVel & effectiveMassesPos;

    //
    // Initialize the virtual displacements using previously computed impulses (cached)
    //
    initVirDProfiler.start();
    PGSInitVirtualDisplacements( velocityDeltasSIMD, positionDeltasSIMD, effectiveMassesVel.data(), effectiveMassesPos.data(), _constraints.size(), dimensions.data(), velocityStage.data(), positionStage.data(), simBodyPairs.data(), solverConfig );
    initVirDProfiler.end();

    //
    // Run the kernel
    //
    kernelProfiler.start();
    if( solverConfig.inconsistentConstraintDetectorEnabled )
    {
        detectInconsistentConstraints(positionDeltasSIMD, positionStage, jacobians, preconditionedJacobiansPosStage, effectiveMassesPos, sorPos, dimensions, offsets, simBodyPairs, _constraints, collisionCount, solverConfig );
    }
    else
    {
        PGSSolveKernel( 
            velocityStage.data(), positionStage.data(),
            velocityDeltasSIMD, positionDeltasSIMD,
            _constraints.size(), collisionCount,
            dimensions.data(), simBodyPairs.data(),
            preconditionedJacobiansVelStage.data(), preconditionedJacobiansPosStage.data(),
            effectiveMassesVel.data(), effectiveMassesPos.data(), solverConfig );
    }
    kernelProfiler.end();

    serializer.tag("Results");
    serializer.serializeComputedImpulse(velocityStage, positionStage);
    serializer & velocityDeltasSIMD & positionDeltasSIMD;

    //
    // Write out the constraint cache
    //
    writeCacheProfiler.start();
    if( solverConfig.constraintCachingEnabled )
    {
        RBXPROFILER_SCOPE("Physics", "updateConstraintCache");

        for( size_t i = 0; i < _constraints.size(); i++ )
        {
            auto currentOffset = offsets[ i ];
            _constraints[ i ]->updateBrokenState( velocityStage.data() + currentOffset, positionStage.data() + currentOffset, solverConfig );
            _constraints[ i ]->cache(velocityStage.data() + currentOffset, positionStage.data() + currentOffset, sorVel.data() + currentOffset, sorPos.data() + currentOffset, solverConfig);
        }
    }
    writeCacheProfiler.end();

    //
    // Integrate positions and write out into SimBodies
    //
    integratePositionsProfiler.start();
    if( solverConfig.updateSimBodies )
    {
        if( !solverConfig.integrateOnlyPositions )
        {
            integratePositionsAndUpdateSimBodies( simBodyList.data(), bodyVariableData.data(), bodyStaticData.data(), simBodyList.size(), velocityDeltasSIMD, positionDeltasSIMD, _dt );
        }
        else
        {
            integratePositionsIgnoreVelocitiesAndUpdateSimBodies( simBodyList.data(), bodyVariableData.data(), bodyStaticData.data(), simBodyList.size(), positionDeltasSIMD, _dt );
        }
    }
    integratePositionsProfiler.end();

    serializer.tag("Integrated");
    serializer & bodyVariableData;

    serializer.tag("Cache");
    serializer.serializeBodyCache(bodyCache);
}

void PGSSolver::solveLegacy( const std::vector< ContactConnector* >& _contactConnectors, float _dt, boost::uint64_t debugTime, bool _throttled )
{
    RBXPROFILER_SCOPE("Physics", "PGSSolver::solve");

    // Align to cache lines
    int defaultAlignment = 64;

    // To enable add '"FFlagPGSSolverFileDump": "True"' in ClientSettings/ClientAppSettings.json
    // Press Ctrl-F8 to start recording, Ctrl-Shift-F8 to stop
    
    serializer.update( dumpLogSwitch, userId, debugTime );

    // Static SolverConfig so we can change values from debugger
    static SolverConfig solverConfig;

    solverProfiler.start();
    gatherCollisionsProfiler.start();

    // All objects will be indexed in this map. Uses DenseHashMap.
    const boost::unordered_set< SimBody* >& selectedSimBodies = _throttled ? highPrioritySimBodies : simBodies;
    BodyIndexation bodyIndexation( (SimBody*)NULL, G3D::ceilPow2( 2 * simBodies.size() ) );
    ArrayDynamic< SimBody* > simBodyList;
    {
        RBXPROFILER_SCOPE("Physics", "generateBodyIndices");

        simBodyList.reserve( simBodies.size() );
        for( auto body : selectedSimBodies )
        {
            bodyIndexation[ body ] = (int)simBodyList.size();
            simBodyList.push_back( body );
        }
    }

    // Form manifolds and match against existing ones
    ArrayDynamic< ContactManifold* > activeManifolds;
    activeManifolds.reserve( contactManifolds.size() );
    boost::uint32_t collisionCount = ( boost::uint32_t )addContactConnectors( activeManifolds, _contactConnectors, selectedSimBodies );

    //
    // Gather collisions and constraints into a single array
    //
    ArrayDynamic< Constraint* > constraints;
    constraints.reserve( pureConstraintSet.size() + collisionCount );

    // Iterate over the (ordered map) of constraints - linear in number of elements.
    {
        RBXPROFILER_SCOPE("Physics", "fillConstraintSet");

        if( _throttled )
        {
            // If we are throttling, ignore any constraints that aren't involved with high priority bodies
            for( const auto& it : pureConstraintSet )
            {
                if( highPrioritySimBodies.find( it.second->getBodyA()->getRootSimBody() ) == highPrioritySimBodies.end() 
                    && ( ( it.second->getBodyB() == NULL ) || highPrioritySimBodies.find( it.second->getBodyB()->getRootSimBody() ) == highPrioritySimBodies.end() ) )
                {
                    continue;
                }
                if( !it.second->isBroken() )
                {
                    constraints.push_back( it.second );
                }
            }
        }
        else
        {
            for( const auto& it : pureConstraintSet )
            {
                if( !it.second->isBroken() )
                {
                    constraints.push_back( it.second );
                }
            }
        }
    
        for( const auto manifold : activeManifolds )
        {
            constraints.insert( constraints.end(), manifold->collisions.cbegin(), manifold->collisions.cend() );
        }
    }

    serializer.serializeConstraints(constraints);

    //
    // Gather sim body indices for each constraint
    //
    ArrayDynamic< boost::uint8_t > dimensions( constraints.size(), ArrayNoInit() );
    ArrayDynamic< boost::uint32_t > offsets( constraints.size(), ArrayNoInit() );
    ArrayDynamic< BodyPairIndices > simBodyPairs( constraints.size(), ArrayNoInit() );

    // Listing anchored bodies interacting with sim bodies
    ArrayDynamic< SimBody* > anchoredBodyList;
    boost::uint32_t totalBlockSize;
    auto totalDimension = gatherConstraintPairData(simBodyPairs, dimensions, offsets, anchoredBodyList, bodyIndexation, totalBlockSize, collisionCount, constraints );
    gatherCollisionsProfiler.end();

    //
    // Init sim bodies, integrate velocities
    //
    integrateVelocitiesProfiler.start();
    ArrayDynamic< SolverBodyDynamicProperties > bodyVariableData;
    ArrayDynamic< SolverBodyStaticProperties > bodyStaticData;
    ArrayDynamic< SolverBodyMassAndInertia > massAndInertia;
    ArrayDynamic< float > effectiveMassMultipliers;
    bodyVariableData.reserve( bodyIndexation.size() );
    bodyStaticData.reserve( bodyIndexation.size() );
    massAndInertia.reserve( bodyIndexation.size() );
    effectiveMassMultipliers.reserve( bodyIndexation.size() );
    integrateVelocitiesAndInitSimulatedObjects( bodyStaticData, bodyVariableData, massAndInertia, effectiveMassMultipliers, simBodyList, _dt, solverConfig );
    integrateVelocitiesProfiler.end();

    initAnchoredBodiesProfiler.start();
    VirtualDisplacementArray velocityDeltasSIMD( bodyIndexation.size(), defaultAlignment );
    VirtualDisplacementArray positionDeltasSIMD( bodyIndexation.size(), defaultAlignment );
    velocityDeltasSIMD.reset();
    positionDeltasSIMD.reset();
    initAnchoredObjects(bodyVariableData, bodyStaticData, massAndInertia, effectiveMassMultipliers, velocityDeltasSIMD, positionDeltasSIMD, (int)simBodyList.size(), anchoredBodyList, solverConfig);

    initAnchoredBodiesProfiler.end();

    serializer.serializeForces(simBodyList, bodyStaticData.size());
    serializer.tag("Bodies");
    serializer & simBodyPairs & bodyVariableData & bodyStaticData & massAndInertia;

    //
    // Build constraint equations: setup the Jacobians and the reaction vectors, restore cached solutions
    //
    buildEquationsProfiler.start();
    ArrayDynamic< ConstraintJacobianPair > jacobians( totalDimension, ArrayNoInit(), defaultAlignment );
    ArrayDynamic< ConstraintVariables > velocityStage( totalDimension, ArrayNoInit(), defaultAlignment );
    ArrayDynamic< ConstraintVariables > positionStage( totalDimension, ArrayNoInit(), defaultAlignment );
    ArrayDynamic< float > sorVel( totalDimension, ArrayNoInit(), defaultAlignment );
    ArrayDynamic< float > sorPos( totalDimension, ArrayNoInit(), defaultAlignment );
    ArrayDynamic< boost::uint8_t > useBlock( totalDimension, ArrayNoInit() );
    {
        RBXPROFILER_SCOPE("Physics", "buildConstraintEquations");

        for( size_t i = 0; i < constraints.size(); i++ )
        {
            const auto& bodyA = bodyVariableData[ simBodyPairs[i].first ];
            const auto& bodyB = bodyVariableData[ simBodyPairs[i].second ];
            boost::uint32_t currentOffset = offsets[ i ];
            constraints[ i ]->restoreCacheAndBuildEquation( 
                jacobians.data() + currentOffset, 
                velocityStage.data() + currentOffset, 
                positionStage.data() + currentOffset,
                sorVel.data() + currentOffset,
                sorPos.data() + currentOffset,
                useBlock.data() + currentOffset,
                bodyA, bodyB, solverConfig, _dt );
        }
    }
    buildEquationsProfiler.end();

    serializer.tag("Variables");
    serializer & velocityStage & positionStage;

    //
    // Compute effective masses: M^-1 * J^t
    //
    computeEffectiveMassesProfiler.start();
    ArrayDynamic< EffectiveMassPair > effectiveMassesVel( totalDimension, ArrayNoInit(), defaultAlignment );
    ArrayDynamic< EffectiveMassPair > effectiveMassesPos( totalDimension, ArrayNoInit(), defaultAlignment );
    PGSComputeEffectiveMasses(effectiveMassesVel.data(), effectiveMassesPos.data(), constraints.size(), dimensions.data(), jacobians.data(), simBodyPairs.data(), massAndInertia.data(), solverConfig);
    computeEffectiveMassesProfiler.end();

    //
    // Pre-condition the Jacobians and reaction vectors
    //
    preconditioningProfiler.start();
    ArrayDynamic< ConstraintJacobianPair > preconditionedJacobiansVelStage( totalDimension, ArrayNoInit(), defaultAlignment );
    ArrayDynamic< ConstraintJacobianPair > preconditionedJacobiansPosStage( totalDimension, ArrayNoInit(), defaultAlignment );
    PGSPreconditionConstraintEquations(preconditionedJacobiansVelStage.data(), preconditionedJacobiansPosStage.data(), 
        velocityStage.data(), positionStage.data(), constraints.size(), dimensions.data(), useBlock.data(), 
        sorVel.data(), sorPos.data(), jacobians.data(), effectiveMassesVel.data(), effectiveMassesPos.data() );
    preconditioningProfiler.end();

    //
    // Collapse the masses of sleeping simulated objects to infinity
    // This has to be done after pre-conditioning
    //
    multiplyEffectiveMassMultipliersProfiler.start();
    PGSApplyEffectiveMassMultipliers( effectiveMassesVel.data(), effectiveMassesPos.data(), constraints.size(), dimensions.data(), effectiveMassMultipliers.data(),
        simBodyPairs.data(), solverConfig );
    multiplyEffectiveMassMultipliersProfiler.end();

    serializer.tag("Jacobians");
    serializer & jacobians & preconditionedJacobiansVelStage & preconditionedJacobiansPosStage & effectiveMassesVel & effectiveMassesPos;

    //
    // Initialize the virtual displacements using previously computed impulses (cached)
    //
    initVirDProfiler.start();
    PGSInitVirtualDisplacements( velocityDeltasSIMD, positionDeltasSIMD, effectiveMassesVel.data(), effectiveMassesPos.data(), constraints.size(), dimensions.data(), velocityStage.data(), positionStage.data(), simBodyPairs.data(), solverConfig );
    initVirDProfiler.end();

    //
    // Run the kernel
    //
    kernelProfiler.start();
    PGSSolveKernel( 
        velocityStage.data(), positionStage.data(),
        velocityDeltasSIMD, positionDeltasSIMD,
        constraints.size(), collisionCount,
        dimensions.data(), simBodyPairs.data(),
        preconditionedJacobiansVelStage.data(), preconditionedJacobiansPosStage.data(),
        effectiveMassesVel.data(), effectiveMassesPos.data(), solverConfig );
    kernelProfiler.end();

    serializer.tag("Results");
    serializer.serializeComputedImpulse(velocityStage, positionStage);
    serializer & velocityDeltasSIMD & positionDeltasSIMD;

    //
    // Write out the constraint cache
    //
    writeCacheProfiler.start();
    {
        RBXPROFILER_SCOPE("Physics", "updateConstraintCache");

        for( size_t i = 0; i < constraints.size(); i++ )
        {
            auto currentOffset = offsets[ i ];
            constraints[ i ]->updateBrokenState( velocityStage.data() + currentOffset, positionStage.data() + currentOffset, solverConfig );
            constraints[ i ]->cache(velocityStage.data() + currentOffset, positionStage.data() + currentOffset, sorVel.data() + currentOffset, sorPos.data() + currentOffset, solverConfig);
        }
    }
    writeCacheProfiler.end();

    //
    // Integrate positions and write out into SimBodies
    //
    integratePositionsProfiler.start();
    integratePositionsAndUpdateSimBodies( simBodyList.data(), bodyVariableData.data(), bodyStaticData.data(), simBodyList.size(), velocityDeltasSIMD, positionDeltasSIMD, _dt );
    integratePositionsProfiler.end();

    serializer.tag("Integrated");
    serializer & bodyVariableData;

    serializer.tag("Cache");
    serializer.serializeBodyCache(bodyCache);

    solverProfiler.end();

    // Print profiling data
    gatherCollisionsProfiler.printStats();
    integrateVelocitiesProfiler.printStats();
    initAnchoredBodiesProfiler.printStats();
    buildEquationsProfiler.printStats();
    computeEffectiveMassesProfiler.printStats();
    preconditioningProfiler.printStats();
    multiplyEffectiveMassMultipliersProfiler.printStats();
    initVirDProfiler.printStats();
    kernelProfiler.printStats();
    writeCacheProfiler.printStats();
    integratePositionsProfiler.printStats();
    solverProfiler.printStats();
}

void PGSSolver::addSimBody( SimBody* _body, bool _highPriority )
{
    if( simBodies.find( _body ) == simBodies.end() )
    {
        simBodies.insert( _body );
    }

    if( _highPriority && highPrioritySimBodies.find( _body ) == highPrioritySimBodies.end() )
    {
        highPrioritySimBodies.insert( _body );
    }
}

void PGSSolver::removeSimBody( SimBody* _body )
{
    auto it = simBodies.find( _body );
    if( it != simBodies.end() )
    {
        simBodies.erase(it);
    }

    it = highPrioritySimBodies.find( _body );
    if( it != highPrioritySimBodies.end() )
    {
        highPrioritySimBodies.erase( it );
    }
}

void PGSSolver::addConstraint( Constraint* _constraint )
{
    // The engine shouldn't explicitly add collisions.
    RBXASSERT( _constraint->getType() != Constraint::Types_Collision );
    if( _constraint->hasValidUID() && pureConstraintSet.find( _constraint->getUID() ) == pureConstraintSet.end() )
    {
        // If the constraint was previously registered with the solver, use the same index.
        // This is to preserve the same ordering of constraints as they are registered and unregistered
        // The solver stability depends on maintaining this order
        pureConstraintSet[ _constraint->getUID() ] = _constraint;
    }
    else
    {
        // We need a new index
        constraintUIDGenerator++;
        _constraint->setUID( constraintUIDGenerator );
        pureConstraintSet[ constraintUIDGenerator ] = _constraint;
    }
}

void PGSSolver::removeConstraint( Constraint* _constraint )
{
    auto it = pureConstraintSet.find( _constraint->getUID() );
    if( it != pureConstraintSet.end() )
    {
        pureConstraintSet.erase( it );
    }
}

ContactManifold::~ContactManifold()
{
    for( size_t i = 0; i < collisions.size(); i++ )
    {
        delete collisions[ i ];
    }
}

void PGSSolver::addContactManifold( boost::uint64_t _uidA, boost::uint64_t _uidB )
{
    BodyUIDPair pair( _uidA, _uidB );
    if( pair.first > pair.second )
    {
        std::swap( pair.first, pair.second );
    }

    auto it = contactManifolds.find( pair );
    if( it == contactManifolds.end() )
    {
        ContactManifold* contactManifold = new ContactManifold();
        contactManifold->referenceCount++;
        contactManifolds[ pair ] = contactManifold;
    }
    else
    {
        it->second->referenceCount++;
    }
}

void PGSSolver::removeContactManifold( boost::uint64_t _uidA, boost::uint64_t _uidB )
{
    BodyUIDPair pair( _uidA, _uidB );
    if( pair.first > pair.second )
    {
        std::swap( pair.first, pair.second );
    }

    auto it = contactManifolds.find( pair );
    RBXASSERT( it != contactManifolds.end() );
    if( it != contactManifolds.end() )
    {
        it->second->referenceCount--;
        if( it->second->referenceCount == 0 )
        {
            delete it->second;
            contactManifolds.erase( it );
        }
    }
}

void PGSSolver::clearBodyCache( boost::uint64_t _uid )
{
    auto it = bodyCache.find( _uid );
    if( it != bodyCache.end() )
    {
        bodyCache.erase( it );
    }
}

static RBX_SIMD_INLINE void updateCollision( ConstraintCollision& _collision, const OrderedConnector& _inputConnector )
{
    ContactConnector* connector = _inputConnector.connector;
    const PairParams& params = _inputConnector.connector->getContactPoint();

    _collision.setFriction( connector->getContactParams().kFriction );
    _collision.setResititution( connector->getContactParams().kElasticity );
    _collision.setDepth( params.length );

    RBXASSERT_VERY_FAST( !RBX::Math::isNanInf( params.normal.length() ) && std::abs( 1.0f - params.normal.length() ) < 0.1f );
    if( !_inputConnector.swap )
    {
        _collision.setBodyA( connector->getBody( Connector::body0 ) );
        _collision.setBodyB( connector->getBody( Connector::body1 ) );
        _collision.setPointA( params.position );
        _collision.setNormal( params.normal );
    }
    else
    {
        _collision.setBodyB( connector->getBody( Connector::body0 ) );
        _collision.setBodyA( connector->getBody( Connector::body1 ) );
        _collision.setPointA( params.position + params.length * params.normal );
        _collision.setNormal( -params.normal );
    }
}

ContactManifold* PGSSolver::updateContactManifold( const BodyUIDPair& _pairId, const ArrayBase< OrderedConnector >& _inputManifold )
{
    // Filter out the bad connectors 
    ArrayDynamic< OrderedConnector > cleanManifold;
    cleanManifold.reserve( _inputManifold.size() );
    for( auto c : _inputManifold )
    {
        if( c.connector->getContactPoint().normal.squaredLength() > 0.8f )
        {
            cleanManifold.push_back( c );
        }
    }

    if( cleanManifold.size() == 0 )
    {
        return 0;
    }

    // Find the contact manifold in the cache
    auto cachedManifoldIt = contactManifolds.find( _pairId );

    // It should find it, but fail safe in case of bugs
    RBXASSERT( cachedManifoldIt != contactManifolds.end() );
    if( cachedManifoldIt != contactManifolds.end() )
    {
        // Match the collisions between the two manifolds
        size_t newContactIndex = 0;
        size_t cachedContactIndex = 0;
        auto& collisions = cachedManifoldIt->second->collisions;

        // Match the collisions simply using their order in the manifold
        for( ; cachedContactIndex < collisions.size(); cachedContactIndex++, newContactIndex++ )
        {
            if( newContactIndex == cleanManifold.size() )
            {
                break;
            }

            updateCollision( *collisions[ cachedContactIndex ], cleanManifold[ newContactIndex ] );
        }

        // Add the new collisions
        for( ; newContactIndex < cleanManifold.size(); newContactIndex++ )
        {
            ConstraintCollision* collision = new ConstraintCollision( 
                cleanManifold[ newContactIndex ].connector->getBody( Connector::body0 ),
                cleanManifold[ newContactIndex ].connector->getBody( Connector::body1 ) );
            constraintUIDGenerator++;
            collision->setUID(constraintUIDGenerator);
            updateCollision( *collision, cleanManifold[ newContactIndex ] );
            collisions.push_back( collision );
        }

        // Remove old collisions
        if( cleanManifold.size() < collisions.size() )
        {
            collisions.resize( cleanManifold.size() );
        }

        return cachedManifoldIt->second;
    }
    return NULL;
}

class OrderedConnectorPair: public std::pair< BodyUIDPair, OrderedConnector >
{
public:
    OrderedConnectorPair( const BodyUIDPair& p, const OrderedConnector& c ): std::pair< BodyUIDPair, OrderedConnector >( p, c ) { }
    inline bool operator<( const OrderedConnectorPair& a ) const
    {
        return first < a.first;
    }
};

size_t PGSSolver::addContactConnectors( ArrayDynamic< ContactManifold* >& _activeManifolds, 
                                       const std::vector< ContactConnector* >& _connectors,
                                       const boost::unordered_set< SimBody* >& _simBodies )
{
    RBXPROFILER_SCOPE("Physics", "addContactConnectors");

    // Ensure the collisions are always in the same order.
    // That also makes the solver more stable for static situations as it reaches more or less the same solution every frame
    ArrayDynamic< OrderedConnectorPair > orderedConnectors;
    orderedConnectors.reserve( _connectors.size() );
    for( size_t j = 0; j < _connectors.size(); j++ )
    {
        Body* _bodyA = _connectors[ j ]->getBody( Connector::body0 );
        Body* _bodyB = _connectors[ j ]->getBody( Connector::body1 );

        // If both bodies are not simulated, drop the connector.
        // This seemed to be happening, must track down why.
        if( _simBodies.find( _bodyA->getRootSimBody() ) == _simBodies.end() && _simBodies.find( _bodyB->getRootSimBody() ) == _simBodies.end() )
        {
            continue;
        }

        OrderedConnector orderedContact;
        orderedContact.connector = _connectors[ j ];
        orderedContact.swap = false;

        BodyUIDPair pair( _bodyA->getUID(), _bodyB->getUID() );
        if( pair.first > pair.second )
        {
            orderedContact.swap = true;
            std::swap( pair.first, pair.second );
        }
        orderedConnectors.push_back( OrderedConnectorPair( pair, orderedContact ) );
    }

    // Sort the array of connectors by body pair
    std::stable_sort( orderedConnectors.begin(), orderedConnectors.end() );

    size_t collisionCount = 0;

    BodyUIDPair pair( 0, 0 );
    ArrayDynamic< OrderedConnector > manifold;
    for (size_t j = 0; j < orderedConnectors.size(); ++j) 
    {
        if( pair != orderedConnectors[ j ].first )
        {
            if( manifold.size() > 0 )
            {
                ContactManifold* m = updateContactManifold( pair, manifold );
                if( m != NULL )
                {
                    collisionCount += m->collisions.size();
                    _activeManifolds.push_back( m );
                }
            }
            manifold.clear();
            pair = orderedConnectors[ j ].first;
        }
        manifold.push_back( orderedConnectors[ j ].second );
    }
    if( manifold.size() > 0 )
    {
        ContactManifold* m = updateContactManifold( pair, manifold );
        if( m != NULL )
        {
            collisionCount += m->collisions.size();
            _activeManifolds.push_back( m );
        }
    }

    return collisionCount;
}

void PGSSolver::dumpLog( bool enable )
{
    if( FFlag::PGSSolverFileDump )
    {
        dumpLogSwitch = enable;
    }
}

void PGSSolver::SolverBodyCache::serialize( DebugSerializer& s ) const
{
    // simBodyDebug->updateIfDirty();
    const CoordinateFrame& c = simBodyDebug->getPV().position;
    s & c.translation;
    s & c.rotation.row(0);
    s & c.rotation.row(1);
    s & c.rotation.row(2);
    s & simBodyDebug->getMassRecip();
}

}