C++ FQuat::Normalize方法代码示例

FQuat FAnimNode_RotationMultiplier::MultiplyQuatBasedOnSourceIndex(const FTransform& RefPoseTransform, const FTransform& LocalBoneTransform, const EBoneAxis Axis, float InMultiplier, const FQuat& ReferenceQuat)
{
	// Find delta angle for source bone.
	FQuat DeltaQuat = ExtractAngle(RefPoseTransform, LocalBoneTransform, Axis);

	// Turn to Axis and Angle
	FVector RotationAxis;
	float	RotationAngle;
	DeltaQuat.ToAxisAndAngle(RotationAxis, RotationAngle);

	const FVector DefaultAxis = GetAxisVector(Axis);

	// See if we need to invert angle - shortest path
	if( (RotationAxis | DefaultAxis) < 0.f )
	{
		RotationAxis = -RotationAxis;
		RotationAngle = -RotationAngle;
	}

	// Make sure it is the shortest angle.
	RotationAngle = FMath::UnwindRadians(RotationAngle);

	// New bone rotation
	FQuat OutQuat = ReferenceQuat * FQuat(RotationAxis, RotationAngle* InMultiplier);
	// Normalize resulting quaternion.
	OutQuat.Normalize();

#if 0 //DEBUG_TWISTBONECONTROLLER
	UE_LOG(LogSkeletalControl, Log, TEXT("\t RefQuat: %s, Rot: %s"), *ReferenceQuat.ToString(), *ReferenceQuat.Rotator().ToString() );
	UE_LOG(LogSkeletalControl, Log, TEXT("\t NewQuat: %s, Rot: %s"), *OutQuat.ToString(), *OutQuat.Rotator().ToString() );
	UE_LOG(LogSkeletalControl, Log, TEXT("\t RollAxis: %s, RollAngle: %f"), *RotationAxis.ToString(), RotationAngle );
#endif

	return OutQuat;
}

/** custom instantiation of Interpolate for FQuats */
template <> FQuat Interpolate<FQuat>(const FQuat& A, const FQuat& B, float Alpha)
{
	FQuat result = FQuat::FastLerp(A,B,Alpha);
	result.Normalize();

	return result;
}

FMatrix USkeletalMeshComponent::GetTransformMatrix() const
{
	FTransform RootTransform = GetBoneTransform(0);
	FVector Translation;
	FQuat Rotation;
	
	// if in editor, it should always use localToWorld
	// if root motion is ignored, use root transform 
	if( GetWorld()->IsGameWorld() || !SkeletalMesh )
	{
		// add root translation info
		Translation = RootTransform.GetLocation();
	}
	else
	{
		Translation = ComponentToWorld.TransformPosition(SkeletalMesh->RefSkeleton.GetRefBonePose()[0].GetTranslation());
	}

	// if root rotation is ignored, use root transform rotation
	Rotation = RootTransform.GetRotation();

	// now I need to get scale
	// only LocalToWorld will have scale
	FVector ScaleVector = ComponentToWorld.GetScale3D();

	Rotation.Normalize();
	return FScaleMatrix(ScaleVector)*FQuatRotationTranslationMatrix(Rotation, Translation);
}

void FGameFrame::PoseToOrientationAndPosition(const ovrPosef& InPose, FQuat& OutOrientation, FVector& OutPosition) const
{
	OutOrientation = ToFQuat(InPose.Orientation);

	check(WorldToMetersScale >= 0);
	// correct position according to BaseOrientation and BaseOffset. 
	const FVector Pos = (ToFVector_M2U(OVR::Vector3f(InPose.Position), WorldToMetersScale) - (Settings->BaseOffset * WorldToMetersScale)) * CameraScale3D;
	OutPosition = Settings->BaseOrientation.Inverse().RotateVector(Pos);

	// apply base orientation correction to OutOrientation
	OutOrientation = Settings->BaseOrientation.Inverse() * OutOrientation;
	OutOrientation.Normalize();
}

