Skip to content

Commit fc7df64

Browse files
mongurimonguri
committed
回転角コンストレイント（LookAtに使う）を実装した
1 parent 0602f73 commit fc7df64

File tree

4 files changed

+101
-75
lines changed

4 files changed

+101
-75
lines changed

‎Content/IKTest/NewAnimBlueprint.uasset

962 Bytes
Binary file not shown.

‎Plugins/IKPluginSandbox/Source/IKPluginSandbox/Private/AnimNode_JacobianIK.cpp

Lines changed: 88 additions & 70 deletions
Original file line numberDiff line numberDiff line change
@@ -42,11 +42,14 @@ void FAnimNode_JacobianIK::InitializeBoneReferences(const FBoneContainer& Requir
4242

4343
for (FIKConstraint& IKConstraint : IKJoint.Constraints)
4444
{
45-
// EffectiveRootJointはすでにどれかのFIKJointで指定されておりInitializeされているはず
46-
if (!IKConstraint.EffectiveRootJoint.Initialize(RequiredBones))
45+
if (IKConstraint.Type == EIKConstraintType::KEEP_POSITION)
4746
{
48-
// TODO:UE_LOG
49-
return;
47+
// EffectiveRootJointはすでにどれかのFIKJointで指定されておりInitializeされているはず
48+
if (!IKConstraint.EffectiveRootJoint.Initialize(RequiredBones))
49+
{
50+
// TODO:UE_LOG
51+
return;
52+
}
5053
}
5154
}
5255
}
@@ -103,42 +106,44 @@ void FAnimNode_JacobianIK::InitializeBoneReferences(const FBoneContainer& Requir
103106
continue;
104107
}
105108

106-
if (!IKConstraint.EffectiveRootJoint.IsValidToEvaluate(RequiredBones))
107-
{
108-
// TODO:UE_LOG
109-
return;
110-
}
111-
112109
const FCompactPoseBoneIndex& ConstraintJointIndex = IKJoint.Joint.GetCompactPoseIndex(RequiredBones);
113-
const FCompactPoseBoneIndex& EffectiveRootIndex = IKConstraint.EffectiveRootJoint.GetCompactPoseIndex(RequiredBones);
114-
if (ConstraintJointIndex == EffectiveRootIndex)
110+
TArray<FCompactPoseBoneIndex> EffectiveJointIndices;
111+
112+
if (IKConstraint.Type == EIKConstraintType::KEEP_POSITION)
115113
{
116-
// TODO:UE_LOG
117-
return;
118-
}
114+
// PositionコンストレイントのときはIK計算に加えるジョイントをすべて洗い出す
119115

120-
TArray<FCompactPoseBoneIndex> EffectiveJointIndices;
116+
if (!IKConstraint.EffectiveRootJoint.IsValidToEvaluate(RequiredBones))
117+
{
118+
// TODO:UE_LOG
119+
return;
120+
}
121121

122-
FCompactPoseBoneIndex JointIndex = ConstraintJointIndex;
123-
IKJointWorkData WorkData = IKJointWorkDataMap[JointIndex.GetInt()];
122+
const FCompactPoseBoneIndex& EffectiveRootIndex = IKConstraint.EffectiveRootJoint.GetCompactPoseIndex(RequiredBones);
123+
if (ConstraintJointIndex == EffectiveRootIndex)
124+
{
125+
// TODO:UE_LOG
126+
return;
127+
}
124128

125-
for (JointIndex = WorkData.ParentJointIndex, WorkData = IKJointWorkDataMap[JointIndex.GetInt()]; // 自分はEffectiveJointIndicesには含めない
126-
JointIndex != EffectiveRootIndex && JointIndex != WorkData.ParentJointIndex; // IKスケルトンのルートに達したときもループは抜ける
127-
JointIndex = WorkData.ParentJointIndex, WorkData = IKJointWorkDataMap[JointIndex.GetInt()])
128-
{
129-
EffectiveJointIndices.Add(JointIndex);
130-
}
131-
EffectiveJointIndices.Add(JointIndex); // EffectiveRootIndexも含める
129+
FCompactPoseBoneIndex JointIndex = ConstraintJointIndex;
130+
IKJointWorkData WorkData = IKJointWorkDataMap[JointIndex.GetInt()];
132131

