BlendTreeTwoLinkIKNodeTests.cpp 27 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548
  1. /*
  2. * Copyright (c) Contributors to the Open 3D Engine Project.
  3. * For complete copyright and license terms please see the LICENSE at the root of this distribution.
  4. *
  5. * SPDX-License-Identifier: Apache-2.0 OR MIT
  6. *
  7. */
  8. #include <Tests/AnimGraphFixture.h>
  9. #include <EMotionFX/Source/AnimGraph.h>
  10. #include <EMotionFX/Source/AnimGraphBindPoseNode.h>
  11. #include <EMotionFX/Source/AnimGraphNode.h>
  12. #include <EMotionFX/Source/AnimGraphStateMachine.h>
  13. #include <EMotionFX/Source/BlendTree.h>
  14. #include <EMotionFX/Source/BlendTreeParameterNode.h>
  15. #include <EMotionFX/Source/BlendTreeTwoLinkIKNode.h>
  16. #include <EMotionFX/Source/EMotionFXManager.h>
  17. #include <EMotionFX/Source/Parameter/FloatSliderParameter.h>
  18. #include <EMotionFX/Source/Parameter/Vector3Parameter.h>
  19. #include <EMotionFX/Source/Parameter/RotationParameter.h>
  20. #include <EMotionFX/Source/Skeleton.h>
  21. #include <EMotionFX/Source/Motion.h>
  22. #include <EMotionFX/Source/TransformData.h>
  23. #include <EMotionFX/Source/BlendTreeVector3ComposeNode.h>
  24. #include <Tests/JackGraphFixture.h>
  25. namespace EMotionFX
  26. {
  27. struct BlendTreeTwoLinkIKNodeTestsData
  28. {
  29. AZStd::string m_testJointName;
  30. std::vector<AZStd::string> m_linkedJointNames;
  31. std::vector<std::vector<float>> m_reachablePositions;
  32. std::vector<std::vector<float>> m_unreachablePositions;
  33. std::vector<std::vector<float>> m_rotations;
  34. std::vector<float> m_bendDirPosition;
  35. std::vector<AZStd::string> m_alignToNodeNames;
  36. };
  37. class BlendTreeTwoLinkIKNodeFixture
  38. : public JackGraphFixture
  39. , public ::testing::WithParamInterface<testing::tuple<bool, BlendTreeTwoLinkIKNodeTestsData>>
  40. {
  41. public:
  42. using NodeAlignmentData = AZStd::pair<AZStd::string, int>;
  43. void ConstructGraph() override
  44. {
  45. JackGraphFixture::ConstructGraph();
  46. m_param = testing::get<1>(GetParam());
  47. m_jackSkeleton = m_actor->GetSkeleton();
  48. /*
  49. Blend tree in animgraph:
  50. +------------+
  51. |bindPoseNode|---+
  52. +------------+ | +-------------+
  53. +--->| | +---------+
  54. |twoLinkIKNode|--->|finalNode|
  55. +-----------+ +--->| | +---------+
  56. |m_paramNode|---+ +-------------+
  57. +-----------+
  58. */
  59. AddParameter<FloatSliderParameter>("WeightParam", 0.0f);
  60. AddParameter<Vector3Parameter>("GoalPosParam", AZ::Vector3(0.0f, 0.0f, 0.0f));
  61. AddParameter<RotationParameter>("RotationParam", AZ::Quaternion(0.0f, 0.0f, 0.0f, 1.0f));
  62. AddParameter<Vector3Parameter>("BendDirParam", AZ::Vector3(0.0f, 0.0f, 0.0f));
  63. AnimGraphBindPoseNode* bindPoseNode = aznew AnimGraphBindPoseNode();
  64. BlendTreeFinalNode* finalNode = aznew BlendTreeFinalNode();
  65. m_paramNode = aznew BlendTreeParameterNode();
  66. m_twoLinkIKNode = aznew BlendTreeTwoLinkIKNode();
  67. m_twoLinkIKNode->SetEndNodeName(m_param.m_testJointName);
  68. m_blendTree = aznew BlendTree();
  69. m_blendTree->AddChildNode(bindPoseNode);
  70. m_blendTree->AddChildNode(m_paramNode);
  71. m_blendTree->AddChildNode(m_twoLinkIKNode);
  72. m_blendTree->AddChildNode(finalNode);
  73. m_animGraph->GetRootStateMachine()->AddChildNode(m_blendTree);
  74. m_animGraph->GetRootStateMachine()->SetEntryState(m_blendTree);
  75. m_twoLinkIKNode->AddConnection(bindPoseNode, AnimGraphBindPoseNode::OUTPUTPORT_RESULT, BlendTreeTwoLinkIKNode::INPUTPORT_POSE);
  76. finalNode->AddConnection(m_twoLinkIKNode, BlendTreeTwoLinkIKNode::OUTPUTPORT_POSE, BlendTreeFinalNode::INPUTPORT_POSE);
  77. };
  78. bool PosePositionCompareClose(const AZ::Vector3& posA, const AZ::Vector3& posB, float tolerance)
  79. {
  80. if (!posA.IsClose(posB, tolerance))
  81. {
  82. return false;
  83. }
  84. return true;
  85. };
  86. bool PositionVectorsPointInSameDirection(const AZ::Vector3& posA, const AZ::Vector3& posB, const AZ::Vector3& posC, const AZ::Vector3& targetPos)
  87. {
  88. const AZ::Vector3& vecCtoB = (posB - posC).GetNormalized();
  89. const AZ::Vector3& vecBtoA = (posA - posB).GetNormalized();
  90. const AZ::Vector3& vecCtoTarget = (targetPos - posC).GetNormalized();
  91. const float dot1 = vecCtoB.Dot(vecBtoA);
  92. const float dot2 = vecCtoB.Dot(vecCtoTarget);
  93. return AZ::IsClose(dot1, 1.0f, 0.001f) &&
  94. AZ::IsClose(dot2, 1.0f, 0.001f);
  95. };
  96. template <class paramType, class inputType>
  97. void ParamSetValue(const AZStd::string& paramName, const inputType& value)
  98. {
  99. const AZ::Outcome<size_t> parameterIndex = m_animGraphInstance->FindParameterIndex(paramName);
  100. MCore::Attribute* param = m_animGraphInstance->GetParameterValue(static_cast<AZ::u32>(parameterIndex.GetValue()));
  101. paramType* typeParam = static_cast<paramType*>(param);
  102. typeParam->SetValue(value);
  103. }
  104. protected:
  105. BlendTree* m_blendTree = nullptr;
  106. BlendTreeParameterNode* m_paramNode = nullptr;
  107. BlendTreeTwoLinkIKNode* m_twoLinkIKNode = nullptr;
  108. BlendTreeTwoLinkIKNodeTestsData m_param;
  109. Skeleton* m_jackSkeleton = nullptr;
  110. private:
  111. template<class ParameterType, class ValueType>
  112. void AddParameter(const AZStd::string& name, const ValueType& defaultValue)
  113. {
  114. ParameterType* parameter = aznew ParameterType();
  115. parameter->SetName(name);
  116. parameter->SetDefaultValue(defaultValue);
  117. m_animGraph->AddParameter(parameter);
  118. }
  119. };
  120. TEST_P(BlendTreeTwoLinkIKNodeFixture, ReachablePositionsOutputCorrectPose)
  121. {
  122. // Set values for vector3 and twoLinkIKNode weight parameter
  123. m_twoLinkIKNode->AddConnection(m_paramNode, static_cast<uint16>(m_paramNode->FindOutputPortByName("WeightParam")->m_portId), BlendTreeTwoLinkIKNode::INPUTPORT_WEIGHT);
  124. m_twoLinkIKNode->AddConnection(m_paramNode,
  125. static_cast<uint16>(m_paramNode->FindOutputPortByName("GoalPosParam")->m_portId), BlendTreeTwoLinkIKNode::INPUTPORT_GOALPOS);
  126. GetEMotionFX().Update(1.0f / 60.0f);
  127. const float weight = testing::get<0>(GetParam());
  128. ParamSetValue<MCore::AttributeFloat, float>("WeightParam", weight);
  129. // Remeber specific joint's original position to compare with its new position later
  130. const Pose* jackPose = m_actorInstance->GetTransformData()->GetCurrentPose();
  131. size_t testJointIndex;
  132. m_jackSkeleton->FindNodeAndIndexByName(m_param.m_testJointName, testJointIndex);
  133. const AZ::Vector3& testJointPos = jackPose->GetModelSpaceTransform(testJointIndex).m_position;
  134. for (std::vector<float> goalPosXYZ : m_param.m_reachablePositions)
  135. {
  136. const float goalX = goalPosXYZ[0];
  137. const float goalY = goalPosXYZ[1];
  138. const float goalZ = goalPosXYZ[2];
  139. ParamSetValue<MCore::AttributeVector3, AZ::Vector3>("GoalPosParam", AZ::Vector3(goalX, goalY, goalZ));
  140. GetEMotionFX().Update(5.0f / 60.0f);
  141. const AZ::Vector3& testJointNewPos = jackPose->GetModelSpaceTransform(testJointIndex).m_position;
  142. // Based on weight, check if position of node changes to reachable goal position
  143. if (weight)
  144. {
  145. const AZ::Vector3 expectedPosition(goalX, goalY, goalZ);
  146. EXPECT_TRUE(PosePositionCompareClose(testJointNewPos, expectedPosition, 0.0001f))
  147. << "Joint position should be similar to expected position.";
  148. }
  149. else
  150. {
  151. EXPECT_TRUE(testJointNewPos == testJointPos) << "Joint position should not change when weight is 0.";
  152. }
  153. }
  154. };
  155. TEST_P(BlendTreeTwoLinkIKNodeFixture, ReachableAlignToNodeOutputCorrectPose)
  156. {
  157. m_twoLinkIKNode->AddConnection(m_paramNode, static_cast<uint16>(m_paramNode->FindOutputPortByName("WeightParam")->m_portId),
  158. BlendTreeTwoLinkIKNode::INPUTPORT_WEIGHT);
  159. GetEMotionFX().Update(1.0f / 60.0f);
  160. const float weight = testing::get<0>(GetParam());
  161. ParamSetValue<MCore::AttributeFloat, float>("WeightParam", weight);
  162. const Pose* jackPose = m_actorInstance->GetTransformData()->GetCurrentPose();
  163. size_t testJointIndex;
  164. m_jackSkeleton->FindNodeAndIndexByName(m_param.m_testJointName, testJointIndex);
  165. const AZ::Vector3& testJointPos = jackPose->GetModelSpaceTransform(testJointIndex).m_position;
  166. for (AZStd::string& nodeName : m_param.m_alignToNodeNames)
  167. {
  168. NodeAlignmentData alignToNode;
  169. alignToNode.first = nodeName;
  170. alignToNode.second = 0;
  171. m_twoLinkIKNode->SetAlignToNode(alignToNode);
  172. // Update will set uniqueData->m_mustUpdate to false for efficiency purposes
  173. // Unique data only updates once unless reset m_mustUpdate to true again
  174. BlendTreeTwoLinkIKNode::UniqueData* uniqueData = static_cast<BlendTreeTwoLinkIKNode::UniqueData*>(m_animGraphInstance->FindOrCreateUniqueNodeData(m_twoLinkIKNode));
  175. uniqueData->Invalidate();
  176. size_t alignToNodeIndex;
  177. m_jackSkeleton->FindNodeAndIndexByName(nodeName, alignToNodeIndex);
  178. GetEMotionFX().Update(1.0f / 60.0f);
  179. const AZ::Vector3& alignToNodePos = jackPose->GetModelSpaceTransform(alignToNodeIndex).m_position;
  180. const AZ::Vector3& testJointNewPos = jackPose->GetModelSpaceTransform(testJointIndex).m_position;
  181. // Based on weight, check if position of node changes to alignToNode position
  182. if (weight)
  183. {
  184. EXPECT_TRUE(PosePositionCompareClose(alignToNodePos, testJointNewPos, 0.0001f))
  185. << "Test joint position should be similar to align-to joint position.";
  186. }
  187. else
  188. {
  189. EXPECT_TRUE(testJointPos == testJointNewPos) << "Joint position should not change when weight is 0.";
  190. }
  191. }
  192. };
  193. TEST_P(BlendTreeTwoLinkIKNodeFixture, UnreachablePositionsOutputCorrectPose)
  194. {
  195. m_twoLinkIKNode->AddConnection(m_paramNode, static_cast<uint16>(m_paramNode->FindOutputPortByName("WeightParam")->m_portId),
  196. BlendTreeTwoLinkIKNode::INPUTPORT_WEIGHT);
  197. m_twoLinkIKNode->AddConnection(m_paramNode,
  198. static_cast<uint16>(m_paramNode->FindOutputPortByName("GoalPosParam")->m_portId), BlendTreeTwoLinkIKNode::INPUTPORT_GOALPOS);
  199. GetEMotionFX().Update(1.0f / 60.0f);
  200. const float weight = testing::get<0>(GetParam());
  201. ParamSetValue<MCore::AttributeFloat, float>("WeightParam", weight);
  202. const Pose* jackPose = m_actorInstance->GetTransformData()->GetCurrentPose();
  203. size_t testJointIndex;
  204. size_t linkedJoint0Index;
  205. size_t linkedJoint1Index;
  206. m_jackSkeleton->FindNodeAndIndexByName(m_param.m_testJointName, testJointIndex);
  207. m_jackSkeleton->FindNodeAndIndexByName(m_param.m_linkedJointNames[0], linkedJoint0Index);
  208. m_jackSkeleton->FindNodeAndIndexByName(m_param.m_linkedJointNames[1], linkedJoint1Index);
  209. const AZ::Vector3& testJointPos = jackPose->GetModelSpaceTransform(testJointIndex).m_position;
  210. for (std::vector<float> goalPosXYZ : m_param.m_unreachablePositions)
  211. {
  212. const float goalX = goalPosXYZ[0];
  213. const float goalY = goalPosXYZ[1];
  214. const float goalZ = goalPosXYZ[2];
  215. ParamSetValue<MCore::AttributeVector3, AZ::Vector3>("GoalPosParam", AZ::Vector3(goalX, goalY, goalZ));
  216. GetEMotionFX().Update(1.0f / 60.0f);
  217. const AZ::Vector3& testJointNewPos = jackPose->GetModelSpaceTransform(testJointIndex).m_position;
  218. const AZ::Vector3& linkedJoint0Pos = jackPose->GetModelSpaceTransform(linkedJoint0Index).m_position;
  219. const AZ::Vector3& linkedJoint1Pos = jackPose->GetModelSpaceTransform(linkedJoint1Index).m_position;
  220. // Based on weight, check if position of the test node
  221. // And its linked nodes are pointing towards the unreachable position
  222. if (weight)
  223. {
  224. const AZ::Vector3 goalVec(goalX, goalY, goalZ);
  225. EXPECT_TRUE(PositionVectorsPointInSameDirection(testJointNewPos, linkedJoint0Pos, linkedJoint1Pos, goalVec))
  226. << "Test joint and its linked joints should point to the expected vector.";
  227. }
  228. else
  229. {
  230. EXPECT_TRUE(testJointNewPos == testJointPos) << "Joint position should not change when weight is 0.";
  231. }
  232. }
  233. };
  234. TEST_P(BlendTreeTwoLinkIKNodeFixture, RotatedPositionsOutputCorrectPose)
  235. {
  236. m_twoLinkIKNode->AddConnection(m_paramNode, static_cast<uint16>(m_paramNode->FindOutputPortByName("WeightParam")->m_portId),
  237. BlendTreeTwoLinkIKNode::INPUTPORT_WEIGHT);
  238. m_twoLinkIKNode->AddConnection(m_paramNode,
  239. static_cast<uint16>(m_paramNode->FindOutputPortByName("GoalPosParam")->m_portId), BlendTreeTwoLinkIKNode::INPUTPORT_GOALPOS);
  240. m_twoLinkIKNode->AddConnection(m_paramNode,
  241. static_cast<uint16>(m_paramNode->FindOutputPortByName("RotationParam")->m_portId), BlendTreeTwoLinkIKNode::INPUTPORT_GOALROT);
  242. m_twoLinkIKNode->SetRotationEnabled(true);
  243. GetEMotionFX().Update(1.0f / 60.0f);
  244. // Set up test joint position and weight
  245. const float weight = testing::get<0>(GetParam());
  246. ParamSetValue<MCore::AttributeFloat, float>("WeightParam", weight);
  247. ParamSetValue<MCore::AttributeVector3, AZ::Vector3>("GoalPosParam", AZ::Vector3(0.0f, 1.0f, 1.0f));
  248. const Pose* jackPose = m_actorInstance->GetTransformData()->GetCurrentPose();
  249. size_t testJointIndex;
  250. m_jackSkeleton->FindNodeAndIndexByName(m_param.m_testJointName, testJointIndex);
  251. const AZ::Quaternion testJointRotation = jackPose->GetModelSpaceTransform(testJointIndex).m_rotation;
  252. for (std::vector<float> rotateXYZ : m_param.m_rotations)
  253. {
  254. const float rotateX = rotateXYZ[0];
  255. const float rotateY = rotateXYZ[1];
  256. const float rotateZ = rotateXYZ[2];
  257. ParamSetValue<MCore::AttributeQuaternion, AZ::Quaternion>("RotationParam", AZ::Quaternion(rotateX, rotateY, rotateZ, 1.0f));
  258. GetEMotionFX().Update(1.0f / 60.0f);
  259. const AZ::Quaternion testJointNewRotation = jackPose->GetModelSpaceTransform(testJointIndex).m_rotation;
  260. if (weight)
  261. {
  262. const AZ::Quaternion goalRotation(rotateX, rotateY, rotateZ, 1.0f);
  263. EXPECT_EQ(testJointNewRotation, goalRotation.GetNormalized()) << "Rotation of the test joint should be the same as the expected rotation.";
  264. }
  265. else
  266. {
  267. EXPECT_EQ(testJointNewRotation, testJointRotation) << "Rotation should not change when weight is 0.";
  268. }
  269. }
  270. };
  271. TEST_P(BlendTreeTwoLinkIKNodeFixture, BendDirectionOutputCorrectPose)
  272. {
  273. m_twoLinkIKNode->AddConnection(m_paramNode, static_cast<uint16>(m_paramNode->FindOutputPortByName("WeightParam")->m_portId),
  274. BlendTreeTwoLinkIKNode::INPUTPORT_WEIGHT);
  275. m_twoLinkIKNode->AddConnection(m_paramNode,
  276. static_cast<uint16>(m_paramNode->FindOutputPortByName("GoalPosParam")->m_portId), BlendTreeTwoLinkIKNode::INPUTPORT_GOALPOS);
  277. m_twoLinkIKNode->AddConnection(m_paramNode,
  278. static_cast<uint16>(m_paramNode->FindOutputPortByName("BendDirParam")->m_portId), BlendTreeTwoLinkIKNode::INPUTPORT_BENDDIR);
  279. m_twoLinkIKNode->SetRelativeBendDir(true);
  280. GetEMotionFX().Update(1.0f / 60.0f);
  281. // Set up Jack's arm to specific position for testing
  282. const float weight = testing::get<0>(GetParam());
  283. const float x = m_param.m_bendDirPosition[0];
  284. const float y = m_param.m_bendDirPosition[1];
  285. const float z = m_param.m_bendDirPosition[2];
  286. ParamSetValue<MCore::AttributeFloat, float>("WeightParam", weight);
  287. ParamSetValue<MCore::AttributeVector3, AZ::Vector3>("GoalPosParam", AZ::Vector3(x, y, z));
  288. GetEMotionFX().Update(1.0f / 60.0f);
  289. Pose* jackPose = m_actorInstance->GetTransformData()->GetCurrentPose();
  290. size_t testBendJointIndex;
  291. size_t testJointIndex;
  292. AZStd::string& bendLoArm = m_param.m_linkedJointNames[0];
  293. m_jackSkeleton->FindNodeAndIndexByName(bendLoArm, testBendJointIndex);
  294. m_jackSkeleton->FindNodeAndIndexByName(m_param.m_testJointName, testJointIndex);
  295. const AZ::Vector3 testJointBendPos = jackPose->GetModelSpaceTransform(testBendJointIndex).m_position;
  296. // Bend the test joint to opposite positions and check positions are opposite
  297. ParamSetValue<MCore::AttributeVector3, AZ::Vector3>("BendDirParam", AZ::Vector3(1.0f, 0.0f, 0.0f));
  298. GetEMotionFX().Update(1.0f / 60.0f);
  299. const AZ::Vector3 testJointBendRightPos = jackPose->GetModelSpaceTransform(testBendJointIndex).m_position;
  300. ParamSetValue<MCore::AttributeVector3, AZ::Vector3>("BendDirParam", AZ::Vector3(-1.0f, 0.0f, 0.0f));
  301. GetEMotionFX().Update(1.0f / 60.0f);
  302. const AZ::Vector3 testJointBendLeftPos = jackPose->GetModelSpaceTransform(testBendJointIndex).m_position;
  303. ParamSetValue<MCore::AttributeVector3, AZ::Vector3>("BendDirParam", AZ::Vector3(0.0f, 1.0f, 0.0f));
  304. GetEMotionFX().Update(1.0f / 60.0f);
  305. const AZ::Vector3 testJointBendDownPos = jackPose->GetModelSpaceTransform(testBendJointIndex).m_position;
  306. ParamSetValue<MCore::AttributeVector3, AZ::Vector3>("BendDirParam", AZ::Vector3(0.0f, -1.0f, 0.0f));
  307. GetEMotionFX().Update(1.0f / 60.0f);
  308. const AZ::Vector3 testJointBendUpPos = jackPose->GetModelSpaceTransform(testBendJointIndex).m_position;
  309. if (weight)
  310. {
  311. // Z-axis(Height) of tested bend joint should behave correctly
  312. EXPECT_TRUE(testJointBendDownPos.GetZ() < testJointBendUpPos.GetZ()) << "Height of bent down joint should be lower than bent up joint.";
  313. EXPECT_TRUE(testJointBendPos.GetZ() < testJointBendUpPos.GetZ()) << "Height of original joint should be lower than bent up joint.";
  314. EXPECT_TRUE(testJointBendDownPos.GetZ() < testJointBendPos.GetZ()) << "Height of bent down joint should be lower than original joint.";
  315. // X-axis(Horizontal) of tested bend joint should behave correctly
  316. EXPECT_TRUE(testJointBendLeftPos.GetX() < testJointBendRightPos.GetX()) << "Bent left joint should be on the left of bent right joint.";
  317. EXPECT_TRUE(testJointBendPos.GetX() < testJointBendRightPos.GetX()) << "Original joint should be on the left of bent right joint.";
  318. EXPECT_TRUE(testJointBendLeftPos.GetX() < testJointBendPos.GetX()) << "Bent left joint should be on the left of original joint.";
  319. }
  320. else
  321. {
  322. // Position should not change if weight is 0
  323. EXPECT_EQ(testJointBendRightPos, testJointBendLeftPos) << "Joint position should not change.";
  324. EXPECT_EQ(testJointBendUpPos, testJointBendDownPos) << "Joint position should not change.";
  325. }
  326. };
  327. TEST_P(BlendTreeTwoLinkIKNodeFixture, CombinedFunctionsOutputCorrectPose)
  328. {
  329. // Two Link IK Node should not break when using all of its functions at the same time
  330. m_twoLinkIKNode->AddConnection(m_paramNode, static_cast<uint16>(m_paramNode->FindOutputPortByName("WeightParam")->m_portId),
  331. BlendTreeTwoLinkIKNode::INPUTPORT_WEIGHT);
  332. m_twoLinkIKNode->AddConnection(m_paramNode,
  333. static_cast<uint16>(m_paramNode->FindOutputPortByName("GoalPosParam")->m_portId), BlendTreeTwoLinkIKNode::INPUTPORT_GOALPOS);
  334. m_twoLinkIKNode->AddConnection(m_paramNode,
  335. static_cast<uint16>(m_paramNode->FindOutputPortByName("RotationParam")->m_portId), BlendTreeTwoLinkIKNode::INPUTPORT_GOALROT);
  336. m_twoLinkIKNode->AddConnection(m_paramNode,
  337. static_cast<uint16>(m_paramNode->FindOutputPortByName("BendDirParam")->m_portId), BlendTreeTwoLinkIKNode::INPUTPORT_BENDDIR);
  338. m_twoLinkIKNode->SetRotationEnabled(true);
  339. m_twoLinkIKNode->SetRelativeBendDir(true);
  340. GetEMotionFX().Update(1.0f / 60.0f);
  341. const Pose* jackPose = m_actorInstance->GetTransformData()->GetCurrentPose();
  342. size_t testJointIndex;
  343. size_t testBendJointIndex;
  344. AZStd::string& bendLoArm = m_param.m_linkedJointNames[0];
  345. m_jackSkeleton->FindNodeAndIndexByName(m_param.m_testJointName, testJointIndex);
  346. m_jackSkeleton->FindNodeAndIndexByName(bendLoArm, testBendJointIndex);
  347. // Adding weight and goal position
  348. const float weight = testing::get<0>(GetParam());
  349. const float posX = m_param.m_bendDirPosition[0];
  350. const float posY = m_param.m_bendDirPosition[1];
  351. const float posZ = m_param.m_bendDirPosition[2];
  352. ParamSetValue<MCore::AttributeFloat, float>("WeightParam", weight);
  353. ParamSetValue<MCore::AttributeVector3, AZ::Vector3>("GoalPosParam", AZ::Vector3(posX, posY, posZ));
  354. GetEMotionFX().Update(1.0f / 60.0f);
  355. // Add bend direction
  356. const AZ::Vector3 testJointBendPos = jackPose->GetModelSpaceTransform(testBendJointIndex).m_position;
  357. ParamSetValue<MCore::AttributeVector3, AZ::Vector3>("BendDirParam", AZ::Vector3(1.0f, 0.0f, 0.0f));
  358. GetEMotionFX().Update(1.0f / 60.0f);
  359. const AZ::Vector3 testJointBendRightPos = jackPose->GetModelSpaceTransform(testBendJointIndex).m_position;
  360. ParamSetValue<MCore::AttributeVector3, AZ::Vector3>("BendDirParam", AZ::Vector3(-1.0f, 0.0f, 0.0f));
  361. GetEMotionFX().Update(1.0f / 60.0f);
  362. const AZ::Vector3 testJointBendLeftPos = jackPose->GetModelSpaceTransform(testBendJointIndex).m_position;
  363. ParamSetValue<MCore::AttributeVector3, AZ::Vector3>("BendDirParam", AZ::Vector3(0.0f, 1.0f, 0.0f));
  364. GetEMotionFX().Update(1.0f / 60.0f);
  365. const AZ::Vector3 testJointBendDownPos = jackPose->GetModelSpaceTransform(testBendJointIndex).m_position;
  366. ParamSetValue<MCore::AttributeVector3, AZ::Vector3>("BendDirParam", AZ::Vector3(0.0f, -1.0f, 0.0f));
  367. GetEMotionFX().Update(1.0f / 60.0f);
  368. const AZ::Vector3 testJointBendUpPos = jackPose->GetModelSpaceTransform(testBendJointIndex).m_position;
  369. // Rotations with bent joint
  370. const AZ::Quaternion testJointOriginalRotation = jackPose->GetModelSpaceTransform(testJointIndex).m_rotation;
  371. for (std::vector<float> rotateXYZ : m_param.m_rotations)
  372. {
  373. const float rotateX = rotateXYZ[0];
  374. const float rotateY = rotateXYZ[1];
  375. const float rotateZ = rotateXYZ[2];
  376. ParamSetValue<MCore::AttributeQuaternion, AZ::Quaternion>("RotationParam", AZ::Quaternion(rotateX, rotateY, rotateZ, 1.0f));
  377. GetEMotionFX().Update(1.0f / 60.0f);
  378. const AZ::Quaternion testJointNewRotation = jackPose->GetModelSpaceTransform(testJointIndex).m_rotation;
  379. if (weight)
  380. {
  381. const AZ::Quaternion goalRotation(rotateX, rotateY, rotateZ, 1.0f);
  382. EXPECT_EQ(testJointNewRotation, goalRotation.GetNormalized()) << "Rotation of the test joint should be the same as the expected rotation.";
  383. }
  384. else
  385. {
  386. EXPECT_EQ(testJointNewRotation, testJointOriginalRotation) << "Rotation should not change when weight is 0.";
  387. }
  388. }
  389. if (weight)
  390. {
  391. // Z-axis(Height) of tested bend joint should behave correctly
  392. EXPECT_TRUE(testJointBendDownPos.GetZ() < testJointBendUpPos.GetZ()) << "Height of bent down joint should be lower than bent up joint.";
  393. EXPECT_TRUE(testJointBendPos.GetZ() < testJointBendUpPos.GetZ()) << "Height of original joint should be lower than bent up joint.";
  394. EXPECT_TRUE(testJointBendDownPos.GetZ() < testJointBendPos.GetZ()) << "Height of bent down joint should be lower than original joint.";
  395. // X-axis(Horizontal) of tested bend joint should behave correctly
  396. EXPECT_TRUE(testJointBendLeftPos.GetX() < testJointBendRightPos.GetX()) << "Bent left joint should be on the left of bent right joint.";
  397. EXPECT_TRUE(testJointBendPos.GetX() < testJointBendRightPos.GetX()) << "Original joint should be on the left of bent right joint.";
  398. EXPECT_TRUE(testJointBendLeftPos.GetX() < testJointBendPos.GetX()) << "Bent left joint should be on the left of original joint.";
  399. }
  400. else
  401. {
  402. // Position should not change if weight is 0
  403. EXPECT_EQ(testJointBendRightPos, testJointBendLeftPos) << "Joint position should not change.";
  404. EXPECT_EQ(testJointBendUpPos, testJointBendDownPos) << "Joint position should not change.";
  405. }
  406. };
  407. std::vector< BlendTreeTwoLinkIKNodeTestsData> testData
  408. {
  409. {
  410. "l_hand" /* Test joint */,
  411. {"l_loArm","l_upArm"} /* Linked joints */,
  412. {
  413. // Reachable positions for the joint
  414. {0.0f, -0.02f, 1.09f},
  415. {-0.08f, 0.03f, 1.50f},
  416. {0.08f, 0.03f, 1.50f},
  417. {0.0f, 0.02f, 1.67f}
  418. },
  419. {
  420. // Unreachable positions for the joint
  421. {0.0f, 0.0f, 0.0f},
  422. {0.0f, 10.0f, 0.0f},
  423. {0.0f, 0.0f, 10.0f},
  424. {0.0f, 10.0f, 10.0f},
  425. {10.0f, 0.0f, 0.0f},
  426. {10.0f, 10.0f, 0.0f},
  427. {10.0f, 10.0f, 10.0f}
  428. },
  429. {
  430. // Rotations of the joint
  431. {-90.0f, 180.0f, 0.0f},
  432. {-540.0f, 0.0f, 1020.0f},
  433. {0.0f, 0.0f, 0.0f},
  434. {0.0f, 480.0f, -60.8f},
  435. {0.0f, -500.0f, 1000.0f},
  436. {1000.0f, -500.0f, 360.0f},
  437. {10.0f, 0.0f, -5.0f}
  438. },
  439. {0.2f, 0.4f, 1.5f} /* Fixed position of the tested joint for bend direction test */,
  440. {"spine1", "r_shldr", "l_shldr", "head"} /* Names of align-to node */
  441. }
  442. ,
  443. {
  444. "r_hand",
  445. {"r_loArm","r_upArm"},
  446. {
  447. {0.0f, -0.02f, 1.09f},
  448. {-0.08f, 0.03f, 1.50f},
  449. {0.08f, 0.03f, 1.50f},
  450. {0.0f, 0.02f, 1.67f}
  451. },
  452. {
  453. {0.0f, 0.0f, 0.0f},
  454. {0.0f, 10.0f, 0.0f},
  455. {0.0f, 0.0f, 10.0f},
  456. {0.0f, 10.0f, 10.0f},
  457. {10.0f, 0.0f, 0.0f},
  458. {10.0f, 10.0f, 0.0f},
  459. {10.0f, 10.0f, 10.0f}
  460. },
  461. {
  462. {-0.05f, .10f, 0.0f},
  463. {-.05f, 0.0f, .10f},
  464. {0.0f, 0.0f, 0.0f},
  465. {0.0f, .10f, -.05f},
  466. {0.0f, -.05f, .10f},
  467. {0.10f, -.05f, 0.0f},
  468. {.10f, 0.0f, -.05f}
  469. },
  470. {-0.2f, 0.4f, 1.5f},
  471. {"spine1", "r_shldr", "l_shldr", "head"}
  472. }
  473. };
  474. INSTANTIATE_TEST_CASE_P(BlendTreeTwoLinkIKNode_OutputTests,
  475. BlendTreeTwoLinkIKNodeFixture,
  476. ::testing::Combine(
  477. ::testing::Bool(),
  478. ::testing::ValuesIn(testData)
  479. )
  480. );
  481. } // end namespace EMotionFX