#include "Mix/Tool/Win32/Core/Dynamics/Design/Basic.h"

#include "Mix/Tool/Win32/Core/Graphics/LineHelper.h"
#include "Mix/Tool/Win32/Core/Dynamics/Factory.h"
#include "Mix/Tool/Win32/Core/Dynamics/World.h"
#include "Mix/Tool/Win32/Core/Dynamics/Shape.h"
#include "Mix/Tool/Win32/Core/Dynamics/RigidBody.h"
#include "Mix/Tool/Win32/Core/Dynamics/Design/Actor.h"

namespace Mix{ namespace Tool{ namespace Win32{ namespace Dynamics{ namespace Design{

Basic::Basic( void ) :
m_pSubject( NULL ),
m_pWorld( NULL ),
m_CoordinateSystem( Basic::LEFT_HANDED ),
m_Mode( Basic::EDIT ),
m_DefCenterRot( 0.0f, 0.0f, 0.0f, 1.0f ),
m_DefCenterPos( 0.0f, 0.0f, 0.0f ),
m_bWorldMatReseted( false ),
m_bWorldRotRefreshed( false ),
m_bWorldPosRefreshed( false ),
m_ColliderObjectPtr( NULL ),
m_SelectedObjectPtr( NULL )
{
	////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// ワールドトランスフォームの初期値
	////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	D3DXMatrixIdentity( &m_InitalWorldMat );

	////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// センタートランスフォーム( m_InitalWorldMat が初期化されている必要がある )
	////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	ComputeObjectTransform( m_DefCenterRot, m_DefCenterPos, m_DefCenterMat, m_DefRestoreMat );

	////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// ワールドトランスフォーム
	////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	SetWorldMatrix( m_InitalWorldMat );

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// コライダーの初期の回転と位置を求める( ジョイント用 )
	////////////////////////////////////////////////////////////////////////////////////////////////////

	ComputeInitalRigidBodyTransform( m_DefCenterMat, m_DefInitalColliderRot, m_DefInitalColliderPos );
}

Basic::Basic(	Actor* pSubject,
				Basic::COORDINATE_SYSTEM coordinateSystem,
				const D3DXVECTOR3& localPos,
				const D3DXMATRIX& worldMat,
				const D3DXQUATERNION& centerRot,
				const D3DXVECTOR3& centerPos ) :
m_pSubject( pSubject ),
m_pWorld( NULL ),
m_CoordinateSystem( coordinateSystem ),
m_Mode( Basic::EDIT ),
m_DefCenterRot( centerRot ),
m_DefCenterPos( centerPos ),
m_LocalPos( localPos ),
m_bWorldMatReseted( false ),
m_bWorldRotRefreshed( false ),
m_bWorldPosRefreshed( false ),
m_ColliderObjectPtr( NULL ),
m_SelectedObjectPtr( NULL )
{
	////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// ワールドトランスフォームの初期値
	////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	m_InitalWorldMat = worldMat;

	////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// センタートランスフォーム
	////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	ComputeObjectTransform( m_DefCenterRot, m_DefCenterPos, m_DefCenterMat/*, m_DefInvCenterMat*/, m_DefRestoreMat );

	////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// ワールドトランスフォーム
	////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	SetWorldMatrix( m_InitalWorldMat );

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// コライダーの初期の回転と位置を求める( ジョイント用 )
	////////////////////////////////////////////////////////////////////////////////////////////////////

	ComputeInitalRigidBodyTransform( m_DefCenterMat, m_DefInitalColliderRot, m_DefInitalColliderPos );
}

Basic::~Basic( void )
{
	////////////////////////////////////////////////////////////////////////////////////////////////////
	// オブジェクトをワールドから取り除く
	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( m_pWorld != NULL )
	{
		for( Basic::ObjectMap::iterator it = m_ObjectMap.begin(); it != m_ObjectMap.end(); ++it )
		{
			Basic::OBJECT* pObj = &( it->second );
			m_pWorld->RemoveCollisionObject( pObj->pRigidBody );
		}
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// オブジェクトを解放
	////////////////////////////////////////////////////////////////////////////////////////////////////

	for( Basic::ObjectMap::iterator it = m_ObjectMap.begin(); it != m_ObjectMap.end(); ++it )
	{
		Basic::OBJECT* pObj = &( it->second );

		MIX_DELETE( pObj->handle );
		MIX_DELETE( pObj->pRigidBody );
	}
}

Basic::COORDINATE_SYSTEM Basic::GetCoordinateSystem( void ) const
{
	return m_CoordinateSystem;
}

Basic::MODE Basic::GetMode( void ) const
{
	return m_Mode;
}

const D3DXMATRIX& Basic::GetInitalWorldMatrix( void ) const
{
	return m_InitalWorldMat;
}

const D3DXVECTOR3& Basic::GetWorldScaling( void ) const
{
	return m_WorldS;
}

const D3DXQUATERNION& Basic::GetWorldRotation( void ) const
{
	return m_WorldR;
}

const D3DXVECTOR3& Basic::GetWorldTranslation( void ) const
{
	return m_WorldT;
}

const D3DXMATRIX& Basic::GetWorldMatrix( void ) const
{
	return m_WorldMat;
}

void Basic::SetWorldMatrix( const D3DXMATRIX& worldMat )
{
	m_PreWorldMat = m_WorldMat;
	m_WorldMat = worldMat;

	DecomposeWorldMatrix();

	Collider_UpdateTransform();
}

void Basic::ResetWorldMatrix( void )
{
	SetWorldMatrix( m_InitalWorldMat );
	m_PreWorldMat = m_InitalWorldMat;

	Collider_Reset();

	m_bWorldMatReseted = true;
}

bool Basic::IsWorldMatrixRefreshed( void ) const
{
	return ( m_bWorldRotRefreshed == true ) || ( m_bWorldPosRefreshed == true );
}

bool Basic::IsWorldRotationRefreshed( void ) const
{
	return m_bWorldRotRefreshed;
}

bool Basic::IsWorldPositionRefreshed( void ) const
{
	return m_bWorldPosRefreshed;
}

void Basic::Draw(	Mix::Tool::Win32::Graphics::LineHelper* pLineHelper,
							bool bSensor,
							bool bCollider,
							const D3DXVECTOR4& selectedColor,
							const D3DXVECTOR4& sensorColor,
							const D3DXVECTOR4& colliderColor,
							float axisScale )
{
	if( pLineHelper == NULL )
	{
		return;
	}

	Basic::ObjectMap::iterator it_begin = m_ObjectMap.begin();
	Basic::ObjectMap::iterator it_end = m_ObjectMap.end();
	Basic::ObjectMap::iterator it;

	for( it = it_begin; it != it_end; ++it )
	{
		Basic::OBJECT* pObj = &( it->second );

		if( ( ( pObj->type == Basic::COLLIDER ) && ( bCollider == true ) ) ||
			( ( pObj->type == Basic::SENSOR ) && ( bSensor == true ) ) )
		{
			Mix::Tool::Win32::Dynamics::RigidBody* pRigidBody = pObj->pRigidBody;

			D3DXVECTOR4 defColor = ( pObj->type == Basic::COLLIDER )? colliderColor : sensorColor;
			D3DXVECTOR4 selColor = ( pObj == m_SelectedObjectPtr )? Lerp_B( defColor, selectedColor ) : defColor;

			pRigidBody->Draw( pLineHelper, defColor, selColor );

			if( MIX_FLOAT_IS_ZERO( selectedColor.w ) == false )
			{
				const D3DXQUATERNION& rot = pRigidBody->GetWorldRotation();
				const D3DXVECTOR3& pos = pRigidBody->GetWorldPosition();

				D3DXMATRIX mat;

				D3DXMatrixRotationQuaternion( &mat, &rot );
				mat._41 = pos.x;
				mat._42 = pos.y;
				mat._43 = pos.z;
				mat._44 = 1.0f;

				pLineHelper->SetColor( D3DXVECTOR4( 1.0f, 1.0f, 1.0f, 1.0f ) );
				pLineHelper->SetMatrix( mat );
				pLineHelper->AddAxis( D3DXVECTOR3( 0.0f, 0.0f, 0.0f ), axisScale );
			}
		}
	}
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// オブジェクト : 基本
////////////////////////////////////////////////////////////////////////////////////////////////////

HANDLE Basic::Object_Add( Basic::OBJECT_TYPE type )
{
	if( ( m_ColliderObjectPtr != NULL ) &&
		( type == Basic::COLLIDER ) )
	{
		return NULL;
	}

	/*
		ハンドルの作成
	*/

	int* handle = new int( 0x5F5F5F5F );
	if( handle == NULL )
	{
		return NULL;
	}

	/*
		リジッドボディの作成
	*/

	Mix::Tool::Win32::Dynamics::RigidBody* pRigidBody = Mix::Tool::Win32::Dynamics::Factory::CreateRigidBody();
	if( pRigidBody != NULL )
	{
		if( type == Basic::COLLIDER )
		{
			pRigidBody->SetStatus( Mix::Tool::Win32::Dynamics::RigidBody::DEFAULT );
		}
		else
		{
			pRigidBody->SetStatus( Mix::Tool::Win32::Dynamics::RigidBody::SENSOR );
		}

		pRigidBody->SetWorldTransform( m_DefInitalColliderRot, m_DefInitalColliderPos );
	}
	else
	{
		MIX_DELETE( handle );
		return NULL;
	}

	/*
		オブジェクトを追加
	*/

	Basic::OBJECT obj;
	Basic::ObjectMap::iterator it_obj;

	obj.type = type;
	obj.handle = handle;
	obj.name = ( type == Basic::COLLIDER )? L"Collider" : L"Sensor";
	obj.pRigidBody = pRigidBody;
	obj.centerRot = m_DefCenterRot;
	obj.centerPos = m_DefCenterPos;
	obj.centerMat = m_DefCenterMat;
//	obj.invCenterMat = m_DefInvCenterMat;
	obj.restoreMat = m_DefRestoreMat;
	obj.colliderType = Basic::DEFAULT;
	obj.initalColliderRot = m_DefInitalColliderRot;
	obj.initalColliderPos = m_DefInitalColliderPos;
	obj.rotAxisFlags = Basic::RA_ALL;

	m_ObjectMap.insert( Basic::ObjectMap::value_type( handle, obj ) );

	it_obj = m_ObjectMap.find( handle );
	if( it_obj == m_ObjectMap.end() )
	{
		MIX_DELETE( pRigidBody );
		MIX_DELETE( handle );
		return NULL;
	}

	m_ObjectHandleList.push_back( handle );

	if( type == Basic::COLLIDER )
	{
		m_ColliderObjectPtr = &( it_obj->second );
	}
	else
	{
		m_SensorObjectPtrList.push_back( &( it_obj->second ) );
	}

	return handle;
}

bool Basic::Object_Remove( HANDLE handle )
{
	Basic::ObjectMap::iterator it_obj = m_ObjectMap.find( handle );
	if( it_obj == m_ObjectMap.end() )
	{
		return false;
	}

	if( ( it_obj->second.shapeMap.size() > 0 ) ||
		( it_obj->second.shapeHandleList.size() > 0 ) )
	{
		//オブジェクトを破棄する前に、すべてのシェイプを破棄しておかないといけないことを意味します
		return false;
	}

	/*
		オブジェクトハンドルリストから削除
	*/

	Basic::HandleList::iterator it_obj_handle = std::find( m_ObjectHandleList.begin(), m_ObjectHandleList.end(), handle );
	if( it_obj_handle != m_ObjectHandleList.end() )
	{
		m_ObjectHandleList.erase( it_obj_handle );
	}
	else
	{
		return false;
	}

	/*
		選択の処理
	*/

	if( m_SelectedObjectPtr == &( it_obj->second ) )
	{
		m_SelectedObjectPtr = NULL;
	}

	/*
		メモリの解放
	*/

	MIX_DELETE( it_obj->second.handle );
	MIX_DELETE( it_obj->second.pRigidBody );

	/*
		オブジェクトマップから削除
	*/

	if( m_ColliderObjectPtr == &( it_obj->second ) )
	{
		m_ColliderObjectPtr = NULL;
	}
	else
	{
		Basic::ObjectPtrList::iterator it = std::find( m_SensorObjectPtrList.begin(), m_SensorObjectPtrList.end(), &( it_obj->second ) );
		if( it != m_SensorObjectPtrList.end() )
		{
			m_SensorObjectPtrList.erase( it );
		}
	}

	m_ObjectMap.erase( it_obj );

	return true;
}

bool Basic::Object_GetType( HANDLE hObject, Basic::OBJECT_TYPE& type ) const
{
	const Basic::OBJECT* pObj = Object_GetConstPtr( hObject );

	if( pObj != NULL )
	{
		type = pObj->type;
	}
	else
	{
		type = Basic::COLLIDER;
		return false;
	}

	return true;
}

int Basic::Object_GetColliderCount( void ) const
{
	return ( m_ColliderObjectPtr != NULL )? 1 : 0;
}

int Basic::Object_GetSensorCount( void ) const
{
	return static_cast<int>( m_SensorObjectPtrList.size() );
}

int Basic::Object_GetHandleCount( void ) const
{
	return static_cast<int>( m_ObjectHandleList.size() );
}

HANDLE Basic::Object_GetHandle( int index ) const
{
	if( ( index < 0 ) ||
		( static_cast<int>( m_ObjectHandleList.size() ) <= index ) )
	{
		return NULL;
	}

	return m_ObjectHandleList[index];
}

void Basic::Object_Selected( HANDLE hObject )
{
	Basic::ObjectMap::iterator it = m_ObjectMap.find( hObject );

	if( it != m_ObjectMap.end() )
	{
		m_SelectedObjectPtr = &( it->second );
	}
	else
	{
		m_SelectedObjectPtr = NULL;
	}
}

bool Basic::Object_GetName( HANDLE hObject, std::wstring& name ) const
{
	const Basic::OBJECT* pObj = Object_GetConstPtr( hObject );
	if( pObj != NULL )
	{
		name = pObj->name;
	}
	else
	{
		return false;
	}

	return true;
}

bool Basic::Object_SetName( HANDLE hObject, const wchar_t* pName )
{
	Basic::OBJECT* pObj = Object_GetPtr( hObject );

	if( pObj != NULL )
	{
		pObj->name = ( pName != NULL )? pName : L"";
	}
	else
	{
		return false;
	}

	return true;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// オブジェクト : センタートランフフォーム
////////////////////////////////////////////////////////////////////////////////////////////////////

bool Basic::Object_GetCenterMatrix( HANDLE hObject, D3DXMATRIX& mat ) const
{
	const Basic::OBJECT* pObj = Object_GetConstPtr( hObject );
	if( pObj != NULL )
	{
		mat = pObj->centerMat;
	}
	else
	{
		return false;
	}

	return true;
}

bool Basic::Object_SetCenterTransform( HANDLE hObject, const D3DXQUATERNION& rot, const D3DXVECTOR3& pos )
{
	Basic::OBJECT* pObj = Object_GetPtr( hObject );

	if( pObj != NULL )
	{
		pObj->centerRot = rot;
		pObj->centerPos = pos;

		Object_UpdateCenterTransform( pObj );
		Object_UpdateRigidBodyTransform( pObj );
		Object_UpdateShapeTransform( pObj );

		if( m_ColliderObjectPtr == pObj )
		{
			NotifyColliderChanged();
		}
	}
	else
	{
		return false;
	}

	return true;
}

bool Basic::Object_GetCenterRotation( HANDLE hObject, D3DXQUATERNION& rot ) const
{
	const Basic::OBJECT* pObj = Object_GetConstPtr( hObject );
	if( pObj != NULL )
	{
		rot = pObj->centerRot;
	}
	else
	{
		return false;
	}

	return true;
}

bool Basic::Object_GetCenterRotation( HANDLE hObject, D3DXVECTOR3& rot ) const
{
	const Basic::OBJECT* pObj = Object_GetConstPtr( hObject );
	if( pObj != NULL )
	{
		rot = Mix::Tool::Win32::ToEulerDegreeZYX( pObj->centerRot );
	}
	else
	{
		return false;
	}

	return true;
}

bool Basic::Object_SetCenterRotation( HANDLE hObject, const D3DXQUATERNION& rot )
{
	Basic::OBJECT* pObj = Object_GetPtr( hObject );

	if( pObj != NULL )
	{
		pObj->centerRot = rot;

		Object_UpdateCenterTransform( pObj );
		Object_UpdateRigidBodyTransform( pObj );
		Object_UpdateShapeTransform( pObj );

		if( m_ColliderObjectPtr == pObj )
		{
			NotifyColliderChanged();
		}
	}
	else
	{
		return false;
	}

	return true;
}

bool Basic::Object_SetCenterRotation( HANDLE hObject, const D3DXVECTOR3& rot )
{
	D3DXMATRIX mx;
	D3DXMATRIX my;
	D3DXMATRIX mz;
	D3DXMATRIX mat;
	D3DXQUATERNION quat;

	D3DXMatrixRotationX( &mx, D3DXToRadian( rot.x ) );
	D3DXMatrixRotationY( &my, D3DXToRadian( rot.y ) );
	D3DXMatrixRotationZ( &mz, D3DXToRadian( rot.z ) );

	D3DXMatrixIdentity( &mat );
	D3DXMatrixMultiply( &mat, &mat, &mz );
	D3DXMatrixMultiply( &mat, &mat, &my );
	D3DXMatrixMultiply( &mat, &mat, &mx );

	D3DXQuaternionRotationMatrix( &quat, &mat );

	return Object_SetCenterRotation( hObject, quat );
}

bool Basic::Object_ResetCenterRotation( HANDLE hObject )
{
	return Object_SetCenterRotation( hObject, m_DefCenterRot );
}

bool Basic::Object_GetCenterPosition( HANDLE hObject, D3DXVECTOR3& pos ) const
{
	const Basic::OBJECT* pObj = Object_GetConstPtr( hObject );
	if( pObj != NULL )
	{
		pos = pObj->centerPos;
	}
	else
	{
		return false;
	}

	return true;
}

bool Basic::Object_SetCenterPosition( HANDLE hObject, const D3DXVECTOR3& pos )
{
	Basic::OBJECT* pObj = Object_GetPtr( hObject );

	if( pObj != NULL )
	{
		pObj->centerPos = pos;

		Object_UpdateCenterTransform( pObj );
		Object_UpdateRigidBodyTransform( pObj );
		Object_UpdateShapeTransform( pObj );

		if( m_ColliderObjectPtr == pObj )
		{
			NotifyColliderChanged();
		}
	}
	else
	{
		return false;
	}

	return true;

}

bool Basic::Object_ResetCenterPosition( HANDLE hObject )
{
	return Object_SetCenterPosition( hObject, m_DefCenterPos );
}


////////////////////////////////////////////////////////////////////////////////////////////////////
// オブジェクト : シェイプ
////////////////////////////////////////////////////////////////////////////////////////////////////

HANDLE Basic::Object_AddShape( HANDLE hObject, Mix::Tool::Win32::Dynamics::Shape* pShape, const D3DXQUATERNION& rot, const D3DXVECTOR3& pos )
{
	if( ( hObject == NULL ) ||
		( pShape == NULL ) )
	{
		return NULL;
	}

	Basic::OBJECT* pObj = Object_GetPtr( hObject );
	if( pObj == NULL )
	{
		return NULL;
	}

	/*
		シェイプのトランスフォームをリジッドボディの相対トランスフォームに変換
	*/

	D3DXQUATERNION relRot;
	D3DXVECTOR3 relPos;

	ComputeShapeTransform( pObj, rot, pos, relRot, relPos );

	/*
		リジッドボディにシェイプを追加
	*/

	HANDLE hShape = pObj->pRigidBody->AddShape( pShape, relRot, relPos );
	if( hShape != NULL )
	{
		if( pObj->pRigidBody->GetShapeHandleCount() == 1 )
		{
			if( ( m_pWorld != NULL ) &&
				( m_Mode != Basic::EDIT ) )
			{
				pObj->pRigidBody->Reset();
				m_pWorld->AddCollisionObject( pObj->pRigidBody );
			}

//			if( m_ColliderObjectPtr == pObj )
			if( pObj->type == Basic::COLLIDER )
			{
				NotifyColliderChanged();
				m_pSubject->UpdateCastMotionPart();
			}
		}
	}
	else
	{
		return NULL;
	}

	/*
		オブジェクトにシェイプを追加
	*/

	pObj->shapeMap.insert( Basic::ShapeMap::value_type( hShape, Basic::SHAPE( rot, pos ) ) );
	pObj->shapeHandleList.push_back( hShape );

	return hShape;
}

HANDLE Basic::Object_AddShape( HANDLE hObject, Mix::Tool::Win32::Dynamics::Shape* pShape, const D3DXVECTOR3& rot, const D3DXVECTOR3& pos )
{
	/*
		オイラー角(DEG)からクォータニオンに変換
	*/

	D3DXMATRIX mx;
	D3DXMATRIX my;
	D3DXMATRIX mz;
	D3DXMATRIX mat;
	D3DXQUATERNION quat;

	D3DXMatrixRotationX( &mx, D3DXToRadian( rot.x ) );
	D3DXMatrixRotationY( &my, D3DXToRadian( rot.y ) );
	D3DXMatrixRotationZ( &mz, D3DXToRadian( rot.z ) );

	D3DXMatrixIdentity( &mat );
	D3DXMatrixMultiply( &mat, &mat, &mz );
	D3DXMatrixMultiply( &mat, &mat, &my );
	D3DXMatrixMultiply( &mat, &mat, &mx );

	D3DXQuaternionRotationMatrix( &quat, &mat );

	return Object_AddShape( hObject, pShape, quat, pos );
}

bool Basic::Object_RemoveShape( HANDLE hObject, HANDLE hShape )
{
	if( ( hObject == NULL ) ||
		( hShape == NULL ) )
	{
		return false;
	}

	Basic::OBJECT* pObj = Object_GetPtr( hObject );
	if( pObj == NULL )
	{
		return false;
	}

	/*
		リジッドボディからシェイプを削除
	*/

	if( pObj->pRigidBody->RemoveShape( hShape ) == true )
	{
		if( pObj->pRigidBody->GetShapeHandleCount() == 0 )
		{
			if( m_pWorld != NULL )
			{
				m_pWorld->RemoveCollisionObject( pObj->pRigidBody );
			}

//			if( m_ColliderObjectPtr == pObj )
			if( pObj->type == Basic::COLLIDER )
			{
				NotifyColliderChanged();
				m_pSubject->UpdateCastMotionPart();
			}
		}
	}
	else
	{
		return false;
	}

	/*
		シェイプマップから削除
	*/

	Basic::ShapeMap::iterator it_shape = pObj->shapeMap.find( hShape );
	if( it_shape != pObj->shapeMap.end() )
	{
		pObj->shapeMap.erase( it_shape );
	}
	else
	{
		return false;
	}

	/*
		シェイプハンドルリストから削除
	*/

	Basic::HandleList::iterator it_shape_handle = std::find( pObj->shapeHandleList.begin(), pObj->shapeHandleList.end(), hShape );
	if( it_shape_handle != pObj->shapeHandleList.end() )
	{
		pObj->shapeHandleList.erase( it_shape_handle );
	}
	else
	{
		return false;
	}

	return true;
}

const Mix::Tool::Win32::Dynamics::Shape* Basic::Object_GetShape( HANDLE hObject, HANDLE hShape ) const
{
	if( ( hObject == NULL ) ||
		( hShape == NULL ) )
	{
		return NULL;
	}

	const Basic::OBJECT* pObj = Object_GetConstPtr( hObject );
	if( pObj == NULL )
	{
		return NULL;
	}

	return pObj->pRigidBody->GetShape( hShape );
}

bool Basic::Object_SetShape( HANDLE hObject, HANDLE hShape, Mix::Tool::Win32::Dynamics::Shape* pShape )
{
	if( ( hObject == NULL ) ||
		( hShape == NULL ) )
	{
		return false;
	}

	Basic::OBJECT* pObj = Object_GetPtr( hObject );
	if( pObj == NULL )
	{
		return false;
	}

	return pObj->pRigidBody->SetShape( hShape, pShape );
}

bool Basic::Object_SetShape( HANDLE hObject, HANDLE hShape, Mix::Tool::Win32::Dynamics::Shape* pShape, const D3DXQUATERNION& rot, const D3DXVECTOR3& pos )
{
	if( ( hObject == NULL ) ||
		( hShape == NULL ) )
	{
		return false;
	}

	Basic::OBJECT* pObj = Object_GetPtr( hObject );
	if( pObj == NULL )
	{
		return false;
	}

	if( pObj->pRigidBody->SetShape( hShape, pShape ) == false )
	{
		return false;
	}

	D3DXQUATERNION relRot;
	D3DXVECTOR3 relPos;

	ComputeShapeTransform( pObj, rot, pos, relRot, relPos );

	if( pObj->pRigidBody->SetShapeTransform( hShape, relRot, relPos ) == true )
	{
		Basic::SHAPE* pShapeInfo = Object_GetShapePtrByObjPtr( pObj, hShape );
		if( pShapeInfo != NULL )
		{
			pShapeInfo->rot = rot;
			pShapeInfo->pos = pos;
		}
		else
		{
			return false;
		}
	}
	else
	{
		return false;
	}

	return true;
}

bool Basic::Object_SetShape( HANDLE hObject, HANDLE hShape, Mix::Tool::Win32::Dynamics::Shape* pShape, const D3DXVECTOR3& rot, const D3DXVECTOR3& pos )
{
	D3DXMATRIX mx;
	D3DXMATRIX my;
	D3DXMATRIX mz;
	D3DXMATRIX mat;
	D3DXQUATERNION quat;

	D3DXMatrixRotationX( &mx, D3DXToRadian( rot.x ) );
	D3DXMatrixRotationY( &my, D3DXToRadian( rot.y ) );
	D3DXMatrixRotationZ( &mz, D3DXToRadian( rot.z ) );

	D3DXMatrixIdentity( &mat );
	D3DXMatrixMultiply( &mat, &mat, &mz );
	D3DXMatrixMultiply( &mat, &mat, &my );
	D3DXMatrixMultiply( &mat, &mat, &mx );

	D3DXQuaternionRotationMatrix( &quat, &mat );

	return Object_SetShape( hObject, hShape, pShape, quat, pos );
}

bool Basic::Object_GetShapeRotation( HANDLE hObject, HANDLE hShape, D3DXQUATERNION& rot ) const
{
	if( ( hObject == NULL ) ||
		( hShape == NULL ) )
	{
		return false;
	}

	const Basic::SHAPE* pShape = Object_GetShapeConstPtr( hObject, hShape );
	if( pShape != NULL )
	{
		rot = pShape->rot;
	}
	else
	{
		return false;
	}

	return true;
}

bool Basic::Object_SetShapeRotation( HANDLE hObject, HANDLE hShape, const D3DXQUATERNION& rot )
{
	if( ( hObject == NULL ) ||
		( hShape == NULL ) )
	{
		return false;
	}

	Basic::OBJECT* pObj = Object_GetPtr( hObject );
	if( pObj == NULL )
	{
		return false;
	}

	Basic::SHAPE* pShape = Object_GetShapePtrByObjPtr( pObj, hShape );
	if( pShape == NULL )
	{
		return false;
	}

	D3DXQUATERNION relRot;
	D3DXVECTOR3 relPos;

	ComputeShapeTransform( pObj, rot, pShape->pos, relRot, relPos );

	if( pObj->pRigidBody->SetShapeRotation( hShape, relRot ) == true )
	{
		pShape->rot = rot;
	}
	else
	{
		return false;
	}

	return true;
}

bool Basic::Object_GetShapeRotation( HANDLE hObject, HANDLE hShape, D3DXVECTOR3& rot ) const
{
	if( ( hObject == NULL ) ||
		( hShape == NULL ) )
	{
		return false;
	}

	/*
		クォータニオンからオイラー角(DEG)に変換
	*/

	const Basic::SHAPE* pShape = Object_GetShapeConstPtr( hObject, hShape );
	if( pShape != NULL )
	{
		rot = ToEulerDegreeZYX( pShape->rot );
	}
	else
	{
		return false;
	}

	return true;
}

bool Basic::Object_SetShapeRotation( HANDLE hObject, HANDLE hShape, const D3DXVECTOR3& rot )
{
	/*
		オイラー角(DEG)からクォータニオンに変換
	*/

	D3DXMATRIX mx;
	D3DXMATRIX my;
	D3DXMATRIX mz;
	D3DXMATRIX mat;
	D3DXQUATERNION quat;

	D3DXMatrixRotationX( &mx, D3DXToRadian( rot.x ) );
	D3DXMatrixRotationY( &my, D3DXToRadian( rot.y ) );
	D3DXMatrixRotationZ( &mz, D3DXToRadian( rot.z ) );

	D3DXMatrixIdentity( &mat );
	D3DXMatrixMultiply( &mat, &mat, &mz );
	D3DXMatrixMultiply( &mat, &mat, &my );
	D3DXMatrixMultiply( &mat, &mat, &mx );

	D3DXQuaternionRotationMatrix( &quat, &mat );

	return Object_SetShapeRotation( hObject, hShape, quat );
}

bool Basic::Object_GetShapePosition( HANDLE hObject, HANDLE hShape, D3DXVECTOR3& pos ) const
{
	if( ( hObject == NULL ) ||
		( hShape == NULL ) )
	{
		return false;
	}

	const Basic::SHAPE* pShape = Object_GetShapeConstPtr( hObject, hShape );
	if( pShape != NULL )
	{
		pos = pShape->pos;
	}
	else
	{
		return false;
	}

	return true;
}

bool Basic::Object_SetShapePosition( HANDLE hObject, HANDLE hShape, const D3DXVECTOR3& pos )
{
	if( ( hObject == NULL ) ||
		( hShape == NULL ) )
	{
		return false;
	}

	Basic::OBJECT* pObj = Object_GetPtr( hObject );
	if( pObj == NULL )
	{
		return false;
	}

	Basic::SHAPE* pShape = Object_GetShapePtrByObjPtr( pObj, hShape );
	if( pShape == NULL )
	{
		return false;
	}

	D3DXQUATERNION relRot;
	D3DXVECTOR3 relPos;

	ComputeShapeTransform( pObj, pShape->rot, pos, relRot, relPos );

	if( pObj->pRigidBody->SetShapePosition( hShape, relPos ) == true )
	{
		pShape->pos = pos;
	}
	else
	{
		return false;
	}

	return true;
}

int Basic::Object_GetShapeHandleCount( HANDLE hObject ) const
{
	if( hObject == NULL )
	{
		return 0;
	}

	const Basic::OBJECT* pObj = Object_GetConstPtr( hObject );
	if( pObj == NULL )
	{
		return 0;
	}

	return pObj->shapeHandleList.size();
}

const HANDLE Basic::Object_GetShapeHandle( HANDLE hObject, int index ) const
{
	if( hObject == NULL )
	{
		return NULL;
	}

	const Basic::OBJECT* pObj = Object_GetConstPtr( hObject );
	if( pObj == NULL )
	{
		return NULL;
	}

	if( ( 0 > index ) ||
		( static_cast<int>( pObj->shapeHandleList.size() ) <= index ) )
	{
		return NULL;
	}

	return pObj->shapeHandleList[index];
}

bool Basic::Object_ShapeSelected( HANDLE hObject, HANDLE hShape )
{
	if( hObject == NULL )
	{
		return false;
	}

	Basic::OBJECT* pObj = Object_GetPtr( hObject );
	if( pObj == NULL )
	{
		return false;
	}

	pObj->pRigidBody->SetHighlightShape( hShape );

	return true;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// オブジェクト : マテリアル
////////////////////////////////////////////////////////////////////////////////////////////////////

bool Basic::Object_GetMaterial( HANDLE hObject, Mix::Tool::Win32::Dynamics::MATERIAL& material ) const
{
	const Basic::OBJECT* pObj = Object_GetConstPtr( hObject );
	if( pObj == NULL )
	{
		return false;
	}

	material = pObj->pRigidBody->GetMaterial();

	return true;
}

bool Basic::Object_SetMaterial( HANDLE hObject, const Mix::Tool::Win32::Dynamics::MATERIAL& material )
{
	Basic::OBJECT* pObj = Object_GetPtr( hObject );

	if( pObj != NULL )
	{
		pObj->pRigidBody->SetMaterial( material );
	}
	else
	{
		return false;
	}

	return true;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// オブジェクト : コライダー
////////////////////////////////////////////////////////////////////////////////////////////////////

bool Basic::Collider_GetType( HANDLE hObject, Basic::COLLIDER_TYPE& type )
{
	const Basic::OBJECT* pObj = Object_GetConstPtr( hObject );

	if( pObj != NULL )
	{
		type = pObj->colliderType;
	}
	else
	{
		type = Basic::DEFAULT;
		return false;
	}

	return true;
}

bool Basic::Collider_SetType( HANDLE hObject, Basic::COLLIDER_TYPE type )
{
	Basic::OBJECT* pObj = Object_GetPtr( hObject );

	if( pObj != NULL )
	{
		pObj->colliderType = type;

		if( pObj->type == Basic::COLLIDER )
		{
			Collider_UpdateType();
			m_pSubject->UpdateCastMotionPart();
		}
	}
	else
	{
		return false;
	}

	return true;
}

bool Basic::Collider_GetMass( HANDLE hObject, float& mass ) const
{
	const Basic::OBJECT* pObj = Object_GetConstPtr( hObject );

	if( pObj != NULL )
	{
		mass = pObj->pRigidBody->GetMass();
	}
	else
	{
		mass = 0.0f;
		return false;
	}

	return true;
}

bool Basic::Collider_SetMass( HANDLE hObject, float mass )
{
	Basic::OBJECT* pObj = Object_GetPtr( hObject );

	if( pObj != NULL )
	{
		pObj->pRigidBody->SetMass( mass );
	}
	else
	{
		return false;
	}

	return true;
}

bool Basic::Collider_GetAlwaysActive( HANDLE hObject, bool& state ) const
{
	const Basic::OBJECT* pObj = Object_GetConstPtr( hObject );

	if( pObj != NULL )
	{
		state = pObj->pRigidBody->GetAlwaysActive();
	}
	else
	{
		state = false;
		return false;
	}

	return true;
}

bool Basic::Collider_SetAlwaysActive( HANDLE hObject, bool state )
{
	Basic::OBJECT* pObj = Object_GetPtr( hObject );

	if( pObj != NULL )
	{
		pObj->pRigidBody->SetAlwaysActive( state );
	}
	else
	{
		return false;
	}

	return true;
}

bool Basic::Collider_GetRotationAxis( HANDLE hObject, int& flags ) const
{
	const Basic::OBJECT* pObj = Object_GetConstPtr( hObject );

	if( pObj != NULL )
	{
		flags = pObj->rotAxisFlags;
	}
	else
	{
		flags = 0;
		return false;
	}

	return true;
}

bool Basic::Collider_SetRotationAxis( HANDLE hObject, int flags )
{
	Basic::OBJECT* pObj = Object_GetPtr( hObject );

	if( pObj != NULL )
	{
		D3DXVECTOR3 factor( 0.0f, 0.0f, 0.0f );

		if( MIX_TEST_BIT( flags, Basic::RA_X ) == Basic::RA_X ) { factor.x = 1.0f; }
		if( MIX_TEST_BIT( flags, Basic::RA_Y ) == Basic::RA_Y ) { factor.y = 1.0f; }
		if( MIX_TEST_BIT( flags, Basic::RA_Z ) == Basic::RA_Z ) { factor.z = 1.0f; }

		pObj->rotAxisFlags = flags;
		pObj->pRigidBody->SetAngularFactor( factor );
	}
	else
	{
		return false;
	}

	return true;
}

bool Basic::Collider_IsAvailable( void ) const
{
	if( ( m_ColliderObjectPtr == NULL ) ||
		( m_ColliderObjectPtr->pRigidBody->GetShapeHandleCount() == 0 ) )
	{
		return false;
	}

	return true;
}

bool Basic::Collider_IsDefault( void ) const
{
	if( Collider_IsAvailable() == false )
	{
		return false;
	}

	return ( m_ColliderObjectPtr->colliderType == Basic::DEFAULT );
}

bool Basic::Collider_IsStatic( void ) const
{
	if( Collider_IsAvailable() == false )
	{
		return false;
	}

	return ( m_ColliderObjectPtr->colliderType == Basic::STATIC );
}

bool Basic::Collider_IsKinematic( void ) const
{
	if( Collider_IsAvailable() == false )
	{
		return false;
	}

	return ( m_ColliderObjectPtr->colliderType == Basic::KINEMATIC );
}

bool Basic::Collider_IsCastMotion( void ) const
{
	if( Collider_IsAvailable() == false )
	{
		return false;
	}

	return m_pSubject->IsCastMotionPart( this );
}

const D3DXMATRIX& Basic::Collider_GetCenterMatrix( void ) const
{
	if( Collider_IsAvailable() == false )
	{
		return m_DefCenterMat;
	}

	return m_ColliderObjectPtr->centerMat;
}

const D3DXQUATERNION& Basic::Collider_GetInitalRotation( void ) const
{
	if( Collider_IsAvailable() == false )
	{
		return m_DefInitalColliderRot;
	}

	return m_ColliderObjectPtr->initalColliderRot;
}

const D3DXVECTOR3& Basic::Collider_GetInitalPosition( void ) const
{
	if( Collider_IsAvailable() == false )
	{
		return m_DefInitalColliderPos;
	}

	return m_ColliderObjectPtr->initalColliderPos;
}

const Basic::OBJECT* Basic::Collider_GetPtr( void ) const
{
	return m_ColliderObjectPtr;
}

Mix::Tool::Win32::Dynamics::RigidBody* Basic::Collider_GetRigidBodyPtr( void )
{
	if( Collider_IsAvailable() == false )
	{
		return NULL;
	}

	return m_ColliderObjectPtr->pRigidBody;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// オブジェクト : センサー
////////////////////////////////////////////////////////////////////////////////////////////////////

int Basic::Sensor_GetCount( void ) const
{
	return static_cast<int>( m_SensorObjectPtrList.size() );
}

const Basic::OBJECT* Basic::Sensor_GetPtr( int index ) const
{
	if( ( index < 0 ) ||
		( static_cast<int>( m_SensorObjectPtrList.size() ) <= index ) )
	{
		return NULL;
	}

	return m_SensorObjectPtrList[index];
}

bool Basic::Sensor_IsAvailale( int index ) const
{
	const Basic::OBJECT* pObj = Sensor_GetPtr( index );
	if( pObj == NULL )
	{
		return false;
	}

	return ( pObj->pRigidBody->GetShapeHandleCount() > 0 );
}

int Basic::Sensor_CountAvailable( void ) const
{
	int num = Sensor_GetCount();
	int count = 0;

	for( int i = 0; i < num; i++ )
	{
		if( Sensor_IsAvailale( i ) == true )
		{
			count++;
		}
	}

	return count;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// ユーティリティ
////////////////////////////////////////////////////////////////////////////////////////////////////

void Basic::Utility_ComputeSimpleTransform( const D3DXQUATERNION& centerRot, const D3DXVECTOR3& centerPos, D3DXMATRIX& centerMat, D3DXMATRIX& restoreMat, D3DXQUATERNION& rot, D3DXVECTOR3& pos )
{
	ComputeObjectTransform( centerRot, centerPos, centerMat, restoreMat );
	ComputeInitalRigidBodyTransform( centerMat, rot, pos );
}

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

void Basic::OnWorldChanged( const Basic::WORLD_CHANGED_EVENT_ARGS& args )
{
	if( args.pOldWorld != args.pNewWorld )
	{
		if( m_Mode != Basic::EDIT )
		{
			if( args.pOldWorld != NULL )
			{
				//直前のワールドからディタッチ
				DetachWorld( args.pOldWorld );
			}

			if( args.pNewWorld != NULL )
			{
				//新しいワールドにアタッチ
				AttachWorld( args.pNewWorld );
			}
		}

		m_pWorld = args.pNewWorld;
	}
}

void Basic::OnModeChanged( const Basic::MODE_CHANGED_EVENT_ARGS& args )
{
	////////////////////////////////////////////////////////////////////////////////////////////////////

	//UpdateBodyStatus で m_Mode を見ているので、ここで変更
	m_Mode = args.newMode;

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

	//ワールドからディタッチする前にリセットしておかないと、UpdateBodyStatus の OnColliderChanged で パートのジョイントのフレームの再設定がおかしくなる

#if 0

	ResetWorldMatrix();

#else

	if( ( args.newMode == Basic::EDIT ) ||
		( args.newMode == Basic::STAND ) )
	{
		ResetWorldMatrix();

		m_bWorldRotRefreshed = true;
		m_bWorldPosRefreshed = true;
	}

#endif

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

	//ワールドにアタッチする前にリジッドボディのステータスを変更しておかないと、
	//グラビティアクセラレーションが設定されない( キネマティック指定のリジッドボディのラグドール対応 )
	if( Collider_UpdateType() == false )
	{
		NotifyColliderChanged();
	}

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

	if( m_pWorld != NULL )
	{
		if( ( args.oldMode != Basic::EDIT ) &&
			( args.newMode == Basic::EDIT ) )
		{
			DetachWorld( m_pWorld );
		}
		else if(	( args.oldMode == Basic::EDIT ) &&
					( args.newMode != Basic::EDIT ) )
		{
			AttachWorld( m_pWorld );
		}
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// コライダー( キネマティック )に撃力を適用する
	////////////////////////////////////////////////////////////////////////////////////////////////////

	if(	( args.newMode == Basic::RAGDOLL ) &&
		( Collider_IsKinematic() == true ) )
	{
		Mix::Tool::Win32::Dynamics::RigidBody* pRigidBody = m_ColliderObjectPtr->pRigidBody;

		D3DXVECTOR3 impulse;
		D3DXMATRIX bodyMat;
		D3DXMATRIX invBodyMat;
		D3DXVECTOR3 relPos;

		impulse.x = ( m_WorldMat._41 - m_PreWorldMat._41 );
		impulse.y = ( m_WorldMat._42 - m_PreWorldMat._42 );
		impulse.z = ( m_WorldMat._43 - m_PreWorldMat._43 );

		D3DXMatrixMultiply( &bodyMat, &( m_ColliderObjectPtr->centerMat ), &m_InitalWorldMat );
		D3DXMatrixInverse( &invBodyMat, NULL, &bodyMat );

		D3DXVec3TransformCoord( &relPos, &D3DXVECTOR3( m_InitalWorldMat._41, m_InitalWorldMat._42, m_InitalWorldMat._43 ), &invBodyMat );

//		relPos.x *= m_AbsWorldS.x;
//		relPos.y *= m_AbsWorldS.y;
//		relPos.z *= m_AbsWorldS.z;
		relPos.x *= m_WorldS.x;
		relPos.y *= m_WorldS.y;
		relPos.z *= m_WorldS.z;

		pRigidBody->ApplyImpulse( args.impulse * pRigidBody->GetInvMass() );
		pRigidBody->ApplyImpulse( impulse * pRigidBody->GetInvMass(), relPos );
	}
}

void Basic::OnColliderChanged( void )
{
}

void Basic::OnRefresh( void )
{
	bool worldTransformRefreshed = m_bWorldMatReseted;

	Basic::ObjectPtrList::iterator it_sensor_begin = m_SensorObjectPtrList.begin();
	Basic::ObjectPtrList::iterator it_sensor_end = m_SensorObjectPtrList.end();
	Basic::ObjectPtrList::iterator it_sensor;

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// コライダー
	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( m_Mode != Basic::EDIT )
	{
		if( Collider_IsAvailable() == true )
		{
			/*
				有効なコライダー
			*/

			Mix::Tool::Win32::Dynamics::RigidBody* pRigidBody = m_ColliderObjectPtr->pRigidBody;
			RigidBody::STATUS rbStatus = pRigidBody->GetStatus();

			if( rbStatus == RigidBody::DEFAULT )
			{
				// デフォルト : リジッドボディからワールドトランスフォームを求める

				D3DXQUATERNION rot;
				D3DXVECTOR3 pos;

				rot = pRigidBody->GetWorldRotation();
				pos = pRigidBody->GetWorldPosition();

				if( m_pSubject->IsCastMotion( this ) == true )
				{
					/*
						モデルにモーションを投げる
						自身のワールドトランスフォームはノード側から設定される
					*/

					D3DXMATRIX bodyMat;

					D3DXMatrixRotationQuaternion( &bodyMat, &rot );
					bodyMat._41 = pos.x;
					bodyMat._42 = pos.y;
					bodyMat._43 = pos.z;
					bodyMat._44 = 1.0f;

					m_pSubject->CastMotion( bodyMat );
				}
				else
				{
					/*
						自身にモーションを伝達
					*/

					D3DXMATRIX tempMat;
					D3DXMATRIX rbMat;
//					D3DXVECTOR3 worldScale;

//					D3DXMatrixScaling( &bodyMat, m_AbsWorldS.x, m_AbsWorldS.y, m_AbsWorldS.z );
					D3DXMatrixScaling( &rbMat, m_WorldS.x, m_WorldS.y, m_WorldS.z );
					rbMat *= *D3DXMatrixRotationQuaternion( &tempMat, &rot );
					rbMat._41 = pos.x; rbMat._42 = pos.y; rbMat._43 = pos.z; rbMat._44 = 1.0f;

					m_PreWorldMat = m_WorldMat;
					D3DXMatrixMultiply( &m_WorldMat, &( m_ColliderObjectPtr->restoreMat ), &rbMat );

					DecomposeWorldMatrix();
//					D3DXMatrixDecompose( &worldScale, &m_WorldR, &m_WorldT, &m_WorldMat ); //座標系の違いに対応するため、スケールは更新しない

					//ワールドトランスフォームがリフレッシュされた
					worldTransformRefreshed = true;
				}
			}
			else
			{
				// スタティック or キネマティック : ワールドトランスフォームは外部から設定されている
				;
			}
		}
		else
		{
			/*
				無効なコライダー
				ワールドトランスフォームのリフレッシュ要求を出す
			*/

			D3DXMATRIX worldMat;

			if( OnRefreshWorldTransform( worldMat ) == true )
			{
				SetWorldMatrix( worldMat );
			}
		}
	}
	else
	{
		// 編集中 : ワールドトランスフォームは外部から設定されている
		;
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// センサー
	////////////////////////////////////////////////////////////////////////////////////////////////////

	for( it_sensor = it_sensor_begin; it_sensor != it_sensor_end; ++it_sensor )
	{
		Basic::OBJECT* pObj = *it_sensor;
		Object_UpdateRigidBodyTransform( pObj );
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// ワールドトランスフォームのリフレッシュ設定
	////////////////////////////////////////////////////////////////////////////////////////////////////

	m_bWorldMatReseted = false;
	m_bWorldRotRefreshed = worldTransformRefreshed;
	m_bWorldPosRefreshed = worldTransformRefreshed;
}

bool Basic::OnRefreshWorldTransform( D3DXMATRIX& worldMat )
{
	worldMat = m_WorldMat;

	return true;
}

Basic::OBJECT* Basic::Object_GetPtr( HANDLE hObject )
{
	Basic::ObjectMap::iterator it_obj = m_ObjectMap.find( hObject );
	if( it_obj == m_ObjectMap.end() )
	{
		return NULL;
	}

	return &( it_obj->second );
}

const Basic::OBJECT* Basic::Object_GetConstPtr( HANDLE hObject ) const
{
	Basic::ObjectMap::const_iterator it_obj = m_ObjectMap.find( hObject );
	if( it_obj == m_ObjectMap.end() )
	{
		return NULL;
	}

	return &( it_obj->second );
}

Basic::SHAPE* Basic::Object_GetShapePtr( HANDLE hObject, HANDLE hShape )
{
	Basic::OBJECT* pObj = Object_GetPtr( hObject );
	if( pObj == NULL )
	{
		return NULL;
	}

	return Object_GetShapePtrByObjPtr( pObj, hShape );
}

const Basic::SHAPE* Basic::Object_GetShapeConstPtr( HANDLE hObject, HANDLE hShape ) const
{
	const Basic::OBJECT* pObj = Object_GetConstPtr( hObject );
	if( pObj == NULL )
	{
		return NULL;
	}

	return Object_GetShapeConstPtrByObjPtr( pObj, hShape );
}

Basic::SHAPE* Basic::Object_GetShapePtrByObjPtr( Basic::OBJECT* pObj, HANDLE hShape )
{
	Basic::ShapeMap::iterator it_shape = pObj->shapeMap.find( hShape );
	if( it_shape == pObj->shapeMap.end() )
	{
		return NULL;
	}

	return &( it_shape->second );
}

const Basic::SHAPE* Basic::Object_GetShapeConstPtrByObjPtr( const Basic::OBJECT* pObj, HANDLE hShape ) const
{
	Basic::ShapeMap::const_iterator it_shape = pObj->shapeMap.find( hShape );
	if( it_shape == pObj->shapeMap.end() )
	{
		return NULL;
	}

	return &( it_shape->second );
}

void Basic::Object_UpdateCenterTransform( Basic::OBJECT* pObj )
{
	ComputeObjectTransform( pObj->centerRot, pObj->centerPos, pObj->centerMat, pObj->restoreMat );
	ComputeInitalRigidBodyTransform( pObj->centerMat, pObj->initalColliderRot, pObj->initalColliderPos );
}

void Basic::Object_UpdateShapeTransform( Basic::OBJECT* pObj )
{
	Basic::ShapeMap::iterator it_begin = pObj->shapeMap.begin();
	Basic::ShapeMap::iterator it_end = pObj->shapeMap.end();
	Basic::ShapeMap::iterator it;

	for( it = it_begin; it != it_end; ++it )
	{
		Basic::SHAPE* pShape = &( it->second );

		D3DXQUATERNION relRot;
		D3DXVECTOR3 relPos;

		/*
			リジッドボディの空間のシェイプの回転と位置を求める
		*/

		ComputeShapeTransform( pObj, pShape->rot, pShape->pos, relRot, relPos );

		/*
			回転と位置をシェイプに設定
		*/

		pObj->pRigidBody->SetShapeRotation( it->first, relRot );
		pObj->pRigidBody->SetShapePosition( it->first, relPos );
	}
}

void Basic::Object_UpdateRigidBodyTransform( Basic::OBJECT* pObj )
{
	D3DXQUATERNION rot;
	D3DXVECTOR3 pos;

	/*
		回転と位置を求める
	*/

	ComputeRigidBodyTransform( pObj->centerMat, rot, pos );

	/*
		回転と位置をリジッドボディに設定
	*/

	pObj->pRigidBody->SetWorldTransform( rot, pos );
}

void Basic::Collider_Reset( void )
{
	if( m_ColliderObjectPtr != NULL )
	{
		m_ColliderObjectPtr->pRigidBody->Reset();
	}
}

bool Basic::Collider_UpdateType( void )
{
	if( m_ColliderObjectPtr == NULL )
	{
		return false;
	}

	Mix::Tool::Win32::Dynamics::RigidBody* pRigidBody = m_ColliderObjectPtr->pRigidBody;
	RigidBody::STATUS oldStatus = pRigidBody->GetStatus();
	bool bChanged = false;

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

	if( m_ColliderObjectPtr->colliderType == Basic::STATIC )
	{
		//スタティック(常にスタティック)
		pRigidBody->SetStatus( RigidBody::STATIC );
	}
	else
	{
		//デフォルト or キネマティック(モードに応じて変化)
		switch( m_Mode )
		{
		case Basic::EDIT:
		case Basic::STAND:
			pRigidBody->SetStatus( ( m_ColliderObjectPtr->colliderType == Basic::KINEMATIC )? RigidBody::KINEMATIC : RigidBody::DEFAULT );
			break;
		case Basic::RAGDOLL:
			pRigidBody->SetStatus( RigidBody::DEFAULT );
			break;
		}
	}
/*
	if( oldStatus == Mix::Tool::Win32::Dynamics::RigidBody::STATIC )
	{
		//スタティック(常にスタティック)
		pRigidBody->SetStatus( RigidBody::STATIC );
	}
	else if( oldStatus == Mix::Tool::Win32::Dynamics::RigidBody::SENSOR )
	{
		//センサー(この動作はないはず)
		pRigidBody->SetStatus( RigidBody::SENSOR );
	}
	else
	{
		//デフォルト or キネマティック(モードに応じて変化)
		switch( m_Mode )
		{
		case Basic::EDIT:
		case Basic::STAND:
			pRigidBody->SetStatus( ( m_ColliderObjectPtr->colliderType == Basic::KINEMATIC )? RigidBody::KINEMATIC : RigidBody::DEFAULT );
			break;
		case Basic::RAGDOLL:
			pRigidBody->SetStatus( RigidBody::DEFAULT );
			break;
		}
	}
*/
	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( pRigidBody->GetStatus() != oldStatus )
	{
		NotifyColliderChanged();
		bChanged = true;
	}

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

	return bChanged;
}

void Basic::Collider_UpdateTransform( void )
{
	if( m_ColliderObjectPtr == NULL )
	{
		return;
	}

	D3DXMATRIX mat;
	D3DXQUATERNION rot;
	D3DXVECTOR3 pos;

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// リジッドボディの行列を求める
	////////////////////////////////////////////////////////////////////////////////////////////////////

	D3DXMatrixMultiply( &mat, &( m_ColliderObjectPtr->centerMat ), &m_WorldMat );

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// サインを残したままスケールを除外して、回転と位置を取り出す( 回転がおかしくならないように )
	////////////////////////////////////////////////////////////////////////////////////////////////////

	mat = Mix::Tool::Win32::Matrix_ExcludeScaling( mat );

	D3DXQuaternionRotationMatrix( &rot, &mat );
	D3DXQuaternionNormalize( &rot, &rot );

	pos.x = mat._41;
	pos.y = mat._42;
	pos.z = mat._43;

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// リジッドボディのトランスフォーム
	// 回転、位置、力、速度、補完なのどパラメータもリセットする
	////////////////////////////////////////////////////////////////////////////////////////////////////

	m_ColliderObjectPtr->pRigidBody->SetWorldTransform( rot, pos );
}

void Basic::ComputeObjectTransform( const D3DXQUATERNION& centerRot, const D3DXVECTOR3& centerPos, D3DXMATRIX& centerMat/*, D3DXMATRIX& invCenterMat*/, D3DXMATRIX& restoreMat )
{
	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 自身( ボーン )からオブジェクトへの変換行列を求める
	////////////////////////////////////////////////////////////////////////////////////////////////////

	if( m_CoordinateSystem == Basic::LEFT_HANDED )
	{
		//左手座標系
		D3DXMatrixRotationQuaternion( &centerMat, &centerRot );

		centerMat._41 = centerPos.x;
		centerMat._42 = centerPos.y;
		centerMat._43 = centerPos.z;
		centerMat._44 = 1.0f;

//		D3DXMatrixInverse( &invCenterMat, NULL, &centerMat );
	}
	else
	{
		//右手座標系
		D3DXMATRIX invertMat;
		D3DXMATRIX tempMat;

		D3DXMatrixScaling( &invertMat, +1.0f, +1.0f, -1.0f );

		D3DXMatrixRotationQuaternion( &centerMat, &centerRot );
		centerMat = *D3DXMatrixMultiply( &tempMat, &centerMat, &invertMat );

		centerMat._41 = centerPos.x;
		centerMat._42 = centerPos.y;
		centerMat._43 = centerPos.z;
		centerMat._44 = 1.0f;

//		D3DXMatrixInverse( &invCenterMat, NULL, &centerMat );
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// オブジェクトから、自身( ボーン )のトランスフォームへの変換行列を求める
	////////////////////////////////////////////////////////////////////////////////////////////////////

	D3DXMATRIX initBodyMat;
	D3DXMATRIX invInitBodyMat;

	D3DXMatrixMultiply( &initBodyMat, &centerMat, &m_InitalWorldMat );
	D3DXMatrixInverse( &invInitBodyMat, NULL, &initBodyMat );

	D3DXMatrixMultiply( &restoreMat, &m_InitalWorldMat, &invInitBodyMat );
}

void Basic::ComputeShapeTransform( Basic::OBJECT* pObj, const D3DXQUATERNION& sr, const D3DXVECTOR3& sp, D3DXQUATERNION& dr, D3DXVECTOR3& dp )
{
	D3DXVECTOR3 tempVec;
	D3DXMATRIX tempMat;

	D3DXMATRIX mat;
	D3DXMATRIX invBodyMat;
	D3DXQUATERNION rbRot;
	D3DXVECTOR3 rbPos;

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 元になる行列を求める
	////////////////////////////////////////////////////////////////////////////////////////////////////

	D3DXMatrixRotationQuaternion( &mat, &sr );

	mat._41 = sp.x;
	mat._42 = sp.y;
	mat._43 = sp.z;
	mat._44 = 1.0f;

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 元の行列をセンター( リジッドボディ )の空間( オフセット )へ変換
	////////////////////////////////////////////////////////////////////////////////////////////////////

	/*
		リジッドボディ空間へ変換するための行列を求める
	*/

	ComputeInitalRigidBodyTransform( pObj->centerMat, rbRot, rbPos );

	D3DXMatrixRotationQuaternion( &tempMat, &rbRot );
	tempMat._41 = rbPos.x;
	tempMat._42 = rbPos.y;
	tempMat._43 = rbPos.z;
	tempMat._44 = 1.0f;

	D3DXMatrixInverse( &invBodyMat, NULL, &tempMat );

	/*
		リジッドボディ空間のシェイプのトランスフォームを求める
	*/

	mat = *D3DXMatrixMultiply( &tempMat, &mat, &m_InitalWorldMat );
	mat = *D3DXMatrixMultiply( &tempMat, &Mix::Tool::Win32::Matrix_ExcludeScaling( mat ), &invBodyMat ); //直前の変換でスケールが適用されているのでスケールを除外する

#if 0
	if( GetCoordinateSystem() == Basic::RIGHT_HANDED )
	{
		D3DXMATRIX scalingMat;

		D3DXMatrixScaling( &scalingMat, +1.0f, +1.0f, -1.0f );
		mat = *D3DXMatrixMultiply( &tempMat, &mat, &scalingMat );
	}

	Mix::Tool::Win32::Matrix_Decompose( mat, NULL, &dr, &dp );
#else
	D3DXMatrixDecompose( &tempVec, &dr, &dp, &mat );
#endif
}

void Basic::ComputeRigidBodyTransform( const D3DXMATRIX& centerMat, D3DXQUATERNION& rot, D3DXVECTOR3& pos )
{
	D3DXMATRIX mat;

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// リジッドボディの行列を求める
	////////////////////////////////////////////////////////////////////////////////////////////////////

	D3DXMatrixMultiply( &mat, &centerMat, &m_WorldMat );

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// サインを残したままスケールを除外して、回転と位置を取り出す( 回転がおかしくならないように )
	////////////////////////////////////////////////////////////////////////////////////////////////////

	mat = Mix::Tool::Win32::Matrix_ExcludeScaling( mat );

	D3DXQuaternionRotationMatrix( &rot, &mat );
	D3DXQuaternionNormalize( &rot, &rot );

	pos.x = mat._41;
	pos.y = mat._42;
	pos.z = mat._43;
}

void Basic::ComputeInitalRigidBodyTransform( const D3DXMATRIX& centerMat, D3DXQUATERNION& rot, D3DXVECTOR3& pos )
{
	D3DXMATRIX mat;

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// リジッドボディの行列を求める
	////////////////////////////////////////////////////////////////////////////////////////////////////

	D3DXMatrixMultiply( &mat, &centerMat, &m_InitalWorldMat );

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// サインを残したままスケールを除外して、回転と位置を取り出す( 回転がおかしくならないように )
	////////////////////////////////////////////////////////////////////////////////////////////////////

	mat = Mix::Tool::Win32::Matrix_ExcludeScaling( mat );

	D3DXQuaternionRotationMatrix( &rot, &mat );
	D3DXQuaternionNormalize( &rot, &rot );

	pos.x = mat._41;
	pos.y = mat._42;
	pos.z = mat._43;
}

void Basic::ComposeWorldMatrix( void )
{
	D3DXMATRIX tempMat;

	D3DXMatrixScaling( &m_WorldMat, m_WorldS.x, m_WorldS.y, m_WorldS.z );

	m_WorldMat *= *::D3DXMatrixRotationQuaternion( &tempMat, &m_WorldR );

	m_WorldMat._41 = m_WorldT.x;
	m_WorldMat._42 = m_WorldT.y;
	m_WorldMat._43 = m_WorldT.z;
	m_WorldMat._44 = 1.0f;
}

void Basic::DecomposeWorldMatrix( void )
{
	Mix::Tool::Win32::Matrix_Decompose( m_WorldMat, &m_WorldS, &m_WorldR, &m_WorldT );
}

Mix::Tool::Win32::Dynamics::World* Basic::GetWorldPtr( void )
{
	return m_pWorld;
}

void Basic::AttachWorld( Mix::Tool::Win32::Dynamics::World* pWorld )
{
	if( m_SensorObjectPtrList.size() > 0 )
	{
		Basic::ObjectPtrList::iterator it_sensor_begin = m_SensorObjectPtrList.begin();
		Basic::ObjectPtrList::iterator it_sensor_end = m_SensorObjectPtrList.end();
		Basic::ObjectPtrList::iterator it_sensor;

		for( it_sensor = it_sensor_begin; it_sensor != it_sensor_end; ++it_sensor )
		{
			Basic::OBJECT* pObj = *it_sensor;

			if( pObj->pRigidBody->GetShapeHandleCount() > 0 )
			{
				pWorld->AddCollisionObject( pObj->pRigidBody );
			}
		}
	}

	if( Collider_IsAvailable() == true )
	{
		Mix::Tool::Win32::Dynamics::RigidBody* pRigidBody = m_ColliderObjectPtr->pRigidBody;

		pRigidBody->Reset();
		pWorld->AddCollisionObject( pRigidBody );
	}
}

void Basic::DetachWorld( Mix::Tool::Win32::Dynamics::World* pWorld )
{
	if( m_SensorObjectPtrList.size() > 0 )
	{
		Basic::ObjectPtrList::iterator it_sensor_begin = m_SensorObjectPtrList.begin();
		Basic::ObjectPtrList::iterator it_sensor_end = m_SensorObjectPtrList.end();
		Basic::ObjectPtrList::iterator it_sensor;

		for( it_sensor = it_sensor_begin; it_sensor != it_sensor_end; ++it_sensor )
		{
			Basic::OBJECT* pObj = *it_sensor;
			pWorld->RemoveCollisionObject( pObj->pRigidBody );
		}
	}

	if( m_ColliderObjectPtr != NULL )
	{
		pWorld->RemoveCollisionObject( m_ColliderObjectPtr->pRigidBody );
	}
}

void Basic::NotifyColliderChanged( void )
{
	if( m_pSubject != NULL )
	{
		m_pSubject->OnColliderChanged();
	}
}

}}}}}
