// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of NVIDIA CORPORATION nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// Copyright (c) 2008-2025 NVIDIA Corporation. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  

#include "PxsIslandSim.h"
#include "foundation/PxSort.h"
#include "foundation/PxUtilities.h"
#include "common/PxProfileZone.h"

using namespace physx;
using namespace IG;

IslandSim::IslandSim(const CPUExternalData& cpuData, GPUExternalData* gpuData, PxU64 contextID) :
	mNodes					("IslandSim::mNodes"),
	mActiveNodeIndex		("IslandSim::mActiveNodeIndex"),
	mHopCounts				("IslandSim::mHopCounts"),
	mFastRoute				("IslandSim::mFastRoute"),
	mIslandIds				("IslandSim::mIslandIds"),
	mIslands				("IslandSim::mIslands"),
	mIslandStaticTouchCount	("IslandSim.activeStaticTouchCount"),
	mActiveKinematicNodes	("IslandSim::mActiveKinematicNodes"),
	mActiveIslands			("IslandSim::mActiveIslands"),
#if IG_LIMIT_DIRTY_NODES
	mLastMapIndex			(0),
#endif
	mActivatingNodes		("IslandSim::mActivatingNodes"),
	mDestroyedEdges			("IslandSim::mDestroyedEdges"),
	mVisitedNodes			("IslandSim::mVisitedNodes"),
	mCpuData				(cpuData),
	mGpuData				(gpuData),
	mContextId				(contextID)
{
	for (PxU32 i = 0; i < Edge::eEDGE_TYPE_COUNT; ++i)
	{
		mInitialActiveNodeCount[i] = 0;
		mActiveEdgeCount[i] = 0;
	}
}

#if PX_ENABLE_ASSERTS
template <typename Thing>
static bool contains(PxArray<Thing>& arr, const Thing& thing)
{
	for(PxU32 a = 0; a < arr.size(); ++a)
	{
		if(thing == arr[a])
			return true;
	}
	return false;
}
#endif

void IslandSim::addNode(bool isActive, bool isKinematic, Node::NodeType type, PxNodeIndex nodeIndex, void* object)
{
	// PT: the nodeIndex is assigned by the SimpleIslandManager one level higher.
	const PxU32 handle = nodeIndex.index();
	{
		if(handle == mNodes.capacity())
		{
			const PxU32 newCapacity = PxMax(2*mNodes.capacity(), 256u);
			mNodes.reserve(newCapacity);
			mIslandIds.reserve(newCapacity);
			mFastRoute.reserve(newCapacity);
			mHopCounts.reserve(newCapacity);
			mActiveNodeIndex.reserve(newCapacity);
		}

		const PxU32 newSize = PxMax(handle+1, mNodes.size());
		mNodes.resize(newSize);
		mIslandIds.resize(newSize);
		mFastRoute.resize(newSize);
		mHopCounts.resize(newSize);
		mActiveNodeIndex.resize(newSize);
	}

	mActiveNodeIndex[handle] = PX_INVALID_NODE;

	Node& node = mNodes[handle];
	node.mType = PxTo8(type);
	//Ensure that the node is not currently being used.
	PX_ASSERT(node.isDeleted());

	PxU8 flags = PxU8(isActive ? 0 : Node::eREADY_FOR_SLEEPING);
	if(isKinematic)
		flags |= Node::eKINEMATIC;
	node.mFlags = flags;
	mIslandIds[handle] = IG_INVALID_ISLAND;
	mFastRoute[handle].setIndices(PX_INVALID_NODE);
	mHopCounts[handle] = 0;

	if(!isKinematic)
	{
		const IslandId islandHandle = mIslandHandles.getHandle();
		
		if(islandHandle == mIslands.capacity())
		{
			const PxU32 newCapacity = PxMax(2*mIslands.capacity(), 256u);
			mIslands.reserve(newCapacity);
			mIslandAwake.resize(newCapacity);
			mIslandStaticTouchCount.reserve(newCapacity);
		}
		const PxU32 newSize = PxMax(islandHandle+1, mIslands.size());
		mIslands.resize(newSize);
		mIslandStaticTouchCount.resize(newSize);
		mIslandAwake.growAndReset(newSize);

		Island& island = mIslands[islandHandle];
		island.mLastNode = island.mRootNode = nodeIndex;
		island.mNodeCount[type] = 1;
		mIslandIds[handle] = islandHandle;
		mIslandStaticTouchCount[islandHandle] = 0;
	}

	if(isActive)
		activateNode(nodeIndex);

	node.mObject = object;
}

// PT: preallocateConnections() and addConnectionPreallocated() are used to replicate IslandSim::addConnection() multi-threaded
void IslandSim::preallocateConnections(EdgeIndex handle)
{
	if(handle >= mEdges.capacity())
	{
		PX_PROFILE_ZONE("ReserveIslandEdges", mContextId);
		const PxU32 newSize = handle + 2048;
		mEdges.reserve(newSize);
		if(mGpuData)
			mGpuData->mActiveContactEdges.resize(mEdges.capacity());
	}
	mEdges.resize(PxMax(mEdges.size(), handle+1));
	if(mGpuData)
		mGpuData->mActiveContactEdges.reset(handle);
}

bool IslandSim::addConnectionPreallocated(PxNodeIndex nodeHandle1, PxNodeIndex nodeHandle2, Edge::EdgeType edgeType, EdgeIndex handle)
{
	// PT: the EdgeIndex is assigned by the SimpleIslandManager one level higher.

	PX_UNUSED(nodeHandle1);
	PX_UNUSED(nodeHandle2);
	Edge& edge = mEdges[handle];

	if(edge.isPendingDestroyed())
	{
		//If it's in this state, then the edge has been tagged for destruction but actually is now not needed to be destroyed
		edge.clearPendingDestroyed();
		return false;
	}

	if(edge.isInDirtyList())
	{
		PX_ASSERT(mCpuData.mEdgeNodeIndices[handle * 2].index() == nodeHandle1.index());
		PX_ASSERT(mCpuData.mEdgeNodeIndices[handle * 2 + 1].index() == nodeHandle2.index());
		PX_ASSERT(edge.mEdgeType == edgeType);
		return false;
	}

	PX_ASSERT(!edge.isInserted());

	PX_ASSERT(edge.isDestroyed());
	edge.clearDestroyed();

	PX_ASSERT(edge.mNextIslandEdge == IG_INVALID_ISLAND);
	PX_ASSERT(edge.mPrevIslandEdge == IG_INVALID_ISLAND);

	PX_ASSERT(mEdgeInstances.size() <= 2*handle || mEdgeInstances[2*handle].mNextEdge == IG_INVALID_EDGE);
	PX_ASSERT(mEdgeInstances.size() <= 2*handle || mEdgeInstances[2*handle+1].mNextEdge == IG_INVALID_EDGE);
	PX_ASSERT(mEdgeInstances.size() <= 2*handle || mEdgeInstances[2*handle].mPrevEdge == IG_INVALID_EDGE);
	PX_ASSERT(mEdgeInstances.size() <= 2*handle || mEdgeInstances[2*handle+1].mPrevEdge == IG_INVALID_EDGE);

	edge.mEdgeType = PxTo16(edgeType);

	PX_ASSERT(handle*2 >= mEdgeInstances.size() || mEdgeInstances[handle*2].mNextEdge == IG_INVALID_EDGE);
	PX_ASSERT(handle*2+1 >= mEdgeInstances.size() || mEdgeInstances[handle*2+1].mNextEdge == IG_INVALID_EDGE);
	PX_ASSERT(handle*2 >= mEdgeInstances.size() || mEdgeInstances[handle*2].mPrevEdge == IG_INVALID_EDGE);
	PX_ASSERT(handle*2+1 >= mEdgeInstances.size() || mEdgeInstances[handle*2+1].mPrevEdge == IG_INVALID_EDGE);
	
	//Add the new handle
	PX_ASSERT(!edge.isInDirtyList());	// PT: otherwise it should have exited the function above
	PX_ASSERT(!contains(mDirtyEdges[edgeType], handle));
	// PT: TODO: we could push back to an array MT but that would break determinism
	//mDirtyEdges[edgeType].pushBack(handle);
	edge.markInDirtyList();

	edge.mEdgeState &= ~(Edge::eACTIVATING);
	return true;
}

void IslandSim::addConnection(PxNodeIndex nodeHandle1, PxNodeIndex nodeHandle2, Edge::EdgeType edgeType, EdgeIndex handle)
{
	// PT: the EdgeIndex is assigned by the SimpleIslandManager one level higher.

	preallocateConnections(handle);

	if(addConnectionPreallocated(nodeHandle1, nodeHandle2, edgeType, handle))
		mDirtyEdges[edgeType].pushBack(handle);
}

// PT: last part of IslandSim::addConnection, not MT in IslandSim::addConnectionPreallocated
void IslandSim::addDelayedDirtyEdges(PxU32 nbHandles, const EdgeIndex* handles)
{
	// PT: TODO: better version
	while(nbHandles--)
	{
		const EdgeIndex h = *handles++;
		const Edge& edge = mEdges[h];
		mDirtyEdges[edge.mEdgeType].pushBack(h);
	}
}

void IslandSim::addConnectionToGraph(EdgeIndex handle)
{
	const EdgeInstanceIndex instanceHandle = 2*handle;
	PX_ASSERT(instanceHandle < mEdgeInstances.capacity());
	/*if(instanceHandle == mEdgeInstances.capacity())
	{
		mEdgeInstances.reserve(2*mEdgeInstances.capacity() + 2);
	}*/
	mEdgeInstances.resize(PxMax(instanceHandle+2, mEdgeInstances.size()));

	Edge& edge = mEdges[handle];
	
	// PT: TODO: int bools
	PxIntBool activeEdge = false;
	bool kinematicKinematicEdge = true;

	const PxNodeIndex nodeIndex1 = mCpuData.mEdgeNodeIndices[instanceHandle];
	const PxNodeIndex nodeIndex2 = mCpuData.mEdgeNodeIndices[instanceHandle+1];

	struct Local
	{
		static PX_FORCE_INLINE void connectEdge(Cm::BlockArray<EdgeInstance>& edgeInstances, EdgeInstanceIndex edgeIndex, Node& source)
		{
			EdgeInstance& instance = edgeInstances[edgeIndex];

			PX_ASSERT(instance.mNextEdge == IG_INVALID_EDGE);
			PX_ASSERT(instance.mPrevEdge == IG_INVALID_EDGE);

			instance.mNextEdge = source.mFirstEdgeIndex;
			if(source.mFirstEdgeIndex != IG_INVALID_EDGE)
			{
				EdgeInstance& firstEdge = edgeInstances[source.mFirstEdgeIndex];
				firstEdge.mPrevEdge = edgeIndex;
			}

			source.mFirstEdgeIndex = edgeIndex;
			instance.mPrevEdge = IG_INVALID_EDGE;
		}
	};

	const PxU32 index1 = nodeIndex1.index();
	if(index1 != PX_INVALID_NODE)
	{
		Node& node = mNodes[index1];
		Local::connectEdge(mEdgeInstances, instanceHandle, node);
		activeEdge = node.isActiveOrActivating();
		kinematicKinematicEdge = node.isKinematic();
	}

	const PxU32 index2 = nodeIndex2.index();
	if(index1 != index2 && index2 != PX_INVALID_NODE)
	{
		Node& node = mNodes[index2];
		Local::connectEdge(mEdgeInstances, instanceHandle + 1, node);
		activeEdge |= node.isActiveOrActivating();
		kinematicKinematicEdge = kinematicKinematicEdge && node.isKinematic();
	}

	if(activeEdge && (!kinematicKinematicEdge || edge.getEdgeType() == IG::Edge::eCONTACT_MANAGER))
	{				
		markEdgeActive(handle, nodeIndex1, nodeIndex2);
		edge.activateEdge();
	}
}

