CastShapeTests.cpp 14 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271
  1. // SPDX-FileCopyrightText: 2021 Jorrit Rouwe
  2. // SPDX-License-Identifier: MIT
  3. #include "UnitTestFramework.h"
  4. #include <Jolt/Physics/Collision/ShapeCast.h>
  5. #include <Jolt/Physics/Collision/CastResult.h>
  6. #include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
  7. #include <Jolt/Physics/Collision/Shape/SphereShape.h>
  8. #include <Jolt/Physics/Collision/Shape/TriangleShape.h>
  9. #include <Jolt/Physics/Collision/Shape/MeshShape.h>
  10. #include <Jolt/Physics/Collision/Shape/ScaledShape.h>
  11. #include <Jolt/Physics/Collision/Shape/BoxShape.h>
  12. #include <Jolt/Physics/Collision/ShapeFilter.h>
  13. #include <Jolt/Physics/Collision/CollisionDispatch.h>
  14. #include "PhysicsTestContext.h"
  15. #include "Layers.h"
  16. TEST_SUITE("CastShapeTests")
  17. {
  18. /// Helper function that tests a sphere against a triangle
  19. static void sTestCastSphereVertexOrEdge(const Shape *inSphere, Vec3Arg inPosition, Vec3Arg inDirection, const Shape *inTriangle)
  20. {
  21. ShapeCast shape_cast(inSphere, Vec3::sReplicate(1.0f), Mat44::sTranslation(inPosition - inDirection), inDirection);
  22. ShapeCastSettings cast_settings;
  23. cast_settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces;
  24. cast_settings.mBackFaceModeConvex = EBackFaceMode::CollideWithBackFaces;
  25. AllHitCollisionCollector<CastShapeCollector> collector;
  26. CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
  27. CHECK(collector.mHits.size() == 1);
  28. const ShapeCastResult &result = collector.mHits.back();
  29. CHECK_APPROX_EQUAL(result.mFraction, 1.0f - 0.2f / inDirection.Length(), 1.0e-4f);
  30. CHECK_APPROX_EQUAL(result.mPenetrationAxis.Normalized(), inDirection.Normalized(), 1.0e-3f);
  31. CHECK_APPROX_EQUAL(result.mPenetrationDepth, 0.0f, 1.0e-3f);
  32. CHECK_APPROX_EQUAL(result.mContactPointOn1, inPosition, 1.0e-3f);
  33. CHECK_APPROX_EQUAL(result.mContactPointOn2, inPosition, 1.0e-3f);
  34. }
  35. /// Helper function that tests a shere against a triangle centered on the origin with normal Z
  36. static void sTestCastSphereTriangle(const Shape *inTriangle)
  37. {
  38. // Create sphere
  39. Ref<Shape> sphere = SphereShapeSettings(0.2f).Create().Get();
  40. {
  41. // Hit front face
  42. ShapeCast shape_cast(sphere, Vec3::sReplicate(1.0f), Mat44::sTranslation(Vec3(0, 0, 15)), Vec3(0, 0, -30));
  43. ShapeCastSettings cast_settings;
  44. cast_settings.mBackFaceModeTriangles = EBackFaceMode::IgnoreBackFaces;
  45. cast_settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces;
  46. cast_settings.mReturnDeepestPoint = false;
  47. AllHitCollisionCollector<CastShapeCollector> collector;
  48. CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
  49. CHECK(collector.mHits.size() == 1);
  50. const ShapeCastResult &result = collector.mHits.back();
  51. CHECK_APPROX_EQUAL(result.mFraction, (15.0f - 0.2f) / 30.0f, 1.0e-4f);
  52. CHECK_APPROX_EQUAL(result.mPenetrationAxis.Normalized(), Vec3(0, 0, -1), 1.0e-3f);
  53. CHECK(result.mPenetrationDepth == 0.0f);
  54. CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3::sZero(), 1.0e-3f);
  55. CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3::sZero(), 1.0e-3f);
  56. CHECK(!result.mIsBackFaceHit);
  57. }
  58. {
  59. // Hit back face -> ignored
  60. ShapeCast shape_cast(sphere, Vec3::sReplicate(1.0f), Mat44::sTranslation(Vec3(0, 0, -15)), Vec3(0, 0, 30));
  61. ShapeCastSettings cast_settings;
  62. cast_settings.mBackFaceModeTriangles = EBackFaceMode::IgnoreBackFaces;
  63. cast_settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces;
  64. cast_settings.mReturnDeepestPoint = false;
  65. AllHitCollisionCollector<CastShapeCollector> collector;
  66. CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
  67. CHECK(collector.mHits.empty());
  68. // Hit back face -> collision
  69. cast_settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces;
  70. cast_settings.mBackFaceModeConvex = EBackFaceMode::CollideWithBackFaces;
  71. CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
  72. CHECK(collector.mHits.size() == 1);
  73. const ShapeCastResult &result = collector.mHits.back();
  74. CHECK_APPROX_EQUAL(result.mFraction, (15.0f - 0.2f) / 30.0f, 1.0e-4f);
  75. CHECK_APPROX_EQUAL(result.mPenetrationAxis.Normalized(), Vec3(0, 0, 1), 1.0e-3f);
  76. CHECK(result.mPenetrationDepth == 0.0f);
  77. CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3::sZero(), 1.0e-3f);
  78. CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3::sZero(), 1.0e-3f);
  79. CHECK(result.mIsBackFaceHit);
  80. }
  81. {
  82. // Hit back face while starting in collision -> ignored
  83. ShapeCast shape_cast(sphere, Vec3::sReplicate(1.0f), Mat44::sTranslation(Vec3(0, 0, -0.1f)), Vec3(0, 0, 15));
  84. ShapeCastSettings cast_settings;
  85. cast_settings.mBackFaceModeTriangles = EBackFaceMode::IgnoreBackFaces;
  86. cast_settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces;
  87. cast_settings.mReturnDeepestPoint = true;
  88. AllHitCollisionCollector<CastShapeCollector> collector;
  89. CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
  90. CHECK(collector.mHits.empty());
  91. // Hit back face while starting in collision -> collision
  92. cast_settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces;
  93. cast_settings.mBackFaceModeConvex = EBackFaceMode::CollideWithBackFaces;
  94. CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, cast_settings, inTriangle, Vec3::sReplicate(1.0f), ShapeFilter(), Mat44::sIdentity(), SubShapeIDCreator(), SubShapeIDCreator(), collector);
  95. CHECK(collector.mHits.size() == 1);
  96. const ShapeCastResult &result = collector.mHits.back();
  97. CHECK_APPROX_EQUAL(result.mFraction, 0.0f);
  98. CHECK_APPROX_EQUAL(result.mPenetrationAxis.Normalized(), Vec3(0, 0, 1), 1.0e-3f);
  99. CHECK_APPROX_EQUAL(result.mPenetrationDepth, 0.1f, 1.0e-3f);
  100. CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3(0, 0, 0.1f), 1.0e-3f);
  101. CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3::sZero(), 1.0e-3f);
  102. CHECK(result.mIsBackFaceHit);
  103. }
  104. // Hit vertex 1, 2 and 3
  105. sTestCastSphereVertexOrEdge(sphere, Vec3(50, 25, 0), Vec3(-10, -10, 0), inTriangle);
  106. sTestCastSphereVertexOrEdge(sphere, Vec3(-50, 25, 0), Vec3(10, -10, 0), inTriangle);
  107. sTestCastSphereVertexOrEdge(sphere, Vec3(0, -25, 0), Vec3(0, 10, 0), inTriangle);
  108. // Hit edge 1, 2 and 3
  109. sTestCastSphereVertexOrEdge(sphere, Vec3(0, 25, 0), Vec3(0, -10, 0), inTriangle); // Edge: Vec3(50, 25, 0), Vec3(-50, 25, 0)
  110. sTestCastSphereVertexOrEdge(sphere, Vec3(-25, 0, 0), Vec3(10, 10, 0), inTriangle); // Edge: Vec3(-50, 25, 0), Vec3(0,-25, 0)
  111. sTestCastSphereVertexOrEdge(sphere, Vec3(25, 0, 0), Vec3(-10, 10, 0), inTriangle); // Edge: Float3(0,-25, 0), Float3(50, 25, 0)
  112. }
  113. TEST_CASE("TestCastSphereTriangle")
  114. {
  115. // Create triangle
  116. Ref<Shape> triangle = TriangleShapeSettings(Vec3(50, 25, 0), Vec3(-50, 25, 0), Vec3(0,-25, 0)).Create().Get();
  117. sTestCastSphereTriangle(triangle);
  118. // Create a triangle mesh shape
  119. Ref<Shape> triangle_mesh = MeshShapeSettings({ Triangle(Float3(50, 25, 0), Float3(-50, 25, 0), Float3(0,-25, 0)) }).Create().Get();
  120. sTestCastSphereTriangle(triangle_mesh);
  121. }
  122. // Test CastShape for a (scaled) sphere vs box
  123. TEST_CASE("TestCastShapeSphereVsBox")
  124. {
  125. PhysicsTestContext c;
  126. // Create box to collide against (shape 2)
  127. // The box is scaled up by a factor 10 in the X axis and then rotated so that the X axis is up
  128. BoxShapeSettings box(Vec3::sReplicate(1.0f));
  129. box.SetEmbedded();
  130. ScaledShapeSettings scaled_box(&box, Vec3(10, 1, 1));
  131. scaled_box.SetEmbedded();
  132. Body &body2 = c.CreateBody(&scaled_box, Vec3(0, 1, 0), Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate);
  133. // Set settings
  134. ShapeCastSettings settings;
  135. settings.mReturnDeepestPoint = true;
  136. settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces;
  137. settings.mBackFaceModeConvex = EBackFaceMode::CollideWithBackFaces;
  138. {
  139. // Create shape cast
  140. Ref<Shape> normal_sphere = new SphereShape(1.0f);
  141. ShapeCast shape_cast { normal_sphere, Vec3::sReplicate(1.0f), Mat44::sTranslation(Vec3(0, 11, 0)), Vec3(0, 1, 0) };
  142. // Shape is intersecting at the start
  143. AllHitCollisionCollector<CastShapeCollector> collector;
  144. c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, settings, collector);
  145. CHECK(collector.mHits.size() == 1);
  146. const ShapeCastResult &result = collector.mHits.front();
  147. CHECK(result.mBodyID2 == body2.GetID());
  148. CHECK_APPROX_EQUAL(result.mFraction, 0.0f);
  149. CHECK_APPROX_EQUAL(result.mPenetrationAxis.Normalized(), Vec3(0, -1, 0), 1.0e-3f);
  150. CHECK_APPROX_EQUAL(result.mPenetrationDepth, 1.0f, 1.0e-5f);
  151. CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3(0, 10, 0), 1.0e-3f);
  152. CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3(0, 11, 0), 1.0e-3f);
  153. CHECK(!result.mIsBackFaceHit);
  154. }
  155. {
  156. // This repeats the same test as above but uses scaling at all levels and validate that the penetration depth is still correct
  157. Ref<Shape> scaled_sphere = new ScaledShape(new SphereShape(0.1f), Vec3::sReplicate(5.0f));
  158. ShapeCast shape_cast { scaled_sphere, Vec3::sReplicate(2.0f), Mat44::sTranslation(Vec3(0, 11, 0)), Vec3(0, 1, 0) };
  159. // Shape is intersecting at the start
  160. AllHitCollisionCollector<CastShapeCollector> collector;
  161. c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, settings, collector);
  162. CHECK(collector.mHits.size() == 1);
  163. const ShapeCastResult &result = collector.mHits.front();
  164. CHECK(result.mBodyID2 == body2.GetID());
  165. CHECK_APPROX_EQUAL(result.mFraction, 0.0f);
  166. CHECK_APPROX_EQUAL(result.mPenetrationAxis.Normalized(), Vec3(0, -1, 0), 1.0e-3f);
  167. CHECK_APPROX_EQUAL(result.mPenetrationDepth, 1.0f, 1.0e-5f);
  168. CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3(0, 10, 0), 1.0e-3f);
  169. CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3(0, 11, 0), 1.0e-3f);
  170. CHECK(!result.mIsBackFaceHit);
  171. }
  172. }
  173. // Test CastShape ordering according to penetration depth
  174. TEST_CASE("TestCastShapePenetrationDepthOrdering")
  175. {
  176. PhysicsTestContext c;
  177. // Create box to collide against (shape 2)
  178. BoxShapeSettings box(Vec3(0.1f, 2.0f, 2.0f));
  179. box.SetEmbedded();
  180. // Create 10 boxes that are 0.2 thick in the X axis and 4 in Y and Z, put them all next to each other on the X axis starting from X = 0 going to X = 2
  181. Array<Body *> bodies;
  182. for (int i = 0; i < 10; ++i)
  183. bodies.push_back(&c.CreateBody(&box, Vec3(0.1f + 0.2f * i, 0, 0), Quat::sIdentity(), EMotionType::Static, EMotionQuality::Discrete, Layers::NON_MOVING, EActivation::DontActivate));
  184. // Set settings
  185. ShapeCastSettings settings;
  186. settings.mReturnDeepestPoint = true;
  187. settings.mBackFaceModeTriangles = EBackFaceMode::CollideWithBackFaces;
  188. settings.mBackFaceModeConvex = EBackFaceMode::CollideWithBackFaces;
  189. settings.mCollisionTolerance = 1.0e-5f; // Increased precision
  190. settings.mPenetrationTolerance = 1.0e-5f;
  191. {
  192. // Create shape cast in X from -5 to 5
  193. RefConst<Shape> sphere = new SphereShape(1.0f);
  194. ShapeCast shape_cast { sphere, Vec3::sReplicate(1.0f), Mat44::sTranslation(Vec3(-5, 0, 0)), Vec3(10, 0, 0) };
  195. // We should hit the first body
  196. ClosestHitCollisionCollector<CastShapeCollector> collector;
  197. c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, settings, collector);
  198. CHECK(collector.HadHit());
  199. CHECK(collector.mHit.mBodyID2 == bodies.front()->GetID());
  200. CHECK_APPROX_EQUAL(collector.mHit.mFraction, 4.0f / 10.0f);
  201. CHECK_APPROX_EQUAL(collector.mHit.mPenetrationAxis.Normalized(), Vec3(1, 0, 0), 2.0e-3f);
  202. CHECK_APPROX_EQUAL(collector.mHit.mPenetrationDepth, 0.0f);
  203. CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn1, Vec3(0, 0, 0), 1.0e-4f);
  204. CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn2, Vec3(0, 0, 0), 1.0e-4f);
  205. CHECK(!collector.mHit.mIsBackFaceHit);
  206. }
  207. {
  208. // Create shape cast in X from 5 to -5
  209. RefConst<Shape> sphere = new SphereShape(1.0f);
  210. ShapeCast shape_cast { sphere, Vec3::sReplicate(1.0f), Mat44::sTranslation(Vec3(5, 0, 0)), Vec3(-10, 0, 0) };
  211. // We should hit the last body
  212. ClosestHitCollisionCollector<CastShapeCollector> collector;
  213. c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, settings, collector);
  214. CHECK(collector.HadHit());
  215. CHECK(collector.mHit.mBodyID2 == bodies.back()->GetID());
  216. CHECK_APPROX_EQUAL(collector.mHit.mFraction, 2.0f / 10.0f, 1.0e-4f);
  217. CHECK_APPROX_EQUAL(collector.mHit.mPenetrationAxis.Normalized(), Vec3(-1, 0, 0), 2.0e-3f);
  218. CHECK_APPROX_EQUAL(collector.mHit.mPenetrationDepth, 0.0f);
  219. CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn1, Vec3(2, 0, 0), 4.0e-4f);
  220. CHECK_APPROX_EQUAL(collector.mHit.mContactPointOn2, Vec3(2, 0, 0), 4.0e-4f);
  221. CHECK(!collector.mHit.mIsBackFaceHit);
  222. }
  223. {
  224. // Create shape cast in X from 1.05 to 11, this should intersect with all bodies and have deepest penetration in bodies[5]
  225. RefConst<Shape> sphere = new SphereShape(1.0f);
  226. ShapeCast shape_cast { sphere, Vec3::sReplicate(1.0f), Mat44::sTranslation(Vec3(1.05f, 0, 0)), Vec3(10, 0, 0) };
  227. // We should hit bodies[5]
  228. AllHitCollisionCollector<CastShapeCollector> collector;
  229. c.GetSystem()->GetNarrowPhaseQuery().CastShape(shape_cast, settings, collector);
  230. collector.Sort();
  231. CHECK(collector.mHits.size() == 10);
  232. const ShapeCastResult &result = collector.mHits.front();
  233. CHECK(result.mBodyID2 == bodies[5]->GetID());
  234. CHECK_APPROX_EQUAL(result.mFraction, 0.0f);
  235. CHECK_APPROX_EQUAL(result.mPenetrationAxis.Normalized(), Vec3(1, 0, 0), 1.0e-3f);
  236. CHECK_APPROX_EQUAL(result.mPenetrationDepth, 1.05f);
  237. CHECK_APPROX_EQUAL(result.mContactPointOn1, Vec3(2.05f, 0, 0), 1.0e-5f); // Box starts at 1.0, center of sphere adds 0.05, radius of sphere is 1
  238. CHECK_APPROX_EQUAL(result.mContactPointOn2, Vec3(1.0f, 0, 0), 1.0e-5f); // Box starts at 1.0
  239. CHECK(!result.mIsBackFaceHit);
  240. }
  241. }
  242. }