From b060a9d1cb45703a10eb00d8bc3d99924cf8acbf Mon Sep 17 00:00:00 2001
From: pnossol <patrick.nossol@gmail.com>
Date: Tue, 31 May 2022 14:46:01 +0200
Subject: [PATCH] Fixed Offset Pullup! (not beautiful, create new branch for
 that)

---
 Content/MoCapMap.umap                         |  4 +-
 Content/SaveSequenceAnimBP.uasset             |  4 +-
 Content/SaveSequenceRig.uasset                |  4 +-
 Source/MoCapPlugin/Private/MCAnimInstance.cpp |  3 +-
 Source/MoCapPlugin/Private/MCController.cpp   |  5 +-
 Source/MoCapPlugin/Private/MCPawn.cpp         | 81 ++++++++++++++++++-
 Source/MoCapPlugin/Private/MCRigUnits.cpp     | 50 +++++++++++-
 Source/MoCapPlugin/Public/MCAnimInstance.h    |  2 +-
 Source/MoCapPlugin/Public/MCDefines.h         |  2 +
 Source/MoCapPlugin/Public/MCPawn.h            |  4 +-
 Source/MoCapPlugin/Public/MCRigUnits.h        | 27 +++++++
 11 files changed, 170 insertions(+), 16 deletions(-)

diff --git a/Content/MoCapMap.umap b/Content/MoCapMap.umap
index 6d1167a..3a13063 100644
--- a/Content/MoCapMap.umap
+++ b/Content/MoCapMap.umap
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:92a5fa84a7731e65598ce589a146da254ef412238115d8187d6ec7ab5c455170
-size 39985
+oid sha256:9a1540b30dea1f08211b890e124aba4d3bef862ea82618770c9118f567cb2e8d
+size 35280
diff --git a/Content/SaveSequenceAnimBP.uasset b/Content/SaveSequenceAnimBP.uasset
index ef9d83b..7bc7422 100644
--- a/Content/SaveSequenceAnimBP.uasset
+++ b/Content/SaveSequenceAnimBP.uasset
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:5de4bf55a201aed7ed8fc6a0c49455b41256b41f52e787ad1e3baa000fdeb2bf
-size 204376
+oid sha256:05c883fe924dd31377fef6dec79b577739c1f2e050048659540953e6f30d2851
+size 206275
diff --git a/Content/SaveSequenceRig.uasset b/Content/SaveSequenceRig.uasset
index dc8dfa1..3f03ff9 100644
--- a/Content/SaveSequenceRig.uasset
+++ b/Content/SaveSequenceRig.uasset
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:62d3c42edf0f9d2db2d91756dcc93681e4c7f7d801973e757ae9840ae0efbc4a
-size 4560157
+oid sha256:ccbeedce4980c6a425a2500298b6f7424dfe0fc1ebe811591bf064c85d62981c
+size 4552248
diff --git a/Source/MoCapPlugin/Private/MCAnimInstance.cpp b/Source/MoCapPlugin/Private/MCAnimInstance.cpp
index aa31c1d..e2c7ba1 100644
--- a/Source/MoCapPlugin/Private/MCAnimInstance.cpp
+++ b/Source/MoCapPlugin/Private/MCAnimInstance.cpp
@@ -199,7 +199,7 @@ void UMCAnimInstance::SetSensorData(EBodyPart BodyPart, const FQuat& Rot) {
 	entry->valid = true;
 }
 