void IslandSim::removeConnectionFromGraph(EdgeIndex edgeIndex)
{
	const PxNodeIndex nodeIndex1 = mCpuData.mEdgeNodeIndices[2 * edgeIndex];
	const PxNodeIndex nodeIndex2 = mCpuData.mEdgeNodeIndices[2 * edgeIndex + 1];

	const PxU32 index1 = nodeIndex1.index();
	const PxU32 index2 = nodeIndex2.index();

	if (index1 != PX_INVALID_NODE)
	{
		Node& node = mNodes[index1];
		if (index2 == mFastRoute[index1].index())
			mFastRoute[index1].setIndices(PX_INVALID_NODE);
		if(!node.isDirty())
		{
			//mDirtyNodes.pushBack(nodeIndex1);
			mDirtyMap.growAndSet(index1);
			node.markDirty();
		}
	}

	if (index2 != PX_INVALID_NODE)
	{
		Node& node = mNodes[index2];
		if (index1 == mFastRoute[index2].index())
			mFastRoute[index2].setIndices(PX_INVALID_NODE);
		if(!node.isDirty())
		{
			mDirtyMap.growAndSet(index2);
			node.markDirty();
		}
	}
}

void IslandSim::removeConnection(EdgeIndex edgeIndex)
{
	Edge& edge = mEdges[edgeIndex];
	if(!edge.isPendingDestroyed())// && edge.isInserted())
	{
		mDestroyedEdges.pushBack(edgeIndex);
		/*if(!edge.isInserted())
			edge.setReportOnlyDestroy();*/
	}
	edge.setPendingDestroyed();
}

void IslandSim::removeConnectionInternal(EdgeIndex edgeIndex)
{
	PX_ASSERT(edgeIndex != IG_INVALID_EDGE);
	const EdgeInstanceIndex edgeInstanceBase = edgeIndex*2;
	const PxNodeIndex nodeIndex1 = mCpuData.mEdgeNodeIndices[edgeIndex * 2];
	const PxNodeIndex nodeIndex2 = mCpuData.mEdgeNodeIndices[edgeIndex * 2 + 1];

	struct Local
	{
		static void disconnectEdge(Cm::BlockArray<EdgeInstance>& edgeInstances, EdgeInstanceIndex edgeIndex, Node& node)
		{
			EdgeInstance& instance = edgeInstances[edgeIndex];

			PX_ASSERT(instance.mNextEdge == IG_INVALID_EDGE || edgeInstances[instance.mNextEdge].mPrevEdge == edgeIndex);
			PX_ASSERT(instance.mPrevEdge == IG_INVALID_EDGE || edgeInstances[instance.mPrevEdge].mNextEdge == edgeIndex);

			if(node.mFirstEdgeIndex == edgeIndex)
			{
				PX_ASSERT(instance.mPrevEdge == IG_INVALID_EDGE);
				node.mFirstEdgeIndex = instance.mNextEdge;
			}
			else
			{
				EdgeInstance& prev = edgeInstances[instance.mPrevEdge];
				PX_ASSERT(prev.mNextEdge == edgeIndex);
				prev.mNextEdge = instance.mNextEdge;
			}

			if(instance.mNextEdge != IG_INVALID_EDGE)
			{
				EdgeInstance& next = edgeInstances[instance.mNextEdge];
				PX_ASSERT(next.mPrevEdge == edgeIndex);
				next.mPrevEdge = instance.mPrevEdge;
			}

			PX_ASSERT(instance.mNextEdge == IG_INVALID_EDGE || edgeInstances[instance.mNextEdge].mPrevEdge == instance.mPrevEdge);
			PX_ASSERT(instance.mPrevEdge == IG_INVALID_EDGE || edgeInstances[instance.mPrevEdge].mNextEdge == instance.mNextEdge);

			instance.mNextEdge = IG_INVALID_EDGE;
			instance.mPrevEdge = IG_INVALID_EDGE;
		}
	};

	const PxU32 index1 = nodeIndex1.index();
	const PxU32 index2 = nodeIndex2.index();

	if (index1 != PX_INVALID_NODE)
		Local::disconnectEdge(mEdgeInstances, edgeInstanceBase, mNodes[index1]);

	if (index2 != PX_INVALID_NODE && index1 != index2)
		Local::disconnectEdge(mEdgeInstances, edgeInstanceBase + 1, mNodes[index2]);
}

void IslandSim::activateNode(PxNodeIndex nodeIndex)
{
	const PxU32 index = nodeIndex.index();
	if(index != PX_INVALID_NODE)
	{
		Node& node = mNodes[index];

		if(!node.isActiveOrActivating())
		{
			//If the node is kinematic and already in the active node list, then we need to remove it
			//from the active kinematic node list, then re-add it after the wake-up. It's a bit dumb
			//but it means that we don't need another index

			if(node.isKinematic() && mActiveNodeIndex[index] != PX_INVALID_NODE)
			{
				//node.setActive();
				//node.clearIsReadyForSleeping(); //Clear the "isReadyForSleeping" flag. Just in case it was set
				//return;

				const PxU32 activeRefCount = node.mActiveRefCount;
				node.mActiveRefCount = 0;
				node.clearActive();
				markKinematicInactive(nodeIndex);
				node.mActiveRefCount = activeRefCount;
			}
			
			node.setActivating(); //Tag it as activating
			PX_ASSERT(mActiveNodeIndex[index] == PX_INVALID_NODE);
			mActiveNodeIndex[index] = mActivatingNodes.size();	
			//Add to waking list
			mActivatingNodes.pushBack(nodeIndex);
		}
		node.clearIsReadyForSleeping(); //Clear the "isReadyForSleeping" flag. Just in case it was set
	}
}

void IslandSim::deactivateNode(PxNodeIndex nodeIndex)
{
	const PxU32 index = nodeIndex.index();
	if(index != PX_INVALID_NODE)
	{
		Node& node = mNodes[index];

		//If the node is activating, clear its activating state and remove it from the activating list. 
		//If it wasn't already activating, then it's probably already in the active list

		const PxIntBool wasActivating = node.isActivating();

		if(wasActivating)
		{
			//Already activating, so remove it from the activating list
			node.clearActivating();
			PX_ASSERT(mActivatingNodes[mActiveNodeIndex[index]].index() == index);
			const PxNodeIndex replaceIndex = mActivatingNodes[mActivatingNodes.size()-1];
			mActiveNodeIndex[replaceIndex.index()] = mActiveNodeIndex[index];
			mActivatingNodes[mActiveNodeIndex[index]] = replaceIndex;
			mActivatingNodes.forceSize_Unsafe(mActivatingNodes.size()-1);
			mActiveNodeIndex[index] = PX_INVALID_NODE;

			if(node.isKinematic())
			{
				//If we were temporarily removed from the active kinematic list to be put in the waking kinematic list
				//then add the node back in before deactivating the node. This is a bit counter-intuitive but the active
				//kinematic list contains all active kinematics and all kinematics that are referenced by an active constraint
				PX_ASSERT(mActiveNodeIndex[index] == PX_INVALID_NODE);
				mActiveNodeIndex[index] = mActiveKinematicNodes.size();
				mActiveKinematicNodes.pushBack(nodeIndex);
			}
		}

		//Raise the "ready for sleeping" flag so that island gen can put this node to sleep
		node.setIsReadyForSleeping();
	}
}

void IslandSim::putNodeToSleep(PxNodeIndex nodeIndex)
{
	if(nodeIndex.index() != PX_INVALID_NODE)
		deactivateNode(nodeIndex);
}

PX_FORCE_INLINE void IslandSim::makeEdgeActive(EdgeInstanceIndex index, bool testEdgeType)
{
	const EdgeIndex idx = index / 2;
	Edge& edge = mEdges[idx];
	if (!edge.isActive() && (!testEdgeType || (edge.getEdgeType() != IG::Edge::eCONSTRAINT)))
	{
		//Make the edge active...
		const PxNodeIndex nodeIndex1 = mCpuData.mEdgeNodeIndices[idx * 2];
		const PxNodeIndex nodeIndex2 = mCpuData.mEdgeNodeIndices[idx * 2 + 1];

		PX_ASSERT(nodeIndex1.index() == PX_INVALID_NODE || !mNodes[nodeIndex1.index()].isActive() || mNodes[nodeIndex1.index()].isKinematic());
		PX_ASSERT(nodeIndex2.index() == PX_INVALID_NODE || !mNodes[nodeIndex2.index()].isActive() || mNodes[nodeIndex2.index()].isKinematic());

		markEdgeActive(idx, nodeIndex1, nodeIndex2);
		edge.activateEdge();
	}
}

void IslandSim::activateNodeInternal(PxNodeIndex nodeIndex)
{
	//This method should activate the node, then activate all the connections involving this node
	Node& node = mNodes[nodeIndex.index()];

	if(!node.isActive())
	{
		PX_ASSERT(mActiveNodeIndex[nodeIndex.index()] == PX_INVALID_NODE);

		//Activate all the edges + nodes...

		EdgeInstanceIndex index = node.mFirstEdgeIndex;

		while(index != IG_INVALID_EDGE)
		{
			makeEdgeActive(index, false);

			index = mEdgeInstances[index].mNextEdge;
		}

		if(node.isKinematic())
			markKinematicActive(nodeIndex);
		else
			markActive(nodeIndex);

		node.setActive();
	}
}

