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

Change LoadModelFromString to return a smart pointer, add MakeData, and update tests to have C++ RAII clean up model and data.

Browse Source 
PiperOrigin-RevId: 935980153
Change-Id: I41d25bfab4935494dc984168820cb7cad123cadf
This commit is contained in:
Kyle Bayes
2026-06-22 04:18:29 -07:00
committed by Copybara-Service
parent 34d142ee50
commit 1490336955
39 changed files with 4616 additions and 5908 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_ray_test.cc
+129 -139
View File
@@ -14,6 +14,8 @@
// Tests for ray casting.
#include "src/engine/engine_ray.h"
#include <cstring>
#include <string>
@@ -23,7 +25,6 @@
#include <mujoco/mjmodel.h>
#include <mujoco/mjtype.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_ray.h"
#include "test/fixture.h"
namespace mujoco {
@@ -85,9 +86,10 @@ using RayTest = MujocoTest;
TEST_F(RayTest, NoExclusions) {
char error[1024];
mjModel* model = LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model =
LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull());
mjtNum pnt[] = {0.0, 0.0, 0.0};
@@ -97,20 +99,19 @@ TEST_F(RayTest, NoExclusions) {
int bodyexclude = -1;
int geomid = -1;
mj_kinematics(model, data);
mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static,
bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "static_group1");
mj_kinematics(model.get(), data.get());
mjtNum distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup,
flg_static, bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "static_group1");
EXPECT_MJTNUM_EQ(distance, 0.9);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(RayTest, Exclusions) {
char error[1024];
mjModel* model = LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model =
LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull());
mjtNum pnt[] = {0.0, 0.0, 0.0};
@@ -120,40 +121,38 @@ TEST_F(RayTest, Exclusions) {
int bodyexclude = -1;
int geomid = -1;
mj_kinematics(model, data);
mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static,
bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "static_group1");
mj_kinematics(model.get(), data.get());
mjtNum distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup,
flg_static, bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "static_group1");
EXPECT_NEAR(distance, 0.9, MjTol(1e-12, 1e-5));
// Exclude nearest geom
geomgroup[1] = 0;
distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude,
&geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group0");
distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup, flg_static,
bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "group0");
EXPECT_NEAR(distance, 2.9, MjTol(1e-12, 1e-5));
geomgroup[0] = 0;
distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude,
&geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group2");
distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup, flg_static,
bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "group2");
EXPECT_NEAR(distance, 4.9, MjTol(1e-12, 1e-5));
geomgroup[2] = 0;
distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude,
&geomid, nullptr);
distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup, flg_static,
bodyexclude, &geomid, nullptr);
EXPECT_EQ(geomid, -1);
EXPECT_NEAR(distance, -1, MjTol(1e-12, 1e-5));
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(RayTest, ExcludeStatic) {
char error[1024];
mjModel* model = LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model =
LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull());
mjtNum pnt[] = {0.0, 0.0, 0.0};
@@ -163,38 +162,36 @@ TEST_F(RayTest, ExcludeStatic) {
int bodyexclude = -1;
int geomid = -1;
mj_kinematics(model, data);
mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static,
bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group0");
mj_kinematics(model.get(), data.get());
mjtNum distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup,
flg_static, bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "group0");
EXPECT_NEAR(distance, 2.9, MjTol(1e-12, 1e-5));
mj_deleteData(data);
mj_deleteModel(model);
}
// ------------------------------- mj_multiRay --------------------------------
TEST_F(RayTest, MultiRayEqualsSingleRay) {
char error[1024];
mjModel* m = LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull());
mj_forward(m, d);
mj_forward(m.get(), d.get());
// create ray array
constexpr int N = 80;
constexpr int M = 60;
mjtNum vec[3*N*M];
mjtNum vec[3 * N * M];
mjtNum pnt[3] = {-1, 0, 0};
mjtNum cone[4][3] = {{1, .2, -.2}, {1, .2, .2}, {1, -.2, -.2}, {1, -.2, .2}};
memset(vec, 0, 3*N*M*sizeof(mjtNum));
memset(vec, 0, 3 * N * M * sizeof(mjtNum));
for (int i = 0; i < N; ++i) {
for (int j = 0; j < M; ++j) {
for (int k = 0; k < 3; ++k) {
vec[3 * (i * M + j) + k] = i * cone[0][k] / (N - 1) +
j * cone[1][1] / (M - 1) +
vec[3 * (i * M + j) + k] = i * cone[0][k] / (N - 1) +
j * cone[1][1] / (M - 1) +
(N - i - 1) * cone[2][k] / (N - 1) +
(M - j - 1) * cone[3][k] / (M - 1);
}
@@ -202,10 +199,10 @@ TEST_F(RayTest, MultiRayEqualsSingleRay) {
}
// compute intersections with multiray functions
mjtNum dist_multiray[N*M];
int rgeomid_multiray[N*M];
mj_multiRay(m, d, pnt, vec, NULL, 1, -1, rgeomid_multiray, dist_multiray,
nullptr, N * M, mjMAXVAL);
mjtNum dist_multiray[N * M];
int rgeomid_multiray[N * M];
mj_multiRay(m.get(), d.get(), pnt, vec, NULL, 1, -1, rgeomid_multiray,
dist_multiray, nullptr, N * M, mjMAXVAL);
// compare results with single ray function
mjtNum dist;
@@ -214,39 +211,37 @@ TEST_F(RayTest, MultiRayEqualsSingleRay) {
for (int i = 0; i < N; ++i) {
for (int j = 0; j < M; ++j) {
int idx = i * M + j;
dist = mj_ray(m, d, pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid, nullptr);
dist = mj_ray(m.get(), d.get(), pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid,
nullptr);
EXPECT_MJTNUM_EQ(dist, dist_multiray[idx]);
EXPECT_EQ(rgeomid, rgeomid_multiray[idx]);
nhits += dist >= 0;
}
}
EXPECT_GT(nhits, 10);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) {
char error[1024];
mjModel* m = LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull());
mj_forward(m, d);
mj_forward(m.get(), d.get());
// create ray array
constexpr int N = 80;
constexpr int M = 60;
mjtNum vec[3*N*M];
mjtNum vec[3 * N * M];
mjtNum pnt[3] = {-1, 0, 0};
mjtNum cone[4][3] = {{1, .2, -.2}, {1, .2, .2}, {1, -.2, -.2}, {1, -.2, .2}};
memset(vec, 0, 3*N*M*sizeof(mjtNum));
memset(vec, 0, 3 * N * M * sizeof(mjtNum));
for (int i = 0; i < N; ++i) {
for (int j = 0; j < M; ++j) {
for (int k = 0; k < 3; ++k) {
vec[3 * (i * M + j) + k] = i * cone[0][k] / (N - 1) +
j * cone[1][1] / (M - 1) +
vec[3 * (i * M + j) + k] = i * cone[0][k] / (N - 1) +
j * cone[1][1] / (M - 1) +
(N - i - 1) * cone[2][k] / (N - 1) +
(M - j - 1) * cone[3][k] / (M - 1);
}
@@ -254,10 +249,10 @@ TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) {
}
// compute intersections with multiray normal function
mjtNum dist_multiray[N*M];
int rgeomid_multiray[N*M];
mjtNum normal_multiray[3*N*M];
mj_multiRay(m, d, pnt, vec, NULL, 1, -1, rgeomid_multiray,
mjtNum dist_multiray[N * M];
int rgeomid_multiray[N * M];
mjtNum normal_multiray[3 * N * M];
mj_multiRay(m.get(), d.get(), pnt, vec, NULL, 1, -1, rgeomid_multiray,
dist_multiray, normal_multiray, N * M, mjMAXVAL);
// compare results with single ray normal function
@@ -268,30 +263,27 @@ TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) {
for (int i = 0; i < N; ++i) {
for (int j = 0; j < M; ++j) {
int idx = i * M + j;
dist = mj_ray(m, d, pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid,
dist = mj_ray(m.get(), d.get(), pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid,
normal);
EXPECT_MJTNUM_EQ(dist, dist_multiray[idx]);
EXPECT_EQ(rgeomid, rgeomid_multiray[idx]);
EXPECT_MJTNUM_EQ(normal[0], normal_multiray[3*idx]);
EXPECT_MJTNUM_EQ(normal[1], normal_multiray[3*idx + 1]);
EXPECT_MJTNUM_EQ(normal[2], normal_multiray[3*idx + 2]);
EXPECT_MJTNUM_EQ(normal[0], normal_multiray[3 * idx]);
EXPECT_MJTNUM_EQ(normal[1], normal_multiray[3 * idx + 1]);
EXPECT_MJTNUM_EQ(normal[2], normal_multiray[3 * idx + 2]);
nhits += dist >= 0;
}
}
EXPECT_GT(nhits, 10);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(RayTest, EdgeCases) {
char error[1024];
mjModel* m = LoadModelFromString(kSingleGeomModel, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
MjModelPtr m = LoadModelFromString(kSingleGeomModel, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
ASSERT_THAT(m->nbvh, 1);
mjData* d = mj_makeData(m);
MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull());
mj_forward(m, d);
mj_forward(m.get(), d.get());
// spherical bounding box and result arrays
mjtNum geom_ba[4];
@@ -301,51 +293,57 @@ TEST_F(RayTest, EdgeCases) {
// pnt contained in bounding box
mjtNum pnt1[] = {-1, 0, 0};
mju_multiRayPrepare(m, d, pnt1, NULL, NULL, 1, -1, mjMAXVAL, geom_ba, flags);
mju_multiRayPrepare(m.get(), d.get(), pnt1, NULL, NULL, 1, -1, mjMAXVAL,
geom_ba, flags);
EXPECT_MJTNUM_EQ(geom_ba[0], -mjPI);
EXPECT_MJTNUM_EQ(geom_ba[1], 0);
EXPECT_MJTNUM_EQ(geom_ba[2], mjPI);
EXPECT_MJTNUM_EQ(geom_ba[3], mjPI);
EXPECT_MJTNUM_EQ(geom_ba[1], 0);
EXPECT_MJTNUM_EQ(geom_ba[2], mjPI);
EXPECT_MJTNUM_EQ(geom_ba[3], mjPI);
mjtNum vec1[] = {1, 0, 0};
mj_multiRay(m, d, pnt1, vec1, NULL, 1, -1, &rgeomid, &dist, nullptr, 1,
mjMAXVAL);
mj_multiRay(m.get(), d.get(), pnt1, vec1, NULL, 1, -1, &rgeomid, &dist,
nullptr, 1, mjMAXVAL);
EXPECT_FLOAT_EQ(dist, 0.1);
// pnt at phi = Pi, -Pi
mjtNum pnt2[] = {-.5, 0, 0};
mju_multiRayPrepare(m, d, pnt2, NULL, NULL, 1, -1, mjMAXVAL, geom_ba, flags);
mju_multiRayPrepare(m.get(), d.get(), pnt2, NULL, NULL, 1, -1, mjMAXVAL,
geom_ba, flags);
EXPECT_FLOAT_EQ(geom_ba[0], -mjPI); // atan(y<0, x<0)
EXPECT_FLOAT_EQ(geom_ba[2], mjPI); // atan(y>0, x<0)
EXPECT_FLOAT_EQ(geom_ba[2], mjPI); // atan(y>0, x<0)
mjtNum vec2[] = {-1, 0, 0};
mj_multiRay(m, d, pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, nullptr, 1,
mjMAXVAL);
mj_multiRay(m.get(), d.get(), pnt2, vec2, NULL, 1, -1, &rgeomid, &dist,
nullptr, 1, mjMAXVAL);
EXPECT_FLOAT_EQ(dist, 0.4);
// with cutoff
mjtNum cutoff1 = 0.41, cutoff2 = 0.39;
mju_multiRayPrepare(m, d, pnt2, NULL, NULL, 1, -1, cutoff1, geom_ba, flags);
mju_multiRayPrepare(m.get(), d.get(), pnt2, NULL, NULL, 1, -1, cutoff1,
geom_ba, flags);
EXPECT_EQ(flags[0], 0);
mju_multiRayPrepare(m, d, pnt2, NULL, NULL, 1, -1, cutoff2, geom_ba, flags);
mju_multiRayPrepare(m.get(), d.get(), pnt2, NULL, NULL, 1, -1, cutoff2,
geom_ba, flags);
EXPECT_EQ(flags[0], 1);
mj_multiRay(m, d, pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, nullptr, 1,
cutoff2);
mj_multiRay(m.get(), d.get(), pnt2, vec2, NULL, 1, -1, &rgeomid, &dist,
nullptr, 1, cutoff2);
EXPECT_FLOAT_EQ(dist, -1);
// pnt on the boundary of the box
mjtNum pnt3[] = {.1, .1, .05};
mju_multiRayPrepare(m, d, pnt3, NULL, NULL, 1, -1, mjMAXVAL, geom_ba, flags);
mju_multiRayPrepare(m.get(), d.get(), pnt3, NULL, NULL, 1, -1, mjMAXVAL,
geom_ba, flags);
EXPECT_FLOAT_EQ(geom_ba[1], 0);
EXPECT_FLOAT_EQ(geom_ba[3], mjPI);
mjtNum vec3[] = {1, 1, 0};
mj_multiRay(m, d, pnt3, vec3, NULL, 1, -1, &rgeomid, &dist, nullptr, 1,
mjMAXVAL);
mj_multiRay(m.get(), d.get(), pnt3, vec3, NULL, 1, -1, &rgeomid, &dist,
nullptr, 1, mjMAXVAL);
EXPECT_FLOAT_EQ(dist, -1);
// size 0 geom
mjtNum pnt4[] = {-2, 0, 0};
m->geom_aabb[0] = m->geom_aabb[1] = m->geom_aabb[2] = 0;
m->geom_aabb[3] = m->geom_aabb[4] = m->geom_aabb[5] = 0;
mju_multiRayPrepare(m, d, pnt4, NULL, NULL, 1, -1, mjMAXVAL, geom_ba, flags);
mju_multiRayPrepare(m.get(), d.get(), pnt4, NULL, NULL, 1, -1, mjMAXVAL,
geom_ba, flags);
// margin = atan(max_half / dist) where max_half = max(aabb[3..5])
// For a zero-size AABB: max_half = 0, so margin = 0
mjtNum dist4 = mju_dist3(pnt4, d->geom_xpos);
@@ -353,16 +351,13 @@ TEST_F(RayTest, EdgeCases) {
mju_max(m->geom_aabb[3], mju_max(m->geom_aabb[4], m->geom_aabb[5]));
mjtNum margin4 = mju_atan2(max_half4, dist4);
EXPECT_NEAR(geom_ba[0], 0 - margin4, 1e-6);
EXPECT_NEAR(geom_ba[1], mjPI/2 - margin4, 1e-6);
EXPECT_NEAR(geom_ba[1], mjPI / 2 - margin4, 1e-6);
EXPECT_NEAR(geom_ba[2], 0 + margin4, 1e-6);
EXPECT_NEAR(geom_ba[3], mjPI/2 + margin4, 1e-6);
EXPECT_NEAR(geom_ba[3], mjPI / 2 + margin4, 1e-6);
mjtNum vec4[] = {1, 0, 0};
mj_multiRay(m, d, pnt4, vec4, NULL, 1, -1, &rgeomid, &dist, nullptr, 1,
mjMAXVAL);
mj_multiRay(m.get(), d.get(), pnt4, vec4, NULL, 1, -1, &rgeomid, &dist,
nullptr, 1, mjMAXVAL);
EXPECT_FLOAT_EQ(dist, 0.9);
mj_deleteData(d);
mj_deleteModel(m);
}
// ------------------------------- mj_rayMesh ---------------------------------
@@ -377,10 +372,10 @@ mjtNum _rayMesh(const mjModel* m, const mjData* d, int geomid,
// map to local frame
mjtNum lpnt[3], lvec[3];
const mjtNum* pos = d->geom_xpos+3*geomid;
const mjtNum dif[3] = {pnt[0]-pos[0], pnt[1]-pos[1], pnt[2]-pos[2]};
mju_mulMatTVec3(lpnt, d->geom_xmat+9*geomid, dif);
mju_mulMatTVec3(lvec, d->geom_xmat+9*geomid, vec);
const mjtNum* pos = d->geom_xpos + 3 * geomid;
const mjtNum dif[3] = {pnt[0] - pos[0], pnt[1] - pos[1], pnt[2] - pos[2]};
mju_mulMatTVec3(lpnt, d->geom_xmat + 9 * geomid, dif);
mju_mulMatTVec3(lvec, d->geom_xmat + 9 * geomid, vec);
// construct basis vectors of normal plane
mjtNum b0[3] = {1, 1, 1}, b1[3];
@@ -392,7 +387,7 @@ mjtNum _rayMesh(const mjModel* m, const mjData* d, int geomid,
} else {
b0[2] = 0;
}
mju_addScl3(b1, b0, lvec, -mju_dot3(lvec, b0)/mju_dot3(lvec, lvec));
mju_addScl3(b1, b0, lvec, -mju_dot3(lvec, b0) / mju_dot3(lvec, lvec));
mju_normalize3(b1);
mju_cross(b0, b1, lvec);
mju_normalize3(b0);
@@ -403,18 +398,20 @@ mjtNum _rayMesh(const mjModel* m, const mjData* d, int geomid,
// process all triangles
int face, meshid = m->geom_dataid[geomid];
for (face = m->mesh_faceadr[meshid];
face < m->mesh_faceadr[meshid] + m->mesh_facenum[meshid];
face++) {
face < m->mesh_faceadr[meshid] + m->mesh_facenum[meshid]; face++) {
// get float vertices
float* vf[3];
vf[0] = m->mesh_vert + 3*(m->mesh_face[3*face] + m->mesh_vertadr[meshid]);
vf[1] = m->mesh_vert + 3*(m->mesh_face[3*face+1] + m->mesh_vertadr[meshid]);
vf[2] = m->mesh_vert + 3*(m->mesh_face[3*face+2] + m->mesh_vertadr[meshid]);
vf[0] =
m->mesh_vert + 3 * (m->mesh_face[3 * face] + m->mesh_vertadr[meshid]);
vf[1] = m->mesh_vert +
3 * (m->mesh_face[3 * face + 1] + m->mesh_vertadr[meshid]);
vf[2] = m->mesh_vert +
3 * (m->mesh_face[3 * face + 2] + m->mesh_vertadr[meshid]);
// convert to mjtNum
mjtNum v[3][3];
for (int i=0; i < 3; i++) {
for (int j=0; j < 3; j++) {
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
v[i][j] = (mjtNum)vf[i][j];
}
}
@@ -440,16 +437,16 @@ void _rayMeshTest(const mjModel* m) {
// create ray array
constexpr int N = 80;
constexpr int M = 60;
mjtNum vec[3*N*M];
mjtNum vec[3 * N * M];
mjtNum pnt[3] = {1, .2, 0};
mjtNum cone[4][3] = {{-1, -1, -1}, {-1, -1, 1}, {1, -1, 1}, {1, -1, -1}};
memset(vec, 0, 3*N*M*sizeof(mjtNum));
memset(vec, 0, 3 * N * M * sizeof(mjtNum));
for (int i = 0; i < N; ++i) {
for (int j = 0; j < M; ++j) {
for (int k = 0; k < 3; ++k) {
vec[3 * (i * M + j) + k] = i * cone[0][k] / (N - 1) +
j * cone[1][1] / (M - 1) +
vec[3 * (i * M + j) + k] = i * cone[0][k] / (N - 1) +
j * cone[1][1] / (M - 1) +
(N - i - 1) * cone[2][k] / (N - 1) +
(M - j - 1) * cone[3][k] / (M - 1);
}
@@ -466,7 +463,6 @@ void _rayMeshTest(const mjModel* m) {
EXPECT_FLOAT_EQ(dist_new, dist_old);
}
}
mj_deleteData(d);
}
@@ -478,15 +474,13 @@ TEST_F(RayTest, RayMeshPruning) {
const string xml_path =
GetTestDataFilePath("engine/testdata/ray/stanford_bunny.xml");
mjModel* m = mj_loadXML(xml_path.c_str(), NULL, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
_rayMeshTest(m);
mj_deleteModel(m);
MjModelPtr m(mj_loadXML(xml_path.c_str(), NULL, error, sizeof(error)));
ASSERT_THAT(m.get(), NotNull()) << error;
_rayMeshTest(m.get());
m = LoadModelFromString(kCubeletModel, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
_rayMeshTest(m);
mj_deleteModel(m);
ASSERT_THAT(m.get(), NotNull()) << error;
_rayMeshTest(m.get());
}
TEST_F(RayTest, RayHfield) {
@@ -523,21 +517,17 @@ TEST_F(RayTest, RayHfield) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mj_forward(model, data);
mj_forward(model.get(), data.get());
EXPECT_THAT(data->sensordata[0], MjNear(1, 1e-8, 1e-5));
EXPECT_THAT(data->sensordata[1], MjNear(1, 1e-8, 1e-5));
EXPECT_THAT(data->sensordata[2], MjNear(1, 1e-8, 1e-5));
EXPECT_THAT(data->sensordata[3], MjNear(0.5, 1e-8, 1e-5));
mj_deleteData(data);
mj_deleteModel(model);
}
static const char* const kPlaneModel = "engine/testdata/ray/plane.xml";
@@ -603,8 +593,8 @@ TEST_F(RayTest, RayNormal) {
} else {
r = mj_rayFlex(m, d, /*flex_layer*/ 0, /*flg_vert*/ 1,
/*flg_edge*/ 1, /*flg_face*/ 1,
/*flg_skin*/ 1, /*flex_id*/ 0,
pnt, vec, nullptr, normal);
/*flg_skin*/ 1, /*flex_id*/ 0, pnt, vec, nullptr,
normal);
// no sensor comparison: rangefinders only intersect with geoms
}
@@ -623,8 +613,7 @@ TEST_F(RayTest, RayNormal) {
mjtNum eps = 1e-6;
mjtNum ds[2][3];
for (int i = 0; i < 2; ++i) {
mjtNum nudge[3] = {d->site_xmat[0 + i],
d->site_xmat[3 + i],
mjtNum nudge[3] = {d->site_xmat[0 + i], d->site_xmat[3 + i],
d->site_xmat[6 + i]};
mjtNum dr, dpnt[3];
mju_addScl3(dpnt, pnt, nudge, eps);
@@ -650,8 +639,9 @@ TEST_F(RayTest, RayNormal) {
mjtNum expected_neg[3] = {-expected[0], -expected[1], -expected[2]};
// compare analytic with fin-diff approximation
EXPECT_THAT(normal, AnyOf(Pointwise(MjNear(100*eps, 1e-3), expected),
Pointwise(MjNear(100*eps, 1e-3), expected_neg)))
EXPECT_THAT(normal,
AnyOf(Pointwise(MjNear(100 * eps, 1e-3), expected),
Pointwise(MjNear(100 * eps, 1e-3), expected_neg)))
<< path << ", time " << d->time;
// increment count