Skip to content

Commit cb59a9d

Browse files
ThomasDuquetteThomasDuquette
authored
Merge pull request #75 from Dynamium-Lab/gjk-epa-reimplementation
GJK-EPA reimplementation
2 parents 83dd90d + d6e79f4 commit cb59a9d

11 files changed

Lines changed: 2217 additions & 131 deletions

File tree

‎CMakePresets.json‎

Lines changed: 130 additions & 129 deletions
Original file line numberDiff line numberDiff line change
@@ -1,130 +1,131 @@
11
{
2-
"version": 6,
3-
"configurePresets": [
4-
{
5-
"name": "base",
6-
"hidden": true,
7-
"cacheVariables": {
8-
"BUILD_SHARED_LIBS": "OFF"
9-
}
10-
},
11-
{
12-
"name": "dev",
13-
"inherits": "base",
14-
"displayName": "Developer build",
15-
"description": "Debug or Release with compile-commands for tooling. AVX2/native tuning is applied per dev target (see examples/ and tests/, which pick /arch:AVX2 for MSVC and -march=native otherwise), so this preset is correct for MSVC, GCC, Clang and MinGW on any OS.",
16-
"binaryDir": "${sourceDir}/build/${presetName}",
17-
"cacheVariables": {
18-
"CMAKE_BUILD_TYPE": "Release",
19-
"CMAKE_EXPORT_COMPILE_COMMANDS": "ON"
20-
}
21-
},
22-
{
23-
"name": "ci",
24-
"inherits": "base",
25-
"displayName": "CI build (MSVC/default)",
26-
"description": "Release build without machine-specific tuning",
27-
"binaryDir": "${sourceDir}/build/${presetName}",
28-
"cacheVariables": {
29-
"CMAKE_BUILD_TYPE": "Release",
30-
"CMAKE_EXPORT_COMPILE_COMMANDS": "ON"
31-
}
32-
},
33-
{
34-
"name": "ci-gcc",
35-
"inherits": "ci",
36-
"displayName": "CI build (GCC)",
37-
"binaryDir": "${sourceDir}/build/${presetName}",
38-
"cacheVariables": {
39-
"CMAKE_C_COMPILER": "gcc",
40-
"CMAKE_CXX_COMPILER": "g++"
41-
}
42-
},
43-
{
44-
"name": "ci-clang",
45-
"inherits": "ci",
46-
"displayName": "CI build (Clang)",
47-
"binaryDir": "${sourceDir}/build/${presetName}",
48-
"cacheVariables": {
49-
"CMAKE_C_COMPILER": "clang",
50-
"CMAKE_CXX_COMPILER": "clang++"
51-
}
52-
},
53-
{
54-
"name": "ci-float",
55-
"inherits": "ci",
56-
"displayName": "CI build (float / native SQP)",
57-
"description": "Single-precision build: blast::real = float, which requires the in-house SQP solver (external NLopt is double-only).",
58-
"binaryDir": "${sourceDir}/build/${presetName}",
59-
"cacheVariables": {
60-
"BLAST_USE_DOUBLES": "OFF",
61-
"BLAST_USE_NATIVE_SQP": "ON"
62-
}
63-
}
64-
],
65-
"buildPresets": [
66-
{
67-
"name": "dev-Debug",
68-
"configurePreset": "dev",
69-
"configuration": "Debug"
70-
},
71-
{"name": "dev-Release",
72-
"configurePreset": "dev",
73-
"configuration": "Release"
74-
},
75-
{
76-
"name": "ci",
77-
"configurePreset": "ci",
78-
"configuration": "Release"
79-
},
80-
{
81-
"name": "ci-gcc",
82-
"configurePreset": "ci-gcc",
83-
"configuration": "Release"
84-
},
85-
{
86-
"name": "ci-clang",
87-
"configurePreset": "ci-clang",
88-
"configuration": "Release"
89-
},
90-
{
91-
"name": "ci-float",
92-
"configurePreset": "ci-float",
93-
"configuration": "Release"
94-
}
95-
],
96-
"testPresets": [
97-
{
98-
"name": "ci",
99-
"configurePreset": "ci",
100-
"configuration": "Release",
101-
"output": {
102-
"outputOnFailure": true
103-
}
104-
},
105-
{
106-
"name": "ci-gcc",
107-
"configurePreset": "ci-gcc",
108-
"configuration": "Release",
109-
"output": {
110-
"outputOnFailure": true
111-
}
112-
},
113-
{
114-
"name": "ci-clang",
115-
"configurePreset": "ci-clang",
116-
"configuration": "Release",
117-
"output": {
118-
"outputOnFailure": true
119-
}
120-
},
121-
{
122-
"name": "ci-float",
123-
"configurePreset": "ci-float",
124-
"configuration": "Release",
125-
"output": {
126-
"outputOnFailure": true
127-
}
128-
}
129-
]
130-
}
2+
"version": 6,
3+
"configurePresets": [
4+
{
5+
"name": "base",
6+
"hidden": true,
7+
"cacheVariables": {
8+
"BUILD_SHARED_LIBS": "OFF"
9+
}
10+
},
11+
{
12+
"name": "dev",
13+
"inherits": "base",
14+
"displayName": "Developer build",
15+
"description": "Debug or Release with compile-commands for tooling. AVX2/native tuning is applied per dev target (see examples/ and tests/, which pick /arch:AVX2 for MSVC and -march=native otherwise), so this preset is correct for MSVC, GCC, Clang and MinGW on any OS.",
16+
"binaryDir": "${sourceDir}/build/${presetName}",
17+
"cacheVariables": {
18+
"CMAKE_BUILD_TYPE": "Release",
19+
"CMAKE_EXPORT_COMPILE_COMMANDS": "ON"
20+
}
21+
},
22+
{
23+
"name": "ci",
24+
"inherits": "base",
25+
"displayName": "CI build (MSVC/default)",
26+
"description": "Release build without machine-specific tuning",
27+
"binaryDir": "${sourceDir}/build/${presetName}",
28+
"cacheVariables": {
29+
"CMAKE_BUILD_TYPE": "Release",
30+
"CMAKE_EXPORT_COMPILE_COMMANDS": "ON"
31+
}
32+
},
33+
{
34+
"name": "ci-gcc",
35+
"inherits": "ci",
36+
"displayName": "CI build (GCC)",
37+
"binaryDir": "${sourceDir}/build/${presetName}",
38+
"cacheVariables": {
39+
"CMAKE_C_COMPILER": "gcc",
40+
"CMAKE_CXX_COMPILER": "g++"
41+
}
42+
},
43+
{
44+
"name": "ci-clang",
45+
"inherits": "ci",
46+
"displayName": "CI build (Clang)",
47+
"binaryDir": "${sourceDir}/build/${presetName}",
48+
"cacheVariables": {
49+
"CMAKE_C_COMPILER": "clang",
50+
"CMAKE_CXX_COMPILER": "clang++"
51+
}
52+
},
53+
{
54+
"name": "ci-float",
55+
"inherits": "ci",
56+
"displayName": "CI build (float / native SQP)",
57+
"description": "Single-precision build: blast::real = float, which requires the in-house SQP solver (external NLopt is double-only).",
58+
"binaryDir": "${sourceDir}/build/${presetName}",
59+
"cacheVariables": {
60+
"BLAST_USE_DOUBLES": "OFF",
61+
"BLAST_USE_NATIVE_SQP": "ON"
62+
}
63+
}
64+
],
65+
"buildPresets": [
66+
{
67+
"name": "dev-Debug",
68+
"configurePreset": "dev",
69+
"configuration": "Debug"
70+
},
71+
{
72+
"name": "dev-Release",
73+
"configurePreset": "dev",
74+
"configuration": "Release"
75+
},
76+
{
77+
"name": "ci",
78+
"configurePreset": "ci",
79+
"configuration": "Release"
80+
},
81+
{
82+
"name": "ci-gcc",
83+
"configurePreset": "ci-gcc",
84+
"configuration": "Release"
85+
},
86+
{
87+
"name": "ci-clang",
88+
"configurePreset": "ci-clang",
89+
"configuration": "Release"
90+
},
91+
{
92+
"name": "ci-float",
93+
"configurePreset": "ci-float",
94+
"configuration": "Release"
95+
}
96+
],
97+
"testPresets": [
98+
{
99+
"name": "ci",
100+
"configurePreset": "ci",
101+
"configuration": "Release",
102+
"output": {
103+
"outputOnFailure": true
104+
}
105+
},
106+
{
107+
"name": "ci-gcc",
108+
"configurePreset": "ci-gcc",
109+
"configuration": "Release",
110+
"output": {
111+
"outputOnFailure": true
112+
}
113+
},
114+
{
115+
"name": "ci-clang",
116+
"configurePreset": "ci-clang",
117+
"configuration": "Release",
118+
"output": {
119+
"outputOnFailure": true
120+
}
121+
},
122+
{
123+
"name": "ci-float",
124+
"configurePreset": "ci-float",
125+
"configuration": "Release",
126+
"output": {
127+
"outputOnFailure": true
128+
}
129+
}
130+
]
131+
}

