// **************************************************
//              NECK BONE HANDLING
// **************************************************
void AnimatedWarrior_cl::TurnHead(float fDegreesPerSecond, float fTimeDiff, float fMinAngle, float fMaxAngle)
{
  float fAmount = fDegreesPerSecond * fTimeDiff;

  // update joint rotation (with clamp)
  m_fHeadRotationAngles[1] += fAmount;
  if (m_fHeadRotationAngles[1] > fMaxAngle)
    m_fHeadRotationAngles[1] = fMaxAngle;
  else if (m_fHeadRotationAngles[1] < fMinAngle)
    m_fHeadRotationAngles[1] = fMinAngle;

  //// set new rotation
  VASSERT(GetAnimConfig() == m_spAnimConfig);
  VisAnimFinalSkeletalResult_cl *pFinalResult = m_spAnimConfig->GetFinalResult();
  VASSERT(pFinalResult != NULL);

  // update the current localspace result and add custom rotation
  pFinalResult->GetCurrentLocalSpaceResult();
  hkvQuat tempRotation(hkvNoInitialization);
  tempRotation.setFromEulerAngles (m_fHeadRotationAngles[2], m_fHeadRotationAngles[1], m_fHeadRotationAngles[0]);
  pFinalResult->SetCustomBoneRotation(m_iHeadBoneIndex, tempRotation, VIS_MODIFY_BONE|VIS_LOCAL_SPACE);

  m_bHeadInMovement = true;
}
void vHavokBehaviorComponent::OnAfterHavokUpdate()
{
	if( m_character == HK_NULL || m_entityOwner == HK_NULL )
	{
		return;
	}

	// Update WFM of skin or override it
	if ( m_useBehaviorWorldFromModel )
	{
		const hkQsTransform& worldFromModel = m_character->getWorldFromModel();

		// Copy the Behavior result into vision
		hkvMat3 visionRotation;
		hkvVec3 visionTranslation;
		vHavokConversionUtils::HkQuatToVisMatrix( worldFromModel.getRotation(), visionRotation );
		vHavokConversionUtils::PhysVecToVisVecWorld( worldFromModel.getTranslation(), visionTranslation );
		m_entityOwner->SetPosition( visionTranslation );
		m_entityOwner->SetRotationMatrix( visionRotation );
	}
	else
	{
		// Override Behavior results.
		// This will currently cause Behavior data being sent to HBT during remote debug to be one frame off.
		// However, the only other solution with the current APIs is to disable motion accumulation on *all*
		// Characters.  A slight delay/offset in the special case of remote debugging in HBT isn't worth that change.
		UpdateHavokTransformFromVision();
	}

	if( m_entityOwner->GetMesh() == HK_NULL || m_entityOwner->GetMesh()->GetSkeleton() == HK_NULL )
	{
		return;
	}

	VisAnimConfig_cl* animConfig = m_entityOwner->GetAnimConfig();
	if( !animConfig )
	{
		return;
	}

	VisAnimFinalSkeletalResult_cl* skeletalResult = animConfig->GetFinalResult();
	if( !skeletalResult )
	{
		return;
	}

	// Try updating the bone index list in case a mesh was added
	if( m_boneIndexList.getSize() == 0 )
	{
		UpdateAnimationAndBoneIndexList();

		// Exit early if there's no bone index list
		if( m_boneIndexList.getSize() == 0 )
		{
			return;
		}
	}

	// Convert pose to Havok model space
	const hkQsTransform* poseLocal = m_character->getPoseLocal();
	hkArray<hkQsTransform> poseModel( m_character->getNumPoseLocal() );
	hkaSkeletonUtils::transformLocalPoseToModelPose( m_character->getNumPoseLocal(), m_character->getSetup()->m_animationSkeleton->m_parentIndices.begin(), poseLocal, poseModel.begin() );
	const hkQsTransform* pose = poseModel.begin();

	float const inverseCharacterScale = 1.0f / m_character->getSetup()->getData()->m_scale;

	// Convert pose to vision units
	VisSkeleton_cl* visionSkeleton = m_entityOwner->GetMesh()->GetSkeleton();

	
	for( int havokBoneIndex = 0; havokBoneIndex < m_character->getNumPoseLocal(); havokBoneIndex++ )
	{
		// Find the bone index
		int visionBoneIndex = m_boneIndexList[havokBoneIndex];
		if( visionBoneIndex != -1 )
		{
			HK_ON_DEBUG( VisSkeletalBone_cl* bone = visionSkeleton->GetBone(visionBoneIndex) );
			HK_ASSERT2(0x68b6649, hkString::strCmp( bone->m_sBoneName.AsChar(), m_character->getSetup()->m_animationSkeleton->m_bones[havokBoneIndex].m_name.cString() ) == 0, "" );

			const hkQsTransform& transform = pose[havokBoneIndex];

			// Convert Havok pose to Vision pose
			hkvQuat quat;
			quat.setIdentity();
			vHavokConversionUtils::HkQuatToVisQuat( transform.getRotation(), quat );
			hkvVec3 scale; vHavokConversionUtils::PhysVecToVisVec_noscale( transform.getScale(), scale );
			hkvVec3 translation; vHavokConversionUtils::PhysVecToVisVecWorld( transform.getTranslation(), translation );

			// Behavior propagates character scale through the skeleton; need to compensate when scaling to Vision
			scale *= inverseCharacterScale;

			// Set the skeletal result
			skeletalResult->SetCustomBoneScaling( visionBoneIndex, scale, VIS_REPLACE_BONE | VIS_OBJECT_SPACE );
			skeletalResult->SetCustomBoneRotation( visionBoneIndex, quat, VIS_REPLACE_BONE | VIS_OBJECT_SPACE );
			skeletalResult->SetCustomBoneTranslation( visionBoneIndex, translation, VIS_REPLACE_BONE | VIS_OBJECT_SPACE );
		}
	}

#if 0

	// Draw skeleton
	for( int i = 0; i < visionSkeleton->GetBoneCount(); i++ )
	{
		VisSkeletalBone_cl* bone = visionSkeleton->GetBone(i);
		if( bone->m_iParentIndex != -1 )
		{
			hkvVec3 translation, subTranslation;
			hkvQuat rotation, subRotation;
			m_entityOwner->GetBoneCurrentWorldSpaceTransformation( i, translation, rotation );
			m_entityOwner->GetBoneCurrentWorldSpaceTransformation( bone->m_iParentIndex, subTranslation, subRotation );

			Vision::Game.DrawSingleLine( translation.x, translation.y, translation.z, subTranslation.x, subTranslation.y, subTranslation.z );
		}
	}

#endif

}