/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
* 
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
* 
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 
* 3. This notice may not be removed or altered from any source distribution.
*/

#include "dgPhysicsStdafx.h"
#include "dgWorld.h"

#include "dgCollisionBox.h"
#include "dgCollisionNull.h"
#include "dgCollisionCone.h"
#include "dgMinkowskiConv.h"
#include "dgCollisionScene.h"
#include "dgCollisionSphere.h"
#include "dgCollisionEllipse.h"
#include "dgCollisionCapsule.h"
#include "dgCollisionCylinder.h"
#include "dgCollisionCompound.h"
#include "dgWorldDynamicUpdate.h"
#include "dgCollisionConvexHull.h"
#include "dgCollisionConvexModifier.h"
#include "dgCollisionChamferCylinder.h"

#include "dgUserConstraint.h"
#include "dgBallConstraint.h"
#include "dgHingeConstraint.h"
#include "dgSlidingConstraint.h"
#include "dgUpVectorConstraint.h"
#include "dgUniversalConstraint.h"
#include "dgCorkscrewConstraint.h"
#include "dgConnectorConstraint.h"
//#include "dgPointToCurveConstraint.h"



#define DG_INITIAL_ISLAND_SIZE			(1024 * 4)
#define DG_INITIAL_BODIES_SIZE			(1024 * 4)
#define DG_INITIAL_JOINTS_SIZE			(1024 * 4)
#define DG_INITIAL_JACOBIAN_SIZE		(1024 * 16)
#define DG_INITIAL_CONTATCT_SIZE		(1024 * 64)


/*
static  char *xxx[10] = {"bbbbbbbbbb",
						 "baaaaaaaab",
						 "babbbbbaab",
					 	 "babaaabaab",
						 "baaababaab",
						 "bbbaaabaab",
						 "bbbbaabaab",
						 "babbbbbaab",
						 "baaaaaaaab",
						 "bbbbbbbbbb"};

struct myPath: public dgPathFinder<dgInt32, float> 
{
	dgInt32 goalx;
	dgInt32 goaly;
	myPath ()
		:dgPathFinder<dgInt32, float>(1024, 10)
	{
		for (int i = 0; i < 10; i ++) {
			strcpy (&m_map[i][0], xxx[i]);
		}
	}

	const dgPathNode<dgInt32, float>* CalCulatePath (dgInt32 source, dgInt32 goal)
	{
		goalx = goal % 10;
		goaly = goal / 10;
		return dgPathFinder<dgInt32, float>::CalCulatePath (source, goal) ;
	}


	float GetCostFromParent(const dgPathNode<dgInt32, float>& node) const
	{
		dgInt32 x;
		dgInt32 y;
		dgInt32 x0;
		dgInt32 y0;
		dgInt32 x1;
		dgInt32 y1;
		dgInt32 id;

		id = node.GetId();
		x = id % 10;
		y = id / 10;

		const dgPathNode<dgInt32, float>* parent = node.GetParent();
		id = parent->GetId(); 
		x0 = id % 10;
		y0 = id / 10;

		const dgPathNode<dgInt32, float>* grandParent = parent->GetParent();
		x1 = 2 * x0 - x;
		y1 = 2 * y0 - y;
		if (grandParent) {
			id = grandParent->GetId(); 
			x1 = id % 10;
			y1 = id / 10;
		}

		dgInt32 dx0;
		dgInt32 dy0;
		dgInt32 dx1;
		dgInt32 dy1;
		float penalty;

		dx0 = x0 - x;
		dy0 = y0 - y;
		dx1 = x1 - x0;
		dy1 = y1 - y0;
		penalty = 0.0f;
		if (dx1 * dy0 - dx0 * dy1)	{
			penalty = dgFloat32(1.0f);
		}

		static dgInt32 xxxx;
		if (!xxxx){
			xxxx = 1;
			penalty = 9.1f;
		}

		return (xxx[y][x] == 'a') ? (dgFloat32(1.0f) + penalty): 50.0f;
	}

	float GetEstimatedCostToGoal(dgInt32 id) const
	{
		dgInt32 x;
		dgInt32 y;

		x = id % 10 - goalx;
		y = id / 10 - goaly;
		return dgSqrt ((float)(x * x + y * y));
	}

	dgInt32 EnumerateChildren(dgInt32 parent, dgInt32 array[]) const
	{
		dgInt32 x;
		dgInt32 y;

		x = parent % 10;
		y = parent / 10;

		array[0]	= (y - 1) * 10 + x;
		array[1]	= (y - 0) * 10 + x - 1;
		array[2]	= (y + 1) * 10 + x;
		array[3]	= (y + 0) * 10 + x + 1;
		return 4;
	}

	char m_map[20][20];
};


void xxxxx()
{
	myPath path; 
	const dgPathNode<dgInt32, float>* firtNode;
	for (firtNode = path.CalCulatePath (5 * 10 + 3, 4 * 10 + 8); firtNode; firtNode= firtNode->GetNext()) {
		dgInt32 id;
		dgInt32 x;
		dgInt32 y;

		id = firtNode->GetId();
		x = id % 10;
		y = id / 10;
		path.m_map[y][x] = '_';
	}
}
*/



