[Unreal] [Example] Animation에서 Physics Velocity 계산

Animation 데이터의 delta transform을 계산하여 각 body에 대한 물리 선형속도, 각속도를 만들어보는 예제이다.

 

TestUtils.h

// Fill out your copyright notice in the Description page of Project Settings.

#pragma once

#include "CoreMinimal.h"


class UWorld;
class USkinnedMeshComponent;
class USkeletalMeshComponent;
class PHYSICSANIMATIONCPP_API TestUtils
{
public:
	TestUtils();
	~TestUtils();

public:
	static const TArray<FName> BoneNames;
	static const TArray<FName> ParentBoneNames;

	static const TArray<FTransform> RunAnimPoseData;
	static int DrawCnt;

	static void DrawTransform(
		const UWorld* InWorld, FTransform T, FColor SphereColor = FColor::White, float Length = 50);

	static void GetBoneTransforms(USkinnedMeshComponent* Mesh, FTransform Parent, TArray<FTransform>& OutArray);

	static FVector QuatToRotationVector(FQuat quat);
	static FQuat RotationVectorToQuat(FVector rv);
};

 

 

TestUtils.cpp

// Fill out your copyright notice in the Description page of Project Settings.
#include "TestUtils.h"
#include "DrawDebugHelpers.h"
#include "Components/SkeletalMeshComponent.h"
#include "Components/SkinnedMeshComponent.h"
#include <cmath>


void TestUtils::DrawTransform(const UWorld* InWorld, FTransform T, FColor SphereColor, float Length)
{
    FVector Loc = T.GetLocation();
    DrawDebugSphere(InWorld, Loc, 10, 26, SphereColor, true, -1, 0, 2);

    FVector Forward = T.GetUnitAxis(EAxis::X);
    FVector Right = T.GetUnitAxis(EAxis::Y);
    FVector Up = T.GetUnitAxis(EAxis::Z);

    DrawDebugDirectionalArrow(InWorld, Loc, Loc + Forward * Length, 25.f, FColor::Red, true, -1.f, 0, 5.f);
    DrawDebugDirectionalArrow(InWorld, Loc, Loc + Right * Length, 25.f, FColor::Green, true, -1.f, 0, 5.f);
    DrawDebugDirectionalArrow(InWorld, Loc, Loc + Up * Length, 25.f, FColor::Blue, true, -1.f, 0, 5.f);
}

void TestUtils::GetBoneTransforms(USkinnedMeshComponent* Mesh, FTransform Parent, TArray<FTransform>& OutArray)
{
	FTransform RootInverse = Parent.Inverse();

	for (int i = 0; i < BoneNames.Num(); ++i)
	{
		int BoneIdx = Mesh->GetBoneIndex(BoneNames[i]);
		FTransform Trans = Mesh->GetBoneTransform(BoneIdx);

		FTransform Relative = Trans * RootInverse;
		OutArray.Add(Relative);
	}
}

FVector TestUtils::QuatToRotationVector(FQuat quat) 
{
	FVector axis = quat.GetRotationAxis();
	float angle = quat.GetAngle();

	if (angle < 0 || angle > 6.29) {
		UE_LOG(LogTemp, Warning, TEXT("QuatToRotationVector: Rotation angle error!!"));
	}

	angle = std::fmod(angle + PI, 2 * PI) - PI;

	return axis * angle;
}

FQuat TestUtils::RotationVectorToQuat(FVector rv) 
{
	float angle = rv.Size();
	FVector axis = rv.GetSafeNormal();
	return FQuat(axis, angle);
}


int TestUtils::DrawCnt = 0;

const TArray<FName> TestUtils::BoneNames =
{
    FName(TEXT("pelvis")),
    FName(TEXT("spine_01")),
    FName(TEXT("spine_02")),
    FName(TEXT("upperarm_l")),
    FName(TEXT("lowerarm_l")),
    FName(TEXT("hand_l")),
    FName(TEXT("upperarm_r")),
    FName(TEXT("lowerarm_r")),
    FName(TEXT("hand_r")),
    FName(TEXT("neck_01")),
    FName(TEXT("head")),
    FName(TEXT("thigh_l")),
    FName(TEXT("calf_l")),
    FName(TEXT("foot_l")),
    FName(TEXT("ball_l")),
    FName(TEXT("thigh_r")),
    FName(TEXT("calf_r")),
    FName(TEXT("foot_r")),
    FName(TEXT("ball_r"))

};


const TArray<FName> TestUtils::ParentBoneNames = 
{
	FName(TEXT("root")),
	FName(TEXT("pelvis")),
	FName(TEXT("spine_01")),

	FName(TEXT("spine_02")),
	FName(TEXT("upperarm_l")),
	FName(TEXT("lowerarm_l")),

	FName(TEXT("spine_02")),
	FName(TEXT("upperarm_r")),
	FName(TEXT("lowerarm_r")),

	FName(TEXT("spine_02")),
	FName(TEXT("neck_01")),

	FName(TEXT("pelvis")),
	FName(TEXT("thigh_l")),
	FName(TEXT("calf_l")),
	FName(TEXT("foot_l")),

	FName(TEXT("pelvis")),
	FName(TEXT("thigh_r")),
	FName(TEXT("calf_r")),
	FName(TEXT("foot_r")),
};


