CreateRigTest.cpp 7.0 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207
  1. // Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
  2. // SPDX-FileCopyrightText: 2022 Jorrit Rouwe
  3. // SPDX-License-Identifier: MIT
  4. #include <TestFramework.h>
  5. #include <Tests/Rig/CreateRigTest.h>
  6. #include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
  7. #include <Jolt/Physics/Constraints/SwingTwistConstraint.h>
  8. #include <Application/DebugUI.h>
  9. #include <Layers.h>
  10. JPH_IMPLEMENT_RTTI_VIRTUAL(CreateRigTest)
  11. {
  12. JPH_ADD_BASE_CLASS(CreateRigTest, Test)
  13. }
  14. CreateRigTest::~CreateRigTest()
  15. {
  16. mRagdoll->RemoveFromPhysicsSystem();
  17. }
  18. void CreateRigTest::Initialize()
  19. {
  20. // Floor
  21. CreateFloor();
  22. // Create skeleton
  23. Ref<Skeleton> skeleton = new Skeleton;
  24. uint lower_body = skeleton->AddJoint("LowerBody");
  25. uint mid_body = skeleton->AddJoint("MidBody", lower_body);
  26. uint upper_body = skeleton->AddJoint("UpperBody", mid_body);
  27. /*uint head =*/ skeleton->AddJoint("Head", upper_body);
  28. uint upper_arm_l = skeleton->AddJoint("UpperArmL", upper_body);
  29. uint upper_arm_r = skeleton->AddJoint("UpperArmR", upper_body);
  30. /*uint lower_arm_l =*/ skeleton->AddJoint("LowerArmL", upper_arm_l);
  31. /*uint lower_arm_r =*/ skeleton->AddJoint("LowerArmR", upper_arm_r);
  32. uint upper_leg_l = skeleton->AddJoint("UpperLegL", lower_body);
  33. uint upper_leg_r = skeleton->AddJoint("UpperLegR", lower_body);
  34. /*uint lower_leg_l =*/ skeleton->AddJoint("LowerLegL", upper_leg_l);
  35. /*uint lower_leg_r =*/ skeleton->AddJoint("LowerLegR", upper_leg_r);
  36. // Create shapes for limbs
  37. Ref<Shape> shapes[] = {
  38. new CapsuleShape(0.15f, 0.10f), // Lower Body
  39. new CapsuleShape(0.15f, 0.10f), // Mid Body
  40. new CapsuleShape(0.15f, 0.10f), // Upper Body
  41. new CapsuleShape(0.075f, 0.10f), // Head
  42. new CapsuleShape(0.15f, 0.06f), // Upper Arm L
  43. new CapsuleShape(0.15f, 0.06f), // Upper Arm R
  44. new CapsuleShape(0.15f, 0.05f), // Lower Arm L
  45. new CapsuleShape(0.15f, 0.05f), // Lower Arm R
  46. new CapsuleShape(0.2f, 0.075f), // Upper Leg L
  47. new CapsuleShape(0.2f, 0.075f), // Upper Leg R
  48. new CapsuleShape(0.2f, 0.06f), // Lower Leg L
  49. new CapsuleShape(0.2f, 0.06f), // Lower Leg R
  50. };
  51. // Positions of body parts in world space
  52. RVec3 positions[] = {
  53. RVec3(0, 1.15f, 0), // Lower Body
  54. RVec3(0, 1.35f, 0), // Mid Body
  55. RVec3(0, 1.55f, 0), // Upper Body
  56. RVec3(0, 1.825f, 0), // Head
  57. RVec3(-0.425f, 1.55f, 0), // Upper Arm L
  58. RVec3(0.425f, 1.55f, 0), // Upper Arm R
  59. RVec3(-0.8f, 1.55f, 0), // Lower Arm L
  60. RVec3(0.8f, 1.55f, 0), // Lower Arm R
  61. RVec3(-0.15f, 0.8f, 0), // Upper Leg L
  62. RVec3(0.15f, 0.8f, 0), // Upper Leg R
  63. RVec3(-0.15f, 0.3f, 0), // Lower Leg L
  64. RVec3(0.15f, 0.3f, 0), // Lower Leg R
  65. };
  66. // Rotations of body parts in world space
  67. Quat rotations[] = {
  68. Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI), // Lower Body
  69. Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI), // Mid Body
  70. Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI), // Upper Body
  71. Quat::sIdentity(), // Head
  72. Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI), // Upper Arm L
  73. Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI), // Upper Arm R
  74. Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI), // Lower Arm L
  75. Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI), // Lower Arm R
  76. Quat::sIdentity(), // Upper Leg L
  77. Quat::sIdentity(), // Upper Leg R
  78. Quat::sIdentity(), // Lower Leg L
  79. Quat::sIdentity() // Lower Leg R
  80. };
  81. // World space constraint positions
  82. RVec3 constraint_positions[] = {
  83. RVec3::sZero(), // Lower Body (unused, there's no parent)
  84. RVec3(0, 1.25f, 0), // Mid Body
  85. RVec3(0, 1.45f, 0), // Upper Body
  86. RVec3(0, 1.65f, 0), // Head
  87. RVec3(-0.225f, 1.55f, 0), // Upper Arm L
  88. RVec3(0.225f, 1.55f, 0), // Upper Arm R
  89. RVec3(-0.65f, 1.55f, 0), // Lower Arm L
  90. RVec3(0.65f, 1.55f, 0), // Lower Arm R
  91. RVec3(-0.15f, 1.05f, 0), // Upper Leg L
  92. RVec3(0.15f, 1.05f, 0), // Upper Leg R
  93. RVec3(-0.15f, 0.55f, 0), // Lower Leg L
  94. RVec3(0.15f, 0.55f, 0), // Lower Leg R
  95. };
  96. // World space twist axis directions
  97. Vec3 twist_axis[] = {
  98. Vec3::sZero(), // Lower Body (unused, there's no parent)
  99. Vec3::sAxisY(), // Mid Body
  100. Vec3::sAxisY(), // Upper Body
  101. Vec3::sAxisY(), // Head
  102. -Vec3::sAxisX(), // Upper Arm L
  103. Vec3::sAxisX(), // Upper Arm R
  104. -Vec3::sAxisX(), // Lower Arm L
  105. Vec3::sAxisX(), // Lower Arm R
  106. -Vec3::sAxisY(), // Upper Leg L
  107. -Vec3::sAxisY(), // Upper Leg R
  108. -Vec3::sAxisY(), // Lower Leg L
  109. -Vec3::sAxisY(), // Lower Leg R
  110. };
  111. // Constraint limits
  112. float twist_angle[] = {
  113. 0.0f, // Lower Body (unused, there's no parent)
  114. 5.0f, // Mid Body
  115. 5.0f, // Upper Body
  116. 90.0f, // Head
  117. 45.0f, // Upper Arm L
  118. 45.0f, // Upper Arm R
  119. 45.0f, // Lower Arm L
  120. 45.0f, // Lower Arm R
  121. 45.0f, // Upper Leg L
  122. 45.0f, // Upper Leg R
  123. 45.0f, // Lower Leg L
  124. 45.0f, // Lower Leg R
  125. };
  126. float normal_angle[] = {
  127. 0.0f, // Lower Body (unused, there's no parent)
  128. 10.0f, // Mid Body
  129. 10.0f, // Upper Body
  130. 45.0f, // Head
  131. 90.0f, // Upper Arm L
  132. 90.0f, // Upper Arm R
  133. 0.0f, // Lower Arm L
  134. 0.0f, // Lower Arm R
  135. 45.0f, // Upper Leg L
  136. 45.0f, // Upper Leg R
  137. 0.0f, // Lower Leg L
  138. 0.0f, // Lower Leg R
  139. };
  140. float plane_angle[] = {
  141. 0.0f, // Lower Body (unused, there's no parent)
  142. 10.0f, // Mid Body
  143. 10.0f, // Upper Body
  144. 45.0f, // Head
  145. 45.0f, // Upper Arm L
  146. 45.0f, // Upper Arm R
  147. 90.0f, // Lower Arm L
  148. 90.0f, // Lower Arm R
  149. 45.0f, // Upper Leg L
  150. 45.0f, // Upper Leg R
  151. 60.0f, // Lower Leg L (cheating here, a knee is not symmetric, we should have rotated the twist axis)
  152. 60.0f, // Lower Leg R
  153. };
  154. // Create ragdoll settings
  155. Ref<RagdollSettings> settings = new RagdollSettings;
  156. settings->mSkeleton = skeleton;
  157. settings->mParts.resize(skeleton->GetJointCount());
  158. for (int p = 0; p < skeleton->GetJointCount(); ++p)
  159. {
  160. RagdollSettings::Part &part = settings->mParts[p];
  161. part.SetShape(shapes[p]);
  162. part.mPosition = positions[p];
  163. part.mRotation = rotations[p];
  164. part.mMotionType = EMotionType::Dynamic;
  165. part.mObjectLayer = Layers::MOVING;
  166. // First part is the root, doesn't have a parent and doesn't have a constraint
  167. if (p > 0)
  168. {
  169. SwingTwistConstraintSettings *constraint = new SwingTwistConstraintSettings;
  170. constraint->mDrawConstraintSize = 0.1f;
  171. constraint->mPosition1 = constraint->mPosition2 = constraint_positions[p];
  172. constraint->mTwistAxis1 = constraint->mTwistAxis2 = twist_axis[p];
  173. constraint->mPlaneAxis1 = constraint->mPlaneAxis2 = Vec3::sAxisZ();
  174. constraint->mTwistMinAngle = -DegreesToRadians(twist_angle[p]);
  175. constraint->mTwistMaxAngle = DegreesToRadians(twist_angle[p]);
  176. constraint->mNormalHalfConeAngle = DegreesToRadians(normal_angle[p]);
  177. constraint->mPlaneHalfConeAngle = DegreesToRadians(plane_angle[p]);
  178. part.mToParent = constraint;
  179. }
  180. }
  181. // Optional: Stabilize the inertia of the limbs
  182. settings->Stabilize();
  183. // Disable parent child collisions so that we don't get collisions between constrained bodies
  184. settings->DisableParentChildCollisions();
  185. // Create ragdoll
  186. mRagdoll = settings->CreateRagdoll(0, 0, mPhysicsSystem);
  187. mRagdoll->AddToPhysicsSystem(EActivation::Activate);
  188. }