//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////


dgWorld::dgWorld(dgMemoryAllocator* allocator):
//	dgThreadHive(),
	dgBodyMasterList(allocator),
	dgBroadPhaseCollision(allocator),
	dgBodyMaterialList(allocator),
	dgBodyCollisionList(allocator),
	dgActiveContacts(allocator), 
	dgCollidingPairCollector(),
	m_perInstanceData(allocator),
	m_threadsManager(),
	m_dynamicSolver()
{
	dgInt32 steps;
	dgFloat32 freezeAccel2;
	dgFloat32 freezeAlpha2;
	dgFloat32 freezeSpeed2;
	dgFloat32 freezeOmega2;

	// init exact arithmetic functions
	m_allocator = allocator;

//dgThreadHive::SetThreadCount(16);
//dgThreadHive::SetThreadCount(32);

//_control87 (_EM_ZERODIVIDE | _EM_INEXACT | _EM_OVERFLOW | _EM_INVALID, _MCW_EM);


	m_inUpdate = 0;
	m_bodyGroupID = 0;
//	m_activeBodiesCount = 0;
	
	m_defualtBodyGroupID = CreateBodyGroupID();
	m_islandMemorySizeInBytes = DG_INITIAL_ISLAND_SIZE;
	m_bodiesMemorySizeInBytes = DG_INITIAL_BODIES_SIZE;
	m_jointsMemorySizeInBytes = DG_INITIAL_JOINTS_SIZE;

	m_pairMemoryBufferSizeInBytes = 1024 * 64 * sizeof (void*);
	m_pairMemoryBuffer = m_allocator->MallocLow (m_pairMemoryBufferSizeInBytes);
	m_islandMemory = m_allocator->MallocLow (m_islandMemorySizeInBytes); 
	m_jointsMemory = m_allocator->MallocLow (m_jointsMemorySizeInBytes); 
	m_bodiesMemory = m_allocator->MallocLow (m_bodiesMemorySizeInBytes); 
	for (dgInt32 i = 0; i < DG_MAXIMUN_THREADS; i ++) {
		m_jacobiansMemorySizeInBytes[i] = DG_INITIAL_JACOBIAN_SIZE;
		m_jacobiansMemory[i] = m_allocator->MallocLow (m_jacobiansMemorySizeInBytes[i]);  

		m_internalForcesMemorySizeInBytes[i] = DG_INITIAL_BODIES_SIZE;
		m_internalForcesMemory[i] = m_allocator->MallocLow (m_internalForcesMemorySizeInBytes[i]);  

		m_contactBuffersSizeInBytes[i] = DG_INITIAL_CONTATCT_SIZE;
		m_contactBuffers[i] = m_allocator->MallocLow (m_contactBuffersSizeInBytes[i]);  
	}

	m_genericLRUMark = 0;
	m_singleIslandMultithreading = 1;

	m_solverMode = 0;
	m_frictionMode = 0;
	m_dynamicsLru = 0;
	m_broadPhaseLru = 0;
		
	m_bodiesUniqueID = 0;
//	m_bodiesCount = 0;
	m_frictiomTheshold = dgFloat32 (0.25f);

	m_userData = NULL;
	m_islandUpdate = NULL;
	m_destroyCollision = NULL;
	m_leavingWorldNotify = NULL;
	m_destroyBodyByExeciveForce = NULL;

	m_freezeAccel2 = DG_FREEZE_MAG2;
	m_freezeAlpha2 = DG_FREEZE_MAG2;
	m_freezeSpeed2 = DG_FREEZE_MAG2 * dgFloat32 (0.1f);
	m_freezeOmega2 = DG_FREEZE_MAG2 * dgFloat32 (0.1f);

	steps = 1;
	freezeAccel2 = m_freezeAccel2;
	freezeAlpha2 = m_freezeAlpha2;
	freezeSpeed2 = m_freezeSpeed2;
	freezeOmega2 = m_freezeOmega2;
	for (dgInt32 i = 0; i < DG_SLEEP_ENTRIES; i ++) {
		m_sleepTable[i].m_maxAccel = freezeAccel2;
		m_sleepTable[i].m_maxAlpha = freezeAlpha2;
		m_sleepTable[i].m_maxVeloc = freezeSpeed2;
		m_sleepTable[i].m_maxOmega = freezeOmega2;
		m_sleepTable[i].m_steps = steps;
		steps += 7;
		freezeAccel2 *= dgFloat32 (1.5f);
		freezeAlpha2 *= dgFloat32 (1.4f);
		freezeSpeed2 *= dgFloat32 (1.5f);
		freezeOmega2 *= dgFloat32 (1.5f);
	}

	steps += 300;
	m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAccel *= dgFloat32 (100.0f);
	m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAlpha *= dgFloat32 (100.0f);
	m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxVeloc = 0.25f;
	m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxOmega = 0.1f;
	m_sleepTable[DG_SLEEP_ENTRIES - 1].m_steps = steps;


	m_cpu = dgNoSimdPresent;
	m_numberOfTheads = 1;

	SetHardwareMode (1);
	SetThreadsCount (DG_MAXIMUN_THREADS);
	m_maxTheads = m_numberOfTheads;

	SetHardwareMode (0);
	SetThreadsCount (1);

	dgBroadPhaseCollision::Init ();
	dgCollidingPairCollector::Init ();
	
	m_pointCollision = new (m_allocator) dgCollisionPoint(m_allocator);
	AddSentinelBody();
	SetPerfomanceCounter(NULL);
}

