2
0

OrientedBox.cpp 7.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177
  1. // SPDX-FileCopyrightText: 2021 Jorrit Rouwe
  2. // SPDX-License-Identifier: MIT
  3. #include <Jolt/Jolt.h>
  4. #include <Jolt/Geometry/AABox.h>
  5. #include <Jolt/Geometry/OrientedBox.h>
  6. JPH_NAMESPACE_BEGIN
  7. bool OrientedBox::Overlaps(const AABox &inBox, float inEpsilon) const
  8. {
  9. // Taken from: Real Time Collision Detection - Christer Ericson
  10. // Chapter 4.4.1, page 103-105.
  11. // Note that the code is swapped around: A is the aabox and B is the oriented box (this saves us from having to invert the orientation of the oriented box)
  12. // Convert AABox to center / extent representation
  13. Vec3 a_center = inBox.GetCenter();
  14. Vec3 a_half_extents = inBox.GetExtent();
  15. // Compute rotation matrix expressing b in a's coordinate frame
  16. Mat44 rot(mOrientation.GetColumn4(0), mOrientation.GetColumn4(1), mOrientation.GetColumn4(2), mOrientation.GetColumn4(3) - Vec4(a_center, 0));
  17. // Compute common subexpressions. Add in an epsilon term to
  18. // counteract arithmetic errors when two edges are parallel and
  19. // their cross product is (near) null (see text for details)
  20. Vec3 epsilon = Vec3::sReplicate(inEpsilon);
  21. Vec3 abs_r[3] { rot.GetAxisX().Abs() + epsilon, rot.GetAxisY().Abs() + epsilon, rot.GetAxisZ().Abs() + epsilon };
  22. // Test axes L = A0, L = A1, L = A2
  23. float ra, rb;
  24. for (int i = 0; i < 3; i++)
  25. {
  26. ra = a_half_extents[i];
  27. rb = mHalfExtents[0] * abs_r[0][i] + mHalfExtents[1] * abs_r[1][i] + mHalfExtents[2] * abs_r[2][i];
  28. if (abs(rot(i, 3)) > ra + rb) return false;
  29. }
  30. // Test axes L = B0, L = B1, L = B2
  31. for (int i = 0; i < 3; i++)
  32. {
  33. ra = a_half_extents.Dot(abs_r[i]);
  34. rb = mHalfExtents[i];
  35. if (abs(rot.GetTranslation().Dot(rot.GetColumn3(i))) > ra + rb) return false;
  36. }
  37. // Test axis L = A0 x B0
  38. ra = a_half_extents[1] * abs_r[0][2] + a_half_extents[2] * abs_r[0][1];
  39. rb = mHalfExtents[1] * abs_r[2][0] + mHalfExtents[2] * abs_r[1][0];
  40. if (abs(rot(2, 3) * rot(1, 0) - rot(1, 3) * rot(2, 0)) > ra + rb) return false;
  41. // Test axis L = A0 x B1
  42. ra = a_half_extents[1] * abs_r[1][2] + a_half_extents[2] * abs_r[1][1];
  43. rb = mHalfExtents[0] * abs_r[2][0] + mHalfExtents[2] * abs_r[0][0];
  44. if (abs(rot(2, 3) * rot(1, 1) - rot(1, 3) * rot(2, 1)) > ra + rb) return false;
  45. // Test axis L = A0 x B2
  46. ra = a_half_extents[1] * abs_r[2][2] + a_half_extents[2] * abs_r[2][1];
  47. rb = mHalfExtents[0] * abs_r[1][0] + mHalfExtents[1] * abs_r[0][0];
  48. if (abs(rot(2, 3) * rot(1, 2) - rot(1, 3) * rot(2, 2)) > ra + rb) return false;
  49. // Test axis L = A1 x B0
  50. ra = a_half_extents[0] * abs_r[0][2] + a_half_extents[2] * abs_r[0][0];
  51. rb = mHalfExtents[1] * abs_r[2][1] + mHalfExtents[2] * abs_r[1][1];
  52. if (abs(rot(0, 3) * rot(2, 0) - rot(2, 3) * rot(0, 0)) > ra + rb) return false;
  53. // Test axis L = A1 x B1
  54. ra = a_half_extents[0] * abs_r[1][2] + a_half_extents[2] * abs_r[1][0];
  55. rb = mHalfExtents[0] * abs_r[2][1] + mHalfExtents[2] * abs_r[0][1];
  56. if (abs(rot(0, 3) * rot(2, 1) - rot(2, 3) * rot(0, 1)) > ra + rb) return false;
  57. // Test axis L = A1 x B2
  58. ra = a_half_extents[0] * abs_r[2][2] + a_half_extents[2] * abs_r[2][0];
  59. rb = mHalfExtents[0] * abs_r[1][1] + mHalfExtents[1] * abs_r[0][1];
  60. if (abs(rot(0, 3) * rot(2, 2) - rot(2, 3) * rot(0, 2)) > ra + rb) return false;
  61. // Test axis L = A2 x B0
  62. ra = a_half_extents[0] * abs_r[0][1] + a_half_extents[1] * abs_r[0][0];
  63. rb = mHalfExtents[1] * abs_r[2][2] + mHalfExtents[2] * abs_r[1][2];
  64. if (abs(rot(1, 3) * rot(0, 0) - rot(0, 3) * rot(1, 0)) > ra + rb) return false;
  65. // Test axis L = A2 x B1
  66. ra = a_half_extents[0] * abs_r[1][1] + a_half_extents[1] * abs_r[1][0];
  67. rb = mHalfExtents[0] * abs_r[2][2] + mHalfExtents[2] * abs_r[0][2];
  68. if (abs(rot(1, 3) * rot(0, 1) - rot(0, 3) * rot(1, 1)) > ra + rb) return false;
  69. // Test axis L = A2 x B2
  70. ra = a_half_extents[0] * abs_r[2][1] + a_half_extents[1] * abs_r[2][0];
  71. rb = mHalfExtents[0] * abs_r[1][2] + mHalfExtents[1] * abs_r[0][2];
  72. if (abs(rot(1, 3) * rot(0, 2) - rot(0, 3) * rot(1, 2)) > ra + rb) return false;
  73. // Since no separating axis is found, the OBB and AAB must be intersecting
  74. return true;
  75. }
  76. bool OrientedBox::Overlaps(const OrientedBox &inBox, float inEpsilon) const
  77. {
  78. // Taken from: Real Time Collision Detection - Christer Ericson
  79. // Chapter 4.4.1, page 103-105.
  80. // Note that A is this, B is inBox
  81. // Compute rotation matrix expressing b in a's coordinate frame
  82. Mat44 rot = mOrientation.InversedRotationTranslation() * inBox.mOrientation;
  83. // Compute common subexpressions. Add in an epsilon term to
  84. // counteract arithmetic errors when two edges are parallel and
  85. // their cross product is (near) null (see text for details)
  86. Vec3 epsilon = Vec3::sReplicate(inEpsilon);
  87. Vec3 abs_r[3] { rot.GetAxisX().Abs() + epsilon, rot.GetAxisY().Abs() + epsilon, rot.GetAxisZ().Abs() + epsilon };
  88. // Test axes L = A0, L = A1, L = A2
  89. float ra, rb;
  90. for (int i = 0; i < 3; i++)
  91. {
  92. ra = mHalfExtents[i];
  93. rb = inBox.mHalfExtents[0] * abs_r[0][i] + inBox.mHalfExtents[1] * abs_r[1][i] + inBox.mHalfExtents[2] * abs_r[2][i];
  94. if (abs(rot(i, 3)) > ra + rb) return false;
  95. }
  96. // Test axes L = B0, L = B1, L = B2
  97. for (int i = 0; i < 3; i++)
  98. {
  99. ra = mHalfExtents.Dot(abs_r[i]);
  100. rb = inBox.mHalfExtents[i];
  101. if (abs(rot.GetTranslation().Dot(rot.GetColumn3(i))) > ra + rb) return false;
  102. }
  103. // Test axis L = A0 x B0
  104. ra = mHalfExtents[1] * abs_r[0][2] + mHalfExtents[2] * abs_r[0][1];
  105. rb = inBox.mHalfExtents[1] * abs_r[2][0] + inBox.mHalfExtents[2] * abs_r[1][0];
  106. if (abs(rot(2, 3) * rot(1, 0) - rot(1, 3) * rot(2, 0)) > ra + rb) return false;
  107. // Test axis L = A0 x B1
  108. ra = mHalfExtents[1] * abs_r[1][2] + mHalfExtents[2] * abs_r[1][1];
  109. rb = inBox.mHalfExtents[0] * abs_r[2][0] + inBox.mHalfExtents[2] * abs_r[0][0];
  110. if (abs(rot(2, 3) * rot(1, 1) - rot(1, 3) * rot(2, 1)) > ra + rb) return false;
  111. // Test axis L = A0 x B2
  112. ra = mHalfExtents[1] * abs_r[2][2] + mHalfExtents[2] * abs_r[2][1];
  113. rb = inBox.mHalfExtents[0] * abs_r[1][0] + inBox.mHalfExtents[1] * abs_r[0][0];
  114. if (abs(rot(2, 3) * rot(1, 2) - rot(1, 3) * rot(2, 2)) > ra + rb) return false;
  115. // Test axis L = A1 x B0
  116. ra = mHalfExtents[0] * abs_r[0][2] + mHalfExtents[2] * abs_r[0][0];
  117. rb = inBox.mHalfExtents[1] * abs_r[2][1] + inBox.mHalfExtents[2] * abs_r[1][1];
  118. if (abs(rot(0, 3) * rot(2, 0) - rot(2, 3) * rot(0, 0)) > ra + rb) return false;
  119. // Test axis L = A1 x B1
  120. ra = mHalfExtents[0] * abs_r[1][2] + mHalfExtents[2] * abs_r[1][0];
  121. rb = inBox.mHalfExtents[0] * abs_r[2][1] + inBox.mHalfExtents[2] * abs_r[0][1];
  122. if (abs(rot(0, 3) * rot(2, 1) - rot(2, 3) * rot(0, 1)) > ra + rb) return false;
  123. // Test axis L = A1 x B2
  124. ra = mHalfExtents[0] * abs_r[2][2] + mHalfExtents[2] * abs_r[2][0];
  125. rb = inBox.mHalfExtents[0] * abs_r[1][1] + inBox.mHalfExtents[1] * abs_r[0][1];
  126. if (abs(rot(0, 3) * rot(2, 2) - rot(2, 3) * rot(0, 2)) > ra + rb) return false;
  127. // Test axis L = A2 x B0
  128. ra = mHalfExtents[0] * abs_r[0][1] + mHalfExtents[1] * abs_r[0][0];
  129. rb = inBox.mHalfExtents[1] * abs_r[2][2] + inBox.mHalfExtents[2] * abs_r[1][2];
  130. if (abs(rot(1, 3) * rot(0, 0) - rot(0, 3) * rot(1, 0)) > ra + rb) return false;
  131. // Test axis L = A2 x B1
  132. ra = mHalfExtents[0] * abs_r[1][1] + mHalfExtents[1] * abs_r[1][0];
  133. rb = inBox.mHalfExtents[0] * abs_r[2][2] + inBox.mHalfExtents[2] * abs_r[0][2];
  134. if (abs(rot(1, 3) * rot(0, 1) - rot(0, 3) * rot(1, 1)) > ra + rb) return false;
  135. // Test axis L = A2 x B2
  136. ra = mHalfExtents[0] * abs_r[2][1] + mHalfExtents[1] * abs_r[2][0];
  137. rb = inBox.mHalfExtents[0] * abs_r[1][2] + inBox.mHalfExtents[1] * abs_r[0][2];
  138. if (abs(rot(1, 3) * rot(0, 2) - rot(0, 3) * rot(1, 2)) > ra + rb) return false;
  139. // Since no separating axis is found, the OBBs must be intersecting
  140. return true;
  141. }
  142. JPH_NAMESPACE_END