#include "Mix/Private/Dynamics/World.h"

#include "Mix/ScopedLock.h"
#include "Mix/Dynamics/IShape.h"
#include "Mix/Dynamics/IJoint.h"
#include "Mix/Graphics/Utility/IPerspectiveRenderer.h"
#include "Mix/Parallel/IManager.h"

#include "Mix/Private/Dynamics/Utility.h"
#include "Mix/Private/Dynamics/Manager.h"
#include "Mix/Private/Dynamics/Shape.h"
#include "Mix/Private/Dynamics/Joint.h"
#include "Mix/Private/Dynamics/Object.h"
#include "Mix/Private/Dynamics/StaticPlane.h"
#include "Mix/Private/Dynamics/StaticMesh.h"
#include "Mix/Private/Dynamics/RigidBody.h"
#include "Mix/Private/Dynamics/KinematicCharacter.h"
#include "Mix/Private/Dynamics/Sensor.h"
#include "Mix/Private/Dynamics/ObjectContext.h"

#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
#include "BulletMultiThreaded/btParallelConstraintSolver.h"
#include "BulletMultiThreaded/SpuCollisionTaskProcess.h"
#include "BulletMultiThreaded/SpuGatheringCollisionDispatcher.h"
#include "BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h"

#ifdef _DEBUG
	#define MIX_DYNAMICS_DEBUG( method ) method
#else //_DEBUG
	#define MIX_DYNAMICS_DEBUG( method )
#endif //_DEBUG

extern ContactAddedCallback gContactAddedCallback;