dgWorld::~dgWorld()
{	
	DestroyAllBodies();
	RemoveAllGroupID();
	m_destroyCollision = NULL;
	ReleaseCollision(m_pointCollision);
	DestroyBody (m_sentionelBody);

	m_allocator->FreeLow (m_jointsMemory); 
	m_allocator->FreeLow (m_bodiesMemory);  
	m_allocator->FreeLow (m_islandMemory);
	m_allocator->FreeLow (m_pairMemoryBuffer);
	for (dgInt32 i = 0; i < DG_MAXIMUN_THREADS; i ++) {
		m_allocator->FreeLow (m_jacobiansMemory[i]);  
		m_allocator->FreeLow (m_internalForcesMemory[i]);  
		m_allocator->FreeLow (m_contactBuffers[i]);  
	}
}

dgUnsigned32 dgWorld::GetPerformanceCount ()
{
	return 0;
}

void dgWorld::AddSentinelBody()
{
	dgCollision* collision;

	collision = new  (m_allocator) dgCollisionNull (m_allocator, 0x4352fe67);
	m_sentionelBody = CreateBody(collision, dgGetIdentityMatrix());
	ReleaseCollision(collision);

//	dgBodyMasterList::m_sentinel = m_sentionelBody;
	dgCollidingPairCollector::m_sentinel = m_sentionelBody;
}

dgBody* dgWorld::GetSentinelBody() const
{
	return m_sentionelBody;
}


void dgWorld::SetSolverMode (dgInt32 mode)
{
	m_solverMode = dgUnsigned32 (GetMax (0, mode));
}

void dgWorld::SetFrictionMode (dgInt32 mode)
{
	m_frictionMode = dgUnsigned32 (mode);
}

void dgWorld::SetHardwareMode (dgInt32 mode)
{
	m_cpu = dgNoSimdPresent;
	if (mode) {
		m_cpu = dgGetCpuType ();
	}
}

dgInt32 dgWorld::GetHardwareMode(char* description)
{
	dgInt32 mode;

//	if (0) {
//	} else {
		if (m_cpu == dgNoSimdPresent){
			mode = 0;
			if (description) {
				sprintf (description, "x87");
			}
		} else {
			mode = 1;
			if (description) {
				sprintf (description, "simd");
			}
		}
//	}
	return mode;
}





void dgWorld::SetThreadsCount (dgInt32 count)
{
//count = 1;

	m_threadsManager.CreateThreaded (count);
	m_numberOfTheads = dgUnsigned32 (m_threadsManager.GetThreadCount());
	
}

dgInt32 dgWorld::GetThreadsCount () const
{
	return dgInt32 (m_numberOfTheads);
}

dgInt32 dgWorld::GetMaxThreadsCount () const
{
	return dgInt32 (m_maxTheads);
}


void dgWorld::EnableThreadOnSingleIsland(dgInt32 mode)
{
	// for now disable this option
//	m_singleIslandMultithreading = 1;
	m_singleIslandMultithreading = mode ? 1 : 0;
}

dgInt32 dgWorld::GetThreadOnSingleIsland() const
{
	return m_singleIslandMultithreading ? 1 : 0;
}


void dgWorld::SetFrictionThreshold (dgFloat32 acceleration)
{
	m_frictiomTheshold = GetMax (dgFloat32(1.0e-2f), acceleration);
}


void dgWorld::RemoveAllGroupID()
{
	while (dgBodyMaterialList::GetCount()) {
		dgBodyMaterialList::Remove (dgBodyMaterialList::GetRoot());
	}
	m_bodyGroupID = 0;
	m_defualtBodyGroupID = CreateBodyGroupID();
}