133-
EffectiveJointIndices.Sort(
134-
[](const FCompactPoseBoneIndex& A, const FCompactPoseBoneIndex& B)
135-
{
136-
return A.GetInt() < B.GetInt();
137-
}
138-
);
132+
for (JointIndex = WorkData.ParentJointIndex, WorkData = IKJointWorkDataMap[JointIndex.GetInt()]; // 自分はEffectiveJointIndicesには含めない
133+
JointIndex != EffectiveRootIndex && JointIndex != WorkData.ParentJointIndex; // IKスケルトンのルートに達したときもループは抜ける
134+
JointIndex = WorkData.ParentJointIndex, WorkData = IKJointWorkDataMap[JointIndex.GetInt()])
135+
{
136+
EffectiveJointIndices.Add(JointIndex);
137+
}
138+
EffectiveJointIndices.Add(JointIndex); // EffectiveRootIndexも含める
139+
140+
EffectiveJointIndices.Sort(
141+
[](const FCompactPoseBoneIndex& A, const FCompactPoseBoneIndex& B)
142+
{
143+
return A.GetInt() < B.GetInt();
144+
}
145+
);
139146

140-
if (IKConstraint.Type == EIKConstraintType::KEEP_POSITION)
141-
{
142147
NumPositionConstraint++;
143148
}
144149

