chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added a collision midphase on geom pairs using rotated AABBs static trees.

Browse Source 
PiperOrigin-RevId: 517357695
Change-Id: I068fee714d9638be2fb6d042d5d86f8ff970635b
This commit is contained in:
Alessio Quaglino
2023-03-17 02:36:38 -07:00
committed by Copybara-Service
parent 47b58f5ccb
commit 70959c1a2c
30 changed files with 2228 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_collision_driver_test.cc
+21
View File
@@ -23,6 +23,8 @@
#include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
#include "test/fixture.h"
#include "src/engine/engine_collision_driver.h"
namespace mujoco {
namespace {
@@ -216,5 +218,24 @@ TEST_F(MjCollisionTest, FilterParentDoesntAffectWorldBody) {
mj_deleteModel(m);
}
TEST_F(MjCollisionTest, TestOBB) {
mjtNum bvh1[6] = {-1, -1, -1, 1, 1, 1};
mjtNum bvh2[6] = {-1, -1, -1, 1, 1, 1};
mjtNum pos1[3] = {0, 0, 0};
mjtNum mat1[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
mjtNum pos2[3] = {1.71, 1.71, 0}; // just a little more than 1+sqrt(2)/2
mjtNum mat2[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
EXPECT_THAT(
mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, NULL, NULL, 0), true);
// rotate by 45 degrees
mat2[0] = 1./mju_sqrt(2.); mat2[1] = -1./mju_sqrt(2.);
mat2[3] = 1./mju_sqrt(2.); mat2[4] = 1./mju_sqrt(2.);
EXPECT_THAT(
mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, NULL, NULL, 0), false);
}
} // namespace
} // namespace mujoco