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

#include "Mix/Tool/Win32/Core/Graphics/LineHelper.h"
#include "Mix/Tool/Win32/Core/Dynamics/Impl/Utility.h"
#include "Mix/Tool/Win32/Core/Dynamics/Impl/Types.h"
#include "Mix/Tool/Win32/Core/Dynamics/Impl/World.h"
#include "Mix/Tool/Win32/Core/Dynamics/Impl/RigidBody.h"

#include "btBulletDynamicsCommon.h"

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

const int BallJoint::DRAW_SWING_SEGMENTS = 8 * 4;
const float BallJoint::DRAW_TWIST_STEP_DEG = 10.0f;

BallJoint::BallJoint( void ) :
m_pConstraint( NULL ),
m_pWorld( NULL ),
m_pRigidBodyA( NULL ),
m_pRigidBodyB( NULL ),
m_bInitalRigidBodyTransform( false ),
m_InitalRigidBodyRotA( 0.0f, 0.0f, 0.0f, 1.0f ),
m_InitalRigidBodyPosA( 0.0f, 0.0f, 0.0f ),
m_InitalRigidBodyRotB( 0.0f, 0.0f, 0.0f, 1.0f ),
m_InitalRigidBodyPosB( 0.0f, 0.0f, 0.0f ),
m_PivotA( 0.0f, 0.0f, 0.0f ),
m_PivotB( 0.0f, 0.0f, 0.0f ),
m_bCollisionDisabled( false ),
m_SwingAxis( 0.0f, 1.0f, 0.0f ),
m_NormSwingAxis( 0.0f, 1.0f, 0.0f ),
m_TwistAxis( 0.0f, 1.0f, 0.0f ),
m_NormTwistAxis( 0.0f, 1.0f, 0.0f ),
m_SwingLimit1( 0.7f ),
m_SwingLimit2( 0.7f ),
m_TwistLimit( 0.5f ),
m_ParamFlags( 0 )
{
	for( int i = 0; i < BallJoint::PT_MAX; i++ )
	{
		for( int j = 0; j < BallJoint::PA_MAX; j++ )
		{
			m_ParamValues[i][j] = 0.0f;
		}
	}
}

BallJoint::BallJoint(	const D3DXVECTOR3& swingAxis,
						const D3DXVECTOR3& twistAxis,
						float swingLimit1,
						float swingLimit2,
						float twistLimit,
						bool bCollisionDisabled,
						float spring,
						float damper,
						float limitSpring,
						float limitDamper ) :
m_pConstraint( NULL ),
m_pWorld( NULL ),
m_pRigidBodyA( NULL ),
m_pRigidBodyB( NULL ),
m_bInitalRigidBodyTransform( false ),
m_InitalRigidBodyRotA( 0.0f, 0.0f, 0.0f, 1.0f ),
m_InitalRigidBodyPosA( 0.0f, 0.0f, 0.0f ),
m_InitalRigidBodyRotB( 0.0f, 0.0f, 0.0f, 1.0f ),
m_InitalRigidBodyPosB( 0.0f, 0.0f, 0.0f ),
m_PivotA( 0.0f, 0.0f, 0.0f ),
m_PivotB( 0.0f, 0.0f, 0.0f ),
m_bCollisionDisabled( bCollisionDisabled ),
m_SwingAxis( swingAxis ),
m_TwistAxis( twistAxis ),
m_SwingLimit1( swingLimit1 ),
m_SwingLimit2( swingLimit2 ),
m_TwistLimit( twistLimit ),
m_ParamFlags( 0 )
{
	D3DXVec3Normalize( &m_NormSwingAxis, &m_SwingAxis );
	D3DXVec3Normalize( &m_NormTwistAxis, &m_TwistAxis );

	for( int i = 0; i < BallJoint::PT_MAX; i++ )
	{
		for( int j = 0; j < BallJoint::PA_MAX; j++ )
		{
			m_ParamValues[i][j] = 0.0f;
		}
	}

	SetSpring( spring );
	SetDamper( damper );
	SetLimitSpring( limitSpring );
	SetLimitDamper( limitDamper );
}

BallJoint::~BallJoint( void )
{
	MIX_DELETE( m_pConstraint );
}

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

const D3DXVECTOR3& BallJoint::GetNormalizeSwingAxis( void ) const
{
	return m_NormSwingAxis;
}

