SkeletonMapperTest.cpp 5.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144
  1. // Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
  2. // SPDX-FileCopyrightText: 2021 Jorrit Rouwe
  3. // SPDX-License-Identifier: MIT
  4. #include <TestFramework.h>
  5. #include <Tests/Rig/SkeletonMapperTest.h>
  6. #include <Jolt/Physics/StateRecorder.h>
  7. #include <Jolt/ObjectStream/ObjectStreamIn.h>
  8. #include <Renderer/DebugRendererImp.h>
  9. #include <Layers.h>
  10. #include <Utils/Log.h>
  11. #include <Application/DebugUI.h>
  12. JPH_IMPLEMENT_RTTI_VIRTUAL(SkeletonMapperTest)
  13. {
  14. JPH_ADD_BASE_CLASS(SkeletonMapperTest, Test)
  15. }
  16. SkeletonMapperTest::~SkeletonMapperTest()
  17. {
  18. mRagdoll->RemoveFromPhysicsSystem();
  19. }
  20. void SkeletonMapperTest::Initialize()
  21. {
  22. // Floor
  23. CreateFloor();
  24. // Load ragdoll
  25. mRagdollSettings = RagdollLoader::sLoad("Assets/Human.tof", EMotionType::Dynamic);
  26. // Create ragdoll
  27. mRagdoll = mRagdollSettings->CreateRagdoll(0, 0, mPhysicsSystem);
  28. mRagdoll->AddToPhysicsSystem(EActivation::Activate);
  29. // Load neutral animation for ragdoll
  30. Ref<SkeletalAnimation> neutral_ragdoll;
  31. if (!ObjectStreamIn::sReadObject("Assets/Human/neutral.tof", neutral_ragdoll))
  32. FatalError("Could not open neutral animation");
  33. // Load animation skeleton
  34. Ref<Skeleton> animation_skeleton;
  35. if (!ObjectStreamIn::sReadObject("Assets/Human/skeleton_hd.tof", animation_skeleton))
  36. FatalError("Could not open skeleton_hd");
  37. animation_skeleton->CalculateParentJointIndices();
  38. // Load neutral animation
  39. Ref<SkeletalAnimation> neutral_animation;
  40. if (!ObjectStreamIn::sReadObject("Assets/Human/neutral_hd.tof", neutral_animation))
  41. FatalError("Could not open neutral_hd animation");
  42. // Load test animation
  43. if (!ObjectStreamIn::sReadObject("Assets/Human/jog_hd.tof", mAnimation))
  44. FatalError("Could not open jog_hd animation");
  45. // Initialize pose
  46. mAnimatedPose.SetSkeleton(animation_skeleton);
  47. mRagdollPose.SetSkeleton(mRagdollSettings->GetSkeleton());
  48. // Calculate neutral poses and initialize skeleton mapper
  49. neutral_ragdoll->Sample(0.0f, mRagdollPose);
  50. mRagdollPose.CalculateJointMatrices();
  51. neutral_animation->Sample(0.0f, mAnimatedPose);
  52. mAnimatedPose.CalculateJointMatrices();
  53. mRagdollToAnimated.Initialize(mRagdollPose.GetSkeleton(), mRagdollPose.GetJointMatrices().data(), mAnimatedPose.GetSkeleton(), mAnimatedPose.GetJointMatrices().data());
  54. // Optionally lock translations (this can be used to prevent ragdolls from stretching)
  55. // Try wildly dragging the ragdoll by the head (using spacebar) to see how the ragdoll stretches under stress
  56. if (sLockTranslations)
  57. mRagdollToAnimated.LockAllTranslations(mAnimatedPose.GetSkeleton(), mAnimatedPose.GetJointMatrices().data());
  58. // Calculate initial pose and set it
  59. CalculateRagdollPose();
  60. mRagdoll->SetPose(mRagdollPose);
  61. }
  62. void SkeletonMapperTest::CalculateRagdollPose()
  63. {
  64. // Sample new animated pose
  65. mAnimation->Sample(mTime, mAnimatedPose);
  66. mAnimatedPose.CalculateJointMatrices();
  67. // Map to ragdoll pose
  68. mRagdollToAnimated.MapReverse(mAnimatedPose.GetJointMatrices().data(), mRagdollPose.GetJointMatrices().data());
  69. mRagdollPose.CalculateJointStates();
  70. }
  71. void SkeletonMapperTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
  72. {
  73. // Update time
  74. mTime += inParams.mDeltaTime;
  75. // Drive the ragdoll pose and drive motors to reach it
  76. CalculateRagdollPose();
  77. mRagdoll->DriveToPoseUsingMotors(mRagdollPose);
  78. #ifdef JPH_DEBUG_RENDERER
  79. // Draw animated skeleton
  80. mAnimatedPose.Draw(*inParams.mPoseDrawSettings, mDebugRenderer);
  81. mDebugRenderer->DrawText3D(mAnimatedPose.GetRootOffset() + mAnimatedPose.GetJointMatrix(0).GetTranslation(), "Animated", Color::sWhite, 0.2f);
  82. // Draw mapped skeleton
  83. RMat44 offset = RMat44::sTranslation(RVec3(1.0f, 0, 0));
  84. mRagdollPose.Draw(*inParams.mPoseDrawSettings, mDebugRenderer, offset);
  85. mDebugRenderer->DrawText3D(offset * (mAnimatedPose.GetRootOffset() + mAnimatedPose.GetJointMatrix(0).GetTranslation()), "Reverse Mapped", Color::sWhite, 0.2f);
  86. #endif // JPH_DEBUG_RENDERER
  87. // Get ragdoll pose in model space
  88. RVec3 root_offset;
  89. Array<Mat44> pose1_model(mRagdollPose.GetJointCount());
  90. mRagdoll->GetPose(root_offset, pose1_model.data());
  91. // Get animated pose in local space
  92. Array<Mat44> pose2_local(mAnimatedPose.GetJointCount());
  93. mAnimatedPose.CalculateLocalSpaceJointMatrices(pose2_local.data());
  94. // Map ragdoll to animated pose, filling in the extra joints using the local space animated pose
  95. SkeletonPose pose2_world;
  96. pose2_world.SetSkeleton(mAnimatedPose.GetSkeleton());
  97. pose2_world.SetRootOffset(root_offset);
  98. mRagdollToAnimated.Map(pose1_model.data(), pose2_local.data(), pose2_world.GetJointMatrices().data());
  99. #ifdef JPH_DEBUG_RENDERER
  100. // Draw mapped pose
  101. pose2_world.Draw(*inParams.mPoseDrawSettings, mDebugRenderer, offset);
  102. mDebugRenderer->DrawText3D(offset * pose2_world.GetJointMatrix(1).GetTranslation(), "Mapped", Color::sWhite, 0.2f);
  103. #endif // JPH_DEBUG_RENDERER
  104. }
  105. void SkeletonMapperTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu)
  106. {
  107. inUI->CreateCheckBox(inSubMenu, "Lock Translations", sLockTranslations, [this](UICheckBox::EState inState) { sLockTranslations = inState == UICheckBox::STATE_CHECKED; RestartTest(); });
  108. }
  109. void SkeletonMapperTest::SaveState(StateRecorder &inStream) const
  110. {
  111. inStream.Write(mTime);
  112. }
  113. void SkeletonMapperTest::RestoreState(StateRecorder &inStream)
  114. {
  115. inStream.Read(mTime);
  116. }