C++ FVector::FindBestAxisVectors方法代码示例

void DrawDebugDirectionalArrow(const UWorld* InWorld, FVector const& LineStart, FVector const& LineEnd, float ArrowSize, FColor const& Color, bool bPersistentLines, float LifeTime, uint8 DepthPriority)
{
	// no debug line drawing on dedicated server
	if (GEngine->GetNetMode(InWorld) != NM_DedicatedServer)
	{
		if (ArrowSize <= 0)
		{
			ArrowSize = 10.f;
		}

		DrawDebugLine(InWorld, LineStart, LineEnd, Color, bPersistentLines, LifeTime);

		FVector Dir = (LineEnd-LineStart);
		Dir.Normalize();
		FVector Up(0, 0, 1);
		FVector Right = Dir ^ Up;
		if (!Right.IsNormalized())
		{
			Dir.FindBestAxisVectors(Up, Right);
		}
		FVector Origin = FVector::ZeroVector;
		FMatrix TM;
		// get matrix with dir/right/up
		TM.SetAxes(&Dir, &Right, &Up, &Origin);

		// since dir is x direction, my arrow will be pointing +y, -x and -y, -x
		float ArrowSqrt = FMath::Sqrt(ArrowSize);
		FVector ArrowPos;
		DrawDebugLine(InWorld, LineEnd, LineEnd + TM.TransformPosition(FVector(-ArrowSqrt, ArrowSqrt, 0)), Color, bPersistentLines, LifeTime, DepthPriority);
		DrawDebugLine(InWorld, LineEnd, LineEnd + TM.TransformPosition(FVector(-ArrowSqrt, -ArrowSqrt, 0)), Color, bPersistentLines, LifeTime, DepthPriority);
	}
}

void UAnimGraphNode_BoneDrivenController::Draw(FPrimitiveDrawInterface* PDI, USkeletalMeshComponent* SkelMeshComp) const
{	
	static const float ArrowHeadWidth = 5.0f;
	static const float ArrowHeadHeight = 8.0f;

	const int32 SourceIdx = SkelMeshComp->GetBoneIndex(Node.SourceBone.BoneName);
	const int32 TargetIdx = SkelMeshComp->GetBoneIndex(Node.TargetBone.BoneName);

	if ((SourceIdx != INDEX_NONE) && (TargetIdx != INDEX_NONE))
	{
		const FTransform SourceTM = SkelMeshComp->GetSpaceBases()[SourceIdx] * SkelMeshComp->ComponentToWorld;
		const FTransform TargetTM = SkelMeshComp->GetSpaceBases()[TargetIdx] * SkelMeshComp->ComponentToWorld;

		PDI->DrawLine(TargetTM.GetLocation(), SourceTM.GetLocation(), FLinearColor(0.0f, 0.0f, 1.0f), SDPG_Foreground, 0.5f);

		const FVector ToTarget = TargetTM.GetTranslation() - SourceTM.GetTranslation();
		const FVector UnitToTarget = ToTarget.GetSafeNormal();
		FVector Midpoint = SourceTM.GetTranslation() + 0.5f * ToTarget + 0.5f * UnitToTarget * ArrowHeadHeight;

		FVector YAxis;
		FVector ZAxis;
		UnitToTarget.FindBestAxisVectors(YAxis, ZAxis);
		const FMatrix ArrowMatrix(UnitToTarget, YAxis, ZAxis, Midpoint);

		DrawConnectedArrow(PDI, ArrowMatrix, FLinearColor(0.0f, 1.0f, 0.0), ArrowHeadHeight, ArrowHeadWidth, SDPG_Foreground);

		PDI->DrawPoint(SourceTM.GetTranslation(), FLinearColor(0.8f, 0.8f, 0.2f), 5.0f, SDPG_Foreground);
		PDI->DrawPoint(SourceTM.GetTranslation() + ToTarget, FLinearColor(0.8f, 0.8f, 0.2f), 5.0f, SDPG_Foreground);
	}
}