-void UMCAnimInstance::SetSensorOffset(EBodyPart BodyPart, float Dist, const FQuat& Rot, const FQuat& AxisRotDiff) {
+void UMCAnimInstance::SetSensorOffset(EBodyPart BodyPart, float Dist, const FQuat& Rot, const FQuat& AxisRotDiff, const FTransform& DeltaTransform) {
 
 	FSensorOffset* entry = nullptr;
 
@@ -237,5 +237,6 @@ void UMCAnimInstance::SetSensorOffset(EBodyPart BodyPart, float Dist, const FQua
 	entry->Distance = Dist;
 	entry->Rot = Rot;
 	entry->AxisRotDiff = AxisRotDiff;
+	entry->DeltaTransform = DeltaTransform;
 
 }
diff --git a/Source/MoCapPlugin/Private/MCController.cpp b/Source/MoCapPlugin/Private/MCController.cpp
index d2a6025..5da59f8 100644
--- a/Source/MoCapPlugin/Private/MCController.cpp
+++ b/Source/MoCapPlugin/Private/MCController.cpp
@@ -346,7 +346,7 @@ void AMCController::PreprocessRecording() {
 		float diff = OffsetPos.Z;
 		FQuat OffsetRot = AnimSaveState.AnimData[0].SensorData.LowerBody.Rot;
 		OffsetRot = OffsetRot * FQuat(FVector(0, 0, 1), FMath::DegreesToRadians(180));
-		AnimSaveState.Pawn->OffsetSensorData(EBodyPart::LowerBody, OffsetPos, OffsetRot);
+		AnimSaveState.Pawn->OffsetSensorData(EBodyPart::LowerBody, OffsetPos, OffsetRot); 
 		diff = OffsetPos.Z - diff;
 		PullDownHip = diff;
 	}
@@ -962,7 +962,6 @@ void AMCController::SaveAnimation() {
 	AnimSaveState.CurrentMarker = 0;
 
 	AnimSaveState.Pawn = Pawn;
-	AnimSaveState.Pawn->GetAnimInstance()->DoRig = true;
 	AnimSaveState.Pawn->GetAnimInstance()->DoFingers = bFingerTrackingEnabled;
 	AnimSaveState.Pawn->GetAnimInstance()->SnapshotAnimations.Empty();
 
@@ -974,6 +973,8 @@ void AMCController::SaveAnimation() {
 
 	PreprocessRecording();
 
+	AnimSaveState.Pawn->GetAnimInstance()->DoRig = true;
+
 	InputNextFrame();
 	
 }
diff --git a/Source/MoCapPlugin/Private/MCPawn.cpp b/Source/MoCapPlugin/Private/MCPawn.cpp
index b8a6454..8c2dcae 100644
--- a/Source/MoCapPlugin/Private/MCPawn.cpp
+++ b/Source/MoCapPlugin/Private/MCPawn.cpp
@@ -153,6 +153,43 @@ USkeleton* AMCPawn::GetSkeleton() {
 	return SkeletalMesh->SkeletalMesh->Skeleton;
 }
 
+FName AMCPawn::GetBoneNameFromBodyPart(EBodyPart BodyPart) {
+	switch (BodyPart) {
+	case EBodyPart::Head:
+		return BoneNames.head;
+	case EBodyPart::HandL:
+		return BoneNames.hand_l;
+	case EBodyPart::HandR:
+		return BoneNames.hand_r;
+	case EBodyPart::FootL:
+		return BoneNames.foot_l;
+	case EBodyPart::FootR:
+		return BoneNames.foot_r;
+	case EBodyPart::UpperArmL:
+		return BoneNames.upperarm_l;
+	case EBodyPart::UpperArmR:
+		return BoneNames.upperarm_r;
+	case EBodyPart::LowerArmL:
+		return BoneNames.lowerarm_l;
+	case EBodyPart::LowerArmR:
+		return BoneNames.lowerarm_r;
+	case EBodyPart::UpperLegL:
+		return BoneNames.thigh_l;
+	case EBodyPart::UpperLegR:
+		return BoneNames.thigh_r;
+	case EBodyPart::LowerLegL:
+		return BoneNames.calf_l;
+	case EBodyPart::LowerLegR:
+		return BoneNames.calf_r;
+	case EBodyPart::UpperBody:
+		return BoneNames.spine03;
+	case EBodyPart::LowerBody:
+		return BoneNames.pelvis;
+	default:
+		return "";
+	}
+}
+
 void AMCPawn::SetMeshVisibility(bool visible) {
 	SkeletalMesh->SetVisibility(visible, true);
 }
@@ -433,6 +470,7 @@ void AMCPawn::InputViveOffsetsToAnimInstance(TSharedPtr<FJsonObject> Data) {
 
 	for (int i = 0; i < EBodyPart::LAST; i++) {
 
+		EBodyPart BodyPart = (EBodyPart)i;
 		FString Type = UEnum::GetDisplayValueAsText(EBodyPart(i)).ToString();
 		if (Data->HasField(Type)) {
 
@@ -443,15 +481,34 @@ void AMCPawn::InputViveOffsetsToAnimInstance(TSharedPtr<FJsonObject> Data) {
 				const float RotY = FCString::Atof(*JsonObj->GetStringField("Rot1Y"));
 				const float RotZ = FCString::Atof(*JsonObj->GetStringField("Rot1Z"));
 				const float RotW = FCString::Atof(*JsonObj->GetStringField("Rot1W"));
+				FQuat Rot(RotX, RotY, RotZ, RotW);
 
 				const float AxisRotX = FCString::Atof(*JsonObj->GetStringField("AxisRot1X"));
 				const float AxisRotY = FCString::Atof(*JsonObj->GetStringField("AxisRot1Y"));
 				const float AxisRotZ = FCString::Atof(*JsonObj->GetStringField("AxisRot1Z"));
 				const float AxisRotW = FCString::Atof(*JsonObj->GetStringField("AxisRot1W"));
+				FQuat AxisRot(AxisRotX, AxisRotY, AxisRotZ, AxisRotW);
 
 				const float Dist = FCString::Atof(*JsonObj->GetStringField("Dist1"));
 
-				AI->SetSensorOffset((EBodyPart)i, Dist, FQuat(RotX, RotY, RotZ, RotW), FQuat(AxisRotX, AxisRotY, AxisRotZ, AxisRotW));
+				AI->SetSensorOffset(BodyPart, Dist, Rot, AxisRot, FTransform::Identity);
+			
+				EBodyPart NamePart = BodyPart;
+				if (BodyPart == EBodyPart::LowerLegL) {
+					NamePart = EBodyPart::FootL;
+				}
+				if (BodyPart == EBodyPart::LowerLegR) {
+					NamePart = EBodyPart::FootR;
+				}
+				FName BoneName = GetBoneNameFromBodyPart(BodyPart);
+				FVector OffsetPos = SkeletalMesh->GetSocketLocation(BoneName);
+				FQuat OffsetRot = SkeletalMesh->GetSocketQuaternion(BoneName);
+				OffsetSensorData(BodyPart, OffsetPos, OffsetRot, true);
+			
+				FTransform DataTransform(OffsetRot, OffsetPos);
+				FTransform FootTransform(SkeletalMesh->GetSocketRotation(BoneName), SkeletalMesh->GetSocketLocation(BoneName));
+				FTransform DeltaTransform = FootTransform.GetRelativeTransform(DataTransform);
+				AI->SetSensorOffset(BodyPart, Dist, Rot, AxisRot, DeltaTransform);
 			}
 
 		}
@@ -487,7 +544,7 @@ void AMCPawn::InputFingerDataToAnimInstance(TSharedPtr<FJsonObject> Data) {
 
 }
 
-void AMCPawn::OffsetSensorData(EBodyPart part, FVector & Pos, FQuat & Rot) {
+void AMCPawn::OffsetSensorData(EBodyPart part, FVector & Pos, FQuat & Rot, bool Inverse) {
 
 	FSensorOffset* Offset = nullptr;
 	UMCAnimInstance* AI = GetAnimInstance();
@@ -526,8 +583,24 @@ void AMCPawn::OffsetSensorData(EBodyPart part, FVector & Pos, FQuat & Rot) {
 
 	FQuat RotTmp = Rot;
 
-	Rot = Rot * Offset->Rot;
-	Pos = Pos - Offset->Distance * (Offset->AxisRotDiff * RotTmp).GetAxisX();
+	/*
+	* ROT
+	* FootRot = DataRot * RotOffset
+	* DataRot = FootRot * Inv(RotOffset)
+	* 
+	* POS
+	* FootPos = DataPos - DistOffset * (AxisRotOffset * DataRot).AxisX()
+	* DataPos = FootPos + DistOffset * (AxisRotOffset * DataRot).AxisX()
+	*/
+
+	if (!Inverse) {
+		Rot = Rot * Offset->Rot;
+		Pos = Pos - Offset->Distance * (Offset->AxisRotDiff * RotTmp).GetAxisX();
+	}
+	else {
+		Rot = Rot * Offset->Rot.Inverse();
+		Pos = Pos + Offset->Distance * (Offset->AxisRotDiff * Rot).GetAxisX();
+	}
 
 }
 
diff --git a/Source/MoCapPlugin/Private/MCRigUnits.cpp b/Source/MoCapPlugin/Private/MCRigUnits.cpp
index 950e55b..0da6b57 100644
--- a/Source/MoCapPlugin/Private/MCRigUnits.cpp
+++ b/Source/MoCapPlugin/Private/MCRigUnits.cpp
@@ -16,8 +16,39 @@ FRigUnit_SensorOffsets_Execute() {
 
 	OutTrans.SetRotation(ControlTrans.GetRotation() * SensorOffset.Rot);
 
-	OutTrans.SetTranslation(ControlTrans.GetTranslation() - SensorOffset.Distance * (SensorOffset.AxisRotDiff * ControlTrans.GetRotation()).GetAxisX());
+	int option = 2;
 
+	//original
+	if (option == 0) {
+		OutTrans.SetTranslation(ControlTrans.GetTranslation() - SensorOffset.Distance * (SensorOffset.AxisRotDiff * ControlTrans.GetRotation()).GetAxisX());
+	}
+	//no trans offset
+	else if (option == 1) {
+		
+		FQuat AxisRotDiff = SensorOffset.AxisRotDiff;
+		if (!AxisRotDiff.IsNormalized()) {
+			AxisRotDiff = AxisRotDiff.GetNormalized();
+		}
+		
+		FQuat ControlRot = ControlTrans.GetRotation();
+		if (!ControlRot.IsNormalized()) {
+			ControlRot = ControlRot.GetNormalized();
+		}
+		
+		check(AxisRotDiff.IsNormalized());
+		check(ControlRot.IsNormalized());
+		FQuat OffsetDir = AxisRotDiff * ControlRot;
+		check(OffsetDir.IsNormalized());
+		FVector OffsetVec = AxisRotDiff.RotateVector(ControlRot.GetAxisX());
+		check(OffsetVec.IsUnit());
+		OutTrans.SetTranslation(ControlTrans.GetTranslation() - SensorOffset.Distance * OffsetVec);
+	}
+	//use delta trans
+	else if (option == 2) {
+		//NewControl = Bone * Control(-1) * NewControl
+		OutTrans = SensorOffset.DeltaTransform * ControlTrans;
+
+	}
 }
 
 FRigUnit_ApplyFingerData::FRigUnit_ApplyFingerData() {
@@ -224,4 +255,21 @@ FRigUnit_FootLocking_Execute() {
 		}
 
 	}
+}
+
+FRigUnit_FromXYZW::FRigUnit_FromXYZW() {
+
+	X = 0.f;
+	Y = 0.f;
+	Z = 0.f;
+	W = 0.f;
+	Quat = FQuat::Identity;
+
+}
+
+FRigUnit_FromXYZW_Execute() {
+	DECLARE_SCOPE_HIERARCHICAL_COUNTER_RIGUNIT()
+
+	Quat = FQuat(X, Y, Z, W);
+	
 }
\ No newline at end of file
diff --git a/Source/MoCapPlugin/Public/MCAnimInstance.h b/Source/MoCapPlugin/Public/MCAnimInstance.h
index 6e3bac1..c32b741 100644
--- a/Source/MoCapPlugin/Public/MCAnimInstance.h
+++ b/Source/MoCapPlugin/Public/MCAnimInstance.h
@@ -57,6 +57,6 @@ public:
 	void SetSensorData(EBodyPart BodyPart, const FVector& Pos, const FQuat& Rot);
 	void SetSensorData(EBodyPart BodyPart, const FQuat& Rot);
 
-	void SetSensorOffset(EBodyPart BodyPart, float Dist, const FQuat& Rot, const FQuat& AxisRotDiff);
+	void SetSensorOffset(EBodyPart BodyPart, float Dist, const FQuat& Rot, const FQuat& AxisRotDiff, const FTransform& DeltaTransform);
 
 };
diff --git a/Source/MoCapPlugin/Public/MCDefines.h b/Source/MoCapPlugin/Public/MCDefines.h
index 335a5ab..f3ec181 100644
--- a/Source/MoCapPlugin/Public/MCDefines.h
+++ b/Source/MoCapPlugin/Public/MCDefines.h
@@ -230,6 +230,8 @@ struct FSensorOffset {
 	FQuat Rot;
 	UPROPERTY(BlueprintReadOnly)
 	FQuat AxisRotDiff;
+	UPROPERTY(BlueprintReadOnly)
+	FTransform DeltaTransform;
 
 };
 
diff --git a/Source/MoCapPlugin/Public/MCPawn.h b/Source/MoCapPlugin/Public/MCPawn.h
index 47acc9b..2a83c6f 100644
--- a/Source/MoCapPlugin/Public/MCPawn.h
+++ b/Source/MoCapPlugin/Public/MCPawn.h
@@ -53,8 +53,10 @@ protected:
 
 public:
 
+	UFUNCTION(BlueprintCallable)
 	UMCAnimInstance* GetAnimInstance();
 	USkeleton* GetSkeleton();
+	FName GetBoneNameFromBodyPart(EBodyPart BodyPart);
 	void SetMeshVisibility(bool visible);
 	void LoadMeasurementFile(const FString& Filepath);
 
@@ -66,7 +68,7 @@ public:
 	void InputViveOffsetsToAnimInstance(TSharedPtr<FJsonObject> Data);
 	void InputFingerDataToAnimInstance(TSharedPtr<FJsonObject> Data);
 
-	void OffsetSensorData(EBodyPart part, FVector& Pos, FQuat& Rot);
+	void OffsetSensorData(EBodyPart part, FVector& Pos, FQuat& Rot, bool Inverse = false);
 
 private:
 
diff --git a/Source/MoCapPlugin/Public/MCRigUnits.h b/Source/MoCapPlugin/Public/MCRigUnits.h
index a6668e4..0e803b9 100644
--- a/Source/MoCapPlugin/Public/MCRigUnits.h
+++ b/Source/MoCapPlugin/Public/MCRigUnits.h
@@ -104,4 +104,31 @@ struct FRigUnit_FootLocking : public FRigUnitMutable
 	UPROPERTY()
 	FTransform LockedTransRight;
 
+};
+
+USTRUCT(meta = (DisplayName = "FromXYZW", PrototypeName = "FromXYZW", Category = "Math|Quaternion", NodeColor = "0.05 0.25 0.05"))
+struct FRigUnit_FromXYZW : public FRigUnit
+{
+	GENERATED_BODY()
+
+	FRigUnit_FromXYZW();
+
+	RIGVM_METHOD()
+		virtual void Execute(const FRigUnitContext& Context) override;
+
+	UPROPERTY(meta = (Input))
+	float X;
+
+	UPROPERTY(meta = (Input))
+	float Y;
+
+	UPROPERTY(meta = (Input))
+	float Z;
+
+	UPROPERTY(meta = (Input))
+	float W;
+
+	UPROPERTY(meta = (Output))
+	FQuat Quat;
+
 };
\ No newline at end of file
-- 
GitLab