void AABBTree::updateBodies()
	{
		for(auto it = bodiesNode.begin(); it!=bodiesNode.end();)
		{
			bool removed = false;
			AbstractWorkBody *body = it->first;
			if(body->isActive())
			{
				AABBNode *leaf = it->second;
				const AABBox<float> &leafFatAABBox = leaf->getAABBox();
				const AABBox<float> &bodyAABBox = leaf->getBodyNodeData()->retrieveBodyAABBox();
				if(!leafFatAABBox.include(bodyAABBox))
				{
					PairContainer *alternativePairContainer = leaf->getBodyNodeData()->getAlternativePairContainer();

					removeNode(leaf);
					it = bodiesNode.erase(it);
					removed = true;

					addBody(body, alternativePairContainer);
				}
			}

			if(!removed)
			{
				++it;
			}
		}
	}
	void SweepAndPrune::updateBodies()
	{
		for(std::map<AbstractWorkBody *, BodyBox *>::iterator it = bodiesBox.begin(); it!=bodiesBox.end(); ++it)
		{
			AbstractWorkBody *body = it->first;
			if(body->isActive())
			{
				BodyBox *bodyBox = it->second;
				const AABBox<float> &aabbox = body->getShape()->toAABBox(body->getPhysicsTransform());

				for(unsigned int axis=0; axis<3; ++axis)
				{
					unsigned int minEndPointIndex = bodyBox->min[axis];
					unsigned int maxEndPointIndex = bodyBox->max[axis];

					const float aabboxMin = aabbox.getMin()[axis] - contactBreakingThreshold;
					const float aabboxMax = aabbox.getMax()[axis] + contactBreakingThreshold;
					const float minDelta = aabboxMin - sortedEndPoints[axis][minEndPointIndex].getValue();
					const float maxDelta = aabboxMax - sortedEndPoints[axis][maxEndPointIndex].getValue();

					sortedEndPoints[axis][bodyBox->min[axis]].setValue(aabboxMin);
					sortedEndPoints[axis][bodyBox->max[axis]].setValue(aabboxMax);

					//1. add overlap if necessary
					if(minDelta < 0.0f)
					{ //new minimum point is smaller
						sortMinDown(axis, minEndPointIndex, true);
					}
					if(maxDelta > 0.0f)
					{ //new maximum point is bigger
						sortMaxUp(axis, maxEndPointIndex, true);
					}

					//2. remove overlap if necessary
					if(minDelta > 0.0f)
					{ //new minimum point is bigger
						sortMinUp(axis, minEndPointIndex, true);
					}
					if(maxDelta < 0.0f)
					{ //new maximum point is smaller
						sortMaxDown(axis, maxEndPointIndex, true);
					}
				}
			}
		}

		#ifdef _DEBUG
			//printSortedEndPoints();
		#endif
	}