ClosestPointTests.cpp 4.0 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112
  1. // Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
  2. // SPDX-FileCopyrightText: 2023 Jorrit Rouwe
  3. // SPDX-License-Identifier: MIT
  4. #include "UnitTestFramework.h"
  5. #include <Jolt/Geometry/ClosestPoint.h>
  6. TEST_SUITE("ClosestPointTests")
  7. {
  8. // Test closest point from inPoint to triangle (inA, inB, inC)
  9. inline static void TestClosestPointToTriangle(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inPoint, Vec3Arg inExpectedClosestPoint, uint32 inExpectedSet)
  10. {
  11. // Make triangle relative to inPoint so we can get the closest poin to the origin
  12. Vec3 a = inA - inPoint;
  13. Vec3 b = inB - inPoint;
  14. Vec3 c = inC - inPoint;
  15. // Extract bits for A, B and C
  16. uint32 expected_a = inExpectedSet & 1;
  17. uint32 expected_b = (inExpectedSet & 2) >> 1;
  18. uint32 expected_c = (inExpectedSet & 4) >> 2;
  19. // Test all permutations of ABC
  20. uint32 set = 0;
  21. Vec3 closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(a, b, c, set);
  22. CHECK(set == inExpectedSet);
  23. CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
  24. closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(a, c, b, set);
  25. CHECK(set == ((expected_b << 2) | (expected_c << 1) | expected_a));
  26. CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
  27. closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(b, a, c, set);
  28. CHECK(set == ((expected_c << 2) | (expected_a << 1) | expected_b));
  29. CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
  30. closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(b, c, a, set);
  31. CHECK(set == ((expected_a << 2) | (expected_c << 1) | expected_b));
  32. CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
  33. closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(c, a, b, set);
  34. CHECK(set == ((expected_b << 2) | (expected_a << 1) | expected_c));
  35. CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
  36. closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(c, b, a, set);
  37. CHECK(set == ((expected_a << 2) | (expected_b << 1) | expected_c));
  38. CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
  39. }
  40. TEST_CASE("TestLongTriangle")
  41. {
  42. Vec3 a(100, 1, 0);
  43. Vec3 b(100, 1, 1);
  44. Vec3 c(-100, 1, 0);
  45. // Test interior
  46. TestClosestPointToTriangle(a, b, c, Vec3(0, 0, 0.1f), Vec3(0, 1, 0.1f), 0b0111);
  47. // Edge AB
  48. TestClosestPointToTriangle(a, b, c, Vec3(101, 0, 0.5f), Vec3(100, 1, 0.5f), 0b0011);
  49. // Edge AC
  50. TestClosestPointToTriangle(a, b, c, Vec3(0, 0, -0.1f), Vec3(0, 1, 0), 0b0101);
  51. // Edge BC
  52. Vec3 point_bc(0, 0, 1);
  53. Vec3 bc = c - b;
  54. Vec3 closest_bc = b + ((point_bc - b).Dot(bc) / bc.LengthSq()) * bc;
  55. TestClosestPointToTriangle(a, b, c, point_bc, closest_bc, 0b0110);
  56. // Vertex A
  57. TestClosestPointToTriangle(a, b, c, Vec3(101, 0, -1), a, 0b0001);
  58. // Vertex B
  59. TestClosestPointToTriangle(a, b, c, Vec3(101, 0, 2), b, 0b0010);
  60. // Vertex C
  61. TestClosestPointToTriangle(a, b, c, Vec3(-101, 0, 0), c, 0b0100);
  62. }
  63. TEST_CASE("TestNearColinearTriangle")
  64. {
  65. // A very long triangle that is nearly colinear
  66. Vec3 a(99.9999847f, 0.946687222f, 99.9999847f);
  67. Vec3 b(-100.010002f, 0.977360725f, -100.010002f);
  68. Vec3 c(-100.000137f, 0.977310658f, -100.000137f);
  69. // Closest point is on edge AC
  70. Vec3 ac = c - a;
  71. Vec3 expected_closest = a + (-a.Dot(ac) / ac.LengthSq()) * ac;
  72. TestClosestPointToTriangle(a, b, c, Vec3::sZero(), expected_closest, 0b0101);
  73. }
  74. TEST_CASE("TestSmallTriangleWithPlaneGoingThroughOrigin")
  75. {
  76. // A small but non-degenerate triangle whose plane almost goes through the origin
  77. Vec3 a(-0.132395342f, -0.294095188f, -0.164812326f);
  78. Vec3 b(-0.126054004f, -0.283950001f, -0.159065604f);
  79. Vec3 c(-0.154956535f, -0.284792334f, -0.160523415f);
  80. float u, v, w;
  81. ClosestPoint::GetBaryCentricCoordinates(a, b, c, u, v, w);
  82. // Closest point should be close to origin
  83. Vec3 p = a * u + b * v + c * w;
  84. CHECK_APPROX_EQUAL(p, Vec3::sZero());
  85. // Closest point should be outside triangle
  86. CHECK((u < 0.0f || v > 0.0f || w < 0.0f));
  87. }
  88. }