const D3DXVECTOR3& BallJoint::GetSwingAxis( void ) const
{
	return m_SwingAxis;
}

void BallJoint::SetSwingAxis( const D3DXVECTOR3& axis )
{
	m_SwingAxis = axis;
	D3DXVec3Normalize( &m_NormSwingAxis, &m_SwingAxis );

	UpdateFrames();
}

const D3DXVECTOR3& BallJoint::GetNormalizeTwistAxis( void ) const
{
	return m_NormTwistAxis;
}

const D3DXVECTOR3& BallJoint::GetTwistAxis( void ) const
{
	return m_TwistAxis;
}

void BallJoint::SetTwistAxis( const D3DXVECTOR3& axis )
{
	m_TwistAxis = axis;
	D3DXVec3Normalize( &m_NormTwistAxis, &m_TwistAxis );

	UpdateFrames();
}

float BallJoint::GetSwingLimit1( void ) const
{
	return m_SwingLimit1;
}

void BallJoint::SetSwingLimit1( float rad )
{
	m_SwingLimit1 = max( 0.1f, rad );

	UpdateLimit();
}

float BallJoint::GetSwingLimit2( void ) const
{
	return m_SwingLimit2;
}

void BallJoint::SetSwingLimit2( float rad )
{
	m_SwingLimit2 = max( 0.1f, rad );

	UpdateLimit();
}

float BallJoint::GetTwistLimit( void ) const
{
	return m_TwistLimit;
}

void BallJoint::SetTwistLimit( float rad )
{
	m_TwistLimit = max( 0.1f, rad );

	UpdateLimit();
}

float BallJoint::GetSpring( void ) const
{
	return GetParam( BallJoint::PT_CFM, BallJoint::PA_TRANSLATION );
}

void BallJoint::SetSpring( float value )
{
	if( value > SIMD_EPSILON )
	{
		SetParam( BallJoint::PT_CFM, BallJoint::PA_TRANSLATION, ( value > ( 1.0f - SIMD_EPSILON ) )? 1.0f : value );
	}
	else
	{
		ResetParam( BallJoint::PT_CFM, BallJoint::PA_TRANSLATION );
	}
}

float BallJoint::GetDamper( void ) const
{
	return GetParam( BallJoint::PT_ERP, BallJoint::PA_TRANSLATION );
}

void BallJoint::SetDamper( float value )
{
	if( value > SIMD_EPSILON )
	{
		SetParam( BallJoint::PT_ERP, BallJoint::PA_TRANSLATION, ( value > ( 1.0f - SIMD_EPSILON ) )? 1.0f : value );
	}
	else
	{
		ResetParam( BallJoint::PT_ERP, BallJoint::PA_TRANSLATION );
	}
}

float BallJoint::GetLimitSpring( void ) const
{
	return GetParam( BallJoint::PT_CFM, BallJoint::PA_ROTATION );
}

void BallJoint::SetLimitSpring( float value )
{
	if( value > SIMD_EPSILON )
	{
		SetParam( BallJoint::PT_CFM, BallJoint::PA_ROTATION, ( value > ( 1.0f - SIMD_EPSILON ) )? 1.0f : value );
	}
	else
	{
		ResetParam( BallJoint::PT_CFM, BallJoint::PA_ROTATION );
	}
}

float BallJoint::GetLimitDamper( void ) const
{
	return GetParam( BallJoint::PT_ERP, BallJoint::PA_ROTATION );
}

void BallJoint::SetLimitDamper( float value )
{
	if( value > SIMD_EPSILON )
	{
		SetParam( BallJoint::PT_ERP, BallJoint::PA_ROTATION, ( value > ( 1.0f - SIMD_EPSILON ) )? 1.0f : value );
	}
	else
	{
		ResetParam( BallJoint::PT_ERP, BallJoint::PA_ROTATION );
	}
}

void BallJoint::ResetInitalRigidBodyTransform( void )
{
	m_bInitalRigidBodyTransform = false;

	D3DXQuaternionIdentity( &m_InitalRigidBodyRotA );
	m_InitalRigidBodyPosA = D3DXVECTOR3( 0.0f, 0.0f, 0.0f );

	D3DXQuaternionIdentity( &m_InitalRigidBodyRotB );
	m_InitalRigidBodyPosB = D3DXVECTOR3( 0.0f, 0.0f, 0.0f );
}

