123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112 |
- // Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
- // SPDX-FileCopyrightText: 2023 Jorrit Rouwe
- // SPDX-License-Identifier: MIT
- #include "UnitTestFramework.h"
- #include <Jolt/Geometry/ClosestPoint.h>
- TEST_SUITE("ClosestPointTests")
- {
- // Test closest point from inPoint to triangle (inA, inB, inC)
- inline static void TestClosestPointToTriangle(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inPoint, Vec3Arg inExpectedClosestPoint, uint32 inExpectedSet)
- {
- // Make triangle relative to inPoint so we can get the closest poin to the origin
- Vec3 a = inA - inPoint;
- Vec3 b = inB - inPoint;
- Vec3 c = inC - inPoint;
- // Extract bits for A, B and C
- uint32 expected_a = inExpectedSet & 1;
- uint32 expected_b = (inExpectedSet & 2) >> 1;
- uint32 expected_c = (inExpectedSet & 4) >> 2;
- // Test all permutations of ABC
- uint32 set = 0;
- Vec3 closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(a, b, c, set);
- CHECK(set == inExpectedSet);
- CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
- closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(a, c, b, set);
- CHECK(set == ((expected_b << 2) | (expected_c << 1) | expected_a));
- CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
- closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(b, a, c, set);
- CHECK(set == ((expected_c << 2) | (expected_a << 1) | expected_b));
- CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
- closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(b, c, a, set);
- CHECK(set == ((expected_a << 2) | (expected_c << 1) | expected_b));
- CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
- closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(c, a, b, set);
- CHECK(set == ((expected_b << 2) | (expected_a << 1) | expected_c));
- CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
- closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(c, b, a, set);
- CHECK(set == ((expected_a << 2) | (expected_b << 1) | expected_c));
- CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
- }
- TEST_CASE("TestLongTriangle")
- {
- Vec3 a(100, 1, 0);
- Vec3 b(100, 1, 1);
- Vec3 c(-100, 1, 0);
- // Test interior
- TestClosestPointToTriangle(a, b, c, Vec3(0, 0, 0.1f), Vec3(0, 1, 0.1f), 0b0111);
- // Edge AB
- TestClosestPointToTriangle(a, b, c, Vec3(101, 0, 0.5f), Vec3(100, 1, 0.5f), 0b0011);
- // Edge AC
- TestClosestPointToTriangle(a, b, c, Vec3(0, 0, -0.1f), Vec3(0, 1, 0), 0b0101);
- // Edge BC
- Vec3 point_bc(0, 0, 1);
- Vec3 bc = c - b;
- Vec3 closest_bc = b + ((point_bc - b).Dot(bc) / bc.LengthSq()) * bc;
- TestClosestPointToTriangle(a, b, c, point_bc, closest_bc, 0b0110);
- // Vertex A
- TestClosestPointToTriangle(a, b, c, Vec3(101, 0, -1), a, 0b0001);
- // Vertex B
- TestClosestPointToTriangle(a, b, c, Vec3(101, 0, 2), b, 0b0010);
- // Vertex C
- TestClosestPointToTriangle(a, b, c, Vec3(-101, 0, 0), c, 0b0100);
- }
- TEST_CASE("TestNearColinearTriangle")
- {
- // A very long triangle that is nearly colinear
- Vec3 a(99.9999847f, 0.946687222f, 99.9999847f);
- Vec3 b(-100.010002f, 0.977360725f, -100.010002f);
- Vec3 c(-100.000137f, 0.977310658f, -100.000137f);
- // Closest point is on edge AC
- Vec3 ac = c - a;
- Vec3 expected_closest = a + (-a.Dot(ac) / ac.LengthSq()) * ac;
- TestClosestPointToTriangle(a, b, c, Vec3::sZero(), expected_closest, 0b0101);
- }
- TEST_CASE("TestSmallTriangleWithPlaneGoingThroughOrigin")
- {
- // A small but non-degenerate triangle whose plane almost goes through the origin
- Vec3 a(-0.132395342f, -0.294095188f, -0.164812326f);
- Vec3 b(-0.126054004f, -0.283950001f, -0.159065604f);
- Vec3 c(-0.154956535f, -0.284792334f, -0.160523415f);
- float u, v, w;
- ClosestPoint::GetBaryCentricCoordinates(a, b, c, u, v, w);
- // Closest point should be close to origin
- Vec3 p = a * u + b * v + c * w;
- CHECK_APPROX_EQUAL(p, Vec3::sZero());
- // Closest point should be outside triangle
- CHECK((u < 0.0f || v > 0.0f || w < 0.0f));
- }
- }
|