void FSteamVRHMD::PoseToOrientationAndPosition(const vr::HmdMatrix34_t& InPose, FQuat& OutOrientation, FVector& OutPosition) const
{
	FMatrix Pose = ToFMatrix(InPose);
	FQuat Orientation(Pose);
 
	OutOrientation.X = -Orientation.Z;
	OutOrientation.Y = Orientation.X;
	OutOrientation.Z = Orientation.Y;
 	OutOrientation.W = -Orientation.W;	

	FVector Position = FVector(-Pose.M[3][2], Pose.M[3][0], Pose.M[3][1]) * WorldToMetersScale;
	OutPosition = BaseOrientation.Inverse().RotateVector(Position);

	OutOrientation = BaseOrientation.Inverse() * OutOrientation;
	OutOrientation.Normalize();
}

FQuat USplineComponent::GetQuaternionAtSplineInputKey(float InKey, ESplineCoordinateSpace::Type CoordinateSpace) const
{
	FQuat Quat = SplineRotInfo.Eval(InKey, FQuat::Identity);
	Quat.Normalize();

	const FVector Direction = SplineInfo.EvalDerivative(InKey, FVector::ZeroVector).GetSafeNormal();
	const FVector UpVector = Quat.RotateVector(DefaultUpVector);

	FQuat Rot = (FRotationMatrix::MakeFromXZ(Direction, UpVector)).ToQuat();

	if (CoordinateSpace == ESplineCoordinateSpace::World)
	{
		Rot = ComponentToWorld.GetRotation() * Rot;
	}

	return Rot;
}

FRotator UKismetMathLibrary::RLerp(FRotator A, FRotator B, float Alpha, bool bShortestPath)
{
	FRotator DeltaAngle = B - A;

	// if shortest path, we use Quaternion to interpolate instead of using FRotator
	if( bShortestPath )
	{
		FQuat AQuat(A);
		FQuat BQuat(B);

		FQuat Result = FQuat::Slerp(AQuat, BQuat, Alpha);
		Result.Normalize();

		return Result.Rotator();
	}

	return A + Alpha*DeltaAngle;
}

void FAnimationRuntime::BlendPosesTogetherPerBoneInMeshSpace(TArray<FCompactPose>& SourcePoses, const TArray<FBlendedCurve>& SourceCurves, const UBlendSpaceBase* BlendSpace, const TArray<FBlendSampleData>& BlendSampleDataCache, FCompactPose& ResultPose, FBlendedCurve& ResultCurve)
{
	FQuat NewRotation;
	USkeleton* Skeleton = BlendSpace->GetSkeleton();

	// all this is going to do is to convert SourcePoses.Rotation to be mesh space, and then once it goes through BlendPosesTogetherPerBone, convert back to local
	for (FCompactPose& Pose : SourcePoses)
	{
		for (const FCompactPoseBoneIndex BoneIndex : Pose.ForEachBoneIndex())
		{
			const FCompactPoseBoneIndex ParentIndex = Pose.GetParentBoneIndex(BoneIndex);
			if (ParentIndex != INDEX_NONE)
			{
				NewRotation = Pose[ParentIndex].GetRotation()*Pose[BoneIndex].GetRotation();
				NewRotation.Normalize();
			}
			else
			{
				NewRotation = Pose[BoneIndex].GetRotation();
			}

			// now copy back to SourcePoses
			Pose[BoneIndex].SetRotation(NewRotation);
		}
	}

	// now we have mesh space rotation, call BlendPosesTogetherPerBone
	BlendPosesTogetherPerBone(SourcePoses, SourceCurves, BlendSpace, BlendSampleDataCache, ResultPose, ResultCurve);

	// now result atoms has the output with mesh space rotation. Convert back to local space, start from back
	for (const FCompactPoseBoneIndex BoneIndex : ResultPose.ForEachBoneIndex())
	{
		const FCompactPoseBoneIndex ParentIndex = ResultPose.GetParentBoneIndex(BoneIndex);
		if (ParentIndex != INDEX_NONE)
		{
			FQuat LocalBlendQuat = ResultPose[ParentIndex].GetRotation().Inverse()*ResultPose[BoneIndex].GetRotation();
			ResultPose[BoneIndex].SetRotation(LocalBlendQuat);
			ResultPose[BoneIndex].NormalizeRotation();
		}
	}
}