/**
* Draws a line with an arrow at the end.
*
* @param Start		Starting point of the line.
* @param End		Ending point of the line.
* @param Color		Color of the line.
* @param Mag		Size of the arrow.
*/
void FDebugRenderSceneProxy::DrawLineArrow(FPrimitiveDrawInterface* PDI,const FVector &Start,const FVector &End,const FColor &Color,float Mag) const
{
	// draw a pretty arrow
	FVector Dir = End - Start;
	const float DirMag = Dir.Size();
	Dir /= DirMag;
	FVector YAxis, ZAxis;
	Dir.FindBestAxisVectors(YAxis,ZAxis);
	FMatrix ArrowTM(Dir,YAxis,ZAxis,Start);
	DrawDirectionalArrow(PDI,ArrowTM,Color,DirMag,Mag,SDPG_World);
}

void DrawDebugCylinder(const UWorld* InWorld, FVector const& Start, FVector const& End, float Radius, int32 Segments, FColor const& Color, bool bPersistentLines, float LifeTime, uint8 DepthPriority)
{
	// no debug line drawing on dedicated server
	if (GEngine->GetNetMode(InWorld) != NM_DedicatedServer)
	{
		// Need at least 4 segments
		Segments = FMath::Max(Segments, 4);

		// Rotate a point around axis to form cylinder segments
		FVector Segment;
		FVector P1, P2, P3, P4;
		const int32 AngleInc = 360.f / Segments;
		int32 Angle = AngleInc;

		// Default for Axis is up
		FVector Axis = (End - Start).SafeNormal();
		if( Axis.IsZero() )
		{
			Axis = FVector(0.f, 0.f, 1.f);
		}

		FVector Perpendicular;
		FVector Dummy;

		Axis.FindBestAxisVectors(Perpendicular, Dummy);


		Segment = Perpendicular.RotateAngleAxis(0, Axis) * Radius;
		P1 = Segment + Start;
		P3 = Segment + End;

		// this means foreground lines can't be persistent 
		ULineBatchComponent* const LineBatcher = GetDebugLineBatcher( InWorld, bPersistentLines, LifeTime, (DepthPriority == SDPG_Foreground) );
		if(LineBatcher != NULL)
		{
			while( Segments-- )
			{
				Segment = Perpendicular.RotateAngleAxis(Angle, Axis) * Radius;
				P2 = Segment + Start;
				P4 = Segment + End;

				LineBatcher->DrawLine(P2, P4, Color, DepthPriority);
				LineBatcher->DrawLine(P1, P2, Color, DepthPriority);
				LineBatcher->DrawLine(P3, P4, Color, DepthPriority);

				P1 = P2;
				P3 = P4;
				Angle += AngleInc;
			}
		}
	}
}

//
// Build an FPoly representing an "infinite" plane (which exceeds the maximum
// dimensions of the world in all directions) for a particular Bsp node.
//
FPoly FBSPOps::BuildInfiniteFPoly( UModel* Model, int32 iNode )
{
	FBspNode &Node   = Model->Nodes  [iNode       ];
	FBspSurf &Poly   = Model->Surfs  [Node.iSurf  ];
	FVector  Base    = Poly.Plane * Poly.Plane.W;
	FVector  Normal  = Poly.Plane;
	FVector	 Axis1,Axis2;

	// Find two non-problematic axis vectors.
	Normal.FindBestAxisVectors( Axis1, Axis2 );

	// Set up the FPoly.
	FPoly EdPoly;
	EdPoly.Init();
	EdPoly.Normal      = Normal;
	EdPoly.Base        = Base;
	new(EdPoly.Vertices) FVector(Base + Axis1*WORLD_MAX + Axis2*WORLD_MAX);
	new(EdPoly.Vertices) FVector(Base - Axis1*WORLD_MAX + Axis2*WORLD_MAX);
	new(EdPoly.Vertices) FVector(Base - Axis1*WORLD_MAX - Axis2*WORLD_MAX);
	new(EdPoly.Vertices) FVector(Base + Axis1*WORLD_MAX - Axis2*WORLD_MAX);

	return EdPoly;
}