void dgWorld::DestroyAllBodies ()
{
	dgBody* body;
	dgBodyMasterList::dgListNode* node;

	dgBodyMasterList& me = *this;
//	dgBodyMasterList::Iterator iter(me);
//	for (iter.Begin(); iter; ) {

	_ASSERTE (dgBodyMasterList::GetFirst()->GetInfo().GetBody() == m_sentionelBody);
	for (node = me.GetFirst()->GetNext(); node; ) {
		body = node->GetInfo().GetBody();
		node = node->GetNext();
		DestroyBody (body);
	}

	_ASSERTE (me.GetFirst()->GetInfo().GetCount() == 0);
	_ASSERTE (dgBodyCollisionList::GetCount() == 0);
}

dgBody* dgWorld::CreateBody(dgCollision* const collision, const dgMatrix& matrix)
{
	dgBody* body;

	_ASSERTE (collision);

	body = new (m_allocator) dgBody();
	_ASSERTE ((sizeof (dgBody) & 0xf) == 0);
	_ASSERTE ((dgUnsigned64 (body) & 0xf) == 0);

	memset (body, 0, sizeof (dgBody));

//	m_bodiesCount ++;
	m_bodiesUniqueID ++;


	body->m_world = this;

	body->m_freeze = false;
	body->m_sleeping = false;
	body->m_autoSleep = true;
	body->m_isInWorld = true;
	body->m_equilibrium = false;
	body->m_continueCollisionMode = false;
	body->m_collideWithLinkedBodies = true;
	body->m_solverInContinueCollision = false;
	body->m_spawnnedFromCallback = dgUnsigned32 (m_inUpdate ? true : false);
	body->m_uniqueID = dgInt32 (m_bodiesUniqueID);


	dgBodyMasterList::AddBody(body);

//	dgBodyActiveList___::AddBody(body);
//	_ASSERTE (body->m_activeNode);


//	body->m_freezeAccel2 = m_freezeAccel2;
//	body->m_freezeAlpha2 = m_freezeAlpha2;
//	body->m_freezeSpeed2 = m_freezeSpeed2;
//	body->m_freezeOmega2 = m_freezeOmega2;

	body->SetCentreOfMass (dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f))); 
	body->SetLinearDamping (dgFloat32 (0.1045f)) ;
	body->SetAngularDamping (dgVector (dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.0f)));

	body->AttachCollision(collision);
	body->m_bodyGroupId = dgInt32 (m_defualtBodyGroupID);

	body->SetMassMatrix (DG_INFINITE_MASS * dgFloat32 (2.0f), DG_INFINITE_MASS, DG_INFINITE_MASS, DG_INFINITE_MASS);
	dgBroadPhaseCollision::Add (body);

	//body->SetMatrix (dgGetIdentityMatrix());
	body->SetMatrix (matrix);
	body->m_invWorldInertiaMatrix[3][3] = dgFloat32 (1.0f);
	return body;
}

void dgWorld::DestroyBody(dgBody* const body)
{
	if (body->m_destructor) {
		body->m_destructor (*body);
	}

	dgBroadPhaseCollision::Remove (body);

//	m_bodiesCount --;
//	while (body->m_firstConstraintLink) {
//		DestroyConstraint (body->m_firstConstraintLink->m_constraint);
//	}
	dgBodyMasterList::RemoveBody (body);

	_ASSERTE (body->m_collision);
	ReleaseCollision (body->m_collision);

	delete body;
}


void dgWorld::DestroyConstraint(dgConstraint* const constraint)
{
	RemoveConstraint (constraint);
	delete constraint;
}

/*
void dgWorld::UnfreezeBody (dgBody *body)
{
	if (body->m_activation && !body->m_active) {
		body->m_activation (*body, 1);
	}
	body->m_active = true;

}

void dgWorld::FreezeBody (dgBody *body)
{
	_ASSERTE (0);
	if (body->m_activeNode) {
		_ASSERTE (0);
		dgBodyActiveList___::RemoveBody(body);
		if (body->m_activation && body->m_active) {
			body->m_activation (*body, 0);
		}
		body->m_active = false;
	}
}
*/