void FAnimationRuntime::BlendMeshPosesPerBoneWeights(
		struct FCompactPose& BasePose,
		const TArray<struct FCompactPose>& BlendPoses,
		struct FBlendedCurve& BaseCurve,
		const TArray<struct FBlendedCurve>& BlendedCurves,
		const TArray<FPerBoneBlendWeight>& BoneBlendWeights,
		ECurveBlendOption::Type CurveBlendOption,
		/*out*/ FCompactPose& OutPose,
		/*out*/ struct FBlendedCurve& OutCurve)
{
	check(BasePose.GetNumBones() == BoneBlendWeights.Num());

	const FBoneContainer& BoneContainer = BasePose.GetBoneContainer();

	TCustomBoneIndexArray<FQuat, FCompactPoseBoneIndex> SourceRotations;
	TCustomBoneIndexArray<FQuat, FCompactPoseBoneIndex> BlendRotations;
	TCustomBoneIndexArray<FQuat, FCompactPoseBoneIndex> TargetRotations;

	SourceRotations.AddUninitialized(BasePose.GetNumBones());
	BlendRotations.AddUninitialized(BasePose.GetNumBones());
	TargetRotations.AddUninitialized(BasePose.GetNumBones());

	int32 PoseNum = BlendPoses.Num();

	TArray<float> MaxPoseWeights;
	MaxPoseWeights.AddZeroed(PoseNum);

	for (FCompactPoseBoneIndex BoneIndex : BasePose.ForEachBoneIndex())
	{
		const int32 PoseIndex = BoneBlendWeights[BoneIndex.GetInt()].SourceIndex;
		const FCompactPoseBoneIndex ParentIndex = BoneContainer.GetParentBoneIndex(BoneIndex);

		FQuat SrcRotationInMesh;
		FQuat TargetRotationInMesh;

		if (ParentIndex != INDEX_NONE)
		{
			SrcRotationInMesh = SourceRotations[ParentIndex] * BasePose[BoneIndex].GetRotation();
			TargetRotationInMesh = TargetRotations[ParentIndex] * BlendPoses[PoseIndex][BoneIndex].GetRotation();
		}
		else
		{
			SrcRotationInMesh = BasePose[BoneIndex].GetRotation();
			TargetRotationInMesh = BlendPoses[PoseIndex][BoneIndex].GetRotation();
		}

		// update mesh based rotations
		SourceRotations[BoneIndex] = SrcRotationInMesh;
		TargetRotations[BoneIndex] = TargetRotationInMesh;

		// now update outer
		FTransform BaseAtom = BasePose[BoneIndex];
		FTransform TargetAtom = BlendPoses[PoseIndex][BoneIndex];
		FTransform BlendAtom;

		const float BlendWeight = FMath::Clamp(BoneBlendWeights[BoneIndex.GetInt()].BlendWeight, 0.f, 1.f);
		MaxPoseWeights[PoseIndex] = FMath::Max(MaxPoseWeights[PoseIndex], BlendWeight);

		if (BlendWeight < ZERO_ANIMWEIGHT_THRESH)
		{
			BlendAtom = BaseAtom;
			BlendRotations[BoneIndex] = SourceRotations[BoneIndex];
		}
		else if ((1.0 - BlendWeight) < ZERO_ANIMWEIGHT_THRESH)
		{
			BlendAtom = TargetAtom;
			BlendRotations[BoneIndex] = TargetRotations[BoneIndex];
		}
		else // we want blend here
		{
			BlendAtom = BaseAtom;
			BlendAtom.BlendWith(TargetAtom, BlendWeight);

			// blend rotation in mesh space
			BlendRotations[BoneIndex] = FQuat::FastLerp(SourceRotations[BoneIndex], TargetRotations[BoneIndex], BlendWeight);

			// Fast lerp produces un-normalized quaternions, re-normalize.
			BlendRotations[BoneIndex].Normalize();
		}

		OutPose[BoneIndex] = BlendAtom;
		if (ParentIndex != INDEX_NONE)
		{
			FQuat LocalBlendQuat = BlendRotations[ParentIndex].Inverse() * BlendRotations[BoneIndex];

			// local -> mesh -> local transformations can cause loss of precision for long bone chains, we have to normalize rotation there.
			LocalBlendQuat.Normalize();
			OutPose[BoneIndex].SetRotation(LocalBlendQuat);
		}
	}

	// time to blend curves
	// the way we blend curve per bone
	// is to find out max weight per that pose, and then apply that weight to the curve
	{
		TArray<const FBlendedCurve*> SourceCurves;
		TArray<float> SourceWegihts;

		SourceCurves.SetNumUninitialized(PoseNum+1);
		SourceWegihts.SetNumUninitialized(PoseNum+1);
//.........这里部分代码省略.........

//.........这里部分代码省略.........
							PositionTracks,
							RotationTracks,
							ScaleTracks,
							BoneIndex,
							HighestTargetBoneIndex,
							false,
							NewWorldBones);
						
						// adjust all rotation keys towards the end effector target
						for ( int32 KeyIndex = 0; KeyIndex < NumRotKeys; ++KeyIndex )
						{
							FQuat& Key = RotTrack.RotKeys[KeyIndex];

							const int32 FrameIndex = FMath::Clamp(KeyIndex, 0, LastFrame);

							const FTransform& NewWorldTransform = NewWorldBones[(BoneIndex*NumFrames) + FrameIndex];

							const FTransform& DesiredChildTransform = RawWorldBones[(FurthestTargetBoneIndex*NumFrames) + FrameIndex].GetRelativeTransform(NewWorldTransform);
							const FTransform& CurrentChildTransform = NewWorldBones[(FurthestTargetBoneIndex*NumFrames) + FrameIndex].GetRelativeTransform(NewWorldTransform);

							// find the two vectors which represent the angular error we are trying to correct
							const FVector& CurrentHeading = CurrentChildTransform.GetTranslation();
							const FVector& DesiredHeading = DesiredChildTransform.GetTranslation();

							// if these are valid, we can continue
							if (!CurrentHeading.IsNearlyZero() && !DesiredHeading.IsNearlyZero())
							{
								const float DotResult = CurrentHeading.SafeNormal() | DesiredHeading.SafeNormal();

								// limit the range we will retarget to something reasonable (~60 degrees)
								if (DotResult < 1.0f && DotResult > 0.5f)
								{
									FQuat Adjustment= FQuat::FindBetween(CurrentHeading, DesiredHeading); 
									Adjustment.Normalize();
									Adjustment= EnforceShortestArc(FQuat::Identity, Adjustment);

									const FVector Test = Adjustment.RotateVector(CurrentHeading);
									const float Delta = (Test - DesiredHeading).Size();
									if (Delta < 0.001f)
									{
										FQuat NewKey = Adjustment * Key;
										NewKey.Normalize();

										const FQuat& AlignedKey = EnforceShortestArc(Key, NewKey);
										Key = AlignedKey;
									}
								}
							}
						}
					}
				}

				if (NumPosKeys > 0 && ParentBoneIndex != INDEX_NONE)
				{
					// update our bone table from the current bone through the last end effector we need to test
					UpdateWorldBoneTransformRange(
						AnimSeq, 
						BoneData, 
						RefPose,
						PositionTracks,
						RotationTracks,
						ScaleTracks,
						BoneIndex,
						HighestTargetBoneIndex,
						false,
						NewWorldBones);