void BallJoint::SetInitalRigidBodyTransform(	const D3DXQUATERNION& rotA,
												const D3DXVECTOR3& posA,
												const D3DXQUATERNION& rotB,
												const D3DXVECTOR3& posB )
{
	m_bInitalRigidBodyTransform = true;

	m_InitalRigidBodyRotA = rotA;
	m_InitalRigidBodyPosA = posA;

	m_InitalRigidBodyRotB = rotB;
	m_InitalRigidBodyPosB = posB;
}

bool BallJoint::SetFrame(	Mix::Tool::Win32::Dynamics::RigidBody* pRigidBodyA,
							const D3DXVECTOR3& pivotA )
{
	bool ret = false;

	//////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// 作成処理
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////

	BeginRefresh();

	MIX_DELETE( m_pConstraint );
	m_pRigidBodyA = NULL;
	m_pRigidBodyB = NULL;

	if( ( pRigidBodyA != NULL ) &&
		( pRigidBodyA->GetShapeHandleCount() > 0 ) )
	{
		RigidBody::STATUS statusA = pRigidBodyA->GetStatus();

		if( ( statusA == RigidBody::DEFAULT ) ||
			( statusA == RigidBody::KINEMATIC ) )
		{
			Mix::Tool::Win32::Dynamics::Impl::RigidBody* pImplRigidBodyA = static_cast<Mix::Tool::Win32::Dynamics::Impl::RigidBody*>( pRigidBodyA );
			btRigidBody* pBulletRigidBodyA = pImplRigidBodyA->Bullet_GetRigidBodyPtr();

			if( pBulletRigidBodyA != NULL )
			{
				btTransform frameA;
				btTransform frameB;

				Bullet_ComputeFrames(	pBulletRigidBodyA,
										&btVector3( pivotA.x, pivotA.y, pivotA.z ),
										&btVector3( m_NormSwingAxis.x, m_NormSwingAxis.y, m_NormSwingAxis.z ),
										&btVector3( m_NormTwistAxis.x, m_NormTwistAxis.y, m_NormTwistAxis.z ),
										&frameA,
										&frameB );

				m_pConstraint = new btConeTwistConstraint( *pBulletRigidBodyA, btTypedConstraint::getFixedBody(), frameA, frameB );
				if( m_pConstraint != NULL )
				{
					const btVector3& pivotB = frameB.getOrigin();

					m_pConstraint->buildJacobian();

					m_pRigidBodyA = pRigidBodyA;
					m_pRigidBodyB = NULL;

					m_PivotA = pivotA;
					m_PivotB = D3DXVECTOR3( pivotB.x(), pivotB.y(), pivotB.z() );

					ret = true;
				}
			}
		}
	}

	UpdateLimit();
	UpdateParams();

	EndRefresh();

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

	return ret;
}


