From 8e2f830fd2d079d4391541bcbb4971d12e164e88 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 18 Jun 2024 09:11:39 -0700 Subject: [PATCH] Fix type errors in compiler and tests. In preparation for a float32 build of MuJoCo. PiperOrigin-RevId: 644407674 Change-Id: I60069f0f865ef89fbf613f5a2ca0c336cc63461e --- plugin/sdf/bolt.cc | 2 +- plugin/sdf/bowl.cc | 2 +- plugin/sdf/gear.cc | 2 +- plugin/sdf/nut.cc | 2 +- plugin/sdf/sdflib.cc | 2 +- plugin/sdf/torus.cc | 2 +- src/user/user_composite.cc | 32 +-- src/user/user_composite.h | 4 +- src/user/user_flexcomp.cc | 29 +-- src/user/user_flexcomp.h | 2 +- src/user/user_mesh.cc | 128 +++++----- src/user/user_model.cc | 389 ++++++++++++++--------------- src/user/user_objects.cc | 152 ++++++----- src/user/user_objects.h | 22 +- src/user/user_util.cc | 345 ++++++++++++++++++++----- src/user/user_util.h | 42 ++-- src/xml/xml_native_reader.cc | 2 +- test/fixture.cc | 27 +- test/user/user_mesh_test.cc | 23 +- test/user/user_model_test.cc | 2 +- test/xml/xml_native_writer_test.cc | 10 +- 21 files changed, 715 insertions(+), 506 deletions(-) diff --git a/plugin/sdf/bolt.cc b/plugin/sdf/bolt.cc index 4030a164..4f052c06 100644 --- a/plugin/sdf/bolt.cc +++ b/plugin/sdf/bolt.cc @@ -148,7 +148,7 @@ void Bolt::RegisterPlugin() { delete reinterpret_cast(d->plugin_data[instance]); d->plugin_data[instance] = 0; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto sdf = reinterpret_cast(plugin_data); sdf->Reset(); diff --git a/plugin/sdf/bowl.cc b/plugin/sdf/bowl.cc index 55b6515f..2ebbc5ac 100644 --- a/plugin/sdf/bowl.cc +++ b/plugin/sdf/bowl.cc @@ -146,7 +146,7 @@ void Bowl::RegisterPlugin() { delete reinterpret_cast(d->plugin_data[instance]); d->plugin_data[instance] = 0; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto sdf = reinterpret_cast(plugin_data); sdf->Reset(); diff --git a/plugin/sdf/gear.cc b/plugin/sdf/gear.cc index d7b49da3..54288c21 100644 --- a/plugin/sdf/gear.cc +++ b/plugin/sdf/gear.cc @@ -232,7 +232,7 @@ void Gear::RegisterPlugin() { delete reinterpret_cast(d->plugin_data[instance]); d->plugin_data[instance] = 0; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto sdf = reinterpret_cast(plugin_data); sdf->Reset(); diff --git a/plugin/sdf/nut.cc b/plugin/sdf/nut.cc index 448fc4a8..117df406 100644 --- a/plugin/sdf/nut.cc +++ b/plugin/sdf/nut.cc @@ -148,7 +148,7 @@ void Nut::RegisterPlugin() { delete reinterpret_cast(d->plugin_data[instance]); d->plugin_data[instance] = 0; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto sdf = reinterpret_cast(plugin_data); sdf->Reset(); diff --git a/plugin/sdf/sdflib.cc b/plugin/sdf/sdflib.cc index 3985ca5b..1032dbc8 100644 --- a/plugin/sdf/sdflib.cc +++ b/plugin/sdf/sdflib.cc @@ -180,7 +180,7 @@ void SdfLib::RegisterPlugin() { delete reinterpret_cast(d->plugin_data[instance]); d->plugin_data[instance] = 0; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto sdf = reinterpret_cast(plugin_data); sdf->Reset(); diff --git a/plugin/sdf/torus.cc b/plugin/sdf/torus.cc index a3c6195c..d48feb51 100644 --- a/plugin/sdf/torus.cc +++ b/plugin/sdf/torus.cc @@ -95,7 +95,7 @@ void Torus::RegisterPlugin() { delete reinterpret_cast(d->plugin_data[instance]); d->plugin_data[instance] = 0; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { // do nothing }; diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index ae1fe677..891ce675 100644 --- a/src/user/user_composite.cc +++ b/src/user/user_composite.cc @@ -31,7 +31,6 @@ #include #include "cc/array_safety.h" #include "engine/engine_io.h" -#include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" #include "user/user_model.h" @@ -428,12 +427,12 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjsBody* body, char* error, int } // compute volume - std::vector volume(uservert.size()/3); - mjtNum t = 1; + std::vector volume(uservert.size()/3); + double thickness = 1; if (dim == 2 && plugin.active) { try { mjCPlugin* pplugin = static_cast(plugin.instance); - t = std::stod(pplugin->config_attribs["thickness"], nullptr); + thickness = std::stod(pplugin->config_attribs["thickness"], nullptr); } catch (const std::invalid_argument& e) { return comperr(error, "Invalid thickness attribute", error_sz); } @@ -441,9 +440,9 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjsBody* body, char* error, int if (!userface.empty()) { face = mjXUtil::String2Vector(userface); for (int j=0; jquat, this_quat, 4); } else { mjuu_setvec(body->pos, length_prev, 0, 0); - mjtNum negquat[4] = {prev_quat[0], -prev_quat[1], -prev_quat[2], -prev_quat[3]}; + double negquat[4] = {prev_quat[0], -prev_quat[1], -prev_quat[2], -prev_quat[3]}; mjuu_mulquat(dquat, negquat, this_quat); mjuu_copyvec(body->quat, dquat, 4); } @@ -1091,7 +1091,7 @@ void mjCComposite::BoxProject(double* pos) { // cylinder else if (type==mjCOMPTYPE_CYLINDER) { - double L0 = mju_max(mju_abs(pos[0]), mju_abs(pos[1])); + double L0 = std::max(std::abs(pos[0]), std::abs(pos[1])); mjuu_normvec(pos, 2); pos[0] *= size[0]*L0; pos[1] *= size[1]*L0; diff --git a/src/user/user_composite.h b/src/user/user_composite.h index 88bccedc..aa16f136 100644 --- a/src/user/user_composite.h +++ b/src/user/user_composite.h @@ -107,7 +107,7 @@ class mjCComposite { std::string initial; // root boundary type std::vector uservert; // user-specified vertex positions std::string userface; // connectivity - mjtNum size[3]; // rope size (meaning depends on the shape) + double size[3]; // rope size (meaning depends on the shape) mjtCompShape curve[3]; // geometric shape // body names used in the skin @@ -137,7 +137,7 @@ class mjCComposite { private: mjsBody* AddRopeBody(mjCModel* model, mjsBody* body, int ix, int ix1); - mjsBody* AddCableBody(mjCModel* model, mjsBody* body, int ix, mjtNum normal[3], mjtNum prev_quat[4]); + mjsBody* AddCableBody(mjCModel* model, mjsBody* body, int ix, double normal[3], double prev_quat[4]); // temporary skin vectors void CopyIntoSkin(mjsSkin* skin); diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 663c8b6e..c9ed36fc 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -29,10 +30,7 @@ #include "cc/array_safety.h" #include "engine/engine_crossplatform.h" #include "engine/engine_resource.h" -#include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" -#include "engine/engine_util_misc.h" -#include "engine/engine_util_spatial.h" #include "user/user_flexcomp.h" #include #include "user/user_model.h" @@ -250,12 +248,9 @@ bool mjCFlexcomp::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) { } // apply pose transform to points - mjtNum posn[3], quatn[4]; - mju_d2n(posn, pos, 3); - mju_d2n(quatn, quat, 4); for (int i=0; i < npnt; i++) { - mjtNum newp[3], oldp[3] = {point[3*i], point[3*i+1], point[3*i+2]}; - mju_trnVecPose(newp, posn, quatn, oldp); + double newp[3], oldp[3] = {point[3*i], point[3*i+1], point[3*i+2]}; + mjuu_trnVecPose(newp, pos, quat, oldp); point[3*i] = newp[0]; point[3*i+1] = newp[1]; point[3*i+2] = newp[2]; @@ -570,7 +565,7 @@ bool mjCFlexcomp::MakeGrid(char* error, int error_sz) { int quad2tri[2][3] = {{0, 1, 2}, {0, 2, 3}}; // add point - mjtNum pos[2] = {spacing[0]*(ix - 0.5*(count[0]-1)), + double pos[2] = {spacing[0]*(ix - 0.5*(count[0]-1)), spacing[1]*(iy - 0.5*(count[1]-1))}; point.push_back(pos[0]); point.push_back(pos[1]); @@ -578,8 +573,8 @@ bool mjCFlexcomp::MakeGrid(char* error, int error_sz) { // add texture coordinates, if not specified explicitly if (!hastex) { - texcoord.push_back(ix/(mjtNum)mjMAX(count[0]-1, 1)); - texcoord.push_back(iy/(mjtNum)mjMAX(count[1]-1, 1)); + texcoord.push_back(ix/(double)std::max(count[0]-1, 1)); + texcoord.push_back(iy/(double)std::max(count[1]-1, 1)); } // flip triangles if radial projection is requested @@ -713,7 +708,7 @@ void mjCFlexcomp::BoxProject(double* pos, int ix, int iy, int iz) { // cylinder else if (type==mjFCOMPTYPE_CYLINDER) { - double L0 = mjMAX(mju_abs(pos[0]), mju_abs(pos[1])); + double L0 = std::max(std::abs(pos[0]), std::abs(pos[1])); mjuu_normvec(pos, 2); pos[0] *= size[0]*L0; pos[1] *= size[1]*L0; @@ -749,8 +744,8 @@ bool mjCFlexcomp::MakeSquare(char* error, int error_sz) { }; for (int i=0; i < point.size()/3; i++) { - mjtNum* pos = point.data() + i*3; - double L0 = mjMAX(mju_abs(pos[0]), mju_abs(pos[1])); + double* pos = point.data() + i*3; + double L0 = std::max(std::abs(pos[0]), std::abs(pos[1])); mjuu_normvec(pos, 2); pos[0] *= size[0]*L0; pos[1] *= size[1]*L0; @@ -952,10 +947,10 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) { // copy faces element = mesh.face_; - // copy vertices, convert from float to mjtNum - point = vector (mesh.nvert()*3); + // copy vertices, convert from float to double + point = vector (mesh.nvert()*3); for (int i=0; i < mesh.nvert()*3; i++) { - point[i] = (mjtNum) mesh.vert_[i]; + point[i] = (double) mesh.vert_[i]; } return true; diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index a22780aa..d090022b 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -89,7 +89,7 @@ class mjCFlexcomp { // set by user or computed internally bool rigid; // all vertices are in parent body (all pinned) bool centered; // all vertex coordinates are (0,0,0) (nothing pinned) - std::vector point; // flex bodies/vertices + std::vector point; // flex bodies/vertices std::vector pinned; // is point pinned (true: no new body) std::vector used; // is point used by any element (false: skip) std::vector element; // flex elements diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 3d09d181..e83660e3 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -57,11 +57,7 @@ #include "engine/engine_io.h" #include "engine/engine_plugin.h" #include "engine/engine_resource.h" -#include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" -#include "engine/engine_util_misc.h" -#include "engine/engine_util_solve.h" -#include "engine/engine_util_spatial.h" #include "user/user_cache.h" #include "user/user_model.h" #include "user/user_objects.h" @@ -77,7 +73,7 @@ using std::string; using std::vector; // compute triangle area, surface normal, center -static mjtNum _triangle(mjtNum* normal, mjtNum* center, +static double _triangle(double* normal, double* center, const float* v1, const float* v2, const float* v3) { // center if (center) { @@ -89,10 +85,10 @@ static mjtNum _triangle(mjtNum* normal, mjtNum* center, // normal = (v2-v1) cross (v3-v1) double b[3] = { v2[0]-v1[0], v2[1]-v1[1], v2[2]-v1[2] }; double c[3] = { v3[0]-v1[0], v3[1]-v1[1], v3[2]-v1[2] }; - mju_cross(normal, b, c); + mjuu_crossvec(normal, b, c); // get length - double len = mju_norm3(normal); + double len = sqrt(mjuu_dot3(normal, normal)); // ignore small faces if (lensdf_aabb(aabb, attributes.data()); mjtNum total = aabb[3] + aabb[4] + aabb[5]; - const mjtNum n = 300; + const double n = 300; int nx, ny, nz; nx = floor(n / total * aabb[3]) + 1; ny = floor(n / total * aabb[4]) + 1; @@ -456,7 +452,7 @@ void mjCMesh::Compile(const mjVFS* vfs) { int v0 = face_[3*i+0]; int v1 = face_[3*i+1]; int v2 = face_[3*i+2]; - mjtNum normal[3]; + double normal[3]; float* vtx = vert_.data(); if (_triangle(normal, nullptr, vtx+3*v0, vtx+3*v1, vtx+3*v2)>sqrt(mjMINVAL)) { halfedge_.push_back(std::pair(v0, v1)); @@ -552,7 +548,7 @@ void mjCMesh::SetBoundingVolume(int faceid) { node->contype = 1; node->pos = center_ + 3*faceid; node->quat = NULL; - mjtNum face_aamm[6] = {1E+10, 1E+10, 1E+10, -1E+10, -1E+10, -1E+10}; + double face_aamm[6] = {1E+10, 1E+10, 1E+10, -1E+10, -1E+10, -1E+10}; for (int j=0; j<3; j++) { int vertid = face_[3*faceid+j]; face_aamm[0] = mjMIN(face_aamm[0], vert_[3*vertid+0]); @@ -1263,15 +1259,15 @@ void mjCMesh::ApplyTransformations() { // rotate if (refquat[0]!=1 || refquat[1]!=0 || refquat[2]!=0 || refquat[3]!=0) { // prepare rotation - mjtNum quat[4] = {refquat[0], refquat[1], refquat[2], refquat[3]}; - mjtNum mat[9]; - mju_normalize4(quat); - mju_quat2Mat(mat, quat); + double quat[4] = {refquat[0], refquat[1], refquat[2], refquat[3]}; + double mat[9]; + mjuu_normvec(quat, 4); + mjuu_quat2mat(mat, quat); // process vertices for (int i=0; i < nvert(); i++) { - mjtNum p1[3], p0[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]}; - mju_mulMatTVec3(p1, mat, p0); + double p1[3], p0[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]}; + mjuu_mulvecmatT(p1, p0, mat); vert_[3*i] = (float) p1[0]; vert_[3*i+1] = (float) p1[1]; vert_[3*i+2] = (float) p1[2]; @@ -1279,8 +1275,8 @@ void mjCMesh::ApplyTransformations() { // process normals for (int i=0; i < nnormal(); i++) { - mjtNum n1[3], n0[3] = {normal_[3*i], normal_[3*i+1], normal_[3*i+2]}; - mju_mulMatTVec3(n1, mat, n0); + double n1[3], n0[3] = {normal_[3*i], normal_[3*i+1], normal_[3*i+2]}; + mjuu_mulvecmatT(n1, n0, mat); normal_[3*i] = (float) n1[0]; normal_[3*i+1] = (float) n1[1]; normal_[3*i+2] = (float) n1[2]; @@ -1449,13 +1445,13 @@ void mjCMesh::Process() { inert[5] = -P[5]; // get quaternion and diagonal inertia - mjtNum eigval[3], eigvec[9], quattmp[4]; - mjtNum full[9] = { + double eigval[3], eigvec[9], quattmp[4]; + double full[9] = { inert[0], inert[3], inert[4], inert[3], inert[1], inert[5], inert[4], inert[5], inert[2] }; - mju_eig3(eigval, eigvec, quattmp, full); + mjuu_eig3(eigval, eigvec, quattmp, full); // check eigval - SHOULD NOT OCCUR if (eigval[2]<=0) { @@ -1479,12 +1475,12 @@ void mjCMesh::Process() { // if volume was valid, copy volume quat to shell and stop, // otherwise use shell quat for coordinate transformations if (type==mjINERTIA_SHELL && validvolume_>0) { - mju_copy4(GetQuatPtr(type), GetQuatPtr(mjINERTIA_VOLUME)); + mjuu_copyvec(GetQuatPtr(type), GetQuatPtr(mjINERTIA_VOLUME), 4); continue; } // rotate vertices and normals into axis-aligned frame - mju_copy4(GetQuatPtr(type), quattmp); + mjuu_copyvec(GetQuatPtr(type), quattmp, 4); double neg[4] = {quattmp[0], -quattmp[1], -quattmp[2], -quattmp[3]}; double mat[9]; mjuu_quat2mat(mat, neg); @@ -1798,16 +1794,16 @@ void mjCMesh::MakeNormal(void) { } // get triangle edges - mjtNum vec01[3], vec02[3]; + double vec01[3], vec02[3]; for (int j=0; j<3; j++) { vec01[j] = vert_[3*vertid[1]+j] - vert_[3*vertid[0]+j]; vec02[j] = vert_[3*vertid[2]+j] - vert_[3*vertid[0]+j]; } // compute face normal - mjtNum nrm[3]; - mju_cross(nrm, vec01, vec02); - mjtNum area = mju_normalize3(nrm); + double nrm[3]; + mjuu_crossvec(nrm, vec01, vec02); + double area = mjuu_normvec(nrm, 3); // add normal to each vertex with weight = area for (int j=0; j<3; j++) { @@ -1833,25 +1829,25 @@ void mjCMesh::MakeNormal(void) { } // get triangle edges - mjtNum vec01[3], vec02[3]; + double vec01[3], vec02[3]; for (int j=0; j<3; j++) { vec01[j] = vert_[3*vertid[1]+j] - vert_[3*vertid[0]+j]; vec02[j] = vert_[3*vertid[2]+j] - vert_[3*vertid[0]+j]; } // compute face normal - mjtNum nrm[3]; - mju_cross(nrm, vec01, vec02); - mjtNum area = mju_normalize3(nrm); + double nrm[3]; + mjuu_crossvec(nrm, vec01, vec02); + double area = mjuu_normvec(nrm, 3); // compare to vertex normal, subtract contribution if dot product too small for (int j=0; j<3; j++) { // normalized vertex normal - mjtNum vnrm[3] = {normal_[3*vertid[j]], normal_[3*vertid[j]+1], normal_[3*vertid[j]+2]}; - mju_normalize3(vnrm); + double vnrm[3] = {normal_[3*vertid[j]], normal_[3*vertid[j]+1], normal_[3*vertid[j]+2]}; + mjuu_normvec(vnrm, 3); // dot too small: remove - if (mju_dot3(nrm, vnrm)<0.8) { + if (mjuu_dot3(nrm, vnrm)<0.8) { for (int k=0; k<3; k++) { nremove[3*vertid[j]+k] += nrm[k]*area; } @@ -1901,28 +1897,28 @@ void mjCMesh::MakeCenter(void) { int* vertid = face_.data() + 3*i; // get triangle edges - mjtNum a[3], b[3]; + double a[3], b[3]; for (int j=0; j<3; j++) { a[j] = vert_[3*vertid[0]+j] - vert_[3*vertid[2]+j]; b[j] = vert_[3*vertid[1]+j] - vert_[3*vertid[2]+j]; } // compute face normal - mjtNum nrm[3]; - mju_cross(nrm, a, b); + double nrm[3]; + mjuu_crossvec(nrm, a, b); // compute circumradius - mjtNum norm_a_2 = mju_dot3(a, a); - mjtNum norm_b_2 = mju_dot3(b, b); - mjtNum area = mju_norm3(nrm); + double norm_a_2 = mjuu_dot3(a, a); + double norm_b_2 = mjuu_dot3(b, b); + double area = mjuu_normvec(nrm, 3); // compute circumcenter - mjtNum res[3], vec[3] = { + double res[3], vec[3] = { norm_a_2 * b[0] - norm_b_2 * a[0], norm_a_2 * b[1] - norm_b_2 * a[1], norm_a_2 * b[2] - norm_b_2 * a[2] }; - mju_cross(res, vec, nrm); + mjuu_crossvec(res, vec, nrm); center_[3*i+0] = res[0]/(2*area*area) + vert_[3*vertid[2]+0]; center_[3*i+1] = res[1]/(2*area*area) + vert_[3*vertid[2]+1]; center_[3*i+2] = res[2]/(2*area*area) + vert_[3*vertid[2]+2]; @@ -2200,13 +2196,13 @@ void mjCSkin::Compile(const mjVFS* vfs) { // normalize bindquat for (int i=0; i < nbone; i++) { - mjtNum quat[4] = { - (mjtNum)bindquat_[4*i], - (mjtNum)bindquat_[4*i+1], - (mjtNum)bindquat_[4*i+2], - (mjtNum)bindquat_[4*i+3] + double quat[4] = { + (double)bindquat_[4*i], + (double)bindquat_[4*i+1], + (double)bindquat_[4*i+2], + (double)bindquat_[4*i+3] }; - mju_normalize4(quat); + mjuu_normvec(quat, 4); bindquat_[4*i] = (float) quat[0]; bindquat_[4*i+1] = (float) quat[1]; @@ -2562,17 +2558,17 @@ void mjCFlex::Compile(const mjVFS* vfs) { } // compute global vertex positions - vertxpos = vector (3*nvert); + vertxpos = vector (3*nvert); for (int i=0; i < nvert; i++) { // get body id, set vertxpos = body.xpos0 int b = rigid ? vertbodyid[0] : vertbodyid[i]; - mju_copy3(vertxpos.data()+3*i, model->Bodies()[b]->xpos0); + mjuu_copyvec(vertxpos.data()+3*i, model->Bodies()[b]->xpos0, 3); // add vertex offset within body if not centered if (!centered) { - mjtNum offset[3]; - mju_rotVecQuat(offset, vert_.data()+3*i, model->Bodies()[b]->xquat0); - mju_addTo3(vertxpos.data()+3*i, offset); + double offset[3]; + mjuu_rotVecQuat(offset, vert_.data()+3*i, model->Bodies()[b]->xquat0); + mjuu_addtovec(vertxpos.data()+3*i, offset, 3); } } @@ -2581,18 +2577,18 @@ void mjCFlex::Compile(const mjVFS* vfs) { if (dim==3) { for (int e=0; e0) { + double nrm[3]; + mjuu_crossvec(nrm, v01, v02); + if (mjuu_dot3(nrm, v03)>0) { // flip orientation int tmp = elem_[e*(dim+1)+1]; elem_[e*(dim+1)+1] = elem_[e*(dim+1)+2]; @@ -2672,9 +2668,9 @@ void mjCFlex::CreateBVH(void) { } // compute min and max along each global axis - mjtNum xmin[3], xmax[3]; - mju_copy3(xmin, vertxpos.data() + 3*edata[0]); - mju_copy3(xmax, vertxpos.data() + 3*edata[0]); + double xmin[3], xmax[3]; + mjuu_copyvec(xmin, vertxpos.data() + 3*edata[0], 3); + mjuu_copyvec(xmax, vertxpos.data() + 3*edata[0], 3); for (int i=1; i <= dim; i++) { for (int j=0; j<3; j++) { xmin[j] = mjMIN(xmin[j], vertxpos[3*edata[i]+j]); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index a8a0d77c..b92232e8 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -15,6 +15,7 @@ #include "user/user_model.h" #include +#include #include #include #include @@ -38,7 +39,6 @@ #include "engine/engine_plugin.h" #include "engine/engine_setconst.h" #include "engine/engine_support.h" -#include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" #include "user/user_api.h" @@ -69,15 +69,6 @@ using std::vector; #endif -// copy real-valued vector -template -static void copyvec(T1* dest, T2* src, int n) { - for (int i=0; idof_invweight0[adr+i]; } - inertia = ((mjtNum)ndim) / mju_max(mjMINVAL, inertia); + inertia = ((mjtNum)ndim) / std::max(mjMINVAL, inertia); // compute stiffness and damping (same as solref computation) - mjtNum stiffness = inertia / mju_max(mjMINVAL, timeconst*timeconst*dampratio*dampratio); - mjtNum damping = 2 * inertia / mju_max(mjMINVAL, timeconst); + mjtNum stiffness = inertia / std::max(mjMINVAL, timeconst*timeconst*dampratio*dampratio); + mjtNum damping = 2 * inertia / std::max(mjMINVAL, timeconst); // assign m->jnt_stiffness[n] = stiffness; @@ -1771,14 +1762,14 @@ void mjCModel::CopyTree(mjModel* m) { m->body_dofadr[i] = (pb->dofnum ? dofadr : -1); m->body_geomnum[i] = (int)pb->geoms.size(); m->body_geomadr[i] = (!pb->geoms.empty() ? pb->geoms[0]->id : -1); - copyvec(m->body_pos+3*i, pb->pos, 3); - copyvec(m->body_quat+4*i, pb->quat, 4); - copyvec(m->body_ipos+3*i, pb->ipos, 3); - copyvec(m->body_iquat+4*i, pb->iquat, 4); + mjuu_copyvec(m->body_pos+3*i, pb->pos, 3); + mjuu_copyvec(m->body_quat+4*i, pb->quat, 4); + mjuu_copyvec(m->body_ipos+3*i, pb->ipos, 3); + mjuu_copyvec(m->body_iquat+4*i, pb->iquat, 4); m->body_mass[i] = (mjtNum)pb->mass; - copyvec(m->body_inertia+3*i, pb->inertia, 3); + mjuu_copyvec(m->body_inertia+3*i, pb->inertia, 3); m->body_gravcomp[i] = pb->gravcomp; - copyvec(m->body_user+nuser_body*i, pb->get_userdata().data(), nuser_body); + mjuu_copyvec(m->body_user+nuser_body*i, pb->get_userdata().data(), nuser_body); m->body_contype[i] = pb->contype; m->body_conaffinity[i] = pb->conaffinity; @@ -1854,23 +1845,23 @@ void mjCModel::CopyTree(mjModel* m) { m->jnt_qposadr[jid] = qposadr; m->jnt_dofadr[jid] = dofadr; m->jnt_bodyid[jid] = pj->body->id; - copyvec(m->jnt_pos+3*jid, pj->pos, 3); - copyvec(m->jnt_axis+3*jid, pj->axis, 3); + mjuu_copyvec(m->jnt_pos+3*jid, pj->pos, 3); + mjuu_copyvec(m->jnt_axis+3*jid, pj->axis, 3); m->jnt_stiffness[jid] = (mjtNum)pj->stiffness; - copyvec(m->jnt_range+2*jid, pj->range, 2); - copyvec(m->jnt_actfrcrange+2*jid, pj->actfrcrange, 2); - copyvec(m->jnt_solref+mjNREF*jid, pj->solref_limit, mjNREF); - copyvec(m->jnt_solimp+mjNIMP*jid, pj->solimp_limit, mjNIMP); + mjuu_copyvec(m->jnt_range+2*jid, pj->range, 2); + mjuu_copyvec(m->jnt_actfrcrange+2*jid, pj->actfrcrange, 2); + mjuu_copyvec(m->jnt_solref+mjNREF*jid, pj->solref_limit, mjNREF); + mjuu_copyvec(m->jnt_solimp+mjNIMP*jid, pj->solimp_limit, mjNIMP); m->jnt_margin[jid] = (mjtNum)pj->margin; - copyvec(m->jnt_user+nuser_jnt*jid, pj->get_userdata().data(), nuser_jnt); + mjuu_copyvec(m->jnt_user+nuser_jnt*jid, pj->get_userdata().data(), nuser_jnt); // not simple if: rotation already found, or pos not zero, or mis-aligned axis if (rotfound || !IsNullPose(m->jnt_pos+3*jid, NULL) || ((pj->type==mjJNT_HINGE || pj->type==mjJNT_SLIDE) && - ((mju_abs(pj->axis[0])>mjEPS) + - (mju_abs(pj->axis[1])>mjEPS) + - (mju_abs(pj->axis[2])>mjEPS)) > 1)) { + ((std::abs(pj->axis[0])>mjEPS) + + (std::abs(pj->axis[1])>mjEPS) + + (std::abs(pj->axis[2])>mjEPS)) > 1)) { m->body_simple[i] = 0; } @@ -1882,9 +1873,9 @@ void mjCModel::CopyTree(mjModel* m) { // set qpos0 and qpos_spring, check type switch (pj->type) { case mjJNT_FREE: - copyvec(m->qpos0+qposadr, pb->pos, 3); - copyvec(m->qpos0+qposadr+3, pb->quat, 4); - mju_copy(m->qpos_spring+qposadr, m->qpos0+qposadr, 7); + mjuu_copyvec(m->qpos0+qposadr, pb->pos, 3); + mjuu_copyvec(m->qpos0+qposadr+3, pb->quat, 4); + mjuu_copyvec(m->qpos_spring+qposadr, m->qpos0+qposadr, 7); break; case mjJNT_BALL: @@ -1892,7 +1883,7 @@ void mjCModel::CopyTree(mjModel* m) { m->qpos0[qposadr+1] = 0; m->qpos0[qposadr+2] = 0; m->qpos0[qposadr+3] = 0; - mju_copy4(m->qpos_spring+qposadr, m->qpos0+qposadr); + mjuu_copyvec(m->qpos_spring+qposadr, m->qpos0+qposadr, 4); break; case mjJNT_SLIDE: @@ -1910,8 +1901,8 @@ void mjCModel::CopyTree(mjModel* m) { // set attributes m->dof_bodyid[dofadr] = pb->id; m->dof_jntid[dofadr] = jid; - copyvec(m->dof_solref+mjNREF*dofadr, pj->solref_friction, mjNREF); - copyvec(m->dof_solimp+mjNIMP*dofadr, pj->solimp_friction, mjNIMP); + mjuu_copyvec(m->dof_solref+mjNREF*dofadr, pj->solref_friction, mjNREF); + mjuu_copyvec(m->dof_solimp+mjNIMP*dofadr, pj->solimp_friction, mjNIMP); m->dof_frictionloss[dofadr] = (mjtNum)pj->frictionloss; m->dof_armature[dofadr] = (mjtNum)pj->armature; m->dof_damping[dofadr] = (mjtNum)pj->damping; @@ -1962,19 +1953,19 @@ void mjCModel::CopyTree(mjModel* m) { m->geom_matid[gid] = pg->matid; m->geom_group[gid] = pg->group; m->geom_priority[gid] = pg->priority; - copyvec(m->geom_size+3*gid, pg->size, 3); - copyvec(m->geom_aabb+6*gid, pg->aabb, 6); - copyvec(m->geom_pos+3*gid, pg->pos, 3); - copyvec(m->geom_quat+4*gid, pg->quat, 4); - copyvec(m->geom_friction+3*gid, pg->friction, 3); + mjuu_copyvec(m->geom_size+3*gid, pg->size, 3); + mjuu_copyvec(m->geom_aabb+6*gid, pg->aabb, 6); + mjuu_copyvec(m->geom_pos+3*gid, pg->pos, 3); + mjuu_copyvec(m->geom_quat+4*gid, pg->quat, 4); + mjuu_copyvec(m->geom_friction+3*gid, pg->friction, 3); m->geom_solmix[gid] = (mjtNum)pg->solmix; - copyvec(m->geom_solref+mjNREF*gid, pg->solref, mjNREF); - copyvec(m->geom_solimp+mjNIMP*gid, pg->solimp, mjNIMP); + mjuu_copyvec(m->geom_solref+mjNREF*gid, pg->solref, mjNREF); + mjuu_copyvec(m->geom_solimp+mjNIMP*gid, pg->solimp, mjNIMP); m->geom_margin[gid] = (mjtNum)pg->margin; m->geom_gap[gid] = (mjtNum)pg->gap; - copyvec(m->geom_fluid+mjNFLUID*gid, pg->fluid, mjNFLUID); - copyvec(m->geom_user+nuser_geom*gid, pg->get_userdata().data(), nuser_geom); - copyvec(m->geom_rgba+4*gid, pg->rgba, 4); + mjuu_copyvec(m->geom_fluid+mjNFLUID*gid, pg->fluid, mjNFLUID); + mjuu_copyvec(m->geom_user+nuser_geom*gid, pg->get_userdata().data(), nuser_geom); + mjuu_copyvec(m->geom_rgba+4*gid, pg->rgba, 4); // determine sameframe if (IsNullPose(m->geom_pos+3*gid, m->geom_quat+4*gid)) { @@ -2006,11 +1997,11 @@ void mjCModel::CopyTree(mjModel* m) { m->site_bodyid[sid] = ps->body->id; m->site_matid[sid] = ps->matid; m->site_group[sid] = ps->group; - copyvec(m->site_size+3*sid, ps->size, 3); - copyvec(m->site_pos+3*sid, ps->pos, 3); - copyvec(m->site_quat+4*sid, ps->quat, 4); - copyvec(m->site_user+nuser_site*sid, ps->userdata_.data(), nuser_site); - copyvec(m->site_rgba+4*sid, ps->rgba, 4); + mjuu_copyvec(m->site_size+3*sid, ps->size, 3); + mjuu_copyvec(m->site_pos+3*sid, ps->pos, 3); + mjuu_copyvec(m->site_quat+4*sid, ps->quat, 4); + mjuu_copyvec(m->site_user+nuser_site*sid, ps->userdata_.data(), nuser_site); + mjuu_copyvec(m->site_rgba+4*sid, ps->rgba, 4); // determine sameframe if (IsNullPose(m->site_pos+3*sid, m->site_quat+4*sid)) { @@ -2038,15 +2029,15 @@ void mjCModel::CopyTree(mjModel* m) { m->cam_bodyid[cid] = pc->body->id; m->cam_mode[cid] = pc->mode; m->cam_targetbodyid[cid] = pc->targetbodyid; - copyvec(m->cam_pos+3*cid, pc->pos, 3); - copyvec(m->cam_quat+4*cid, pc->quat, 4); + mjuu_copyvec(m->cam_pos+3*cid, pc->pos, 3); + mjuu_copyvec(m->cam_quat+4*cid, pc->quat, 4); m->cam_orthographic[cid] = pc->orthographic; m->cam_fovy[cid] = (mjtNum)pc->fovy; m->cam_ipd[cid] = (mjtNum)pc->ipd; - copyvec(m->cam_resolution+2*cid, pc->resolution, 2); - copyvec(m->cam_sensorsize+2*cid, pc->sensor_size, 2); - copyvec(m->cam_intrinsic+4*cid, pc->intrinsic, 4); - copyvec(m->cam_user+nuser_cam*cid, pc->get_userdata().data(), nuser_cam); + mjuu_copyvec(m->cam_resolution+2*cid, pc->resolution, 2); + mjuu_copyvec(m->cam_sensorsize+2*cid, pc->sensor_size, 2); + mjuu_copyvec(m->cam_intrinsic+4*cid, pc->intrinsic, 4); + mjuu_copyvec(m->cam_user+nuser_cam*cid, pc->get_userdata().data(), nuser_cam); } // loop over lights for this body @@ -2062,15 +2053,15 @@ void mjCModel::CopyTree(mjModel* m) { m->light_directional[lid] = (mjtByte)pl->directional; m->light_castshadow[lid] = (mjtByte)pl->castshadow; m->light_active[lid] = (mjtByte)pl->active; - copyvec(m->light_pos+3*lid, pl->pos, 3); - copyvec(m->light_dir+3*lid, pl->dir, 3); + mjuu_copyvec(m->light_pos+3*lid, pl->pos, 3); + mjuu_copyvec(m->light_dir+3*lid, pl->dir, 3); m->light_bulbradius[lid] = pl->bulbradius; - copyvec(m->light_attenuation+3*lid, pl->attenuation, 3); + mjuu_copyvec(m->light_attenuation+3*lid, pl->attenuation, 3); m->light_cutoff[lid] = pl->cutoff; m->light_exponent[lid] = pl->exponent; - copyvec(m->light_ambient+3*lid, pl->ambient, 3); - copyvec(m->light_diffuse+3*lid, pl->diffuse, 3); - copyvec(m->light_specular+3*lid, pl->specular, 3); + mjuu_copyvec(m->light_ambient+3*lid, pl->ambient, 3); + mjuu_copyvec(m->light_diffuse+3*lid, pl->diffuse, 3); + mjuu_copyvec(m->light_specular+3*lid, pl->specular, 3); } } @@ -2210,9 +2201,9 @@ void mjCModel::CopyObjects(mjModel* m) { m->mesh_graphadr[i] = (pme->szgraph() ? graph_adr : -1); m->mesh_bvhnum[i] = pme->tree().nbvh; m->mesh_bvhadr[i] = pme->tree().nbvh ? bvh_adr : -1; - copyvec(&m->mesh_scale[3 * i], pme->get_scale(), 3); - copyvec(&m->mesh_pos[3 * i], pme->GetOffsetPosPtr(), 3); - copyvec(&m->mesh_quat[4 * i], pme->GetOffsetQuatPtr(), 4); + mjuu_copyvec(&m->mesh_scale[3 * i], pme->get_scale(), 3); + mjuu_copyvec(&m->mesh_pos[3 * i], pme->GetOffsetPosPtr(), 3); + mjuu_copyvec(&m->mesh_quat[4 * i], pme->GetOffsetQuatPtr(), 4); // copy vertices, normals, faces, texcoords, aux data pme->CopyVert(m->mesh_vert + 3*vert_adr); @@ -2268,13 +2259,13 @@ void mjCModel::CopyObjects(mjModel* m) { m->flex_group[i] = pfl->group; m->flex_priority[i] = pfl->priority; m->flex_solmix[i] = (mjtNum)pfl->solmix; - copyvec(m->flex_solref + mjNREF * i, pfl->solref, mjNREF); - copyvec(m->flex_solimp + mjNIMP * i, pfl->solimp, mjNIMP); + mjuu_copyvec(m->flex_solref + mjNREF * i, pfl->solref, mjNREF); + mjuu_copyvec(m->flex_solimp + mjNIMP * i, pfl->solimp, mjNIMP); m->flex_radius[i] = (mjtNum)pfl->radius; - copyvec(m->flex_friction + 3 * i, pfl->friction, 3); + mjuu_copyvec(m->flex_friction + 3 * i, pfl->friction, 3); m->flex_margin[i] = (mjtNum)pfl->margin; m->flex_gap[i] = (mjtNum)pfl->gap; - copyvec(m->flex_rgba + 4 * i, pfl->rgba, 4); + mjuu_copyvec(m->flex_rgba + 4 * i, pfl->rgba, 4); // set fields: mesh-like m->flex_dim[i] = pfl->dim; @@ -2338,7 +2329,7 @@ void mjCModel::CopyObjects(mjModel* m) { // copy or set vert if (pfl->centered) { - mju_zero(m->flex_vert + 3*vert_adr, 3*pfl->nvert); + mjuu_zerovec(m->flex_vert + 3*vert_adr, 3*pfl->nvert); } else { memcpy(m->flex_vert + 3*vert_adr, pfl->vert_.data(), 3*pfl->nvert*sizeof(mjtNum)); @@ -2393,7 +2384,7 @@ void mjCModel::CopyObjects(mjModel* m) { // set fields m->skin_matid[i] = psk->matid; m->skin_group[i] = psk->group; - copyvec(m->skin_rgba+4*i, psk->rgba, 4); + mjuu_copyvec(m->skin_rgba+4*i, psk->rgba, 4); m->skin_inflate[i] = psk->inflate; m->skin_vertadr[i] = vert_adr; m->skin_vertnum[i] = psk->get_vert().size()/3; @@ -2448,7 +2439,7 @@ void mjCModel::CopyObjects(mjModel* m) { mjCHField* phf = hfields_[i]; // set fields - copyvec(m->hfield_size+4*i, phf->size, 4); + mjuu_copyvec(m->hfield_size+4*i, phf->size, 4); m->hfield_nrow[i] = phf->nrow; m->hfield_ncol[i] = phf->ncol; m->hfield_adr[i] = data_adr; @@ -2487,14 +2478,14 @@ void mjCModel::CopyObjects(mjModel* m) { // set fields m->mat_texid[i] = pmat->texid; m->mat_texuniform[i] = pmat->texuniform; - copyvec(m->mat_texrepeat+2*i, pmat->texrepeat, 2); + mjuu_copyvec(m->mat_texrepeat+2*i, pmat->texrepeat, 2); m->mat_emission[i] = pmat->emission; m->mat_specular[i] = pmat->specular; m->mat_shininess[i] = pmat->shininess; m->mat_reflectance[i] = pmat->reflectance; m->mat_metallic[i] = pmat->metallic; m->mat_roughness[i] = pmat->roughness; - copyvec(m->mat_rgba+4*i, pmat->rgba, 4); + mjuu_copyvec(m->mat_rgba+4*i, pmat->rgba, 4); } // geom pairs to include @@ -2503,12 +2494,12 @@ void mjCModel::CopyObjects(mjModel* m) { m->pair_geom1[i] = pairs_[i]->geom1->id; m->pair_geom2[i] = pairs_[i]->geom2->id; m->pair_signature[i] = pairs_[i]->signature; - copyvec(m->pair_solref+mjNREF*i, pairs_[i]->solref, mjNREF); - copyvec(m->pair_solreffriction+mjNREF*i, pairs_[i]->solreffriction, mjNREF); - copyvec(m->pair_solimp+mjNIMP*i, pairs_[i]->solimp, mjNIMP); + mjuu_copyvec(m->pair_solref+mjNREF*i, pairs_[i]->solref, mjNREF); + mjuu_copyvec(m->pair_solreffriction+mjNREF*i, pairs_[i]->solreffriction, mjNREF); + mjuu_copyvec(m->pair_solimp+mjNIMP*i, pairs_[i]->solimp, mjNIMP); m->pair_margin[i] = (mjtNum)pairs_[i]->margin; m->pair_gap[i] = (mjtNum)pairs_[i]->gap; - copyvec(m->pair_friction+5*i, pairs_[i]->friction, 5); + mjuu_copyvec(m->pair_friction+5*i, pairs_[i]->friction, 5); } // body pairs to exclude @@ -2526,9 +2517,9 @@ void mjCModel::CopyObjects(mjModel* m) { m->eq_obj1id[i] = peq->obj1id; m->eq_obj2id[i] = peq->obj2id; m->eq_active0[i] = peq->active; - copyvec(m->eq_solref+mjNREF*i, peq->solref, mjNREF); - copyvec(m->eq_solimp+mjNIMP*i, peq->solimp, mjNIMP); - copyvec(m->eq_data+mjNEQDATA*i, peq->data, mjNEQDATA); + mjuu_copyvec(m->eq_solref+mjNREF*i, peq->solref, mjNREF); + mjuu_copyvec(m->eq_solimp+mjNIMP*i, peq->solimp, mjNIMP); + mjuu_copyvec(m->eq_data+mjNEQDATA*i, peq->data, mjNEQDATA); } // tendons and wraps @@ -2544,10 +2535,10 @@ void mjCModel::CopyObjects(mjModel* m) { m->tendon_group[i] = pte->group; m->tendon_limited[i] = (mjtByte)pte->is_limited(); m->tendon_width[i] = (mjtNum)pte->width; - copyvec(m->tendon_solref_lim+mjNREF*i, pte->solref_limit, mjNREF); - copyvec(m->tendon_solimp_lim+mjNIMP*i, pte->solimp_limit, mjNIMP); - copyvec(m->tendon_solref_fri+mjNREF*i, pte->solref_friction, mjNREF); - copyvec(m->tendon_solimp_fri+mjNIMP*i, pte->solimp_friction, mjNIMP); + mjuu_copyvec(m->tendon_solref_lim+mjNREF*i, pte->solref_limit, mjNREF); + mjuu_copyvec(m->tendon_solimp_lim+mjNIMP*i, pte->solimp_limit, mjNIMP); + mjuu_copyvec(m->tendon_solref_fri+mjNREF*i, pte->solref_friction, mjNREF); + mjuu_copyvec(m->tendon_solimp_fri+mjNIMP*i, pte->solimp_friction, mjNIMP); m->tendon_range[2*i] = (mjtNum)pte->range[0]; m->tendon_range[2*i+1] = (mjtNum)pte->range[1]; m->tendon_margin[i] = (mjtNum)pte->margin; @@ -2556,8 +2547,8 @@ void mjCModel::CopyObjects(mjModel* m) { m->tendon_frictionloss[i] = (mjtNum)pte->frictionloss; m->tendon_lengthspring[2*i] = (mjtNum)pte->springlength[0]; m->tendon_lengthspring[2*i+1] = (mjtNum)pte->springlength[1]; - copyvec(m->tendon_user+nuser_tendon*i, pte->get_userdata().data(), nuser_tendon); - copyvec(m->tendon_rgba+4*i, pte->rgba, 4); + mjuu_copyvec(m->tendon_user+nuser_tendon*i, pte->get_userdata().data(), nuser_tendon); + mjuu_copyvec(m->tendon_rgba+4*i, pte->rgba, 4); // set wraps for (int j=0; j<(int)pte->path.size(); j++) { @@ -2597,15 +2588,15 @@ void mjCModel::CopyObjects(mjModel* m) { m->actuator_actlimited[i] = (mjtByte)pac->is_actlimited(); m->actuator_actearly[i] = pac->actearly; m->actuator_cranklength[i] = (mjtNum)pac->cranklength; - copyvec(m->actuator_gear + 6*i, pac->gear, 6); - copyvec(m->actuator_dynprm + mjNDYN*i, pac->dynprm, mjNDYN); - copyvec(m->actuator_gainprm + mjNGAIN*i, pac->gainprm, mjNGAIN); - copyvec(m->actuator_biasprm + mjNBIAS*i, pac->biasprm, mjNBIAS); - copyvec(m->actuator_ctrlrange + 2*i, pac->ctrlrange, 2); - copyvec(m->actuator_forcerange + 2*i, pac->forcerange, 2); - copyvec(m->actuator_actrange + 2*i, pac->actrange, 2); - copyvec(m->actuator_lengthrange + 2*i, pac->lengthrange, 2); - copyvec(m->actuator_user+nuser_actuator*i, pac->get_userdata().data(), nuser_actuator); + mjuu_copyvec(m->actuator_gear + 6*i, pac->gear, 6); + mjuu_copyvec(m->actuator_dynprm + mjNDYN*i, pac->dynprm, mjNDYN); + mjuu_copyvec(m->actuator_gainprm + mjNGAIN*i, pac->gainprm, mjNGAIN); + mjuu_copyvec(m->actuator_biasprm + mjNBIAS*i, pac->biasprm, mjNBIAS); + mjuu_copyvec(m->actuator_ctrlrange + 2*i, pac->ctrlrange, 2); + mjuu_copyvec(m->actuator_forcerange + 2*i, pac->forcerange, 2); + mjuu_copyvec(m->actuator_actrange + 2*i, pac->actrange, 2); + mjuu_copyvec(m->actuator_lengthrange + 2*i, pac->lengthrange, 2); + mjuu_copyvec(m->actuator_user+nuser_actuator*i, pac->get_userdata().data(), nuser_actuator); } // sensors @@ -2625,7 +2616,7 @@ void mjCModel::CopyObjects(mjModel* m) { m->sensor_dim[i] = psen->dim; m->sensor_cutoff[i] = (mjtNum)psen->cutoff; m->sensor_noise[i] = (mjtNum)psen->noise; - copyvec(m->sensor_user+nuser_sensor*i, psen->get_userdata().data(), nuser_sensor); + mjuu_copyvec(m->sensor_user+nuser_sensor*i, psen->get_userdata().data(), nuser_sensor); // calculate address and advance m->sensor_adr[i] = adr; @@ -2690,34 +2681,34 @@ void mjCModel::CopyObjects(mjModel* m) { for (int i=0; ikey_time[i] = (mjtNum)keys_[i]->time; - copyvec(m->key_qpos+i*nq, keys_[i]->qpos_.data(), nq); - copyvec(m->key_qvel+i*nv, keys_[i]->qvel_.data(), nv); + mjuu_copyvec(m->key_qpos+i*nq, keys_[i]->qpos_.data(), nq); + mjuu_copyvec(m->key_qvel+i*nv, keys_[i]->qvel_.data(), nv); if (na) { - copyvec(m->key_act+i*na, keys_[i]->act_.data(), na); + mjuu_copyvec(m->key_act+i*na, keys_[i]->act_.data(), na); } if (nmocap) { - copyvec(m->key_mpos + i*3*nmocap, keys_[i]->mpos_.data(), 3*nmocap); - copyvec(m->key_mquat + i*4*nmocap, keys_[i]->mquat_.data(), 4*nmocap); + mjuu_copyvec(m->key_mpos + i*3*nmocap, keys_[i]->mpos_.data(), 3*nmocap); + mjuu_copyvec(m->key_mquat + i*4*nmocap, keys_[i]->mquat_.data(), 4*nmocap); } // normalize quaternions in m->key_qpos for (int j=0; jnjnt; j++) { if (m->jnt_type[j]==mjJNT_BALL || m->jnt_type[j]==mjJNT_FREE) { - mju_normalize4(m->key_qpos+i*nq+m->jnt_qposadr[j]+3*(m->jnt_type[j]==mjJNT_FREE)); + mjuu_normvec(m->key_qpos+i*nq+m->jnt_qposadr[j]+3*(m->jnt_type[j]==mjJNT_FREE), 4); } } // normalize quaternions in m->key_mquat for (int j=0; jkey_mquat+i*4*nmocap+4*j); + mjuu_normvec(m->key_mquat+i*4*nmocap+4*j, 4); } - copyvec(m->key_ctrl+i*nu, keys_[i]->ctrl_.data(), nu); + mjuu_copyvec(m->key_ctrl+i*nu, keys_[i]->ctrl_.data(), nu); } // save qpos0 in user model (to recognize changed key_qpos in write) qpos0.resize(nq); - mju_copy(qpos0.data(), m->qpos0, nq); + mjuu_copyvec(qpos0.data(), m->qpos0, nq); } @@ -2727,17 +2718,17 @@ void mjCModel::SaveState(const mjData* d) { for (auto joint : joints_) { switch (joint->type) { case mjJNT_FREE: - mju_copy(joint->qpos, d->qpos + joint->qposadr_, 7); - mju_copy(joint->qvel, d->qvel + joint->dofadr_, 6); + mjuu_copyvec(joint->qpos, d->qpos + joint->qposadr_, 7); + mjuu_copyvec(joint->qvel, d->qvel + joint->dofadr_, 6); break; case mjJNT_BALL: - mju_copy(joint->qpos, d->qpos + joint->qposadr_, 4); - mju_copy(joint->qvel, d->qvel + joint->dofadr_, 3); + mjuu_copyvec(joint->qpos, d->qpos + joint->qposadr_, 4); + mjuu_copyvec(joint->qvel, d->qvel + joint->dofadr_, 3); break; case mjJNT_HINGE: case mjJNT_SLIDE: - mju_copy(joint->qpos, d->qpos + joint->qposadr_, 1); - mju_copy(joint->qvel, d->qvel + joint->dofadr_, 1); + mjuu_copyvec(joint->qpos, d->qpos + joint->qposadr_, 1); + mjuu_copyvec(joint->qvel, d->qvel + joint->dofadr_, 1); break; } } @@ -2745,7 +2736,7 @@ void mjCModel::SaveState(const mjData* d) { for (auto actuator : actuators_) { if (actuator->actadr_ != -1) { actuator->act.assign(actuator->actnum_, 0); - mju_copy(actuator->act.data(), d->act + actuator->actadr_, actuator->actnum_); + mjuu_copyvec(actuator->act.data(), d->act + actuator->actadr_, actuator->actnum_); } } } @@ -2767,24 +2758,24 @@ void mjCModel::RestoreState(const mjModel* m, mjData** dest) { } switch (joint->type) { case mjJNT_FREE: - mju_copy(d->qpos + joint->qposadr_, joint->qpos, 7); - mju_copy(d->qvel + joint->dofadr_, joint->qvel, 6); + mjuu_copyvec(d->qpos + joint->qposadr_, joint->qpos, 7); + mjuu_copyvec(d->qvel + joint->dofadr_, joint->qvel, 6); break; case mjJNT_BALL: - mju_copy(d->qpos + joint->qposadr_, joint->qpos, 4); - mju_copy(d->qvel + joint->dofadr_, joint->qvel, 3); + mjuu_copyvec(d->qpos + joint->qposadr_, joint->qpos, 4); + mjuu_copyvec(d->qvel + joint->dofadr_, joint->qvel, 3); break; case mjJNT_HINGE: case mjJNT_SLIDE: - mju_copy(d->qpos + joint->qposadr_, joint->qpos, 1); - mju_copy(d->qvel + joint->dofadr_, joint->qvel, 1); + mjuu_copyvec(d->qpos + joint->qposadr_, joint->qpos, 1); + mjuu_copyvec(d->qvel + joint->dofadr_, joint->qvel, 1); break; } } for (auto actuator : actuators_) { if (mjuu_defined(actuator->act[0])) { - mju_copy(d->act + actuator->actadr_, actuator->act.data(), actuator->actnum_); + mjuu_copyvec(d->act + actuator->actadr_, actuator->act.data(), actuator->actnum_); } } } @@ -3566,14 +3557,14 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { meaninertia_auto = m->stat.meaninertia; meanmass_auto = m->stat.meanmass; meansize_auto = m->stat.meansize; - copyvec(center_auto, m->stat.center, 3); + mjuu_copyvec(center_auto, m->stat.center, 3); // override model statistics if defined by user if (mjuu_defined(stat.extent)) m->stat.extent = (mjtNum)stat.extent; if (mjuu_defined(stat.meaninertia)) m->stat.meaninertia = (mjtNum)stat.meaninertia; if (mjuu_defined(stat.meanmass)) m->stat.meanmass = (mjtNum)stat.meanmass; if (mjuu_defined(stat.meansize)) m->stat.meansize = (mjtNum)stat.meansize; - if (mjuu_defined(stat.center[0])) copyvec(m->stat.center, stat.center, 3); + if (mjuu_defined(stat.center[0])) mjuu_copyvec(m->stat.center, stat.center, 3); // assert that model has valid references const char* validationerr = mj_validateReferences(m); @@ -3652,15 +3643,15 @@ bool mjCModel::CopyBack(const mjModel* m) { if (m->stat.center[0] != center_auto[0] || m->stat.center[1] != center_auto[1] || m->stat.center[2] != center_auto[2]) { - mju_copy3(stat.center, m->stat.center); + mjuu_copyvec(stat.center, m->stat.center, 3); } // qpos0, qpos_spring for (int i=0; itype) { case mjJNT_FREE: - copyvec(bodies_[m->jnt_bodyid[i]]->pos, m->qpos0+m->jnt_qposadr[i], 3); - copyvec(bodies_[m->jnt_bodyid[i]]->quat, m->qpos0+m->jnt_qposadr[i]+3, 4); + mjuu_copyvec(bodies_[m->jnt_bodyid[i]]->pos, m->qpos0+m->jnt_qposadr[i], 3); + mjuu_copyvec(bodies_[m->jnt_bodyid[i]]->quat, m->qpos0+m->jnt_qposadr[i]+3, 4); break; case mjJNT_SLIDE: @@ -3674,22 +3665,22 @@ bool mjCModel::CopyBack(const mjModel* m) { break; } } - mju_copy(qpos0.data(), m->qpos0, m->nq); + mjuu_copyvec(qpos0.data(), m->qpos0, m->nq); // body mjCBody* pb; for (int i=0; ipos, m->body_pos+3*i, 3); - copyvec(pb->quat, m->body_quat+4*i, 4); - copyvec(pb->ipos, m->body_ipos+3*i, 3); - copyvec(pb->iquat, m->body_iquat+4*i, 4); + mjuu_copyvec(pb->pos, m->body_pos+3*i, 3); + mjuu_copyvec(pb->quat, m->body_quat+4*i, 4); + mjuu_copyvec(pb->ipos, m->body_ipos+3*i, 3); + mjuu_copyvec(pb->iquat, m->body_iquat+4*i, 4); pb->mass = (double)m->body_mass[i]; - copyvec(pb->inertia, m->body_inertia+3*i, 3); + mjuu_copyvec(pb->inertia, m->body_inertia+3*i, 3); if (nuser_body) { - copyvec(pb->userdata_.data(), m->body_user + nuser_body*i, nuser_body); + mjuu_copyvec(pb->userdata_.data(), m->body_user + nuser_body*i, nuser_body); } } @@ -3699,22 +3690,22 @@ bool mjCModel::CopyBack(const mjModel* m) { pj = joints_[i]; // joint data - copyvec(pj->pos, m->jnt_pos+3*i, 3); - copyvec(pj->axis, m->jnt_axis+3*i, 3); + mjuu_copyvec(pj->pos, m->jnt_pos+3*i, 3); + mjuu_copyvec(pj->axis, m->jnt_axis+3*i, 3); pj->stiffness = (double)m->jnt_stiffness[i]; - copyvec(pj->range, m->jnt_range+2*i, 2); - copyvec(pj->solref_limit, m->jnt_solref+mjNREF*i, mjNREF); - copyvec(pj->solimp_limit, m->jnt_solimp+mjNIMP*i, mjNIMP); + mjuu_copyvec(pj->range, m->jnt_range+2*i, 2); + mjuu_copyvec(pj->solref_limit, m->jnt_solref+mjNREF*i, mjNREF); + mjuu_copyvec(pj->solimp_limit, m->jnt_solimp+mjNIMP*i, mjNIMP); pj->margin = (double)m->jnt_margin[i]; if (nuser_jnt) { - copyvec(pj->userdata_.data(), m->jnt_user + nuser_jnt*i, nuser_jnt); + mjuu_copyvec(pj->userdata_.data(), m->jnt_user + nuser_jnt*i, nuser_jnt); } // dof data int j = m->jnt_dofadr[i]; - copyvec(pj->solref_friction, m->dof_solref+mjNREF*j, mjNREF); - copyvec(pj->solimp_friction, m->dof_solimp+mjNIMP*j, mjNIMP); + mjuu_copyvec(pj->solref_friction, m->dof_solref+mjNREF*j, mjNREF); + mjuu_copyvec(pj->solimp_friction, m->dof_solimp+mjNIMP*j, mjNIMP); pj->armature = (double)m->dof_armature[j]; pj->damping = (double)m->dof_damping[j]; pj->frictionloss = (double)m->dof_frictionloss[j]; @@ -3725,19 +3716,19 @@ bool mjCModel::CopyBack(const mjModel* m) { for (int i=0; isize, m->geom_size+3*i, 3); - copyvec(pg->pos, m->geom_pos+3*i, 3); - copyvec(pg->quat, m->geom_quat+4*i, 4); - copyvec(pg->friction, m->geom_friction+3*i, 3); - copyvec(pg->solref, m->geom_solref+mjNREF*i, mjNREF); - copyvec(pg->solimp, m->geom_solimp+mjNIMP*i, mjNIMP); - copyvec(pg->rgba, m->geom_rgba+4*i, 4); + mjuu_copyvec(pg->size, m->geom_size+3*i, 3); + mjuu_copyvec(pg->pos, m->geom_pos+3*i, 3); + mjuu_copyvec(pg->quat, m->geom_quat+4*i, 4); + mjuu_copyvec(pg->friction, m->geom_friction+3*i, 3); + mjuu_copyvec(pg->solref, m->geom_solref+mjNREF*i, mjNREF); + mjuu_copyvec(pg->solimp, m->geom_solimp+mjNIMP*i, mjNIMP); + mjuu_copyvec(pg->rgba, m->geom_rgba+4*i, 4); pg->solmix = (double)m->geom_solmix[i]; pg->margin = (double)m->geom_margin[i]; pg->gap = (double)m->geom_gap[i]; if (nuser_geom) { - copyvec(pg->userdata_.data(), m->geom_user + nuser_geom*i, nuser_geom); + mjuu_copyvec(pg->userdata_.data(), m->geom_user + nuser_geom*i, nuser_geom); } } @@ -3745,8 +3736,8 @@ bool mjCModel::CopyBack(const mjModel* m) { mjCMesh* pm; for (int i=0; iGetOffsetPosPtr(), m->mesh_pos+3*i, 3); - copyvec(pm->GetOffsetQuatPtr(), m->mesh_quat+4*i, 4); + mjuu_copyvec(pm->GetOffsetPosPtr(), m->mesh_pos+3*i, 3); + mjuu_copyvec(pm->GetOffsetQuatPtr(), m->mesh_quat+4*i, 4); } // heightfield @@ -3762,84 +3753,84 @@ bool mjCModel::CopyBack(const mjModel* m) { // copy back in reverse row order for (int j=0; jsize, m->site_size + 3 * i, 3); - copyvec(sites_[i]->pos, m->site_pos+3*i, 3); - copyvec(sites_[i]->quat, m->site_quat+4*i, 4); - copyvec(sites_[i]->rgba, m->site_rgba+4*i, 4); + mjuu_copyvec(sites_[i]->size, m->site_size + 3 * i, 3); + mjuu_copyvec(sites_[i]->pos, m->site_pos+3*i, 3); + mjuu_copyvec(sites_[i]->quat, m->site_quat+4*i, 4); + mjuu_copyvec(sites_[i]->rgba, m->site_rgba+4*i, 4); if (nuser_site) { - copyvec(sites_[i]->userdata_.data(), m->site_user + nuser_site*i, nuser_site); + mjuu_copyvec(sites_[i]->userdata_.data(), m->site_user + nuser_site*i, nuser_site); } } // cameras for (int i=0; ipos, m->cam_pos+3*i, 3); - copyvec(cameras_[i]->quat, m->cam_quat+4*i, 4); + mjuu_copyvec(cameras_[i]->pos, m->cam_pos+3*i, 3); + mjuu_copyvec(cameras_[i]->quat, m->cam_quat+4*i, 4); cameras_[i]->fovy = (double)m->cam_fovy[i]; cameras_[i]->ipd = (double)m->cam_ipd[i]; - copyvec(cameras_[i]->resolution, m->cam_resolution+2*i, 2); - copyvec(cameras_[i]->intrinsic, m->cam_intrinsic+4*i, 4); + mjuu_copyvec(cameras_[i]->resolution, m->cam_resolution+2*i, 2); + mjuu_copyvec(cameras_[i]->intrinsic, m->cam_intrinsic+4*i, 4); if (nuser_cam) { - copyvec(cameras_[i]->userdata_.data(), m->cam_user + nuser_cam*i, nuser_cam); + mjuu_copyvec(cameras_[i]->userdata_.data(), m->cam_user + nuser_cam*i, nuser_cam); } } // lights for (int i=0; ipos, m->light_pos+3*i, 3); - copyvec(lights_[i]->dir, m->light_dir+3*i, 3); - copyvec(lights_[i]->attenuation, m->light_attenuation+3*i, 3); + mjuu_copyvec(lights_[i]->pos, m->light_pos+3*i, 3); + mjuu_copyvec(lights_[i]->dir, m->light_dir+3*i, 3); + mjuu_copyvec(lights_[i]->attenuation, m->light_attenuation+3*i, 3); lights_[i]->cutoff = m->light_cutoff[i]; lights_[i]->exponent = m->light_exponent[i]; - copyvec(lights_[i]->ambient, m->light_ambient+3*i, 3); - copyvec(lights_[i]->diffuse, m->light_diffuse+3*i, 3); - copyvec(lights_[i]->specular, m->light_specular+3*i, 3); + mjuu_copyvec(lights_[i]->ambient, m->light_ambient+3*i, 3); + mjuu_copyvec(lights_[i]->diffuse, m->light_diffuse+3*i, 3); + mjuu_copyvec(lights_[i]->specular, m->light_specular+3*i, 3); } // materials for (int i=0; itexrepeat, m->mat_texrepeat+2*i, 2); + mjuu_copyvec(materials_[i]->texrepeat, m->mat_texrepeat+2*i, 2); materials_[i]->emission = m->mat_emission[i]; materials_[i]->specular = m->mat_specular[i]; materials_[i]->shininess = m->mat_shininess[i]; materials_[i]->reflectance = m->mat_reflectance[i]; - copyvec(materials_[i]->rgba, m->mat_rgba+4*i, 4); + mjuu_copyvec(materials_[i]->rgba, m->mat_rgba+4*i, 4); } // pairs for (int i=0; isolref, m->pair_solref+mjNREF*i, mjNREF); - copyvec(pairs_[i]->solreffriction, m->pair_solreffriction+mjNREF*i, mjNREF); - copyvec(pairs_[i]->solimp, m->pair_solimp+mjNIMP*i, mjNIMP); + mjuu_copyvec(pairs_[i]->solref, m->pair_solref+mjNREF*i, mjNREF); + mjuu_copyvec(pairs_[i]->solreffriction, m->pair_solreffriction+mjNREF*i, mjNREF); + mjuu_copyvec(pairs_[i]->solimp, m->pair_solimp+mjNIMP*i, mjNIMP); pairs_[i]->margin = (double)m->pair_margin[i]; pairs_[i]->gap = (double)m->pair_gap[i]; - copyvec(pairs_[i]->friction, m->pair_friction+5*i, 5); + mjuu_copyvec(pairs_[i]->friction, m->pair_friction+5*i, 5); } // equality constraints for (int i=0; idata, m->eq_data+mjNEQDATA*i, mjNEQDATA); - copyvec(equalities_[i]->solref, m->eq_solref+mjNREF*i, mjNREF); - copyvec(equalities_[i]->solimp, m->eq_solimp+mjNIMP*i, mjNIMP); + mjuu_copyvec(equalities_[i]->data, m->eq_data+mjNEQDATA*i, mjNEQDATA); + mjuu_copyvec(equalities_[i]->solref, m->eq_solref+mjNREF*i, mjNREF); + mjuu_copyvec(equalities_[i]->solimp, m->eq_solimp+mjNIMP*i, mjNIMP); } // tendons for (int i=0; irange, m->tendon_range+2*i, 2); - copyvec(tendons_[i]->solref_limit, m->tendon_solref_lim+mjNREF*i, mjNREF); - copyvec(tendons_[i]->solimp_limit, m->tendon_solimp_lim+mjNIMP*i, mjNIMP); - copyvec(tendons_[i]->solref_friction, m->tendon_solref_fri+mjNREF*i, mjNREF); - copyvec(tendons_[i]->solimp_friction, m->tendon_solimp_fri+mjNIMP*i, mjNIMP); - copyvec(tendons_[i]->rgba, m->tendon_rgba+4*i, 4); + mjuu_copyvec(tendons_[i]->range, m->tendon_range+2*i, 2); + mjuu_copyvec(tendons_[i]->solref_limit, m->tendon_solref_lim+mjNREF*i, mjNREF); + mjuu_copyvec(tendons_[i]->solimp_limit, m->tendon_solimp_lim+mjNIMP*i, mjNIMP); + mjuu_copyvec(tendons_[i]->solref_friction, m->tendon_solref_fri+mjNREF*i, mjNREF); + mjuu_copyvec(tendons_[i]->solimp_friction, m->tendon_solimp_fri+mjNIMP*i, mjNIMP); + mjuu_copyvec(tendons_[i]->rgba, m->tendon_rgba+4*i, 4); tendons_[i]->width = (double)m->tendon_width[i]; tendons_[i]->margin = (double)m->tendon_margin[i]; tendons_[i]->stiffness = (double)m->tendon_stiffness[i]; @@ -3847,7 +3838,7 @@ bool mjCModel::CopyBack(const mjModel* m) { tendons_[i]->frictionloss = (double)m->tendon_frictionloss[i]; if (nuser_tendon) { - copyvec(tendons_[i]->userdata_.data(), m->tendon_user + nuser_tendon*i, nuser_tendon); + mjuu_copyvec(tendons_[i]->userdata_.data(), m->tendon_user + nuser_tendon*i, nuser_tendon); } } @@ -3856,18 +3847,18 @@ bool mjCModel::CopyBack(const mjModel* m) { for (int i=0; idynprm, m->actuator_dynprm+i*mjNDYN, mjNDYN); - copyvec(pa->gainprm, m->actuator_gainprm+i*mjNGAIN, mjNGAIN); - copyvec(pa->biasprm, m->actuator_biasprm+i*mjNBIAS, mjNBIAS); - copyvec(pa->ctrlrange, m->actuator_ctrlrange+2*i, 2); - copyvec(pa->forcerange, m->actuator_forcerange+2*i, 2); - copyvec(pa->actrange, m->actuator_actrange+2*i, 2); - copyvec(pa->lengthrange, m->actuator_lengthrange+2*i, 2); - copyvec(pa->gear, m->actuator_gear+6*i, 6); + mjuu_copyvec(pa->dynprm, m->actuator_dynprm+i*mjNDYN, mjNDYN); + mjuu_copyvec(pa->gainprm, m->actuator_gainprm+i*mjNGAIN, mjNGAIN); + mjuu_copyvec(pa->biasprm, m->actuator_biasprm+i*mjNBIAS, mjNBIAS); + mjuu_copyvec(pa->ctrlrange, m->actuator_ctrlrange+2*i, 2); + mjuu_copyvec(pa->forcerange, m->actuator_forcerange+2*i, 2); + mjuu_copyvec(pa->actrange, m->actuator_actrange+2*i, 2); + mjuu_copyvec(pa->lengthrange, m->actuator_lengthrange+2*i, 2); + mjuu_copyvec(pa->gear, m->actuator_gear+6*i, 6); pa->cranklength = (double)m->actuator_cranklength[i]; if (nuser_actuator) { - copyvec(pa->userdata_.data(), m->actuator_user + nuser_actuator*i, nuser_actuator); + mjuu_copyvec(pa->userdata_.data(), m->actuator_user + nuser_actuator*i, nuser_actuator); } } @@ -3877,7 +3868,7 @@ bool mjCModel::CopyBack(const mjModel* m) { sensors_[i]->noise = (double)m->sensor_noise[i]; if (nuser_sensor) { - copyvec(sensors_[i]->userdata_.data(), m->sensor_user + nuser_sensor*i, nuser_sensor); + mjuu_copyvec(sensors_[i]->userdata_.data(), m->sensor_user + nuser_sensor*i, nuser_sensor); } } @@ -3900,17 +3891,17 @@ bool mjCModel::CopyBack(const mjModel* m) { mjCKey* pk = keys_[i]; pk->time = (double)m->key_time[i]; - copyvec(pk->qpos_.data(), m->key_qpos + i*nq, nq); - copyvec(pk->qvel_.data(), m->key_qvel + i*nv, nv); + mjuu_copyvec(pk->qpos_.data(), m->key_qpos + i*nq, nq); + mjuu_copyvec(pk->qvel_.data(), m->key_qvel + i*nv, nv); if (na) { - copyvec(pk->act_.data(), m->key_act + i*na, na); + mjuu_copyvec(pk->act_.data(), m->key_act + i*na, na); } if (nmocap) { - copyvec(pk->mpos_.data(), m->key_mpos + i*3*nmocap, 3*nmocap); - copyvec(pk->mquat_.data(), m->key_mquat + i*4*nmocap, 4*nmocap); + mjuu_copyvec(pk->mpos_.data(), m->key_mpos + i*3*nmocap, 3*nmocap); + mjuu_copyvec(pk->mquat_.data(), m->key_mquat + i*4*nmocap, 4*nmocap); } if (nu) { - copyvec(pk->ctrl_.data(), m->key_ctrl + i*nu, nu); + mjuu_copyvec(pk->ctrl_.data(), m->key_ctrl + i*nu, nu); } } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 223d951e..bacf8129 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -37,14 +37,7 @@ #include #include "cc/array_safety.h" #include "engine/engine_resource.h" -#include "engine/engine_io.h" #include "engine/engine_passive.h" -#include "engine/engine_plugin.h" -#include "engine/engine_util_blas.h" -#include "engine/engine_util_errmem.h" -#include "engine/engine_util_misc.h" -#include "engine/engine_util_solve.h" -#include "engine/engine_util_spatial.h" #include #include "user/user_api.h" #include "user/user_cache.h" @@ -104,14 +97,14 @@ const char* FullInertia(double quat[4], double inertia[3], const double fulliner return nullptr; } - mjtNum eigval[3], eigvec[9], quattmp[4]; - mjtNum full[9] = { + double eigval[3], eigvec[9], quattmp[4]; + double full[9] = { fullinertia[0], fullinertia[3], fullinertia[4], fullinertia[3], fullinertia[1], fullinertia[5], fullinertia[4], fullinertia[5], fullinertia[2] }; - mju_eig3(eigval, eigvec, quattmp, full); + mjuu_eig3(eigval, eigvec, quattmp, full); // check mimimal eigenvalue if (eigval[2] elements; elements.reserve(bvleaf_.size()); - mjtNum qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]}; + double qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]}; for (int i = 0; i < bvleaf_.size(); i++) { if (bvleaf_[i].conaffinity || bvleaf_[i].contype) { BVElement element; element.e = &bvleaf_[i]; - mjtNum vert[3] = {element.e->pos[0] - ipos_[0], + double vert[3] = {element.e->pos[0] - ipos_[0], element.e->pos[1] - ipos_[1], element.e->pos[2] - ipos_[2]}; - mju_rotVecQuat(element.lpos, vert, qinv); + mjuu_rotVecQuat(element.lpos, vert, qinv); elements.push_back(std::move(element)); } } @@ -448,15 +441,15 @@ int mjCBoundingVolumeHierarchy::MakeBVH( if (nelements == 0) { return -1; } - mjtNum AAMM[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL}; + double AAMM[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL}; // inverse transformation - mjtNum qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]}; + double qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]}; // accumulate AAMM over elements for (auto element = elements_begin; element != elements_end; ++element) { // transform element aabb to aamm format - mjtNum aamm[6] = {element->e->aabb[0] - element->e->aabb[3], + double aamm[6] = {element->e->aabb[0] - element->e->aabb[3], element->e->aabb[1] - element->e->aabb[4], element->e->aabb[2] - element->e->aabb[5], element->e->aabb[0] + element->e->aabb[3], @@ -465,32 +458,32 @@ int mjCBoundingVolumeHierarchy::MakeBVH( // update node AAMM for (int v=0; v<8; v++) { - mjtNum vert[3], box[3]; + double vert[3], box[3]; vert[0] = (v&1 ? aamm[3] : aamm[0]); vert[1] = (v&2 ? aamm[4] : aamm[1]); vert[2] = (v&4 ? aamm[5] : aamm[2]); // rotate to the body inertial frame if specified if (element->e->quat) { - mju_rotVecQuat(box, vert, element->e->quat); + mjuu_rotVecQuat(box, vert, element->e->quat); box[0] += element->e->pos[0] - ipos_[0]; box[1] += element->e->pos[1] - ipos_[1]; box[2] += element->e->pos[2] - ipos_[2]; - mju_rotVecQuat(vert, box, qinv); + mjuu_rotVecQuat(vert, box, qinv); } - AAMM[0] = mjMIN(AAMM[0], vert[0]); - AAMM[1] = mjMIN(AAMM[1], vert[1]); - AAMM[2] = mjMIN(AAMM[2], vert[2]); - AAMM[3] = mjMAX(AAMM[3], vert[0]); - AAMM[4] = mjMAX(AAMM[4], vert[1]); - AAMM[5] = mjMAX(AAMM[5], vert[2]); + AAMM[0] = std::min(AAMM[0], vert[0]); + AAMM[1] = std::min(AAMM[1], vert[1]); + AAMM[2] = std::min(AAMM[2], vert[2]); + AAMM[3] = std::max(AAMM[3], vert[0]); + AAMM[4] = std::max(AAMM[4], vert[1]); + AAMM[5] = std::max(AAMM[5], vert[2]); } } // inflate flat AABBs for (int i=0; i<3; i++) { - if (mju_abs(AAMM[i]-AAMM[i+3])= edges[0] + mjEPS) axis = 1; if (edges[2] >= edges[axis] + mjEPS) axis = 2; @@ -1340,7 +1333,7 @@ void mjCBody::GeomFrame(void) { } // check for small mass - if (mass0) { // fix minimum - mass = mju_max(mass, model->boundmass); - inertia[0] = mju_max(inertia[0], model->boundinertia); - inertia[1] = mju_max(inertia[1], model->boundinertia); - inertia[2] = mju_max(inertia[2], model->boundinertia); + mass = std::max(mass, model->boundmass); + inertia[0] = std::max(inertia[0], model->boundinertia); + inertia[1] = std::max(inertia[1], model->boundinertia); + inertia[2] = std::max(inertia[2], model->boundinertia); // check for negative values if (mass<0 || inertia[0]<0 || inertia[1]<0 ||inertia[2]<0) { @@ -1509,7 +1502,7 @@ void mjCBody::Compile(void) { for (int i=0; icontype; conaffinity |= geoms[i]->conaffinity; - margin = mju_max(margin, geoms[i]->margin); + margin = std::max(margin, geoms[i]->margin); } // compute bounding volume hierarchy @@ -1546,9 +1539,9 @@ void mjCBody::Compile(void) { // compute body global pose (no joint transformations in qpos0) if (id>0) { mjCBody* par = model->Bodies()[parentid]; - mju_rotVecQuat(xpos0, pos, par->xquat0); - mju_addTo3(xpos0, par->xpos0); - mju_mulQuat(xquat0, par->xquat0, quat); + mjuu_rotVecQuat(xpos0, pos, par->xquat0); + mjuu_addtovec(xpos0, par->xpos0, 3); + mjuu_mulquat(xquat0, par->xquat0, quat); } // compile all sites @@ -1680,8 +1673,8 @@ void mjCFrame::PointToLocal() { void mjCFrame::CopyFromSpec() { *static_cast(this) = spec; - mju_copy3(pos, spec.pos); - mju_copy4(quat, spec.quat); + mjuu_copyvec(pos, spec.pos, 3); + mjuu_copyvec(quat, spec.quat, 4); } @@ -2147,7 +2140,7 @@ double mjCGeom::GetRBound(void) { case mjGEOM_HFIELD: hsize = hfield->size; return sqrt(hsize[0]*hsize[0] + hsize[1]*hsize[1] + - mjMAX(hsize[2]*hsize[2], hsize[3]*hsize[3])); + std::max(hsize[2]*hsize[2], hsize[3]*hsize[3])); case mjGEOM_SPHERE: return size[0]; @@ -2159,7 +2152,7 @@ double mjCGeom::GetRBound(void) { return sqrt(size[0]*size[0]+size[1]*size[1]); case mjGEOM_ELLIPSOID: - return mju_max(mju_max(size[0], size[1]), size[2]); + return std::max(std::max(size[0], size[1]), size[2]); case mjGEOM_BOX: return sqrt(size[0]*size[0]+size[1]*size[1]+size[2]*size[2]); @@ -2167,9 +2160,9 @@ double mjCGeom::GetRBound(void) { case mjGEOM_MESH: case mjGEOM_SDF: aamm = mesh->aamm(); - haabb[0] = mju_max(fabs(aamm[0]), fabs(aamm[3])); - haabb[1] = mju_max(fabs(aamm[1]), fabs(aamm[4])); - haabb[2] = mju_max(fabs(aamm[2]), fabs(aamm[5])); + haabb[0] = std::max(std::abs(aamm[0]), std::abs(aamm[3])); + haabb[1] = std::max(std::abs(aamm[1]), std::abs(aamm[4])); + haabb[2] = std::max(std::abs(aamm[2]), std::abs(aamm[5])); return sqrt(haabb[0]*haabb[0] + haabb[1]*haabb[1] + haabb[2]*haabb[2]); default: @@ -2270,18 +2263,21 @@ void mjCGeom::SetFluidCoefs(void) { // coefficients of virtual moment of inertia. Note: if (kz-ky) in numerator // is negative, also the denom is negative. Abs both and clip to MINVAL const auto pow2 = [](const double val) { return val * val; }; - const double Ixfac = pow2(dy*dy - dz*dz) * std::fabs(kz - ky) / std::max( - mjMINVAL, std::fabs(2*(dy*dy - dz*dz) + (dy*dy + dz*dz)*(ky - kz))); - const double Iyfac = pow2(dz*dz - dx*dx) * std::fabs(kx - kz) / std::max( - mjMINVAL, std::fabs(2*(dz*dz - dx*dx) + (dz*dz + dx*dx)*(kz - kx))); - const double Izfac = pow2(dx*dx - dy*dy) * std::fabs(ky - kx) / std::max( - mjMINVAL, std::fabs(2*(dx*dx - dy*dy) + (dx*dx + dy*dy)*(kx - ky))); + const double Ixfac = pow2(dy*dy - dz*dz) * std::abs(kz - ky) / std::max( + mjEPS, std::abs(2*(dy*dy - dz*dz) + (dy*dy + dz*dz)*(ky - kz))); + const double Iyfac = pow2(dz*dz - dx*dx) * std::abs(kx - kz) / std::max( + mjEPS, std::abs(2*(dz*dz - dx*dx) + (dz*dz + dx*dx)*(kz - kx))); + const double Izfac = pow2(dx*dx - dy*dy) * std::abs(ky - kx) / std::max( + mjEPS, std::abs(2*(dx*dx - dy*dy) + (dx*dx + dy*dy)*(kx - ky))); - const mjtNum virtual_mass[3] = { - volume * kx / std::max(mjMINVAL, 2-kx), - volume * ky / std::max(mjMINVAL, 2-ky), - volume * kz / std::max(mjMINVAL, 2-kz)}; - const mjtNum virtual_inertia[3] = {volume*Ixfac/5, volume*Iyfac/5, volume*Izfac/5}; + mjtNum virtual_mass[3]; + virtual_mass[0] = volume * kx / std::max(mjEPS, 2-kx); + virtual_mass[1] = volume * ky / std::max(mjEPS, 2-ky); + virtual_mass[2] = volume * kz / std::max(mjEPS, 2-kz); + mjtNum virtual_inertia[3]; + virtual_inertia[0] = volume*Ixfac/5; + virtual_inertia[1] = volume*Iyfac/5; + virtual_inertia[2] = volume*Izfac/5; writeFluidGeomInteraction(fluid, &fluid_ellipsoid, &fluid_coefs[0], &fluid_coefs[1], &fluid_coefs[2], @@ -2477,9 +2473,9 @@ void mjCGeom::Compile(void) { size[2] = 0.25 * hfield->size[2] + 0.5 * hfield->size[3]; } else if (type==mjGEOM_MESH || type==mjGEOM_SDF) { const double* aamm = mesh->aamm(); - size[0] = mju_max(fabs(aamm[0]), fabs(aamm[3])); - size[1] = mju_max(fabs(aamm[1]), fabs(aamm[4])); - size[2] = mju_max(fabs(aamm[2]), fabs(aamm[5])); + size[0] = std::max(std::abs(aamm[0]), std::abs(aamm[3])); + size[1] = std::max(std::abs(aamm[1]), std::abs(aamm[4])); + size[2] = std::max(std::abs(aamm[2]), std::abs(aamm[5])); } for (double s : size) { @@ -2497,7 +2493,7 @@ void mjCGeom::Compile(void) { if (mass==0) { mass_ = 0; density = 0; - } else if (GetVolume()>mjMINVAL) { + } else if (GetVolume()>mjEPS) { mass_ = mass; density = mass / GetVolume(); SetInertia(); @@ -2847,7 +2843,7 @@ void mjCCamera::Compile(void) { intrinsic[3] = principal_pixel[1] / pixel_density[1] + principal_length[1]; // fovy with principal point at (0, 0) - fovy = mju_atan2((float)sensor_size[1]/2, intrinsic[1]) * 360.0 / mjPI; + fovy = std::atan2(sensor_size[1]/2, intrinsic[1]) * 360.0 / mjPI; } else { intrinsic[0] = model->visual.map.znear; intrinsic[1] = model->visual.map.znear; @@ -2942,7 +2938,7 @@ void mjCLight::Compile(void) { } // normalize direction, make sure it is not zero - if (mjuu_normvec(dir, 3)emax) { throw mjCError(this, "invalid data range in hfield '%s'", file_.c_str()); } for (int i=0; imjMINVAL) { + if (emax-emin>mjEPS) { data[i] /= (emax - emin); } } @@ -4122,12 +4118,12 @@ void mjCPair::Compile(void) { // set undefined margin: max if (!mjuu_defined(margin)) { - margin = mjMAX(geom1->margin, geom2->margin); + margin = std::max(geom1->margin, geom2->margin); } // set undefined gap: max if (!mjuu_defined(gap)) { - gap = mjMAX(geom1->gap, geom2->gap); + gap = std::max(geom1->gap, geom2->gap); } // set undefined condim, friction, solref, solimp: different priority @@ -4165,23 +4161,23 @@ void mjCPair::Compile(void) { else { // condim: max if (condim<0) { - condim = mjMAX(geom1->condim, geom2->condim); + condim = std::max(geom1->condim, geom2->condim); } // friction: max if (!mjuu_defined(friction[0])) { - friction[0] = friction[1] = mju_max(geom1->friction[0], geom2->friction[0]); - friction[2] = mju_max(geom1->friction[1], geom2->friction[1]); - friction[3] = friction[4] = mju_max(geom1->friction[2], geom2->friction[2]); + friction[0] = friction[1] = std::max(geom1->friction[0], geom2->friction[0]); + friction[2] = std::max(geom1->friction[1], geom2->friction[1]); + friction[3] = friction[4] = std::max(geom1->friction[2], geom2->friction[2]); } // solver mix factor double mix; - if (geom1->solmix>=mjMINVAL && geom2->solmix>=mjMINVAL) { + if (geom1->solmix>=mjEPS && geom2->solmix>=mjEPS) { mix = geom1->solmix / (geom1->solmix + geom2->solmix); - } else if (geom1->solmixsolmixsolmixsolmixsolmixsolmixsolref[i], geom2->solref[i]); + solref[i] = std::min(geom1->solref[i], geom2->solref[i]); } } } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 2135421d..6b3cdf4e 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -98,9 +98,9 @@ class mjCBoundingVolume { int contype; // contact type int conaffinity; // contact affinity - const mjtNum* aabb; // axis-aligned bounding box (center, size) - const mjtNum* pos; // position (set by user or Compile1) - const mjtNum* quat; // orientation (set by user or Compile1) + const double* aabb; // axis-aligned bounding box (center, size) + const double* pos; // position (set by user or Compile1) + const double* quat; // orientation (set by user or Compile1) const int* GetId() const { if (id_) return id_; else return &idval_; } void SetId(const int* id) { id_ = id; } @@ -134,7 +134,7 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ { // make bounding volume hierarchy void CreateBVH(void); - void Set(mjtNum ipos_element[3], mjtNum iquat_element[4]); + void Set(double ipos_element[3], double iquat_element[4]); void AllocateBoundingVolumes(int nleaf); void RemoveInactiveVolumes(int nmax); mjCBoundingVolume* GetBoundingVolume(int id); @@ -144,7 +144,7 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ { struct BVElement { const mjCBoundingVolume* e; // position of the element in the BVH axes - mjtNum lpos[3]; + double lpos[3]; }; struct BVElementCompare { @@ -231,8 +231,8 @@ class mjCBody_ : public mjCBase { int contype; // OR over geom contypes int conaffinity; // OR over geom conaffinities double margin; // MAX over geom margins - mjtNum xpos0[3]; // global position in qpos0 - mjtNum xquat0[4]; // global orientation in qpos0 + double xpos0[3]; // global position in qpos0 + double xquat0[4]; // global orientation in qpos0 // used internally by compiler int lastdof; // id of last dof @@ -664,20 +664,20 @@ class mjCFlex_ : public mjCBase { std::vector shell; // shell fragment vertex ids (dim per fragment) std::vector elemlayer; // element layer (distance from border) std::vector evpair; // element-vertex pairs - std::vector vertxpos; // global vertex positions + std::vector vertxpos; // global vertex positions mjCBoundingVolumeHierarchy tree; // bounding volume hierarchy - std::vector elemaabb_; // element bounding volume + std::vector elemaabb_; // element bounding volume // variable-size data std::vector vertbody_; // vertex body names - std::vector vert_; // vertex positions + std::vector vert_; // vertex positions std::vector elem_; // element vertex ids std::vector texcoord_; // vertex texture coordinates std::string material_; // name of material used for rendering std::string spec_material_; std::vector spec_vertbody_; - std::vector spec_vert_; + std::vector spec_vert_; std::vector spec_elem_; std::vector spec_texcoord_; }; diff --git a/src/user/user_util.cc b/src/user/user_util.cc index a2a91ee9..13dc5610 100644 --- a/src/user/user_util.cc +++ b/src/user/user_util.cc @@ -25,8 +25,6 @@ #include #include #include "engine/engine_crossplatform.h" -#include "engine/engine_util_misc.h" -#include "engine/engine_util_spatial.h" using std::isnan; using std::string; @@ -90,22 +88,13 @@ void mjuu_setvec(double* dest, const double x, const double y) { } -// copy double array -void mjuu_copyvec(double* dest, const double* src, const int n) { +// add to double array +void mjuu_addtovec(double* dest, const double* src, const int n) { for (int i=0; i mjEPS) { + for (int i=0; i mjEPS) { + for (int i=0; i std::abs(D[2]) && std::abs(D[1]) > std::abs(D[5])) { + rk = 0; // row + ck = 1; // column + rotk = 2; // rotation axis + } else if (std::abs(D[2]) > std::abs(D[5])) { + rk = 0; + ck = 2; + rotk = 1; + } else { + rk = 1; + ck = 2; + rotk = 0; + } + + // terminate if max off-diagonal element too small + if (std::abs(D[3*rk+ck]) < kEigEPS) { + break; + } + + // 2x2 symmetric Schur decomposition + tau = (D[4*ck]-D[4*rk])/(2*D[3*rk+ck]); + if (tau >= 0) { + t = 1.0/(tau + sqrt(1 + tau*tau)); + } else { + t = -1.0/(-tau + sqrt(1 + tau*tau)); + } + c = 1.0/sqrt(1 + t*t); + + // terminate if cosine too close to 1 + if (c > 1.0-kEigEPS) { + break; + } + + // express rotation as quaternion + tmp[1] = tmp[2] = tmp[3] = 0; + tmp[rotk+1] = (tau >= 0 ? -sqrt(0.5-0.5*c) : sqrt(0.5-0.5*c)); + if (rotk == 1) { + tmp[rotk+1] = -tmp[rotk+1]; + } + tmp[0] = sqrt(1.0 - tmp[rotk+1]*tmp[rotk+1]); + mjuu_normvec(tmp, 4); + + // accumulate quaternion rotation + mjuu_mulquat(quat, quat, tmp); + mjuu_normvec(quat, 4); + } + + // sort eigenvalues in decreasing order (bubblesort: 0, 1, 0) + for (int j=0; j < 3; j++) { + int j1 = j%2; // lead index + + // only swap if the eigenvalues are different + if (eigval[j1]+kEigEPS < eigval[j1+1]) { + // swap eigenvalues + t = eigval[j1]; + eigval[j1] = eigval[j1+1]; + eigval[j1+1] = t; + + // rotate quaternion + tmp[0] = 0.707106781186548; // cos(pi/4) = sin(pi/4) + tmp[1] = tmp[2] = tmp[3] = 0; + tmp[(j1+2)%3+1] = tmp[0]; + mjuu_mulquat(quat, quat, tmp); + mjuu_normvec(quat, 4); + } + } + + // recompute eigvec + mjuu_quat2mat(eigvec, quat); + + return iter; +} + +// transform vector by pose +void mjuu_trnVecPose(double res[3], const double pos[3], const double quat[4], + const double vec[3]) { + // res = quat*vec + pos + mjuu_rotVecQuat(res, vec, quat); + res[0] += pos[0]; + res[1] += pos[1]; + res[2] += pos[2]; +} + // strip directory from filename string mjuu_strippath(string filename) { // find last pathsymbol diff --git a/src/user/user_util.h b/src/user/user_util.h index 29ff5dd8..62527fcc 100644 --- a/src/user/user_util.h +++ b/src/user/user_util.h @@ -41,15 +41,23 @@ void mjuu_setvec(float* dest, const double x, const double y, const double z); // set 2D vector void mjuu_setvec(double* dest, const double x, const double y); -// copy double array -void mjuu_copyvec(double* dest, const double* src, const int n); +// copy real-valued vector +template +void mjuu_copyvec(T1* dest, const T2* src, int n) { + for (int i=0; i(text); + fcomp.point = String2Vector(text); } if (ReadAttrTxt(elem, "element", text)){ fcomp.element = String2Vector(text); diff --git a/test/fixture.cc b/test/fixture.cc index c3c1df28..6a141ee3 100644 --- a/test/fixture.cc +++ b/test/fixture.cc @@ -214,18 +214,21 @@ mjtNum CompareModel(const mjModel* m1, const mjModel* m2, #undef X if (maxdif > 0) return maxdif; - // compare arrays -#define X(type, name, nr, nc) \ - for (int r = 0; r < m1->nr; r++) { \ - for (int c = 0; c < nc; c++) { \ - dif = Compare(m1->name[r * nc + c], m2->name[r * nc + c]); \ - if (dif > maxdif) { \ - maxdif = dif; \ - field = #name; \ - field += " row: " + std::to_string(r); \ - field += " col: " + std::to_string(c); \ - } \ - } \ + // compare arrays, apart from bvh-related ones, as those are sensitive to + // numerical differences when meshes are perfectly symmetric. +#define X(type, name, nr, nc) \ + if (strncmp(#name, "bvh_", 4)) { \ + for (int r = 0; r < m1->nr; r++) { \ + for (int c = 0; c < nc; c++) { \ + dif = Compare(m1->name[r * nc + c], m2->name[r * nc + c]); \ + if (dif > maxdif) { \ + maxdif = dif; \ + field = #name; \ + field += " row: " + std::to_string(r); \ + field += " col: " + std::to_string(c); \ + } \ + } \ + } \ } // NOLINT MJMODEL_POINTERS #undef X diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index cf62423d..11fe0e9b 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -127,7 +128,7 @@ TEST_F(MjCMeshTest, LoadMSHWithVFS) { // should fallback to OS filesystem mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + EXPECT_THAT(error, HasSubstr("resource not found via provider or OS")); } TEST_F(MjCMeshTest, LoadOBJWithVFS) { @@ -152,7 +153,7 @@ TEST_F(MjCMeshTest, LoadOBJWithVFS) { // should fallback to OS filesystem mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + EXPECT_THAT(error, HasSubstr("resource not found via provider or OS")); } TEST_F(MjCMeshTest, LoadSTLWithVFS) { @@ -177,7 +178,7 @@ TEST_F(MjCMeshTest, LoadSTLWithVFS) { // should fallback to OS filesystem mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + EXPECT_THAT(error, HasSubstr("resource not found via provider or OS")); } // ------------- test content_type attributes ---------------------------------- @@ -204,7 +205,7 @@ TEST_F(MjCMeshTest, LoadMSHWithContentType) { // should try opening the file (not found obviously) mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + EXPECT_THAT(error, HasSubstr("resource not found via provider or OS")); } TEST_F(MjCMeshTest, LoadOBJWithContentType) { @@ -229,7 +230,7 @@ TEST_F(MjCMeshTest, LoadOBJWithContentType) { // should try opening the file (not found obviously) mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + EXPECT_THAT(error, HasSubstr("resource not found via provider or OS")); } TEST_F(MjCMeshTest, LoadSTLWithContentType) { @@ -254,7 +255,7 @@ TEST_F(MjCMeshTest, LoadSTLWithContentType) { // should try opening the file (not found obviously) mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + EXPECT_THAT(error, HasSubstr("resource not found via provider or OS")); } TEST_F(MjCMeshTest, LoadMSHWithContentTypeError) { @@ -304,7 +305,7 @@ TEST_F(MjCMeshTest, LoadMSHWithContentTypeParam) { // should try opening the file (not found obviously) mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + EXPECT_THAT(error, HasSubstr("resource not found via provider or OS")); } // ------------- test vertex de-duplication (STL) ------------------------------ @@ -420,7 +421,8 @@ TEST_F(MjCMeshTest, MaxHullVert) { } TEST_F(MjCMeshTest, MaxHullVertDefault) { - const std::string xml_path = GetTestDataFilePath(kTorusDefaultMaxhullVertPath); + const std::string xml_path = + GetTestDataFilePath(kTorusDefaultMaxhullVertPath); std::array error; mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); ASSERT_GT(model->ngeom, 0); @@ -465,8 +467,9 @@ TEST_F(MjCMeshTest, SmallInertiaLoads) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); mj_deleteModel(model); } diff --git a/test/user/user_model_test.cc b/test/user/user_model_test.cc index 22b46ec0..d067d81a 100644 --- a/test/user/user_model_test.cc +++ b/test/user/user_model_test.cc @@ -283,7 +283,7 @@ TEST_F(FuseStaticTest, FuseStaticEquivalent) { mj_step(m_fuse, d_fuse); mj_step(m_no_fuse, d_no_fuse); - EXPECT_THAT(d_fuse->qvel[0], DoubleNear(d_no_fuse->qvel[0], 1e-17)) + EXPECT_THAT(d_fuse->qvel[0], DoubleNear(d_no_fuse->qvel[0], 2e-17)) << "Velocity should be the same after 1 step"; EXPECT_NE(d_fuse->qvel[0], 0); diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index bcb8c68b..09176bd5 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -1302,9 +1302,13 @@ TEST_F(XMLWriterTest, WriteReadCompare) { ASSERT_THAT(error.data(), HasSubstr("file")) << error.data() << " from " << xml.c_str(); } else { - // for a particularly difficult example, relax the tolerance - mjtNum tol = - absl::StrContains(p.path().string(), "belt.xml") ? 1e-13 : 0; + mjtNum tol = 0; + + // for particularly sensitive models, relax the tolerance + if (absl::StrContains(p.path().string(), "belt.xml") || + absl::StrContains(p.path().string(), "cable.xml")) { + tol = 1e-13; + } // compare and delete std::string field = "";