const TArray<FTransform> TestUtils::RunAnimPoseData =
{
FTransform(FQuat(-0.210569, -0.722517, 0.012822, 0.658381), FVector(-2.170135, -14.501923, 89.277664)),
FTransform(FQuat(-0.333414, -0.637773, 0.247801, 0.648595), FVector(-2.405243, -10.254883, 99.250824)),
FTransform(FQuat(-0.211386, -0.640788, 0.239675, 0.698042), FVector(-1.597931, 1.350677, 114.593155)),
FTransform(FQuat(-0.073529, 0.604244, -0.038889, 0.792446), FVector(18.269012, 14.343872, 134.843353)),
FTransform(FQuat(0.471670, 0.384680, 0.649377, 0.455915), FVector(26.362305, 9.777924, 105.961502)),
FTransform(FQuat(0.515488, 0.780751, 0.353065, 0.006741), FVector(22.603638, 35.539307, 113.024170)),
FTransform(FQuat(-0.751374, -0.258965, -0.472291, -0.381204), FVector(-15.459869, 7.944244, 142.091064)),
FTransform(FQuat(-0.717102, 0.342602, -0.604006, 0.059709), FVector(-28.195313, -14.787659, 126.547974)),
FTransform(FQuat(-0.528727, -0.020096, -0.754025, 0.389218), FVector(-29.155670, 0.412628, 104.283844)),
FTransform(FQuat(-0.178228, -0.710655, 0.248566, 0.633577), FVector(0.230164, 17.749115, 139.596313)),
FTransform(FQuat(-0.039714, -0.695251, 0.047908, 0.716068), FVector(-0.987976, 22.728851, 147.344604)),
FTransform(FQuat(-0.253225, -0.702817, 0.096461, 0.657739), FVector(6.663513, -16.813293, 88.945328)),
FTransform(FQuat(-0.281557, -0.669788, 0.197346, 0.658152), FVector(6.940765, -37.368530, 51.665527)),
FTransform(FQuat(-0.270941, -0.613615, 0.165374, 0.722993), FVector(5.940735, -62.971191, 20.693329)),
FTransform(FQuat(-0.629436, -0.231986, 0.634510, 0.383914), FVector(5.400604, -55.694672, 2.503357)),
FTransform(FQuat(-0.706084, -0.307633, 0.636741, 0.036988), FVector(-10.569733, -12.152893, 86.554657)),
FTransform(FQuat(-0.575821, 0.448882, 0.523413, -0.439289), FVector(-10.576263, 8.347076, 49.243164)),
FTransform(FQuat(-0.614030, 0.368417, 0.626013, -0.308776), FVector(-8.603088, -30.917572, 40.865875)),
FTransform(FQuat(-0.162227, 0.697397, 0.213437, -0.664654), FVector(-8.009277, -30.778839, 21.276581)),
};

 

TestPhysicsDataActor.h

// Fill out your copyright notice in the Description page of Project Settings.

#pragma once

#include "CoreMinimal.h"
#include "GameFramework/Actor.h"
#include "TestPhysicsDataActor.generated.h"

class UInputComponent;
class USkeletalMeshComponent;

UENUM(BlueprintType)
enum EVelocityType
{
	None		UMETA(DisplayName = "None"),
	Copy		UMETA(DisplayName = "Copy"),
	Make		UMETA(DisplayName = "Make"),
};

UCLASS()
class PHYSICSANIMATIONCPP_API ATestPhysicsDataActor : public AActor
{
	GENERATED_BODY()
	
public:
	UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "My")
		TEnumAsByte<EVelocityType> BodyVelocityType;

public:	
	// Sets default values for this actor's properties
	ATestPhysicsDataActor();

protected:
	// Called when the game starts or when spawned
	virtual void BeginPlay() override;

	virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override;

public:	
	// Called every frame
	virtual void Tick(float DeltaTime) override;

	void StartRagdoll();
	void OnRagdollKeyPress();

	void TestSetBodyTransform();
	void TestCopyBodyVelocity();
	void TestMakeBodyVelocity(float DeltaTime);

private:
	USkeletalMeshComponent* AnimMesh;
	USkeletalMeshComponent* PhysicsMesh;
	UInputComponent* InputComponent;


	TArray<FTransform> CurrentAnimPose;
	TArray<FTransform> PrevAnimPose;

	bool bSimulate;
};

 

 

 

TestPhysicsDataActor.cpp

// Fill out your copyright notice in the Description page of Project Settings.


#include "TestPhysicsDataActor.h"
#include "Components/SkeletalMeshComponent.h"
#include "Components/InputComponent.h"
#include "GameFramework/PlayerController.h"
#include "TestUtils.h"