void dgWorld::UpdateCollision ()
{
	dgFloat32 timestep;
	_ASSERTE (m_inUpdate == 0);

	
	m_threadsManager.ClearTimers();
	memset (m_perfomanceCounters, 0, sizeof (m_perfomanceCounters));
	dgUnsigned32 ticks = m_getPerformanceCount();


	m_inUpdate ++;
	_ASSERTE (m_numberOfTheads  >= 1);

#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
	#ifndef __USE_DOUBLE_PRECISION__
		dgUnsigned32 controlWorld = dgControlFP (0xffffffff, 0);
		dgControlFP (_PC_53, _MCW_PC);
	#endif
#endif

	timestep = dgFloat32 (0.0f);

	if (m_cpu == dgSimdPresent) {
#ifdef DG_BUILD_SIMD_CODE	
		simd_env rounding;
		rounding = simd_get_ctrl();
		simd_set_FZ_mode();

		UpdateContactsSimd (timestep, true);
		simd_set_ctrl (rounding);
#endif

	} else {
		UpdateContacts (timestep, true);
	}
	m_inUpdate --;

#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
	#ifndef __USE_DOUBLE_PRECISION__
		dgControlFP (controlWorld, _MCW_PC);
	#endif
#endif

	m_perfomanceCounters[m_worldTicks] = m_getPerformanceCount() - ticks;
}



void dgWorld::Update (dgFloat32 timestep)
{
	dgUnsigned32 ticks;
//timestep = 1.0f/ 60.0f;
//timestep = 1.0f/ 120.0f;
//timestep = 1.0f/ 180.0f;
//timestep = 1.0f/ 240.0f;
//timestep = 1.0f/ 300.0f;
//timestep = 1.0f/ 600.0f;
//timestep = 1.0f/ 1000.0f;

//m_cpu = dgNoSimdPresent;
//m_cpu = dgSimdPresent;
//m_solverMode = 1;

//xxxxx();

//static int xxx;
//dgTrace (("pass %d\n", xxx));
//xxx ++;

//m_cpu = dgNoSimdPresent;
	#ifdef _LINUX_VER
//		m_cpu = dgNoSimdPresent;
	#endif

	_ASSERTE (m_inUpdate == 0);

	m_threadsManager.ClearTimers();
	memset (m_perfomanceCounters, 0, sizeof (m_perfomanceCounters));

	ticks = m_getPerformanceCount();

	m_destroyeddBodiesPool.m_count = 0;
	
	m_inUpdate ++;
	_ASSERTE (m_numberOfTheads  >= 1);

#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
	#ifndef __USE_DOUBLE_PRECISION__
		dgUnsigned32 controlWorld = dgControlFP (0xffffffff, 0);
		dgControlFP (_PC_53, _MCW_PC);
	#endif
#endif

	if (m_cpu == dgSimdPresent) {
#ifdef DG_BUILD_SIMD_CODE	
			simd_env rounding = simd_get_ctrl();
			simd_set_FZ_mode();

			UpdateContactsSimd (timestep, false);
			m_dynamicSolver.UpdateDynamics (this, 1, timestep);

			simd_set_ctrl (rounding);
#endif

	} else {
		UpdateContacts (timestep, false);
		m_dynamicSolver.UpdateDynamics (this, 0, timestep);
	}
	m_inUpdate --;


#if (defined (_WIN_32_VER) || defined (_WIN_64_VER))
	#ifndef __USE_DOUBLE_PRECISION__
		dgControlFP (controlWorld, _MCW_PC);
	#endif
#endif

	if (m_destroyBodyByExeciveForce) {
		for (dgInt32 i = 0; i < m_destroyeddBodiesPool.m_count; i ++) {
			m_destroyBodyByExeciveForce (m_destroyeddBodiesPool.m_bodies[i], m_destroyeddBodiesPool.m_joint[i]);
		}
	}


	m_perfomanceCounters[m_worldTicks] = m_getPerformanceCount() - ticks;
}

OnGetPerformanceCountCallback dgWorld::GetPerformaceFuntion() const
{
	return m_getPerformanceCount;
};

void dgWorld::SetPerfomanceCounter(OnGetPerformanceCountCallback callback)
{
	m_threadsManager.SetPerfomanceCounter (callback);

	if (!callback) {
		callback = GetPerformanceCount;
	}
	m_getPerformanceCount = callback;
	memset (m_perfomanceCounters, 0, sizeof (m_perfomanceCounters));
}

//dgUnsigned32 dgWorld::GetPerfomanceTicks (dgInt32 thread, dgUnsigned32 entry) const
dgUnsigned32 dgWorld::GetPerfomanceTicks (dgUnsigned32 entry) const
{
	entry = ClampValue(dgUnsigned32 (entry), dgUnsigned32 (0), dgUnsigned32 (m_counterSize - 1));
	return m_perfomanceCounters[entry];

}

dgUnsigned32 dgWorld::GetThreadPerfomanceTicks (dgUnsigned32 threadIndex) const
{
	return m_threadsManager.GetPerfomanceTicks (threadIndex);
}

void dgWorld::SetUserData (void* const userData)
{
	m_userData = userData;
}

void* dgWorld::GetUserData() const
{
	return m_userData;
}


void dgWorld::SetIslandUpdateCallback (OnIslandUpdate callback)
{
	m_islandUpdate = callback;
}