void IslandSim::deactivateNodeInternal(PxNodeIndex nodeIndex)
{
	//We deactivate a node, we need to loop through all the edges and deactivate them *if* both bodies are asleep

	Node& node = mNodes[nodeIndex.index()];

	if(node.isActive())
	{
		if(node.isKinematic())
			markKinematicInactive(nodeIndex);
		else
			markInactive(nodeIndex);

		//Clear the active status flag
		node.clearActive();
		node.clearActivating();

		EdgeInstanceIndex index = node.mFirstEdgeIndex;

		while(index != IG_INVALID_EDGE)
		{
			const EdgeInstance& instance = mEdgeInstances[index];

			const PxNodeIndex outboundNode = mCpuData.mEdgeNodeIndices[index ^ 1];
			if(outboundNode.index() == PX_INVALID_NODE || 
				!mNodes[outboundNode.index()].isActive())
			{
				const EdgeIndex idx = index/2;
				Edge& edge = mEdges[idx]; //InstanceIndex/2 = edgeIndex
				//PX_ASSERT(edge.isActive()); //The edge must currently be inactive because the node was active
				//Deactivate the edge if both nodes connected are inactive OR if one node is static/kinematic and the other is inactive...
				PX_ASSERT(mCpuData.mEdgeNodeIndices[index & (~1)].index() == PX_INVALID_NODE || !mNodes[mCpuData.mEdgeNodeIndices[index & (~1)].index()].isActive());
				PX_ASSERT(mCpuData.mEdgeNodeIndices[index | 1].index() == PX_INVALID_NODE || !mNodes[mCpuData.mEdgeNodeIndices[index | 1].index()].isActive());
				if(edge.isActive())
				{
					edge.deactivateEdge();
					mActiveEdgeCount[edge.mEdgeType]--;
					removeEdgeFromActivatingList(idx);
					mDeactivatingEdges[edge.mEdgeType].pushBack(idx);
				}
			}
			index = instance.mNextEdge;
		}		
	}
}

#if IG_SANITY_CHECKS
bool IslandSim::canFindRoot(PxNodeIndex startNode, PxNodeIndex targetNode, PxArray<PxNodeIndex>* visitedNodes)
{
	if(visitedNodes)
		visitedNodes->pushBack(startNode);
	if(startNode.index() == targetNode.index())
		return true;
	PxBitMap visitedState;
	visitedState.resizeAndClear(mNodes.size());

	PxArray<PxNodeIndex> stack;

	stack.pushBack(startNode);

	visitedState.set(startNode.index());

	do
	{
		const PxNodeIndex currentIndex = stack.popBack();
		const Node& currentNode = mNodes[currentIndex.index()];

		EdgeInstanceIndex currentEdge = currentNode.mFirstEdgeIndex;

		while(currentEdge != IG_INVALID_EDGE)
		{
			const EdgeInstance& edge = mEdgeInstances[currentEdge];
			const PxNodeIndex outboundNode = mCpuData.mEdgeNodeIndices[currentEdge ^ 1];
			if(outboundNode.index() != PX_INVALID_NODE && !mNodes[outboundNode.index()].isKinematic() && !visitedState.test(outboundNode.index()))
			{
				if(outboundNode.index() == targetNode.index())
					return true;

				visitedState.set(outboundNode.index());
				stack.pushBack(outboundNode);
				if(visitedNodes)
					visitedNodes->pushBack(outboundNode);
			}

			currentEdge = edge.mNextEdge;
		}
	}
	while(stack.size());

	return false;
}
#endif

void IslandSim::unwindRoute(PxU32 traversalIndex, PxNodeIndex lastNode, PxU32 hopCount, IslandId id)
{
	//We have found either a witness *or* the root node with this traversal. In the event of finding the root node, hopCount will be 0. In the event of finding
	//a witness, hopCount will be the hopCount that witness reported as being the distance to the root.

	PxU32 currIndex = traversalIndex;
	PxU32 hc = hopCount+1; //Add on 1 for the hop to the witness/root node.
	do
	{
		TraversalState& state = mVisitedNodes[currIndex];
		mHopCounts[state.mNodeIndex.index()] = hc++;
		mIslandIds[state.mNodeIndex.index()] = id;
		mFastRoute[state.mNodeIndex.index()] = lastNode;
		currIndex = state.mPrevIndex;
		lastNode = state.mNodeIndex;
	}
	while(currIndex != PX_INVALID_NODE);
}

void IslandSim::activateIslandInternal(const Island& island)
{
	PxNodeIndex currentNode = island.mRootNode;
	while(currentNode.index() != PX_INVALID_NODE)
	{
		activateNodeInternal(currentNode);
		currentNode = mNodes[currentNode.index()].mNextNode;
	}
}

void IslandSim::activateIsland(IslandId islandId)
{
	Island& island = mIslands[islandId];
	PX_ASSERT(!mIslandAwake.test(islandId));
	PX_ASSERT(island.mActiveIndex == IG_INVALID_ISLAND);
	
	activateIslandInternal(island);

	markIslandActive(islandId);
}

void IslandSim::deactivateIsland(IslandId islandId)
{
	PX_ASSERT(mIslandAwake.test(islandId));
	Island& island = mIslands[islandId];
	
	PxNodeIndex currentNode = island.mRootNode;
	while(currentNode.index() != PX_INVALID_NODE)
	{
		const Node& node = mNodes[currentNode.index()];
		
		//if(mActiveNodeIndex[currentNode.index()] < mInitialActiveNodeCount[node.mType])
		mNodesToPutToSleep[node.mType].pushBack(currentNode); //If this node was previously active, then push it to the list of nodes to deactivate
		deactivateNodeInternal(currentNode);
		currentNode = node.mNextNode;
	}
	markIslandInactive(islandId);
}

void IslandSim::wakeIslandsInternal(bool flag)
{
	//(1) Iterate over activating nodes and activate them

	const PxU32 originalActiveIslands = mActiveIslands.size();

	if(flag)
	{
		for (PxU32 a = 0; a < Edge::eEDGE_TYPE_COUNT; ++a)
		{
			for (PxU32 i = 0, count = mActivatedEdges[a].size(); i < count; ++i)
			{
				IG::Edge& edge = mEdges[mActivatedEdges[a][i]];
				edge.mEdgeState &= (~Edge::eACTIVATING);
			}

			mActivatedEdges[a].forceSize_Unsafe(0);
		}

		for (PxU32 a = 0; a < Edge::eEDGE_TYPE_COUNT; ++a)
		{
			mInitialActiveNodeCount[a] = mActiveNodes[a].size();
		}
	}

	for(PxU32 a = 0; a < mActivatingNodes.size(); ++a)
	{
		const PxNodeIndex wakeNode = mActivatingNodes[a];

		const IslandId islandId = mIslandIds[wakeNode.index()];

		Node& node = mNodes[wakeNode.index()];
		node.clearActivating();
		if(islandId != IG_INVALID_ISLAND)
		{
			if(!mIslandAwake.test(islandId))
				markIslandActive(islandId);						

			mActiveNodeIndex[wakeNode.index()] = PX_INVALID_NODE; //Mark active node as invalid.
			activateNodeInternal(wakeNode);
		}
		else
		{
			PX_ASSERT(node.isKinematic());
			node.setActive();
			PX_ASSERT(mActiveNodeIndex[wakeNode.index()] == a);
			mActiveNodeIndex[wakeNode.index()] = mActiveKinematicNodes.size();
			mActiveKinematicNodes.pushBack(wakeNode);

			//Wake up the islands connected to this waking kinematic!
			EdgeInstanceIndex index = node.mFirstEdgeIndex;
			while(index != IG_INVALID_EDGE)
			{
				const EdgeInstance& edgeInstance = mEdgeInstances[index];

				const PxNodeIndex outboundNode = mCpuData.mEdgeNodeIndices[index ^ 1];
				//Edge& edge = mEdges[index/2];
				//if(edge.isConnected()) //Only wake up if the edge is not connected...
				const PxNodeIndex nodeIndex = outboundNode;

				if (nodeIndex.isStaticBody() || mIslandIds[nodeIndex.index()] == IG_INVALID_ISLAND)
				{
					//If the edge connects to a static body *or* it connects to a node which is not part of an island (i.e. a kinematic), then activate the edge
					makeEdgeActive(index, true);
				}
				else
				{
					const IslandId connectedIslandId = mIslandIds[nodeIndex.index()];
					if(!mIslandAwake.test(connectedIslandId))
					{
						//Wake up that island
						markIslandActive(connectedIslandId);
					}
				}

				index = edgeInstance.mNextEdge;
			}
		}
	}

	mActivatingNodes.forceSize_Unsafe(0);

	for(PxU32 a = originalActiveIslands; a < mActiveIslands.size(); ++a)
		activateIslandInternal(mIslands[mActiveIslands[a]]);
}

void IslandSim::wakeIslands()
{
	PX_PROFILE_ZONE("Basic.wakeIslands", mContextId);
	wakeIslandsInternal(true);
}

void IslandSim::wakeIslands2()
{
	PX_PROFILE_ZONE("Basic.wakeIslands2", mContextId);
	wakeIslandsInternal(false);
}

void IslandSim::insertNewEdges()
{
	PX_PROFILE_ZONE("Basic.insertNewEdges", mContextId);

	mEdgeInstances.reserve(mEdges.capacity()*2);
	
	for(PxU32 i = 0; i < Edge::eEDGE_TYPE_COUNT; ++i)
	{
		for(PxU32 a = 0; a < mDirtyEdges[i].size(); ++a)
		{
			const EdgeIndex edgeIndex = mDirtyEdges[i][a];

			Edge& edge = mEdges[edgeIndex];

			if(!edge.isPendingDestroyed())
			{
				//PX_ASSERT(!edge.isInserted());
				if(!edge.isInserted())
				{
					addConnectionToGraph(edgeIndex);
					edge.setInserted();
				}
			}
		}
	}
}

void IslandSim::removeDestroyedEdges()
{
	PX_PROFILE_ZONE("Basic.removeDestroyedEdges", mContextId);

	for(PxU32 a = 0; a < mDestroyedEdges.size(); ++a)
	{
		const EdgeIndex edgeIndex = mDestroyedEdges[a];

		const Edge& edge = mEdges[edgeIndex];
		
		if(edge.isPendingDestroyed())
		{
			if(!edge.isInDirtyList() && edge.isInserted())
			{
				removeConnectionInternal(edgeIndex);
				removeConnectionFromGraph(edgeIndex);
				//edge.clearInserted();
			}
			//edge.clearDestroyed();
		}
	}		
}

