RigPileTest.cpp 4.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148
  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/RigPileTest.h>
  6. #include <Jolt/Physics/PhysicsScene.h>
  7. #include <Jolt/Physics/Collision/RayCast.h>
  8. #include <Jolt/Physics/Collision/CastResult.h>
  9. #include <Jolt/Skeleton/Skeleton.h>
  10. #include <Jolt/Skeleton/SkeletalAnimation.h>
  11. #include <Jolt/Skeleton/SkeletonPose.h>
  12. #include <Jolt/Core/StringTools.h>
  13. #include <Jolt/ObjectStream/ObjectStreamIn.h>
  14. #include <Utils/RagdollLoader.h>
  15. #include <Application/DebugUI.h>
  16. #include <Layers.h>
  17. #include <Utils/Log.h>
  18. #include <random>
  19. JPH_IMPLEMENT_RTTI_VIRTUAL(RigPileTest)
  20. {
  21. JPH_ADD_BASE_CLASS(RigPileTest, Test)
  22. }
  23. const char *RigPileTest::sScenes[] =
  24. {
  25. "PerlinMesh",
  26. "PerlinHeightField",
  27. "Terrain1",
  28. "Terrain2",
  29. };
  30. #ifdef _DEBUG
  31. const char *RigPileTest::sSceneName = "PerlinMesh";
  32. int RigPileTest::sPileSize = 5;
  33. int RigPileTest::sNumPilesPerAxis = 2;
  34. #else
  35. const char *RigPileTest::sSceneName = "Terrain1";
  36. int RigPileTest::sPileSize = 10;
  37. int RigPileTest::sNumPilesPerAxis = 4;
  38. #endif
  39. RigPileTest::~RigPileTest()
  40. {
  41. for (Ragdoll *r : mRagdolls)
  42. r->RemoveFromPhysicsSystem();
  43. }
  44. void RigPileTest::Initialize()
  45. {
  46. if (strcmp(sSceneName, "PerlinMesh") == 0)
  47. {
  48. // Default terrain
  49. CreateMeshTerrain();
  50. }
  51. else if (strcmp(sSceneName, "PerlinHeightField") == 0)
  52. {
  53. // Default terrain
  54. CreateHeightFieldTerrain();
  55. }
  56. else
  57. {
  58. // Load scene
  59. Ref<PhysicsScene> scene;
  60. if (!ObjectStreamIn::sReadObject((String("Assets/") + sSceneName + ".bof").c_str(), scene))
  61. FatalError("Failed to load scene");
  62. for (BodyCreationSettings &body : scene->GetBodies())
  63. body.mObjectLayer = Layers::NON_MOVING;
  64. scene->FixInvalidScales();
  65. scene->CreateBodies(mPhysicsSystem);
  66. }
  67. // Load ragdoll
  68. Ref<RagdollSettings> settings = RagdollLoader::sLoad("Assets/Human.tof", EMotionType::Dynamic);
  69. // Load animation
  70. const int cAnimationCount = 4;
  71. Ref<SkeletalAnimation> animation[cAnimationCount];
  72. for (int i = 0; i < cAnimationCount; ++i)
  73. {
  74. if (!ObjectStreamIn::sReadObject(StringFormat("Assets/Human/Dead_Pose%d.tof", i + 1).c_str(), animation[i]))
  75. FatalError("Could not open animation");
  76. }
  77. const float cHorizontalSeparation = 4.0f;
  78. const float cVerticalSeparation = 0.6f;
  79. // Limit the size of the piles so we don't go over 160 ragdolls
  80. int pile_size = min(sPileSize, 160 / Square(sNumPilesPerAxis));
  81. // Create piles
  82. default_random_engine random;
  83. uniform_real_distribution<float> angle(0.0f, JPH_PI);
  84. CollisionGroup::GroupID group_id = 1;
  85. for (int row = 0; row < sNumPilesPerAxis; ++row)
  86. for (int col = 0; col < sNumPilesPerAxis; ++col)
  87. {
  88. // Determine start location of ray
  89. RVec3 start = RVec3(cHorizontalSeparation * (col - (sNumPilesPerAxis - 1) / 2.0f), 100, cHorizontalSeparation * (row - (sNumPilesPerAxis - 1) / 2.0f));
  90. // Cast ray down to terrain
  91. RayCastResult hit;
  92. Vec3 ray_direction(0, -200, 0);
  93. RRayCast ray { start, ray_direction };
  94. if (mPhysicsSystem->GetNarrowPhaseQuery().CastRay(ray, hit, SpecifiedBroadPhaseLayerFilter(BroadPhaseLayers::NON_MOVING), SpecifiedObjectLayerFilter(Layers::NON_MOVING)))
  95. start = ray.GetPointOnRay(hit.mFraction);
  96. for (int i = 0; i < pile_size; ++i)
  97. {
  98. // Create ragdoll
  99. Ref<Ragdoll> ragdoll = settings->CreateRagdoll(group_id++, 0, mPhysicsSystem);
  100. // Sample pose
  101. SkeletonPose pose;
  102. pose.SetSkeleton(settings->GetSkeleton());
  103. animation[random() % cAnimationCount]->Sample(0.0f, pose);
  104. // Override root
  105. pose.SetRootOffset(start);
  106. SkeletonPose::JointState &root = pose.GetJoint(0);
  107. root.mTranslation = Vec3(0, cVerticalSeparation * (i + 1), 0);
  108. root.mRotation = Quat::sRotation(Vec3::sAxisY(), angle(random)) * root.mRotation;
  109. pose.CalculateJointMatrices();
  110. // Drive to pose
  111. ragdoll->SetPose(pose);
  112. ragdoll->DriveToPoseUsingMotors(pose);
  113. ragdoll->AddToPhysicsSystem(EActivation::Activate);
  114. mRagdolls.push_back(ragdoll);
  115. }
  116. }
  117. }
  118. void RigPileTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMenu)
  119. {
  120. inUI->CreateTextButton(inSubMenu, "Select Scene", [this, inUI]() {
  121. UIElement *scene_name = inUI->CreateMenu();
  122. for (uint i = 0; i < size(sScenes); ++i)
  123. inUI->CreateTextButton(scene_name, sScenes[i], [this, i]() { sSceneName = sScenes[i]; RestartTest(); });
  124. inUI->ShowMenu(scene_name);
  125. });
  126. inUI->CreateSlider(inSubMenu, "Num Ragdolls Per Pile", float(sPileSize), 1, 160, 1, [](float inValue) { sPileSize = (int)inValue; });
  127. inUI->CreateSlider(inSubMenu, "Num Piles Per Axis", float(sNumPilesPerAxis), 1, 4, 1, [](float inValue) { sNumPilesPerAxis = (int)inValue; });
  128. }