void dgWorld::SetDestroyCollisionCallback (OnDestroyCollision callback)
{
	m_destroyCollision = callback;
}

void dgWorld::SetLeavingWorldCallback (OnLeavingWorldAction callback) 
{
	m_leavingWorldNotify = callback;
}


void dgWorld::SetBodyDestructionByExeciveForce (OnBodyDestructionByExeciveForce callback) 
{
	m_destroyBodyByExeciveForce = callback;
}



dgBallConstraint* dgWorld::CreateBallConstraint (
	const dgVector& pivot, 
	dgBody* const body0, 
	dgBody* const body1)
{
	dgBallConstraint *constraint;
	
	_ASSERTE (body0);
	_ASSERTE (body0 != body1);
//	constraint = dgBallConstraint::Create(this);
	constraint = new (m_allocator) dgBallConstraint;

	AttachConstraint (constraint, body0, body1);
	constraint->SetPivotPoint (pivot);
	return constraint;
}


dgHingeConstraint* dgWorld::CreateHingeConstraint (
	const dgVector& pivot, 
	const dgVector& pinDir, 
	dgBody* const body0, 
	dgBody* const body1)
{
	dgHingeConstraint *constraint;

	_ASSERTE (body0);
	_ASSERTE (body0 != body1);
//	constraint = dgHingeConstraint::Create(this);
	constraint = new (m_allocator) dgHingeConstraint;

	AttachConstraint (constraint, body0, body1);
	constraint->SetPivotAndPinDir (pivot, pinDir);
	return constraint;
}


dgUpVectorConstraint* dgWorld::CreateUpVectorConstraint (const dgVector& pin, dgBody *body)
{
	dgUpVectorConstraint *constraint;
	
	_ASSERTE (body);
//	constraint = dgUpVectorConstraint::Create(this);
	constraint = new (m_allocator) dgUpVectorConstraint;

	AttachConstraint (constraint, body, NULL);
	constraint->InitPinDir (pin);
	return constraint;
}



dgSlidingConstraint* dgWorld::CreateSlidingConstraint (
	const dgVector& pivot, 
	const dgVector& pinDir, 
	dgBody* const body0, 
	dgBody* const body1)
{
	dgSlidingConstraint *constraint;

	_ASSERTE (body0);
	_ASSERTE (body0 != body1);
//	constraint = dgSlidingConstraint::Create(this);
	constraint = new (m_allocator) dgSlidingConstraint;

	AttachConstraint (constraint, body0, body1);
	constraint->SetPivotAndPinDir (pivot, pinDir);
	return constraint;
}


dgCorkscrewConstraint* dgWorld::CreateCorkscrewConstraint (
	const dgVector& pivot, 
	const dgVector& pinDir, 
	dgBody* const body0, 
	dgBody* const body1)
{
	dgCorkscrewConstraint *constraint;

	_ASSERTE (body0);
	_ASSERTE (body0 != body1);
//	constraint = dgCorkscrewConstraint::Create(this);
	constraint = new (m_allocator) dgCorkscrewConstraint;

	AttachConstraint (constraint, body0, body1);
	constraint->SetPivotAndPinDir (pivot, pinDir);
	return constraint;
}


dgUniversalConstraint* dgWorld::CreateUniversalConstraint (
	const dgVector& pivot, 
	const dgVector& pin0, 
	const dgVector& pin1, 
	dgBody* const body0, 
	dgBody* const body1)
{
	dgUniversalConstraint *constraint;

	_ASSERTE (body0);
	_ASSERTE (body0 != body1);
//	constraint = dgUniversalConstraint::Create(this);
	constraint = new (m_allocator) dgUniversalConstraint;

	AttachConstraint (constraint, body0, body1);
	constraint->SetPivotAndPinDir(pivot, pin0, pin1);
	return constraint;
}

/*
dgPointToCurveConstraint* dgWorld::AttachPointToCurveConstraint (
	const dgVector& pivot, 
	dgBody* const body0, 
	OnPointToCurveCallback funt,
	void *curveContext)
{
	dgPointToCurveConstraint *constraint;

	_ASSERTE (body0);
	constraint = dgPointToCurveConstraint::Create();

	AttachConstraint (constraint, body0, NULL);
	constraint->SetPivotPoint (pivot, funt, curveContext);
	return constraint;
}
*/

/*
dgConnectorConstraint* dgWorld::CreateConnectorConstraint (dgBody *body0__, dgBody *body1__)
{
	_ASSERTE (0);
	return NULL;

	dgConnectorConstraint *constraint;

	_ASSERTE (body0);
	_ASSERTE (body0 != body1);
	constraint = dgConnectorConstraint::Create();

	AttachConstraint (constraint, body0, body1);
	constraint->Setup();
	return constraint;
}
*/