bool BallJoint::SetFrame(	Mix::Tool::Win32::Dynamics::RigidBody* pRigidBodyA,
							Mix::Tool::Win32::Dynamics::RigidBody* pRigidBodyB,
							const D3DXVECTOR3& pivotA,
							const D3DXVECTOR3& pivotB )
{
	bool ret = false;

	//////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// 作成処理
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////

	BeginRefresh();

	MIX_DELETE( m_pConstraint );
	m_pRigidBodyA = NULL;
	m_pRigidBodyB = NULL;

	if( ( pRigidBodyA != NULL ) &&
		( pRigidBodyB != NULL ) &&
		( pRigidBodyA->GetShapeHandleCount() > 0 ) &&
		( pRigidBodyB->GetShapeHandleCount() > 0 ) )
	{
		Mix::Tool::Win32::Dynamics::RigidBody::STATUS statusA = pRigidBodyA->GetStatus();
		Mix::Tool::Win32::Dynamics::RigidBody::STATUS statusB = pRigidBodyB->GetStatus();

		if( ( statusA == Mix::Tool::Win32::Dynamics::RigidBody::DEFAULT ) ||
			( statusB == Mix::Tool::Win32::Dynamics::RigidBody::DEFAULT ) ||
			( statusA == Mix::Tool::Win32::Dynamics::RigidBody::KINEMATIC ) ||
			( statusB == Mix::Tool::Win32::Dynamics::RigidBody::KINEMATIC ) )
		{
			Mix::Tool::Win32::Dynamics::Impl::RigidBody* pImplRigidBodyA = static_cast<Mix::Tool::Win32::Dynamics::Impl::RigidBody*>( pRigidBodyA );
			Mix::Tool::Win32::Dynamics::Impl::RigidBody* pImplRigidBodyB = static_cast<Mix::Tool::Win32::Dynamics::Impl::RigidBody*>( pRigidBodyB );
			btRigidBody* pBulletRigidBodyA = pImplRigidBodyA->Bullet_GetRigidBodyPtr();
			btRigidBody* pBulletRigidBodyB = pImplRigidBodyB->Bullet_GetRigidBodyPtr();

			if( ( pBulletRigidBodyA != NULL ) &&
				( pBulletRigidBodyB != NULL ) )
			{
				btTransform frameA;
				btTransform frameB;

				Bullet_ComputeFrames(	pBulletRigidBodyA,
										pBulletRigidBodyB,
										&btVector3( pivotA.x, pivotA.y, pivotA.z ),
										&btVector3( pivotB.x, pivotB.y, pivotB.z ),
										&btVector3( m_NormSwingAxis.x, m_NormSwingAxis.y, m_NormSwingAxis.z ),
										&btVector3( m_NormTwistAxis.x, m_NormTwistAxis.y, m_NormTwistAxis.z ),
										&frameA,
										&frameB );

				m_pConstraint = new btConeTwistConstraint(	*pBulletRigidBodyA,
															*pBulletRigidBodyB,
															frameA,
															frameB );
				if( m_pConstraint != NULL )
				{
					m_pConstraint->buildJacobian();

					m_pRigidBodyA = pRigidBodyA;
					m_pRigidBodyB = pRigidBodyB;

					m_PivotA = pivotA;
					m_PivotB = pivotB;

					ret = true;
				}
			}
		}
	}

	UpdateLimit();
	UpdateParams();

	EndRefresh();

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

	return ret;
}

const Mix::Tool::Win32::Dynamics::RigidBody* BallJoint::GetRigidBodyA( void ) const
{
	return m_pRigidBodyA;
}

const Mix::Tool::Win32::Dynamics::RigidBody* BallJoint::GetRigidBodyB( void ) const
{
	return m_pRigidBodyB;
}

D3DXVECTOR3 BallJoint::GetPivotA( void )
{
	return m_PivotA;
}

void BallJoint::SetPivotA( const D3DXVECTOR3& pivot )
{
	m_PivotA = pivot;

	UpdatePivot();
}

D3DXVECTOR3 BallJoint::GetPivotB( void )
{
	return m_PivotB;
}

void BallJoint::SetPivotB( const D3DXVECTOR3& pivot )
{
	m_PivotB = pivot;

	UpdatePivot();
}

bool BallJoint::GetCollisionDisabled( void ) const
{
	return m_bCollisionDisabled;
}

void BallJoint::SetCollisionDisabled( bool state )
{
	m_bCollisionDisabled = state;

	Refresh();

	if( m_pRigidBodyA != NULL ) { m_pRigidBodyA->Activate(); }
	if( m_pRigidBodyB != NULL ) { m_pRigidBodyB->Activate(); }
}

bool BallJoint::IsParamSupported( int type, int axis ) const
{
	if( ( type == BallJoint::PT_ERP ) &&
		( ( axis == BallJoint::PA_TRANSLATION ) || ( axis == BallJoint::PA_ROTATION ) ) )
	{
		return true;
	}

	if( ( type == BallJoint::PT_CFM ) &&
		( ( axis == BallJoint::PA_TRANSLATION ) || ( axis == BallJoint::PA_ROTATION ) ) )
	{
		return true;
	}

	return false;
}

bool BallJoint::IsParamEnabled( int type, int axis ) const
{
	if( IsParamSupported( type, axis ) == false )
	{
		return false;
	}

	unsigned int bit = GetParamBit( type, axis );

	return ( MIX_TEST_BIT( m_ParamFlags, bit ) == bit );
}

float BallJoint::GetParam( int type, int axis ) const
{
	if( IsParamSupported( type, axis ) == false )
	{
		return 0.0f;
	}

	return m_ParamValues[type][axis];
}

