1 files changed, 1571 insertions, 1571 deletions

diff --git a/sdk/extensions/stress/source/NvBlastExtStressSolver.cpp b/sdk/extensions/stress/source/NvBlastExtStressSolver.cpp
index e4aea6b..c8918b6 100644..100755
--- a/sdk/extensions/stress/source/NvBlastExtStressSolver.cpp
+++ b/sdk/extensions/stress/source/NvBlastExtStressSolver.cpp
@@ -1,1571 +1,1571 @@
-// This code contains NVIDIA Confidential Information and is disclosed to you
-// under a form of NVIDIA software license agreement provided separately to you.
-//
-// Notice
-// NVIDIA Corporation and its licensors retain all intellectual property and
-// proprietary rights in and to this software and related documentation and
-// any modifications thereto. Any use, reproduction, disclosure, or
-// distribution of this software and related documentation without an express
-// license agreement from NVIDIA Corporation is strictly prohibited.
-//
-// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
-// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
-// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
-// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
-//
-// Information and code furnished is believed to be accurate and reliable.
-// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
-// information or for any infringement of patents or other rights of third parties that may
-// result from its use. No license is granted by implication or otherwise under any patent
-// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
-// This code supersedes and replaces all information previously supplied.
-// NVIDIA Corporation products are not authorized for use as critical
-// components in life support devices or systems without express written approval of
-// NVIDIA Corporation.
-//
-// Copyright (c) 2016-2018 NVIDIA Corporation. All rights reserved.
-
-
-#include "NvBlastExtStressSolver.h"
-#include "NvBlast.h"
-#include "NvBlastGlobals.h"
-#include "NvBlastArray.h"
-#include "NvBlastHashMap.h"
-#include "NvBlastHashSet.h"
-#include "NvBlastAssert.h"
-#include "NvBlastIndexFns.h"
-
-#include <PsVecMath.h>
-#include "PsFPU.h"
-
-#include <algorithm>
-
-#define USE_SCALAR_IMPL 0
-#define WARM_START 1
-#define GRAPH_INTERGRIRY_CHECK 0
-
-#if GRAPH_INTERGRIRY_CHECK
-#include <set>
-#endif
-
-
-namespace Nv
-{
-namespace Blast
-{
-
-using namespace physx;
-
-
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-//													 Solver
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-class SequentialImpulseSolver
-{
-public:
-	PX_ALIGN_PREFIX(16)
-	struct BondData
-	{
-		physx::PxVec3 impulseLinear;
-		uint32_t node0;
-		physx::PxVec3 impulseAngular;
-		uint32_t node1;
-		physx::PxVec3 offset0;
-		float invOffsetSqrLength;
-	}
-	PX_ALIGN_SUFFIX(16);
-
-	PX_ALIGN_PREFIX(16)
-	struct NodeData
-	{
-		physx::PxVec3 velocityLinear;
-		float invI;
-		physx::PxVec3 velocityAngular;
-		float invMass;
-	}
-	PX_ALIGN_SUFFIX(16);
-
-	SequentialImpulseSolver(uint32_t nodeCount, uint32_t maxBondCount)
-	{
-		m_nodesData.resize(nodeCount);
-		m_bondsData.reserve(maxBondCount);
-	}
-
-	const NodeData& getNodeData(uint32_t node) const
-	{
-		return m_nodesData[node];
-	}
-
-	const BondData& getBondData(uint32_t bond) const
-	{
-		return m_bondsData[bond];
-	}
-
-	uint32_t getBondCount() const
-	{
-		return m_bondsData.size();
-	}
-
-	uint32_t getNodeCount() const
-	{
-		return m_nodesData.size();;
-	}
-
-	void setNodeMassInfo(uint32_t node, float invMass, float invI)
-	{
-		m_nodesData[node].invMass = invMass;
-		m_nodesData[node].invI = invI;
-	}
-
-	void initialize()
-	{
-		for (auto& node : m_nodesData)
-		{
-			node.velocityLinear = PxVec3(PxZero);
-			node.velocityAngular = PxVec3(PxZero);
-		}
-	}
-
-	void setNodeVelocities(uint32_t node, const PxVec3& velocityLinear, const PxVec3& velocityAngular)
-	{
-		m_nodesData[node].velocityLinear = velocityLinear;
-		m_nodesData[node].velocityAngular = velocityAngular;
-	}
-
-	uint32_t addBond(uint32_t node0, uint32_t node1, const PxVec3& offset)
-	{
-		const BondData data = {
-			PxVec3(PxZero), 
-			node0, 
-			PxVec3(PxZero), 
-			node1, 
-			offset, 
-			1.0f / offset.magnitudeSquared() 
-		};
-		m_bondsData.pushBack(data);
-		return m_bondsData.size() - 1;
-	}
-
-	void replaceWithLast(uint32_t bondIndex)
-	{
-		m_bondsData.replaceWithLast(bondIndex);
-	}
-
-	void reset(uint32_t nodeCount)
-	{
-		m_bondsData.clear();
-		m_nodesData.resize(nodeCount);
-	}
-
-	void clearBonds()
-	{
-		m_bondsData.clear();
-	}
-
-	void solve(uint32_t iterationCount, bool warmStart = true)
-	{
-		solveInit(warmStart);
-
-		for (uint32_t i = 0; i < iterationCount; ++i)
-		{
-			iterate();
-		}
-	}
-
-	void calcError(float& linear, float& angular)
-	{
-		linear = 0.0f;
-		angular = 0.0f;
-		for (BondData& bond : m_bondsData)
-		{
-			NodeData* node0 = &m_nodesData[bond.node0];
-			NodeData* node1 = &m_nodesData[bond.node1];
-
-			const PxVec3 vA = node0->velocityLinear - node0->velocityAngular.cross(bond.offset0);
-			const PxVec3 vB = node1->velocityLinear + node1->velocityAngular.cross(bond.offset0);
-
-			const PxVec3 vErrorLinear = vA - vB;
-			const PxVec3 vErrorAngular = node0->velocityAngular - node1->velocityAngular;
-
-			linear += vErrorLinear.magnitude();
-			angular += vErrorAngular.magnitude();
-		}
-	}
-
-private:
-	void solveInit(bool warmStart = false)
-	{
-		if (warmStart)
-		{
-			for (BondData& bond : m_bondsData)
-			{
-				NodeData* node0 = &m_nodesData[bond.node0];
-				NodeData* node1 = &m_nodesData[bond.node1];
-
-				const PxVec3 velocityLinearCorr0 = bond.impulseLinear * node0->invMass;
-				const PxVec3 velocityLinearCorr1 = bond.impulseLinear * node1->invMass;
-
-				const PxVec3 velocityAngularCorr0 = bond.impulseAngular * node0->invI - bond.offset0.cross(velocityLinearCorr0) * bond.invOffsetSqrLength;
-				const PxVec3 velocityAngularCorr1 = bond.impulseAngular * node1->invI + bond.offset0.cross(velocityLinearCorr1) * bond.invOffsetSqrLength;
-
-				node0->velocityLinear += velocityLinearCorr0;
-				node1->velocityLinear -= velocityLinearCorr1;
-
-				node0->velocityAngular += velocityAngularCorr0;
-				node1->velocityAngular -= velocityAngularCorr1;
-			}
-		}
-		else
-		{
-			for (BondData& bond : m_bondsData)
-			{
-				bond.impulseLinear = PxVec3(PxZero);
-				bond.impulseAngular = PxVec3(PxZero);
-			}
-		}
-	}
-
-
-	void iterate()
-	{
-		using namespace physx::shdfnd::aos;
-
-		for (BondData& bond : m_bondsData)
-		{
-			NodeData* node0 = &m_nodesData[bond.node0];
-			NodeData* node1 = &m_nodesData[bond.node1];
-
-#if USE_SCALAR_IMPL
-			const PxVec3 vA = node0->velocityLinear - node0->velocityAngular.cross(bond.offset0);
-			const PxVec3 vB = node1->velocityLinear + node1->velocityAngular.cross(bond.offset0);
-
-			const PxVec3 vErrorLinear = vA - vB;
-			const PxVec3 vErrorAngular = node0->velocityAngular - node1->velocityAngular;
-
-			const float weightedMass = 1.0f / (node0->invMass + node1->invMass);
-			const float weightedInertia = 1.0f / (node0->invI + node1->invI);
-
-			const PxVec3 outImpulseLinear = -vErrorLinear * weightedMass * 0.5f;
-			const PxVec3 outImpulseAngular = -vErrorAngular * weightedInertia * 0.5f;
-
-			bond.impulseLinear += outImpulseLinear;
-			bond.impulseAngular += outImpulseAngular;
-
-			const PxVec3 velocityLinearCorr0 = outImpulseLinear * node0->invMass;
-			const PxVec3 velocityLinearCorr1 = outImpulseLinear * node1->invMass;
-
-			const PxVec3 velocityAngularCorr0 = outImpulseAngular * node0->invI - bond.offset0.cross(velocityLinearCorr0) * bond.invOffsetSqrLength;
-			const PxVec3 velocityAngularCorr1 = outImpulseAngular * node1->invI + bond.offset0.cross(velocityLinearCorr1) * bond.invOffsetSqrLength;
-
-			node0->velocityLinear += velocityLinearCorr0;
-			node1->velocityLinear -= velocityLinearCorr1;
-
-			node0->velocityAngular += velocityAngularCorr0;
-			node1->velocityAngular -= velocityAngularCorr1;
-#else
-			const Vec3V velocityLinear0 = V3LoadUnsafeA(node0->velocityLinear);
-			const Vec3V velocityLinear1 = V3LoadUnsafeA(node1->velocityLinear);
-			const Vec3V velocityAngular0 = V3LoadUnsafeA(node0->velocityAngular);
-			const Vec3V velocityAngular1 = V3LoadUnsafeA(node1->velocityAngular);
-
-			const Vec3V offset = V3LoadUnsafeA(bond.offset0);
-			const Vec3V vA = V3Add(velocityLinear0, V3Neg(V3Cross(velocityAngular0, offset)));
-			const Vec3V vB = V3Add(velocityLinear1, V3Cross(velocityAngular1, offset));
-
-			const Vec3V vErrorLinear = V3Sub(vA, vB);
-			const Vec3V vErrorAngular = V3Sub(velocityAngular0, velocityAngular1);
-
-			const FloatV invM0 = FLoad(node0->invMass);
-			const FloatV invM1 = FLoad(node1->invMass);
-			const FloatV invI0 = FLoad(node0->invI);
-			const FloatV invI1 = FLoad(node1->invI);
-			const FloatV invOffsetSqrLength = FLoad(bond.invOffsetSqrLength);
-
-			const FloatV weightedMass = FLoad(-0.5f / (node0->invMass + node1->invMass));
-			const FloatV weightedInertia = FLoad(-0.5f / (node0->invI + node1->invI));
-
-			const Vec3V outImpulseLinear = V3Scale(vErrorLinear, weightedMass);
-			const Vec3V outImpulseAngular = V3Scale(vErrorAngular, weightedInertia);
-
-			V3StoreA(V3Add(V3LoadUnsafeA(bond.impulseLinear), outImpulseLinear), bond.impulseLinear);
-			V3StoreA(V3Add(V3LoadUnsafeA(bond.impulseAngular), outImpulseAngular), bond.impulseAngular);
-
-			const Vec3V velocityLinearCorr0 = V3Scale(outImpulseLinear, invM0);
-			const Vec3V velocityLinearCorr1 = V3Scale(outImpulseLinear, invM1);
-
-			const Vec3V velocityAngularCorr0 = V3Sub(V3Scale(outImpulseAngular, invI0), V3Scale(V3Cross(offset, velocityLinearCorr0), invOffsetSqrLength));
-			const Vec3V velocityAngularCorr1 = V3Add(V3Scale(outImpulseAngular, invI1),	V3Scale(V3Cross(offset, velocityLinearCorr1), invOffsetSqrLength));
-
-			V3StoreA(V3Add(velocityLinear0, velocityLinearCorr0), node0->velocityLinear);
-			V3StoreA(V3Sub(velocityLinear1, velocityLinearCorr1), node1->velocityLinear);
-
-			V3StoreA(V3Add(velocityAngular0, velocityAngularCorr0), node0->velocityAngular);
-			V3StoreA(V3Sub(velocityAngular1, velocityAngularCorr1), node1->velocityAngular);
-#endif
-		}
-	}
-
-	Array<BondData>::type		m_bondsData;
-	Array<NodeData>::type		m_nodesData;
-};
-
-
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-//													 Graph Processor
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-#if GRAPH_INTERGRIRY_CHECK
-#define CHECK_GRAPH_INTEGRITY checkGraphIntegrity()
-#else
-#define CHECK_GRAPH_INTEGRITY ((void)0)
-#endif
-
-class SupportGraphProcessor
-{
-
-public:
-	struct BondData
-	{
-		uint32_t node0;
-		uint32_t node1;
-		uint32_t blastBondIndex;
-		float	 stress;
-	};
-
-	struct NodeData
-	{
-		float mass;
-		float volume;
-		PxVec3 localPos;
-		bool isStatic;
-		uint32_t solverNode;
-		uint32_t neighborsCount;
-		PxVec3 impulse;
-	};
-
-	struct SolverNodeData
-	{
-		uint32_t supportNodesCount;
-		PxVec3 localPos;
-		union
-		{
-			float mass;
-			int32_t indexShift;
-		};
-		float volume;
-		bool isStatic;
-	};
-
-	struct SolverBondData
-	{
-		InlineArray<uint32_t, 8>::type blastBondIndices;
-	};
-
-	SupportGraphProcessor(uint32_t nodeCount, uint32_t maxBondCount) : m_solver(nodeCount, maxBondCount), m_nodesDirty(true)
-	{
-		m_nodesData.resize(nodeCount);
-		m_bondsData.reserve(maxBondCount);
-
-		m_solverNodesData.resize(nodeCount);
-		m_solverBondsData.reserve(maxBondCount);
-
-		m_solverBondsMap.reserve(maxBondCount);
-
-		m_blastBondIndexMap.resize(maxBondCount);
-		memset(m_blastBondIndexMap.begin(), 0xFF, m_blastBondIndexMap.size() * sizeof(uint32_t));
-
-		resetImpulses();
-	}
-
-	const NodeData& getNodeData(uint32_t node) const
-	{
-		return m_nodesData[node];
-	}
-
-	const BondData& getBondData(uint32_t bond) const
-	{
-		return m_bondsData[bond];
-	}
-
-	const SolverNodeData& getSolverNodeData(uint32_t node) const
-	{
-		return m_solverNodesData[node];
-	}
-
-	const SolverBondData& getSolverBondData(uint32_t bond) const
-	{
-		return m_solverBondsData[bond];
-	}
-
-	const SequentialImpulseSolver::BondData& getSolverInternalBondData(uint32_t bond) const
-	{
-		return m_solver.getBondData(bond);
-	}
-
-	const SequentialImpulseSolver::NodeData& getSolverInternalNodeData(uint32_t node) const
-	{
-		return m_solver.getNodeData(node);
-	}
-
-	uint32_t getBondCount() const
-	{
-		return m_bondsData.size();
-	}
-
-	uint32_t getNodeCount() const
-	{
-		return m_nodesData.size();;
-	}
-
-	uint32_t getSolverBondCount() const
-	{
-		return m_solverBondsData.size();
-	}
-
-	uint32_t getSolverNodeCount() const
-	{
-		return m_solverNodesData.size();;
-	}
-
-	uint32_t getOverstressedBondCount() const
-	{
-		return m_overstressedBondCount;
-	}
-
-	float getSolverBondStressHealth(uint32_t bond, const ExtStressSolverSettings& settings) const
-	{
-		const auto& solverBond = getSolverInternalBondData(bond);
-		const float impulse = solverBond.impulseLinear.magnitude() * settings.stressLinearFactor + solverBond.impulseAngular.magnitude() * settings.stressAngularFactor;
-		// We then divide uniformly across bonds, which is obviously rough estimate.
-		// Potentially we can add bond area there and norm across area sum
-		const auto& blastBondIndices = m_solverBondsData[bond].blastBondIndices;
-		return blastBondIndices.empty() ? 0.0f : impulse / (blastBondIndices.size() * settings.hardness);
-	}
-
-	void setNodeInfo(uint32_t node, float mass, float volume, PxVec3 localPos, bool isStatic)
-	{
-		m_nodesData[node].mass = mass;
-		m_nodesData[node].volume = volume;
-		m_nodesData[node].localPos = localPos;
-		m_nodesData[node].isStatic = isStatic;
-		m_nodesDirty = true;
-	}
-
-	void setNodeNeighborsCount(uint32_t node, uint32_t neighborsCount)
-	{
-		// neighbors count is expected to be the number of nodes on 1 island/actor.
-		m_nodesData[node].neighborsCount = neighborsCount;
-
-		// check for too huge aggregates (happens after island's split)
-		if (!m_nodesDirty)
-		{
-			m_nodesDirty |= (m_solverNodesData[m_nodesData[node].solverNode].supportNodesCount > neighborsCount / 2);
-		}
-	}
-
-	void addNodeForce(uint32_t node, const PxVec3& force, ExtForceMode::Enum mode)
-	{
-		const PxVec3 impuse = (mode == ExtForceMode::IMPULSE) ? force : force * m_nodesData[node].mass;
-		m_nodesData[node].impulse += impuse;
-	}
-
-	void addNodeVelocity(uint32_t node, const PxVec3& velocity)
-	{
-		addNodeForce(node, velocity, ExtForceMode::VELOCITY);
-	}
-
-	void addNodeImpulse(uint32_t node, const PxVec3& impulse)
-	{
-		addNodeForce(node, impulse, ExtForceMode::IMPULSE);
-	}
-
-	void addBond(uint32_t node0, uint32_t node1, uint32_t blastBondIndex)
-	{
-		if (isInvalidIndex(m_blastBondIndexMap[blastBondIndex]))
-		{
-			const BondData data = {
-				node0,
-				node1,
-				blastBondIndex,
-				0.0f
-			};
-			m_bondsData.pushBack(data);
-			m_blastBondIndexMap[blastBondIndex] = m_bondsData.size() - 1;
-		}
-	}
-
-	void removeBondIfExists(uint32_t blastBondIndex)
-	{
-		const uint32_t bondIndex = m_blastBondIndexMap[blastBondIndex];
-
-		if (!isInvalidIndex(bondIndex))
-		{
-			const BondData& bond = m_bondsData[bondIndex];
-			const uint32_t solverNode0 = m_nodesData[bond.node0].solverNode;
-			const uint32_t solverNode1 = m_nodesData[bond.node1].solverNode;
-			bool isBondInternal = (solverNode0 == solverNode1);
-
-			if (isBondInternal)
-			{
-				// internal bond sadly requires graph resync (it never happens on reduction level '0')
-				m_nodesDirty = true;
-			}
-			else if (!m_nodesDirty)
-			{
-				// otherwise it's external bond, we can remove it manually and keep graph synced
-				// we don't need to spend time there if (m_nodesDirty == true), graph will be resynced anyways
-
-				BondKey solverBondKey(solverNode0, solverNode1);
-				auto entry = m_solverBondsMap.find(solverBondKey);
-				if (entry)
-				{
-					const uint32_t solverBondIndex = entry->second;
-					auto& blastBondIndices = m_solverBondsData[solverBondIndex].blastBondIndices;
-					blastBondIndices.findAndReplaceWithLast(blastBondIndex);
-					if (blastBondIndices.empty())
-					{
-						// all bonds associated with this solver bond were removed, so let's remove solver bond
-
-						m_solverBondsData.replaceWithLast(solverBondIndex);
-						m_solver.replaceWithLast(solverBondIndex);
-						if (m_solver.getBondCount() > 0)
-						{
-							// update 'previously last' solver bond mapping
-							const auto& solverBond = m_solver.getBondData(solverBondIndex);
-							m_solverBondsMap[BondKey(solverBond.node0, solverBond.node1)] = solverBondIndex;
-						}
-
-						m_solverBondsMap.erase(solverBondKey);
-					}
-				}
-
-				CHECK_GRAPH_INTEGRITY;
-			}
-
-			// remove bond from graph processor's list
-			m_blastBondIndexMap[blastBondIndex] = invalidIndex<uint32_t>();
-			m_bondsData.replaceWithLast(bondIndex);
-			m_blastBondIndexMap[m_bondsData[bondIndex].blastBondIndex] = m_bondsData.size() > bondIndex ? bondIndex : invalidIndex<uint32_t>();
-		}
-	}
-
-	void setGraphReductionLevel(uint32_t level)
-	{
-		m_graphReductionLevel = level;
-		m_nodesDirty = true;
-	}
-
-	uint32_t getGraphReductionLevel() const
-	{
-		return m_graphReductionLevel;
-	}
-
-	void solve(const ExtStressSolverSettings& settings, const float* bondHealth, bool warmStart = true)
-	{
-		sync();
-
-		m_solver.initialize();
-
-		for (const NodeData& node : m_nodesData)
-		{
-			const SequentialImpulseSolver::NodeData& solverNode = m_solver.getNodeData(node.solverNode);
-			m_solver.setNodeVelocities(node.solverNode, solverNode.velocityLinear + node.impulse * solverNode.invMass, PxVec3(PxZero));
-		}
-
-		uint32_t iterationCount = ExtStressSolver::getIterationsPerFrame(settings, getSolverBondCount());
-		m_solver.solve(iterationCount, warmStart);
-
-		resetImpulses();
-
-		updateBondStress(settings, bondHealth);
-	}
-
-	void calcError(float& linear, float& angular)
-	{
-		m_solver.calcError(linear, angular);
-	}
-
-	float getBondStress(uint32_t blastBondIndex)
-	{
-		const uint32_t bondIndex = m_blastBondIndexMap[blastBondIndex];
-		return isInvalidIndex(bondIndex) ? 0.0f : m_bondsData[bondIndex].stress;
-	}
-
-private:
-
-	void resetImpulses()
-	{
-		for (auto& node : m_nodesData)
-		{
-			node.impulse = PxVec3(PxZero);
-		}
-	}
-
-	void updateBondStress(const ExtStressSolverSettings& settings, const float* bondHealth)
-	{
-		m_overstressedBondCount = 0;
-
-		for (uint32_t i = 0; i < m_solverBondsData.size(); ++i)
-		{
-			const float stress = getSolverBondStressHealth(i, settings);
-			const auto& blastBondIndices = m_solverBondsData[i].blastBondIndices;
-			const float stressPerBond = blastBondIndices.size() > 0 ? stress / blastBondIndices.size() : 0.0f;
-			for (auto blastBondIndex : blastBondIndices)
-			{
-				const uint32_t bondIndex = m_blastBondIndexMap[blastBondIndex];
-				if (!isInvalidIndex(bondIndex))
-				{
-					BondData& bond = m_bondsData[bondIndex];
-
-					NVBLAST_ASSERT(getNodeData(bond.node0).solverNode != getNodeData(bond.node1).solverNode);
-					NVBLAST_ASSERT(bond.blastBondIndex == blastBondIndex);
-					
-					bond.stress = stressPerBond;
-
-					if (stress > bondHealth[blastBondIndex])
-					{
-						m_overstressedBondCount++;
-					}
-				}
-			}
-		}
-	}
-
-	void sync()
-	{
-		if (m_nodesDirty)
-		{
-			syncNodes();
-		}
-		if (m_bondsDirty)
-		{
-			syncBonds();
-		}
-
-		CHECK_GRAPH_INTEGRITY;
-	}
-
-	void syncNodes()
-	{
-		// init with 1<->1 blast nodes to solver nodes mapping
-		m_solverNodesData.resize(m_nodesData.size());
-		for (uint32_t i = 0; i < m_nodesData.size(); ++i)
-		{
-			m_nodesData[i].solverNode = i;
-			m_solverNodesData[i].supportNodesCount = 1;
-			m_solverNodesData[i].indexShift = 0;
-		}
-
-		// for static nodes aggregate size per graph reduction level is lower, it
-		// falls behind on few levels. (can be made as parameter)
-		const uint32_t STATIC_NODES_COUNT_PENALTY = 2 << 2;
-
-		// reducing graph by aggregating nodes level by level
-		// NOTE (@anovoselov):  Recently, I found a flow in the algorithm below. In very rare situations aggregate (solver node)
-		// can contain more then one connected component. I didn't notice it to produce any visual artifacts and it's 
-		// unlikely to influence stress solvement a lot. Possible solution is to merge *whole* solver nodes, that
-		// will raise complexity a bit (at least will add another loop on nodes for every reduction level. 
-		for (uint32_t k = 0; k < m_graphReductionLevel; k++)
-		{
-			const uint32_t maxAggregateSize = 1 << (k + 1);
-
-			for (const BondData& bond : m_bondsData)
-			{
-				NodeData& node0 = m_nodesData[bond.node0];
-				NodeData& node1 = m_nodesData[bond.node1];
-
-				if (node0.isStatic != node1.isStatic)
-					continue;
-
-				if (node0.solverNode == node1.solverNode)
-					continue;
-
-				SolverNodeData& solverNode0 = m_solverNodesData[node0.solverNode];
-				SolverNodeData& solverNode1 = m_solverNodesData[node1.solverNode];
-				
-				const int countPenalty = node0.isStatic ? STATIC_NODES_COUNT_PENALTY : 1;
-				const uint32_t aggregateSize = std::min<uint32_t>(maxAggregateSize, node0.neighborsCount / 2);
-
-				if (solverNode0.supportNodesCount * countPenalty >= aggregateSize)
-					continue;
-				if (solverNode1.supportNodesCount * countPenalty >= aggregateSize)
-					continue;
-
-				if (solverNode0.supportNodesCount >= solverNode1.supportNodesCount)
-				{
-					solverNode1.supportNodesCount--;
-					solverNode0.supportNodesCount++;
-					node1.solverNode = node0.solverNode;
-				}
-				else if (solverNode1.supportNodesCount >= solverNode0.supportNodesCount)
-				{
-					solverNode1.supportNodesCount++;
-					solverNode0.supportNodesCount--;
-					node0.solverNode = node1.solverNode;
-				}
-			}
-		}
-
-		// Solver Nodes now sparse, a lot of empty ones. Rearrange them by moving all non-empty to the front
-		// 2 passes used for that
-		{
-			uint32_t currentNode = 0;
-			for (; currentNode < m_solverNodesData.size(); ++currentNode)
-			{
-				if (m_solverNodesData[currentNode].supportNodesCount > 0)
-					continue;
-
-				// 'currentNode' is free
-
-				// search next occupied node
-				uint32_t k = currentNode + 1;
-				for (; k < m_solverNodesData.size(); ++k)
-				{
-					if (m_solverNodesData[k].supportNodesCount > 0)
-					{
-						// replace currentNode and keep indexShift
-						m_solverNodesData[currentNode].supportNodesCount = m_solverNodesData[k].supportNodesCount;
-						m_solverNodesData[k].indexShift = k - currentNode;
-						m_solverNodesData[k].supportNodesCount = 0;
-						break;
-					}
-				}
-
-				if (k == m_solverNodesData.size())
-				{
-					break;
-				}
-			}
-			for (auto& node : m_nodesData)
-			{
-				node.solverNode -= m_solverNodesData[node.solverNode].indexShift;
-			}
-
-			// now, we know total solver nodes count and which nodes are aggregated into them
-			m_solverNodesData.resize(currentNode);
-		}
-
-
-		// calculate all needed data
-		for (SolverNodeData& solverNode : m_solverNodesData)
-		{
-			solverNode.supportNodesCount = 0;
-			solverNode.localPos = PxVec3(PxZero);
-			solverNode.mass = 0.0f;
-			solverNode.volume = 0.0f;
-			solverNode.isStatic = false;
-		}
-
-		for (NodeData& node : m_nodesData)
-		{
-			SolverNodeData& solverNode = m_solverNodesData[node.solverNode];
-			solverNode.supportNodesCount++;
-			solverNode.localPos += node.localPos;
-			solverNode.mass += node.mass;
-			solverNode.volume += node.volume;
-			solverNode.isStatic |= node.isStatic;
-		}
-
-		for (SolverNodeData& solverNode : m_solverNodesData)
-		{
-			solverNode.localPos /= (float)solverNode.supportNodesCount;
-		}
-
-		m_solver.reset(m_solverNodesData.size());
-		for (uint32_t nodeIndex = 0; nodeIndex < m_solverNodesData.size(); ++nodeIndex)
-		{
-			const SolverNodeData& solverNode = m_solverNodesData[nodeIndex];
-
-			const float invMass = solverNode.isStatic ? 0.0f : 1.0f / solverNode.mass;
-			const float R = PxPow(solverNode.volume * 3.0f * PxInvPi / 4.0f, 1.0f / 3.0f); // sphere volume approximation
-			const float invI = invMass / (R * R * 0.4f); // sphere inertia tensor approximation: I = 2/5 * M * R^2 ; invI = 1 / I;
-			m_solver.setNodeMassInfo(nodeIndex, invMass, invI);
-		}
-
-		m_nodesDirty = false;
-
-		syncBonds();
-	}
-
-	void syncBonds()
-	{
-		// traverse all blast bonds and aggregate
-		m_solver.clearBonds();
-		m_solverBondsMap.clear();
-		m_solverBondsData.clear();
-		for (BondData& bond : m_bondsData)
-		{
-			const NodeData& node0 = m_nodesData[bond.node0];
-			const NodeData& node1 = m_nodesData[bond.node1];
-
-			// reset stress, bond structure changed and internal bonds stress won't be updated during updateBondStress()
-			bond.stress = 0.0f;
-
-			if (node0.solverNode == node1.solverNode)
-				continue; // skip (internal)
-
-			if (node0.isStatic && node1.isStatic)
-				continue;
-
-			BondKey key(node0.solverNode, node1.solverNode);
-			auto entry = m_solverBondsMap.find(key);
-			SolverBondData* data;
-			if (!entry)
-			{
-				m_solverBondsData.pushBack(SolverBondData());
-				data = &m_solverBondsData.back();
-				m_solverBondsMap[key] = m_solverBondsData.size() - 1;
-
-				SolverNodeData& solverNode0 = m_solverNodesData[node0.solverNode];
-				SolverNodeData& solverNode1 = m_solverNodesData[node1.solverNode];
-				m_solver.addBond(node0.solverNode, node1.solverNode, (solverNode1.localPos - solverNode0.localPos) * 0.5f);
-			}
-			else
-			{
-				data = &m_solverBondsData[entry->second];
-			}
-			data->blastBondIndices.pushBack(bond.blastBondIndex);
-		}
-
-		m_bondsDirty = false;
-	}
-
-#if GRAPH_INTERGRIRY_CHECK
-	void checkGraphIntegrity()
-	{
-		NVBLAST_ASSERT(m_solver.getBondCount() == m_solverBondsData.size());
-		NVBLAST_ASSERT(m_solver.getNodeCount() == m_solverNodesData.size());
-
-		std::set<uint64_t> solverBonds;
-		for (uint32_t i = 0; i < m_solverBondsData.size(); ++i)
-		{
-			const auto& bondData = m_solver.getBondData(i);
-			BondKey key(bondData.node0, bondData.node1);
-			NVBLAST_ASSERT(solverBonds.find(key) == solverBonds.end());
-			solverBonds.emplace(key);
-			auto entry = m_solverBondsMap.find(key);
-			NVBLAST_ASSERT(entry != nullptr);
-			const auto& solverBond = m_solverBondsData[entry->second];
-			for (auto& blastBondIndex : solverBond.blastBondIndices)
-			{
-				if (!isInvalidIndex(m_blastBondIndexMap[blastBondIndex]))
-				{
-					auto& b = m_bondsData[m_blastBondIndexMap[blastBondIndex]];
-					BondKey key2(m_nodesData[b.node0].solverNode, m_nodesData[b.node1].solverNode);
-					NVBLAST_ASSERT(key2 == key);
-				}
-			}
-		}
-
-		for (auto& solverBond : m_solverBondsData)
-		{
-			for (auto& blastBondIndex : solverBond.blastBondIndices)
-			{
-				if (!isInvalidIndex(m_blastBondIndexMap[blastBondIndex]))
-				{
-					auto& b = m_bondsData[m_blastBondIndexMap[blastBondIndex]];
-					NVBLAST_ASSERT(m_nodesData[b.node0].solverNode != m_nodesData[b.node1].solverNode);
-				}
-			}
-		}
-		uint32_t mappedBondCount = 0;
-		for (uint32_t i = 0; i < m_blastBondIndexMap.size(); i++)
-		{
-			const auto& bondIndex = m_blastBondIndexMap[i];
-			if (!isInvalidIndex(bondIndex))
-			{
-				mappedBondCount++;
-				NVBLAST_ASSERT(m_bondsData[bondIndex].blastBondIndex == i);
-			}
-		}
-		NVBLAST_ASSERT(m_bondsData.size() == mappedBondCount);
-	}
-#endif
-
-	struct BondKey
-	{
-		uint32_t node0;
-		uint32_t node1;
-
-		BondKey(uint32_t n0, uint32_t n1)
-		{
-			node0 = n0 < n1 ? n0 : n1;
-			node1 = n0 < n1 ? n1 : n0;
-		}
-
-		operator uint64_t() const
-		{
-			return static_cast<uint64_t>(node0) + (static_cast<uint64_t>(node1) << 32);
-		}
-	};
-
-	SequentialImpulseSolver			    m_solver;
-	Array<SolverNodeData>::type			m_solverNodesData;
-	Array<SolverBondData>::type			m_solverBondsData;
-
-	uint32_t							m_graphReductionLevel;
-
-	bool	                            m_nodesDirty;
-	bool	                            m_bondsDirty;
-
-	uint32_t							m_overstressedBondCount;
-
-	HashMap<BondKey, uint32_t>::type	m_solverBondsMap;
-	Array<uint32_t>::type				m_blastBondIndexMap;
-
-	Array<BondData>::type				m_bondsData;
-	Array<NodeData>::type				m_nodesData;
-};
-
-
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-//											 ExtStressSolver
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-struct ExtStressNodeCachedData
-{
-	physx::PxVec3 localPos;
-	bool isStatic;
-};
-
-
-struct ExtStressBondCachedData
-{
-	uint32_t bondIndex;
-};
-
-/**
-*/
-class ExtStressSolverImpl final : public ExtStressSolver
-{
-	NV_NOCOPY(ExtStressSolverImpl)
-
-public:
-	ExtStressSolverImpl(NvBlastFamily& family, ExtStressSolverSettings settings);
-	virtual void							release() override;
-
-
-	//////// ExtStressSolverImpl interface ////////
-
-	virtual void							setAllNodesInfoFromLL(float density = 1.0f) override;
-
-	virtual void							setNodeInfo(uint32_t graphNode, float mass, float volume, PxVec3 localPos, bool isStatic) override;
-
-	virtual void							setSettings(const ExtStressSolverSettings& settings) override
-	{
-		m_settings = settings;
-	}
-
-	virtual const ExtStressSolverSettings&	getSettings() const override
-	{
-		return m_settings;
-	}
-
-	virtual bool							addForce(const NvBlastActor& actor, physx::PxVec3 localPosition, physx::PxVec3 localForce, ExtForceMode::Enum mode) override;
-
-	virtual void							addForce(uint32_t graphNode, physx::PxVec3 localForce, ExtForceMode::Enum mode) override;
-
-	virtual bool							addGravityForce(const NvBlastActor& actor, physx::PxVec3 localGravity) override;
-	virtual bool							addAngularVelocity(const NvBlastActor& actor, PxVec3 localCenterMass, physx::PxVec3 localAngularVelocity) override;
-
-	virtual void							update() override;
-
-	virtual uint32_t						getOverstressedBondCount() const override
-	{
-		return m_graphProcessor->getOverstressedBondCount();
-	}
-
-	virtual void							generateFractureCommands(const NvBlastActor& actor, NvBlastFractureBuffers& commands) override;
-	virtual void							generateFractureCommands(NvBlastFractureBuffers& commands) override;
-	virtual uint32_t						generateFractureCommandsPerActor(const NvBlastActor** actorBuffer, NvBlastFractureBuffers* commandsBuffer, uint32_t bufferSize) override;
-
-
-	void									reset() override
-	{
-		m_reset = true;
-	}
-
-	virtual float							getStressErrorLinear() const override
-	{
-		return m_errorLinear;
-	}
-
-	virtual float							getStressErrorAngular() const override
-	{
-		return m_errorAngular;
-	}
-
-	virtual uint32_t						getFrameCount() const override
-	{
-		return m_framesCount;
-	}
-
-	virtual uint32_t						getBondCount() const override
-	{
-		return m_graphProcessor->getSolverBondCount();
-	}
-
-	virtual bool							notifyActorCreated(const NvBlastActor& actor) override;
-
-	virtual void							notifyActorDestroyed(const NvBlastActor& actor) override;
-
-	virtual const DebugBuffer				fillDebugRender(const uint32_t* nodes, uint32_t nodeCount, DebugRenderMode mode, float scale) override;
-
-
-private:
-	~ExtStressSolverImpl();
-
-
-	//////// private methods ////////
-
-	void									solve();
-
-	void									fillFractureCommands(const NvBlastActor& actor, NvBlastFractureBuffers& commands);
-
-	void									initialize();
-
-	void									iterate();
-
-	void									syncSolver();
-
-	template<class T>
-	T*										getScratchArray(uint32_t size);
-
-
-	//////// data ////////
-
-	struct ImpulseData
-	{
-		physx::PxVec3 position;
-		physx::PxVec3 impulse;
-	};
-
-	NvBlastFamily&														m_family;
-	HashSet<const NvBlastActor*>::type									m_activeActors;
-	ExtStressSolverSettings												m_settings;
-	NvBlastSupportGraph													m_graph;
-	bool																m_isDirty;
-	bool																m_reset;
-	const float*														m_bondHealths;
-	SupportGraphProcessor*												m_graphProcessor;
-	float																m_errorAngular;
-	float																m_errorLinear;
-	uint32_t															m_framesCount;
-	Array<NvBlastBondFractureData>::type								m_bondFractureBuffer;
-	Array<uint8_t>::type												m_scratch;
-	Array<DebugLine>::type												m_debugLineBuffer;
-};
-
-
-template<class T>
-NV_INLINE T* ExtStressSolverImpl::getScratchArray(uint32_t size)
-{
-	const uint32_t scratchSize = sizeof(T) * size;
-	if (m_scratch.size() < scratchSize)
-	{
-		m_scratch.resize(scratchSize);
-	}
-	return reinterpret_cast<T*>(m_scratch.begin());
-}
-
-
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-//													Creation
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-ExtStressSolverImpl::ExtStressSolverImpl(NvBlastFamily& family, ExtStressSolverSettings settings)
-	: m_family(family), m_settings(settings), m_isDirty(false), m_reset(false), 
-	m_errorAngular(std::numeric_limits<float>::max()), m_errorLinear(std::numeric_limits<float>::max()), m_framesCount(0)
-{
-	const NvBlastAsset* asset = NvBlastFamilyGetAsset(&m_family, logLL);
-	NVBLAST_ASSERT(asset);
-
-	m_graph = NvBlastAssetGetSupportGraph(asset, logLL);
-	const uint32_t bondCount = NvBlastAssetGetBondCount(asset, logLL);
-
-	m_bondFractureBuffer.reserve(bondCount);
-
-	{
-		NvBlastActor* actor;
-		NvBlastFamilyGetActors(&actor, 1, &family, logLL);
-		m_bondHealths = NvBlastActorGetBondHealths(actor, logLL);
-	}
-
-	m_graphProcessor = NVBLAST_NEW(SupportGraphProcessor)(m_graph.nodeCount, bondCount);
-
-	// traverse graph and fill bond info
-	for (uint32_t node0 = 0; node0 < m_graph.nodeCount; ++node0)
-	{
-		for (uint32_t adjacencyIndex = m_graph.adjacencyPartition[node0]; adjacencyIndex < m_graph.adjacencyPartition[node0 + 1]; adjacencyIndex++)
-		{
-			uint32_t bondIndex = m_graph.adjacentBondIndices[adjacencyIndex];
-			if (m_bondHealths[bondIndex] <= 0.0f)
-				continue;
-			uint32_t node1 = m_graph.adjacentNodeIndices[adjacencyIndex];
-
-			if (node0 < node1)
-			{
-				m_graphProcessor->addBond(node0, node1, bondIndex);
-			}
-		}
-	}
-}
-
-ExtStressSolverImpl::~ExtStressSolverImpl()
-{
-	NVBLAST_DELETE(m_graphProcessor, SupportGraphProcessor);
-}
-
-ExtStressSolver* ExtStressSolver::create(NvBlastFamily& family, ExtStressSolverSettings settings)
-{
-	return NVBLAST_NEW(ExtStressSolverImpl) (family, settings);
-}
-
-void ExtStressSolverImpl::release()
-{
-	NVBLAST_DELETE(this, ExtStressSolverImpl);
-}
-
-
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-//											Actors & Graph Data
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-void ExtStressSolverImpl::setAllNodesInfoFromLL(float density)
-{
-	const NvBlastAsset* asset = NvBlastFamilyGetAsset(&m_family, logLL);
-	NVBLAST_ASSERT(asset);
-
-	const uint32_t chunkCount = NvBlastAssetGetChunkCount(asset, logLL);
-	const NvBlastChunk* chunks = NvBlastAssetGetChunks(asset, logLL);
-
-	// traverse graph and fill node info
-	for (uint32_t node0 = 0; node0 < m_graph.nodeCount; ++node0)
-	{
-		const uint32_t chunkIndex0 = m_graph.chunkIndices[node0];
-		if (chunkIndex0 >= chunkCount)
-		{
-			// chunkIndex is invalid means it is static node (represents world)
-			m_graphProcessor->setNodeInfo(node0, 0.0f, 0.0f, PxVec3(), true);
-		}
-		else
-		{
-			// Check if node is static. There is at maximum only one static node in LL that represents world, but we consider all nodes 
-			// connected to it directly to be static too. It's better for general stress solver quality to have more then 1 static node.
-			bool isNodeConnectedToStatic = false;
-			for (uint32_t adjacencyIndex = m_graph.adjacencyPartition[node0]; adjacencyIndex < m_graph.adjacencyPartition[node0 + 1]; adjacencyIndex++)
-			{
-				uint32_t bondIndex = m_graph.adjacentBondIndices[adjacencyIndex];
-				if (m_bondHealths[bondIndex] <= 0.0f)
-					continue;
-				uint32_t node1 = m_graph.adjacentNodeIndices[adjacencyIndex];
-				uint32_t chunkIndex1 = m_graph.chunkIndices[node1];
-				if (chunkIndex1 >= chunkCount)
-				{
-					isNodeConnectedToStatic = true;
-					break;
-				}
-			}
-
-			// fill node info
-			const NvBlastChunk& chunk = chunks[chunkIndex0];
-			const float volume = chunk.volume;
-			const float mass = volume * density;
-			const PxVec3 localPos = *reinterpret_cast<const PxVec3*>(chunk.centroid);
-			m_graphProcessor->setNodeInfo(node0, mass, volume, localPos, isNodeConnectedToStatic);
-		}
-	}
-}
-
-void ExtStressSolverImpl::setNodeInfo(uint32_t graphNode, float mass, float volume, PxVec3 localPos, bool isStatic)
-{
-	m_graphProcessor->setNodeInfo(graphNode, mass, volume, localPos, isStatic);
-}
-
-bool ExtStressSolverImpl::notifyActorCreated(const NvBlastActor& actor)
-{
-	const uint32_t graphNodeCount = NvBlastActorGetGraphNodeCount(&actor, logLL);
-	if (graphNodeCount > 1)
-	{
-		// update neighbors
-		{
-			uint32_t* graphNodeIndices = getScratchArray<uint32_t>(graphNodeCount);
-			const uint32_t nodeCount = NvBlastActorGetGraphNodeIndices(graphNodeIndices, graphNodeCount, &actor, logLL);
-			for (uint32_t i = 0; i < nodeCount; ++i)
-			{
-				m_graphProcessor->setNodeNeighborsCount(graphNodeIndices[i], nodeCount);
-			}
-		}
-
-		m_activeActors.insert(&actor);
-		m_isDirty = true;
-		return true;
-	}
-	return false;
-}
-
-void ExtStressSolverImpl::notifyActorDestroyed(const NvBlastActor& actor)
-{
-	if (m_activeActors.erase(&actor))
-	{
-		m_isDirty = true;
-	}
-}
-
-void ExtStressSolverImpl::syncSolver()
-{
-	// traverse graph and remove dead bonds
-	for (uint32_t node0 = 0; node0 < m_graph.nodeCount; ++node0)
-	{
-		for (uint32_t adjacencyIndex = m_graph.adjacencyPartition[node0]; adjacencyIndex < m_graph.adjacencyPartition[node0 + 1]; adjacencyIndex++)
-		{
-			uint32_t node1 = m_graph.adjacentNodeIndices[adjacencyIndex];
-			if (node0 < node1)
-			{
-				uint32_t bondIndex = m_graph.adjacentBondIndices[adjacencyIndex];
-
-				if (m_bondHealths[bondIndex] <= 0.0f)
-				{
-					m_graphProcessor->removeBondIfExists(bondIndex);
-				}
-			}
-		}
-	}
-
-	m_isDirty = false;
-}
-
-void ExtStressSolverImpl::initialize()
-{
-	if (m_reset)
-	{
-		m_framesCount = 0;
-	}
-
-	if (m_isDirty)
-	{
-		syncSolver();
-	}
-
-	if (m_settings.graphReductionLevel != m_graphProcessor->getGraphReductionLevel())
-	{
-		m_graphProcessor->setGraphReductionLevel(m_settings.graphReductionLevel);
-	}
-}
-
-bool ExtStressSolverImpl::addForce(const NvBlastActor& actor, physx::PxVec3 localPosition, physx::PxVec3 localForce, ExtForceMode::Enum mode)
-{
-	float bestDist = FLT_MAX;
-	uint32_t bestNode = invalidIndex<uint32_t>();
-
-	const uint32_t graphNodeCount = NvBlastActorGetGraphNodeCount(&actor, logLL);
-	if (graphNodeCount > 1)
-	{
-		uint32_t* graphNodeIndices = getScratchArray<uint32_t>(graphNodeCount);
-		const uint32_t nodeCount = NvBlastActorGetGraphNodeIndices(graphNodeIndices, graphNodeCount, &actor, logLL);
-
-		for (uint32_t i = 0; i < nodeCount; ++i)
-		{
-			const uint32_t node = graphNodeIndices[i];
-			const float sqrDist = (localPosition - m_graphProcessor->getNodeData(node).localPos).magnitudeSquared();
-			if (sqrDist < bestDist)
-			{
-				bestDist = sqrDist;
-				bestNode = node;
-			}
-		}
-
-		if (!isInvalidIndex(bestNode))
-		{
-			m_graphProcessor->addNodeForce(bestNode, localForce, mode);
-			return true;
-		}
-	}
-	return false;
-}
-
-void ExtStressSolverImpl::addForce(uint32_t graphNode, physx::PxVec3 localForce, ExtForceMode::Enum mode)
-{
-	m_graphProcessor->addNodeForce(graphNode, localForce, mode);
-}
-
-bool ExtStressSolverImpl::addGravityForce(const NvBlastActor& actor, physx::PxVec3 localGravity)
-{
-	const uint32_t graphNodeCount = NvBlastActorGetGraphNodeCount(&actor, logLL);
-	if (graphNodeCount > 1)
-	{
-		uint32_t* graphNodeIndices = getScratchArray<uint32_t>(graphNodeCount);
-		const uint32_t nodeCount = NvBlastActorGetGraphNodeIndices(graphNodeIndices, graphNodeCount, &actor, logLL);
-
-		for (uint32_t i = 0; i < nodeCount; ++i)
-		{
-			const uint32_t node = graphNodeIndices[i];
-			m_graphProcessor->addNodeVelocity(node, localGravity);
-		}
-		return true;
-	}
-	return false;
-}
-
-bool ExtStressSolverImpl::addAngularVelocity(const NvBlastActor& actor, PxVec3 localCenterMass, physx::PxVec3 localAngularVelocity)
-{
-	const uint32_t graphNodeCount = NvBlastActorGetGraphNodeCount(&actor, logLL);
-	if (graphNodeCount > 1)
-	{
-		uint32_t* graphNodeIndices = getScratchArray<uint32_t>(graphNodeCount);
-		const uint32_t nodeCount = NvBlastActorGetGraphNodeIndices(graphNodeIndices, graphNodeCount, &actor, logLL);
-
-		// Apply centrifugal force
-		for (uint32_t i = 0; i < nodeCount; ++i)
-		{
-			const uint32_t node = graphNodeIndices[i];
-			const auto& localPos = m_graphProcessor->getNodeData(node).localPos;
-			// a = w x (w x r)
-			const PxVec3 centrifugalAcceleration = localAngularVelocity.cross(localAngularVelocity.cross(localPos - localCenterMass));
-			m_graphProcessor->addNodeVelocity(node, centrifugalAcceleration);
-		}
-		return true;
-	}
-	return false;
-}
-
-
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-//													Update
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-void ExtStressSolverImpl::update()
-{
-	initialize();
-
-	solve();
-
-	m_framesCount++;
-}
-
-void ExtStressSolverImpl::solve()
-{
-	PX_SIMD_GUARD;
-
-	m_graphProcessor->solve(m_settings, m_bondHealths, WARM_START && !m_reset);
-	m_reset = false;
-
-	m_graphProcessor->calcError(m_errorLinear, m_errorAngular);
-}
-
-
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-//													Damage
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-void ExtStressSolverImpl::fillFractureCommands(const NvBlastActor& actor, NvBlastFractureBuffers& commands)
-{
-	const uint32_t graphNodeCount = NvBlastActorGetGraphNodeCount(&actor, logLL);
-	uint32_t commandCount = 0;
-
-	if (graphNodeCount > 1 && m_graphProcessor->getOverstressedBondCount() > 0)
-	{
-		uint32_t* graphNodeIndices = getScratchArray<uint32_t>(graphNodeCount);
-		const uint32_t nodeCount = NvBlastActorGetGraphNodeIndices(graphNodeIndices, graphNodeCount, &actor, logLL);
-
-		for (uint32_t i = 0; i < nodeCount; ++i)
-		{
-			const uint32_t node0 = graphNodeIndices[i];
-			for (uint32_t adjacencyIndex = m_graph.adjacencyPartition[node0]; adjacencyIndex < m_graph.adjacencyPartition[node0 + 1]; adjacencyIndex++)
-			{
-				uint32_t node1 = m_graph.adjacentNodeIndices[adjacencyIndex];
-				if (node0 < node1)
-				{
-					uint32_t bondIndex = m_graph.adjacentBondIndices[adjacencyIndex];
-					const float bondHealth = m_bondHealths[bondIndex];
-					const float bondStress = m_graphProcessor->getBondStress(bondIndex);
-
-					if (bondHealth > 0.0f && bondStress > bondHealth)
-					{
-						const NvBlastBondFractureData data = {
-							0,
-							node0,
-							node1,
-							bondHealth
-						};
-						m_bondFractureBuffer.pushBack(data);
-						commandCount++;
-					}
-				}
-			}
-		}
-	}
-
-	commands.chunkFractureCount = 0;
-	commands.chunkFractures = nullptr;
-	commands.bondFractureCount = commandCount;
-	commands.bondFractures = commandCount > 0 ? m_bondFractureBuffer.end() - commandCount : nullptr;
-}
-
-void ExtStressSolverImpl::generateFractureCommands(const NvBlastActor& actor, NvBlastFractureBuffers& commands)
-{
-	m_bondFractureBuffer.clear();
-	fillFractureCommands(actor, commands);
-}
-
-void ExtStressSolverImpl::generateFractureCommands(NvBlastFractureBuffers& commands)
-{
-	m_bondFractureBuffer.clear();
-
-	const uint32_t bondCount = m_graphProcessor->getBondCount();
-	const uint32_t overstressedBondCount = m_graphProcessor->getOverstressedBondCount();
-	for (uint32_t i = 0; i < bondCount && m_bondFractureBuffer.size() < overstressedBondCount; i++)
-	{
-		const auto& bondData = m_graphProcessor->getBondData(i);
-		const float bondHealth = m_bondHealths[bondData.blastBondIndex];
-		if (bondHealth > 0.0f && bondData.stress > bondHealth)
-		{
-			const NvBlastBondFractureData data = {
-				0,
-				bondData.node0,
-				bondData.node1,
-				bondHealth
-			};
-			m_bondFractureBuffer.pushBack(data);
-		}
-	}
-
-	commands.chunkFractureCount = 0;
-	commands.chunkFractures = nullptr;
-	commands.bondFractureCount = m_bondFractureBuffer.size();
-	commands.bondFractures = m_bondFractureBuffer.size() > 0 ? m_bondFractureBuffer.begin() : nullptr;
-}
-
-uint32_t ExtStressSolverImpl::generateFractureCommandsPerActor(const NvBlastActor**  actorBuffer, NvBlastFractureBuffers* commandsBuffer, uint32_t bufferSize)
-{
-	if (m_graphProcessor->getOverstressedBondCount() == 0)
-		return 0;
-
-	m_bondFractureBuffer.clear();
-	uint32_t index = 0;
-	for (auto it = m_activeActors.getIterator(); !it.done() && index < bufferSize; ++it)
-	{
-		const NvBlastActor* actor = *it;
-		NvBlastFractureBuffers& nextCommand = commandsBuffer[index];
-		fillFractureCommands(*actor, nextCommand);
-		if (nextCommand.bondFractureCount > 0)
-		{
-			actorBuffer[index] = actor;
-			index++;
-		}
-	}
-	return index;
-}
-
-
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-//													Debug Render
-///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-
-static PxU32 PxVec4ToU32Color(const PxVec4& color)
-{
-	PxU32 c = 0;
-	c |= (int)(color.w * 255); c <<= 8;
-	c |= (int)(color.z * 255); c <<= 8;
-	c |= (int)(color.y * 255); c <<= 8;
-	c |= (int)(color.x * 255);
-	return c;
-}
-
-static PxVec4 PxVec4Lerp(const PxVec4 v0, const PxVec4 v1, float val)
-{
-	PxVec4 v(
-		v0.x * (1 - val) + v1.x * val,
-		v0.y * (1 - val) + v1.y * val,
-		v0.z * (1 - val) + v1.z * val,
-		v0.w * (1 - val) + v1.w * val
-	);
-	return v;
-}
-
-inline float clamp01(float v)
-{
-	return v < 0.0f ? 0.0f : (v > 1.0f ? 1.0f : v);
-}
-
-inline PxVec4 bondHealthColor(float healthFraction)
-{
-	healthFraction = clamp01(healthFraction);
-
-	const PxVec4 BOND_HEALTHY_COLOR(0.0f, 1.0f, 1.0f, 1.0f);
-	const PxVec4 BOND_MID_COLOR(1.0f, 1.0f, 0.0f, 1.0f);
-	const PxVec4 BOND_BROKEN_COLOR(1.0f, 0.0f, 0.0f, 1.0f);
-
-	return healthFraction < 0.5 ? PxVec4Lerp(BOND_BROKEN_COLOR, BOND_MID_COLOR, 2.0f * healthFraction) : PxVec4Lerp(BOND_MID_COLOR, BOND_HEALTHY_COLOR, 2.0f * healthFraction - 1.0f);
-}
-
-const ExtStressSolver::DebugBuffer ExtStressSolverImpl::fillDebugRender(const uint32_t* nodes, uint32_t nodeCount, DebugRenderMode mode, float scale)
-{
-	const PxVec4 BOND_IMPULSE_LINEAR_COLOR(0.0f, 1.0f, 0.0f, 1.0f);
-	const PxVec4 BOND_IMPULSE_ANGULAR_COLOR(1.0f, 0.0f, 0.0f, 1.0f);
-
-	ExtStressSolver::DebugBuffer debugBuffer = { nullptr, 0 };
-
-	if (m_isDirty)
-		return debugBuffer;
-
-	m_debugLineBuffer.clear();
-
-	Array<uint8_t>::type& nodesSet = m_scratch;
-
-	nodesSet.resize(m_graphProcessor->getSolverNodeCount());
-	memset(nodesSet.begin(), 0, nodesSet.size() * sizeof(uint8_t));
-	for (uint32_t i = 0; i < nodeCount; ++i)
-	{
-		NVBLAST_ASSERT(m_graphProcessor->getNodeData(nodes[i]).solverNode < nodesSet.size());
-		nodesSet[m_graphProcessor->getNodeData(nodes[i]).solverNode] = 1;
-	}
-
-	const uint32_t bondCount = m_graphProcessor->getSolverBondCount();
-	for (uint32_t i = 0; i < bondCount; ++i)
-	{
-		const auto& solverInternalBondData = m_graphProcessor->getSolverInternalBondData(i);
-		if (nodesSet[solverInternalBondData.node0] != 0)
-		{
-			//NVBLAST_ASSERT(nodesSet[solverInternalBondData.node1] != 0);
-			const auto& solverInternalNode0 = m_graphProcessor->getSolverInternalNodeData(solverInternalBondData.node0);
-			const auto& solverInternalNode1 = m_graphProcessor->getSolverInternalNodeData(solverInternalBondData.node1);
-			const auto& solverNode0 = m_graphProcessor->getSolverNodeData(solverInternalBondData.node0);
-			const auto& solverNode1 = m_graphProcessor->getSolverNodeData(solverInternalBondData.node1);
-
-			PxVec3 p0 = solverNode0.localPos;
-			PxVec3 p1 = solverNode1.localPos;
-			PxVec3 center = (p0 + p1) * 0.5f;
-
-			const float stress = std::min<float>(m_graphProcessor->getSolverBondStressHealth(i, m_settings), 1.0f);
-			PxVec4 color = bondHealthColor(1.0f - stress);
-
-			m_debugLineBuffer.pushBack(DebugLine(p0, p1, PxVec4ToU32Color(color)));
-
-			float impulseScale = scale;
-
-			if (mode == DebugRenderMode::STRESS_GRAPH_NODES_IMPULSES)
-			{
-				m_debugLineBuffer.pushBack(DebugLine(p0, p0 + solverInternalNode0.velocityLinear * impulseScale, PxVec4ToU32Color(BOND_IMPULSE_LINEAR_COLOR)));
-				m_debugLineBuffer.pushBack(DebugLine(p0, p0 + solverInternalNode0.velocityAngular * impulseScale, PxVec4ToU32Color(BOND_IMPULSE_ANGULAR_COLOR)));
-				m_debugLineBuffer.pushBack(DebugLine(p1, p1 + solverInternalNode1.velocityLinear * impulseScale, PxVec4ToU32Color(BOND_IMPULSE_LINEAR_COLOR)));
-				m_debugLineBuffer.pushBack(DebugLine(p1, p1 + solverInternalNode1.velocityAngular * impulseScale, PxVec4ToU32Color(BOND_IMPULSE_ANGULAR_COLOR)));
-			}
-			else if (mode == DebugRenderMode::STRESS_GRAPH_BONDS_IMPULSES)
-			{
-				m_debugLineBuffer.pushBack(DebugLine(center, center + solverInternalBondData.impulseLinear * impulseScale, PxVec4ToU32Color(BOND_IMPULSE_LINEAR_COLOR)));
-				m_debugLineBuffer.pushBack(DebugLine(center, center + solverInternalBondData.impulseAngular * impulseScale, PxVec4ToU32Color(BOND_IMPULSE_ANGULAR_COLOR)));
-			}
-		}
-	}
-
-	debugBuffer.lines = m_debugLineBuffer.begin();
-	debugBuffer.lineCount = m_debugLineBuffer.size();
-
-	return debugBuffer;
-}
-
-
-} // namespace Blast
-} // namespace Nv
+// This code contains NVIDIA Confidential Information and is disclosed to you