/*
dgInt32 dgWorld::GetActiveBodiesCount() const
{
	return m_activeBodiesCount;
}
*/

dgInt32 dgWorld::GetBodiesCount() const
{
	const dgBodyMasterList& list = *this;
	return list.GetCount() - 1;
}

dgInt32 dgWorld::GetConstraintsCount() const
{
	const dgBodyMasterList& list = *this;
	return dgInt32 (list.m_constraintCount);
}


/*
dgLink* dgWorld::FindConstraintLink (const dgBody* const body0, const dgBody* const body1) const
{
	_ASSERTE (0);

	dgLink *ptr;
	dgLink *link;

	_ASSERTE (body0);
	if (!body0) {
		_ASSERTE (0);
		Swap (body0, body1);
	}

	if (body0) {
		link = body0->m_firstConstraintLink;
		if (link) {
			ptr = link;
			do {
				_ASSERTE (ptr->m_body == body0);
				if (ptr->m_twin->m_body == body1) {
					return ptr;
				}
			
				ptr = ptr->m_twin->m_next;
			} while (ptr != link);
		}
	}

	return NULL;
}
*/
/*
dgConstraint* dgWorld::GetConstraint (const dgLink* constraintLink) const
{
	return constraintLink->m_constraint;
}
*/



void dgWorld::BodySetMatrix (dgBody* body, const dgMatrix& matrix)
{
	#define DG_RECURSIVE_SIZE	1024
	dgInt32 index;
	dgBody* queue[DG_RECURSIVE_SIZE];


	index = 1;
	queue[0] = body;
	m_genericLRUMark ++;
	body->m_genericLRUMark = m_genericLRUMark;
	dgMatrix relMatrix (body->GetMatrix().Inverse() * matrix);
	while (index) {
		dgBody* body;
		
		index --;
		body = queue[index];
		_ASSERTE (body != m_sentionelBody);

		dgBroadPhaseCollision::Remove (body);
		dgBroadPhaseCollision::Add (body);

		dgMatrix matrix (body->GetMatrix() * relMatrix);
		body->SetVelocity (dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)));    
		body->SetOmega (dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)));    
		body->SetMatrix (matrix);
		body->m_isInWorld = true;

		for (dgBodyMasterListRow::dgListNode* jointNode = body->m_masterNode->GetInfo().GetFirst(); jointNode; jointNode = jointNode->GetNext()) {
			dgBodyMasterListCell& cell = jointNode->GetInfo();
			body = cell.m_bodyNode;
			if (body != m_sentionelBody) {
				if (body->m_genericLRUMark != m_genericLRUMark) {
					dgConstraint* constraint;
					constraint = cell.m_joint;
					if (constraint->GetId() != dgContactConstraintId) {
						body->m_genericLRUMark = m_genericLRUMark;
						queue[index] = body;
						index ++;
						_ASSERTE (index < DG_RECURSIVE_SIZE);
					}
				}
			}
		}
	}
}


void dgWorld::AddBodyImpulse (dgBody* body, const dgVector& pointVeloc, const dgVector& pointPosit)
{
	if (body->m_invMass.m_w > dgFloat32 (0.0f)) {
//		UnfreezeBody (body);
		body->AddImpulse (pointVeloc, pointPosit);
	}
}