void BallJoint::SetParam( int type, int axis, float value )
{
	if( IsParamSupported( type, axis ) == true )
	{
		MIX_SET_BIT( m_ParamFlags, GetParamBit( type, axis ) );
		m_ParamValues[type][axis] = value;

		if( m_pConstraint != NULL )
		{
			m_pConstraint->setParam( type, value, axis );
		}
	}
}

void BallJoint::ResetParam( int type, int axis )
{
	if( IsParamSupported( type, axis ) == true )
	{
		unsigned int bit = GetParamBit( type, axis );

		if( MIX_TEST_BIT( m_ParamFlags, bit ) == bit )
		{
			MIX_RESET_BIT( m_ParamFlags, bit );
			m_ParamValues[type][axis] = 0.0f;

			if( m_pConstraint != NULL )
			{
				if( ( m_pRigidBodyA != NULL ) &&
					( m_pRigidBodyB == NULL ) )
				{
					SetFrame( m_pRigidBodyA, m_PivotA );
				}
				else if(	( m_pRigidBodyA != NULL ) &&
							( m_pRigidBodyB != NULL ) )
				{
					SetFrame( m_pRigidBodyA, m_pRigidBodyB, m_PivotA, m_PivotB );
				}
			}
		}
	}
}

void BallJoint::Draw(	Mix::Tool::Win32::Graphics::LineHelper* pLineHelper,
						const D3DXVECTOR4& axisColor,
						const D3DXVECTOR4& ballColor,
						const D3DXVECTOR4& limitColor,
						float scale )
{
	if( ( pLineHelper == NULL ) ||
		( m_pConstraint == NULL ) )
	{
		return;
	}

	const btTransform& frameA = m_pConstraint->getAFrame();
	const btTransform& frameB = m_pConstraint->getBFrame();
	const btVector3& pivotA = frameA.getOrigin();
	const btVector3& pivotB = frameB.getOrigin();
	const btRigidBody* pRigidBodyA = &( m_pConstraint->getRigidBodyA() );
	const btRigidBody* pRigidBodyB = &( m_pConstraint->getRigidBodyB() );

	D3DXVECTOR3 pointA;
	D3DXVECTOR3 pointB;

	Mix::Tool::Win32::Geometry::SPHERE sphere;

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 初期化
	////////////////////////////////////////////////////////////////////////////////////////////////////

	pLineHelper->IdentityMatrix();

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// 軸
	////////////////////////////////////////////////////////////////////////////////////////////////////

	pLineHelper->SetColor( axisColor );

	if( pRigidBodyA != &( btTypedConstraint::getFixedBody() ) )
	{
		btTransform tr;
		btQuaternion rot;
		btVector3 pos;
		D3DXMATRIX mat;

		if( MIX_TEST_BIT( pRigidBodyA->getCollisionFlags(), btCollisionObject::CF_KINEMATIC_OBJECT ) == btCollisionObject::CF_KINEMATIC_OBJECT )
		{
			pRigidBodyA->getMotionState()->getWorldTransform( tr );
		}
		else
		{
			tr = pRigidBodyA->getWorldTransform();
		}

		rot = tr.getRotation();
		pos = tr.getOrigin();

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

		D3DXVec3TransformCoord( &pointA, &D3DXVECTOR3( pivotA.x(), pivotA.y(), pivotA.z() ), &mat );

		pLineHelper->AddPoints( pointA, D3DXVECTOR3( pos.x(), pos.y(), pos.z() ) );
	}
	else
	{
		pointA = D3DXVECTOR3( pivotA.x(), pivotA.y(), pivotA.z() );
	}

	if( pRigidBodyB != &( btTypedConstraint::getFixedBody() ) )
	{
		btTransform tr;
		btQuaternion rot;
		btVector3 pos;
		D3DXMATRIX mat;
		D3DXVECTOR3 point;

		if( MIX_TEST_BIT( pRigidBodyB->getCollisionFlags(), btCollisionObject::CF_KINEMATIC_OBJECT ) == btCollisionObject::CF_KINEMATIC_OBJECT )
		{
			pRigidBodyB->getMotionState()->getWorldTransform( tr );
		}
		else
		{
			tr = pRigidBodyB->getWorldTransform();
		}

		rot = tr.getRotation();
		pos = tr.getOrigin();

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

		D3DXVec3TransformCoord( &pointB, &D3DXVECTOR3( pivotB.x(), pivotB.y(), pivotB.z() ), &mat );

		pLineHelper->AddPoints( pointB, D3DXVECTOR3( pos.x(), pos.y(), pos.z() ) );
	}
	else
	{
		pointB = D3DXVECTOR3( pivotB.x(), pivotB.y(), pivotB.z() );
	}

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// ボール
	////////////////////////////////////////////////////////////////////////////////////////////////////

	sphere.pos = ( pointA + pointB ) * 0.5f;
	sphere.radius = max( Mix::Tool::Win32::Dynamics::Impl::CONSTRAINT_MIN_JOINT_RADIUS, D3DXVec3Length( &( pointA - pointB ) ) * 0.5f );

	pLineHelper->SetColor( ballColor );
	pLineHelper->AddSphere( sphere );

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// リミット
	////////////////////////////////////////////////////////////////////////////////////////////////////

	btTransform	tr;

	pLineHelper->IdentityMatrix();
	pLineHelper->SetColor( limitColor );

	/*
		スィング
	*/

	float angleInRadians = btScalar( 2.0f * 3.1415926f ) * ( btScalar )( DRAW_SWING_SEGMENTS - 1 ) / btScalar( DRAW_SWING_SEGMENTS );
	btVector3 prev = m_pConstraint->GetPointForAngle( angleInRadians, scale );
	btVector3 cur;

	if( pRigidBodyB != &btTypedConstraint::getFixedBody() )
	{
		if( pRigidBodyB->isKinematicObject() == true )
		{
			btTransform mstr;
			pRigidBodyB->getMotionState()->getWorldTransform( mstr );
			tr = mstr * m_pConstraint->getBFrame();
		}
		else
		{
			tr = pRigidBodyB->getCenterOfMassTransform() * m_pConstraint->getBFrame();
		}
	}
	else
	{
		tr = pRigidBodyA->getCenterOfMassTransform() * m_pConstraint->getAFrame();
	}

	prev = tr * prev;

	for( int i = 0; i < DRAW_SWING_SEGMENTS; i++ )
	{
		angleInRadians = btScalar( 2.0f * 3.1415926f ) * ( btScalar )( i / btScalar( DRAW_SWING_SEGMENTS ) );
		cur = m_pConstraint->GetPointForAngle( angleInRadians, scale );

		cur = tr * cur;
		pLineHelper->AddPoints( D3DXVECTOR3( prev.x(), prev.y(), prev.z() ), D3DXVECTOR3( cur.x(), cur.y(), cur.z() ) );

		if( i % ( DRAW_SWING_SEGMENTS / 8 ) == 0 )
		{
			const btVector3& origin = tr.getOrigin();
			pLineHelper->AddPoints( D3DXVECTOR3( origin.x(), origin.y(), origin.z() ), D3DXVECTOR3( cur.x(), cur.y(), cur.z() ) );
		}

		prev = cur;
	}

	/*
		ツイスト
	*/

	btScalar tws = m_pConstraint->getTwistSpan();
	btScalar twa = m_pConstraint->getTwistAngle();
	float minAngle = -twa - tws;
	float maxAngle = -twa + tws;

	if( pRigidBodyA->isKinematicObject() == true )
	{
		btTransform mstr;
		pRigidBodyA->getMotionState()->getWorldTransform( mstr );
		tr = mstr * m_pConstraint->getAFrame();
	}
	else
	{
		tr = pRigidBodyA->getCenterOfMassTransform() * m_pConstraint->getAFrame();
	}

	if( minAngle != maxAngle )
	{
		const float step = DRAW_TWIST_STEP_DEG * SIMD_RADS_PER_DEG;

		float radiusA = scale;
		float radiusB = scale;

		D3DXVECTOR3 center = ToD3DXVector( tr.getOrigin() );
		D3DXVECTOR3 normal = ToD3DXVector( tr.getBasis().getColumn( 0 ) );
		D3DXVECTOR3 axis = ToD3DXVector( tr.getBasis().getColumn( 1 ) );
		bool bDrawSect = ( minAngle <= maxAngle );

		if( bDrawSect == false )
		{
			minAngle = 0.0f;
			maxAngle = SIMD_2_PI;
		}

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

		int stepCount = max( 1, static_cast<int>( ( maxAngle - minAngle ) / step ) );
		const D3DXVECTOR3& vx = axis;
		D3DXVECTOR3 vy;
		D3DXVECTOR3 prev;

		D3DXVec3Cross( &vy, &normal, &axis );
	
		prev = center + radiusA * vx * ::cosf( minAngle ) + radiusB * vy * ::sinf( minAngle );
		if( bDrawSect == true )
		{
			pLineHelper->AddPoints( center, prev );
		}

		for( int i = 1; i <= stepCount; i++ )
		{
			float angle = minAngle + ( maxAngle - minAngle ) * static_cast<float>( i ) / static_cast<float>( stepCount );
			D3DXVECTOR3 next = center + radiusA * vx * ::cosf( angle ) + radiusB * vy * ::sinf( angle );

			pLineHelper->AddPoints( prev, next );

			prev = next;
		}

		if( bDrawSect == true )
		{
			pLineHelper->AddPoints( center, prev );
		}
	}
}