@@ -195,34 +200,41 @@ void FAnimNode_JacobianIK::EvaluateSkeletalControl_AnyThread(FComponentSpacePose
195200

196201
const FBoneContainer& BoneContainer = Output.Pose.GetPose().GetBoneContainer();
197202

203+
// ワークデータのTransformの初期化
204+
for (TPair<int32, IKJointWorkData>& WorkData : IKJointWorkDataMap)
205+
{
206+
WorkData.Value.ComponentTransform = Output.Pose.GetComponentSpaceTransform(FCompactPoseBoneIndex(WorkData.Key));
207+
WorkData.Value.LocalTransform = WorkData.Value.ComponentTransform * Output.Pose.GetComponentSpaceTransform(FCompactPoseBoneIndex(WorkData.Value.ParentJointIndex)).Inverse();
208+
}
209+
198210
// TODO:各IKノードと共通化しよう
199-
// そもそもジョイントの長さ的にIKの解に到達しうるかの確認
200-
// コンストレイントごとに確認する
201211
uint32 PositionConstraintIndex = 0;
202212
for (const IKConstraintWorkData& Constraint : IKConstraintWorkDataArray)
203213
{
204-
float IKJointTotalLength = 0; // このアニメーションノードに入力されるポーズにScaleがないなら、一度だけ計算してキャッシュしておけばよいが、今は毎回計算する
205-
// TODO:だがチェック処理にしては毎フレームの計算コスト高すぎかも
206-
207-
// エフェクタのボーン以外の長さの合計
208-
for (int32 i = 0; i < Constraint.EffectiveJointIndices.Num() - 1; i++)
209-
{
210-
IKJointTotalLength += (Output.Pose.GetComponentSpaceTransform(Constraint.EffectiveJointIndices[i + 1]).GetLocation() - Output.Pose.GetComponentSpaceTransform(Constraint.EffectiveJointIndices[i]).GetLocation()).Size();
211-
}
212-
// エフェクタのボーンも合計する
213-
IKJointTotalLength += (Output.Pose.GetComponentSpaceTransform(Constraint.JointIndex).GetLocation() - Output.Pose.GetComponentSpaceTransform(Constraint.EffectiveJointIndices[Constraint.EffectiveJointIndices.Num() - 1]).GetLocation()).Size();
214-
215-
float EffectorToIKRootLength = (Constraint.Position - Output.Pose.GetComponentSpaceTransform(Constraint.EffectiveJointIndices[0]).GetLocation()).Size();
216-
if (IKJointTotalLength < EffectorToIKRootLength)
217-
{
218-
UE_LOG(LogAnimation, Warning, TEXT("IK cannot reach effector target location. The total length of joints is not enough."));
219-
return;
220-
}
221-
222214
switch (Constraint.Type)
223215
{
224216
case EIKConstraintType::KEEP_POSITION:
225217
{
218+
// そもそもジョイントの長さ的にIKの解に到達しうるかの確認
219+
// コンストレイントごとに確認する
220+
float IKJointTotalLength = 0; // このアニメーションノードに入力されるポーズにScaleがないなら、一度だけ計算してキャッシュしておけばよいが、今は毎回計算する
221+
// TODO:だがチェック処理にしては毎フレームの計算コスト高すぎかも
222+
223+
// エフェクタのボーン以外の長さの合計
224+
for (int32 i = 0; i < Constraint.EffectiveJointIndices.Num() - 1; i++)
225+
{
226+
IKJointTotalLength += (Output.Pose.GetComponentSpaceTransform(Constraint.EffectiveJointIndices[i + 1]).GetLocation() - Output.Pose.GetComponentSpaceTransform(Constraint.EffectiveJointIndices[i]).GetLocation()).Size();
227+
}
228+
// エフェクタのボーンも合計する
229+
IKJointTotalLength += (Output.Pose.GetComponentSpaceTransform(Constraint.JointIndex).GetLocation() - Output.Pose.GetComponentSpaceTransform(Constraint.EffectiveJointIndices[Constraint.EffectiveJointIndices.Num() - 1]).GetLocation()).Size();
230+
231+
float EffectorToIKRootLength = (Constraint.Position - Output.Pose.GetComponentSpaceTransform(Constraint.EffectiveJointIndices[0]).GetLocation()).Size();
232+
if (IKJointTotalLength < EffectorToIKRootLength)
233+
{
234+
UE_LOG(LogAnimation, Warning, TEXT("IK cannot reach effector target location. The total length of joints is not enough."));
235+
return;
236+
}
237+
226238
// ノードの入力されたエフェクタの位置から目標位置への差分ベクトル
227239
const FVector& DeltaLocation = Constraint.Position - Output.Pose.GetComponentSpaceTransform(Constraint.JointIndex).GetLocation();
228240

@@ -236,7 +248,13 @@ void FAnimNode_JacobianIK::EvaluateSkeletalControl_AnyThread(FComponentSpacePose
236248
break;
237249
case EIKConstraintType::KEEP_ROTATION:
238250
{
251+
IKJointWorkData& WorkData = IKJointWorkDataMap.FindChecked(Constraint.JointIndex.GetInt());
239252

253+
const FRotator& DeltaRotation = Constraint.Rotation - Output.Pose.GetComponentSpaceTransform(Constraint.JointIndex).GetRotation().Rotator();
254+
WorkData.LocalTransform.SetRotation(FQuat(Constraint.Rotation));
255+
WorkData.LocalTransform.NormalizeRotation();
256+
257+
WorkData.ComponentTransform = WorkData.LocalTransform * Output.Pose.GetComponentSpaceTransform(FCompactPoseBoneIndex(WorkData.ParentJointIndex));
240258
}
241259
break;
242260
case EIKConstraintType::INVALID:
@@ -247,13 +265,6 @@ void FAnimNode_JacobianIK::EvaluateSkeletalControl_AnyThread(FComponentSpacePose
247265
}
248266
}
249267

250-
// ワークデータのTransformの初期化
251-
for (TPair<int32, IKJointWorkData>& WorkData : IKJointWorkDataMap)
252-
{
253-
WorkData.Value.ComponentTransform = Output.Pose.GetComponentSpaceTransform(FCompactPoseBoneIndex(WorkData.Key));
254-
WorkData.Value.LocalTransform = WorkData.Value.ComponentTransform * Output.Pose.GetComponentSpaceTransform(FCompactPoseBoneIndex(WorkData.Value.ParentJointIndex)).Inverse();
255-
}
256-
257268
// JacobianIKのメインアルゴリズム
258269
// JacobianIKアルゴリズムについてはComputer Graphics Gems JP 2012の8章を参照
259270
uint32 iterCount = 0;
@@ -271,9 +282,14 @@ void FAnimNode_JacobianIK::EvaluateSkeletalControl_AnyThread(FComponentSpacePose
271282
IKJointWorkData& WorkData = WorkDataPair.Value;
272283
const FCompactPoseBoneIndex& JointIndex = FCompactPoseBoneIndex(WorkDataPair.Key);
273284

274-
for (int32 ConstraintIndex = 0; ConstraintIndex < IKConstraintWorkDataArray.Num(); ConstraintIndex++)
285+
PositionConstraintIndex = 0;
286+
for (IKConstraintWorkData& Constraint : IKConstraintWorkDataArray)
275287
{
276-
const IKConstraintWorkData& Constraint = IKConstraintWorkDataArray[ConstraintIndex];
288+
if (Constraint.Type != EIKConstraintType::KEEP_POSITION)
289+
{
290+
continue;
291+
}
292+
277293
bool bEffectiveJoint = false;
278294
for (const FCompactPoseBoneIndex& EffectiveJoint : Constraint.EffectiveJointIndices)
279295
{
@@ -339,21 +355,23 @@ void FAnimNode_JacobianIK::EvaluateSkeletalControl_AnyThread(FComponentSpacePose
339355
for (int32 RotAxis = 0; RotAxis < ROTATION_AXIS_COUNT; ++RotAxis)
340356
{
341357
const FVector& JacobianRow = (ChildRestMatrix * LocalMatrix[RotAxis] * ParentRestMatrix).TransformPosition(FVector::ZeroVector);
342-
Jacobian.Set(RotationIndex * ROTATION_AXIS_COUNT + RotAxis, ConstraintIndex * AXIS_COUNT + 0, JacobianRow.X);
343-
Jacobian.Set(RotationIndex * ROTATION_AXIS_COUNT + RotAxis, ConstraintIndex * AXIS_COUNT + 1, JacobianRow.Y);
344-
Jacobian.Set(RotationIndex * ROTATION_AXIS_COUNT + RotAxis, ConstraintIndex * AXIS_COUNT + 2, JacobianRow.Z);
358+
Jacobian.Set(RotationIndex * ROTATION_AXIS_COUNT + RotAxis, PositionConstraintIndex * AXIS_COUNT + 0, JacobianRow.X);
359+
Jacobian.Set(RotationIndex * ROTATION_AXIS_COUNT + RotAxis, PositionConstraintIndex * AXIS_COUNT + 1, JacobianRow.Y);
360+
Jacobian.Set(RotationIndex * ROTATION_AXIS_COUNT + RotAxis, PositionConstraintIndex * AXIS_COUNT + 2, JacobianRow.Z);
345361
}
346362
}
347363
else
348364
{
349365
// このジョイントがこのコンストレイントに影響を及ぼすジョイントでないときはヤコビアンの要素は0
350366
for (int32 RotAxis = 0; RotAxis < ROTATION_AXIS_COUNT; ++RotAxis)
351367
{
352-
Jacobian.Set(RotationIndex * ROTATION_AXIS_COUNT + RotAxis, ConstraintIndex * AXIS_COUNT + 0, 0.0f);
353-
Jacobian.Set(RotationIndex * ROTATION_AXIS_COUNT + RotAxis, ConstraintIndex * AXIS_COUNT + 1, 0.0f);
354-
Jacobian.Set(RotationIndex * ROTATION_AXIS_COUNT + RotAxis, ConstraintIndex * AXIS_COUNT + 2, 0.0f);
368+
Jacobian.Set(RotationIndex * ROTATION_AXIS_COUNT + RotAxis, PositionConstraintIndex * AXIS_COUNT + 0, 0.0f);
369+
Jacobian.Set(RotationIndex * ROTATION_AXIS_COUNT + RotAxis, PositionConstraintIndex * AXIS_COUNT + 1, 0.0f);
370+
Jacobian.Set(RotationIndex * ROTATION_AXIS_COUNT + RotAxis, PositionConstraintIndex * AXIS_COUNT + 2, 0.0f);
355371
}
356372
}
373+
374+
PositionConstraintIndex++;
357375
}
358376

359377
RotationIndex++;
@@ -374,9 +392,9 @@ void FAnimNode_JacobianIK::EvaluateSkeletalControl_AnyThread(FComponentSpacePose
374392

375393
JJti.ZeroClear(); // 最終的に全要素に値が入るので0クリアする必要はないがデバッグのしやすさのために0クリアする
376394
#if 0
377-
float Determinant = AnySizeMatrix::InverseNxN(AXIS_COUNT * IKConstraintWorkDataArray.Num(), JJt, JJti);
395+
float Determinant = AnySizeMatrix::InverseNxN(JJt, JJti);
378396
#else
379-
float Determinant = AnySizeMatrix::InverseNxN(AXIS_COUNT * IKConstraintWorkDataArray.Num(), JJtPlusLambdaI, JJti);
397+
float Determinant = AnySizeMatrix::InverseNxN(JJtPlusLambdaI, JJti);
380398
#endif
381399
//if (FMath::Abs(Determinant) < KINDA_SMALL_NUMBER)
382400
if (FMath::Abs(Determinant) < SMALL_NUMBER)

‎Plugins/IKPluginSandbox/Source/IKPluginSandbox/Private/JacobianIKUtility.h

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -85,7 +85,7 @@ struct AnySizeMatrix
8585
static void Multiply(const AnySizeMatrix& A, const AnySizeMatrix& B, AnySizeMatrix& OutResult);
8686
static void Add(const AnySizeMatrix& A, const AnySizeMatrix& B, AnySizeMatrix& OutResult);
8787
static float Inverse3x3(const AnySizeMatrix& InMatrix, AnySizeMatrix& OutMatrix);
88-
static float InverseNxN(uint8 Size, const AnySizeMatrix& InMatrix, AnySizeMatrix& OutMatrix);
88+
static float InverseNxN(const AnySizeMatrix& InMatrix, AnySizeMatrix& OutMatrix);
8989
static void TransformVector(const AnySizeMatrix& InMatrix, const TArray<float>& InVector, TArray<float>& OutVector);
9090

9191
TArray<float> Elements; // 1-dimensional array for access speed.
@@ -197,11 +197,16 @@ FORCEINLINE float AnySizeMatrix::Inverse3x3(const AnySizeMatrix& InMatrix, AnySi
197197
return Determinant;
198198
}
199199

200-
FORCEINLINE float AnySizeMatrix::InverseNxN(uint8 Size, const AnySizeMatrix& InMatrix, AnySizeMatrix& OutMatrix)
200+
FORCEINLINE float AnySizeMatrix::InverseNxN(const AnySizeMatrix& InMatrix, AnySizeMatrix& OutMatrix)
201201
{
202202
// http://thira.plavox.info/blog/2008/06/_c.html　https://seesaawiki.jp/w/pafuhana1213/d/n%BC%A1%A4%CE%B5%D5%B9%D4%CE%F3 をそのまま使っている
203203
float Determinant = 1.0;
204204

205+
uint8 Size = InMatrix.NumRow;
206+
check(Size == InMatrix.NumColumn);
207+
check(Size == OutMatrix.NumRow);
208+
check(Size == OutMatrix.NumColumn);
209+
205210
// 三角行列を作成
206211
AnySizeMatrix WorkMatrix = InMatrix; // コピー
207212

‎Plugins/IKPluginSandbox/Source/IKPluginSandbox/Public/AnimNode_JacobianIK.h

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,9 @@ UENUM()
1414
enum class EIKConstraintType : uint8
1515
{
1616
INVALID,
17+
// IKSkeletonのコンポーネントスペースにおける位置を指定して維持する
1718
KEEP_POSITION,
19+
// IKSkeletonのジョイントのローカルスペースにおける回転角を指定して維持する
1820
KEEP_ROTATION,
1921
};
2022

@@ -36,7 +38,8 @@ struct FIKConstraint
3638
UPROPERTY(EditAnywhere)
3739
FRotator Rotation;
3840

39-
/** root joint of joints chain which effects to this IK constraint. **/
41+
// TODO:位置コンストレイント以外のタイプなら非表示にした方がいいかも
42+
/** root joint of joints chain which effects to this IK constraint. It is necessary at position constraint. **/
4043
UPROPERTY(EditAnywhere)
4144
FBoneReference EffectiveRootJoint;
4245
};
@@ -102,8 +105,8 @@ struct IKPLUGINSANDBOX_API FAnimNode_JacobianIK : public FAnimNode_SkeletalContr
102105
{
103106
/** target joint index of constraint. **/
104107
FCompactPoseBoneIndex JointIndex;
105-
/** joints chain indices which effects to this IK constraint. It does not include self joint. **/
106-
TArray<FCompactPoseBoneIndex> EffectiveJointIndices; // このコンストレイントIK計算を行う
108+
/** joints chain indices which effects to this IK position constraint. It does not include self joint. **/
109+
TArray<FCompactPoseBoneIndex> EffectiveJointIndices;
107110
/** constraint type. **/
108111
EIKConstraintType Type;
109112
/** constraint position **/

0 commit comments

Comments
 (0)