IslandId IslandSim::addNodeToIsland(PxNodeIndex nodeIndex1, PxNodeIndex nodeIndex2, IslandId islandId2, bool active1, bool active2)
{
	PX_ASSERT(islandId2 != IG_INVALID_ISLAND);
	if (nodeIndex1.index() != PX_INVALID_NODE)
	{
		if (!mNodes[nodeIndex1.index()].isKinematic())
		{
			//We need to add node 1 to island2
			PX_ASSERT(mNodes[nodeIndex1.index()].mNextNode.index() == PX_INVALID_NODE); //Ensure that this node is not in any other island
			PX_ASSERT(mNodes[nodeIndex1.index()].mPrevNode.index() == PX_INVALID_NODE); //Ensure that this node is not in any other island
							
			Island& island = mIslands[islandId2];

			Node& lastNode = mNodes[island.mLastNode.index()];

			PX_ASSERT(lastNode.mNextNode.index() == PX_INVALID_NODE);

			Node& node = mNodes[nodeIndex1.index()];
			lastNode.mNextNode = nodeIndex1;
			node.mPrevNode = island.mLastNode;
			island.mLastNode = nodeIndex1;
			island.mNodeCount[node.mType]++;
			mIslandIds[nodeIndex1.index()] = islandId2;
			mHopCounts[nodeIndex1.index()] = mHopCounts[nodeIndex2.index()] + 1;
			mFastRoute[nodeIndex1.index()] = nodeIndex2;

			if(active1 || active2)
			{
				if(!mIslandAwake.test(islandId2))
				{
					//This island wasn't already awake, so need to wake the whole island up
					activateIsland(islandId2);
				}
				if(!active1)
				{
					//Wake up this node...
					activateNodeInternal(nodeIndex1);
				}
			}
		}
		else if(active1 && !active2)
		{
			//Active kinematic object -> wake island!
			activateIsland(islandId2);
		}
	}
	else
	{
		//A new touch with a static body...
		Node& node = mNodes[nodeIndex2.index()];
		node.mStaticTouchCount++; //Increment static touch counter on the body
		//Island& island = mIslands[islandId2];
		//island.mStaticTouchCount++; //Increment static touch counter on the island
		mIslandStaticTouchCount[islandId2]++;
	}
	return islandId2;
}

void IslandSim::processNewEdges()
{
	PX_PROFILE_ZONE("Basic.processNewEdges", mContextId);
	//Stage 1: we process the list of new pairs. To do this, we need to first sort them based on a predicate...

	insertNewEdges();

	mHopCounts.resize(mNodes.size()); //Make sure we have enough space for hop counts for all nodes
	mFastRoute.resize(mNodes.size());

	for(PxU32 i = 0; i < Edge::eEDGE_TYPE_COUNT; ++i)
	{
		for(PxU32 a = 0; a < mDirtyEdges[i].size(); ++a)
		{
			const EdgeIndex edgeIndex = mDirtyEdges[i][a];
			const Edge& edge = mEdges[edgeIndex];
			
			/*PX_ASSERT(edge.mState != Edge::eDESTROYED || ((edge.mNode1.index() == PX_INVALID_NODE || mNodes[edge.mNode1.index()].isKinematic() || mNodes[edge.mNode1.index()].isActive() == false) &&
				(edge.mNode2.index() == PX_INVALID_NODE || mNodes[edge.mNode2.index()].isKinematic() || mNodes[edge.mNode2.index()].isActive() == false)));*/

			//edge.clearInDirtyList();
			
			//We do not process either destroyed or disconnected edges
			if(/*edge.isConnected() && */!edge.isPendingDestroyed())
			{
				//Conditions:
				//(1)	Neither body is in an island (static/kinematics are never in islands) so we need to create a new island containing these bodies 
				//		or just 1 body if the other is kinematic/static
				//(2)	Both bodies are already in the same island. Update root node hop count estimates for the bodies if a route through the new connection
				//		is shorter for either body
				//(3)	One body is already in an island and the other isn't, so we just add the new body to the existing island.
				//(4)	Both bodies are in different islands. In that case, we merge the islands

				const PxNodeIndex nodeIndex1 = mCpuData.mEdgeNodeIndices[2 * edgeIndex];
				const PxNodeIndex nodeIndex2 = mCpuData.mEdgeNodeIndices[2 * edgeIndex+1];

				const PxU32 index1 = nodeIndex1.index();
				const PxU32 index2 = nodeIndex2.index();

				const IslandId islandId1 = index1 == PX_INVALID_NODE ? IG_INVALID_ISLAND : mIslandIds[index1];
				const IslandId islandId2 = index2 == PX_INVALID_NODE ? IG_INVALID_ISLAND : mIslandIds[index2];

				//TODO - wake ups!!!!
				//If one of the nodes is awake and the other is asleep, we need to wake 'em up

				//When a node is activated, the island must also be activated...

				const bool active1 = index1 != PX_INVALID_NODE && mNodes[index1].isActive();
				const bool active2 = index2 != PX_INVALID_NODE && mNodes[index2].isActive();

				IslandId islandId = IG_INVALID_ISLAND;

				if(islandId1 == IG_INVALID_ISLAND && islandId2 == IG_INVALID_ISLAND)
				{
					//All nodes should be introduced in an island now unless they are static or kinematic. Therefore, if we get here, we have an edge
					//between 2 kinematic nodes or a kinematic and static node. These should not influence island management so we should just ignore
					//these edges.
				}
				else if(islandId1 == islandId2)
				{
					islandId = islandId1;
					if(active1 || active2)
					{
						PX_ASSERT(mIslandAwake.test(islandId1)); //If we got here, where the 2 were already in an island, if 1 node is awake, the whole island must be awake
					}
					//Both bodies in the same island. Nothing major to do already but we should see if this creates a shorter path to root for either node
					const PxU32 hopCount1 = mHopCounts[index1];
					const PxU32 hopCount2 = mHopCounts[index2];
					if((hopCount1+1) < hopCount2)
					{
						//It would be faster for node 2 to go through node 1
						mHopCounts[index2] = hopCount1 + 1;
						mFastRoute[index2] = nodeIndex1;
					}
					else if((hopCount2+1) < hopCount1)
					{
						//It would be faster for node 1 to go through node 2
						mHopCounts[index1] = hopCount2 + 1;
						mFastRoute[index1] = nodeIndex2;
					}

					//No need to activate/deactivate the island. Its state won't have changed
				}
				else if(islandId1 == IG_INVALID_ISLAND)
				{
					islandId = addNodeToIsland(nodeIndex1, nodeIndex2, islandId2, active1, active2);
				}
				else if (islandId2 == IG_INVALID_ISLAND)
				{
					islandId = addNodeToIsland(nodeIndex2, nodeIndex1, islandId1, active2, active1);
				}
				else
				{
					PX_ASSERT(islandId1 != islandId2);
					PX_ASSERT(islandId1 != IG_INVALID_ISLAND && islandId2 != IG_INVALID_ISLAND);

					if(active1 || active2)
					{
						//One of the 2 islands was awake, so need to wake the other one! We do this now, before we merge the islands, to ensure that all 
						//the bodies are activated
						if(!mIslandAwake.test(islandId1))
						{
							//This island wasn't already awake, so need to wake the whole island up
							activateIsland(islandId1);
						}
						if(!mIslandAwake.test(islandId2))
						{
							//This island wasn't already awake, so need to wake the whole island up
							activateIsland(islandId2);
						}
					}

					//OK. We need to merge these islands together...
					islandId = mergeIslands(islandId1, islandId2, nodeIndex1, nodeIndex2);
				}

				if(islandId != IG_INVALID_ISLAND)
				{
					//Add new edge to existing island
					Island& island = mIslands[islandId];
					addEdgeToIsland(island, edgeIndex);
				}
			}
		}
	}
}

#if PX_DEBUG
bool IslandSim::isPathTo(PxNodeIndex startNode, PxNodeIndex targetNode) const
{
	const Node& node = mNodes[startNode.index()];

	EdgeInstanceIndex index = node.mFirstEdgeIndex;
	while(index != IG_INVALID_EDGE)
	{
		const EdgeInstance& instance = mEdgeInstances[index];
		if(/*mEdges[index/2].isConnected() &&*/ mCpuData.mEdgeNodeIndices[index^1].index() == targetNode.index())
			return true;
		index = instance.mNextEdge;
	}
	return false;
}
#endif

bool IslandSim::tryFastPath(PxNodeIndex startNode, PxNodeIndex targetNode, IslandId islandId)
{
	PX_UNUSED(startNode);
	PX_UNUSED(targetNode);

	PxNodeIndex currentNode = startNode;

	const PxU32 currentVisitedNodes = mVisitedNodes.size();

	PxU32 depth = 0;
	
	bool found = false;
	do
	{
		//Get the fast path from this node...
		
		if(mVisitedState.test(currentNode.index()))
		{
			found = mIslandIds[currentNode.index()] != IG_INVALID_ISLAND; //Already visited and not tagged with invalid island == a witness!
			break;
		}
		if( currentNode.index() == targetNode.index())
		{
			found = true;
			break;
		}

		mVisitedNodes.pushBack(TraversalState(currentNode, mVisitedNodes.size(), mVisitedNodes.size()-1, depth++));

		PX_ASSERT(mFastRoute[currentNode.index()].index() == PX_INVALID_NODE || isPathTo(currentNode, mFastRoute[currentNode.index()]));

		mIslandIds[currentNode.index()] = IG_INVALID_ISLAND;
		mVisitedState.set(currentNode.index());

		currentNode = mFastRoute[currentNode.index()];
	}
	while(currentNode.index() != PX_INVALID_NODE);

	for(PxU32 a = currentVisitedNodes; a < mVisitedNodes.size(); ++a)
	{
		const TraversalState& state = mVisitedNodes[a];
		mIslandIds[state.mNodeIndex.index()] = islandId;
	}

	if(!found)
	{
		for(PxU32 a = currentVisitedNodes; a < mVisitedNodes.size(); ++a)
		{
			const TraversalState& state = mVisitedNodes[a];
			mVisitedState.reset(state.mNodeIndex.index());
		}

		mVisitedNodes.forceSize_Unsafe(currentVisitedNodes);
	}
	return found;
}