void* BallJoint::Bullet_GetTypedConstraintPtr( void )
{
	return m_pConstraint;
}

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

void BallJoint::EndRefresh( void )
{
	if( m_pWorld != NULL )
	{
		m_pWorld->AttachJoint( this, false );
	}
}

void BallJoint::Refresh( void )
{
	if( m_pWorld != NULL )
	{
		m_pWorld->DetachJoint( this, false );
		m_pWorld->AttachJoint( this, false );
	}
}

void BallJoint::UpdateLimit( void )
{
	if( m_pConstraint != NULL )
	{
		m_pConstraint->setLimit( m_SwingLimit1, m_SwingLimit2, m_TwistLimit );
	}
}

void BallJoint::UpdatePivot( void )
{
	if( m_pConstraint != NULL )
	{
		btTransform frameA = m_pConstraint->getAFrame();
		btTransform frameB = m_pConstraint->getBFrame();

		frameA.getOrigin().setValue( m_PivotA.x, m_PivotA.y, m_PivotA.z );
		frameB.getOrigin().setValue( m_PivotB.x, m_PivotB.y, m_PivotB.z );

		m_pConstraint->setFrames( frameA, frameB );
	}
}

void BallJoint::UpdateFrames( void )
{
	if( ( m_pRigidBodyA != NULL ) &&
		( m_pRigidBodyB == NULL ) )
	{
		D3DXVECTOR3 pivotA = m_PivotA;

		SetFrame( m_pRigidBodyA, pivotA );
	}
	else if(	( m_pRigidBodyA != NULL ) &&
				( m_pRigidBodyB != NULL ) )
	{
		D3DXVECTOR3 pivotA = m_PivotA;
		D3DXVECTOR3 pivotB = m_PivotB;

		SetFrame( m_pRigidBodyA, m_pRigidBodyB, pivotA, pivotB );
	}
}