+// under a form of NVIDIA software license agreement provided separately to you.

+//

+// Notice

+// NVIDIA Corporation and its licensors retain all intellectual property and

+// proprietary rights in and to this software and related documentation and

+// any modifications thereto. Any use, reproduction, disclosure, or

+// distribution of this software and related documentation without an express

+// license agreement from NVIDIA Corporation is strictly prohibited.

+//

+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES

+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO

+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,

+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.

+//

+// Information and code furnished is believed to be accurate and reliable.

+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such

+// information or for any infringement of patents or other rights of third parties that may

+// result from its use. No license is granted by implication or otherwise under any patent

+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.

+// This code supersedes and replaces all information previously supplied.

+// NVIDIA Corporation products are not authorized for use as critical

+// components in life support devices or systems without express written approval of

+// NVIDIA Corporation.

+//

+// Copyright (c) 2016-2018 NVIDIA Corporation. All rights reserved.

+

+

+#include "NvBlastExtStressSolver.h"

+#include "NvBlast.h"

+#include "NvBlastGlobals.h"

+#include "NvBlastArray.h"

+#include "NvBlastHashMap.h"

+#include "NvBlastHashSet.h"

+#include "NvBlastAssert.h"

+#include "NvBlastIndexFns.h"

+

+#include <PsVecMath.h>