void FAnimNode_TwoBoneIK::EvaluateBoneTransforms(USkeletalMeshComponent* SkelComp, const FBoneContainer & RequiredBones, FA2CSPose& MeshBases, TArray<FBoneTransform>& OutBoneTransforms)
{
	check(OutBoneTransforms.Num() == 0);

	// Get indices of the lower and upper limb bones and check validity.
	bool bInvalidLimb = false;

	const int32 EndBoneIndex = IKBone.BoneIndex;
	const int32 LowerLimbIndex = RequiredBones.GetParentBoneIndex(EndBoneIndex);
	if (LowerLimbIndex == INDEX_NONE)
	{
		bInvalidLimb = true;
	}

	const int32 UpperLimbIndex = RequiredBones.GetParentBoneIndex(LowerLimbIndex);
	if (UpperLimbIndex == INDEX_NONE)
	{
		bInvalidLimb = true;
	}

	const bool bInBoneSpace = (EffectorLocationSpace == BCS_ParentBoneSpace) || (EffectorLocationSpace == BCS_BoneSpace);
	const int32 EffectorSpaceBoneIndex = bInBoneSpace ? RequiredBones.GetPoseBoneIndexForBoneName(EffectorSpaceBoneName) : INDEX_NONE;

	if (bInBoneSpace && ((EffectorSpaceBoneIndex == INDEX_NONE) || !RequiredBones.Contains(EffectorSpaceBoneIndex)))
	{
		bInvalidLimb = true;
	}

	// If we walked past the root, this controlled is invalid, so return no affected bones.
	if( bInvalidLimb )
	{
		return;
	}

	// Get Local Space transforms for our bones. We do this first in case they already are local.
	// As right after we get them in component space. (And that does the auto conversion).
	// We might save one transform by doing local first...
	const FTransform EndBoneLocalTransform = MeshBases.GetLocalSpaceTransform(IKBone.BoneIndex);

	// Now get those in component space...
	FTransform UpperLimbCSTransform = MeshBases.GetComponentSpaceTransform(UpperLimbIndex);
	FTransform LowerLimbCSTransform = MeshBases.GetComponentSpaceTransform(LowerLimbIndex);
	FTransform EndBoneCSTransform = MeshBases.GetComponentSpaceTransform(IKBone.BoneIndex);

	// Get current position of root of limb.
	// All position are in Component space.
	const FVector RootPos = UpperLimbCSTransform.GetTranslation();
	const FVector InitialJointPos = LowerLimbCSTransform.GetTranslation();
	const FVector InitialEndPos = EndBoneCSTransform.GetTranslation();

	// Transform EffectorLocation from EffectorLocationSpace to ComponentSpace.
	FTransform EffectorTransform(EffectorLocation);
	FAnimationRuntime::ConvertBoneSpaceTransformToCS(SkelComp, MeshBases, EffectorTransform, EffectorSpaceBoneIndex, EffectorLocationSpace);

	// This is our reach goal.
	FVector DesiredPos = EffectorTransform.GetTranslation();
	FVector DesiredDelta = DesiredPos - RootPos;
	float DesiredLength = DesiredDelta.Size();

	// Check to handle case where DesiredPos is the same as RootPos.
	FVector	DesiredDir;
	if (DesiredLength < (float)KINDA_SMALL_NUMBER)
	{
		DesiredLength = (float)KINDA_SMALL_NUMBER;
		DesiredDir = FVector(1,0,0);
	}
	else
	{
		DesiredDir = DesiredDelta / DesiredLength;
	}

	// Get joint target (used for defining plane that joint should be in).
	FTransform JointTargetTransform(JointTargetLocation);
	const int32 JointTargetSpaceBoneIndex = (JointTargetLocationSpace == BCS_ParentBoneSpace || JointTargetLocationSpace == BCS_BoneSpace) ? RequiredBones.GetPoseBoneIndexForBoneName(JointTargetSpaceBoneName) : INDEX_NONE;
	FAnimationRuntime::ConvertBoneSpaceTransformToCS(SkelComp, MeshBases, JointTargetTransform, JointTargetSpaceBoneIndex, JointTargetLocationSpace);

	FVector	JointTargetPos = JointTargetTransform.GetTranslation();
	FVector JointTargetDelta = JointTargetPos - RootPos;
	float JointTargetLength = JointTargetDelta.Size();

	// Same check as above, to cover case when JointTarget position is the same as RootPos.
	FVector JointPlaneNormal, JointBendDir;
	if (JointTargetLength < (float)KINDA_SMALL_NUMBER)
	{
		JointBendDir = FVector(0,1,0);
		JointPlaneNormal = FVector(0,0,1);
	}
	else
	{
		JointPlaneNormal = DesiredDir ^ JointTargetDelta;

		// If we are trying to point the limb in the same direction that we are supposed to displace the joint in, 
		// we have to just pick 2 random vector perp to DesiredDir and each other.
		if (JointPlaneNormal.Size() < (float)KINDA_SMALL_NUMBER)
		{
			DesiredDir.FindBestAxisVectors(JointPlaneNormal, JointBendDir);
		}
		else
		{
			JointPlaneNormal.Normalize();
//.........这里部分代码省略.........

void DrawDebugCone(const UWorld* InWorld, FVector const& Origin, FVector const& Direction, float Length, float AngleWidth, float AngleHeight, int32 NumSides, FColor const& DrawColor, bool bPersistentLines, float LifeTime, uint8 DepthPriority)
{
	// no debug line drawing on dedicated server
	if (GEngine->GetNetMode(InWorld) != NM_DedicatedServer)
	{
		// Need at least 4 sides
		NumSides = FMath::Max(NumSides, 4);

		const float Angle1 = FMath::Clamp<float>(AngleHeight, (float)KINDA_SMALL_NUMBER, (float)(PI - KINDA_SMALL_NUMBER));
		const float Angle2 = FMath::Clamp<float>(AngleWidth, (float)KINDA_SMALL_NUMBER, (float)(PI - KINDA_SMALL_NUMBER));

		const float SinX_2 = FMath::Sin(0.5f * Angle1);
		const float SinY_2 = FMath::Sin(0.5f * Angle2);

		const float SinSqX_2 = SinX_2 * SinX_2;
		const float SinSqY_2 = SinY_2 * SinY_2;

		const float TanX_2 = FMath::Tan(0.5f * Angle1);
		const float TanY_2 = FMath::Tan(0.5f * Angle2);

		TArray<FVector> ConeVerts;
		ConeVerts.AddUninitialized(NumSides);

		for(int32 i = 0; i < NumSides; i++)
		{
			const float Fraction	= (float)i/(float)(NumSides);
			const float Thi			= 2.f * PI * Fraction;
			const float Phi			= FMath::Atan2(FMath::Sin(Thi)*SinY_2, FMath::Cos(Thi)*SinX_2);
			const float SinPhi		= FMath::Sin(Phi);
			const float CosPhi		= FMath::Cos(Phi);
			const float SinSqPhi	= SinPhi*SinPhi;
			const float CosSqPhi	= CosPhi*CosPhi;

			const float RSq			= SinSqX_2*SinSqY_2 / (SinSqX_2*SinSqPhi + SinSqY_2*CosSqPhi);
			const float R			= FMath::Sqrt(RSq);
			const float Sqr			= FMath::Sqrt(1-RSq);
			const float Alpha		= R*CosPhi;
			const float Beta		= R*SinPhi;

			ConeVerts[i].X = (1 - 2*RSq);
			ConeVerts[i].Y = 2 * Sqr * Alpha;
			ConeVerts[i].Z = 2 * Sqr * Beta;
		}

		// Calculate transform for cone.
		FVector YAxis, ZAxis;
		FVector DirectionNorm = Direction.SafeNormal();
		DirectionNorm.FindBestAxisVectors(YAxis, ZAxis);
		const FMatrix ConeToWorld = FScaleMatrix(FVector(Length)) * FMatrix(DirectionNorm, YAxis, ZAxis, Origin);

		// this means foreground lines can't be persistent 
		ULineBatchComponent* const LineBatcher = GetDebugLineBatcher( InWorld, bPersistentLines, LifeTime, (DepthPriority == SDPG_Foreground) );
		if(LineBatcher != NULL)
		{
			float const LineLifeTime = (LifeTime > 0.f) ? LifeTime : LineBatcher->DefaultLifeTime;

			TArray<FBatchedLine> Lines;
			Lines.Empty(NumSides);

			FVector CurrentPoint, PrevPoint, FirstPoint;
			for(int32 i = 0; i < NumSides; i++)
			{
				CurrentPoint = ConeToWorld.TransformPosition(ConeVerts[i]);
				Lines.Add(FBatchedLine(ConeToWorld.GetOrigin(), CurrentPoint, DrawColor, LineLifeTime, 0.0f, DepthPriority));

				// PrevPoint must be defined to draw junctions
				if( i > 0 )
				{
					Lines.Add(FBatchedLine(PrevPoint, CurrentPoint, DrawColor, LineLifeTime, 0.0f, DepthPriority));
				}
				else
				{
					FirstPoint = CurrentPoint;
				}

				PrevPoint = CurrentPoint;
			}
			// Connect last junction to first
			Lines.Add(FBatchedLine(CurrentPoint, FirstPoint, DrawColor, LineLifeTime, 0.0f, DepthPriority));

			LineBatcher->DrawLines(Lines);
		}
	}
}

//.........这里部分代码省略.........
			float DesiredLength = DesiredDelta.Size();

			// Check to handle case where DesiredPos is the same as RootPos.
			FVector	DesiredDir;
			if (DesiredLength < (float)KINDA_SMALL_NUMBER)
			{
				DesiredLength = (float)KINDA_SMALL_NUMBER;
				DesiredDir = FVector(1, 0, 0);
			}
			else
			{
				DesiredDir = DesiredDelta / DesiredLength;
			}

			FVector	JointTargetPos = JointTargetTransform.GetTranslation();
			FVector JointTargetDelta = JointTargetPos - RootPos;
			float JointTargetLength = JointTargetDelta.Size();

			// Same check as above, to cover case when JointTarget position is the same as RootPos.
			FVector JointPlaneNormal, JointBendDir;
			if (JointTargetLength < (float)KINDA_SMALL_NUMBER)
			{
				JointBendDir = FVector(0, 1, 0);
				JointPlaneNormal = FVector(0, 0, 1);
			}
			else
			{
				JointPlaneNormal = DesiredDir ^ JointTargetDelta;

				// If we are trying to point the limb in the same direction that we are supposed to displace the joint in,
				// we have to just pick 2 random vector perp to DesiredDir and each other.
				if (JointPlaneNormal.Size() < (float)KINDA_SMALL_NUMBER)
				{
					DesiredDir.FindBestAxisVectors(JointPlaneNormal, JointBendDir);
				}
				else
				{
					JointPlaneNormal.Normalize();

					// Find the final member of the reference frame by removing any component of JointTargetDelta along DesiredDir.
					// This should never leave a zero vector, because we've checked DesiredDir and JointTargetDelta are not parallel.
					JointBendDir = JointTargetDelta - ((JointTargetDelta | DesiredDir) * DesiredDir);
					JointBendDir.Normalize();
				}
			}

			// Find lengths of upper and lower limb in the ref skeleton.
			// Use actual sizes instead of ref skeleton, so we take into account translation and scaling from other bone controllers.
			float LowerLimbLength = (InitialEndPos - InitialJointPos).Size();
			float UpperLimbLength = (InitialJointPos - RootPos).Size();
			float MaxLimbLength = LowerLimbLength + UpperLimbLength;

			FVector OutEndPos = DesiredPos;
			FVector OutJointPos = InitialJointPos;

			// If we are trying to reach a goal beyond the length of the limb, clamp it to something solvable and extend limb fully.
			if (DesiredLength > MaxLimbLength)
			{
				OutEndPos = RootPos + (MaxLimbLength * DesiredDir);
				OutJointPos = RootPos + (UpperLimbLength * DesiredDir);
			}
			else
			{
				// So we have a triangle we know the side lengths of. We can work out the angle between DesiredDir and the direction of the upper limb
				// using the sin rule:
				const float TwoAB = 2.f * UpperLimbLength * DesiredLength;