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

Modify mjCMesh vert_ field to be vector<float> instead of vector<double>.

Browse Source 
PiperOrigin-RevId: 882162404
Change-Id: Ia97439420f284d594f5cb1a91cd0fb640ac8dff0
This commit is contained in:
Sam Haves
2026-03-11 13:14:35 -07:00
committed by Copybara-Service
parent de3b9d3426
commit 3601b063dc
4 changed files with 131 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_flexcomp.cc
+1 -1
View File
@@ -1185,7 +1185,7 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, mjsCompiler* compiler, char* error,
} }
// copy vertices // copy vertices
point = mesh.Vert(); point.assign(mesh.Vert().begin(), mesh.Vert().end());
if (mesh.HasTexcoord()) { if (mesh.HasTexcoord()) {
texcoord = mesh.Texcoord(); texcoord = mesh.Texcoord();
src/user/user_mesh.cc
+108 -102
View File
@@ -492,7 +492,7 @@ void mjCMesh::CacheMesh(mjCCache* cache, const mjResource* resource) {
// calculate estimated size of mesh // calculate estimated size of mesh
std::size_t size = sizeof(mjCMesh) std::size_t size = sizeof(mjCMesh)
+ (sizeof(double) * vert_.size()) + (sizeof(float) * vert_.size())
+ (sizeof(float) * normal_.size()) + (sizeof(float) * normal_.size())
+ (sizeof(float) * texcoord_.size()) + (sizeof(float) * texcoord_.size())
+ (sizeof(int) * face_.size()) + (sizeof(int) * face_.size())
@@ -538,7 +538,7 @@ struct VertexKey {
// convert vertices to double precision and remove repeated vertices if requested // process and remove repeated vertices if requested
void mjCMesh::ProcessVertices(const std::vector<float>& vert, bool remove_repeated) { void mjCMesh::ProcessVertices(const std::vector<float>& vert, bool remove_repeated) {
vert_.clear(); vert_.clear();
int nvert = vert.size(); int nvert = vert.size();
@@ -775,24 +775,24 @@ void mjCMesh::TryCompile(const mjVFS* vfs) {
// compute mesh properties // compute mesh properties
if (!fromCache) { if (!fromCache) {
Process(); Process();
}
// make octree if (!file_.empty()) {
if (!needsdf) { CacheMesh(cache, resource_);
octree_.Clear(); // this occurs when a non-SDF mesh is loaded from a cached SDF mesh }
} else if (octree_.NumNodes() == 0) { } else {
octree_.SetFace(vert_, face_); // When a mesh is loaded from the cache, has no octree but needs one,
octree_.CreateOctree(aamm_); // we need to compute it here. If inversely it has an octree but we *do not*
// need one, we clear it.
// compute sdf coefficients if (!needsdf) {
if (!plugin.active) { octree_.Clear();
octree_.ComputeSdfCoeffs(vert_.data(), nvert(), face_.data(), nface(), tree_); } else if (octree_.NumNodes() == 0) {
std::vector<double> dvert(vert_.begin(), vert_.end());
octree_.SetFace(dvert, face_);
octree_.CreateOctree(aamm_);
if (!plugin.active) {
octree_.ComputeSdfCoeffs(dvert.data(), nvert(), face_.data(), nface(), tree_);
}
} }
}
// cache mesh
if (!fromCache && !file_.empty()) {
CacheMesh(cache, resource_);
} }
// close resource // close resource
@@ -805,18 +805,18 @@ void mjCMesh::TryCompile(const mjVFS* vfs) {
// get bounding volume // get bounding volume
void mjCMesh::SetBoundingVolume(int faceid) { void mjCMesh::SetBoundingVolume(int faceid, const double* dvert) {
constexpr double kMaxVal = std::numeric_limits<double>::max(); constexpr double kMaxVal = std::numeric_limits<double>::max();
double face_aamm[6] = {kMaxVal, kMaxVal, kMaxVal, -kMaxVal, -kMaxVal, -kMaxVal}; double face_aamm[6] = {kMaxVal, kMaxVal, kMaxVal, -kMaxVal, -kMaxVal, -kMaxVal};
for (int j = 0; j < 3; j++) { for (int j = 0; j < 3; j++) {
int vertid = face_[3*faceid + j]; int vertid = face_[3*faceid + j];
face_aamm[0] = std::min(face_aamm[0], vert_[3*vertid + 0]); face_aamm[0] = std::min(face_aamm[0], dvert[3*vertid + 0]);
face_aamm[1] = std::min(face_aamm[1], vert_[3*vertid + 1]); face_aamm[1] = std::min(face_aamm[1], dvert[3*vertid + 1]);
face_aamm[2] = std::min(face_aamm[2], vert_[3*vertid + 2]); face_aamm[2] = std::min(face_aamm[2], dvert[3*vertid + 2]);
face_aamm[3] = std::max(face_aamm[3], vert_[3*vertid + 0]); face_aamm[3] = std::max(face_aamm[3], dvert[3*vertid + 0]);
face_aamm[4] = std::max(face_aamm[4], vert_[3*vertid + 1]); face_aamm[4] = std::max(face_aamm[4], dvert[3*vertid + 1]);
face_aamm[5] = std::max(face_aamm[5], vert_[3*vertid + 2]); face_aamm[5] = std::max(face_aamm[5], dvert[3*vertid + 2]);
} }
face_aabb_.push_back(.5 * (face_aamm[0] + face_aamm[3])); face_aabb_.push_back(.5 * (face_aamm[0] + face_aamm[3]));
@@ -851,9 +851,7 @@ bool mjCMesh::HasTexcoord() const {
void mjCMesh::CopyVert(float* arr) const { void mjCMesh::CopyVert(float* arr) const {
for (int i = 0; i < vert_.size(); ++i) { std::copy(vert_.begin(), vert_.end(), arr);
arr[i] = (float)vert_[i];
}
} }
@@ -1187,16 +1185,15 @@ void mjCMesh::LoadMSH(mjResource* resource, bool remove_repeated) {
// compute the volume and center-of-mass of the mesh given the face centroid // compute the volume and center-of-mass of the mesh given the face centroid
double mjCMesh::ComputeVolume(double CoM[3], const double facecen[3]) const { double mjCMesh::ComputeVolume(double CoM[3], const double facecen[3], const double* dvert) const {
double normal[3], center[3], total_volume = 0; double normal[3], center[3], total_volume = 0;
CoM[0] = CoM[1] = CoM[2] = 0; CoM[0] = CoM[1] = CoM[2] = 0;
int nf = (inertia == mjMESH_INERTIA_CONVEX) ? graph_[1] : nface(); int nf = (inertia == mjMESH_INERTIA_CONVEX) ? graph_[1] : nface();
const int* f = (inertia == mjMESH_INERTIA_CONVEX) ? GraphFaces() : face_.data(); const int* f = (inertia == mjMESH_INERTIA_CONVEX) ? GraphFaces() : face_.data();
for (int i = 0; i < nf; i++) { for (int i = 0; i < nf; i++) {
// get area, normal and center double area = triangle(normal, center, &dvert[3*f[3*i]], &dvert[3*f[3*i + 1]],
double area = triangle(normal, center, &vert_[3*f[3*i]], &vert_[3*f[3*i + 1]], &dvert[3*f[3*i + 2]]);
&vert_[3*f[3*i + 2]]);
// compute and add volume // compute and add volume
double vec[3] = {center[0] - facecen[0], center[1] - facecen[1], center[2] - facecen[2]}; double vec[3] = {center[0] - facecen[0], center[1] - facecen[1], center[2] - facecen[2]};
@@ -1226,14 +1223,13 @@ double mjCMesh::ComputeVolume(double CoM[3], const double facecen[3]) const {
// compute the surface area and center-of-mass of the mesh given the face centroid // compute the surface area and center-of-mass of the mesh given the face centroid
double mjCMesh::ComputeSurfaceArea(double CoM[3], const double facecen[3]) const { double mjCMesh::ComputeSurfaceArea(double CoM[3], const double facecen[3], const double* dvert) const {
double surface = 0; double surface = 0;
CoM[0] = CoM[1] = CoM[2] = 0; CoM[0] = CoM[1] = CoM[2] = 0;
for (int i = 0; i < nface(); i++) { for (int i = 0; i < nface(); i++) {
// get area and center
double area, center[3]; double area, center[3];
area = triangle(nullptr, center, &vert_[3*face_[3*i]], area = triangle(nullptr, center, &dvert[3*face_[3*i]],
&vert_[3*face_[3*i + 1]], &vert_[3*face_[3*i + 2]]); &dvert[3*face_[3*i + 1]], &dvert[3*face_[3*i + 2]]);
// add pyramid com // add pyramid com
surface += area; surface += area;
@@ -1254,28 +1250,24 @@ double mjCMesh::ComputeSurfaceArea(double CoM[3], const double facecen[3]) const
// apply transformations // apply transformations
void mjCMesh::ApplyTransformations() { void mjCMesh::ApplyTransformations(double* dvert) {
// translate
if (refpos[0] != 0 || refpos[1] != 0 || refpos[2] != 0) { if (refpos[0] != 0 || refpos[1] != 0 || refpos[2] != 0) {
int nv = nvert(); int nv = nvert();
for (int i = 0; i < nv; i++) { for (int i = 0; i < nv; i++) {
vert_[3*i + 0] -= refpos[0]; dvert[3*i + 0] -= refpos[0];
vert_[3*i + 1] -= refpos[1]; dvert[3*i + 1] -= refpos[1];
vert_[3*i + 2] -= refpos[2]; dvert[3*i + 2] -= refpos[2];
} }
} }
// rotate
if (refquat[0] != 1 || refquat[1] != 0 || refquat[2] != 0 || refquat[3] != 0) { if (refquat[0] != 1 || refquat[1] != 0 || refquat[2] != 0 || refquat[3] != 0) {
// prepare rotation
double quat[4] = {refquat[0], refquat[1], refquat[2], refquat[3]}; double quat[4] = {refquat[0], refquat[1], refquat[2], refquat[3]};
double mat[9]; double mat[9];
mjuu_normvec(quat, 4); mjuu_normvec(quat, 4);
mjuu_quat2mat(mat, quat); mjuu_quat2mat(mat, quat);
// process vertices
for (int i = 0; i < nvert(); i++) { for (int i = 0; i < nvert(); i++) {
mjuu_mulvecmatT(&vert_[3*i], &vert_[3*i], mat); mjuu_mulvecmatT(&dvert[3*i], &dvert[3*i], mat);
} }
// process normals // process normals
@@ -1291,9 +1283,9 @@ void mjCMesh::ApplyTransformations() {
// scale // scale
if (scale[0] != 1 || scale[1] != 1 || scale[2] != 1) { if (scale[0] != 1 || scale[1] != 1 || scale[2] != 1) {
for (int i = 0; i < nvert(); i++) { for (int i = 0; i < nvert(); i++) {
vert_[3*i + 0] *= scale[0]; dvert[3*i + 0] *= scale[0];
vert_[3*i + 1] *= scale[1]; dvert[3*i + 1] *= scale[1];
vert_[3*i + 2] *= scale[2]; dvert[3*i + 2] *= scale[2];
} }
for (int i = 0; i < nnormal(); i++) { for (int i = 0; i < nnormal(); i++) {
@@ -1325,14 +1317,13 @@ void mjCMesh::ApplyTransformations() {
// find centroid of faces, return total area // find centroid of faces, return total area
double mjCMesh::ComputeFaceCentroid(double facecen[3]) const { double mjCMesh::ComputeFaceCentroid(double facecen[3], const double* dvert) const {
double total_area = 0; double total_area = 0;
for (int i = 0; i < nface(); i++) { for (int i = 0; i < nface(); i++) {
// get area and center
double area, center[3]; double area, center[3];
area = triangle(nullptr, center, &vert_[3*face_[3*i]], area = triangle(nullptr, center, &dvert[3*face_[3*i]],
&vert_[3*face_[3*i + 1]], &vert_[3*face_[3*i + 2]]); &dvert[3*face_[3*i + 1]], &dvert[3*face_[3*i + 2]]);
// accumulate // accumulate
facecen[0] += area * center[0]; facecen[0] += area * center[0];
@@ -1353,13 +1344,15 @@ double mjCMesh::ComputeFaceCentroid(double facecen[3]) const {
void mjCMesh::Process() { void mjCMesh::Process() {
std::vector<double> dvert(vert_.begin(), vert_.end());
// create half-edge structure (if mesh was in XML) // create half-edge structure (if mesh was in XML)
if (halfedge_.empty()) { if (halfedge_.empty()) {
for (int i = 0; i < nface(); i++) { for (int i = 0; i < nface(); i++) {
int v0 = face_[3*i + 0]; int v0 = face_[3*i + 0];
int v1 = face_[3*i + 1]; int v1 = face_[3*i + 1];
int v2 = face_[3*i + 2]; int v2 = face_[3*i + 2];
if (triangle(nullptr, nullptr, &vert_[3*v0], &vert_[3*v1], &vert_[3*v2]) > sqrt(mjMINVAL)) { if (triangle(nullptr, nullptr, &dvert[3*v0], &dvert[3*v1], &dvert[3*v2]) > sqrt(mjMINVAL)) {
halfedge_.push_back({v0, v1}); halfedge_.push_back({v0, v1});
halfedge_.push_back({v1, v2}); halfedge_.push_back({v1, v2});
halfedge_.push_back({v2, v0}); halfedge_.push_back({v2, v0});
@@ -1383,7 +1376,7 @@ void mjCMesh::Process() {
// make graph describing convex hull // make graph describing convex hull
if (needhull_ || face_.empty()) { if (needhull_ || face_.empty()) {
MakeGraph(); MakeGraph(dvert.data());
} }
// no faces: copy from convex hull // no faces: copy from convex hull
@@ -1393,7 +1386,7 @@ void mjCMesh::Process() {
// no normals: make // no normals: make
if (normal_.empty()) { if (normal_.empty()) {
MakeNormal(); MakeNormal(dvert.data());
} }
// check facenormal size // check facenormal size
@@ -1416,7 +1409,7 @@ void mjCMesh::Process() {
} }
if (szgraph_) { if (szgraph_) {
MakePolygons(); MakePolygons(dvert.data());
} else { } else {
polygon_map_.resize(nvert()); polygon_map_.resize(nvert());
} }
@@ -1436,11 +1429,11 @@ void mjCMesh::Process() {
} }
// user offset, rotation, scaling // user offset, rotation, scaling
ApplyTransformations(); ApplyTransformations(dvert.data());
// find centroid of faces // find centroid of faces
double facecen[3] = {0, 0, 0}; double facecen[3] = {0, 0, 0};
if (ComputeFaceCentroid(facecen) < mjMINVAL) { if (ComputeFaceCentroid(facecen, dvert.data()) < mjMINVAL) {
throw mjCError(this, "mesh surface area is too small: %s", name.c_str()); throw mjCError(this, "mesh surface area is too small: %s", name.c_str());
} }
@@ -1451,12 +1444,12 @@ void mjCMesh::Process() {
// compute CoM and volume/area // compute CoM and volume/area
if (inertia == mjMESH_INERTIA_SHELL) { if (inertia == mjMESH_INERTIA_SHELL) {
surface_ = ComputeSurfaceArea(CoM, facecen); surface_ = ComputeSurfaceArea(CoM, facecen, dvert.data());
if (surface_ < mjMINVAL) { if (surface_ < mjMINVAL) {
throw mjCError(this, "mesh surface area is too small: %s", name.c_str()); throw mjCError(this, "mesh surface area is too small: %s", name.c_str());
} }
} else { } else {
if ((volume_ = ComputeVolume(CoM, facecen)) < mjMINVAL) { if ((volume_ = ComputeVolume(CoM, facecen, dvert.data())) < mjMINVAL) {
if (volume_ < 0) { if (volume_ < 0) {
throw mjCError(this, "mesh volume is negative (misoriented triangles): %s", name.c_str()); throw mjCError(this, "mesh volume is negative (misoriented triangles): %s", name.c_str());
} else { } else {
@@ -1467,7 +1460,7 @@ void mjCMesh::Process() {
} }
// compute inertia // compute inertia
double total_volume = ComputeInertia(inert, CoM); double total_volume = ComputeInertia(inert, CoM, dvert.data());
if (inertia == mjMESH_INERTIA_SHELL) { if (inertia == mjMESH_INERTIA_SHELL) {
surface_ = total_volume; surface_ = total_volume;
} else { } else {
@@ -1511,11 +1504,11 @@ void mjCMesh::Process() {
// transform CoM to origin // transform CoM to origin
for (int i=0; i < nvert(); i++) { for (int i=0; i < nvert(); i++) {
vert_[3*i + 0] -= CoM[0]; dvert[3*i + 0] -= CoM[0];
vert_[3*i + 1] -= CoM[1]; dvert[3*i + 1] -= CoM[1];
vert_[3*i + 2] -= CoM[2]; dvert[3*i + 2] -= CoM[2];
} }
Rotate(quattmp); Rotate(quattmp, dvert.data());
// save the pos and quat that was used to transform the mesh // save the pos and quat that was used to transform the mesh
mjuu_copyvec(pos_, CoM, 3); mjuu_copyvec(pos_, CoM, 3);
@@ -1525,11 +1518,11 @@ void mjCMesh::Process() {
// no radii: make // no radii: make
if (!center_) { if (!center_) {
MakeCenter(); MakeCenter(dvert.data());
} }
// recompute polygon normals // recompute polygon normals
MakePolygonNormals(); MakePolygonNormals(dvert.data());
// make bounding volume hierarchy // make bounding volume hierarchy
if (tree_.Bvh().empty()) { if (tree_.Bvh().empty()) {
@@ -1537,16 +1530,31 @@ void mjCMesh::Process() {
face_aabb_.reserve(3*face_.size()); face_aabb_.reserve(3*face_.size());
tree_.AllocateBoundingVolumes(nface()); tree_.AllocateBoundingVolumes(nface());
for (int i = 0; i < nface(); i++) { for (int i = 0; i < nface(); i++) {
SetBoundingVolume(i); SetBoundingVolume(i, dvert.data());
} }
tree_.CreateBVH(); tree_.CreateBVH();
} }
// make octree
if (needsdf) {
octree_.SetFace(dvert, face_);
octree_.CreateOctree(aamm_);
if (!plugin.active) {
octree_.ComputeSdfCoeffs(dvert.data(), nvert(), face_.data(), nface(), tree_);
}
}
// narrow back to float
for (int i = 0; i < (int)dvert.size(); i++) {
vert_[i] = (float)dvert[i];
}
} }
// compute abstract (unitless) inertia, recompute area / volume // compute abstract (unitless) inertia, recompute area / volume
double mjCMesh::ComputeInertia(double inert[6], const double CoM[3]) const { double mjCMesh::ComputeInertia(double inert[6], const double CoM[3], const double* dvert) const {
double total_volume = 0; double total_volume = 0;
// copy vertices to avoid modifying the original mesh // copy vertices to avoid modifying the original mesh
@@ -1555,9 +1563,9 @@ double mjCMesh::ComputeInertia(double inert[6], const double CoM[3]) const {
// translate vertices to origin in order to compute inertia // translate vertices to origin in order to compute inertia
for (int i = 0; i < nvert(); i++) { for (int i = 0; i < nvert(); i++) {
vert_centered.push_back(vert_[3*i + 0] - CoM[0]); vert_centered.push_back(dvert[3*i + 0] - CoM[0]);
vert_centered.push_back(vert_[3*i + 1] - CoM[1]); vert_centered.push_back(dvert[3*i + 1] - CoM[1]);
vert_centered.push_back(vert_[3*i + 2] - CoM[2]); vert_centered.push_back(dvert[3*i + 2] - CoM[2]);
} }
// accumulate products of inertia, recompute volume // accumulate products of inertia, recompute volume
@@ -1612,22 +1620,20 @@ double mjCMesh::ComputeInertia(double inert[6], const double CoM[3]) const {
void mjCMesh::Rotate(double quat[4]) { void mjCMesh::Rotate(double quat[4], double* dvert) {
// rotate vertices and normals of mesh by quaternion
double neg[4] = {quat[0], -quat[1], -quat[2], -quat[3]}; double neg[4] = {quat[0], -quat[1], -quat[2], -quat[3]};
double mat[9]; double mat[9];
mjuu_quat2mat(mat, neg); mjuu_quat2mat(mat, neg);
int nv = nvert(); int nv = nvert();
for (int i = 0; i < nv; i++) { for (int i = 0; i < nv; i++) {
mjuu_mulvecmat(&vert_[3*i], &vert_[3*i], mat); mjuu_mulvecmat(&dvert[3*i], &dvert[3*i], mat);
// axis-aligned bounding box aamm_[0] = std::min(aamm_[0], dvert[3*i + 0]);
aamm_[0] = std::min(aamm_[0], vert_[3*i + 0]); aamm_[3] = std::max(aamm_[3], dvert[3*i + 0]);
aamm_[3] = std::max(aamm_[3], vert_[3*i + 0]); aamm_[1] = std::min(aamm_[1], dvert[3*i + 1]);
aamm_[1] = std::min(aamm_[1], vert_[3*i + 1]); aamm_[4] = std::max(aamm_[4], dvert[3*i + 1]);
aamm_[4] = std::max(aamm_[4], vert_[3*i + 1]); aamm_[2] = std::min(aamm_[2], dvert[3*i + 2]);
aamm_[2] = std::min(aamm_[2], vert_[3*i + 2]); aamm_[5] = std::max(aamm_[5], dvert[3*i + 2]);
aamm_[5] = std::max(aamm_[5], vert_[3*i + 2]);
} }
for (int i=0; i < nnormal(); i++) { for (int i=0; i < nnormal(); i++) {
@@ -1699,7 +1705,7 @@ double mjCMesh::GetVolumeRef() const {
// make graph describing convex hull // make graph describing convex hull
void mjCMesh::MakeGraph() { void mjCMesh::MakeGraph(const double* dvert) {
int adr, ok, curlong, totlong, exitcode; int adr, ok, curlong, totlong, exitcode;
facetT* facet, **facetp; facetT* facet, **facetp;
vertexT* vertex, *vertex1, **vertex1p; vertexT* vertex, *vertex1, **vertex1p;
@@ -1728,7 +1734,7 @@ void mjCMesh::MakeGraph() {
if (!exitcode) { if (!exitcode) {
// actual init // actual init
qh_initflags(qh, const_cast<char*>(qhopt.c_str())); qh_initflags(qh, const_cast<char*>(qhopt.c_str()));
qh_init_B(qh, vert_.data(), nvert(), 3, qh_False); qh_init_B(qh, const_cast<double*>(dvert), nvert(), 3, qh_False);
// construct convex hull // construct convex hull
qh_qhull(qh); qh_qhull(qh);
@@ -2403,7 +2409,7 @@ void mjCMesh::MakeCone(int nedge, double radius) {
// compute vertex normals // compute vertex normals
void mjCMesh::MakeNormal() { void mjCMesh::MakeNormal(const double* dvert) {
// only if normal data is missing // only if normal data is missing
if (!normal_.empty()) { if (!normal_.empty()) {
return; return;
@@ -2427,8 +2433,8 @@ void mjCMesh::MakeNormal() {
// get triangle edges // get triangle edges
double vec01[3], vec02[3]; double vec01[3], vec02[3];
for (int j=0; j < 3; j++) { for (int j=0; j < 3; j++) {
vec01[j] = vert_[3*vertid[1]+j] - vert_[3*vertid[0]+j]; vec01[j] = dvert[3*vertid[1]+j] - dvert[3*vertid[0]+j];
vec02[j] = vert_[3*vertid[2]+j] - vert_[3*vertid[0]+j]; vec02[j] = dvert[3*vertid[2]+j] - dvert[3*vertid[0]+j];
} }
// compute face normal // compute face normal
@@ -2462,8 +2468,8 @@ void mjCMesh::MakeNormal() {
// get triangle edges // get triangle edges
double vec01[3], vec02[3]; double vec01[3], vec02[3];
for (int j=0; j < 3; j++) { for (int j=0; j < 3; j++) {
vec01[j] = vert_[3*vertid[1]+j] - vert_[3*vertid[0]+j]; vec01[j] = dvert[3*vertid[1]+j] - dvert[3*vertid[0]+j];
vec02[j] = vert_[3*vertid[2]+j] - vert_[3*vertid[0]+j]; vec02[j] = dvert[3*vertid[2]+j] - dvert[3*vertid[0]+j];
} }
// compute face normal // compute face normal
@@ -2515,7 +2521,7 @@ void mjCMesh::MakeNormal() {
// compute face circumradii // compute face circumradii
void mjCMesh::MakeCenter() { void mjCMesh::MakeCenter(const double* dvert) {
if (center_) { if (center_) {
return; return;
} }
@@ -2531,8 +2537,8 @@ void mjCMesh::MakeCenter() {
// get triangle edges // get triangle edges
double a[3], b[3]; double a[3], b[3];
for (int j=0; j < 3; j++) { for (int j=0; j < 3; j++) {
a[j] = vert_[3*vertid[0]+j] - vert_[3*vertid[2]+j]; a[j] = dvert[3*vertid[0]+j] - dvert[3*vertid[2]+j];
b[j] = vert_[3*vertid[1]+j] - vert_[3*vertid[2]+j]; b[j] = dvert[3*vertid[1]+j] - dvert[3*vertid[2]+j];
} }
// compute face normal // compute face normal
@@ -2551,20 +2557,20 @@ void mjCMesh::MakeCenter() {
norm_a_2 * b[2] - norm_b_2 * a[2] norm_a_2 * b[2] - norm_b_2 * a[2]
}; };
mjuu_crossvec(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+0] = res[0]/(2*area*area) + dvert[3*vertid[2]+0];
center_[3*i+1] = res[1]/(2*area*area) + vert_[3*vertid[2]+1]; center_[3*i+1] = res[1]/(2*area*area) + dvert[3*vertid[2]+1];
center_[3*i+2] = res[2]/(2*area*area) + vert_[3*vertid[2]+2]; center_[3*i+2] = res[2]/(2*area*area) + dvert[3*vertid[2]+2];
} }
} }
// compute the normals of the polygons // compute the normals of the polygons
void mjCMesh::MakePolygonNormals() { void mjCMesh::MakePolygonNormals(const double* dvert) {
for (int i = 0; i < polygons_.size(); ++i) { for (int i = 0; i < polygons_.size(); ++i) {
double n[3]; double n[3];
mjuu_makenormal(n, &vert_[3*polygons_[i][0]], &vert_[3*polygons_[i][1]], mjuu_makenormal(n, &dvert[3*polygons_[i][0]], &dvert[3*polygons_[i][1]],
&vert_[3*polygons_[i][2]]); &dvert[3*polygons_[i][2]]);
polygon_normals_[3*i + 0] = n[0]; polygon_normals_[3*i + 0] = n[0];
polygon_normals_[3*i + 1] = n[1]; polygon_normals_[3*i + 1] = n[1];
polygon_normals_[3*i + 2] = n[2]; polygon_normals_[3*i + 2] = n[2];
@@ -2803,7 +2809,7 @@ struct PairHash {
// merge coplanar mesh triangular faces into polygonal sides to represent the geometry of the mesh // merge coplanar mesh triangular faces into polygonal sides to represent the geometry of the mesh
void mjCMesh::MakePolygons() { void mjCMesh::MakePolygons(const double* dvert) {
constexpr double kAngleTol = 0.01; constexpr double kAngleTol = 0.01;
std::unordered_map<std::pair<double, double>, MeshPolygon, PairHash> mesh_polygons; std::unordered_map<std::pair<double, double>, MeshPolygon, PairHash> mesh_polygons;
polygons_.clear(); polygons_.clear();
@@ -2820,9 +2826,9 @@ void mjCMesh::MakePolygons() {
int vi1 = faces[3*i + 0]; int vi1 = faces[3*i + 0];
int vi2 = faces[3*i + 1]; int vi2 = faces[3*i + 1];
int vi3 = faces[3*i + 2]; int vi3 = faces[3*i + 2];
double* v1 = &vert_[3*vi1]; const double* v1 = &dvert[3*vi1];
double* v2 = &vert_[3*vi2]; const double* v2 = &dvert[3*vi2];
double* v3 = &vert_[3*vi3]; const double* v3 = &dvert[3*vi3];
std::pair<double, double> key; std::pair<double, double> key;
if (!MeshPolygonKey(key, v1, v2, v3, kAngleTol)) { if (!MeshPolygonKey(key, v1, v2, v3, kAngleTol)) {
src/user/user_objects.cc
+3
View File
@@ -587,6 +587,7 @@ void mjCOctree::CopyCoeff(mjtNum* coeff) const {
void mjCOctree::SetFace(const std::vector<double>& vert, const std::vector<int>& face) { void mjCOctree::SetFace(const std::vector<double>& vert, const std::vector<int>& face) {
face_.reserve(face.size()/3);
for (int i = 0; i < face.size(); i += 3) { for (int i = 0; i < face.size(); i += 3) {
std::array<double, 3> v0 = {vert[3*face[i+0]], vert[3*face[i+0]+1], vert[3*face[i+0]+2]}; std::array<double, 3> v0 = {vert[3*face[i+0]], vert[3*face[i+0]+1], vert[3*face[i+0]+2]};
std::array<double, 3> v1 = {vert[3*face[i+1]], vert[3*face[i+1]+1], vert[3*face[i+1]+2]}; std::array<double, 3> v1 = {vert[3*face[i+1]], vert[3*face[i+1]+1], vert[3*face[i+1]+2]};
@@ -613,6 +614,8 @@ void mjCOctree::Make(std::vector<Triangle>& elements) {
void mjCOctree::CreateOctree(const double aamm[6]) { void mjCOctree::CreateOctree(const double aamm[6]) {
Clear();
double aabb[6] = {(aamm[0] + aamm[3]) / 2, (aamm[1] + aamm[4]) / 2, (aamm[2] + aamm[5]) / 2, double aabb[6] = {(aamm[0] + aamm[3]) / 2, (aamm[1] + aamm[4]) / 2, (aamm[2] + aamm[5]) / 2,
(aamm[3] - aamm[0]) / 2, (aamm[4] - aamm[1]) / 2, (aamm[5] - aamm[2]) / 2}; (aamm[3] - aamm[0]) / 2, (aamm[4] - aamm[1]) / 2, (aamm[5] - aamm[2]) / 2};
double box[6] = {aabb[0] - 1.1 * aabb[3], aabb[1] - 1.1 * aabb[4], aabb[2] - 1.1 * aabb[5], double box[6] = {aabb[0] - 1.1 * aabb[3], aabb[1] - 1.1 * aabb[4], aabb[2] - 1.1 * aabb[5],
src/user/user_objects.h
+19 -17
View File
@@ -299,8 +299,11 @@ class mjCOctree : public mjCOctree_ {
sizeof(Point) * vert_.size(); sizeof(Point) * vert_.size();
} }
void Clear() { void Clear() {
nnode_ = 0;
nvert_ = 0;
node_.clear(); node_.clear();
face_.clear(); vert_.clear();
hang_.clear();
} }
void AddCoeff(int n, int v, double coeff) { node_[n].coeff[v] = coeff; } void AddCoeff(int n, int v, double coeff) { node_[n].coeff[v] = coeff; }
double Coeff(int n, int v) const { return node_[n].coeff[v]; } double Coeff(int n, int v) const { return node_[n].coeff[v]; }
@@ -1071,7 +1074,7 @@ class mjCMesh_ : public mjCBase {
std::string content_type_ = ""; // content type of file std::string content_type_ = ""; // content type of file
std::string file_; // mesh file std::string file_; // mesh file
mjResource* resource_ = nullptr; // resource for mesh file mjResource* resource_ = nullptr; // resource for mesh file
std::vector<double> vert_; // vertex data std::vector<float> vert_; // vertex data
std::vector<float> normal_; // normal data std::vector<float> normal_; // normal data
std::vector<float> texcoord_; // texcoord data std::vector<float> texcoord_; // texcoord data
std::vector<int> face_; // vertex indices std::vector<int> face_; // vertex indices
@@ -1156,8 +1159,8 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
const double* Refquat() const { return refquat; } const double* Refquat() const { return refquat; }
const double* Scale() const { return scale; } const double* Scale() const { return scale; }
bool SmoothNormal() const { return smoothnormal; } bool SmoothNormal() const { return smoothnormal; }
const std::vector<double>& Vert() const { return vert_; } const std::vector<float>& Vert() const { return vert_; }
double Vert(int i) const { return vert_[i]; } float Vert(int i) const { return vert_[i]; }
const std::vector<float>& UserVert() const { return spec_vert_; } const std::vector<float>& UserVert() const { return spec_vert_; }
const std::vector<float>& UserNormal() const { return spec_normal_; } const std::vector<float>& UserNormal() const { return spec_normal_; }
const std::vector<float>& Texcoord() const { return texcoord_; } const std::vector<float>& Texcoord() const { return texcoord_; }
@@ -1233,7 +1236,7 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
void CopyPolygonNormals(mjtNum* arr); void CopyPolygonNormals(mjtNum* arr);
// sets properties of a bounding volume given a face id // sets properties of a bounding volume given a face id
void SetBoundingVolume(int faceid); void SetBoundingVolume(int faceid, const double* dvert);
// load from OBJ, STL, or MSH file; throws mjCError on failure // load from OBJ, STL, or MSH file; throws mjCError on failure
void LoadFromResource(mjResource* resource, bool remove_repeated = false); void LoadFromResource(mjResource* resource, bool remove_repeated = false);
@@ -1264,22 +1267,22 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
void LoadMSH(mjResource* resource, bool remove_repeated); // load mesh in MSH BIN format void LoadMSH(mjResource* resource, bool remove_repeated); // load mesh in MSH BIN format
void LoadSDF(); // generate mesh using marching cubes void LoadSDF(); // generate mesh using marching cubes
void MakeGraph(); // make graph of convex hull void MakeGraph(const double* dvert); // make graph of convex hull
void CopyGraph(); // copy graph into face data void CopyGraph(); // copy graph into face data
void MakeNormal(); // compute vertex normals void MakeNormal(const double* dvert); // compute vertex normals
void MakeCenter(); // compute face circumcircle data void MakeCenter(const double* dvert); // compute face circumcircle data
void Process(); // compute inertial properties void Process(); // compute inertial properties
void ApplyTransformations(); // apply user transformations void ApplyTransformations(double* dvert); // apply user transformations
double ComputeFaceCentroid(double[3]) const; // compute centroid of all faces double ComputeFaceCentroid(double[3], const double* dvert) const;
void CheckInitialMesh() const; // check if initial mesh is valid void CheckInitialMesh() const; // check if initial mesh is valid
void CopyPlugin(); void CopyPlugin();
void Rotate(double quat[4]); // rotate mesh by quaternion void Rotate(double quat[4], double* dvert); // rotate mesh by quaternion
void Transform(double pos[3], double quat[4]); // transform mesh by position and quaternion void Transform(double pos[3], double quat[4]); // transform mesh by position and quaternion
void MakePolygons(); // compute the polygon sides of the mesh void MakePolygons(const double* dvert); // compute the polygon sides of the mesh
void MakePolygonNormals(); // compute the normals of the polygons void MakePolygonNormals(const double* dvert); // compute the normals of the polygons
// computes the inertia matrix of the mesh given the type of inertia // computes the inertia matrix of the mesh given the type of inertia
double ComputeInertia(double inert[6], const double CoM[3]) const; double ComputeInertia(double inert[6], const double CoM[3], const double* dvert) const;
int* GraphFaces() const { int* GraphFaces() const {
return graph_ + 2 + 3*(graph_[0] + graph_[1]); return graph_ + 2 + 3*(graph_[0] + graph_[1]);
@@ -1295,9 +1298,8 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
std::vector<std::vector<int>> polygon_map_; // map from vertex to polygon std::vector<std::vector<int>> polygon_map_; // map from vertex to polygon
// compute the volume and center-of-mass of the mesh given the face centroid // compute the volume and center-of-mass of the mesh given the face centroid
double ComputeVolume(double CoM[3], const double facecen[3]) const; double ComputeVolume(double CoM[3], const double facecen[3], const double* dvert) const;
// compute the surface area and center-of-mass of the mesh given the face centroid double ComputeSurfaceArea(double CoM[3], const double facecen[3], const double* dvert) const;
double ComputeSurfaceArea(double CoM[3], const double facecen[3]) const;
}; };