+#include "PsFPU.h"

+

+#include <algorithm>

+

+#define USE_SCALAR_IMPL 0

+#define WARM_START 1

+#define GRAPH_INTERGRIRY_CHECK 0

+

+#if GRAPH_INTERGRIRY_CHECK

+#include <set>

+#endif

+

+

+namespace Nv

+{

+namespace Blast

+{

+

+using namespace physx;

+

+

+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

+//													 Solver

+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

+

+class SequentialImpulseSolver

+{

+public:

+	PX_ALIGN_PREFIX(16)

+	struct BondData

+	{

+		physx::PxVec3 impulseLinear;

+		uint32_t node0;

+		physx::PxVec3 impulseAngular;

+		uint32_t node1;

+		physx::PxVec3 offset0;

+		float invOffsetSqrLength;

+	}

+	PX_ALIGN_SUFFIX(16);

+

+	PX_ALIGN_PREFIX(16)

+	struct NodeData

+	{

+		physx::PxVec3 velocityLinear;

+		float invI;

+		physx::PxVec3 velocityAngular;

+		float invMass;

+	}

+	PX_ALIGN_SUFFIX(16);

+

+	SequentialImpulseSolver(uint32_t nodeCount, uint32_t maxBondCount)

+	{

+		m_nodesData.resize(nodeCount);

+		m_bondsData.reserve(maxBondCount);

+	}

+

+	const NodeData& getNodeData(uint32_t node) const

+	{

+		return m_nodesData[node];

+	}

+

+	const BondData& getBondData(uint32_t bond) const

+	{

+		return m_bondsData[bond];

+	}

+

+	uint32_t getBondCount() const

+	{

+		return m_bondsData.size();

+	}

+

+	uint32_t getNodeCount() const

+	{

+		return m_nodesData.size();;

+	}

+

+	void setNodeMassInfo(uint32_t node, float invMass, float invI)

+	{

+		m_nodesData[node].invMass = invMass;

+		m_nodesData[node].invI = invI;

+	}

+

+	void initialize()

+	{

+		for (auto& node : m_nodesData)

+		{

+			node.velocityLinear = PxVec3(PxZero);

+			node.velocityAngular = PxVec3(PxZero);

+		}

+	}

+

+	void setNodeVelocities(uint32_t node, const PxVec3& velocityLinear, const PxVec3& velocityAngular)

+	{

+		m_nodesData[node].velocityLinear = velocityLinear;

+		m_nodesData[node].velocityAngular = velocityAngular;

+	}

+

+	uint32_t addBond(uint32_t node0, uint32_t node1, const PxVec3& offset)

+	{

+		const BondData data = {

+			PxVec3(PxZero), 

+			node0, 

+			PxVec3(PxZero), 

+			node1, 

+			offset, 

+			1.0f / offset.magnitudeSquared() 

+		};

+		m_bondsData.pushBack(data);

+		return m_bondsData.size() - 1;

+	}

+

+	void replaceWithLast(uint32_t bondIndex)

+	{

+		m_bondsData.replaceWithLast(bondIndex);

+	}

+

+	void reset(uint32_t nodeCount)

+	{

+		m_bondsData.clear();

+		m_nodesData.resize(nodeCount);

+	}

+

+	void clearBonds()

+	{

+		m_bondsData.clear();

+	}

+

+	void solve(uint32_t iterationCount, bool warmStart = true)

+	{

+		solveInit(warmStart);

+

+		for (uint32_t i = 0; i < iterationCount; ++i)

+		{

+			iterate();

+		}

+	}

+

+	void calcError(float& linear, float& angular)

+	{

+		linear = 0.0f;

+		angular = 0.0f;

+		for (BondData& bond : m_bondsData)

+		{

+			NodeData* node0 = &m_nodesData[bond.node0];

+			NodeData* node1 = &m_nodesData[bond.node1];

+

+			const PxVec3 vA = node0->velocityLinear - node0->velocityAngular.cross(bond.offset0);

+			const PxVec3 vB = node1->velocityLinear + node1->velocityAngular.cross(bond.offset0);

+

+			const PxVec3 vErrorLinear = vA - vB;

+			const PxVec3 vErrorAngular = node0->velocityAngular - node1->velocityAngular;

+

+			linear += vErrorLinear.magnitude();

+			angular += vErrorAngular.magnitude();

+		}

+	}

+

+private:

+	void solveInit(bool warmStart = false)

+	{

+		if (warmStart)

+		{

+			for (BondData& bond : m_bondsData)

+			{

+				NodeData* node0 = &m_nodesData[bond.node0];

+				NodeData* node1 = &m_nodesData[bond.node1];

+

+				const PxVec3 velocityLinearCorr0 = bond.impulseLinear * node0->invMass;

+				const PxVec3 velocityLinearCorr1 = bond.impulseLinear * node1->invMass;

+

+				const PxVec3 velocityAngularCorr0 = bond.impulseAngular * node0->invI - bond.offset0.cross(velocityLinearCorr0) * bond.invOffsetSqrLength;

+				const PxVec3 velocityAngularCorr1 = bond.impulseAngular * node1->invI + bond.offset0.cross(velocityLinearCorr1) * bond.invOffsetSqrLength;

+

+				node0->velocityLinear += velocityLinearCorr0;

+				node1->velocityLinear -= velocityLinearCorr1;

+

+				node0->velocityAngular += velocityAngularCorr0;

+				node1->velocityAngular -= velocityAngularCorr1;

+			}

+		}

+		else

+		{

+			for (BondData& bond : m_bondsData)

+			{

+				bond.impulseLinear = PxVec3(PxZero);

+				bond.impulseAngular = PxVec3(PxZero);

+			}

+		}

+	}

+

+

+	void iterate()

+	{

+		using namespace physx::shdfnd::aos;

+

+		for (BondData& bond : m_bondsData)

+		{

+			NodeData* node0 = &m_nodesData[bond.node0];

+			NodeData* node1 = &m_nodesData[bond.node1];

+

+#if USE_SCALAR_IMPL

+			const PxVec3 vA = node0->velocityLinear - node0->velocityAngular.cross(bond.offset0);

+			const PxVec3 vB = node1->velocityLinear + node1->velocityAngular.cross(bond.offset0);

+

+			const PxVec3 vErrorLinear = vA - vB;

+			const PxVec3 vErrorAngular = node0->velocityAngular - node1->velocityAngular;

+

+			const float weightedMass = 1.0f / (node0->invMass + node1->invMass);

+			const float weightedInertia = 1.0f / (node0->invI + node1->invI);

+

+			const PxVec3 outImpulseLinear = -vErrorLinear * weightedMass * 0.5f;

+			const PxVec3 outImpulseAngular = -vErrorAngular * weightedInertia * 0.5f;

+

+			bond.impulseLinear += outImpulseLinear;

+			bond.impulseAngular += outImpulseAngular;

+

+			const PxVec3 velocityLinearCorr0 = outImpulseLinear * node0->invMass;

+			const PxVec3 velocityLinearCorr1 = outImpulseLinear * node1->invMass;

+

+			const PxVec3 velocityAngularCorr0 = outImpulseAngular * node0->invI - bond.offset0.cross(velocityLinearCorr0) * bond.invOffsetSqrLength;

+			const PxVec3 velocityAngularCorr1 = outImpulseAngular * node1->invI + bond.offset0.cross(velocityLinearCorr1) * bond.invOffsetSqrLength;

+

+			node0->velocityLinear += velocityLinearCorr0;

+			node1->velocityLinear -= velocityLinearCorr1;

+

+			node0->velocityAngular += velocityAngularCorr0;

+			node1->velocityAngular -= velocityAngularCorr1;

+#else

+			const Vec3V velocityLinear0 = V3LoadUnsafeA(node0->velocityLinear);

+			const Vec3V velocityLinear1 = V3LoadUnsafeA(node1->velocityLinear);

+			const Vec3V velocityAngular0 = V3LoadUnsafeA(node0->velocityAngular);

+			const Vec3V velocityAngular1 = V3LoadUnsafeA(node1->velocityAngular);

+

+			const Vec3V offset = V3LoadUnsafeA(bond.offset0);

+			const Vec3V vA = V3Add(velocityLinear0, V3Neg(V3Cross(velocityAngular0, offset)));

+			const Vec3V vB = V3Add(velocityLinear1, V3Cross(velocityAngular1, offset));

+

+			const Vec3V vErrorLinear = V3Sub(vA, vB);

+			const Vec3V vErrorAngular = V3Sub(velocityAngular0, velocityAngular1);

+

+			const FloatV invM0 = FLoad(node0->invMass);

+			const FloatV invM1 = FLoad(node1->invMass);

+			const FloatV invI0 = FLoad(node0->invI);

+			const FloatV invI1 = FLoad(node1->invI);

+			const FloatV invOffsetSqrLength = FLoad(bond.invOffsetSqrLength);

+

+			const FloatV weightedMass = FLoad(-0.5f / (node0->invMass + node1->invMass));

+			const FloatV weightedInertia = FLoad(-0.5f / (node0->invI + node1->invI));

+

+			const Vec3V outImpulseLinear = V3Scale(vErrorLinear, weightedMass);

+			const Vec3V outImpulseAngular = V3Scale(vErrorAngular, weightedInertia);

+

+			V3StoreA(V3Add(V3LoadUnsafeA(bond.impulseLinear), outImpulseLinear), bond.impulseLinear);

+			V3StoreA(V3Add(V3LoadUnsafeA(bond.impulseAngular), outImpulseAngular), bond.impulseAngular);

+

+			const Vec3V velocityLinearCorr0 = V3Scale(outImpulseLinear, invM0);

+			const Vec3V velocityLinearCorr1 = V3Scale(outImpulseLinear, invM1);

+

+			const Vec3V velocityAngularCorr0 = V3Sub(V3Scale(outImpulseAngular, invI0), V3Scale(V3Cross(offset, velocityLinearCorr0), invOffsetSqrLength));

+			const Vec3V velocityAngularCorr1 = V3Add(V3Scale(outImpulseAngular, invI1),	V3Scale(V3Cross(offset, velocityLinearCorr1), invOffsetSqrLength));

+

+			V3StoreA(V3Add(velocityLinear0, velocityLinearCorr0), node0->velocityLinear);

+			V3StoreA(V3Sub(velocityLinear1, velocityLinearCorr1), node1->velocityLinear);

+

+			V3StoreA(V3Add(velocityAngular0, velocityAngularCorr0), node0->velocityAngular);

+			V3StoreA(V3Sub(velocityAngular1, velocityAngularCorr1), node1->velocityAngular);

+#endif

+		}

+	}

+

+	Array<BondData>::type		m_bondsData;

+	Array<NodeData>::type		m_nodesData;

+};

+

+

+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

+//													 Graph Processor

+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

+

+#if GRAPH_INTERGRIRY_CHECK

+#define CHECK_GRAPH_INTEGRITY checkGraphIntegrity()

+#else

+#define CHECK_GRAPH_INTEGRITY ((void)0)

+#endif

+

+class SupportGraphProcessor

+{

+

+public:

+	struct BondData

+	{

+		uint32_t node0;

+		uint32_t node1;

+		uint32_t blastBondIndex;

+		float	 stress;

+	};

+

+	struct NodeData

+	{

+		float mass;

+		float volume;

+		PxVec3 localPos;

+		bool isStatic;

+		uint32_t solverNode;

+		uint32_t neighborsCount;

+		PxVec3 impulse;

+	};

+

+	struct SolverNodeData

+	{

+		uint32_t supportNodesCount;

+		PxVec3 localPos;

+		union

+		{

+			float mass;

+			int32_t indexShift;

+		};

+		float volume;

+		bool isStatic;

+	};

+

+	struct SolverBondData

+	{

+		InlineArray<uint32_t, 8>::type blastBondIndices;

+	};

+

+	SupportGraphProcessor(uint32_t nodeCount, uint32_t maxBondCount) : m_solver(nodeCount, maxBondCount), m_nodesDirty(true)

+	{

+		m_nodesData.resize(nodeCount);

+		m_bondsData.reserve(maxBondCount);

+

+		m_solverNodesData.resize(nodeCount);

+		m_solverBondsData.reserve(maxBondCount);

+

+		m_solverBondsMap.reserve(maxBondCount);

+

+		m_blastBondIndexMap.resize(maxBondCount);

+		memset(m_blastBondIndexMap.begin(), 0xFF, m_blastBondIndexMap.size() * sizeof(uint32_t));

+

+		resetImpulses();

+	}

+

+	const NodeData& getNodeData(uint32_t node) const

+	{

+		return m_nodesData[node];

+	}

+

+	const BondData& getBondData(uint32_t bond) const

+	{

+		return m_bondsData[bond];

+	}

+

+	const SolverNodeData& getSolverNodeData(uint32_t node) const

+	{

+		return m_solverNodesData[node];

+	}

+

+	const SolverBondData& getSolverBondData(uint32_t bond) const

+	{

+		return m_solverBondsData[bond];

+	}

+

+	const SequentialImpulseSolver::BondData& getSolverInternalBondData(uint32_t bond) const

+	{

+		return m_solver.getBondData(bond);

+	}

+

+	const SequentialImpulseSolver::NodeData& getSolverInternalNodeData(uint32_t node) const

+	{

+		return m_solver.getNodeData(node);

+	}

+

+	uint32_t getBondCount() const

+	{

+		return m_bondsData.size();

+	}

+

+	uint32_t getNodeCount() const

+	{

+		return m_nodesData.size();;

+	}

+

+	uint32_t getSolverBondCount() const

+	{

+		return m_solverBondsData.size();

+	}

+

+	uint32_t getSolverNodeCount() const

+	{

+		return m_solverNodesData.size();;

+	}

+

+	uint32_t getOverstressedBondCount() const

+	{

+		return m_overstressedBondCount;

+	}

+

+	float getSolverBondStressHealth(uint32_t bond, const ExtStressSolverSettings& settings) const

+	{

+		const auto& solverBond = getSolverInternalBondData(bond);

+		const float impulse = solverBond.impulseLinear.magnitude() * settings.stressLinearFactor + solverBond.impulseAngular.magnitude() * settings.stressAngularFactor;

+		// We then divide uniformly across bonds, which is obviously rough estimate.

+		// Potentially we can add bond area there and norm across area sum

+		const auto& blastBondIndices = m_solverBondsData[bond].blastBondIndices;

+		return blastBondIndices.empty() ? 0.0f : impulse / (blastBondIndices.size() * settings.hardness);

+	}

+

+	void setNodeInfo(uint32_t node, float mass, float volume, PxVec3 localPos, bool isStatic)

+	{

+		m_nodesData[node].mass = mass;

+		m_nodesData[node].volume = volume;

+		m_nodesData[node].localPos = localPos;

+		m_nodesData[node].isStatic = isStatic;

+		m_nodesDirty = true;

+	}

+

+	void setNodeNeighborsCount(uint32_t node, uint32_t neighborsCount)

+	{

+		// neighbors count is expected to be the number of nodes on 1 island/actor.

+		m_nodesData[node].neighborsCount = neighborsCount;

+

+		// check for too huge aggregates (happens after island's split)

+		if (!m_nodesDirty)

+		{

+			m_nodesDirty |= (m_solverNodesData[m_nodesData[node].solverNode].supportNodesCount > neighborsCount / 2);

+		}

+	}

+

+	void addNodeForce(uint32_t node, const PxVec3& force, ExtForceMode::Enum mode)

+	{

+		const PxVec3 impuse = (mode == ExtForceMode::IMPULSE) ? force : force * m_nodesData[node].mass;

+		m_nodesData[node].impulse += impuse;

+	}

+

+	void addNodeVelocity(uint32_t node, const PxVec3& velocity)

+	{

+		addNodeForce(node, velocity, ExtForceMode::VELOCITY);

+	}

+

+	void addNodeImpulse(uint32_t node, const PxVec3& impulse)

+	{

+		addNodeForce(node, impulse, ExtForceMode::IMPULSE);

+	}

+

+	void addBond(uint32_t node0, uint32_t node1, uint32_t blastBondIndex)

+	{

+		if (isInvalidIndex(m_blastBondIndexMap[blastBondIndex]))

+		{

+			const BondData data = {

+				node0,

+				node1,

+				blastBondIndex,

+				0.0f

+			};

+			m_bondsData.pushBack(data);

+			m_blastBondIndexMap[blastBondIndex] = m_bondsData.size() - 1;

+		}

+	}

+

+	void removeBondIfExists(uint32_t blastBondIndex)

+	{

+		const uint32_t bondIndex = m_blastBondIndexMap[blastBondIndex];

+

+		if (!isInvalidIndex(bondIndex))

+		{

+			const BondData& bond = m_bondsData[bondIndex];

+			const uint32_t solverNode0 = m_nodesData[bond.node0].solverNode;

+			const uint32_t solverNode1 = m_nodesData[bond.node1].solverNode;

+			bool isBondInternal = (solverNode0 == solverNode1);

+

+			if (isBondInternal)

+			{

+				// internal bond sadly requires graph resync (it never happens on reduction level '0')

+				m_nodesDirty = true;

+			}

+			else if (!m_nodesDirty)

+			{

+				// otherwise it's external bond, we can remove it manually and keep graph synced

+				// we don't need to spend time there if (m_nodesDirty == true), graph will be resynced anyways

+

+				BondKey solverBondKey(solverNode0, solverNode1);

+				auto entry = m_solverBondsMap.find(solverBondKey);

+				if (entry)

+				{

+					const uint32_t solverBondIndex = entry->second;

+					auto& blastBondIndices = m_solverBondsData[solverBondIndex].blastBondIndices;

+					blastBondIndices.findAndReplaceWithLast(blastBondIndex);

+					if (blastBondIndices.empty())

+					{

+						// all bonds associated with this solver bond were removed, so let's remove solver bond

+

+						m_solverBondsData.replaceWithLast(solverBondIndex);

+						m_solver.replaceWithLast(solverBondIndex);

+						if (m_solver.getBondCount() > 0)

+						{

+							// update 'previously last' solver bond mapping

+							const auto& solverBond = m_solver.getBondData(solverBondIndex);

+							m_solverBondsMap[BondKey(solverBond.node0, solverBond.node1)] = solverBondIndex;

+						}

+

+						m_solverBondsMap.erase(solverBondKey);

+					}

+				}

+

+				CHECK_GRAPH_INTEGRITY;

+			}

+

+			// remove bond from graph processor's list

+			m_blastBondIndexMap[blastBondIndex] = invalidIndex<uint32_t>();

+			m_bondsData.replaceWithLast(bondIndex);

+			m_blastBondIndexMap[m_bondsData[bondIndex].blastBondIndex] = m_bondsData.size() > bondIndex ? bondIndex : invalidIndex<uint32_t>();

+		}

+	}

+

+	void setGraphReductionLevel(uint32_t level)

+	{

+		m_graphReductionLevel = level;

+		m_nodesDirty = true;

+	}

+

+	uint32_t getGraphReductionLevel() const

+	{

+		return m_graphReductionLevel;

+	}

+

+	void solve(const ExtStressSolverSettings& settings, const float* bondHealth, bool warmStart = true)

+	{

+		sync();

+

+		m_solver.initialize();

+

+		for (const NodeData& node : m_nodesData)

+		{

+			const SequentialImpulseSolver::NodeData& solverNode = m_solver.getNodeData(node.solverNode);

+			m_solver.setNodeVelocities(node.solverNode, solverNode.velocityLinear + node.impulse * solverNode.invMass, PxVec3(PxZero));

+		}

+

+		uint32_t iterationCount = ExtStressSolver::getIterationsPerFrame(settings, getSolverBondCount());

+		m_solver.solve(iterationCount, warmStart);

+

+		resetImpulses();

+

+		updateBondStress(settings, bondHealth);

+	}

+

+	void calcError(float& linear, float& angular)

+	{

+		m_solver.calcError(linear, angular);

+	}

+

+	float getBondStress(uint32_t blastBondIndex)

+	{

+		const uint32_t bondIndex = m_blastBondIndexMap[blastBondIndex];

+		return isInvalidIndex(bondIndex) ? 0.0f : m_bondsData[bondIndex].stress;

+	}

+

+private:

+

+	void resetImpulses()

+	{

+		for (auto& node : m_nodesData)

+		{

+			node.impulse = PxVec3(PxZero);

+		}

+	}

+

+	void updateBondStress(const ExtStressSolverSettings& settings, const float* bondHealth)

+	{

+		m_overstressedBondCount = 0;

+

+		for (uint32_t i = 0; i < m_solverBondsData.size(); ++i)

+		{

+			const float stress = getSolverBondStressHealth(i, settings);

+			const auto& blastBondIndices = m_solverBondsData[i].blastBondIndices;

+			const float stressPerBond = blastBondIndices.size() > 0 ? stress / blastBondIndices.size() : 0.0f;

+			for (auto blastBondIndex : blastBondIndices)

+			{

+				const uint32_t bondIndex = m_blastBondIndexMap[blastBondIndex];

+				if (!isInvalidIndex(bondIndex))

+				{

+					BondData& bond = m_bondsData[bondIndex];

+

+					NVBLAST_ASSERT(getNodeData(bond.node0).solverNode != getNodeData(bond.node1).solverNode);

+					NVBLAST_ASSERT(bond.blastBondIndex == blastBondIndex);

+					

+					bond.stress = stressPerBond;

+

+					if (stress > bondHealth[blastBondIndex])

+					{

+						m_overstressedBondCount++;

+					}

+				}

+			}

+		}

+	}

+

+	void sync()

+	{

+		if (m_nodesDirty)

+		{

+			syncNodes();

+		}

+		if (m_bondsDirty)

+		{

+			syncBonds();

+		}

+

+		CHECK_GRAPH_INTEGRITY;

+	}

+

+	void syncNodes()

+	{

+		// init with 1<->1 blast nodes to solver nodes mapping

+		m_solverNodesData.resize(m_nodesData.size());

+		for (uint32_t i = 0; i < m_nodesData.size(); ++i)

+		{

+			m_nodesData[i].solverNode = i;

+			m_solverNodesData[i].supportNodesCount = 1;

+			m_solverNodesData[i].indexShift = 0;

+		}

+

+		// for static nodes aggregate size per graph reduction level is lower, it

+		// falls behind on few levels. (can be made as parameter)

+		const uint32_t STATIC_NODES_COUNT_PENALTY = 2 << 2;

+

+		// reducing graph by aggregating nodes level by level

+		// NOTE (@anovoselov):  Recently, I found a flow in the algorithm below. In very rare situations aggregate (solver node)

+		// can contain more then one connected component. I didn't notice it to produce any visual artifacts and it's 

+		// unlikely to influence stress solvement a lot. Possible solution is to merge *whole* solver nodes, that

+		// will raise complexity a bit (at least will add another loop on nodes for every reduction level. 

+		for (uint32_t k = 0; k < m_graphReductionLevel; k++)

+		{

+			const uint32_t maxAggregateSize = 1 << (k + 1);

+

+			for (const BondData& bond : m_bondsData)

+			{

+				NodeData& node0 = m_nodesData[bond.node0];

+				NodeData& node1 = m_nodesData[bond.node1];

+

+				if (node0.isStatic != node1.isStatic)

+					continue;

+

+				if (node0.solverNode == node1.solverNode)

+					continue;

+

+				SolverNodeData& solverNode0 = m_solverNodesData[node0.solverNode];

+				SolverNodeData& solverNode1 = m_solverNodesData[node1.solverNode];

+				

+				const int countPenalty = node0.isStatic ? STATIC_NODES_COUNT_PENALTY : 1;

+				const uint32_t aggregateSize = std::min<uint32_t>(maxAggregateSize, node0.neighborsCount / 2);

+

+				if (solverNode0.supportNodesCount * countPenalty >= aggregateSize)

+					continue;

+				if (solverNode1.supportNodesCount * countPenalty >= aggregateSize)

+					continue;

+

+				if (solverNode0.supportNodesCount >= solverNode1.supportNodesCount)

+				{

+					solverNode1.supportNodesCount--;

+					solverNode0.supportNodesCount++;

+					node1.solverNode = node0.solverNode;

+				}

+				else if (solverNode1.supportNodesCount >= solverNode0.supportNodesCount)

+				{

+					solverNode1.supportNodesCount++;

+					solverNode0.supportNodesCount--;

+					node0.solverNode = node1.solverNode;

+				}

+			}

+		}

+

+		// Solver Nodes now sparse, a lot of empty ones. Rearrange them by moving all non-empty to the front

+		// 2 passes used for that

+		{

+			uint32_t currentNode = 0;

+			for (; currentNode < m_solverNodesData.size(); ++currentNode)

+			{

+				if (m_solverNodesData[currentNode].supportNodesCount > 0)

+					continue;

+

+				// 'currentNode' is free

+

+				// search next occupied node

+				uint32_t k = currentNode + 1;

+				for (; k < m_solverNodesData.size(); ++k)

+				{

+					if (m_solverNodesData[k].supportNodesCount > 0)

+					{

+						// replace currentNode and keep indexShift

+						m_solverNodesData[currentNode].supportNodesCount = m_solverNodesData[k].supportNodesCount;

+						m_solverNodesData[k].indexShift = k - currentNode;

+						m_solverNodesData[k].supportNodesCount = 0;

+						break;

+					}

+				}

+

+				if (k == m_solverNodesData.size())

+				{

+					break;

+				}

+			}

+			for (auto& node : m_nodesData)

+			{

+				node.solverNode -= m_solverNodesData[node.solverNode].indexShift;

+			}

+

+			// now, we know total solver nodes count and which nodes are aggregated into them

+			m_solverNodesData.resize(currentNode);

+		}

+

+

+		// calculate all needed data

+		for (SolverNodeData& solverNode : m_solverNodesData)

+		{

+			solverNode.supportNodesCount = 0;

+			solverNode.localPos = PxVec3(PxZero);

+			solverNode.mass = 0.0f;

+			solverNode.volume = 0.0f;

+			solverNode.isStatic = false;

+		}

+

+		for (NodeData& node : m_nodesData)

+		{

+			SolverNodeData& solverNode = m_solverNodesData[node.solverNode];

+			solverNode.supportNodesCount++;

+			solverNode.localPos += node.localPos;

+			solverNode.mass += node.mass;

+			solverNode.volume += node.volume;

+			solverNode.isStatic |= node.isStatic;

+		}

+

+		for (SolverNodeData& solverNode : m_solverNodesData)

+		{

+			solverNode.localPos /= (float)solverNode.supportNodesCount;

+		}

+

+		m_solver.reset(m_solverNodesData.size());

+		for (uint32_t nodeIndex = 0; nodeIndex < m_solverNodesData.size(); ++nodeIndex)

+		{

+			const SolverNodeData& solverNode = m_solverNodesData[nodeIndex];

+

+			const float invMass = solverNode.isStatic ? 0.0f : 1.0f / solverNode.mass;

+			const float R = PxPow(solverNode.volume * 3.0f * PxInvPi / 4.0f, 1.0f / 3.0f); // sphere volume approximation

+			const float invI = invMass / (R * R * 0.4f); // sphere inertia tensor approximation: I = 2/5 * M * R^2 ; invI = 1 / I;

+			m_solver.setNodeMassInfo(nodeIndex, invMass, invI);

+		}

+

+		m_nodesDirty = false;

+

+		syncBonds();

+	}

+

+	void syncBonds()

+	{

+		// traverse all blast bonds and aggregate

+		m_solver.clearBonds();

+		m_solverBondsMap.clear();

+		m_solverBondsData.clear();

+		for (BondData& bond : m_bondsData)

+		{

+			const NodeData& node0 = m_nodesData[bond.node0];

+			const NodeData& node1 = m_nodesData[bond.node1];

+

+			// reset stress, bond structure changed and internal bonds stress won't be updated during updateBondStress()

+			bond.stress = 0.0f;

+

+			if (node0.solverNode == node1.solverNode)

+				continue; // skip (internal)

+

+			if (node0.isStatic && node1.isStatic)

+				continue;

+

+			BondKey key(node0.solverNode, node1.solverNode);

+			auto entry = m_solverBondsMap.find(key);

+			SolverBondData* data;

+			if (!entry)

+			{

+				m_solverBondsData.pushBack(SolverBondData());

+				data = &m_solverBondsData.back();

+				m_solverBondsMap[key] = m_solverBondsData.size() - 1;

+

+				SolverNodeData& solverNode0 = m_solverNodesData[node0.solverNode];

+				SolverNodeData& solverNode1 = m_solverNodesData[node1.solverNode];

+				m_solver.addBond(node0.solverNode, node1.solverNode, (solverNode1.localPos - solverNode0.localPos) * 0.5f);

+			}

+			else

+			{

+				data = &m_solverBondsData[entry->second];

+			}

+			data->blastBondIndices.pushBack(bond.blastBondIndex);

+		}

+

+		m_bondsDirty = false;

+	}

+

+#if GRAPH_INTERGRIRY_CHECK

+	void checkGraphIntegrity()

+	{

+		NVBLAST_ASSERT(m_solver.getBondCount() == m_solverBondsData.size());

+		NVBLAST_ASSERT(m_solver.getNodeCount() == m_solverNodesData.size());

+

+		std::set<uint64_t> solverBonds;

+		for (uint32_t i = 0; i < m_solverBondsData.size(); ++i)

+		{

+			const auto& bondData = m_solver.getBondData(i);

+			BondKey key(bondData.node0, bondData.node1);

+			NVBLAST_ASSERT(solverBonds.find(key) == solverBonds.end());

+			solverBonds.emplace(key);

+			auto entry = m_solverBondsMap.find(key);

+			NVBLAST_ASSERT(entry != nullptr);

+			const auto& solverBond = m_solverBondsData[entry->second];

+			for (auto& blastBondIndex : solverBond.blastBondIndices)

+			{

+				if (!isInvalidIndex(m_blastBondIndexMap[blastBondIndex]))

+				{

+					auto& b = m_bondsData[m_blastBondIndexMap[blastBondIndex]];

+					BondKey key2(m_nodesData[b.node0].solverNode, m_nodesData[b.node1].solverNode);

+					NVBLAST_ASSERT(key2 == key);

+				}

+			}

+		}

+

+		for (auto& solverBond : m_solverBondsData)

+		{

+			for (auto& blastBondIndex : solverBond.blastBondIndices)

+			{

+				if (!isInvalidIndex(m_blastBondIndexMap[blastBondIndex]))

+				{

+					auto& b = m_bondsData[m_blastBondIndexMap[blastBondIndex]];

+					NVBLAST_ASSERT(m_nodesData[b.node0].solverNode != m_nodesData[b.node1].solverNode);

+				}

+			}

+		}

+		uint32_t mappedBondCount = 0;

+		for (uint32_t i = 0; i < m_blastBondIndexMap.size(); i++)

+		{

+			const auto& bondIndex = m_blastBondIndexMap[i];

+			if (!isInvalidIndex(bondIndex))

+			{

+				mappedBondCount++;

+				NVBLAST_ASSERT(m_bondsData[bondIndex].blastBondIndex == i);

+			}

+		}

+		NVBLAST_ASSERT(m_bondsData.size() == mappedBondCount);

+	}

+#endif

+

+	struct BondKey

+	{

+		uint32_t node0;

+		uint32_t node1;

+

+		BondKey(uint32_t n0, uint32_t n1)

+		{

+			node0 = n0 < n1 ? n0 : n1;

+			node1 = n0 < n1 ? n1 : n0;

+		}

+

+		operator uint64_t() const

+		{

+			return static_cast<uint64_t>(node0) + (static_cast<uint64_t>(node1) << 32);

+		}

+	};

+

+	SequentialImpulseSolver			    m_solver;

+	Array<SolverNodeData>::type			m_solverNodesData;

+	Array<SolverBondData>::type			m_solverBondsData;

+

+	uint32_t							m_graphReductionLevel;

+

+	bool	                            m_nodesDirty;

+	bool	                            m_bondsDirty;

+

+	uint32_t							m_overstressedBondCount;

+

+	HashMap<BondKey, uint32_t>::type	m_solverBondsMap;

+	Array<uint32_t>::type				m_blastBondIndexMap;

+

+	Array<BondData>::type				m_bondsData;

+	Array<NodeData>::type				m_nodesData;

+};

+

+

+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

+//											 ExtStressSolver

+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

+

+struct ExtStressNodeCachedData

+{

+	physx::PxVec3 localPos;

+	bool isStatic;

+};

+

+

+struct ExtStressBondCachedData

+{

+	uint32_t bondIndex;

+};

+

+/**

+*/

+class ExtStressSolverImpl final : public ExtStressSolver

+{

+	NV_NOCOPY(ExtStressSolverImpl)

+

+public:

+	ExtStressSolverImpl(NvBlastFamily& family, ExtStressSolverSettings settings);

+	virtual void							release() override;

+

+

+	//////// ExtStressSolverImpl interface ////////

+

+	virtual void							setAllNodesInfoFromLL(float density = 1.0f) override;

+

+	virtual void							setNodeInfo(uint32_t graphNode, float mass, float volume, PxVec3 localPos, bool isStatic) override;

+

+	virtual void							setSettings(const ExtStressSolverSettings& settings) override

+	{

+		m_settings = settings;

+	}

+

+	virtual const ExtStressSolverSettings&	getSettings() const override

+	{

+		return m_settings;

+	}

+

+	virtual bool							addForce(const NvBlastActor& actor, physx::PxVec3 localPosition, physx::PxVec3 localForce, ExtForceMode::Enum mode) override;

+

+	virtual void							addForce(uint32_t graphNode, physx::PxVec3 localForce, ExtForceMode::Enum mode) override;

+

+	virtual bool							addGravityForce(const NvBlastActor& actor, physx::PxVec3 localGravity) override;

+	virtual bool							addAngularVelocity(const NvBlastActor& actor, PxVec3 localCenterMass, physx::PxVec3 localAngularVelocity) override;

+

+	virtual void							update() override;

+

+	virtual uint32_t						getOverstressedBondCount() const override

+	{

+		return m_graphProcessor->getOverstressedBondCount();

+	}

+

+	virtual void							generateFractureCommands(const NvBlastActor& actor, NvBlastFractureBuffers& commands) override;

+	virtual void							generateFractureCommands(NvBlastFractureBuffers& commands) override;

+	virtual uint32_t						generateFractureCommandsPerActor(const NvBlastActor** actorBuffer, NvBlastFractureBuffers* commandsBuffer, uint32_t bufferSize) override;

+

+

+	void									reset() override

+	{

+		m_reset = true;

+	}

+

+	virtual float							getStressErrorLinear() const override

+	{

+		return m_errorLinear;

+	}

+

+	virtual float							getStressErrorAngular() const override

+	{

+		return m_errorAngular;

+	}

+

+	virtual uint32_t						getFrameCount() const override

+	{

+		return m_framesCount;

+	}

+

+	virtual uint32_t						getBondCount() const override

+	{

+		return m_graphProcessor->getSolverBondCount();

+	}

+

+	virtual bool							notifyActorCreated(const NvBlastActor& actor) override;

+

+	virtual void							notifyActorDestroyed(const NvBlastActor& actor) override;

+

+	virtual const DebugBuffer				fillDebugRender(const uint32_t* nodes, uint32_t nodeCount, DebugRenderMode mode, float scale) override;

+

+

+private:

+	~ExtStressSolverImpl();

+

+

+	//////// private methods ////////

+

+	void									solve();

+

+	void									fillFractureCommands(const NvBlastActor& actor, NvBlastFractureBuffers& commands);

+

+	void									initialize();

+

+	void									iterate();

+

+	void									syncSolver();

+

+	template<class T>

+	T*										getScratchArray(uint32_t size);

+

+

+	//////// data ////////

+

+	struct ImpulseData

+	{

+		physx::PxVec3 position;

+		physx::PxVec3 impulse;

+	};

+

+	NvBlastFamily&														m_family;

+	HashSet<const NvBlastActor*>::type									m_activeActors;

+	ExtStressSolverSettings												m_settings;

+	NvBlastSupportGraph													m_graph;

+	bool																m_isDirty;

+	bool																m_reset;

+	const float*														m_bondHealths;

+	SupportGraphProcessor*												m_graphProcessor;

+	float																m_errorAngular;

+	float																m_errorLinear;

+	uint32_t															m_framesCount;

+	Array<NvBlastBondFractureData>::type								m_bondFractureBuffer;

+	Array<uint8_t>::type												m_scratch;

+	Array<DebugLine>::type												m_debugLineBuffer;

+};

+

+

+template<class T>

+NV_INLINE T* ExtStressSolverImpl::getScratchArray(uint32_t size)

+{

+	const uint32_t scratchSize = sizeof(T) * size;

+	if (m_scratch.size() < scratchSize)

+	{

+		m_scratch.resize(scratchSize);

+	}

+	return reinterpret_cast<T*>(m_scratch.begin());

+}

+

+

+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

+//													Creation

+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

+

+ExtStressSolverImpl::ExtStressSolverImpl(NvBlastFamily& family, ExtStressSolverSettings settings)

+	: m_family(family), m_settings(settings), m_isDirty(false), m_reset(false), 

+	m_errorAngular(std::numeric_limits<float>::max()), m_errorLinear(std::numeric_limits<float>::max()), m_framesCount(0)

+{

+	const NvBlastAsset* asset = NvBlastFamilyGetAsset(&m_family, logLL);

+	NVBLAST_ASSERT(asset);

+

+	m_graph = NvBlastAssetGetSupportGraph(asset, logLL);

+	const uint32_t bondCount = NvBlastAssetGetBondCount(asset, logLL);

+

+	m_bondFractureBuffer.reserve(bondCount);

+

+	{

+		NvBlastActor* actor;

+		NvBlastFamilyGetActors(&actor, 1, &family, logLL);

+		m_bondHealths = NvBlastActorGetBondHealths(actor, logLL);

+	}

+

+	m_graphProcessor = NVBLAST_NEW(SupportGraphProcessor)(m_graph.nodeCount, bondCount);

+

+	// traverse graph and fill bond info

+	for (uint32_t node0 = 0; node0 < m_graph.nodeCount; ++node0)

+	{

+		for (uint32_t adjacencyIndex = m_graph.adjacencyPartition[node0]; adjacencyIndex < m_graph.adjacencyPartition[node0 + 1]; adjacencyIndex++)

+		{

+			uint32_t bondIndex = m_graph.adjacentBondIndices[adjacencyIndex];

+			if (m_bondHealths[bondIndex] <= 0.0f)

+				continue;

+			uint32_t node1 = m_graph.adjacentNodeIndices[adjacencyIndex];

+

+			if (node0 < node1)

+			{

+				m_graphProcessor->addBond(node0, node1, bondIndex);

+			}

+		}

+	}

+}

+

+ExtStressSolverImpl::~ExtStressSolverImpl()

+{

+	NVBLAST_DELETE(m_graphProcessor, SupportGraphProcessor);

+}

+

+ExtStressSolver* ExtStressSolver::create(NvBlastFamily& family, ExtStressSolverSettings settings)

+{

+	return NVBLAST_NEW(ExtStressSolverImpl) (family, settings);

+}

+

+void ExtStressSolverImpl::release()

+{

+	NVBLAST_DELETE(this, ExtStressSolverImpl);

+}

+

+

+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

+//											Actors & Graph Data

+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

+

+void ExtStressSolverImpl::setAllNodesInfoFromLL(float density)

+{

+	const NvBlastAsset* asset = NvBlastFamilyGetAsset(&m_family, logLL);

+	NVBLAST_ASSERT(asset);

+

+	const uint32_t chunkCount = NvBlastAssetGetChunkCount(asset, logLL);

+	const NvBlastChunk* chunks = NvBlastAssetGetChunks(asset, logLL);

+

+	// traverse graph and fill node info

+	for (uint32_t node0 = 0; node0 < m_graph.nodeCount; ++node0)

+	{

+		const uint32_t chunkIndex0 = m_graph.chunkIndices[node0];

+		if (chunkIndex0 >= chunkCount)

+		{

+			// chunkIndex is invalid means it is static node (represents world)

+			m_graphProcessor->setNodeInfo(node0, 0.0f, 0.0f, PxVec3(), true);

+		}

+		else

+		{

+			// Check if node is static. There is at maximum only one static node in LL that represents world, but we consider all nodes 

+			// connected to it directly to be static too. It's better for general stress solver quality to have more then 1 static node.

+			bool isNodeConnectedToStatic = false;

+			for (uint32_t adjacencyIndex = m_graph.adjacencyPartition[node0]; adjacencyIndex < m_graph.adjacencyPartition[node0 + 1]; adjacencyIndex++)

+			{

+				uint32_t bondIndex = m_graph.adjacentBondIndices[adjacencyIndex];

+				if (m_bondHealths[bondIndex] <= 0.0f)

+					continue;

+				uint32_t node1 = m_graph.adjacentNodeIndices[adjacencyIndex];

+				uint32_t chunkIndex1 = m_graph.chunkIndices[node1];

+				if (chunkIndex1 >= chunkCount)

+				{

+					isNodeConnectedToStatic = true;

+					break;

+				}

+			}

+

+			// fill node info

+			const NvBlastChunk& chunk = chunks[chunkIndex0];

+			const float volume = chunk.volume;

+			const float mass = volume * density;

+			const PxVec3 localPos = *reinterpret_cast<const PxVec3*>(chunk.centroid);

+			m_graphProcessor->setNodeInfo(node0, mass, volume, localPos, isNodeConnectedToStatic);

+		}

+	}

+}

+

+void ExtStressSolverImpl::setNodeInfo(uint32_t graphNode, float mass, float volume, PxVec3 localPos, bool isStatic)

+{

+	m_graphProcessor->setNodeInfo(graphNode, mass, volume, localPos, isStatic);

+}

+

+bool ExtStressSolverImpl::notifyActorCreated(const NvBlastActor& actor)

+{

+	const uint32_t graphNodeCount = NvBlastActorGetGraphNodeCount(&actor, logLL);

+	if (graphNodeCount > 1)

+	{

+		// update neighbors

+		{

+			uint32_t* graphNodeIndices = getScratchArray<uint32_t>(graphNodeCount);

+			const uint32_t nodeCount = NvBlastActorGetGraphNodeIndices(graphNodeIndices, graphNodeCount, &actor, logLL);

+			for (uint32_t i = 0; i < nodeCount; ++i)

+			{

+				m_graphProcessor->setNodeNeighborsCount(graphNodeIndices[i], nodeCount);

+			}

+		}

+

+		m_activeActors.insert(&actor);

+		m_isDirty = true;

+		return true;

+	}

+	return false;

+}

+

+void ExtStressSolverImpl::notifyActorDestroyed(const NvBlastActor& actor)

+{

+	if (m_activeActors.erase(&actor))

+	{

+		m_isDirty = true;

+	}

+}

+

+void ExtStressSolverImpl::syncSolver()

+{

+	// traverse graph and remove dead bonds

+	for (uint32_t node0 = 0; node0 < m_graph.nodeCount; ++node0)

+	{

+		for (uint32_t adjacencyIndex = m_graph.adjacencyPartition[node0]; adjacencyIndex < m_graph.adjacencyPartition[node0 + 1]; adjacencyIndex++)

+		{

+			uint32_t node1 = m_graph.adjacentNodeIndices[adjacencyIndex];

+			if (node0 < node1)

+			{

+				uint32_t bondIndex = m_graph.adjacentBondIndices[adjacencyIndex];

+

+				if (m_bondHealths[bondIndex] <= 0.0f)

+				{

+					m_graphProcessor->removeBondIfExists(bondIndex);

+				}

+			}

+		}

+	}

+

+	m_isDirty = false;

+}

+

+void ExtStressSolverImpl::initialize()

+{

+	if (m_reset)

+	{

+		m_framesCount = 0;

+	}

+

+	if (m_isDirty)

+	{

+		syncSolver();

+	}

+

+	if (m_settings.graphReductionLevel != m_graphProcessor->getGraphReductionLevel())

+	{

+		m_graphProcessor->setGraphReductionLevel(m_settings.graphReductionLevel);

+	}

+}

+

+bool ExtStressSolverImpl::addForce(const NvBlastActor& actor, physx::PxVec3 localPosition, physx::PxVec3 localForce, ExtForceMode::Enum mode)

+{

+	float bestDist = FLT_MAX;

+	uint32_t bestNode = invalidIndex<uint32_t>();

+

+	const uint32_t graphNodeCount = NvBlastActorGetGraphNodeCount(&actor, logLL);

+	if (graphNodeCount > 1)

+	{

+		uint32_t* graphNodeIndices = getScratchArray<uint32_t>(graphNodeCount);

+		const uint32_t nodeCount = NvBlastActorGetGraphNodeIndices(graphNodeIndices, graphNodeCount, &actor, logLL);

+

+		for (uint32_t i = 0; i < nodeCount; ++i)

+		{

+			const uint32_t node = graphNodeIndices[i];

+			const float sqrDist = (localPosition - m_graphProcessor->getNodeData(node).localPos).magnitudeSquared();

+			if (sqrDist < bestDist)

+			{

+				bestDist = sqrDist;

+				bestNode = node;

+			}

+		}

+

+		if (!isInvalidIndex(bestNode))

+		{

+			m_graphProcessor->addNodeForce(bestNode, localForce, mode);

+			return true;

+		}

+	}

+	return false;

+}

+

+void ExtStressSolverImpl::addForce(uint32_t graphNode, physx::PxVec3 localForce, ExtForceMode::Enum mode)

+{

+	m_graphProcessor->addNodeForce(graphNode, localForce, mode);

+}

+

+bool ExtStressSolverImpl::addGravityForce(const NvBlastActor& actor, physx::PxVec3 localGravity)

+{

+	const uint32_t graphNodeCount = NvBlastActorGetGraphNodeCount(&actor, logLL);

+	if (graphNodeCount > 1)

+	{

+		uint32_t* graphNodeIndices = getScratchArray<uint32_t>(graphNodeCount);

+		const uint32_t nodeCount = NvBlastActorGetGraphNodeIndices(graphNodeIndices, graphNodeCount, &actor, logLL);

+

+		for (uint32_t i = 0; i < nodeCount; ++i)

+		{

+			const uint32_t node = graphNodeIndices[i];

+			m_graphProcessor->addNodeVelocity(node, localGravity);

+		}

+		return true;

+	}

+	return false;

+}

+

+bool ExtStressSolverImpl::addAngularVelocity(const NvBlastActor& actor, PxVec3 localCenterMass, physx::PxVec3 localAngularVelocity)

+{

+	const uint32_t graphNodeCount = NvBlastActorGetGraphNodeCount(&actor, logLL);

+	if (graphNodeCount > 1)

+	{

+		uint32_t* graphNodeIndices = getScratchArray<uint32_t>(graphNodeCount);

+		const uint32_t nodeCount = NvBlastActorGetGraphNodeIndices(graphNodeIndices, graphNodeCount, &actor, logLL);

+

+		// Apply centrifugal force

+		for (uint32_t i = 0; i < nodeCount; ++i)

+		{

+			const uint32_t node = graphNodeIndices[i];

+			const auto& localPos = m_graphProcessor->getNodeData(node).localPos;

+			// a = w x (w x r)

+			const PxVec3 centrifugalAcceleration = localAngularVelocity.cross(localAngularVelocity.cross(localPos - localCenterMass));

+			m_graphProcessor->addNodeVelocity(node, centrifugalAcceleration);

+		}

+		return true;

+	}

+	return false;

+}

+

+

+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

+//													Update

+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

+

+void ExtStressSolverImpl::update()

+{

+	initialize();

+

+	solve();

+

+	m_framesCount++;

+}

+

+void ExtStressSolverImpl::solve()

+{

+	PX_SIMD_GUARD;

+

+	m_graphProcessor->solve(m_settings, m_bondHealths, WARM_START && !m_reset);

+	m_reset = false;

+

+	m_graphProcessor->calcError(m_errorLinear, m_errorAngular);

+}

+

+

+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

+//													Damage

+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

+

+void ExtStressSolverImpl::fillFractureCommands(const NvBlastActor& actor, NvBlastFractureBuffers& commands)

+{

+	const uint32_t graphNodeCount = NvBlastActorGetGraphNodeCount(&actor, logLL);

+	uint32_t commandCount = 0;

+

+	if (graphNodeCount > 1 && m_graphProcessor->getOverstressedBondCount() > 0)

+	{

+		uint32_t* graphNodeIndices = getScratchArray<uint32_t>(graphNodeCount);

+		const uint32_t nodeCount = NvBlastActorGetGraphNodeIndices(graphNodeIndices, graphNodeCount, &actor, logLL);

+

+		for (uint32_t i = 0; i < nodeCount; ++i)

+		{

+			const uint32_t node0 = graphNodeIndices[i];

+			for (uint32_t adjacencyIndex = m_graph.adjacencyPartition[node0]; adjacencyIndex < m_graph.adjacencyPartition[node0 + 1]; adjacencyIndex++)

+			{

+				uint32_t node1 = m_graph.adjacentNodeIndices[adjacencyIndex];

+				if (node0 < node1)

+				{

+					uint32_t bondIndex = m_graph.adjacentBondIndices[adjacencyIndex];

+					const float bondHealth = m_bondHealths[bondIndex];

+					const float bondStress = m_graphProcessor->getBondStress(bondIndex);

+

+					if (bondHealth > 0.0f && bondStress > bondHealth)

+					{

+						const NvBlastBondFractureData data = {

+							0,

+							node0,

+							node1,

+							bondHealth

+						};

+						m_bondFractureBuffer.pushBack(data);

+						commandCount++;

+					}

+				}

+			}

+		}

+	}

+

+	commands.chunkFractureCount = 0;

+	commands.chunkFractures = nullptr;

+	commands.bondFractureCount = commandCount;

+	commands.bondFractures = commandCount > 0 ? m_bondFractureBuffer.end() - commandCount : nullptr;

+}

+

+void ExtStressSolverImpl::generateFractureCommands(const NvBlastActor& actor, NvBlastFractureBuffers& commands)

+{

+	m_bondFractureBuffer.clear();

+	fillFractureCommands(actor, commands);

+}

+

+void ExtStressSolverImpl::generateFractureCommands(NvBlastFractureBuffers& commands)

+{

+	m_bondFractureBuffer.clear();

+

+	const uint32_t bondCount = m_graphProcessor->getBondCount();

+	const uint32_t overstressedBondCount = m_graphProcessor->getOverstressedBondCount();

+	for (uint32_t i = 0; i < bondCount && m_bondFractureBuffer.size() < overstressedBondCount; i++)

+	{

+		const auto& bondData = m_graphProcessor->getBondData(i);

+		const float bondHealth = m_bondHealths[bondData.blastBondIndex];

+		if (bondHealth > 0.0f && bondData.stress > bondHealth)

+		{

+			const NvBlastBondFractureData data = {

+				0,

+				bondData.node0,

+				bondData.node1,

+				bondHealth

+			};

+			m_bondFractureBuffer.pushBack(data);

+		}

+	}

+

+	commands.chunkFractureCount = 0;

+	commands.chunkFractures = nullptr;

+	commands.bondFractureCount = m_bondFractureBuffer.size();

+	commands.bondFractures = m_bondFractureBuffer.size() > 0 ? m_bondFractureBuffer.begin() : nullptr;

+}

+

+uint32_t ExtStressSolverImpl::generateFractureCommandsPerActor(const NvBlastActor**  actorBuffer, NvBlastFractureBuffers* commandsBuffer, uint32_t bufferSize)

+{

+	if (m_graphProcessor->getOverstressedBondCount() == 0)

+		return 0;

+

+	m_bondFractureBuffer.clear();

+	uint32_t index = 0;

+	for (auto it = m_activeActors.getIterator(); !it.done() && index < bufferSize; ++it)

+	{

+		const NvBlastActor* actor = *it;

+		NvBlastFractureBuffers& nextCommand = commandsBuffer[index];

+		fillFractureCommands(*actor, nextCommand);

+		if (nextCommand.bondFractureCount > 0)

+		{

+			actorBuffer[index] = actor;

+			index++;

+		}

+	}

+	return index;

+}

+

+

+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

+//													Debug Render

+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

+

+static PxU32 PxVec4ToU32Color(const PxVec4& color)

+{

+	PxU32 c = 0;

+	c |= (int)(color.w * 255); c <<= 8;

+	c |= (int)(color.z * 255); c <<= 8;

+	c |= (int)(color.y * 255); c <<= 8;

+	c |= (int)(color.x * 255);

+	return c;

+}

+

+static PxVec4 PxVec4Lerp(const PxVec4 v0, const PxVec4 v1, float val)

+{

+	PxVec4 v(

+		v0.x * (1 - val) + v1.x * val,

+		v0.y * (1 - val) + v1.y * val,

+		v0.z * (1 - val) + v1.z * val,

+		v0.w * (1 - val) + v1.w * val

+	);

+	return v;

+}

+

+inline float clamp01(float v)

+{

+	return v < 0.0f ? 0.0f : (v > 1.0f ? 1.0f : v);

+}

+

+inline PxVec4 bondHealthColor(float healthFraction)

+{

+	healthFraction = clamp01(healthFraction);

+

+	const PxVec4 BOND_HEALTHY_COLOR(0.0f, 1.0f, 1.0f, 1.0f);

+	const PxVec4 BOND_MID_COLOR(1.0f, 1.0f, 0.0f, 1.0f);

+	const PxVec4 BOND_BROKEN_COLOR(1.0f, 0.0f, 0.0f, 1.0f);

+

+	return healthFraction < 0.5 ? PxVec4Lerp(BOND_BROKEN_COLOR, BOND_MID_COLOR, 2.0f * healthFraction) : PxVec4Lerp(BOND_MID_COLOR, BOND_HEALTHY_COLOR, 2.0f * healthFraction - 1.0f);

+}

+

+const ExtStressSolver::DebugBuffer ExtStressSolverImpl::fillDebugRender(const uint32_t* nodes, uint32_t nodeCount, DebugRenderMode mode, float scale)

+{

+	const PxVec4 BOND_IMPULSE_LINEAR_COLOR(0.0f, 1.0f, 0.0f, 1.0f);

+	const PxVec4 BOND_IMPULSE_ANGULAR_COLOR(1.0f, 0.0f, 0.0f, 1.0f);

+

+	ExtStressSolver::DebugBuffer debugBuffer = { nullptr, 0 };

+

+	if (m_isDirty)

+		return debugBuffer;

+

+	m_debugLineBuffer.clear();

+

+	Array<uint8_t>::type& nodesSet = m_scratch;

+

+	nodesSet.resize(m_graphProcessor->getSolverNodeCount());

+	memset(nodesSet.begin(), 0, nodesSet.size() * sizeof(uint8_t));

+	for (uint32_t i = 0; i < nodeCount; ++i)

+	{

+		NVBLAST_ASSERT(m_graphProcessor->getNodeData(nodes[i]).solverNode < nodesSet.size());

+		nodesSet[m_graphProcessor->getNodeData(nodes[i]).solverNode] = 1;

+	}

+

+	const uint32_t bondCount = m_graphProcessor->getSolverBondCount();

+	for (uint32_t i = 0; i < bondCount; ++i)

+	{

+		const auto& solverInternalBondData = m_graphProcessor->getSolverInternalBondData(i);

+		if (nodesSet[solverInternalBondData.node0] != 0)

+		{

+			//NVBLAST_ASSERT(nodesSet[solverInternalBondData.node1] != 0);

+			const auto& solverInternalNode0 = m_graphProcessor->getSolverInternalNodeData(solverInternalBondData.node0);

+			const auto& solverInternalNode1 = m_graphProcessor->getSolverInternalNodeData(solverInternalBondData.node1);

+			const auto& solverNode0 = m_graphProcessor->getSolverNodeData(solverInternalBondData.node0);

+			const auto& solverNode1 = m_graphProcessor->getSolverNodeData(solverInternalBondData.node1);

+

+			PxVec3 p0 = solverNode0.localPos;

+			PxVec3 p1 = solverNode1.localPos;

+			PxVec3 center = (p0 + p1) * 0.5f;

+

+			const float stress = std::min<float>(m_graphProcessor->getSolverBondStressHealth(i, m_settings), 1.0f);

+			PxVec4 color = bondHealthColor(1.0f - stress);

+

+			m_debugLineBuffer.pushBack(DebugLine(p0, p1, PxVec4ToU32Color(color)));

+

+			float impulseScale = scale;

+

+			if (mode == DebugRenderMode::STRESS_GRAPH_NODES_IMPULSES)

+			{

+				m_debugLineBuffer.pushBack(DebugLine(p0, p0 + solverInternalNode0.velocityLinear * impulseScale, PxVec4ToU32Color(BOND_IMPULSE_LINEAR_COLOR)));

+				m_debugLineBuffer.pushBack(DebugLine(p0, p0 + solverInternalNode0.velocityAngular * impulseScale, PxVec4ToU32Color(BOND_IMPULSE_ANGULAR_COLOR)));

+				m_debugLineBuffer.pushBack(DebugLine(p1, p1 + solverInternalNode1.velocityLinear * impulseScale, PxVec4ToU32Color(BOND_IMPULSE_LINEAR_COLOR)));

+				m_debugLineBuffer.pushBack(DebugLine(p1, p1 + solverInternalNode1.velocityAngular * impulseScale, PxVec4ToU32Color(BOND_IMPULSE_ANGULAR_COLOR)));

+			}

+			else if (mode == DebugRenderMode::STRESS_GRAPH_BONDS_IMPULSES)

+			{

+				m_debugLineBuffer.pushBack(DebugLine(center, center + solverInternalBondData.impulseLinear * impulseScale, PxVec4ToU32Color(BOND_IMPULSE_LINEAR_COLOR)));

+				m_debugLineBuffer.pushBack(DebugLine(center, center + solverInternalBondData.impulseAngular * impulseScale, PxVec4ToU32Color(BOND_IMPULSE_ANGULAR_COLOR)));

+			}

+		}

+	}

+

+	debugBuffer.lines = m_debugLineBuffer.begin();

+	debugBuffer.lineCount = m_debugLineBuffer.size();

+

+	return debugBuffer;

+}

+

+

+} // namespace Blast

+} // namespace Nv