‎blast/blast_math.hpp‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -42,10 +42,14 @@ constexpr real PI = 3.141592653589793;
4242
constexpr real BLAST_GEOMETRY_EPSILON = 1e-9; // near-zero threshold in geometry/collision math
4343
constexpr real BLAST_SOLVER_TOL = 1e-8; // iterative-solver convergence (e.g. eigenvalues)
4444
constexpr real BLAST_FD_STEP = 1e-5; // finite-difference gradient step
45+
constexpr real BLAST_GJK_EPSILON = 1e-7;
46+
constexpr real BLAST_GJK_EPSILON_SQ = 1e-14;
4547
#else
4648
constexpr real BLAST_GEOMETRY_EPSILON = 1e-6f;
4749
constexpr real BLAST_SOLVER_TOL = 1e-5f;
4850
constexpr real BLAST_FD_STEP = 3e-4f;
51+
constexpr real BLAST_GJK_EPSILON = 1e-4;
52+
constexpr real BLAST_GJK_EPSILON_SQ = 1e-8;
4953
#endif
5054
#ifdef __CUDA_ARCH__
5155
#if BLAST_USE_DOUBLES

‎blast/blast_world.hpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,6 @@ struct World;
99
struct Box;
1010
struct Capsule;
1111
struct Sphere;
12-
struct Cylinder;
1312
struct DynamicBox;
1413
struct DynamicSphere;
1514
struct DynamicCapsule;
@@ -165,3 +164,5 @@ inline blast_fn Vec3 get_point(const Array& x, const Matrix& capsule_list);
165164
#include "world/CoDO.hpp"
166165