void BallJoint::UpdateParams( void )
{
	if( m_pConstraint != NULL )
	{
		for( int i = 0; i < BallJoint::PT_MAX; i++ )
		{
			for( int j = 0; j < BallJoint::PA_MAX; j++ )
			{
				if( IsParamEnabled( i, j ) == true )
				{
					m_pConstraint->setParam( i, m_ParamValues[i][j], j );
				}
			}
		}
	}
}

void BallJoint::Bullet_ComputeFrames(	const btRigidBody* rbA,
										const btVector3* pivotA,
										const btVector3* swingAxis,
										const btVector3* twistAxis,
										btTransform* frameA,
										btTransform* frameB )
{
	////////////////////////////////////////////////////////////////////////////////////////////////////
	// Aフレーム( Twist )
	////////////////////////////////////////////////////////////////////////////////////////////////////

	const btVector3& rbAxisA0 = *twistAxis;
	btVector3 rbAxisA1;
	btVector3 rbAxisA2;

	btPlaneSpace1( rbAxisA0, rbAxisA1, rbAxisA2 );

	frameA->getOrigin() = *pivotA;
	frameA->getBasis().setValue(	rbAxisA0.getX(), rbAxisA1.getX(), rbAxisA2.getX(),
									rbAxisA0.getY(), rbAxisA1.getY(), rbAxisA2.getY(),
									rbAxisA0.getZ(), rbAxisA1.getZ(), rbAxisA2.getZ() );

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// Bフレーム( Swing )
	////////////////////////////////////////////////////////////////////////////////////////////////////

	const btVector3& rbAxisB0 = *swingAxis;
	btVector3 rbAxisB1;
	btVector3 rbAxisB2;

	btPlaneSpace1( rbAxisB0, rbAxisB1, rbAxisB2 );

	frameB->getOrigin() = *pivotA;
	frameB->getBasis().setValue(	rbAxisB0.getX(), rbAxisB1.getX(), rbAxisB2.getX(),
									rbAxisB0.getY(), rbAxisB1.getY(), rbAxisB2.getY(),
									rbAxisB0.getZ(), rbAxisB1.getZ(), rbAxisB2.getZ() );

	if( m_bInitalRigidBodyTransform == true )
	{
		btTransform tr;

		tr.setRotation( btQuaternion( m_InitalRigidBodyRotA.x, m_InitalRigidBodyRotA.y, m_InitalRigidBodyRotA.z, m_InitalRigidBodyRotA.w ) );
		tr.setOrigin( btVector3( m_InitalRigidBodyPosA.x, m_InitalRigidBodyPosA.y, m_InitalRigidBodyPosA.z ) );

		*frameB = tr * *frameB;
	}
	else
	{
		*frameB = rbA->getCenterOfMassTransform() * *frameB;
	}
}

