#include "Mix/Tool/Win32/Core/Dynamics/Impl/RigidBody.h"

#include "Mix/Tool/Win32/Core/Graphics/LineHelper.h"
#include "Mix/Tool/Win32/Core/Dynamics/Impl/World.h"
#include "Mix/Tool/Win32/Core/Dynamics/Impl/BoxShape.h"

#include "btBulletCollisionCommon.h"

namespace Mix{ namespace Tool{ namespace Win32{ namespace Dynamics{ namespace Impl{

RigidBody::RigidBody( void ) :
m_pWorld( NULL ),
m_pMotionState( NULL ),
m_pShape( NULL ),
m_pRigidBody( NULL ),
m_Mass( 1.0f ),
m_bPick( false ),
m_bAlwaysActive( false ),
m_Status( RigidBody::DEFAULT ),
m_Group( FG_DEFAULT ),
m_GroupMask( FG_DEFAULT | FG_STATIC | FG_KINEMATIC | FG_SENSOR ),
m_hHighlightShape( INVALID_HANDLE_VALUE ),
m_pHighlightShape( NULL )
{
}

RigidBody::~RigidBody( void )
{
	for( RigidBody::ShapeInfoMap::iterator it = m_ShapeInfoMap.begin(); it != m_ShapeInfoMap.end(); ++it )
	{
		MIX_DELETE( it->second.handle );
	}

	MIX_DELETE( m_pRigidBody );
	MIX_DELETE( m_pMotionState );
	MIX_DELETE( m_pShape );
}

bool RigidBody::Initialize( void )
{
	if( ( m_pShape != NULL ) ||
		( m_pMotionState != NULL ) ||
		( m_pRigidBody != NULL ) )
	{
		return false;
	}

	m_pMotionState = new btDefaultMotionState();
	if( m_pMotionState == NULL )
	{
		return false;
	}

	m_pShape = new btCompoundShape();
	if( m_pShape != NULL )
	{
		m_pShape->setUserPointer( NULL );
	}
	else
	{
		MIX_DELETE( m_pMotionState );
		return false;
	}

	m_pRigidBody = new ::btRigidBody( m_Mass, m_pMotionState, m_pShape );
	if( m_pRigidBody != NULL )
	{
		m_pRigidBody->setFriction( m_Material.friction );
		m_pRigidBody->setRestitution( m_Material.restitution );
		m_pRigidBody->setUserPointer( this );
	}
	else
	{
		MIX_DELETE( m_pShape );
		MIX_DELETE( m_pMotionState );
		return false;
	}

	return true;
}

void RigidBody::SetWorldPtr( Mix::Tool::Win32::Dynamics::Impl::World* pWorld )
{
	m_pWorld = pWorld;
}

btRigidBody* RigidBody::Bullet_GetRigidBodyPtr( void )
{
	return m_pRigidBody;
}

void RigidBody::BeginRefresh( void )
{
	if( m_pWorld != NULL )
	{
		m_pWorld->DetachCollisionObject( this, false );
	}
}

void RigidBody::EndRefresh( void )
{
	if( m_pWorld != NULL )
	{
		if( m_pShape->getNumChildShapes() > 0 )
		{
			m_pWorld->AttachCollisionObject( this, false );
		}
	}
}

bool RigidBody::UpdateShapeIndex( void )
{
	int count = m_pShape->getNumChildShapes();
	int updateCount = 0;

	for( int i = 0; i < count; i++ )
	{
		btCollisionShape* pCompShape = m_pShape->getChildShape( i );

		for( RigidBody::ShapeInfoMap::iterator it = m_ShapeInfoMap.begin(); it != m_ShapeInfoMap.end(); ++it )
		{
			RigidBody::SHAPE_INFO* pInfo = &( it->second );

			if( pCompShape == pInfo->pCollisionShape )
			{
				pInfo->index = i;
				updateCount++;
				break;
			}
		}
	}

	return ( updateCount == count );
}

void RigidBody::UpdateInertiaTensor( float mass )
{
	btVector3 localInertia( 0.0f, 0.0f, 0.0f );

	if( mass > SIMD_EPSILON )
	{
		btCollisionShape* pCollisionShape = m_pRigidBody->getCollisionShape();

		if( pCollisionShape != NULL )
		{
			pCollisionShape->calculateLocalInertia( mass, localInertia );
		}
		else
		{
			mass = 0.0f;
		}
	}
	else
	{
		mass = 0.0f;
	}

	m_pRigidBody->setMassProps( mass, localInertia );
	m_pRigidBody->updateInertiaTensor();
}

void RigidBody::UpdateInertiaTensor( void )
{
	UpdateInertiaTensor( ( m_pRigidBody->isStaticOrKinematicObject() == true )? 0.0f : m_Mass );
}

void RigidBody::UpdateStatus( void )
{
	if( m_pRigidBody->hasContactResponse() == true )
	{
		if( m_pRigidBody->isKinematicObject() == true )
		{
			//キネマティックは質量が 0 なので、スタティックより前にチェックしておかないと
			//ステータスがスタティックになってしまう
			m_Status = RigidBody::KINEMATIC;
		}
		else if( m_pRigidBody->isStaticObject() == true )
		{
			m_Status = RigidBody::STATIC;
		}
		else
		{
			m_Status = RigidBody::DEFAULT;
		}
	}
	else
	{
		m_Status = RigidBody::SENSOR;
	}
}

HANDLE RigidBody::AddShape( Mix::Tool::Win32::Dynamics::Shape* pShape, const D3DXQUATERNION& rot, const D3DXVECTOR3& pos )
{
	if( pShape == NULL )
	{
		return false;
	}

	btCollisionShape* pCollisionShape = static_cast<btCollisionShape*>( pShape->Bullet_GetCollisionShape() );
	RigidBody::ShapeInfoMap::iterator it_si;

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

	BeginRefresh();

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

	btTransform csCenter(	btQuaternion( rot.x, rot.y, rot.z, rot.w ),
							btVector3( pos.x, pos.y, pos.z ) );

	int* srcHandle = new int;

	int index;
	HANDLE handle;

	/*
		シェイプ情報用に先に追加しておく
	*/

	m_pShape->addChildShape( csCenter, pCollisionShape );

	/*
		シェイプ情報を追加
	*/

	index = m_pShape->getNumChildShapes() - 1;
	handle = srcHandle;
	m_ShapeInfoMap.insert( RigidBody::ShapeInfoMap::value_type( handle, RigidBody::SHAPE_INFO( srcHandle, index, pShape, pCollisionShape ) ) );

	/*
		コリジョンシェイプ、リジッドボディの設定を変更
	*/

	it_si = m_ShapeInfoMap.find( handle );
	if( it_si != m_ShapeInfoMap.end() )
	{
		//シェイプハンドルを追加
		m_ShapeHandleList.push_back( handle );

		//慣性テンソルの更新
		UpdateInertiaTensor();
	}
	else
	{
		//失敗
		m_pShape->removeChildShape( pCollisionShape );
		pCollisionShape->setUserPointer( NULL );
		handle = NULL;
	}

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

	EndRefresh();

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

	return handle;
}

bool RigidBody::RemoveShape( const HANDLE handle )
{
	if( handle == NULL )
	{
		return false;
	}

	bool ret = true;

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

	BeginRefresh();

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

	RigidBody::ShapeInfoMap::iterator it_si = m_ShapeInfoMap.find( handle );
	if( it_si != m_ShapeInfoMap.end() )
	{
		btCollisionShape* pCollisionShape = it_si->second.pCollisionShape;
		int removeIndex = it_si->second.index;

		//ハンドルリストから外す
		RigidBody::ShapeHandleList::iterator it_sh = std::find( m_ShapeHandleList.begin(), m_ShapeHandleList.end(), handle );
		if( it_sh != m_ShapeHandleList.end() )
		{
			m_ShapeHandleList.erase( it_sh );
		}

		//ハイライトの無効化
		if( m_hHighlightShape == handle )
		{
			m_hHighlightShape = INVALID_HANDLE_VALUE;
			m_pHighlightShape = NULL;
		}

		//ハンドルの無効化
		MIX_DELETE( it_si->second.handle );

		//シェイプを削除
		m_ShapeInfoMap.erase( it_si );
		m_pShape->removeChildShapeByIndex( removeIndex );
		pCollisionShape->setUserPointer( NULL );

		//シェイプのインデックスを更新
		UpdateShapeIndex();

		//慣性テンソルの更新
		UpdateInertiaTensor();
	}
	else
	{
		ret = false;
	}

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

	EndRefresh();

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

	return ret;
}

Mix::Tool::Win32::Dynamics::Shape* RigidBody::GetShape( const HANDLE handle ) const
{
	if( handle == NULL )
	{
		return NULL;
	}

	RigidBody::ShapeInfoMap::const_iterator it_si = m_ShapeInfoMap.find( handle );
	if( it_si == m_ShapeInfoMap.end() )
	{
		return NULL;
	}

	return it_si->second.pShape;
}

bool RigidBody::SetShape( const HANDLE handle, Mix::Tool::Win32::Dynamics::Shape* pShape )
{
	if( ( handle == NULL ) ||
		( pShape == NULL ) )
	{
		return false;
	}

	RigidBody::ShapeInfoMap::iterator it_si = m_ShapeInfoMap.find( handle );
	if( it_si == m_ShapeInfoMap.end() )
	{
		return false;
	}

	RigidBody::SHAPE_INFO* pInfo = &( it_si->second );
	if( pInfo->pShape != pShape )
	{
		btCollisionShape* pCollisionShape = static_cast<btCollisionShape*>( pShape->Bullet_GetCollisionShape() );
		btTransform localTransform = m_pShape->getChildTransform( pInfo->index );

		/*
			一旦削除
		*/

		m_pShape->removeChildShapeByIndex( pInfo->index );

		/*
			追加しなおす
		*/

		m_pShape->addChildShape( localTransform, pCollisionShape );

		pInfo->index = m_pShape->getNumChildShapes() - 1;
		pInfo->pShape = pShape;
		pInfo->pCollisionShape = pCollisionShape;

		/*
			ハイライトの設定
		*/

		if( m_hHighlightShape == handle )
		{
			m_pHighlightShape = pCollisionShape;
		}

		/*
			インデックスを更新
		*/

		UpdateShapeIndex();
	}

	return true;
}

bool RigidBody::SetShapeTransform( const HANDLE handle, const D3DXQUATERNION& rot, const D3DXVECTOR3& pos )
{
	if( ( m_pShape == NULL ) ||
		( handle == NULL ) )
	{
		return false;
	}

	RigidBody::ShapeInfoMap::iterator it_si = m_ShapeInfoMap.find( handle );
	if( it_si != m_ShapeInfoMap.end() )
	{
		const RigidBody::SHAPE_INFO* pInfo = &( it_si->second );
		btTransform transform;

		transform.setRotation( btQuaternion( rot.x, rot.y, rot.z, rot.w ) );
		transform.setOrigin( btVector3( pos.x, pos.y, pos.z ) );

		m_pShape->updateChildTransform( pInfo->index, transform );

		UpdateInertiaTensor();
	}
	else
	{
		return false;
	}

	return true;
}

bool RigidBody::GetShapeRotation( const HANDLE handle, D3DXQUATERNION& rot ) const
{
	RigidBody::ShapeInfoMap::const_iterator it_si = m_ShapeInfoMap.find( handle );
	if( it_si != m_ShapeInfoMap.end() )
	{
		const btTransform& transform = m_pShape->getChildTransform( it_si->second.index );
		btQuaternion quat = transform.getRotation();

		rot.x = quat.x();
		rot.y = quat.y();
		rot.z = quat.z();
		rot.w = quat.w();
	}
	else
	{
		return false;
	}

	return true;
}

bool RigidBody::SetShapeRotation( const HANDLE handle, const D3DXQUATERNION& rot )
{
	if( ( m_pShape == NULL ) ||
		( handle == NULL ) )
	{
		return false;
	}

	RigidBody::ShapeInfoMap::iterator it_si = m_ShapeInfoMap.find( handle );
	if( it_si != m_ShapeInfoMap.end() )
	{
		const RigidBody::SHAPE_INFO* pInfo = &( it_si->second );
		btTransform transform = m_pShape->getChildTransform( pInfo->index );

		transform.setRotation( btQuaternion( rot.x, rot.y, rot.z, rot.w ) );
		m_pShape->updateChildTransform( pInfo->index, transform );

		UpdateInertiaTensor();
	}
	else
	{
		return false;
	}

	return true;
}

bool RigidBody::GetShapePosition( const HANDLE handle, D3DXVECTOR3& pos ) const
{
	if( ( m_pShape == NULL ) ||
		( handle == NULL ) )
	{
		return false;
	}

	RigidBody::ShapeInfoMap::const_iterator it_si = m_ShapeInfoMap.find( handle );
	if( it_si != m_ShapeInfoMap.end() )
	{
		const btTransform& transform = m_pShape->getChildTransform( it_si->second.index );
		const btVector3& vec = transform.getOrigin();

		pos.x = vec.x();
		pos.y = vec.y();
		pos.z = vec.z();
	}
	else
	{
		return false;
	}

	return true;
}

bool RigidBody::SetShapePosition( const HANDLE handle, const D3DXVECTOR3& pos )
{
	if( ( m_pShape == NULL ) ||
		( handle == NULL ) )
	{
		return false;
	}

	RigidBody::ShapeInfoMap::iterator it_si = m_ShapeInfoMap.find( handle );
	if( it_si != m_ShapeInfoMap.end() )
	{
		const RigidBody::SHAPE_INFO* pInfo = &( it_si->second );
		btTransform transform = m_pShape->getChildTransform( pInfo->index );

		transform.setOrigin( btVector3( pos.x, pos.y, pos.z ) );
		m_pShape->updateChildTransform( pInfo->index, transform );

		UpdateInertiaTensor();
	}
	else
	{
		return false;
	}

	return true;
}

void RigidBody::SetHighlightShape( const HANDLE handle )
{
	ShapeInfoMap::iterator it = m_ShapeInfoMap.find( handle );

	if( it != m_ShapeInfoMap.end() )
	{
		m_hHighlightShape = handle;
		m_pHighlightShape = it->second.pCollisionShape;
	}
	else
	{
		m_hHighlightShape = INVALID_HANDLE_VALUE;
		m_pHighlightShape = NULL;
	}
}

const HANDLE RigidBody::GetHighlightShapeHandle( void ) const
{
	return m_hHighlightShape;
}

int RigidBody::GetShapeHandleCount( void ) const
{
	return m_ShapeHandleList.size();
}

const HANDLE RigidBody::GetShapeHandle( int index ) const
{
	if( ( index < 0 ) ||
		( static_cast<int>( m_ShapeHandleList.size() ) <= index ) )
	{
		return NULL;
	}

	return m_ShapeHandleList[index];
}

float RigidBody::GetMass( void ) const
{
	return m_Mass;
}

float RigidBody::GetInvMass( void ) const
{
	return m_InvMass;
}

void RigidBody::SetMass( float mass )
{
	m_Mass = ( mass > FLT_EPSILON )? mass : 0.0f;
	m_InvMass = MIX_FLOAT_DIV( 1.0f, m_Mass );

	UpdateInertiaTensor();
	UpdateStatus();
}

const Mix::Tool::Win32::Dynamics::MATERIAL& RigidBody::GetMaterial( void ) const
{
	return m_Material;
}

void RigidBody::SetMaterial( const Mix::Tool::Win32::Dynamics::MATERIAL& material )
{
	m_Material = material;

	m_pRigidBody->setFriction( m_Material.friction );
	m_pRigidBody->setRestitution( m_Material.restitution );
}

D3DXVECTOR3 RigidBody::GetLinearVelocity( void )
{
	const btVector3& vel = m_pRigidBody->getLinearVelocity();

	return D3DXVECTOR3( vel.x(), vel.y(), vel.z() );
}

void RigidBody::SetLinearVelocity( const D3DXVECTOR3& vel )
{
	m_pRigidBody->setLinearVelocity( btVector3( vel.x, vel.y, vel.z ) );
}

D3DXVECTOR3 RigidBody::GetAngularVelocity( void )
{
	const btVector3& vel = m_pRigidBody->getAngularVelocity();

	return D3DXVECTOR3( vel.x(), vel.y(), vel.z() );
}

void RigidBody::SetAngularVelocity( const D3DXVECTOR3& vel )
{
	m_pRigidBody->setAngularVelocity( btVector3( vel.x, vel.y, vel.z ) );
}

D3DXVECTOR3 RigidBody::GetLinearFactor( void )
{
	const btVector3& factor = m_pRigidBody->getLinearFactor();
	return D3DXVECTOR3( factor.x(), factor.y(), factor.z() );
}

void RigidBody::SetLinearFactor( const D3DXVECTOR3& factor )
{
	m_pRigidBody->setLinearFactor( btVector3( factor.x, factor.y, factor.z ) );
}

D3DXVECTOR3 RigidBody::GetAngularFactor( void )
{
	const btVector3& factor = m_pRigidBody->getAngularFactor();
	return D3DXVECTOR3( factor.x(), factor.y(), factor.z() );
}

void RigidBody::SetAngularFactor( const D3DXVECTOR3& factor )
{
	m_pRigidBody->setAngularFactor( btVector3( factor.x, factor.y, factor.z ) );
}

float RigidBody::GetLinearDamping( void ) const
{
	return m_pRigidBody->getLinearDamping();
}

void RigidBody::SetLinearDamping( float damping )
{
	m_pRigidBody->setDamping( damping, m_pRigidBody->getAngularDamping() );
}

float RigidBody::GetAngularDamping( void ) const
{
	return m_pRigidBody->getAngularDamping();
}

void RigidBody::SetAngularDamping( float damping )
{
	m_pRigidBody->setDamping( m_pRigidBody->getLinearDamping(), damping );
}

void RigidBody::SetPick( bool state )
{
	if( m_pRigidBody->isStaticOrKinematicObject() == false )
	{
		m_bPick = state;

		if( m_bPick == true )
		{
			m_pRigidBody->forceActivationState( DISABLE_DEACTIVATION );
		}
		else
		{
			m_pRigidBody->forceActivationState( ( m_bAlwaysActive == true )? DISABLE_DEACTIVATION : ACTIVE_TAG );
		}
	}
}

bool RigidBody::GetAlwaysActive( void ) const
{
	return m_bAlwaysActive;
}

void RigidBody::SetAlwaysActive( bool state )
{
	if( m_pRigidBody->isStaticOrKinematicObject() == false )
	{
		if( m_bPick == false )
		{
			m_pRigidBody->forceActivationState( ( state == true )? DISABLE_DEACTIVATION : ACTIVE_TAG );
		}

		m_bAlwaysActive = state;
	}
}

RigidBody::STATUS RigidBody::GetStatus( void ) const
{
	return m_Status;
}

void RigidBody::SetStatus( RigidBody::STATUS status )
{
	if( m_Status == status )
	{
		return;
	}

	int flags = m_pRigidBody->getCollisionFlags();
	float mass = 0.0f;
	D3DXVECTOR3 pos = GetWorldPosition();
	D3DXQUATERNION rot = GetWorldRotation();

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// リフレッシュ開始
	// キネマティック、スタティックからデフォルトに変更した場合、一回ワールドから外して、
	// 入れなおしてあげないと、グラビティアクセラレーションが設定されない
	////////////////////////////////////////////////////////////////////////////////////////////////////

	BeginRefresh();

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 直前のステータスを解除
	////////////////////////////////////////////////////////////////////////////////////////////////////

	switch( m_Status )
	{
	case RigidBody::KINEMATIC:
		MIX_RESET_BIT( flags, btCollisionObject::CF_KINEMATIC_OBJECT );
		m_pRigidBody->setCollisionFlags( flags );
		m_pRigidBody->forceActivationState( ACTIVE_TAG );
		break;

	case RigidBody::SENSOR:
		MIX_RESET_BIT( flags, btCollisionObject::CF_NO_CONTACT_RESPONSE );
		m_pRigidBody->setCollisionFlags( flags );
		break;
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 新しいステータスを設定
	////////////////////////////////////////////////////////////////////////////////////////////////////

	switch( status )
	{
	case RigidBody::DEFAULT:
		mass = m_Mass;
		break;

	case RigidBody::KINEMATIC:
		MIX_SET_BIT( flags, btCollisionObject::CF_KINEMATIC_OBJECT );
		m_pRigidBody->setCollisionFlags( flags );
		m_pRigidBody->setActivationState( DISABLE_DEACTIVATION );
		break;

	case RigidBody::SENSOR:
		MIX_SET_BIT( flags, btCollisionObject::CF_NO_CONTACT_RESPONSE );
		m_pRigidBody->setCollisionFlags( flags );
		break;
	}

	SetWorldPosition( pos );
	SetWorldRotation( rot );

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 慣性テンソルの更新
	////////////////////////////////////////////////////////////////////////////////////////////////////

	UpdateInertiaTensor( mass );

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// ステータスの更新
	////////////////////////////////////////////////////////////////////////////////////////////////////

	UpdateStatus();

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// リフレッシュ終了
	////////////////////////////////////////////////////////////////////////////////////////////////////

	EndRefresh();
}

void RigidBody::Reset( void )
{
	btTransform tr = m_pRigidBody->getCenterOfMassTransform();

	m_pRigidBody->setLinearVelocity( btVector3( 0.0f, 0.0f, 0.0f ) );
	m_pRigidBody->setAngularVelocity( btVector3( 0.0f, 0.0f, 0.0f ) );
	m_pRigidBody->setCenterOfMassTransform( tr );
	m_pRigidBody->clearForces();
	m_pRigidBody->activate( true );
}

void RigidBody::Reset( const D3DXQUATERNION& rot, const D3DXVECTOR3& pos )
{
	btTransform tr( btQuaternion( rot.x, rot.y, rot.z, rot.w ), btVector3( pos.x, pos.y, pos.z ) );

	m_pRigidBody->setLinearVelocity( btVector3( 0.0f, 0.0f, 0.0f ) );
	m_pRigidBody->setAngularVelocity( btVector3( 0.0f, 0.0f, 0.0f ) );
	m_pRigidBody->setCenterOfMassTransform( tr );
	m_pRigidBody->clearForces();
	m_pRigidBody->activate( true );
}

void RigidBody::Activate( void )
{
	m_pRigidBody->activate();
}

void RigidBody::Deactivate( void )
{
	if( m_pRigidBody->isStaticOrKinematicObject() == false )
	{
		m_pRigidBody->setActivationState( WANTS_DEACTIVATION );
	}
}

void RigidBody::ApplyImpulse( const D3DXVECTOR3& impulse )
{
	m_pRigidBody->applyCentralImpulse( btVector3( impulse.x, impulse.y, impulse.z ) );
}

void RigidBody::ApplyImpulse( const D3DXVECTOR3& impulse, const D3DXVECTOR3& relPos )
{
	m_pRigidBody->applyImpulse( btVector3( impulse.x, impulse.y, impulse.z ), btVector3( relPos.x, relPos.y, relPos.z ) );
}

void RigidBody::Draw( Mix::Tool::Win32::Graphics::LineHelper* pLineHelper, const D3DXVECTOR4& defColor, const D3DXVECTOR4& selColor )
{
	if( ( m_pShape == NULL ) ||
		( m_pRigidBody == NULL ) )
	{
		return;
	}

	D3DXQUATERNION worldRot = GetWorldRotation();
	D3DXVECTOR3 worldPos = GetWorldPosition();
	D3DXMATRIX worldMat;

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

	D3DXMatrixIdentity( &worldMat );
	D3DXMatrixRotationQuaternion( &worldMat, &worldRot );
	worldMat._41 = worldPos.x; worldMat._42 = worldPos.y; worldMat._43 = worldPos.z; worldMat._44 = 1.0f;

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 描画
	////////////////////////////////////////////////////////////////////////////////////////////////////

	int shapeCount = m_pShape->getNumChildShapes();

	for( int i = 0; i < shapeCount; i++ )
	{
		const btCollisionShape* pCollisionShape = m_pShape->getChildShape( i );
		const btTransform& localTransform = m_pShape->getChildTransform( i );

		int type = pCollisionShape->getShapeType();
		btQuaternion localRot = localTransform.getRotation();
		btVector3 localPos = localTransform.getOrigin();

		D3DXMATRIX localMat;

		D3DXMatrixIdentity( &localMat );
		D3DXMatrixRotationQuaternion( &localMat, &D3DXQUATERNION( localRot.x(), localRot.y(), localRot.z(), localRot.w() ) );
		localMat._41 = localPos.x(); localMat._42 = localPos.y(); localMat._43 = localPos.z(); localMat._44 = 1.0f;

		pLineHelper->SetColor( ( m_pHighlightShape == pCollisionShape )? selColor : defColor );
		pLineHelper->SetMatrix( localMat * worldMat );

		if( type == BOX_SHAPE_PROXYTYPE )
		{
			const btBoxShape* pBoxShape = static_cast<const btBoxShape*>( pCollisionShape );
			const btVector3& halfExtents = pBoxShape->getHalfExtentsWithoutMargin();

			pLineHelper->AddBox( D3DXVECTOR3( halfExtents.x(), halfExtents.y(), halfExtents.z() ) );
		}
		else if( type == CAPSULE_SHAPE_PROXYTYPE )
		{
			const btCapsuleShape* pCapsuleShape = static_cast<const btCapsuleShape*>( pCollisionShape );
			Mix::Tool::Win32::Geometry::CAPSULE capsule;

			switch( pCapsuleShape->getUpAxis() )
			{
			case 0:
				capsule.axisType = Mix::Tool::Win32::Geometry::CAPSULE::AXIS_X;
				break;
			case 1:
				capsule.axisType = Mix::Tool::Win32::Geometry::CAPSULE::AXIS_Y;
				break;
			case 2:
				capsule.axisType = Mix::Tool::Win32::Geometry::CAPSULE::AXIS_Z;
				break;
			}

			capsule.pos = D3DXVECTOR3( 0.0f, 0.0f, 0.0f );
			capsule.halfLen = pCapsuleShape->getHalfHeight();
			capsule.radius = pCapsuleShape->getRadius();

			pLineHelper->AddCapsule( capsule );
		}
		else if( type == SPHERE_SHAPE_PROXYTYPE )
		{
			const btSphereShape* pSphereShape = static_cast<const btSphereShape*>( pCollisionShape );
			Mix::Tool::Win32::Geometry::SPHERE sphere;

			sphere.pos = D3DXVECTOR3( 0.0f, 0.0f, 0.0f );
			sphere.radius = pSphereShape->getRadius();

			pLineHelper->AddSphere( sphere );
		}
	}
}

short RigidBody::GetGroup( void ) const
{
	return m_Group;
}

void RigidBody::SetGroup( short data )
{
	BeginRefresh();
	m_Group = data;
	EndRefresh();
}

short RigidBody::GetGroupMask( void ) const
{
	return m_GroupMask;
}

void RigidBody::SetGroupMask( short data )
{
	BeginRefresh();
	m_GroupMask = data;
	EndRefresh();
}

D3DXQUATERNION RigidBody::GetWorldRotation( void ) const
{
	btTransform world;

	if( m_pRigidBody->isKinematicObject() == true )
	{
		m_pRigidBody->getMotionState()->getWorldTransform( world );
	}
	else
	{
		world = m_pRigidBody->getWorldTransform();
	}

	const btQuaternion& rot = world.getRotation();

	return D3DXQUATERNION( rot.x(), rot.y(), rot.z(), rot.w() );
}

void RigidBody::SetWorldRotation( const D3DXQUATERNION& rot )
{
	btTransform world;

	if( m_pRigidBody->isKinematicObject() == true )
	{
		m_pRigidBody->getMotionState()->getWorldTransform( world );
		world.setRotation( btQuaternion( rot.x, rot.y, rot.z, rot.w ) );
	}
	else
	{
		world = m_pRigidBody->getWorldTransform();
		world.setRotation( btQuaternion( rot.x, rot.y, rot.z, rot.w ) );
	}

	m_pRigidBody->setWorldTransform( world );
	m_pRigidBody->getMotionState()->setWorldTransform( world );
}

D3DXVECTOR3 RigidBody::GetWorldPosition( void ) const
{
	btTransform world;

	if( m_pRigidBody->isKinematicObject() == true )
	{
		m_pRigidBody->getMotionState()->getWorldTransform( world );
	}
	else
	{
		world = m_pRigidBody->getWorldTransform();
	}

	const btVector3& pos = world.getOrigin();

	return D3DXVECTOR3( pos.x(), pos.y(), pos.z() );
}

void RigidBody::SetWorldPosition( const D3DXVECTOR3& pos )
{
	btTransform world;

	if( m_pRigidBody->isKinematicObject() == true )
	{
		m_pRigidBody->getMotionState()->getWorldTransform( world );
		world.setOrigin( btVector3( pos.x, pos.y, pos.z ) );
	}
	else
	{
		world = m_pRigidBody->getWorldTransform();
		world.setOrigin( btVector3( pos.x, pos.y, pos.z ) );

	}

	m_pRigidBody->setWorldTransform( world );
	m_pRigidBody->getMotionState()->setWorldTransform( world );
}

D3DXMATRIX RigidBody::GetWorldMatrix( void ) const
{
	D3DXQUATERNION rot = GetWorldRotation();
	D3DXVECTOR3 pos = GetWorldPosition();
	D3DXMATRIX mat;

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

	return mat;
}

void RigidBody::SetWorldMatrix( const D3DXMATRIX& mat )
{
	D3DXQUATERNION rot;
	D3DXVECTOR3 pos;

	Mix::Tool::Win32::Matrix_Decompose( mat, NULL, &rot, &pos );

	SetWorldTransform( rot, pos );
}

void RigidBody::SetWorldTransform( const D3DXQUATERNION& rot, const D3DXVECTOR3& pos )
{
	btTransform world;

	world.setRotation( btQuaternion( rot.x, rot.y, rot.z, rot.w ) );
	world.setOrigin( btVector3( pos.x, pos.y, pos.z ) );

	m_pRigidBody->getMotionState()->setWorldTransform( world );
	m_pRigidBody->setWorldTransform( world );
}

bool RigidBody::IsInWorld( void ) const
{
	return ( m_pWorld != NULL );
}

void RigidBody::Draw( Mix::Tool::Win32::Graphics::LineHelper* pLineHelper, const D3DXVECTOR4& color )
{
	Draw( pLineHelper, color, color );
}

}}}}}