167166
#include "world/scenes.hpp"
167+
168+
#include "world/gjk_epa.hpp"

‎blast/world/distance.hpp‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -255,7 +255,19 @@ inline blast_fn real distance(Segment segment, Vec3 point) {
255255
return norm(d - point);
256256
}
257257

258+
inline blast_fn void print_vec(Vec3 v, std::string name = "") {
259+
printf("%s = [% 0.6f, % 0.6f, % 0.6f]\n", name.c_str(), v.x, v.y, v.z);
260+
}
261+
258262
inline blast_fn real distance(const Capsule& capsule, const Box& box) {
263+
// ZoneScopedN("distance");
264+
Assert(is_close(norm(box.rotation.col_copy(0)), 1.0, 1e-6));
265+
Assert(is_close(norm(box.rotation.col_copy(1)), 1.0, 1e-6));
266+
Assert(is_close(norm(box.rotation.col_copy(2)), 1.0, 1e-6));
267+
Assert(is_close(norm(Vec3(box.rotation(0, 0), box.rotation(0, 1), box.rotation(0, 2))), 1.0, 1e-6));
268+
Assert(is_close(norm(Vec3(box.rotation(1, 0), box.rotation(1, 1), box.rotation(1, 2))), 1.0, 1e-6));
269+
Assert(is_close(norm(Vec3(box.rotation(2, 0), box.rotation(2, 1), box.rotation(2, 2))), 1.0, 1e-6));
270+
259271
// Transferring points to OBB frame
260272
Mat3 R_trans = transpose(box.rotation);
261273

0 commit comments

Comments
 (0)