void BallJoint::Bullet_ComputeFrames(	const btRigidBody* rbA,
										const btRigidBody* rbB,
										const btVector3* pivotA,
										const btVector3* pivotB,
										const btVector3* swingAxis,
										const btVector3* twistAxis,
										btTransform* frameA,
										btTransform* frameB )
{
	////////////////////////////////////////////////////////////////////////////////////////////////////
	// Aフレーム( Twist )
	////////////////////////////////////////////////////////////////////////////////////////////////////

	btVector3 rbAxisA0 = *twistAxis;
	btVector3 rbAxisA1;
	btVector3 rbAxisA2;

	btPlaneSpace1( rbAxisA0, rbAxisA1, rbAxisA2 );

	frameA->getOrigin() = *pivotA;
	frameA->getBasis().setValue(	rbAxisA0.getX(), rbAxisA1.getX(), rbAxisA2.getX(),
									rbAxisA0.getY(), rbAxisA1.getY(), rbAxisA2.getY(),
									rbAxisA0.getZ(), rbAxisA1.getZ(), rbAxisA2.getZ() );

	////////////////////////////////////////////////////////////////////////////////////////////////////
	// Bフレーム( Swing )
	////////////////////////////////////////////////////////////////////////////////////////////////////

	const btVector3& rbAxisB0 = *swingAxis;
	btVector3 rbAxisB1;
	btVector3 rbAxisB2;

	btMatrix3x3 basisA;
	btMatrix3x3 basisB;

	btPlaneSpace1( rbAxisB0, rbAxisB1, rbAxisB2 );

	frameB->getOrigin() = *pivotB;
	frameB->getBasis().setValue(	rbAxisB0.getX(), rbAxisB1.getX(), rbAxisB2.getX(),
									rbAxisB0.getY(), rbAxisB1.getY(), rbAxisB2.getY(),
									rbAxisB0.getZ(), rbAxisB1.getZ(), rbAxisB2.getZ() );

	if( m_bInitalRigidBodyTransform == true )
	{
		//初期姿勢が設定されている場合
		basisA.setRotation( btQuaternion( m_InitalRigidBodyRotA.x, m_InitalRigidBodyRotA.y, m_InitalRigidBodyRotA.z, m_InitalRigidBodyRotA.w ) );
		basisB.setRotation( btQuaternion( m_InitalRigidBodyRotB.x, m_InitalRigidBodyRotB.y, m_InitalRigidBodyRotB.z, m_InitalRigidBodyRotB.w ) );
	}
	else
	{
		//初期姿勢が設定されていない場合は、リジッドボディから直接取得
		basisA = rbA->getCenterOfMassTransform().getBasis();
		basisB = rbB->getCenterOfMassTransform().getBasis();
	}

	//Aの空間に変換
	frameB->getBasis() = basisA * frameB->getBasis();

	//Aの空間で作成されたフレームをBの空間へ変換
	frameB->getBasis() = basisB.inverse() * frameB->getBasis();
}

unsigned int BallJoint::GetParamBit( int type, int axis )
{
	return 1 << ( type * ( axis + 1 ) );
}

}}}}}