/*
dgInt32 dgWorld::GetBodyArray (dgBody* root, dgBody** array, dgInt32 maxSize) const
{
	_ASSERTE (0);
	return 0;

	dgInt32 i;
	dgInt32 count;
	dgLink* ptr;
	dgLink* link;
	dgBody* body;
	dgConstraint* constraint;
	dgLink* stack[2048];
	const dgInt64 mask = dgInt64 (65536) * dgInt64 (65536) * dgInt64 (65536) * dgInt64 (4096);

	count = 0;
	link = root->m_firstConstraintLink; 
	if (link) {
		root->m_lru |= mask;
		stack[0] = link;
		i = 1;
		while (i) {
			i --;
			link = stack[i];
			ptr = link;
			do {
				body = ptr->m_twin->m_body;
				if (body) {
					_ASSERTE (body);
					if (~body->m_lru & mask) {
						constraint = ptr->m_constraint;
						if (constraint->IsBilateral()) {
							if (count <	maxSize) {
								array[count] = body; 
								count ++;
							}
							stack[i] = ptr->m_twin;
							i ++;
						}
						body->m_lru |= mask;
					}
				}

				ptr = ptr->m_twin->m_next;
			} while (ptr != link);
		}

		link = root->m_firstConstraintLink; 
		root->m_lru &= ~ mask;
		stack[0] = link;
		i = 1;
		while (i) {
			i --;
			link = stack[i];
			ptr = link;
			do {
				body = ptr->m_twin->m_body;
				if (body) {
					_ASSERTE (body);
					if (body->m_lru & mask) {
						constraint = ptr->m_constraint;
						if (constraint->IsBilateral()) {
							stack[i] = ptr->m_twin;
							i ++;
						}
						body->m_lru &= ~ mask;
					}
				}
				ptr = ptr->m_twin->m_next;
			} while (ptr != link);
		}
	}

	return count;
}


dgInt32 dgWorld::GetConstraintArray (dgConstraint* root, dgConstraint** constraintArray, dgInt32 maxSize) const
{
_ASSERTE (0);
return 0;

	dgInt32 i;
	dgInt32 stack;
	dgInt32 count;
	dgLink* ptr;
	dgLink* link;
	dgBody* body;
	dgConstraint* constraint;
	dgConstraint* constraintPtr;
	dgConstraint* stackPool[2048];
	const dgInt64 mask = dgInt64 (65536) * dgInt64 (65536) * dgInt64 (65536) * dgInt64 (4096);

	count = 0;
	stack = 1;
	stackPool[0] = root;
	root->m_lru |= mask;

	while (stack && (count < maxSize)) {
		stack --;
		constraint = stackPool[stack];
		constraintArray[count] = constraint;
		count ++;

		body = constraint->m_body0;
		if (body && (body->m_invMass.m_w != dgFloat32 (0.0f))) {
			link = body->m_firstConstraintLink; 
			_ASSERTE (link);
			ptr = link;
			do {
				constraintPtr = ptr->m_constraint;
				if (!(constraintPtr->m_lru & mask)) {
					constraintPtr->m_lru |= mask;
					stackPool[stack] = constraintPtr;
					stack ++;
				}
			
				ptr = ptr->m_twin->m_next;
			} while (ptr != link); 
		}

		body = constraint->m_body1;
		if (body && (body->m_invMass.m_w != dgFloat32 (0.0f))) {
			link = body->m_firstConstraintLink; 
			_ASSERTE (link);
			ptr = link;
			do {
				constraintPtr = ptr->m_constraint;
				if (!(constraintPtr->m_lru & mask)) {
					constraintPtr->m_lru |= mask;
					stackPool[stack] = constraintPtr;
					stack ++;
				}

				ptr = ptr->m_twin->m_next;
			} while (ptr != link); 
		}
	} 

	for (i = 0; i < stack; i ++) {
		constraint = stackPool[i];
		constraint->m_lru &= ~mask;
	}

	for (i = 0; i < count; i ++) {
		constraint = constraintArray[i];
		constraint->m_lru &= ~mask;

		#ifdef _DEBUG
		for (stack = i + 1; stack < count; stack ++) {                                                                 
			_ASSERTE (constraint != constraintArray[stack]);
		}
		#endif                             
	}

	return count;

}
*/

bool dgWorld::AreBodyConnectedByJoints (dgBody* const originSrc, dgBody* const targetSrc)
{
	#define DG_QEUEU_SIZE	1024
	dgBody* queue[DG_QEUEU_SIZE];

	m_genericLRUMark ++;

	dgBody* origin1 = originSrc;
	dgBody* target1 = targetSrc;
	if (origin1->m_mass[3] == dgFloat32 (0.0f)) {
		Swap (origin1, target1);
	}

	dgBody* const origin = origin1;
	dgBody* const target = target1;

	dgInt32 end = 1;
	dgInt32 start = 0;
	queue[0] = origin;
	origin->m_genericLRUMark = m_genericLRUMark;

	while (start != end) {
		dgBody* const origin = queue[start];
		start ++;
		start &= (DG_QEUEU_SIZE - 1);
		
		for (dgBodyMasterListRow::dgListNode* jointNode = origin->m_masterNode->GetInfo().GetFirst(); jointNode; jointNode = jointNode->GetNext()) {
			dgBodyMasterListCell& cell = jointNode->GetInfo();

			dgBody* const body = cell.m_bodyNode;
			if (body->m_genericLRUMark != m_genericLRUMark) {
				dgConstraint* const constraint = cell.m_joint;
				if (constraint->GetId() != dgContactConstraintId) {
					if (body == target) {
						return true;
					}
					body->m_genericLRUMark = m_genericLRUMark;
					queue[end] = body;
					end ++;
					end &= (DG_QEUEU_SIZE - 1);
				}
			}
		}
	}

	return false;
}

void dgWorld::FlushCache()
{

	// delete all contacts
	dgActiveContacts& contactList = *this;
	for (dgActiveContacts::dgListNode* contactNode = contactList.GetFirst(); contactNode; ) {
		dgContact* contact;
		contact = contactNode->GetInfo();
		contactNode = contactNode->GetNext();
		DestroyConstraint (contact);
	}

	// clean up memory in bradPhase
	dgBroadPhaseCollision::InvalidateCache ();

	// sort body list
	SortMasterList();
}