bool IslandSim::findRoute(PxNodeIndex startNode, PxNodeIndex targetNode, IslandId islandId)
{
	//Firstly, traverse the fast path and tag up witnesses. TryFastPath can fail. In that case, no witnesses are left but this node is permitted to report
	//that it is still part of the island. Whichever node lost its fast path will be tagged as dirty and will be responsible for recovering the fast path
	//and tagging up the visited nodes
	if(mFastRoute[startNode.index()].index() != PX_INVALID_NODE)
	{
		if(tryFastPath(startNode, targetNode, islandId))
			return true;

		//Try fast path can either be successful or not. If it was successful, then we had a valid fast path cached and all nodes on that fast path were tagged
		//as witness nodes (visited and with a valid island ID). If the fast path was not successful, then no nodes were tagged as witnesses. 
		//Technically, we need to find a route to the root node but, as an optimization, we can simply return true from here with no witnesses added. 
		//Whichever node actually broke the "fast path" will also be on the list of dirty nodes and will be processed later. 
		//If that broken edge triggered an island separation, this node will be re-visited and added to that island, otherwise
		//the path to the root node will be re-established. The end result is the same - the island state is computed - this just saves us some work.
		//return true;
	}

	{
		//If we got here, there was no fast path. Therefore, we need to fall back on searching for the root node. This is optimized by using "hop counts".
		//These are per-node counts that indicate the expected number of hops from this node to the root node. These are lazily evaluated and updated
		//as new edges are formed or when traversals occur to re-establish islands. As a result, they may be inaccurate but they still serve the purpose
		//of guiding our search to minimize the chances of us doing an exhaustive search to find the root node.
		mIslandIds[startNode.index()] = IG_INVALID_ISLAND;
		TraversalState* startTraversal = &mVisitedNodes.pushBack(TraversalState(startNode, mVisitedNodes.size(), PX_INVALID_NODE, 0));
		mVisitedState.set(startNode.index());
		QueueElement element(startTraversal, mHopCounts[startNode.index()]);
		mPriorityQueue.push(element);

		do
		{
			const QueueElement currentQE = mPriorityQueue.pop();

			const TraversalState& currentState = *currentQE.mState;

			const Node& currentNode = mNodes[currentState.mNodeIndex.index()];

			EdgeInstanceIndex edge = currentNode.mFirstEdgeIndex;

			while(edge != IG_INVALID_EDGE)
			{
				const EdgeInstance& instance = mEdgeInstances[edge];
				{
					const PxNodeIndex nextIndex = mCpuData.mEdgeNodeIndices[edge ^ 1];

					//Static or kinematic nodes don't connect islands.
					if(nextIndex.index() != PX_INVALID_NODE && !mNodes[nextIndex.index()].isKinematic())
					{
						if(nextIndex.index() == targetNode.index())
						{
							unwindRoute(currentState.mCurrentIndex, nextIndex, 0, islandId);
							return true;
						}

						if(mVisitedState.test(nextIndex.index()))
						{
							//We already visited this node. This means that it's either in the priority queue already or we 
							//visited in on a previous pass. If it was visited on a previous pass, then it already knows what island it's in. 
							//We now need to test the island id to find out if this node knows the root.
							//If it has a valid root id, that id *is* our new root. We can guesstimate our hop count based on the node's properties
							
							const IslandId visitedIslandId = mIslandIds[nextIndex.index()];
							if(visitedIslandId != IG_INVALID_ISLAND)
							{
								//If we get here, we must have found a node that knows a route to our root node. It must not be a different island
								//because that would caused me to have been visited already because totally separate islands trigger a full traversal on 
								//the orphaned side.
								PX_ASSERT(visitedIslandId == islandId);
								unwindRoute(currentState.mCurrentIndex, nextIndex, mHopCounts[nextIndex.index()], islandId);
								return true;
							}
						}
						else
						{
							//This node has not been visited yet, so we need to push it into the stack and continue traversing
							TraversalState* state = &mVisitedNodes.pushBack(TraversalState(nextIndex, mVisitedNodes.size(), currentState.mCurrentIndex, currentState.mDepth+1));
							QueueElement qe(state, mHopCounts[nextIndex.index()]);
							mPriorityQueue.push(qe);
							mVisitedState.set(nextIndex.index());
							PX_ASSERT(mIslandIds[nextIndex.index()] == islandId);
							mIslandIds[nextIndex.index()] = IG_INVALID_ISLAND; //Flag as invalid island until we know whether we can find root or an island id.
						}
					}
				}

				edge = instance.mNextEdge;
			}
		}
		while(mPriorityQueue.size());

		return false;
	}
}