// Sets default values
ATestPhysicsDataActor::ATestPhysicsDataActor()
{
 	// Set this actor to call Tick() every frame.  You can turn this off to improve performance if you don't need it.
	PrimaryActorTick.bCanEverTick = true;
	bSimulate = true;
}

// Called when the game starts or when spawned
void ATestPhysicsDataActor::BeginPlay()
{
	Super::BeginPlay();

	// set input
	GetWorld()->GetFirstPlayerController()->InputComponent->BindAction(
		"RagdollAction", IE_Pressed, this, &ATestPhysicsDataActor::OnRagdollKeyPress);

	TArray<USkeletalMeshComponent*> Meshs;
	GetComponents(Meshs);

	for (USkeletalMeshComponent* Mesh : Meshs)
	{
		FName Name = Mesh->GetFName();

		if (Name == FName(TEXT("PhysicsMesh")))
		{
			PhysicsMesh = Mesh;
		}
		else if (Name == FName(TEXT("AnimMesh")))
		{
			AnimMesh = Mesh;
		}
	}

	if (PhysicsMesh == nullptr)
	{
		UE_LOG(LogTemp, Error, TEXT("PhysicsMesh is null!!"));
	}
	if (AnimMesh == nullptr)
	{
		UE_LOG(LogTemp, Error, TEXT("AnimMesh is null!!"));
	}

	TestSetBodyTransform();
}

void ATestPhysicsDataActor::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
}

// Called every frame
void ATestPhysicsDataActor::Tick(float DeltaTime)
{
	Super::Tick(DeltaTime);


	if (bSimulate)
	{
		TestSetBodyTransform();

		if (BodyVelocityType == EVelocityType::Make)
		{
			TestMakeBodyVelocity(DeltaTime);
		}
		else if (BodyVelocityType == EVelocityType::Copy)
		{
			TestCopyBodyVelocity();
		}
		else
		{
		}
	}
}

void ATestPhysicsDataActor::StartRagdoll()
{
	AnimMesh->SetAllBodiesBelowSimulatePhysics(FName(TEXT("pelvis")), true, true);
}

void ATestPhysicsDataActor::OnRagdollKeyPress()
{
	StartRagdoll();
	bSimulate = false;
	PhysicsMesh->SetEnableGravity(true);
}

void ATestPhysicsDataActor::TestSetBodyTransform()
{
	const TArray<FName> BoneNames = TestUtils::BoneNames;
	
	{
		// get bone transforms
		PrevAnimPose.Empty();
		PrevAnimPose.Append(CurrentAnimPose);
		CurrentAnimPose.Empty();

		TestUtils::GetBoneTransforms(AnimMesh, GetActorTransform(), CurrentAnimPose);

		// set body transforms
		for (int i = 0; i < BoneNames.Num(); ++i)
		{
			FName BName = BoneNames[i];
			FBodyInstance* Body = PhysicsMesh->GetBodyInstance(BName);
			if (Body != nullptr)
			{
				FTransform Trans = GetActorTransform();
				Trans = CurrentAnimPose[i] * Trans;
				Body->SetBodyTransform(Trans, ETeleportType::ResetPhysics);
			}
		}
	}
}

void ATestPhysicsDataActor::TestCopyBodyVelocity()
{
	const TArray<FName> BoneNames = TestUtils::BoneNames;
	for (int i = 0; i < BoneNames.Num(); ++i)
	{
		FName BName = BoneNames[i];
		FBodyInstance* AnimBody = AnimMesh->GetBodyInstance(BName);
		FBodyInstance* Body = PhysicsMesh->GetBodyInstance(BName);
		if (AnimBody != nullptr && Body != nullptr)
		{
			Body->SetLinearVelocity(AnimBody->GetUnrealWorldVelocity(), false);

			Body->SetAngularVelocityInRadians(AnimBody->GetUnrealWorldAngularVelocityInRadians(), false);
		}
	}
	
}

void ATestPhysicsDataActor::TestMakeBodyVelocity(float DeltaTime)
{
	const TArray<FName> BoneNames = TestUtils::BoneNames;

	for (int i = 0; i < BoneNames.Num(); ++i)
	{
		FName BName = BoneNames[i];
		FBodyInstance* AnimBody = AnimMesh->GetBodyInstance(BName);
		FBodyInstance* Body = PhysicsMesh->GetBodyInstance(BName);
		if (AnimBody != nullptr && Body != nullptr)
		{

			FTransform PrevT = PrevAnimPose[i];
			FTransform CurT = CurrentAnimPose[i];

			FVector AV = TestUtils::QuatToRotationVector(
				CurT.GetRotation() * PrevT.GetRotation().Inverse()) / DeltaTime;

			FVector LV = (CurT.GetTranslation() - PrevT.GetTranslation()) / DeltaTime;

			Body->SetAngularVelocityInRadians(AV, false);
			Body->SetLinearVelocity(LV, false);

		}
	}

}