namespace Mix{ namespace Dynamics{

////////////////////////////////////////////////////////////////////////////////////////////////////
// エラーメッセージ
////////////////////////////////////////////////////////////////////////////////////////////////////

const wchar_t* World::FAILED_ADDCONSTRAINT    = L"ジョイントの追加に失敗";
const wchar_t* World::FAILED_REMOVECONSTRAINT = L"ジョイントの削除に失敗";
const wchar_t* World::FAILED_ADDOBJECT        = L"オブジェクトの追加に失敗";
const wchar_t* World::FAILED_REMOVEOBJECT     = L"オブジェクトの削除に失敗";
const wchar_t* World::FAILED_TESTSWEEP        = L"ワールドとスウィープの交差判定に失敗";

////////////////////////////////////////////////////////////////////////////////////////////////////
// World::Barrier
////////////////////////////////////////////////////////////////////////////////////////////////////

World::Barrier::Barrier( void )
{
	m_Counter = 0;
	m_MaxCount = 1;
	m_EnableCounter = 0;

	::InitializeCriticalSection( &m_ExternalCriticalSection );
	::InitializeCriticalSection( &m_LocalCriticalSection );

	m_RunEvent = CreateEvent( NULL, TRUE, FALSE, NULL );
	m_NotifyEvent = CreateEvent( NULL, TRUE, FALSE, NULL );
}

World::Barrier::~Barrier( void )
{
	::DeleteCriticalSection( &m_ExternalCriticalSection );
	::DeleteCriticalSection( &m_LocalCriticalSection );
	::CloseHandle( m_RunEvent );
	::CloseHandle( m_NotifyEvent );
}

void World::Barrier::sync( void )
{
	int eventId;

	::EnterCriticalSection( &m_ExternalCriticalSection );

	if( m_EnableCounter > 0 )
	{
		::ResetEvent( m_NotifyEvent );
		::LeaveCriticalSection( &m_ExternalCriticalSection );
		::WaitForSingleObject( m_NotifyEvent, INFINITE );
		::EnterCriticalSection( &m_ExternalCriticalSection );
	}

	eventId = m_Counter;
	m_Counter++;

	if( eventId == ( m_MaxCount - 1 ) )
	{
		::SetEvent( m_RunEvent );

		m_EnableCounter = m_Counter - 1;
		m_Counter = 0;
	}
	else
	{
		::ResetEvent( m_RunEvent );
		::LeaveCriticalSection( &m_ExternalCriticalSection );
		::WaitForSingleObject( m_RunEvent, INFINITE ); 
		::EnterCriticalSection( &m_ExternalCriticalSection );
		m_EnableCounter--;
	}

	if( m_EnableCounter == 0 )
	{
		::SetEvent( m_NotifyEvent );
	}

	::LeaveCriticalSection( &m_ExternalCriticalSection );
}

void World::Barrier::setMaxCount( int n )
{
	m_MaxCount = n;
}

int World::Barrier::getMaxCount( void )
{
	return m_MaxCount;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// World::CriticalSection
////////////////////////////////////////////////////////////////////////////////////////////////////

World::CriticalSection::CriticalSection( void )
{
	::InitializeCriticalSection( &m_CriticalSection );
}

World::CriticalSection::~CriticalSection( void )
{
	::DeleteCriticalSection( &m_CriticalSection );
}

unsigned int World::CriticalSection::getSharedParam( int i )
{
	MIX_ASSERT( ( i >= 0 ) && ( i < 31 ) );

	return mCommonBuff[i+1];
}

void World::CriticalSection::setSharedParam( int i, unsigned int p )
{
	MIX_ASSERT( ( i >= 0 ) && ( i < 31 ) );
	mCommonBuff[i+1] = p;
}

void World::CriticalSection::lock()
{
	::EnterCriticalSection( &m_CriticalSection );
	mCommonBuff[0] = 1;
}

void World::CriticalSection::unlock( void )
{
	mCommonBuff[0] = 0;
	::LeaveCriticalSection( &m_CriticalSection );
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// World::ThreadSupport
////////////////////////////////////////////////////////////////////////////////////////////////////

World::ThreadSupport::ThreadSupport( void )
{
}

World::ThreadSupport::~ThreadSupport( void )
{
	stopSPU();
}

Boolean World::ThreadSupport::Initialize( World::ThreadFunc threadFunc, lsMemoryFunc lsMemoryFunc, UInt32 debID )
{
	Mix::Parallel::IManager* pParallelMgr = Mix::Parallel::GetManagerPtr();
	MIX_ASSERT( pParallelMgr != NULL );

	UInt32 threadCount = pParallelMgr->GetThreadCount();
	Mix::STL::Vector<Mix::Memory::SECTION_DYNAMICS, Mix::Parallel::WORK> parallelWorks;

	MIX_ASSERT( threadCount > 1 );

	parallelWorks.resize( threadCount );

	m_ActiveStatus.resize( threadCount );
	m_CompHandles.resize( threadCount );
	m_NumTasks = static_cast<Int32>( threadCount );

	for( Int32 i = 0; i < m_NumTasks; i++ )
	{
		ThreadSupport::STATUS& status = m_ActiveStatus[i];
		Mix::Parallel::WORK& parallelWork = parallelWorks[i];

		LPSECURITY_ATTRIBUTES lpThreadAttributes = NULL;

		status.userPtr = NULL;

		status.eventStartHandle = CreateEvent( 0, FALSE, FALSE, NULL );
		status.eventCompHandle = CreateEvent( 0, FALSE, FALSE, NULL );

		m_CompHandles[i] = status.eventCompHandle;

		status.taskId = i;
		status.commandId = 0;
		status.status = 0;
		status.lsMemory = lsMemoryFunc();//createCollisionLocalStoreMemory();
		status.threadFunc = threadFunc;//processCollisionTask;

		parallelWork.threadIndex = static_cast<UInt32>( i );
		parallelWork.hWakeup = status.eventStartHandle;
		parallelWork.pFunc = ThreadSupport::ParallelFunction;
		parallelWork.pData = &status;
		parallelWork.debugID = debID;
	}

	m_hWork = pParallelMgr->MapWorks( MIX_UIT_TO_UI32( parallelWorks.size() ), &( parallelWorks[0] ) );
	if( m_hWork == NULL )
	{
		return False;
	}

	return True;
}

void World::ThreadSupport::sendRequest( uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t taskId )
{
	switch( uiCommand )
	{
		case CMD_GATHER_AND_PROCESS_PAIRLIST:
		{
			ThreadSupport::STATUS& status = m_ActiveStatus[taskId];

			MIX_ASSERT( taskId >= 0 );
			MIX_ASSERT( Int32( taskId ) < m_ActiveStatus.size() );

			status.commandId = uiCommand;
			status.status = 1;
			status.userPtr = reinterpret_cast<void*>( uiArgument0 );

			::SetEvent( status.eventStartHandle );

			break;
		}

		default:
		{
			MIX_ASSERT( 0 );
		}
	};
}

void World::ThreadSupport::waitForResponse( unsigned int *puiArgument0, unsigned int *puiArgument1 )
{
	MIX_ASSERT( m_ActiveStatus.size() > 0 );

	Int32 last = -1;
	UInt32 res = WaitForMultipleObjects( m_CompHandles.size(), &m_CompHandles[0], FALSE, INFINITE );

	MIX_ASSERT( res != WAIT_FAILED );

	last = res - WAIT_OBJECT_0;

	ThreadSupport::STATUS& status = m_ActiveStatus[last];

	MIX_ASSERT( status.eventCompHandle != NULL );
	MIX_ASSERT( status.status > 1 );

	status.status = 0;

	MIX_ASSERT( last >= 0 );

	*puiArgument0 = status.taskId;
	*puiArgument1 = status.status;
}

bool World::ThreadSupport::isTaskCompleted( unsigned int *puiArgument0, unsigned int *puiArgument1, int timeOutInMilliseconds )
{
	MIX_ASSERT( m_ActiveStatus.size() > 0 );

	UInt32 res = ::WaitForMultipleObjects( m_CompHandles.size(), &m_CompHandles[0], FALSE, timeOutInMilliseconds );

	if( ( res != STATUS_TIMEOUT ) && ( res != WAIT_FAILED ) )
	{
		MIX_ASSERT( res != WAIT_FAILED );

		Int32 last = res - WAIT_OBJECT_0;

		ThreadSupport::STATUS& status = m_ActiveStatus[last];

		MIX_ASSERT( status.eventCompHandle != NULL );
		MIX_ASSERT( status.status > 1 );

		status.status = 0;

		MIX_ASSERT( last >= 0 );

		*puiArgument0 = status.taskId;
		*puiArgument1 = status.status;

		return true;
	} 

	return false;
}

void World::ThreadSupport::startSPU( void )
{
}

void World::ThreadSupport::stopSPU( void )
{
	Mix::Parallel::IManager* pParallelMgr = Mix::Parallel::GetManagerPtr();
	MIX_ASSERT( pParallelMgr != NULL );

	Int32 numActiveStatus = m_ActiveStatus.size();
	Int32 i;

	// ワークを終了する

	for( i = 0; i < numActiveStatus; i++ )
	{
		ThreadSupport::STATUS& status = m_ActiveStatus[i];

		if( status.status > 0 )
		{
			WaitForSingleObject( status.eventCompHandle, INFINITE );
		}
		
		status.userPtr = NULL;

		::SetEvent( status.eventStartHandle );
		::WaitForSingleObject( status.eventCompHandle, INFINITE );
	}

	// ワークをアンマップ

	if( m_hWork != NULL )
	{
		pParallelMgr->UnmapWorks( m_hWork );
		m_hWork = NULL;
	}

	// 解放

	for( i = 0; i < numActiveStatus; i++ )
	{
		ThreadSupport::STATUS& status = m_ActiveStatus[i];

		::CloseHandle( status.eventCompHandle );
		::CloseHandle( status.eventStartHandle );
	}

	m_ActiveStatus.clear();
	m_CompHandles.clear();
}

void World::ThreadSupport::setNumTasks( int numTasks )
{
	m_NumTasks = numTasks;
}

int World::ThreadSupport::getNumTasks( void ) const
{
	return m_NumTasks;
}

void* World::ThreadSupport::getThreadLocalMemory( int taskId )
{
	return m_ActiveStatus[taskId].lsMemory;
}

btBarrier* World::ThreadSupport::createBarrier( void )
{
	UInt8* pMem = static_cast<UInt8*>( btAlignedAlloc(sizeof( World::Barrier ), 16 ) );
	MIX_ASSERT( pMem != NULL );

	World::Barrier* pBarrier = new( pMem ) World::Barrier();

	MIX_ASSERT( pBarrier != NULL );
	pBarrier->setMaxCount( getNumTasks() );

	return pBarrier;
}

btCriticalSection* World::ThreadSupport::createCriticalSection( void )
{
	UInt8* pMem = static_cast<unsigned char*>( btAlignedAlloc( sizeof( World::CriticalSection ), 16 ) );
	MIX_ASSERT( pMem != NULL );

	World::CriticalSection* pCS = new( pMem ) World::CriticalSection();
	MIX_ASSERT( pCS != NULL );

	return pCS;
}

void World::ThreadSupport::deleteBarrier( btBarrier* barrier )
{
	barrier->~btBarrier();
	btAlignedFree( barrier );
}

void World::ThreadSupport::deleteCriticalSection( btCriticalSection* criticalSection )
{
	criticalSection->~btCriticalSection();
	btAlignedFree( criticalSection );
}

void __stdcall World::ThreadSupport::ParallelFunction( UInt32 threadNum, UInt32 threadIndex, void* pData )
{
	ThreadSupport::STATUS* pStatus = static_cast<ThreadSupport::STATUS*>( pData );
	void* userPtr = pStatus->userPtr;

	if( userPtr != NULL )
	{
		MIX_ASSERT( pStatus->status > 0 );

		pStatus->threadFunc( userPtr, pStatus->lsMemory );
		pStatus->status = 2;

		::SetEvent( pStatus->eventCompHandle );
	}
	else
	{
		pStatus->status = 3;
		::SetEvent( pStatus->eventCompHandle );
	}
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// World::TestSegmentResultCallback
////////////////////////////////////////////////////////////////////////////////////////////////////

World::TestSegmentResultCallback::TestSegmentResultCallback( const btVector3& rayFromWorld, const btVector3& rayToWorld ) :
btCollisionWorld::ClosestRayResultCallback( rayFromWorld, rayToWorld )
{
	m_partId = -1;
	m_triangleIndex = -1;
}

btScalar World::TestSegmentResultCallback::addSingleResult( btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace )
{
	btScalar fraction = btCollisionWorld::ClosestRayResultCallback::addSingleResult( rayResult, normalInWorldSpace );

	if( rayResult.m_localShapeInfo != NULL )
	{
		m_partId = rayResult.m_localShapeInfo->m_shapePart;
		m_triangleIndex = rayResult.m_localShapeInfo->m_triangleIndex;
	}
	else
	{
		m_partId = -1;
		m_triangleIndex = -1;
	}

	return fraction;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// World::TestSegmentCustomResultCallback
////////////////////////////////////////////////////////////////////////////////////////////////////

World::TestSegmentCustomResultCallback::TestSegmentCustomResultCallback( const btVector3& rayFromWorld, const btVector3& rayToWorld, Mix::Dynamics::IWorld::TestCallback* pCallback ) :
World::TestSegmentResultCallback( rayFromWorld, rayToWorld )
{
	MIX_ASSERT( pCallback != NULL );

	m_partId = -1;
	m_triangleIndex = -1;
	m_pCallback = pCallback;
}

btScalar World::TestSegmentCustomResultCallback::addSingleResult( btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace )
{
	Mix::Dynamics::ObjectContext* pObjectContext = static_cast<Mix::Dynamics::ObjectContext*>( rayResult.m_collisionObject->getUserPointer() );

	if( m_pCallback->OnHit( pObjectContext->GetOwnerPtr() ) == False )
	{
		return btScalar( 1.0 );
	}

	btScalar fraction = btCollisionWorld::ClosestRayResultCallback::addSingleResult( rayResult, normalInWorldSpace );

	if( rayResult.m_localShapeInfo != NULL )
	{
		m_partId = rayResult.m_localShapeInfo->m_shapePart;
		m_triangleIndex = rayResult.m_localShapeInfo->m_triangleIndex;
	}
	else
	{
		m_partId = -1;
		m_triangleIndex = -1;
	}

	return fraction;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// World::TestSweepResultCallback
////////////////////////////////////////////////////////////////////////////////////////////////////

World::TestSweepResultCallback::TestSweepResultCallback( const btVector3& convexFromWorld, const btVector3& convexToWorld ) :
btCollisionWorld::ClosestConvexResultCallback( convexFromWorld, convexToWorld )
{
	m_partId = -1;
	m_triangleIndex = -1;
}

btScalar World::TestSweepResultCallback::addSingleResult( btCollisionWorld::LocalConvexResult& convexResult, bool normalInWorldSpace )
{
	btScalar fraction = btCollisionWorld::ClosestConvexResultCallback::addSingleResult( convexResult, normalInWorldSpace );

	if( convexResult.m_localShapeInfo != NULL )
	{
		m_partId = convexResult.m_localShapeInfo->m_shapePart;
		m_triangleIndex = convexResult.m_localShapeInfo->m_triangleIndex;
	}
	else
	{
		m_partId = -1;
		m_triangleIndex = -1;
	}

	return fraction;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// World::TestSweepCustomResultCallback
////////////////////////////////////////////////////////////////////////////////////////////////////

World::TestSweepCustomResultCallback::TestSweepCustomResultCallback( const btVector3& convexFromWorld, const btVector3& convexToWorld, Mix::Dynamics::IWorld::TestCallback* pCallback ) :
World::TestSweepResultCallback( convexFromWorld, convexToWorld )
{
	MIX_ASSERT( pCallback != NULL );

	m_partId = -1;
	m_triangleIndex = -1;
	m_pCallback = pCallback;
}

btScalar World::TestSweepCustomResultCallback::addSingleResult( btCollisionWorld::LocalConvexResult& convexResult, bool normalInWorldSpace )
{
	Mix::Dynamics::ObjectContext* pObjectContext = static_cast<Mix::Dynamics::ObjectContext*>( convexResult.m_hitCollisionObject->getUserPointer() );

	if( m_pCallback->OnHit( pObjectContext->GetOwnerPtr() ) == False )
	{
		return btScalar( 1.0 );
	}

	btScalar fraction = btCollisionWorld::ClosestConvexResultCallback::addSingleResult( convexResult, normalInWorldSpace );

	if( convexResult.m_localShapeInfo != NULL )
	{
		m_partId = convexResult.m_localShapeInfo->m_shapePart;
		m_triangleIndex = convexResult.m_localShapeInfo->m_triangleIndex;
	}
	else
	{
		m_partId = -1;
		m_triangleIndex = -1;
	}

	return fraction;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// World
////////////////////////////////////////////////////////////////////////////////////////////////////

void World::InternalBeginRefreshObject( Mix::Dynamics::Object* pObject )
{
	// アクションインターフェース //

	if( pObject->Bullet_GetCharacterControllerInterfacePtr() != NULL )
	{
		//キャラクター
		m_pWorld->removeCharacter( pObject->Bullet_GetCharacterControllerInterfacePtr() );
	}
	else if( pObject->Bullet_GetRaycastVehiclePtr() != NULL )
	{
		//ビハイクル
		m_pWorld->removeVehicle( pObject->Bullet_GetRaycastVehiclePtr() );
	}
	else if( pObject->Bullet_GetActionInterfacePtr() != NULL )
	{
		//その他
		m_pWorld->removeAction( pObject->Bullet_GetActionInterfacePtr() );
	}

	// コリジョンオブジェクト //

	if( pObject->Bullet_GetRigidBodyPtr() != NULL )
	{
		//リジッドボディ
		m_pWorld->removeRigidBody( pObject->Bullet_GetRigidBodyPtr() );
	}
	else if( pObject->Bullet_GetCollisionObjectPtr() != NULL )
	{
		//その他
		m_pWorld->removeCollisionObject( pObject->Bullet_GetCollisionObjectPtr() );
	}
}

void World::InternalEndRefreshObject( Mix::Dynamics::Object* pObject )
{
	Int16 filterGroup = pObject->Bullet_GetFilterGroup();
	Int16 filterMask = pObject->Bullet_GetFilterMask();

	// アクションインターフェース //

	if( pObject->Bullet_GetCharacterControllerInterfacePtr() != NULL )
	{
		//キャラクター
		m_pWorld->addCharacter( pObject->Bullet_GetCharacterControllerInterfacePtr() );
	}
	else if( pObject->Bullet_GetRaycastVehiclePtr() != NULL )
	{
		//ビハイクル
		m_pWorld->addVehicle( pObject->Bullet_GetRaycastVehiclePtr() );
	}
	else if( pObject->Bullet_GetActionInterfacePtr() != NULL )
	{
		//その他
		m_pWorld->addAction( pObject->Bullet_GetActionInterfacePtr() );
	}

	// コリジョンオブジェクト //

	if( pObject->Bullet_GetRigidBodyPtr() != NULL )
	{
		//リジッドボディ
		m_pWorld->addRigidBody( pObject->Bullet_GetRigidBodyPtr(), filterGroup, filterMask );
	}
	else if( pObject->Bullet_GetCollisionObjectPtr() != NULL )
	{
		//その他
		m_pWorld->addCollisionObject( pObject->Bullet_GetCollisionObjectPtr(), filterGroup, filterMask );
	}
}

bool World::ContactAddedCallback(	btManifoldPoint& cp,
									const btCollisionObjectWrapper* cow0, int partId0, int index0,
									const btCollisionObjectWrapper* cow1, int partId1, int index1 )
{
	const btCollisionObject* collObject0 = cow0->m_collisionObject;
	const btCollisionObject* collObject1 = cow1->m_collisionObject;

	Mix::Dynamics::ObjectContext* pContextA = static_cast<Mix::Dynamics::ObjectContext*>( collObject0->getUserPointer() );
	Mix::Dynamics::ObjectContext* pContextB = static_cast<Mix::Dynamics::ObjectContext*>( collObject1->getUserPointer() );

	const Mix::Dynamics::MATERIAL& materialA = pContextA->QueryMaterial( ( partId0 - 1 ), index0 );
	const Mix::Dynamics::MATERIAL& materialB = pContextB->QueryMaterial( ( partId1 - 1 ), index1 );

	cp.m_combinedFriction = ( materialA.friction + materialB.friction ) * 0.5f;
	cp.m_combinedRestitution = ( materialA.restitution + materialB.restitution ) * 0.5f;

	return true;
}

World* World::CreateInstance( const wchar_t* pDebugName )
{
	return MIX_LIB_NEW_T( Mix::Memory::SECTION_DYNAMICS, World, pDebugName );
}

World::World( const wchar_t* pDebugName ) :
m_pThreadSupportCD( NULL ),
m_pThreadSupportCS( NULL ),
m_pCollisionDispatcher( NULL ),
m_pBroadphas( NULL ),
m_pConstraintSolver( NULL ),
m_pConfiguration( NULL ),
m_pWorld( NULL ),
m_pGhostPairCallback( NULL )
{
	gContactAddedCallback = World::ContactAddedCallback;

	Mix::Memory::Zero( m_JointCountTable, sizeof( m_JointCountTable ) );
	Mix::Memory::Zero( m_ObjectCountTable, sizeof( m_ObjectCountTable ) );

	m_ManifoldPoints.reserve( World::MANIFOLD_POINT_DEFAULT_CAPACITY );

	m_Profile.ssElapsedTime = 0.0f;
	m_Profile.cpElapsedTime = 0.0f;

#ifdef _DEBUG
	m_DebName = pDebugName;
#endif //_DEBUG
}

World::~World( void )
{
	for( World::JointList::iterator it = m_JointList.begin(); it != m_JointList.end(); ++it )
	{
		Mix::Dynamics::IJoint* pJoint = ( *it );

		m_pWorld->removeConstraint( dynamic_cast<Mix::Dynamics::Joint*>( pJoint )->Bullet_GetTypedConstraintPtr() );
		MIX_RELEASE( pJoint );
	}

	for( World::ObjectList::iterator it = m_ObjectList.begin(); it != m_ObjectList.end(); ++it )
	{
		Mix::Dynamics::IObject* pObject = ( *it );
		Mix::Dynamics::Object* pInternalObject = dynamic_cast<Mix::Dynamics::Object*>( pObject );

		if( pObject->GetType() == Mix::Dynamics::IObject::KINEMATIC_CHARACTER )
		{
			MIX_ASSERT( pInternalObject->Bullet_GetCollisionObjectPtr() != NULL );
			MIX_ASSERT( pInternalObject->Bullet_GetActionInterfacePtr() != NULL );

			m_pWorld->removeCollisionObject( pInternalObject->Bullet_GetCollisionObjectPtr() );
			m_pWorld->removeCharacter( pInternalObject->Bullet_GetActionInterfacePtr() );
		}
		else
		{
			MIX_ASSERT( pInternalObject->Bullet_GetRigidBodyPtr() != NULL );

			m_pWorld->removeRigidBody( pInternalObject->Bullet_GetRigidBodyPtr() );
		}

		pInternalObject->GetContextPtr()->NotifyRemovedFromWorld( this );
		pInternalObject->SetWorldPtr( NULL );

		MIX_RELEASE( pObject );
	}

	MIX_LIB_DELETE( m_pWorld );
	MIX_LIB_DELETE_T( btGhostPairCallback, m_pGhostPairCallback );
	MIX_LIB_DELETE_T( btCollisionDispatcher, m_pCollisionDispatcher );
	MIX_LIB_DELETE_T( ThreadSupport, m_pThreadSupportCD );
	MIX_LIB_DELETE_T( btDbvtBroadphase, m_pBroadphas );
	MIX_LIB_DELETE( m_pConstraintSolver );
	MIX_LIB_DELETE_T( ThreadSupport, m_pThreadSupportCS );
	MIX_LIB_DELETE_T( btDefaultCollisionConfiguration, m_pConfiguration );

	// 並列処理時のローカルストアメモリの解放
	deleteCollisionLocalStoreMemory();
}

Boolean World::Initialize( const Mix::Dynamics::WORLD_CONFIG& config, Boolean bParallel )
{
	Boolean bParallenCD;
	Boolean bParallenJS;

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 並列処理をするかどうか
	////////////////////////////////////////////////////////////////////////////////////////////////////

	bParallenCD = ( bParallel == True && config.bParallelCollisionDispatcher == True );
	bParallenJS = ( bParallel == True && config.bParallelJointSolver == True );

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// コンフィギュレーションの作成
	////////////////////////////////////////////////////////////////////////////////////////////////////

	btDefaultCollisionConstructionInfo collConstInfo;

	collConstInfo.m_defaultMaxPersistentManifoldPoolSize = config.persistentManifoldPoolSize;
	collConstInfo.m_defaultMaxCollisionAlgorithmPoolSize = config.collisionAlgorithmPoolSize;

	m_pConfiguration = MIX_LIB_NEW_T( Mix::Memory::SECTION_DYNAMICS, btDefaultCollisionConfiguration, collConstInfo );
	if( m_pConfiguration == NULL )
	{
		return False;
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// コリジョンディスパッチャーの作成
	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( bParallenCD == True )
	{
		m_pThreadSupportCD = MIX_LIB_NEW_T( Mix::Memory::SECTION_DYNAMICS, World::ThreadSupport );
		if( m_pThreadSupportCD == NULL )
		{
			return False;
		}

		if( m_pThreadSupportCD->Initialize( processCollisionTask, createCollisionLocalStoreMemory, Mix::Parallel::DEBUG_DYNAMICS_COLLISION_DISPATCHER ) == False )
		{
			return False;
		}

		m_pCollisionDispatcher = MIX_LIB_NEW_T( Mix::Memory::SECTION_DYNAMICS,
			SpuGatheringCollisionDispatcher,
			m_pThreadSupportCD,
			static_cast<unsigned int>( m_pThreadSupportCD->getNumTasks() ),
			m_pConfiguration );
	}
	else
	{
		m_pCollisionDispatcher = MIX_LIB_NEW_T( Mix::Memory::SECTION_DYNAMICS, btCollisionDispatcher, m_pConfiguration );
	}

	if( m_pCollisionDispatcher == NULL )
	{
		return False;
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// ブロードフェイズの作成
	////////////////////////////////////////////////////////////////////////////////////////////////////

	m_pBroadphas = MIX_LIB_NEW_T( Mix::Memory::SECTION_DYNAMICS, btDbvtBroadphase );
	if( m_pBroadphas == NULL )
	{
		return False;
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// コンストレイントソルバーの作成
	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( bParallenJS == True )
	{
		m_pThreadSupportCS = MIX_LIB_NEW_T( Mix::Memory::SECTION_SCENE, World::ThreadSupport );
		if( m_pThreadSupportCS == NULL )
		{
			return False;
		}

		if( m_pThreadSupportCS->Initialize( SolverThreadFunc, SolverlsMemoryFunc, Mix::Parallel::DEBUG_DYNAMICS_JOINT_SOLVER ) == False )
		{
			return False;
		}

		m_pConstraintSolver = MIX_LIB_NEW btParallelConstraintSolver( m_pThreadSupportCS );
	}
	else
	{
		m_pConstraintSolver = MIX_LIB_NEW btSequentialImpulseConstraintSolver();
	}

	if( m_pConstraintSolver == NULL )
	{
		return False;
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// キャラクターコントローラー用のゴーストペアコールバックの作成
	////////////////////////////////////////////////////////////////////////////////////////////////////

	m_pGhostPairCallback = MIX_LIB_NEW_T( Mix::Memory::SECTION_DYNAMICS, btGhostPairCallback );
	if( m_pGhostPairCallback == NULL )
	{
		return False;
	}

	m_pBroadphas->getOverlappingPairCache()->setInternalGhostPairCallback( m_pGhostPairCallback );

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// ワールドの作成
	////////////////////////////////////////////////////////////////////////////////////////////////////

	m_pWorld = MIX_LIB_NEW btDiscreteDynamicsWorld( m_pCollisionDispatcher, m_pBroadphas, m_pConstraintSolver, m_pConfiguration );
	if( m_pWorld != NULL )
	{
		m_Gravity = ToMixVector3( m_pWorld->getGravity() );
	}
	else
	{
		return False;
	}

	m_pWorld->getSimulationIslandManager()->setSplitIslands( ( bParallenJS == True )? false : true ); // false の場合は、Sensor は正しく機能しません
	m_pWorld->getSolverInfo().m_numIterations = static_cast<Int32>( MIX_CLAMP( config.numContactSolverIterations, Mix::Dynamics::LCPS_MIN_ITE, Mix::Dynamics::LCPS_MAX_ITE ) );
	m_pWorld->getSolverInfo().m_solverMode = SOLVER_SIMD | SOLVER_USE_WARMSTARTING;
	m_pWorld->getDispatchInfo().m_allowedCcdPenetration = max( Mix::Dynamics::CCD_MIN_PENE, config.allowedCcdPenetration );
	m_pWorld->getDispatchInfo().m_enableSPU = ( ( bParallenCD == True ) || ( bParallenJS == True ) );

	return True;
}

void World::BeginRefreshObject( Mix::Dynamics::Object* pObject )
{
	Mix::ScopedLock lock( m_ObjectSync );

	MIX_ASSERT( pObject != NULL );
	MIX_ASSERT( pObject->GetWorldPtr() == this );

	InternalBeginRefreshObject( pObject );
}

void World::EndRefreshObject( Mix::Dynamics::Object* pObject )
{
	Mix::ScopedLock lock( m_ObjectSync );

	MIX_ASSERT( pObject != NULL );
	MIX_ASSERT( pObject->GetWorldPtr() == this );

	InternalEndRefreshObject( pObject );
}

void World::RefreshObject( Mix::Dynamics::Object* pObject )
{
	Mix::ScopedLock lock( m_ObjectSync );

	MIX_ASSERT( pObject != NULL );
	MIX_ASSERT( pObject->GetWorldPtr() == this );

	InternalBeginRefreshObject( pObject );
	InternalEndRefreshObject( pObject );
}

btDynamicsWorld* World::Bullet_GetDynamicsWorldPtr( void ) const
{
	return m_pWorld;
}

Boolean World::ContainsJoint( Mix::Dynamics::IJoint* pJoint ) const
{
	return Mix::STL::Vector_Contains( m_JointList, pJoint );
}

Boolean World::AddJoint( Mix::Dynamics::IJoint* pJoint, Boolean bCollisionDisabled )
{
	if( pJoint == NULL )
	{
		MIX_LOG_ERROR( L"%s : %s : pJoint[%s]",
			FAILED_ADDCONSTRAINT,
			Mix::STR_ILLEGALARG,
			( pJoint != NULL )? L"○" : L"NULL"
			);

		return False;
	}

	World::JointList::iterator it = std::find( m_JointList.begin(), m_JointList.end(), pJoint );
	if( it != m_JointList.end() )
	{
		//既に現在のワールドに配置されている
		return False;
	}

	Mix::Dynamics::Joint* pInternalJoint = dynamic_cast<Mix::Dynamics::Joint*>( pJoint );
	MIX_ASSERT( pInternalJoint->Bullet_GetTypedConstraintPtr() != NULL );
	if( pInternalJoint->IsInWorld() == True )
	{
		//既に他のワールドに配置されている
		return False;
	}

	Mix::Dynamics::IJoint::TYPE jointType = pJoint->GetType();

	MIX_ADD_REF( pJoint );
	m_JointList.push_back( pJoint );

	if( ( bCollisionDisabled == False ) ||
		( pJoint->IsSingle() == True ) )
	{
		m_ObjectSync.Enter();
		m_pWorld->addConstraint( pInternalJoint->Bullet_GetTypedConstraintPtr() );
		m_ObjectSync.Leave();
	}
	else
	{
		m_ObjectSync.Enter();
		m_pWorld->addConstraint( pInternalJoint->Bullet_GetTypedConstraintPtr(), true );
		m_ObjectSync.Leave();
	}

	pInternalJoint->m_bInWorld = True;

	m_JointCountTable[jointType]++;

	return True;
}

Boolean World::RemoveJoint( Mix::Dynamics::IJoint* pJoint )
{
	if( pJoint == NULL )
	{
		MIX_LOG_ERROR( L"%s : %s : pJoint[%s]",
			FAILED_REMOVECONSTRAINT,
			Mix::STR_ILLEGALARG,
			( pJoint != NULL )? L"○" : L"NULL"
			);

		return False;
	}

	World::JointList::iterator it = std::find( m_JointList.begin(), m_JointList.end(), pJoint );
	if( it != m_JointList.end() )
	{
		Mix::Dynamics::IJoint::TYPE jointType = pJoint->GetType();
		Mix::Dynamics::Joint* pInternalJoint = dynamic_cast<Mix::Dynamics::Joint*>( pJoint );

		MIX_ASSERT( m_JointCountTable[jointType] > 0 );
		m_JointCountTable[jointType]--;

		MIX_ASSERT( pInternalJoint->m_bInWorld == True );
		pInternalJoint->m_bInWorld = False;

		MIX_ASSERT( pInternalJoint->Bullet_GetTypedConstraintPtr() != NULL );
		m_ObjectSync.Enter();
		m_pWorld->removeConstraint( pInternalJoint->Bullet_GetTypedConstraintPtr() );
		m_ObjectSync.Leave();

		Mix::STL::Vector_FirstErase( m_JointList, it );

		MIX_RELEASE( pJoint );
	}
	else
	{
		return False;
	}

	return True;
}

UInt32 World::GetJointCount( Mix::Dynamics::IJoint::TYPE type ) const
{
	return m_JointCountTable[type];
}

UInt32 World::GetTotalJointCount( void ) const
{
	return MIX_UIT_TO_UI32( m_JointList.size() );
}

Boolean World::ContainsObject( Mix::Dynamics::IObject* pObject ) const
{
	return Mix::STL::Vector_Contains( m_ObjectList, pObject );
}

Boolean World::AddObject( Mix::Dynamics::IObject* pObject )
{
	if( pObject == NULL )
	{
		MIX_LOG_ERROR( L"%s : %s : pObject[%s]",
			FAILED_ADDOBJECT,
			Mix::STR_ILLEGALARG,
			( pObject != NULL )? L"○" : L"NULL" );

		return False;
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// ワールドに存在しているかどうかをチェック
	////////////////////////////////////////////////////////////////////////////////////////////////////

	World::ObjectList::iterator it = std::find( m_ObjectList.begin(), m_ObjectList.end(), pObject );
	if( it != m_ObjectList.end() )
	{
		return False;
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// ワールドに追加
	////////////////////////////////////////////////////////////////////////////////////////////////////

	Mix::Dynamics::IObject::TYPE objType = pObject->GetType();
	Mix::Dynamics::Object* pInternalObject = dynamic_cast<Mix::Dynamics::Object*>( pObject );
	UInt16 filterGroup = pObject->GetFilterGroup();
	UInt16 filterMask = pObject->GetFilterMask();

	MIX_ADD_REF( pObject );
	m_ObjectList.push_back( pObject );

	MIX_ASSERT( pInternalObject->GetContextPtr() != NULL );
	m_ObjectContextList.push_back( pInternalObject->GetContextPtr() );

	pInternalObject->SetWorldPtr( this );

	if( objType == Mix::Dynamics::IObject::KINEMATIC_CHARACTER )
	{
		MIX_ASSERT( pInternalObject->Bullet_GetCollisionObjectPtr() != NULL );
		MIX_ASSERT( pInternalObject->Bullet_GetCharacterControllerInterfacePtr() != NULL );

		m_ObjectSync.Enter();
		m_pWorld->addCollisionObject( pInternalObject->Bullet_GetCollisionObjectPtr(), filterGroup, filterMask );
		m_pWorld->addCharacter( pInternalObject->Bullet_GetCharacterControllerInterfacePtr() );
		m_ObjectSync.Leave();
	}
	else if( objType == Mix::Dynamics::IObject::VEHICLE )
	{
		MIX_ASSERT( pInternalObject->Bullet_GetRigidBodyPtr() != NULL );
		MIX_ASSERT( pInternalObject->Bullet_GetRaycastVehiclePtr() != NULL );

		m_ObjectSync.Enter();
		m_pWorld->addRigidBody( pInternalObject->Bullet_GetRigidBodyPtr(), filterGroup, filterMask );
		m_pWorld->addVehicle( pInternalObject->Bullet_GetRaycastVehiclePtr() );
		m_ObjectSync.Leave();
	}
	else
	{
		MIX_ASSERT( pInternalObject->Bullet_GetRigidBodyPtr() != NULL );

		if( ( m_pThreadSupportCS != NULL ) && // m_pThreadSupportCD が NULL でないということは、並列処理が有効であること
			( pObject->GetType() == Mix::Dynamics::IObject::SENSOR ) )
		{
#ifdef _DEBUG
			MIX_LOG_WARNING( L"Mix::Dynamics::IWorld::AddObject : ジョイントソルバーの並列処理が有効な場合、センサーは正しく動作しません : World DebugName[%s]",
				m_DebName.GetConstPtr() );
#else //_DEBUG
			MIX_LOG_WARNING( L"Mix::Dynamics::IWorld::AddObject : ジョイントソルバーの並列処理が有効な場合、センサーは正しく動作しません" );
#endif //_DEBUG
		}

		if( pObject->GetType() == Mix::Dynamics::IObject::RIGIDBODY )
		{
			m_RigidBodyList.push_back( static_cast<Mix::Dynamics::IRigidBody*>( pObject ) );
		}

		m_ObjectSync.Enter();
		m_pWorld->addRigidBody( pInternalObject->Bullet_GetRigidBodyPtr(), filterGroup, filterMask );
		m_ObjectSync.Leave();
	}

	m_ObjectCountTable[objType]++;

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// ワールドに追加されたことを通知
	////////////////////////////////////////////////////////////////////////////////////////////////////

	pInternalObject->GetContextPtr()->NotifyAddedToWorld( this );

	////////////////////////////////////////////////////////////////////////////////////////////////////

	return True;
}

Boolean World::RemoveObject( Mix::Dynamics::IObject* pObject )
{
	if( pObject == NULL )
	{
		MIX_LOG_ERROR( L"%s : %s : pObject[%s]",
			FAILED_REMOVEOBJECT,
			Mix::STR_ILLEGALARG,
			( pObject != NULL )? L"○" : L"NULL" );

		return False;
	}

	World::ObjectList::iterator it = std::find( m_ObjectList.begin(), m_ObjectList.end(), pObject );
	if( it != m_ObjectList.end() )
	{
		Mix::Dynamics::IObject::TYPE objType = pObject->GetType();
		Mix::Dynamics::Object* pInternalObject = dynamic_cast<Mix::Dynamics::Object*>( pObject );

		if( objType == Mix::Dynamics::IObject::KINEMATIC_CHARACTER )
		{
			MIX_ASSERT( pInternalObject->Bullet_GetCollisionObjectPtr() != NULL );
			MIX_ASSERT( pInternalObject->Bullet_GetCharacterControllerInterfacePtr() != NULL );

			m_ObjectSync.Enter();
			m_pWorld->removeCharacter( pInternalObject->Bullet_GetCharacterControllerInterfacePtr() );
			m_pWorld->removeCollisionObject( pInternalObject->Bullet_GetCollisionObjectPtr() );
			m_ObjectSync.Leave();
		}
		else if( objType == Mix::Dynamics::IObject::VEHICLE )
		{
			MIX_ASSERT( pInternalObject->Bullet_GetRigidBodyPtr() != NULL );
			MIX_ASSERT( pInternalObject->Bullet_GetRaycastVehiclePtr() != NULL );

			m_ObjectSync.Enter();
			m_pWorld->removeVehicle( pInternalObject->Bullet_GetRaycastVehiclePtr() );
			m_pWorld->removeRigidBody( pInternalObject->Bullet_GetRigidBodyPtr() );
			m_ObjectSync.Leave();
		}
		else
		{
			MIX_ASSERT( pInternalObject->Bullet_GetRigidBodyPtr() != NULL );

			m_ObjectSync.Enter();
			m_pWorld->removeRigidBody( pInternalObject->Bullet_GetRigidBodyPtr() );
			m_ObjectSync.Leave();

			if( pObject->GetType() == Mix::Dynamics::IObject::RIGIDBODY )
			{
				Mix::STL::Vector_FirstErase( m_RigidBodyList, static_cast<Mix::Dynamics::IRigidBody*>( pObject ) );
			}
		}

		MIX_ASSERT( pInternalObject->GetContextPtr() != NULL );
		Mix::STL::Vector_FirstErase( m_ObjectContextList, pInternalObject->GetContextPtr() );

		Mix::STL::Vector_FirstErase( m_ObjectList, it );

		MIX_ASSERT( m_ObjectCountTable[objType] > 0 );
		m_ObjectCountTable[objType]--;

		//削除されたことを通知
		pInternalObject->GetContextPtr()->NotifyRemovedFromWorld( this );
		pInternalObject->SetWorldPtr( NULL );

		MIX_RELEASE( pObject );
	}
	else
	{
		return False;
	}

	return True;
}

UInt32 World::GetObjectCount( Mix::Dynamics::IObject::TYPE type ) const
{
	return m_ObjectCountTable[type];
}

UInt32 World::GetTotalObjectCount( void ) const
{
	return MIX_UIT_TO_UI32( m_ObjectList.size() );
}

void World::SetGravity( const Mix::Vector3& gravity )
{
	Mix::ScopedLock lock( m_ObjectSync );

	m_pWorld->setGravity( btVector3( gravity.x, gravity.y, gravity.z ) );
	m_Gravity = gravity;
}

const Mix::Vector3& World::GetGravity( void ) const
{
	return m_Gravity;
}

UInt32 World::Activate( void )
{
	UInt32 count = 0;

	if( m_RigidBodyList.size() > 0 )
	{
		World::RigidBodyList::iterator it_begin = m_RigidBodyList.begin();
		World::RigidBodyList::iterator it_end = m_RigidBodyList.end();
		World::RigidBodyList::iterator it;

		for( it = it_begin; it != it_end; ++it )
		{
			if( ( *it )->Activate() == True )
			{
				count++;
			}
		}
	}

	return count;
}

UInt32 World::Deactivate( Boolean bForce )
{
	UInt32 count = 0;

	if( m_RigidBodyList.size() > 0 )
	{
		World::RigidBodyList::iterator it_begin = m_RigidBodyList.begin();
		World::RigidBodyList::iterator it_end = m_RigidBodyList.end();
		World::RigidBodyList::iterator it;

		for( it = it_begin; it != it_end; ++it )
		{
			if( ( *it )->Deactivate( bForce ) == True )
			{
				count++;
			}
		}
	}

	return count;
}

void World::Update( Float32 dt, Float32 baseDt )
{
	////////////////////////////////////////////////////////////////////////////////////////////////////
	// ワールドのシミュレーションを進める
	////////////////////////////////////////////////////////////////////////////////////////////////////

	MIX_DYNAMICS_DEBUG( m_DebTimer.Reset() );

	m_pWorld->stepSimulation( dt, 1, baseDt );

	MIX_DYNAMICS_DEBUG( m_Profile.ssElapsedTime = m_DebTimer.GetF32() );

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// オブジェクトの接触情報を収集
	////////////////////////////////////////////////////////////////////////////////////////////////////

	MIX_DYNAMICS_DEBUG( m_DebTimer.Reset() );

	Int32 manifoldCount = m_pCollisionDispatcher->getNumManifolds();

	for( Int32 i = 0; i < manifoldCount; i++ )
	{
		btPersistentManifold* pManifold = m_pCollisionDispatcher->getManifoldByIndexInternal( i );
		Int32 pointCount = pManifold->getNumContacts();

		if( pointCount <= 0 )
		{
			//接触が存在しない
			continue;
		}

		const btCollisionObject* pObjA = pManifold->getBody0();
		const btCollisionObject* pObjB = pManifold->getBody1();
		Mix::Dynamics::ObjectContext* pContextA = static_cast<Mix::Dynamics::ObjectContext*>( pObjA->getUserPointer() );
		Mix::Dynamics::ObjectContext* pContextB = static_cast<Mix::Dynamics::ObjectContext*>( pObjB->getUserPointer() );

		if( ( pContextA->IsExistsListener() == False ) &&
			( pContextB->IsExistsListener() == False ) )
		{
			//両方ともリスナーを所持していなかったら処理しない
			continue;
		}

		Mix::Dynamics::MANIFOLD_POINT* pPointA;
		Mix::Dynamics::MANIFOLD_POINT* pPointB;
		Mix::Dynamics::MANIFOLD manifoldA;
		Mix::Dynamics::MANIFOLD manifoldB;

		//接触オブジェクト二つ分を確保
		m_ManifoldPoints.resize( pointCount << 1 );

		pPointA = &( m_ManifoldPoints[0] );
		pPointB = &( m_ManifoldPoints[pointCount] );

		manifoldA.pObjectB = pContextB->GetOwnerPtr();
		manifoldA.pointCount = static_cast<UInt32>( pointCount );
		manifoldA.points = pPointA;

		manifoldB.pObjectB = pContextA->GetOwnerPtr();
		manifoldB.pointCount = static_cast<UInt32>( pointCount );
		manifoldB.points = pPointB;

		for( Int32 j = 0; j < pointCount; j++ )
		{
			const btManifoldPoint& point = pManifold->getContactPoint( j );

			Int32 partIndexA = ( point.m_partId0 >= 0 )? point.m_partId0 - 1 : point.m_partId0;
			Int32 partIndexB = ( point.m_partId1 >= 0 )? point.m_partId1 - 1 : point.m_partId1;
			Int32 polygonIndexA = point.m_index0;
			Int32 polygonIndexB = point.m_index1;

			const Mix::Dynamics::MATERIAL& materialA = pContextA->QueryMaterial( partIndexA, polygonIndexA );
			const Mix::Dynamics::MATERIAL& materialB = pContextB->QueryMaterial( partIndexB, polygonIndexB );

			const btVector3& lPosA = point.m_localPointA;
			const btVector3& lPosB = point.m_localPointB;
			const btVector3& wPosA = point.getPositionWorldOnA();
			const btVector3& wPosB = point.getPositionWorldOnB();
			const btVector3& wNormB = point.m_normalWorldOnB;

			Float32 distanceB = point.m_distance1;

			pPointA->localPositionA.Set( lPosA.x(), lPosA.y(), lPosA.z() );
			pPointA->localPositionB.Set( lPosB.x(), lPosB.y(), lPosB.z() );
			pPointA->worldPositionA.Set( wPosA.x(), wPosA.y(), wPosA.z() );
			pPointA->worldPositionB.Set( wPosB.x(), wPosB.y(), wPosB.z() );
			pPointA->worldNormalB.Set( +wNormB.x(), +wNormB.y(), +wNormB.z() );
			pPointA->materialA.id = materialA.id;
			pPointA->materialA.attr = materialA.attr;
			pPointA->materialA.friction = materialA.friction;
			pPointA->materialA.restitution = materialA.restitution;
			pPointA->materialB.id = materialB.id;
			pPointA->materialB.attr = materialB.attr;
			pPointA->materialB.friction = materialB.friction;
			pPointA->materialB.restitution = materialB.restitution;
			pPointA->meshA.partIndex = partIndexA;
			pPointA->meshA.polygonIndex = polygonIndexA;
			pPointA->meshB.partIndex = partIndexB;
			pPointA->meshB.polygonIndex = polygonIndexB;
			pPointA->penetrationB = distanceB;
			pPointA->impulse = point.m_appliedImpulse;
			pPointA->combinedFriction = point.m_combinedFriction;
			pPointA->combinedRestitution = point.m_combinedRestitution;

			pPointB->localPositionA.Set( lPosB.x(), lPosB.y(), lPosB.z() );
			pPointB->localPositionB.Set( lPosA.x(), lPosA.y(), lPosA.z() );
			pPointB->worldPositionA.Set( wPosB.x(), wPosB.y(), wPosB.z() );
			pPointB->worldPositionB.Set( wPosA.x(), wPosA.y(), wPosA.z() );
			pPointB->worldNormalB.Set( -wNormB.x(), -wNormB.y(), -wNormB.z() );
			pPointB->materialA.id = materialB.id;
			pPointB->materialA.attr = materialB.attr;
			pPointB->materialA.friction = materialB.friction;
			pPointB->materialA.restitution = materialB.restitution;
			pPointB->materialB.id = materialA.id;
			pPointB->materialB.attr = materialA.attr;
			pPointB->materialB.friction = materialA.friction;
			pPointB->materialB.restitution = materialA.restitution;
			pPointB->meshA.partIndex = partIndexB;
			pPointB->meshA.polygonIndex = polygonIndexB;
			pPointB->meshB.partIndex = partIndexA;
			pPointB->meshB.polygonIndex = polygonIndexA;
			pPointB->penetrationB = distanceB;
			pPointB->impulse = point.m_appliedImpulse;
			pPointB->combinedFriction = point.m_combinedFriction;
			pPointB->combinedRestitution = point.m_combinedRestitution;

			pPointA++;
			pPointB++;
		}

		pContextA->NotifyContact( manifoldA );
		pContextB->NotifyContact( manifoldB );
	}

	MIX_DYNAMICS_DEBUG( m_Profile.cpElapsedTime = m_DebTimer.GetF32() );
}

Boolean World::TestRay(	const Mix::Vector3& fromWorldPos,
						const Mix::Vector3& toWorldPos,
						UInt16 filterGroup,
						UInt16 filterMask,
						Mix::Dynamics::IWorld::TestCallback* pCallback,
						Mix::Dynamics::IWorld::TEST_RESULT& result )
{
	Boolean bHit;

	if( pCallback != NULL )
	{
		World::TestSegmentCustomResultCallback callback( ToBulletVector3( fromWorldPos ), ToBulletVector3( toWorldPos ), pCallback );

		callback.m_collisionFilterGroup = filterGroup;
		callback.m_collisionFilterMask = filterMask;

		bHit = TestRay( callback, result );
	}
	else
	{
		World::TestSegmentResultCallback callback( ToBulletVector3( fromWorldPos ), ToBulletVector3( toWorldPos ) );

		callback.m_collisionFilterGroup = filterGroup;
		callback.m_collisionFilterMask = filterMask;

		bHit = TestRay( callback, result );
	}

	return bHit;
}

Boolean World::TestSweep(	Mix::Dynamics::IShape* pShape,
							const Mix::Vector3& fromWorldPos,
							const Mix::Vector3& toWorldPos,
							UInt16 filterGroup,
							UInt16 filterMask,
							Mix::Dynamics::IWorld::TestCallback* pCallback,
							Mix::Dynamics::IWorld::TEST_RESULT& result )
{
	if( pShape == NULL )
	{
		MIX_LOG_ERROR( L"%s : %s : pShape[%s]", FAILED_TESTSWEEP, Mix::STR_ILLEGALARG, ( pShape != NULL )? L"○" : L"NULL" );
		return False;
	}

	if( pShape->GetType() == Mix::Dynamics::IShape::COMPOUND )
	{
		MIX_LOG_ERROR( L"%s : コンパウンドシェイプはサポートしていません", FAILED_TESTSWEEP );
		return False;
	}

	Boolean bHit;

	if( pCallback != NULL )
	{
		World::TestSweepCustomResultCallback callback( ToBulletVector3( fromWorldPos ), ToBulletVector3( toWorldPos ), pCallback );

		callback.m_collisionFilterGroup = filterGroup;
		callback.m_collisionFilterMask = filterMask;

		bHit = TestSweep( pShape, callback, result );
	}
	else
	{
		World::TestSweepResultCallback callback( ToBulletVector3( fromWorldPos ), ToBulletVector3( toWorldPos ) );

		callback.m_collisionFilterGroup = filterGroup;
		callback.m_collisionFilterMask = filterMask;

		bHit = TestSweep( pShape, callback, result );
	}

	return bHit;
}

Boolean World::TestRay(	World::TestSegmentResultCallback& callback, Mix::Dynamics::IWorld::TEST_RESULT& result )
{
	m_pWorld->rayTest( callback.m_rayFromWorld, callback.m_rayToWorld, callback );

	if( callback.hasHit() == true )
	{
		result.pObject = static_cast<Mix::Dynamics::ObjectContext*>( callback.m_collisionObject->getUserPointer() )->GetOwnerPtr();
		result.partIndex = ( callback.m_partId > 0 )? ( callback.m_partId - 1 ) : -1;
		result.polygonIndex = callback.m_triangleIndex;
		result.worldPos = ToMixVector3( callback.m_hitPointWorld );
		result.worldNormal = ToMixVector3( callback.m_hitNormalWorld );
	}
	else
	{
		return False;
	}

	return True;
}

Boolean World::TestSweep( Mix::Dynamics::IShape* pShape, World::TestSweepResultCallback& callback, Mix::Dynamics::IWorld::TEST_RESULT& result )
{
	if( pShape == NULL )
	{
		MIX_LOG_ERROR( L"%s : %s : pShape[%s]", FAILED_TESTSWEEP, Mix::STR_ILLEGALARG, ( pShape != NULL )? L"○" : L"NULL" );
		return False;
	}

	if( pShape->GetType() == Mix::Dynamics::IShape::COMPOUND )
	{
		MIX_LOG_ERROR( L"%s : コンパウンドシェイプはサポートしていません", FAILED_TESTSWEEP );
		return False;
	}

	m_pWorld->convexSweepTest(	dynamic_cast<Mix::Dynamics::Shape*>( pShape )->Bullet_GetConvexShapePtr(),
								btTransform( btQuaternion::getIdentity(), callback.m_convexFromWorld ),
								btTransform( btQuaternion::getIdentity(), callback.m_convexToWorld ),
								callback );

	if( callback.hasHit() == true )
	{
		result.pObject = static_cast<Mix::Dynamics::ObjectContext*>( callback.m_hitCollisionObject->getUserPointer() )->GetOwnerPtr();
		result.partIndex = ( callback.m_partId > 0 )? ( callback.m_partId - 1 ) : -1;
		result.polygonIndex = callback.m_triangleIndex;
		result.worldPos = ToMixVector3( callback.m_hitPointWorld );
		result.worldNormal = ToMixVector3( callback.m_hitNormalWorld );
	}
	else
	{
		return False;
	}

	return True;
}

const Mix::Dynamics::DEBUG_WORLD_PROFILE& World::Debug_GetProfile( void ) const
{
	return m_Profile;
}

void World::Debug_Draw( Mix::Graphics::Utility::IPerspectiveRenderer* pPerspectiveRenderer )
{
	if( pPerspectiveRenderer != NULL )
	{
		World::ObjectList::iterator it_obj_begin = m_ObjectList.begin();
		World::ObjectList::iterator it_obj_end = m_ObjectList.end();
		World::ObjectList::iterator it_obj;

		World::JointList::iterator it_joint_begin = m_JointList.begin();
		World::JointList::iterator it_joint_end = m_JointList.end();
		World::JointList::iterator it_joint;

		for( it_obj = it_obj_begin; it_obj != it_obj_end; ++it_obj )
		{
			( *it_obj )->Debug_Draw( pPerspectiveRenderer, 1.0f );
		}

		for( it_joint = it_joint_begin; it_joint != it_joint_end; ++it_joint )
		{
			( *it_joint )->Debug_Draw( pPerspectiveRenderer, 1.0f );
		}
	}
}

}}