void IslandSim::processLostEdges(const PxArray<PxNodeIndex>& destroyedNodes, bool allowDeactivation, bool permitKinematicDeactivation, PxU32 dirtyNodeLimit)
{
	PX_UNUSED(dirtyNodeLimit);
	PX_PROFILE_ZONE("Basic.processLostEdges", mContextId);
	//At this point, all nodes and edges are activated. 

	//Bit map for visited
	mVisitedState.resizeAndClear(mNodes.size());

	//Reserve space on priority queue for at least 1024 nodes. It will resize if more memory is required during traversal.
	mPriorityQueue.reserve(1024);

	for (PxU32 i = 0; i < Edge::eEDGE_TYPE_COUNT; ++i)
		mIslandSplitEdges[i].reserve(1024);
	
	mVisitedNodes.reserve(mNodes.size()); //Make sure we have enough space for all nodes!

	const PxU32 nbDestroyedEdges = mDestroyedEdges.size();
	PX_UNUSED(nbDestroyedEdges);
	{
		PX_PROFILE_ZONE("Basic.removeEdgesFromIslands", mContextId);
		for (PxU32 a = 0; a < mDestroyedEdges.size(); ++a)
		{
			const EdgeIndex lostIndex = mDestroyedEdges[a];
			Edge& lostEdge = mEdges[lostIndex];

			if (lostEdge.isPendingDestroyed() && !lostEdge.isInDirtyList())
			{
				//Process this edge...
				if (!lostEdge.isReportOnlyDestroy() && lostEdge.isInserted())
				{
					const PxU32 index1 = mCpuData.mEdgeNodeIndices[mDestroyedEdges[a] * 2].index();
					const PxU32 index2 = mCpuData.mEdgeNodeIndices[mDestroyedEdges[a] * 2 + 1].index();

					IslandId islandId = IG_INVALID_ISLAND;
					if (index1 != PX_INVALID_NODE && index2 != PX_INVALID_NODE)
					{
						PX_ASSERT(mIslandIds[index1] == IG_INVALID_ISLAND || mIslandIds[index2] == IG_INVALID_ISLAND ||
							mIslandIds[index1] == mIslandIds[index2]);
						islandId = mIslandIds[index1] != IG_INVALID_ISLAND ? mIslandIds[index1] : mIslandIds[index2];
					}
					else if (index1 != PX_INVALID_NODE)
					{
						PX_ASSERT(index2 == PX_INVALID_NODE);
						Node& node = mNodes[index1];
						if (!node.isKinematic())
						{
							islandId = mIslandIds[index1];
							node.mStaticTouchCount--;
							//Island& island = mIslands[islandId];
							mIslandStaticTouchCount[islandId]--;
							//island.mStaticTouchCount--;
						}
					}
					else if (index2 != PX_INVALID_NODE)
					{
						PX_ASSERT(index1 == PX_INVALID_NODE);
						Node& node = mNodes[index2];
						if (!node.isKinematic())
						{
							islandId = mIslandIds[index2];
							node.mStaticTouchCount--;
							//Island& island = mIslands[islandId];
							mIslandStaticTouchCount[islandId]--;
							//island.mStaticTouchCount--;
						}
					}

					if (islandId != IG_INVALID_ISLAND)
					{
						//We need to remove this edge from the island
						Island& island = mIslands[islandId];
						removeEdgeFromIsland(island, lostIndex);
					}
				}

				lostEdge.clearInserted();
			}
		}
	}

	if (allowDeactivation)
	{
		PX_PROFILE_ZONE("Basic.findPathsAndBreakIslands", mContextId);

		//KS - process only this many dirty nodes, deferring future dirty nodes to subsequent frames. 
		//This means that it may take several frames for broken edges to trigger islands to completely break but this is better
		//than triggering large performance spikes.
#if IG_LIMIT_DIRTY_NODES
		PxBitMap::PxCircularIterator iter(mDirtyMap, mLastMapIndex);
		const PxU32 MaxCount = dirtyNodeLimit;// +10000000;
		PxU32 lastMapIndex = mLastMapIndex;
		PxU32 count = 0;
#else
		PxBitMap::Iterator iter(mDirtyMap);
#endif

		PxU32 dirtyIdx;

#if IG_LIMIT_DIRTY_NODES
		while ((dirtyIdx = iter.getNext()) != PxBitMap::PxCircularIterator::DONE
			&& (count++ < MaxCount)
#else
		while ((dirtyIdx = iter.getNext()) != PxBitMap::Iterator::DONE
#endif
			)
		{
#if IG_LIMIT_DIRTY_NODES
			lastMapIndex = dirtyIdx + 1;
#endif
			//Process dirty nodes. Figure out if we can make our way from the dirty node to the root.

			mPriorityQueue.clear(); //Clear the queue used for traversal
			mVisitedNodes.forceSize_Unsafe(0); //Clear the list of nodes in this island
			const PxNodeIndex dirtyNodeIndex(dirtyIdx);
			Node& dirtyNode = mNodes[dirtyNodeIndex.index()];

			//Check whether this node has already been touched. If it has been touched this frame, then its island state is reliable 
			//and we can just unclear the dirty flag on the body. If we were already visited, then the state should have already been confirmed in a 
			//previous pass.
			if (!dirtyNode.isKinematic() && !dirtyNode.isDeleted() && !mVisitedState.test(dirtyNodeIndex.index()))
			{
				//We haven't visited this node in our island repair passes yet, so we still need to process until we've hit a visited node or found
				//our root node. Note that, as soon as we hit a visited node that has already been processed in a previous pass, we know that we can rely
				//on its island information although the hop counts may not be optimal. It also indicates that this island was not broken immediately because
				//otherwise, the entire new sub-island would already have been visited and this node would have already had its new island state assigned.

				//Indicate that I've been visited

				const IslandId islandId = mIslandIds[dirtyNodeIndex.index()];
				const Island& findIsland = mIslands[islandId];

				const PxNodeIndex searchNode = findIsland.mRootNode;//The node that we're searching for!

				if (searchNode.index() != dirtyNodeIndex.index()) //If we are the root node, we don't need to do anything!
				{
					if (findRoute(dirtyNodeIndex, searchNode, islandId))
					{
						//We found the root node so let's let every visited node know that we found its root
						//and we can also update our hop counts because we recorded how many hops it took to reach this
						//node

						//We already filled in the path to the root/witness with accurate hop counts. Now we just need to fill in the estimates
						//for the remaining nodes and re-define their islandIds. We approximate their path to the root by just routing them through
						//the route we already found.

						//This loop works because mVisitedNodes are recorded in the order they were visited and we already filled in the critical path
						//so the remainder of the paths will just fork from that path.

						//Verify state (that we can see the root from this node)...

#if IG_SANITY_CHECKS
						PX_ASSERT(canFindRoot(dirtyNodeIndex, searchNode, NULL)); //Verify that we found the connection
#endif

						for (PxU32 b = 0; b < mVisitedNodes.size(); ++b)
						{
							TraversalState& state = mVisitedNodes[b];
							if (mIslandIds[state.mNodeIndex.index()] == IG_INVALID_ISLAND)
							{
								mHopCounts[state.mNodeIndex.index()] = mHopCounts[mVisitedNodes[state.mPrevIndex].mNodeIndex.index()] + 1;
								mFastRoute[state.mNodeIndex.index()] = mVisitedNodes[state.mPrevIndex].mNodeIndex;
								mIslandIds[state.mNodeIndex.index()] = islandId;
							}
						}
					}
					else
					{
						//If I traversed and could not find the root node, then I have established a new island. In this island, I am the root node
						//and I will point all my nodes towards me. Furthermore, I have established how many steps it took to reach all nodes in my island

						//OK. We need to separate the islands. We have a list of nodes that are part of the new island (mVisitedNodes) and we know that the 
						//first node in that list is the root node.


						//OK, we need to remove all these actors from their current island, then add them to the new island...

						Island& oldIsland = mIslands[islandId];
						//We can just unpick these nodes from the island because they do not contain the root node (if they did, then we wouldn't be
						//removing this node from the island at all). The only challenge is if we need to remove the last node. In that case
						//we need to re-establish the new last node in the island but perhaps the simplest way to do that would be to traverse
						//the island to establish the last node again

#if IG_SANITY_CHECKS
						PX_ASSERT(!canFindRoot(dirtyNodeIndex, searchNode, NULL));
#endif

						PxU32 totalStaticTouchCount = 0;
						PxU32 nodeCount[Node::eTYPE_COUNT];
						for (PxU32 t = 0; t < Node::eTYPE_COUNT; ++t)
						{
							nodeCount[t] = 0;
						}

						for (PxU32 t = 0; t < Edge::eEDGE_TYPE_COUNT; ++t)
						{
							mIslandSplitEdges[t].forceSize_Unsafe(0);
						}

						//NodeIndex lastIndex = oldIsland.mLastNode;

						//nodeCount[node.mType] = 1;

						for (PxU32 a = 0; a < mVisitedNodes.size(); ++a)
						{
							const PxNodeIndex index = mVisitedNodes[a].mNodeIndex;
							Node& node = mNodes[index.index()];

							if (node.mNextNode.index() != PX_INVALID_NODE)
								mNodes[node.mNextNode.index()].mPrevNode = node.mPrevNode;
							else
								oldIsland.mLastNode = node.mPrevNode;
							if (node.mPrevNode.index() != PX_INVALID_NODE)
								mNodes[node.mPrevNode.index()].mNextNode = node.mNextNode;

							nodeCount[node.mType]++;

							node.mNextNode.setIndices(PX_INVALID_NODE);
							node.mPrevNode.setIndices(PX_INVALID_NODE);

							PX_ASSERT(mNodes[oldIsland.mLastNode.index()].mNextNode.index() == PX_INVALID_NODE);

							totalStaticTouchCount += node.mStaticTouchCount;

							EdgeInstanceIndex idx = node.mFirstEdgeIndex;

							while (idx != IG_INVALID_EDGE)
							{
								const EdgeInstance& instance = mEdgeInstances[idx];
								const EdgeIndex edgeIndex = idx / 2;
								const Edge& edge = mEdges[edgeIndex];

								//Only split the island if we're processing the first node or if the first node is infinte-mass
								if (!(idx & 1) || (mCpuData.mEdgeNodeIndices[idx & (~1)].index() == PX_INVALID_NODE || mNodes[mCpuData.mEdgeNodeIndices[idx & (~1)].index()].isKinematic()))
								{
									//We will remove this edge from the island...
									mIslandSplitEdges[edge.mEdgeType].pushBack(edgeIndex);

									removeEdgeFromIsland(oldIsland, edgeIndex);
								}
								idx = instance.mNextEdge;
							}
						}

						//oldIsland.mStaticTouchCount -= totalStaticTouchCount;
						mIslandStaticTouchCount[islandId] -= totalStaticTouchCount;

						for (PxU32 i = 0; i < Node::eTYPE_COUNT; ++i)
						{
							PX_ASSERT(nodeCount[i] <= oldIsland.mNodeCount[i]);
							oldIsland.mNodeCount[i] -= nodeCount[i];
						}

						//Now add all these nodes to the new island

						//(1) Create the new island...
						const IslandId newIslandHandle = mIslandHandles.getHandle();
						/*if(newIslandHandle == mIslands.capacity())
						{
						mIslands.reserve(2*mIslands.capacity() + 1);
						}*/
						mIslands.resize(PxMax(newIslandHandle + 1, mIslands.size()));
						mIslandStaticTouchCount.resize(PxMax(newIslandHandle + 1, mIslandStaticTouchCount.size()));
						Island& newIsland = mIslands[newIslandHandle];

						if (mIslandAwake.test(islandId))
						{
							newIsland.mActiveIndex = mActiveIslands.size();
							mActiveIslands.pushBack(newIslandHandle);
							mIslandAwake.growAndSet(newIslandHandle); //Separated island, so it should be awake
						}
						else
						{
							mIslandAwake.growAndReset(newIslandHandle);
						}

						newIsland.mRootNode = dirtyNodeIndex;
						mHopCounts[dirtyNodeIndex.index()] = 0;
						mIslandIds[dirtyNodeIndex.index()] = newIslandHandle;
						//newIsland.mTotalSize = mVisitedNodes.size();

						mNodes[dirtyNodeIndex.index()].mPrevNode.setIndices(PX_INVALID_NODE); //First node so doesn't have a preceding node
						mFastRoute[dirtyNodeIndex.index()].setIndices(PX_INVALID_NODE);

						for (PxU32 i = 0; i < Node::eTYPE_COUNT; ++i)
							nodeCount[i] = 0;

						nodeCount[dirtyNode.mType] = 1;

						for (PxU32 a = 1; a < mVisitedNodes.size(); ++a)
						{
							const PxNodeIndex index = mVisitedNodes[a].mNodeIndex;
							Node& thisNode = mNodes[index.index()];
							const PxNodeIndex prevNodeIndex = mVisitedNodes[a - 1].mNodeIndex;
							thisNode.mPrevNode = prevNodeIndex;
							mNodes[prevNodeIndex.index()].mNextNode = index;
							nodeCount[thisNode.mType]++;
							mIslandIds[index.index()] = newIslandHandle;
							mHopCounts[index.index()] = mVisitedNodes[a].mDepth; //How many hops to root
							mFastRoute[index.index()] = mVisitedNodes[mVisitedNodes[a].mPrevIndex].mNodeIndex;
						}

						for (PxU32 i = 0; i < Node::eTYPE_COUNT; ++i)
							newIsland.mNodeCount[i] = nodeCount[i];

						//Last node in the island
						const PxNodeIndex lastIndex = mVisitedNodes[mVisitedNodes.size() - 1].mNodeIndex;
						mNodes[lastIndex.index()].mNextNode.setIndices(PX_INVALID_NODE);
						newIsland.mLastNode = lastIndex;
						//newIsland.mStaticTouchCount = totalStaticTouchCount;
						mIslandStaticTouchCount[newIslandHandle] = totalStaticTouchCount;

						PX_ASSERT(mNodes[newIsland.mLastNode.index()].mNextNode.index() == PX_INVALID_NODE);

						for (PxU32 j = 0; j < IG::Edge::eEDGE_TYPE_COUNT; ++j)
						{
							PxArray<EdgeIndex>& splitEdges = mIslandSplitEdges[j];
							const PxU32 splitEdgeSize = splitEdges.size();
							if (splitEdgeSize)
							{
								splitEdges.pushBack(IG_INVALID_EDGE); //Push in a dummy invalid edge to complete the connectivity
								mEdges[splitEdges[0]].mNextIslandEdge = splitEdges[1];
								for (PxU32 a = 1; a < splitEdgeSize; ++a)
								{
									const EdgeIndex edgeIndex = splitEdges[a];
									Edge& edge = mEdges[edgeIndex];
									edge.mNextIslandEdge = splitEdges[a + 1];
									edge.mPrevIslandEdge = splitEdges[a - 1];
								}

								newIsland.mFirstEdge[j] = splitEdges[0];
								newIsland.mLastEdge[j] = splitEdges[splitEdgeSize - 1];
								newIsland.mEdgeCount[j] = splitEdgeSize;
							}
						}
					}
				}
			}
			dirtyNode.clearDirty();
#if IG_LIMIT_DIRTY_NODES
			mDirtyMap.reset(dirtyIdx);
#endif
		}

#if IG_LIMIT_DIRTY_NODES
		mLastMapIndex = lastMapIndex;
		if (count < MaxCount)
			mLastMapIndex = 0;
#else
		mDirtyMap.clear();
#endif

		//mDirtyNodes.forceSize_Unsafe(0);
	}

	{
		PX_PROFILE_ZONE("Basic.clearDestroyedEdges", mContextId);
		//Now process the lost edges...
		for (PxU32 a = 0; a < mDestroyedEdges.size(); ++a)
		{
			//Process these destroyed edges. Recompute island information. Update the islands and hop counters accordingly
			const EdgeIndex index = mDestroyedEdges[a];

			Edge& edge = mEdges[index];
			if (edge.isPendingDestroyed())
			{
				PartitionEdge* pEdge = mGpuData ? mGpuData->mFirstPartitionEdges[index] : NULL;
				if (pEdge)
				{
					mGpuData->mDestroyedPartitionEdges.pushBack(pEdge);
					mGpuData->mFirstPartitionEdges[index] = NULL; //Force first partition edge to NULL to ensure we don't have a clash
				}
				if (edge.isActive())
				{
					removeEdgeFromActivatingList(index); //TODO - can we remove this call? Can we handle this elsewhere, e.g. when destroying the nodes...
					mActiveEdgeCount[edge.mEdgeType]--;
				}

				edge = Edge(); //Reset edge
				if(mGpuData)
					mGpuData->mActiveContactEdges.growAndReset(index);
			}
		}

		mDestroyedEdges.forceSize_Unsafe(0);
	}

	{
		PX_PROFILE_ZONE("Basic.clearDestroyedNodes", mContextId);

		for (PxU32 a = 0; a < destroyedNodes.size(); ++a)
		{
			const PxNodeIndex nodeIndex = destroyedNodes[a];
			const IslandId islandId = mIslandIds[nodeIndex.index()];
			Node& node = mNodes[nodeIndex.index()];
			if (islandId != IG_INVALID_ISLAND)
			{
				Island& island = mIslands[islandId];

				removeNodeFromIsland(island, nodeIndex);

				mIslandIds[nodeIndex.index()] = IG_INVALID_ISLAND;

				PxU32 nodeCountTotal = 0;
				for (PxU32 t = 0; t < Node::eTYPE_COUNT; ++t)
				{
					nodeCountTotal += island.mNodeCount[t];
				}

				if (nodeCountTotal == 0)
				{
					mIslandHandles.freeHandle(islandId);
					if (island.mActiveIndex != IG_INVALID_ISLAND)
					{
						const IslandId replaceId = mActiveIslands[mActiveIslands.size() - 1];
						Island& replaceIsland = mIslands[replaceId];
						replaceIsland.mActiveIndex = island.mActiveIndex;
						mActiveIslands[island.mActiveIndex] = replaceId;
						mActiveIslands.forceSize_Unsafe(mActiveIslands.size() - 1);
						island.mActiveIndex = IG_INVALID_ISLAND;
						//island.mStaticTouchCount -= node.mStaticTouchCount; //Remove the static touch count from the island
						mIslandStaticTouchCount[islandId] -= node.mStaticTouchCount;
					}
					mIslandAwake.reset(islandId);
					island.mLastNode.setIndices(PX_INVALID_NODE);
					island.mRootNode.setIndices(PX_INVALID_NODE);
					island.mActiveIndex = IG_INVALID_ISLAND;
				}
			}

			if (node.isKinematic())
			{
				if (mActiveNodeIndex[nodeIndex.index()] != PX_INVALID_NODE)
				{
					//Remove from the active kinematics list...
					markKinematicInactive(nodeIndex);
				}
			}
			else
			{
				if (mActiveNodeIndex[nodeIndex.index()] != PX_INVALID_NODE)
				{
					markInactive(nodeIndex);
				}
			}

			//node.reset();
			node.mFlags |= Node::eDELETED;
		}
	}
	//Now we need to produce the list of active edges and nodes!!!

	//If we get here, we have a list of active islands. From this, we need to iterate over all active islands and establish if that island
	//can, in fact, go to sleep. In order to become deactivated, all nodes in the island must be ready for sleeping...

	if (allowDeactivation)
	{
		PX_PROFILE_ZONE("Basic.deactivation", mContextId);
		for (PxU32 a = 0; a < mActiveIslands.size(); a++)
		{
			const IslandId islandId = mActiveIslands[a];

			mIslandAwake.reset(islandId);
		}

		//Loop over the active kinematic nodes and tag all islands touched by active kinematics as awake
		for (PxU32 a = mActiveKinematicNodes.size(); a > 0; --a)
		{
			const PxNodeIndex kinematicIndex = mActiveKinematicNodes[a - 1];

			Node& kinematicNode = mNodes[kinematicIndex.index()];

			if (kinematicNode.isReadyForSleeping())
			{
				if (permitKinematicDeactivation)
				{
					kinematicNode.clearActive();
					markKinematicInactive(kinematicIndex);
				}
			}
			else //if(!kinematicNode.isReadyForSleeping())
			{
				//KS - if kinematic is active, then wake up all islands the kinematic is touching
				EdgeInstanceIndex edgeId = kinematicNode.mFirstEdgeIndex;
				while (edgeId != IG_INVALID_EDGE)
				{
					const EdgeInstance& instance = mEdgeInstances[edgeId];
					//Edge& edge = mEdges[edgeId/2];
					//Only wake up islands if a connection was present
					//if(edge.isConnected())
					{
						PxNodeIndex outNode = mCpuData.mEdgeNodeIndices[edgeId ^ 1];
						if (outNode.index() != PX_INVALID_NODE)
						{
							IslandId islandId = mIslandIds[outNode.index()];
							if (islandId != IG_INVALID_ISLAND)
							{
								mIslandAwake.set(islandId);
								PX_ASSERT(mIslands[islandId].mActiveIndex != IG_INVALID_ISLAND);
							}
						}
					}
					edgeId = instance.mNextEdge;
				}
			}
		}

		for (PxU32 a = mActiveIslands.size(); a > 0; --a)
		{
			const IslandId islandId = mActiveIslands[a - 1];

			const Island& island = mIslands[islandId];

			bool canDeactivate = !mIslandAwake.test(islandId);
			mIslandAwake.set(islandId);

			//If it was touched by an active kinematic in the above loop, we can't deactivate it.
			//Therefore, no point in testing the nodes in the island. They must remain awake
			if (canDeactivate)
			{
				PxNodeIndex nodeId = island.mRootNode;
				while (nodeId.index() != PX_INVALID_NODE)
				{
					Node& node = mNodes[nodeId.index()];
					if (!node.isReadyForSleeping())
					{
						canDeactivate = false;
						break;
					}
					nodeId = node.mNextNode;
				}
				if (canDeactivate)
				{
					//If all nodes in this island are ready for sleeping and there were no active 
					//kinematics interacting with the any bodies in the island, we can deactivate the island.
					deactivateIsland(islandId);
				}
			}
		}
	}

	{
		PX_PROFILE_ZONE("Basic.resetDirtyEdges", mContextId);
		for (PxU32 i = 0; i < Edge::eEDGE_TYPE_COUNT; ++i)
		{
			for (PxU32 a = 0; a < mDirtyEdges[i].size(); ++a)
			{
				Edge& edge = mEdges[mDirtyEdges[i][a]];
				edge.clearInDirtyList();
			}
			mDirtyEdges[i].clear(); //All new edges processed
		}
	}
}

IslandId IslandSim::mergeIslands(IslandId island0, IslandId island1, PxNodeIndex node0, PxNodeIndex node1)
{
	Island& is0 = mIslands[island0];
	Island& is1 = mIslands[island1];

	//We defer this process and do it later instead. That way, if we have some pathological 
	//case where multiple islands get merged repeatedly, we don't end up repeatedly remapping all the nodes in those islands 
	//to their new island. Instead, we just choose the largest island and remap the smaller island to that.

	PxU32 totalSize0 = 0;
	PxU32 totalSize1 = 0;

	for (PxU32 i = 0; i < Node::eTYPE_COUNT; ++i)
	{
		totalSize0 += is0.mNodeCount[i];
		totalSize1 += is1.mNodeCount[i];
	}
	if(totalSize0 > totalSize1)
	{
		mergeIslandsInternal(is0, is1, island0, island1, node0, node1);
		mIslandAwake.reset(island1);
		mIslandHandles.freeHandle(island1);
		mFastRoute[node1.index()] = node0;
		return island0;
	}
	else
	{
		mergeIslandsInternal(is1, is0, island1, island0, node1, node0);
		mIslandAwake.reset(island0);
		mIslandHandles.freeHandle(island0);
		mFastRoute[node0.index()] = node1;
		return island1;
	}
}

bool IslandSim::checkInternalConsistency() const
{
	//Loop over islands, confirming that the island data is consistent...
	//Really expensive. Turn off unless investigating some random issue...
#if 0
	for (PxU32 a = 0; a < mIslands.size(); ++a)
	{
		const Island& island = mIslands[a];

		PxU32 expectedNodeCount = 0;
		for (PxU32 t = 0; t < Node::eTYPE_COUNT; ++t)
		{
			expectedNodeCount += island.mNodeCount[t];
		}
		bool metLastNode = expectedNodeCount == 0;

		PxNodeIndex nodeId = island.mRootNode;

		while (nodeId.index() != PX_INVALID_NODE)
		{
			PX_ASSERT(mIslandIds[nodeId.index()] == a);

			if (nodeId.index() == island.mLastNode.index())
			{
				metLastNode = true;
				PX_ASSERT(mNodes[nodeId.index()].mNextNode.index() == PX_INVALID_NODE);
			}

			--expectedNodeCount;

			nodeId = mNodes[nodeId.index()].mNextNode;
		}

		PX_ASSERT(expectedNodeCount == 0);
		PX_ASSERT(metLastNode);
	}
#endif

	return true;
}

void IslandSim::mergeIslandsInternal(Island& island0, Island& island1, IslandId islandId0, IslandId islandId1, PxNodeIndex nodeIndex0, PxNodeIndex nodeIndex1)
{	
#if PX_ENABLE_ASSERTS
	PxU32 island0Size = 0;
	PxU32 island1Size = 0;
	for(PxU32 nodeType = 0; nodeType < Node::eTYPE_COUNT; ++nodeType)
	{
		island0Size += island0.mNodeCount[nodeType];
		island1Size += island1.mNodeCount[nodeType];
	}
#endif
	PX_ASSERT(island0Size >= island1Size); //We only ever merge the smaller island to the larger island
	//Stage 1 - we need to move all the nodes across to the new island ID (i.e. write all their new island indices, move them to the 
	//island and then also update their estimated hop counts to the root. As we don't want to do a full traversal at this point,
	//instead, we estimate based on the route from the node to their previous root and then from that root to the new connection
	//between the 2 islands. This is probably a very indirect route but it will be refined later.

	//In this case, island1 is subsumed by island0

	//It takes mHopCounts[nodeIndex1] to get from node1 to its old root. It takes mHopCounts[nodeIndex0] to get from nodeIndex0 to the new root
	//and it takes 1 extra hop to go from node1 to node0. Therefore, a sub-optimal route can be planned going via the old root node that should take
	//mHopCounts[nodeIndex0] + mHopCounts[nodeIndex1] + 1 + mHopCounts[nodeIndex] to travel from any arbitrary node (nodeIndex) in island1 to the root
	//of island2.

	const PxU32 extraPath = mHopCounts[nodeIndex0.index()] + mHopCounts[nodeIndex1.index()] + 1;

	PxNodeIndex islandNode = island1.mRootNode;
	while(islandNode.index() != PX_INVALID_NODE)
	{
		mHopCounts[islandNode.index()] += extraPath;
		mIslandIds[islandNode.index()] = islandId0;

		//mFastRoute[islandNode] = PX_INVALID_NODE;
		
		Node& node = mNodes[islandNode.index()];
		islandNode = node.mNextNode;				
	}

	//Now fill in the hop count for node1, which is directly connected to node0.
	mHopCounts[nodeIndex1.index()] = mHopCounts[nodeIndex0.index()] + 1;
	Node& lastNode = mNodes[island0.mLastNode.index()];
	Node& firstNode = mNodes[island1.mRootNode.index()];
	PX_ASSERT(lastNode.mNextNode.index() == PX_INVALID_NODE);
	PX_ASSERT(firstNode.mPrevNode.index() == PX_INVALID_NODE);
	PX_ASSERT(island1.mRootNode.index() != island0.mLastNode.index());

	PX_ASSERT(mNodes[island0.mLastNode.index()].mNextNode.index() == PX_INVALID_NODE);
	PX_ASSERT(mNodes[island1.mLastNode.index()].mNextNode.index() == PX_INVALID_NODE);

	PX_ASSERT(mIslandIds[island0.mLastNode.index()] == islandId0);

	lastNode.mNextNode = island1.mRootNode;
	firstNode.mPrevNode = island0.mLastNode;

	island0.mLastNode = island1.mLastNode;
	//island0.mStaticTouchCount += island1.mStaticTouchCount;
	mIslandStaticTouchCount[islandId0] += mIslandStaticTouchCount[islandId1];

	//Merge the edge list for the islands...
	for(PxU32 a = 0; a < IG::Edge::eEDGE_TYPE_COUNT; ++a)
	{
		if(island0.mLastEdge[a] != IG_INVALID_EDGE)
		{
			PX_ASSERT(mEdges[island0.mLastEdge[a]].mNextIslandEdge == IG_INVALID_EDGE);
			mEdges[island0.mLastEdge[a]].mNextIslandEdge = island1.mFirstEdge[a];
		}
		else
		{
			PX_ASSERT(island0.mFirstEdge[a] == IG_INVALID_EDGE);
			island0.mFirstEdge[a] = island1.mFirstEdge[a];
		}
		if(island1.mFirstEdge[a] != IG_INVALID_EDGE)
		{
			PX_ASSERT(mEdges[island1.mFirstEdge[a]].mPrevIslandEdge == IG_INVALID_EDGE);
			mEdges[island1.mFirstEdge[a]].mPrevIslandEdge = island0.mLastEdge[a];
			island0.mLastEdge[a] = island1.mLastEdge[a];
		}

		island0.mEdgeCount[a] += island1.mEdgeCount[a];
		island1.mFirstEdge[a] = IG_INVALID_EDGE;
		island1.mLastEdge[a] = IG_INVALID_EDGE;
		island1.mEdgeCount[a] = 0;
	}

	for (PxU32 a = 0; a < IG::Node::eTYPE_COUNT; ++a)
	{
		island0.mNodeCount[a] += island1.mNodeCount[a];
		island1.mNodeCount[a] = 0;
	}

	island1.mLastNode.setIndices(PX_INVALID_NODE);
	island1.mRootNode.setIndices(PX_INVALID_NODE);
	
	mIslandStaticTouchCount[islandId1] = 0;
	//island1.mStaticTouchCount = 0;
	
	//Remove from active island list
	if(island1.mActiveIndex != IG_INVALID_ISLAND)
		markIslandInactive(islandId1);
}

void IslandSim::removeEdgeFromActivatingList(EdgeIndex index)
{
	Edge& edge = mEdges[index];

	if (edge.mEdgeState & Edge::eACTIVATING)
	{
		for (PxU32 a = 0, count = mActivatedEdges[edge.mEdgeType].size(); a < count; a++)
		{
			if (mActivatedEdges[edge.mEdgeType][a] == index)
			{
				mActivatedEdges[edge.mEdgeType].replaceWithLast(a);
				break;
			}
		}

		edge.mEdgeState &= (~Edge::eACTIVATING);
	}

	const PxNodeIndex nodeIndex1 = mCpuData.mEdgeNodeIndices[index * 2];
	const PxNodeIndex nodeIndex2 = mCpuData.mEdgeNodeIndices[index * 2 + 1];

	if (nodeIndex1.isValid() && nodeIndex2.isValid())
	{
		mNodes[nodeIndex1.index()].mActiveRefCount--;
		mNodes[nodeIndex2.index()].mActiveRefCount--;
	}

	if(mGpuData && edge.mEdgeType == Edge::eCONTACT_MANAGER)
		mGpuData->mActiveContactEdges.reset(index);
}

void IslandSim::setKinematic(PxNodeIndex nodeIndex)
{
	Node& node = mNodes[nodeIndex.index()];

	if(!node.isKinematic())
	{
		//Transition from dynamic to kinematic:
		//(1) Remove this node from the island
		//(2) Remove this node from the active node list
		//(3) If active or referenced, add it to the active kinematic list
		//(4) Tag the node as kinematic
		//External code will re-filter interactions and lost touches will be reported

		const IslandId islandId = mIslandIds[nodeIndex.index()];
		PX_ASSERT(islandId != IG_INVALID_ISLAND);

		Island& island = mIslands[islandId];

		mIslandIds[nodeIndex.index()] = IG_INVALID_ISLAND;

		removeNodeFromIsland(island, nodeIndex);

		const bool isActive = node.isActive()!=0;

		if (isActive)
		{
			//Remove from active list...
			markInactive(nodeIndex);
		}
		else if (node.isActivating())
		{
			//Remove from activating list...
			node.clearActivating();
			PX_ASSERT(mActivatingNodes[mActiveNodeIndex[nodeIndex.index()]].index() == nodeIndex.index());

			const PxNodeIndex replaceIndex = mActivatingNodes[mActivatingNodes.size() - 1];
			mActiveNodeIndex[replaceIndex.index()] = mActiveNodeIndex[nodeIndex.index()];
			mActivatingNodes[mActiveNodeIndex[nodeIndex.index()]] = replaceIndex;
			mActivatingNodes.forceSize_Unsafe(mActivatingNodes.size() - 1);
			mActiveNodeIndex[nodeIndex.index()] = PX_INVALID_NODE;
		}

		node.setKinematicFlag();

		node.clearActive();

		if (/*isActive || */node.mActiveRefCount != 0)
		{
			//Add to active kinematic list...
			PX_ASSERT(mActiveNodeIndex[nodeIndex.index()] == PX_INVALID_NODE);

			mActiveNodeIndex[nodeIndex.index()] = mActivatingNodes.size();
			mActivatingNodes.pushBack(nodeIndex);
			node.setActivating();
		}

		{
			//This node was potentially in an island with other bodies. We need to force an island recomputation in case the
			//islands became broken due to losing this connection. Same rules as losing a contact, we just 
			//tag the nodes directly connected to the lost edge as "dirty" and force an island recomputation if 
			//it resulted in lost connections
			EdgeInstanceIndex edgeId = node.mFirstEdgeIndex;
			while(edgeId != IG_INVALID_EDGE)
			{
				const EdgeInstance& instance = mEdgeInstances[edgeId];
				const EdgeInstanceIndex nextId = instance.mNextEdge;

				const PxU32 idx = edgeId/2;
				IG::Edge& edge = mEdges[edgeId/2];

				removeEdgeFromIsland(island, idx);

				removeConnectionInternal(idx);
				removeConnectionFromGraph(idx);

				edge.clearInserted();

				if (edge.isActive())
				{
					removeEdgeFromActivatingList(idx);
					edge.deactivateEdge();
					mActiveEdgeCount[edge.mEdgeType]--;
					mDeactivatingEdges[edge.mEdgeType].pushBack(idx);
				}

				if(!edge.isPendingDestroyed())
				{
					if(!edge.isInDirtyList())
					{
						PX_ASSERT(!contains(mDirtyEdges[edge.mEdgeType], idx));
						mDirtyEdges[edge.mEdgeType].pushBack(idx);
						edge.markInDirtyList();
					}
				}
				else
				{
					edge.setReportOnlyDestroy();
				}

				edgeId = nextId;
			}
		}

		PxU32 newNodeCount = 0;
		for(PxU32 i = 0; i < Node::eTYPE_COUNT; ++i)
			newNodeCount += island.mNodeCount[i];

		if(newNodeCount == 0)
		{
			// If this island is empty after having removed the edges of the node we've just set to kinematic
			// we invalidate all edges and set the island to inactive
			for(PxU32 a = 0; a < Edge::eEDGE_TYPE_COUNT; ++a)
			{
				island.mFirstEdge[a] = island.mLastEdge[a] = IG_INVALID_EDGE;
				island.mEdgeCount[a] = 0;
				mIslandStaticTouchCount[islandId] = 0;
				//island.mStaticTouchCount = 0;
			}

			if(island.mActiveIndex != IG_INVALID_ISLAND)
			{
				markIslandInactive(islandId);
			}

			mIslandAwake.reset(islandId);
			mIslandHandles.freeHandle(islandId);
		}
	}
}

void IslandSim::setDynamic(PxNodeIndex nodeIndex)
{
	//(1) Remove all edges involving this node from all islands they may be in
	//(2) Mark all edges as "new" edges - let island gen re-process them!
	//(3) Remove this node from the active kinematic list
	//(4) Add this node to the active dynamic list (if it is active)
	//(5) Mark node as dynamic
	
	Node& node = mNodes[nodeIndex.index()];

	if(node.isKinematic())
	{
		//EdgeInstanceIndex edgeIndex = node.mFirstEdgeIndex;

		EdgeInstanceIndex edgeId = node.mFirstEdgeIndex;
		while(edgeId != IG_INVALID_EDGE)
		{
			const EdgeInstance& instance = mEdgeInstances[edgeId];
			const EdgeInstanceIndex nextId = instance.mNextEdge;

			const PxNodeIndex otherNode = mCpuData.mEdgeNodeIndices[edgeId^1];

			const PxU32 idx = edgeId/2;
			IG::Edge& edge = mEdges[edgeId/2];

			if(!otherNode.isStaticBody())
			{
				const IslandId islandId = mIslandIds[otherNode.index()];
				if(islandId != IG_INVALID_ISLAND)
					removeEdgeFromIsland(mIslands[islandId], idx);
			}

			removeConnectionInternal(idx);
			removeConnectionFromGraph(idx);

			edge.clearInserted();
			if (edge.isActive())
			{
				edge.deactivateEdge();
				removeEdgeFromActivatingList(idx);
				mActiveEdgeCount[edge.mEdgeType]--;
			}
			
			if(!edge.isPendingDestroyed())
			{
				if(!edge.isInDirtyList())
				{
					PX_ASSERT(!contains(mDirtyEdges[edge.mEdgeType], idx));
					mDirtyEdges[edge.mEdgeType].pushBack(idx);
					edge.markInDirtyList();
				}
			}
			else
			{
				edge.setReportOnlyDestroy();
			}

			edgeId = nextId;
		}

		if(!node.isActivating() && mActiveNodeIndex[nodeIndex.index()] != PX_INVALID_NODE)
		{
			//Remove from active kinematic list, add to active dynamic list
			const PxU32 oldRefCount = node.mActiveRefCount;
			node.mActiveRefCount = 0;
			markKinematicInactive(nodeIndex);
			node.mActiveRefCount = oldRefCount;
		}

		node.clearKinematicFlag();

		//Create an island for this node. If there are any edges affecting this node, they will have been marked as 
		//"new" and will be processed next island update.
		{
			const IslandId islandHandle = mIslandHandles.getHandle();
			
			if(islandHandle == mIslands.capacity())
			{
				const PxU32 newCapacity = 2*mIslands.capacity()+1;
				mIslands.reserve(newCapacity);
				mIslandAwake.resize(newCapacity);
				mIslandStaticTouchCount.resize(newCapacity);
			}
			mIslandAwake.reset(islandHandle);
			mIslands.resize(PxMax(islandHandle+1, mIslands.size()));
			mIslandStaticTouchCount.resize(PxMax(islandHandle + 1, mIslands.size()));
			Island& island = mIslands[islandHandle];
			island.mLastNode = island.mRootNode = nodeIndex;
			PX_ASSERT(mNodes[nodeIndex.index()].mNextNode.index() == PX_INVALID_NODE);
			island.mNodeCount[node.mType] = 1;
			mIslandIds[nodeIndex.index()] = islandHandle;
			mIslandStaticTouchCount[islandHandle] = 0;

			if(node.isActive())
			{
				node.clearActive();

				activateNode(nodeIndex);
			}
		}
	}
}