chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity
Files
a891782553abf2d433a91415b53b97a5456490be
Mujoco_WASM/src/user/user_mesh.cc
T
Alessio Quaglino a891782553 Clean-up flex assumptions. 
Do not allow a mix of `elastic2d != none` with `dof = trilinear` since the latter assumes 3d elasticity.

Also, do not assume that `flex_interp > 0` in the engine. This will enable to use, e.g., `flex_interp = -1` to mean a linear surface finite element instead of a 3d finite element which is currently identified with `flex_interp = 1`.

PiperOrigin-RevId: 903852035
Change-Id: Ia6290b4a05e9e510ffb7f36d141cd525b40d3110
2026-04-22 08:01:34 -07:00

4802 lines
139 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
// Copyright 2021 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <algorithm>
#include <array>
#include <climits>
#include <cmath>
#include <csetjmp>
#include <cstddef>
#include <cstdio>
#include <cstring>
#include <deque>
#include <functional>
#include <limits>
#include <map>
#include <memory>
#include <set>
#include <string>
#include <string_view>
#include <unordered_map>
#include <utility>
#include <vector>
#include <mujoco/mjspec.h>
#include "user/user_api.h"
#ifdef MUJOCO_TINYOBJLOADER_IMPL
#define TINYOBJLOADER_IMPLEMENTATION
#endif
#if defined(__clang__)
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wgnu-anonymous-struct"
#pragma clang diagnostic ignored "-Wnested-anon-types"
#elif defined(__GNUC__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
#endif
#include <MC.h>
#if defined(__clang__)
#pragma clang diagnostic pop
#elif defined(__GNUC__)
#pragma GCC diagnostic pop
#endif
#include <mujoco/mjmacro.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjplugin.h>
#include <mujoco/mjtnum.h>
#include "engine/engine_crossplatform.h" // IWYU pragma: keep
#include "engine/engine_plugin.h"
#include "engine/engine_util_errmem.h"
#include "user/user_cache.h"
#include "user/user_model.h"
#include "user/user_objects.h"
#include "user/user_resource.h"
#include "user/user_util.h"
extern "C" {
#include "qhull_ra.h"
}
namespace {
using mujoco::user::FilePath;
using std::max;
using std::min;
using std::sin;
using std::cos;
using std::pow;
// Parametrized linear/quintic interpolated nonlinearity.
double Fovea(double x, double gamma) {
// Quick return.
if (!gamma) return x;
// Foveal deformation.
double g = mjMAX(0, mjMIN(1, gamma));
return g * pow(x, 5) + (1 - g) * x;
}
// Evenly spaced numbers over a specified interval.
void LinSpace(double lower, double upper, int n, double array[]) {
double increment = n > 1 ? (upper - lower) / (n - 1) : 0;
for (int i = 0; i < n; ++i) {
*array = lower;
++array;
lower += increment;
}
}
// Make bin edges.
void BinEdges(double* x_edges, double* y_edges, int size[2], double fov[2],
double gamma) {
// Make unit bin edges.
LinSpace(-1, 1, size[0] + 1, x_edges);
LinSpace(-1, 1, size[1] + 1, y_edges);
// Apply foveal deformation.
for (int i = 0; i < size[0] + 1; i++) {
x_edges[i] = Fovea(x_edges[i], gamma);
}
for (int i = 0; i < size[1] + 1; i++) {
y_edges[i] = Fovea(y_edges[i], gamma);
}
// Scale by field-of-view.
mjuu_scalevec(x_edges, x_edges, fov[0] * mjPI / 180, size[0] + 1);
mjuu_scalevec(y_edges, y_edges, fov[1] * mjPI / 180, size[1] + 1);
}
// Transform spherical (azimuth, elevation, radius) to Cartesian (x,y,z).
void SphericalToCartesian(const double aer[3], float xyz[3]) {
double a = aer[0], e = aer[1], r = aer[2];
xyz[0] = r * cos(e) * sin(a);
xyz[1] = r * sin(e);
xyz[2] = -r * cos(e) * cos(a);
}
// Tangent frame in Cartesian coordinates.
void TangentFrame(const double aer[3], float mat[9]) {
double a = aer[0], e = aer[1], r = aer[2];
double ta[3] = {r * cos(e) * cos(a), 0, r * cos(e) * sin(a)};
double te[3] = {-r * sin(e) * sin(a), r * cos(e), r * sin(e) * cos(a)};
double n[3];
mjuu_normvec(ta, 3);
mjuu_normvec(te, 3);
mjuu_copyvec(mat + 3, ta, 3);
mjuu_copyvec(mat + 6, te, 3);
mjuu_crossvec(n, te, ta);
mjuu_copyvec(mat, n, 3);
}
// parametric superellipsoid/supertoroid helper functions
double aux_c(double omega, double m) {
return std::copysign(pow(std::abs(cos(omega)), m), cos(omega));
}
double aux_s(double omega, double m) {
return std::copysign(pow(std::abs(sin(omega)), m), sin(omega));
}
} // namespace
// compute triangle area, surface normal, center
static double triangle(double* normal, double* center,
const double* v1, const double* v2, const double* v3) {
double normal_local[3]; // if normal is nullptr
double* normal_ptr = (normal) ? normal : normal_local;
// center
if (center) {
center[0] = (v1[0] + v2[0] + v3[0])/3;
center[1] = (v1[1] + v2[1] + v3[1])/3;
center[2] = (v1[2] + v2[2] + v3[2])/3;
}
// 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] };
mjuu_crossvec(normal_ptr, b, c);
// get length
double len = sqrt(mjuu_dot3(normal_ptr, normal_ptr));
// ignore small faces
if (len < mjMINVAL) {
return 0;
}
// normalize
if (normal) {
normal_ptr[0] /= len;
normal_ptr[1] /= len;
normal_ptr[2] /= len;
}
// return area
return 0.5 * len;
}
// Read data of type T from a potentially unaligned buffer pointer.
template <typename T>
static void ReadFromBuffer(T* dst, const char* src) {
std::memcpy(dst, src, sizeof(T));
}
//------------------ class mjCMesh implementation --------------------------------------------------
mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
mjs_defaultMesh(&spec);
elemtype = mjOBJ_MESH;
// clear internal variables
mjuu_setvec(pos_, 0, 0, 0);
mjuu_setvec(quat_, 1, 0, 0, 0);
mjuu_setvec(boxsz_, 0, 0, 0);
mjuu_setvec(aamm_, 1e10, 1e10, 1e10);
mjuu_setvec(aamm_+3, -1e10, -1e10, -1e10);
szgraph_ = 0;
center_ = nullptr;
graph_ = nullptr;
needhull_ = false;
maxhullvert_ = -1;
processed_ = false;
visual_ = true;
needreorient_ = true;
// reset to default if given
if (_def) {
*this = _def->Mesh();
}
// set model, def
model = _model;
if (_model) compiler = &_model->spec.compiler;
classname = (_def ? _def->name : (_model ? "main" : ""));
// in case this body is not compiled
CopyFromSpec();
// point to local
PointToLocal();
}
mjCMesh::mjCMesh(const mjCMesh& other) {
*this = other;
}
mjCMesh& mjCMesh::operator=(const mjCMesh& other) {
if (this != &other) {
this->spec = other.spec;
*static_cast<mjCMesh_*>(this) = static_cast<const mjCMesh_&>(other);
*static_cast<mjsMesh*>(this) = static_cast<const mjsMesh&>(other);
if (other.center_) {
size_t ncenter = 3*other.nface()*sizeof(double);
this->center_ = (double*)mju_malloc(ncenter);
memcpy(this->center_, other.center_, ncenter);
} else {
this->center_ = nullptr;
}
if (other.graph_) {
size_t szgraph = szgraph_*sizeof(int);
this->graph_ = (int*)mju_malloc(szgraph);
memcpy(this->graph_, other.graph_, szgraph);
} else {
this->graph_ = nullptr;
}
}
PointToLocal();
return *this;
}
void mjCMesh::PointToLocal() {
spec.element = static_cast<mjsElement*>(this);
spec.file = &spec_file_;
spec.content_type = &spec_content_type_;
spec.uservert = &spec_vert_;
spec.usernormal = &spec_normal_;
spec.userface = &spec_face_;
spec.userfacenormal = &spec_facenormal_;
spec.usertexcoord = &spec_texcoord_;
spec.userfacetexcoord = &spec_facetexcoord_;
spec.material = &spec_material_;
spec.plugin.plugin_name = &plugin_name;
spec.plugin.name = &plugin_instance_name;
spec.info = &info;
file = nullptr;
content_type = nullptr;
uservert = nullptr;
usernormal = nullptr;
userface = nullptr;
userfacenormal = nullptr;
usertexcoord = nullptr;
userfacetexcoord = nullptr;
}
void mjCMesh::NameSpace(const mjCModel* m) {
if (name.empty()) {
std::string stripped = mjuu_strippath(spec_file_);
name = mjuu_stripext(stripped);
}
mjCBase::NameSpace(m);
if (!plugin_instance_name.empty()) {
plugin_instance_name = m->prefix + plugin_instance_name + m->suffix;
}
}
void mjCMesh::CopyFromSpec() {
*static_cast<mjsMesh*>(this) = spec;
file_ = spec_file_;
content_type_ = spec_content_type_;
normal_ = spec_normal_;
face_ = spec_face_;
material_ = spec_material_;
ProcessVertices(spec_vert_);
texcoord_ = spec_texcoord_;
facetexcoord_ = spec_facetexcoord_;
maxhullvert_ = spec.maxhullvert;
plugin.active = spec.plugin.active;
plugin.element = spec.plugin.element;
plugin.plugin_name = spec.plugin.plugin_name;
plugin.name = spec.plugin.name;
// clear precompiled asset. TODO: use asset cache
if (center_) mju_free(center_);
if (graph_) mju_free(graph_);
szgraph_ = 0;
center_ = nullptr;
graph_ = nullptr;
// use filename if name is missing
if (name.empty()) {
std::string stripped = mjuu_strippath(file_);
name = mjuu_stripext(stripped);
}
}
void mjCMesh::CopyPlugin() {
model->CopyExplicitPlugin(this);
}
mjCMesh::~mjCMesh() {
if (center_) mju_free(center_);
if (graph_) mju_free(graph_);
}
// generate mesh using marching cubes
void mjCMesh::LoadSDF() {
if (plugin_name.empty() && plugin_instance_name.empty()) {
throw mjCError(
this, "neither 'plugin' nor 'instance' is specified for mesh '%s', (id = %d)",
name.c_str(), id);
}
if (scale[0] != 1 || scale[1] != 1 || scale[2] != 1) {
throw mjCError(this, "attribute scale is not compatible with SDFs in mesh '%s', (id = %d)",
name.c_str(), id);
}
mjCPlugin* plugin_instance = static_cast<mjCPlugin*>(plugin.element);
model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance);
plugin.element = plugin_instance;
const mjpPlugin* pplugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot);
if (!(pplugin->capabilityflags & mjPLUGIN_SDF)) {
throw mjCError(this, "plugin '%s' does not support signed distance fields", pplugin->name);
}
std::vector<mjtNum> attributes(pplugin->nattribute, 0);
std::vector<const char*> names(pplugin->nattribute, 0);
std::vector<const char*> values(pplugin->nattribute, 0);
for (int i=0; i < pplugin->nattribute; i++) {
names[i] = pplugin->attributes[i];
values[i] = plugin_instance->config_attribs[names[i]].c_str();
}
if (pplugin->sdf_attribute) {
pplugin->sdf_attribute(attributes.data(), names.data(), values.data());
}
mjtNum aabb[6] = {0};
pplugin->sdf_aabb(aabb, attributes.data());
mjtNum total = aabb[3] + aabb[4] + aabb[5];
const double n = 300;
int nx, ny, nz;
nx = floor(n / total * aabb[3]) + 1;
ny = floor(n / total * aabb[4]) + 1;
nz = floor(n / total * aabb[5]) + 1;
MC::MC_FLOAT* field = new MC::MC_FLOAT[nx * ny * nz];
for (int i = 0; i < nx; i++) {
for (int j = 0; j < ny; j++) {
for (int k = 0; k < nz; k++) {
mjtNum point[] = {aabb[0]-aabb[3] + 2 * aabb[3] * i / (nx-1),
aabb[1]-aabb[4] + 2 * aabb[4] * j / (ny-1),
aabb[2]-aabb[5] + 2 * aabb[5] * k / (nz-1)};
field[(k * ny + j) * nx + i] = pplugin->sdf_staticdistance(point, attributes.data());
}
}
}
MC::mcMesh mesh;
MC::marching_cube(field, nx, ny, nz, mesh);
std::vector<float> uservert;
std::vector<float> usernormal;
std::vector<int> userface;
uservert.reserve(mesh.vertices.size() * 3);
usernormal.reserve(mesh.normals.size() * 3);
userface.reserve(mesh.indices.size());
for (const auto& vertex : mesh.vertices) {
uservert.push_back(2*aabb[3]*vertex.x/(nx-1) + aabb[0]-aabb[3]);
uservert.push_back(2*aabb[4]*vertex.y/(ny-1) + aabb[1]-aabb[4]);
uservert.push_back(2*aabb[5]*vertex.z/(nz-1) + aabb[2]-aabb[5]);
}
for (const auto& normal : mesh.normals) {
usernormal.push_back(normal.x);
usernormal.push_back(normal.y);
usernormal.push_back(normal.z);
}
for (unsigned int index : mesh.indices) {
userface.push_back(index);
}
needreorient_ = false;
needsdf = false;
normal_ = std::move(usernormal);
face_ = std::move(userface);
ProcessVertices(uservert);
delete[] field;
}
void mjCMesh::CacheMesh(mjCCache* cache, const mjResource* resource) {
if (cache == nullptr) return;
// cache mesh data into new mesh object
mjCMesh *mesh = new mjCMesh();
// mesh properties
mesh->maxhullvert_ = maxhullvert_;
mesh->inertia = inertia;
std::copy(scale, scale + 3, mesh->scale);
// mesh processed data
mesh->processed_ = processed_;
mesh->vert_ = vert_;
mesh->normal_ = normal_;
mesh->texcoord_ = texcoord_;
mesh->face_ = face_;
mesh->facenormal_ = facenormal_;
mesh->facetexcoord_ = facetexcoord_;
mesh->halfedge_ = halfedge_;
mesh->szgraph_ = szgraph_;
if (szgraph_) {
mesh->graph_ = (int*)mju_malloc(szgraph_*sizeof(int));
std::copy(graph_, graph_ + szgraph_, mesh->graph_);
mesh->polygons_ = polygons_;
mesh->polygon_normals_ = polygon_normals_;
mesh->polygon_map_ = polygon_map_;
}
mesh->surface_ = surface_;
mesh->volume_ = volume_;
mesh->material_ = material_;
std::copy(boxsz_, boxsz_ + 3, mesh->boxsz_);
std::copy(aamm_, aamm_ + 6, mesh->aamm_);
std::copy(pos_, pos_ + 3, mesh->pos_);
std::copy(quat_, quat_ + 4, mesh->quat_);
int ncenter = face_.size();
if (ncenter) {
mesh->center_ = (double*)mju_malloc(ncenter * sizeof(double));
std::copy(center_, center_ + ncenter, mesh->center_);
}
mesh->tree_ = tree_;
mesh->face_aabb_ = face_aabb_;
mesh->octree_ = octree_;
// calculate estimated size of mesh
std::size_t size = sizeof(mjCMesh)
+ (sizeof(float) * vert_.size())
+ (sizeof(float) * normal_.size())
+ (sizeof(float) * texcoord_.size())
+ (sizeof(int) * face_.size())
+ (sizeof(int) * facenormal_.size())
+ (sizeof(int) * facetexcoord_.size())
+ (sizeof(int) * 2 * halfedge_.size())
+ (sizeof(int) * szgraph_)
+ (sizeof(int) * npolygonvert())
+ (sizeof(double) * polygon_normals_.size())
+ (sizeof(int) * (szgraph_ ? npolygonmap() : 0))
+ (sizeof(double) * 18)
+ (sizeof(int) * ncenter)
+ tree_.Size()
+ octree_.Size()
+ (sizeof(double) * face_aabb_.size());
std::shared_ptr<const void> cached_data(mesh, +[] (const void* data) {
const mjCMesh* mesh = static_cast<const mjCMesh*>(data);
delete mesh;
});
cache->Insert("", resource->name, resource, cached_data, size);
}
namespace {
// vertex key for hash map
struct VertexKey {
float v[3];
bool operator==(const VertexKey& other) const {
return (v[0] == other.v[0] && v[1] == other.v[1] && v[2] == other.v[2]);
}
std::size_t operator()(const VertexKey& vertex) const {
// combine all three hash values into a single hash value
return ((std::hash<float>()(vertex.v[0])
^ (std::hash<float>()(vertex.v[1]) << 1)) >> 1)
^ (std::hash<float>()(vertex.v[2]) << 1);
}
};
} // namespace
// process and remove repeated vertices if requested
void mjCMesh::ProcessVertices(const std::vector<float>& vert, bool remove_repeated) {
vert_.clear();
int nvert = vert.size();
if (nvert % 3) {
throw mjCError(this, "vertex data must be a multiple of 3");
}
if (face_.size() % 3) {
throw mjCError(this, "face data must be a multiple of 3");
}
// convert vertices to double precision, may contain repeated vertices
if (!remove_repeated) {
vert_.reserve(nvert);
for (int i = 0; i < nvert / 3; ++i) {
const float* v = &vert[3 * i];
if (!std::isfinite(v[0]) || !std::isfinite(v[1]) || !std::isfinite(v[2])) {
throw mjCError(this, "vertex coordinate %d is not finite", nullptr, i);
}
vert_.push_back(v[0]);
vert_.push_back(v[1]);
vert_.push_back(v[2]);
}
return;
}
int index = 0;
std::unordered_map<VertexKey, int, VertexKey> vertex_map;
// populate vertex map with new vertex indices
for (int i = 0; i < nvert; i += 3) {
const float* v = &vert[i];
if (!std::isfinite(v[0]) || !std::isfinite(v[1]) || !std::isfinite(v[2])) {
throw mjCError(this, "vertex coordinate %d is not finite", nullptr, i);
}
VertexKey key = {v[0], v[1], v[2]};
if (vertex_map.find(key) == vertex_map.end()) {
vertex_map.insert({key, index});
++index;
}
}
// no repeated vertices (just copy vertex data)
if (3*index == nvert) {
vert_.reserve(nvert);
for (float v : vert) {
vert_.push_back(v);
}
return;
}
// update face vertex indices
for (int i = 0; i < face_.size(); ++i) {
VertexKey key = {vert[3*face_[i]], vert[3*face_[i] + 1],
vert[3*face_[i] + 2]};
face_[i] = vertex_map[key];
}
// repopulate vertex data
vert_.resize(3 * index);
for (const auto& pair : vertex_map) {
const VertexKey& key = pair.first;
int index = pair.second;
// double precision
vert_[3*index + 0] = key.v[0];
vert_[3*index + 1] = key.v[1];
vert_[3*index + 2] = key.v[2];
}
}
bool mjCMesh::IsMSH(std::string_view filename, std::string_view ct) {
std::string asset_type = GetAssetContentType(filename, ct);
return asset_type == "model/vnd.mujoco.msh";
}
bool mjCMesh::IsMSH() const {
return content_type_ == "model/vnd.mujoco.msh";
}
// load mesh using decoder plugin
void mjCMesh::LoadFromDecoder(mjResource* resource, bool remove_repeated) {
const mjpDecoder* decoder = mjp_findDecoder(resource, content_type_.c_str());
if (!decoder) {
throw mjCError(this, "no decoder found for mesh file '%s'", resource->name);
}
mjSpec* mesh_spec = decoder->decode(resource, nullptr);
if (!mesh_spec) {
throw mjCError(this, "decoder failed for mesh file '%s'", resource->name);
}
mjsElement* elem = mjs_firstElement(mesh_spec, mjOBJ_MESH);
if (elem) {
mjsMesh* src_mesh = mjs_asMesh(elem);
if (src_mesh) {
normal_.assign(src_mesh->usernormal->begin(), src_mesh->usernormal->end());
texcoord_.assign(src_mesh->usertexcoord->begin(), src_mesh->usertexcoord->end());
face_.assign(src_mesh->userface->begin(), src_mesh->userface->end());
facenormal_.assign(src_mesh->userfacenormal->begin(), src_mesh->userfacenormal->end());
facetexcoord_.assign(src_mesh->userfacetexcoord->begin(), src_mesh->userfacetexcoord->end());
std::vector<float> vert(src_mesh->uservert->begin(), src_mesh->uservert->end());
mj_deleteSpec(mesh_spec);
ProcessVertices(vert, remove_repeated);
return;
}
}
mj_deleteSpec(mesh_spec);
}
// load mesh from resource; throw error on failure
void mjCMesh::LoadFromResource(mjResource* resource, bool remove_repeated) {
// set content type from resource name
std::string asset_type = GetAssetContentType(resource->name, content_type_);
content_type_ = asset_type;
if (IsMSH()) {
LoadMSH(resource, remove_repeated);
} else {
LoadFromDecoder(resource, remove_repeated);
}
}
// compiler wrapper
void mjCMesh::Compile(const mjVFS* vfs) {
try {
TryCompile(vfs);
} catch (mjCError err) {
if (resource_ != nullptr) {
mju_closeResource(resource_);
resource_ = nullptr;
}
throw err;
}
}
// compiler
void mjCMesh::TryCompile(const mjVFS* vfs) {
bool fromCache = false;
CopyFromSpec();
visual_ = true;
mjCCache *cache = reinterpret_cast<mjCCache*>(mj_getCache()->impl_);
// load file
if (!file_.empty()) {
vert_.clear();
face_.clear();
normal_.clear();
texcoord_.clear();
facenormal_.clear();
facetexcoord_.clear();
if (resource_ != nullptr) {
mju_closeResource(resource_);
resource_ = nullptr;
}
// copy paths from model if not already defined
mujoco::user::FilePath meshdir_;
meshdir_ = FilePath(mjs_getString(compiler->meshdir));
// remove path from file if necessary
if (model->strippath) {
file_ = mjuu_strippath(file_);
}
mjSpec* owning_spec = model->FindSpec(compiler);
FilePath filename = meshdir_ + FilePath(file_);
resource_ = LoadResource(owning_spec->modelfiledir->c_str(), filename.Str(), vfs);
// try loading from cache
if (cache != nullptr && LoadCachedMesh(cache, resource_)) {
mju_closeResource(resource_);
resource_ = nullptr;
fromCache = true;
}
if (!fromCache) {
LoadFromResource(resource_);
// check repeated mesh data
if (!normal_.empty() && !spec_normal_.empty()) {
throw mjCError(this, "repeated normal specification");
} else if (normal_.empty()) {
normal_ = spec_normal_;
}
if (!texcoord_.empty() && !spec_texcoord_.empty()) {
throw mjCError(this, "repeated texcoord specification");
} else if (texcoord_.empty()) {
texcoord_ = spec_texcoord_;
}
if (!face_.empty() && !spec_face_.empty()) {
throw mjCError(this, "repeated face specification");
} else if (face_.empty()) {
face_ = spec_face_;
}
if (!vert_.empty() && !spec_vert_.empty()) {
throw mjCError(this, "repeated vertex specification");
} else if (vert_.empty()) {
ProcessVertices(spec_vert_);
}
if (!facenormal_.empty() && !spec_normal_.empty()) {
throw mjCError(this, "repeated facenormal specification");
} else if (facenormal_.empty()) {
facenormal_ = spec_facenormal_;
}
if (!facetexcoord_.empty() && !spec_facetexcoord_.empty()) {
throw mjCError(this, "repeated facetexcoord specification");
} else if (facetexcoord_.empty()) {
facetexcoord_ = spec_facetexcoord_;
}
}
} else if (plugin.active) {
LoadSDF(); // create using marching cubes
}
CheckInitialMesh();
// compute mesh properties
if (!fromCache) {
Process();
if (!file_.empty()) {
CacheMesh(cache, resource_);
}
} else {
// When a mesh is loaded from the cache, has no octree but needs one,
// we need to compute it here. If inversely it has an octree but we *do not*
// need one, we clear it.
if (!needsdf) {
octree_.Clear();
} 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_);
}
}
}
// close resource
if (resource_ != nullptr) {
mju_closeResource(resource_);
resource_ = nullptr;
}
}
// get bounding volume
void mjCMesh::SetBoundingVolume(int faceid, const double* dvert) {
constexpr double kMaxVal = std::numeric_limits<double>::max();
double face_aamm[6] = {kMaxVal, kMaxVal, kMaxVal, -kMaxVal, -kMaxVal, -kMaxVal};
for (int j = 0; j < 3; j++) {
int vertid = face_[3*faceid + j];
face_aamm[0] = std::min(face_aamm[0], dvert[3*vertid + 0]);
face_aamm[1] = std::min(face_aamm[1], dvert[3*vertid + 1]);
face_aamm[2] = std::min(face_aamm[2], dvert[3*vertid + 2]);
face_aamm[3] = std::max(face_aamm[3], dvert[3*vertid + 0]);
face_aamm[4] = std::max(face_aamm[4], dvert[3*vertid + 1]);
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[1] + face_aamm[4]));
face_aabb_.push_back(.5 * (face_aamm[2] + face_aamm[5]));
face_aabb_.push_back(.5 * (face_aamm[3] - face_aamm[0]));
face_aabb_.push_back(.5 * (face_aamm[4] - face_aamm[1]));
face_aabb_.push_back(.5 * (face_aamm[5] - face_aamm[2]));
tree_.AddBoundingVolume(faceid, 1, 1, center_ + 3*faceid, nullptr,
&face_aabb_[6*faceid]);
}
double* mjCMesh::GetPosPtr() {
return pos_;
}
double* mjCMesh::GetQuatPtr() {
return quat_;
}
bool mjCMesh::HasTexcoord() const {
return !texcoord_.empty();
}
void mjCMesh::CopyVert(float* arr) const {
std::copy(vert_.begin(), vert_.end(), arr);
}
void mjCMesh::CopyNormal(float* arr) const {
std::copy(normal_.begin(), normal_.end(), arr);
}
void mjCMesh::CopyFace(int* arr) const {
std::copy(face_.begin(), face_.end(), arr);
}
void mjCMesh::CopyFaceTexcoord(int* arr) const {
std::copy(facetexcoord_.begin(), facetexcoord_.end(), arr);
}
void mjCMesh::CopyFaceNormal(int* arr) const {
std::copy(facenormal_.begin(), facenormal_.end(), arr);
}
void mjCMesh::CopyTexcoord(float* arr) const {
std::copy(texcoord_.begin(), texcoord_.end(), arr);
}
void mjCMesh::CopyGraph(int* arr) const {
std::copy(graph_, graph_+szgraph_, arr);
}
void mjCMesh::CopyPolygons(int* verts, int* adr, int* num, int poly_adr) const {
int n = polygons_.size(), count = 0;
for (int i = 0; i < n; ++i) {
int m = num[i] = polygons_[i].size();
adr[i] = poly_adr + count;
count += m;
for (int j = 0; j < m; ++j) {
verts[adr[i] + j - poly_adr] = polygons_[i][j];
}
}
}
void mjCMesh::CopyPolygonMap(int* faces, int* adr, int* num, int poly_adr) const {
int n = polygon_map_.size(), count = 0;
for (int i = 0; i < n; ++i) {
int m = num[i] = polygon_map_[i].size();
adr[i] = poly_adr + count;
count += m;
for (int j = 0; j < m; ++j) {
faces[adr[i] + j - poly_adr] = polygon_map_[i][j];
}
}
}
void mjCMesh::CopyPolygonNormals(mjtNum* arr) {
for (int i = 0; i < polygon_normals_.size(); i += 3) {
arr[i + 0] = (mjtNum)polygon_normals_[i + 0];
arr[i + 1] = (mjtNum)polygon_normals_[i + 1];
arr[i + 2] = (mjtNum)polygon_normals_[i + 2];
}
}
void mjCMesh::DelTexcoord() {
texcoord_.clear();
}
// set geom size to match mesh
void mjCMesh::FitGeom(mjCGeom* geom, double center[3]) {
// use inertial box
if (!model->compiler.fitaabb) {
// get inertia box type (shell or volume)
double* boxsz = GetInertiaBoxPtr();
switch (geom->type) {
case mjGEOM_SPHERE:
geom->size[0] = (boxsz[0] + boxsz[1] + boxsz[2])/3;
break;
case mjGEOM_CAPSULE:
geom->size[0] = (boxsz[0] + boxsz[1])/2;
geom->size[1] = max(0.0, boxsz[2] - geom->size[0]/2);
break;
case mjGEOM_CYLINDER:
geom->size[0] = (boxsz[0] + boxsz[1])/2;
geom->size[1] = boxsz[2];
break;
case mjGEOM_ELLIPSOID:
case mjGEOM_BOX:
geom->size[0] = boxsz[0];
geom->size[1] = boxsz[1];
geom->size[2] = boxsz[2];
break;
default:
throw mjCError(this, "invalid geom type in fitting mesh %s", name.c_str());
}
}
// use aamm
else {
// find aabb box center and size
center[0] = (aamm_[0]+aamm_[3])/2;
center[1] = (aamm_[1]+aamm_[4])/2;
center[2] = (aamm_[2]+aamm_[5])/2;
double size[3] = {aamm_[3] - center[0], aamm_[4] - center[1], aamm_[5] - center[2]};
// compute smallest geom whose aabb contains the mesh aabb
switch (geom->type) {
case mjGEOM_SPHERE:
geom->size[0] = max(max(size[0], size[1]), size[2]);
break;
case mjGEOM_CAPSULE:
case mjGEOM_CYLINDER:
// find maximum distance in XY, separately in Z
geom->size[0] = max(size[0], size[1]);
geom->size[1] = size[2];
// special handling of capsule: consider curved cap
if (geom->type == mjGEOM_CAPSULE) {
geom->size[1] -= geom->size[0];
}
break;
case mjGEOM_ELLIPSOID:
case mjGEOM_BOX:
geom->size[0] = size[0];
geom->size[1] = size[1];
geom->size[2] = size[2];
break;
default:
throw mjCError(this, "invalid fittype in mesh %s", name.c_str());
}
}
// rescale size
geom->size[0] *= geom->fitscale;
geom->size[1] *= geom->fitscale;
geom->size[2] *= geom->fitscale;
}
// load mesh from cached asset, return true on success
bool mjCMesh::LoadCachedMesh(mjCCache *cache, const mjResource* resource) {
auto process_mesh = [&](const void* data) {
const mjCMesh* mesh = static_cast<const mjCMesh*>(data);
// check if maxhullvert is different
if (maxhullvert_ != mesh->maxhullvert_) {
return false;
}
// check if inertia is different
if (inertia != mesh->inertia) {
return false;
}
// check if scale is different
if (scale[0] != mesh->scale[0] ||
scale[1] != mesh->scale[1] ||
scale[2] != mesh->scale[2]) {
return false;
}
// check if need hull
if (needhull_ && !mesh->szgraph_) {
return false;
}
processed_ = mesh->processed_;
vert_ = mesh->vert_;
normal_ = mesh->normal_;
texcoord_ = mesh->texcoord_;
face_ = mesh->face_;
facenormal_ = mesh->facenormal_;
facetexcoord_ = mesh->facetexcoord_;
halfedge_ = mesh->halfedge_;
// only copy graph if needed
if (needhull_ || mesh->face_.empty()) {
szgraph_ = mesh->szgraph_;
graph_ = nullptr;
if (szgraph_) {
graph_ = (int*)mju_malloc(szgraph_*sizeof(int));
std::copy(mesh->graph_, mesh->graph_ + szgraph_, graph_);
}
}
if (szgraph_) {
polygons_ = mesh->polygons_;
polygon_normals_ = mesh->polygon_normals_;
polygon_map_ = mesh->polygon_map_;
} else {
polygon_map_.resize(nvert());
}
surface_ = mesh->surface_;
volume_ = mesh->volume_;
std::copy(mesh->boxsz_, mesh->boxsz_ + 3, boxsz_);
std::copy(mesh->aamm_, mesh->aamm_ + 6, aamm_);
std::copy(mesh->pos_, mesh->pos_ + 3, pos_);
std::copy(mesh->quat_, mesh->quat_ + 4, quat_);
center_ = nullptr;
int ncenter = mesh->face_.size();
if (ncenter) {
center_ = (double*)mju_malloc(ncenter * sizeof(double));
std::copy(mesh->center_, mesh->center_ + ncenter, center_);
}
tree_ = mesh->tree_;
face_aabb_ = mesh->face_aabb_;
octree_ = mesh->octree_;
return true;
};
// check that cached asset has all data
return cache->PopulateData(resource->name, resource, process_mesh);
}
// load MSH binary mesh
void mjCMesh::LoadMSH(mjResource* resource, bool remove_repeated) {
bool righthand = scale[0] * scale[1] * scale[2] > 0;
// get file data in buffer
char* buffer = 0;
int buffer_sz = mju_readResource(resource, (const void**)&buffer);
// still not found
if (buffer_sz < 0) {
throw mjCError(this, "could not read MSH file '%s'", resource->name);
} else if (!buffer_sz) {
throw mjCError(this, "MSH file '%s' is empty", resource->name);
}
// make sure header is present
if (buffer_sz < 4*sizeof(int)) {
throw mjCError(this, "missing header in MSH file '%s'", resource->name);
}
// get sizes from header
int nvbuf = 0, nfbuf = 0, nnbuf = 0, ntbuf = 0;
ReadFromBuffer(&nvbuf, buffer);
ReadFromBuffer(&nnbuf, buffer + sizeof(int));
ReadFromBuffer(&ntbuf, buffer + 2*sizeof(int));
ReadFromBuffer(&nfbuf, buffer + 3*sizeof(int));
// check sizes
if (nvbuf < 4 || nfbuf < 0 || nnbuf < 0 || ntbuf < 0 ||
(nnbuf > 0 && nnbuf != nvbuf) ||
(ntbuf > 0 && ntbuf != nvbuf)) {
throw mjCError(this, "invalid sizes in MSH file '%s'", resource->name);
}
if (nvbuf >= INT_MAX / sizeof(float) / 3 ||
nnbuf >= INT_MAX / sizeof(float) / 3 ||
ntbuf >= INT_MAX / sizeof(float) / 2 ||
nfbuf >= INT_MAX / sizeof(int) / 3) {
throw mjCError(this, "too large sizes in MSH file '%s'.", resource->name);
}
// check file size
if (buffer_sz != 4*sizeof(int) + 3*nvbuf*sizeof(float) + 3*nnbuf*sizeof(float) +
2*ntbuf*sizeof(float) + 3*nfbuf*sizeof(int)) {
throw mjCError(this, "unexpected file size in MSH file '%s'", resource->name);
}
// allocate and copy
using UnalignedFloat = char[sizeof(float)];
auto fdata = reinterpret_cast<UnalignedFloat*>(buffer + 4*sizeof(int));
std::vector<float> vert;
int nvert = 0;
if (nvbuf) {
vert.assign(3*nvbuf, 0);
nvert = 3*nvbuf;
memcpy(vert.data(), fdata, nvert*sizeof(float));
fdata += nvert;
}
if (nnbuf) {
normal_.assign(nvert, 0);
memcpy(normal_.data(), fdata, nvert*sizeof(float));
fdata += nvert;
}
if (ntbuf) {
texcoord_.assign(2*(nvert / 3), 0);
memcpy(texcoord_.data(), fdata, 2*(nvert/3)*sizeof(float));
fdata += 2*(nvert / 3);
}
if (nfbuf) {
face_.assign(3*nfbuf, 0);
facenormal_.assign(3*nfbuf, 0);
memcpy(face_.data(), fdata, 3*nfbuf*sizeof(int));
memcpy(facenormal_.data(), fdata, 3*nfbuf*sizeof(int));
}
if (nfbuf && !texcoord_.empty()) {
facetexcoord_.assign(3*nfbuf, 0);
memcpy(facetexcoord_.data(), fdata, 3*nfbuf*sizeof(int));
}
// rearrange face data if left-handed scaling
if (nfbuf && !righthand) {
for (int i=0; i < nfbuf; i++) {
int tmp = face_[3*i+1];
face_[3*i+1] = face_[3*i+2];
face_[3*i+2] = tmp;
}
}
ProcessVertices(vert, remove_repeated);
}
// 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* dvert) const {
double normal[3], center[3], total_volume = 0;
CoM[0] = CoM[1] = CoM[2] = 0;
int nf = (inertia == mjMESH_INERTIA_CONVEX) ? graph_[1] : nface();
const int* f = (inertia == mjMESH_INERTIA_CONVEX) ? GraphFaces() : face_.data();
for (int i = 0; i < nf; i++) {
double area = triangle(normal, center, &dvert[3*f[3*i]], &dvert[3*f[3*i + 1]],
&dvert[3*f[3*i + 2]]);
// compute and add volume
double vec[3] = {center[0] - facecen[0], center[1] - facecen[1], center[2] - facecen[2]};
double volume = mjuu_dot3(vec, normal) * area / 3;
// if legacy computation requested, then always positive
if (inertia == mjMESH_INERTIA_LEGACY) {
volume = std::abs(volume);
}
// add pyramid com
total_volume += volume;
CoM[0] += volume*(center[0]*3.0/4.0 + facecen[0]/4.0);
CoM[1] += volume*(center[1]*3.0/4.0 + facecen[1]/4.0);
CoM[2] += volume*(center[2]*3.0/4.0 + facecen[2]/4.0);
}
// if volume is valid normalize CoM
if (total_volume >= mjMINVAL) {
CoM[0] /= total_volume;
CoM[1] /= total_volume;
CoM[2] /= total_volume;
}
return total_volume;
}
// 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* dvert) const {
double surface = 0;
CoM[0] = CoM[1] = CoM[2] = 0;
for (int i = 0; i < nface(); i++) {
double area, center[3];
area = triangle(nullptr, center, &dvert[3*face_[3*i]],
&dvert[3*face_[3*i + 1]], &dvert[3*face_[3*i + 2]]);
// add pyramid com
surface += area;
CoM[0] += area*(center[0]*3.0/4.0 + facecen[0]/4.0);
CoM[1] += area*(center[1]*3.0/4.0 + facecen[1]/4.0);
CoM[2] += area*(center[2]*3.0/4.0 + facecen[2]/4.0);
}
// if area is valid normalize CoM
if (surface >= mjMINVAL) {
CoM[0] /= surface;
CoM[1] /= surface;
CoM[2] /= surface;
}
return surface;
}
// apply transformations
void mjCMesh::ApplyTransformations(double* dvert) {
if (refpos[0] != 0 || refpos[1] != 0 || refpos[2] != 0) {
int nv = nvert();
for (int i = 0; i < nv; i++) {
dvert[3*i + 0] -= refpos[0];
dvert[3*i + 1] -= refpos[1];
dvert[3*i + 2] -= refpos[2];
}
}
if (refquat[0] != 1 || refquat[1] != 0 || refquat[2] != 0 || refquat[3] != 0) {
double quat[4] = {refquat[0], refquat[1], refquat[2], refquat[3]};
double mat[9];
mjuu_normvec(quat, 4);
mjuu_quat2mat(mat, quat);
for (int i = 0; i < nvert(); i++) {
mjuu_mulvecmatT(&dvert[3*i], &dvert[3*i], mat);
}
// process normals
for (int i = 0; i < nnormal(); i++) {
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];
}
}
// scale
if (scale[0] != 1 || scale[1] != 1 || scale[2] != 1) {
for (int i = 0; i < nvert(); i++) {
dvert[3*i + 0] *= scale[0];
dvert[3*i + 1] *= scale[1];
dvert[3*i + 2] *= scale[2];
}
for (int i = 0; i < nnormal(); i++) {
normal_[3*i + 0] *= scale[0];
normal_[3*i + 1] *= scale[1];
normal_[3*i + 2] *= scale[2];
}
}
// normalize normals
for (int i = 0; i < nnormal(); i++) {
// compute length
float len = normal_[3*i]*normal_[3*i] + normal_[3*i+1]*normal_[3*i+1] + normal_[3*i+2]*normal_[3*i+2];
// rescale
if (len > mjMINVAL) {
float scl = 1/sqrtf(len);
normal_[3*i + 0] *= scl;
normal_[3*i + 1] *= scl;
normal_[3*i + 2] *= scl;
} else {
normal_[3*i + 0] = 0;
normal_[3*i + 1] = 0;
normal_[3*i + 2] = 1;
}
}
}
// find centroid of faces, return total area
double mjCMesh::ComputeFaceCentroid(double facecen[3], const double* dvert) const {
double total_area = 0;
for (int i = 0; i < nface(); i++) {
double area, center[3];
area = triangle(nullptr, center, &dvert[3*face_[3*i]],
&dvert[3*face_[3*i + 1]], &dvert[3*face_[3*i + 2]]);
// accumulate
facecen[0] += area * center[0];
facecen[1] += area * center[1];
facecen[2] += area * center[2];
total_area += area;
}
// finalize centroid of faces
if (total_area >= mjMINVAL) {
facecen[0] /= total_area;
facecen[1] /= total_area;
facecen[2] /= total_area;
}
return total_area;
}
void mjCMesh::Process() {
std::vector<double> dvert(vert_.begin(), vert_.end());
// create half-edge structure (if mesh was in XML)
if (halfedge_.empty()) {
for (int i = 0; i < nface(); i++) {
int v0 = face_[3*i + 0];
int v1 = face_[3*i + 1];
int v2 = face_[3*i + 2];
if (triangle(nullptr, nullptr, &dvert[3*v0], &dvert[3*v1], &dvert[3*v2]) > sqrt(mjMINVAL)) {
halfedge_.push_back({v0, v1});
halfedge_.push_back({v1, v2});
halfedge_.push_back({v2, v0});
} else {
// TODO(b/255525326)
}
}
}
// check for inconsistent face orientations
if (!halfedge_.empty()) {
std::stable_sort(halfedge_.begin(), halfedge_.end());
auto iterator = std::adjacent_find(halfedge_.begin(), halfedge_.end());
if (iterator != halfedge_.end() && inertia == mjMESH_INERTIA_EXACT) {
throw mjCError(this,
"faces of mesh '%s' have inconsistent orientation. Please check the "
"faces containing the vertices %d and %d.",
name.c_str(), iterator->first + 1, iterator->second + 1);
}
}
// make graph describing convex hull
if (needhull_ || face_.empty()) {
MakeGraph(dvert.data());
}
// no faces: copy from convex hull
if (face_.empty()) {
CopyGraph();
}
// no normals: make
if (normal_.empty()) {
MakeNormal(dvert.data());
}
// check facenormal size
if (!facenormal_.empty() && facenormal_.size() != face_.size()) {
throw mjCError(this, "face data must have the same size as face normal data");
}
// no facetexcoord: copy from faces
if (facetexcoord_.empty() && !texcoord_.empty()) {
facetexcoord_ = face_;
}
// facenormal might not exist if usernormal was specified
if (facenormal_.empty()) {
int normal_per_vertex = normal_.size() / vert_.size();
facenormal_.assign(face_.size(), 0);
for (int i = 0; i < face_.size(); i++) {
facenormal_[i] = normal_per_vertex * face_[i];
}
}
if (szgraph_) {
MakePolygons(dvert.data());
} else {
polygon_map_.resize(nvert());
}
// correct winding order for left-handed coordinate systems
bool righthand = scale[0] * scale[1] * scale[2] > 0;
if (!righthand) {
for (size_t i = 0; i < face_.size(); i += 3) {
std::swap(face_[i + 1], face_[i + 2]);
}
for (size_t i = 0; i < facenormal_.size(); i += 3) {
std::swap(facenormal_[i + 1], facenormal_[i + 2]);
}
for (size_t i = 0; i < facetexcoord_.size(); i += 3) {
std::swap(facetexcoord_[i + 1], facetexcoord_[i + 2]);
}
}
// user offset, rotation, scaling
ApplyTransformations(dvert.data());
// find centroid of faces
double facecen[3] = {0, 0, 0};
if (ComputeFaceCentroid(facecen, dvert.data()) < mjMINVAL) {
throw mjCError(this, "mesh surface area is too small: %s", name.c_str());
}
// compute inertia and transform mesh. The mesh is transformed such that it is
// centered at the CoM and the axes are the principle axes of inertia
double CoM[3] = {0, 0, 0};
double inert[6] = {0, 0, 0, 0, 0, 0};
// compute CoM and volume/area
if (inertia == mjMESH_INERTIA_SHELL) {
surface_ = ComputeSurfaceArea(CoM, facecen, dvert.data());
if (surface_ < mjMINVAL) {
throw mjCError(this, "mesh surface area is too small: %s", name.c_str());
}
} else {
if ((volume_ = ComputeVolume(CoM, facecen, dvert.data())) < mjMINVAL) {
if (volume_ < 0) {
throw mjCError(this, "mesh volume is negative (misoriented triangles): %s", name.c_str());
} else {
throw mjCError(this, "mesh volume is too small: %s . Try setting inertia to shell",
name.c_str());
}
}
}
// compute inertia
double total_volume = ComputeInertia(inert, CoM, dvert.data());
if (inertia == mjMESH_INERTIA_SHELL) {
surface_ = total_volume;
} else {
volume_ = total_volume;
}
// get quaternion and diagonal inertia
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]
};
mjuu_eig3(eigval, eigvec, quattmp, full);
constexpr double inequality_atol = 1e-9;
constexpr double inequality_rtol = 1e-6;
// check eigval - SHOULD NOT OCCUR
if (eigval[2] <= 0) {
throw mjCError(this, "eigenvalue of mesh inertia must be positive: %s", name.c_str());
}
if (eigval[0] + eigval[1] < eigval[2] * (1.0 - inequality_rtol) - inequality_atol ||
eigval[0] + eigval[2] < eigval[1] * (1.0 - inequality_rtol) - inequality_atol ||
eigval[1] + eigval[2] < eigval[0] * (1.0 - inequality_rtol) - inequality_atol) {
throw mjCError(this, "eigenvalues of mesh inertia violate A + B >= C: %s", name.c_str());
}
// compute sizes of equivalent inertia box
double volume = GetVolumeRef();
boxsz_[0] = 0.5 * std::sqrt(6*(eigval[1] + eigval[2] - eigval[0])/volume);
boxsz_[1] = 0.5 * std::sqrt(6*(eigval[0] + eigval[2] - eigval[1])/volume);
boxsz_[2] = 0.5 * std::sqrt(6*(eigval[0] + eigval[1] - eigval[2])/volume);
// prevent reorientation if the mesh was autogenerated using marching cubes
if (!needreorient_) {
mjuu_setvec(CoM, 0, 0, 0);
mjuu_setvec(quattmp, 1, 0, 0, 0);
}
// transform CoM to origin
for (int i=0; i < nvert(); i++) {
dvert[3*i + 0] -= CoM[0];
dvert[3*i + 1] -= CoM[1];
dvert[3*i + 2] -= CoM[2];
}
Rotate(quattmp, dvert.data());
// save the pos and quat that was used to transform the mesh
mjuu_copyvec(pos_, CoM, 3);
mjuu_copyvec(quat_, quattmp, 4);
processed_ = true;
// no radii: make
if (!center_) {
MakeCenter(dvert.data());
}
// recompute polygon normals
MakePolygonNormals(dvert.data());
// make bounding volume hierarchy
if (tree_.Bvh().empty()) {
face_aabb_.clear();
face_aabb_.reserve(3*face_.size());
tree_.AllocateBoundingVolumes(nface());
for (int i = 0; i < nface(); i++) {
SetBoundingVolume(i, dvert.data());
}
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
double mjCMesh::ComputeInertia(double inert[6], const double CoM[3], const double* dvert) const {
double total_volume = 0;
// copy vertices to avoid modifying the original mesh
std::vector<double> vert_centered;
vert_centered.reserve(3*nvert());
// translate vertices to origin in order to compute inertia
for (int i = 0; i < nvert(); i++) {
vert_centered.push_back(dvert[3*i + 0] - CoM[0]);
vert_centered.push_back(dvert[3*i + 1] - CoM[1]);
vert_centered.push_back(dvert[3*i + 2] - CoM[2]);
}
// accumulate products of inertia, recompute volume
const int k[6][2] = {{0, 0}, {1, 1}, {2, 2}, {0, 1}, {0, 2}, {1, 2}};
double P[6] = {0, 0, 0, 0, 0, 0};
int nf = (inertia == mjMESH_INERTIA_CONVEX) ? graph_[1] : nface();
const int* f = (inertia == mjMESH_INERTIA_CONVEX) ? GraphFaces() : face_.data();
for (int i=0; i < nf; i++) {
const double* D = &vert_centered[3*f[3*i + 0]];
const double* E = &vert_centered[3*f[3*i + 1]];
const double* F = &vert_centered[3*f[3*i + 2]];
// get area, normal and center; update volume
double normal[3], center[3];
double volume, area = triangle(normal, center, D, E, F);
if (inertia == mjMESH_INERTIA_SHELL) {
volume = area;
} else {
volume = mjuu_dot3(center, normal) * area / 3;
}
// if legacy computation requested, then always positive
if (inertia == mjMESH_INERTIA_LEGACY) {
volume = abs(volume);
}
// apply formula, accumulate
total_volume += volume;
int C = (inertia == mjMESH_INERTIA_SHELL) ? 12 : 20;
for (int j = 0; j < 6; j++) {
P[j] += volume /
C * (
2*(D[k[j][0]] * D[k[j][1]] +
E[k[j][0]] * E[k[j][1]] +
F[k[j][0]] * F[k[j][1]]) +
D[k[j][0]] * E[k[j][1]] + D[k[j][1]] * E[k[j][0]] +
D[k[j][0]] * F[k[j][1]] + D[k[j][1]] * F[k[j][0]] +
E[k[j][0]] * F[k[j][1]] + E[k[j][1]] * F[k[j][0]]);
}
}
// convert from products of inertia to moments of inertia
inert[0] = P[1] + P[2];
inert[1] = P[0] + P[2];
inert[2] = P[0] + P[1];
inert[3] = -P[3];
inert[4] = -P[4];
inert[5] = -P[5];
return total_volume;
}
void mjCMesh::Rotate(double quat[4], double* dvert) {
double neg[4] = {quat[0], -quat[1], -quat[2], -quat[3]};
double mat[9];
mjuu_quat2mat(mat, neg);
int nv = nvert();
for (int i = 0; i < nv; i++) {
mjuu_mulvecmat(&dvert[3*i], &dvert[3*i], mat);
aamm_[0] = std::min(aamm_[0], dvert[3*i + 0]);
aamm_[3] = std::max(aamm_[3], dvert[3*i + 0]);
aamm_[1] = std::min(aamm_[1], dvert[3*i + 1]);
aamm_[4] = std::max(aamm_[4], dvert[3*i + 1]);
aamm_[2] = std::min(aamm_[2], dvert[3*i + 2]);
aamm_[5] = std::max(aamm_[5], dvert[3*i + 2]);
}
for (int i=0; i < nnormal(); i++) {
// normals
const double nrm[3] = {normal_[3*i], normal_[3*i+1], normal_[3*i+2]};
double res[3];
mjuu_mulvecmat(res, nrm, mat);
for (int j=0; j < 3; j++) {
normal_[3*i+j] = (float) res[j];
}
}
}
void mjCMesh::CheckInitialMesh() const {
if (vert_.size() < 12) {
throw mjCError(this, "at least 4 vertices required");
}
if (vert_.size() % 3) {
throw mjCError(this, "vertex data must be a multiple of 3");
}
if (normal_.size() % 3) {
throw mjCError(this, "normal data must be a multiple of 3");
}
if (texcoord_.size() % 2) {
throw mjCError(this, "texcoord must be a multiple of 2");
}
if (face_.size() % 3) {
throw mjCError(this, "face data must be a multiple of 3");
}
// check texcoord size if no face texcoord indices are given
if (!texcoord_.empty() && texcoord_.size() != 2 * nvert() &&
facetexcoord_.empty() && content_type_ != "model/obj") {
throw mjCError(this,
"texcoord must be 2*nv if face texcoord indices are not provided in an OBJ file");
}
// require vertices
if (vert_.empty()) {
throw mjCError(this, "no vertices");
}
// check vertices exist
int nv = nvert(), nf = face_.size();
for (int i = 0; i < nf; i++) {
if (face_[i] >= nv || face_[i] < 0) {
throw mjCError(this, "in face %d, vertex index %d does not exist",
nullptr, i / 3, face_[i]);
}
}
}
// return inertia pointer
double* mjCMesh::GetInertiaBoxPtr() {
return boxsz_;
}
// return volume or surface area
double mjCMesh::GetVolumeRef() const {
return (inertia == mjMESH_INERTIA_SHELL) ? surface_ : volume_;
}
// make graph describing convex hull
void mjCMesh::MakeGraph(const double* dvert) {
int adr, ok, curlong, totlong, exitcode;
facetT* facet, **facetp;
vertexT* vertex, *vertex1, **vertex1p;
std::string qhopt = "qhull Qt";
if (maxhullvert_ > -1) {
// qhull "TA" actually means "number of vertices added after the initial simplex"
qhopt += " TA" + std::to_string(maxhullvert_ - 4);
}
// graph not needed for small meshes
if (nvert() < 4) {
return;
}
qhT qh_qh;
qhT* qh = &qh_qh;
qh_zero(qh, stderr);
// qhull basic init
qh_init_A(qh, stdin, stdout, stderr, 0, nullptr);
// install longjmp error handler
exitcode = setjmp(qh->errexit);
qh->NOerrexit = false;
if (!exitcode) {
// actual init
qh_initflags(qh, const_cast<char*>(qhopt.c_str()));
qh_init_B(qh, const_cast<double*>(dvert), nvert(), 3, qh_False);
// construct convex hull
qh_qhull(qh);
qh_triangulate(qh);
qh_vertexneighbors(qh);
// allocate graph:
// numvert, numface, vert_edgeadr[numvert], vert_globalid[numvert],
// edge_localid[numvert+3*numface], face_globalid[3*numface]
int numvert = qh->num_vertices;
int numface = qh->num_facets;
szgraph_ = 2 + 3*numvert + 6*numface;
graph_ = (int*) mju_malloc(szgraph_*sizeof(int));
graph_[0] = numvert;
graph_[1] = numface;
// pointers for convenience
int* vert_edgeadr = graph_ + 2;
int* vert_globalid = graph_ + 2 + numvert;
int* edge_localid = graph_ + 2 + 2*numvert;
int* face_globalid = graph_ + 2 + 3*numvert + 3*numface;
// fill in graph data
int i = adr = 0;
ok = 1;
FORALLvertices {
// point id of this vertex, check
int pid = qh_pointid(qh, vertex->point);
if (pid < 0 || pid >= nvert()) {
ok = 0;
break;
}
// save edge address and global id of this vertex
vert_edgeadr[i] = adr;
vert_globalid[i] = pid;
// process neighboring faces and their vertices
int start = adr;
FOREACHsetelement_(facetT, vertex->neighbors, facet) {
int cnt = 0;
FOREACHsetelement_(vertexT, facet->vertices, vertex1) {
cnt++;
// point id of face vertex, check
int pid1 = qh_pointid(qh, vertex1->point);
if (pid1 < 0 || pid1 >= nvert()) {
ok = 0;
break;
}
// if different from vertex id, try to insert
if (pid != pid1) {
// check for previous record
int j;
for (j=start; j < adr; j++)
if (pid1 == edge_localid[j]) {
break;
}
// not found: insert
if (j >= adr) {
edge_localid[adr++] = pid1;
}
}
}
// make sure we have triangle: SHOULD NOT OCCUR
if (cnt != 3) {
mju_error("Qhull did not return triangle");
}
}
// insert separator, advance to next vertex
edge_localid[adr++] = -1;
i++;
}
// size check: SHOULD NOT OCCUR
if (adr != numvert+3*numface) {
mju_error("Wrong size in convex hull graph");
}
// add triangle data, reorient faces if flipped
adr = 0;
FORALLfacets {
int ii = 0;
int ind[3] = {0, 1, 2};
if (facet->toporient) {
ind[0] = 1;
ind[1] = 0;
}
// copy triangle data
FOREACHsetelement_(vertexT, facet->vertices, vertex1) {
// make sure we have triangle: SHOULD NOT OCCUR
if (ii >= 3) {
mju_error("Qhull did not return triangle");
}
face_globalid[adr + ind[ii++]] = qh_pointid(qh, vertex1->point);
}
// advance to next triangle
adr += 3;
}
// free all
qh_freeqhull(qh, !qh_ALL);
qh_memfreeshort(qh, &curlong, &totlong);
// bad graph: delete
if (!ok) {
szgraph_ = 0;
mju_free(graph_);
graph_ = 0;
mju_warning("Could not construct convex hull graph");
}
// replace global ids with local ids in edge data
for (int i=0; i < numvert+3*numface; i++) {
if (edge_localid[i] >= 0) {
// search vert_globalid for match
int adr;
for (adr=0; adr < numvert; adr++) {
if (vert_globalid[adr] == edge_localid[i]) {
edge_localid[i] = adr;
break;
}
}
// make sure we found a match: SHOULD NOT OCCUR
if (adr >= numvert) {
mju_error("Vertex id not found in convex hull");
}
}
}
}
// longjmp error handler
else {
// free all
qh_freeqhull(qh, !qh_ALL);
qh_memfreeshort(qh, &curlong, &totlong);
if (graph_) {
mju_free(graph_);
szgraph_ = 0;
}
throw mjCError(this, "qhull error");
}
}
// copy graph into face data
void mjCMesh::CopyGraph() {
// only if face data is missing
if (!face_.empty()) {
return;
}
// get info from graph, allocate
int numvert = graph_[0];
face_.assign(3*graph_[1], 0);
// copy faces
for (int i=0; i < nface(); i++) {
// address in graph
int j = 2 + 3*numvert + 3*nface() + 3*i;
// copy
face_[3*i + 0] = graph_[j + 0];
face_[3*i + 1] = graph_[j + 1];
face_[3*i + 2] = graph_[j + 2];
}
}
// make a mesh of a hemisphere (quad projected)
void mjCMesh::MakeHemisphere(int res, bool make_faces, bool make_cap) {
constexpr double kNorthPole[3] = {0, 0, 1};
constexpr double kEquator[4][3] = {
{1, 0, 0},
{0, 1, 0},
{-1, 0, 0},
{0, -1, 0},
};
// allocate vertices
int nvert = 1 + 2 * (res + 1) * (res + 2);
nvert += make_cap && make_faces; // add center vertex for bottom cap faces
std::vector<float> vert(3 * nvert);
// north pole
vert[0] = kNorthPole[0];
vert[1] = kNorthPole[1];
vert[2] = kNorthPole[2];
// iterate through rows from north pole to equator, compute vertices
int v = 1;
for (int row = 0; row <= res; row++) {
// iterate through the four sides
for (int side = 0; side < 4; side++) {
double factor = static_cast<double>(row + 1) / (res + 1);
double start[3], end[3];
// start and end points of current arc
for (int i = 0; i < 3; i++) {
start[i] = kNorthPole[i] + factor * (kEquator[side][i] - kNorthPole[i]);
end[i] = kNorthPole[i] + factor * (kEquator[(side + 1) % 4][i] - kNorthPole[i]);
}
// step size for interpolation along the arc
double delta[3];
for (int i = 0; i < 3; i++) {
delta[i] = (end[i] - start[i]) / (row + 1);
}
// interpolate points along the arc
for (int i = 0; i < row + 1; i++) {
double p[3];
for (int j = 0; j < 3; j++) {
p[j] = start[j] + i * delta[j];
}
// normalize point to lie on hemisphere surface
double norm = std::sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]);
vert[3 * v + 0] = p[0] / norm;
vert[3 * v + 1] = p[1] / norm;
vert[3 * v + 2] = p[2] / norm;
v++;
}
}
}
// optional center vertex for bottom cap (for symmetry)
if (make_faces && make_cap) {
vert[3 * (nvert - 1) + 0] = 0;
vert[3 * (nvert - 1) + 1] = 0;
vert[3 * (nvert - 1) + 2] = 0;
}
// save vertices
mjs_setFloat(spec.uservert, vert.data(), 3 * nvert);
if (make_faces) {
// allocate faces
int nface = 4 * (res + 1) * (res + 1);
nface += make_cap * (4 * (res + 1)); // bottom cap faces
std::vector<int> face(3 * nface);
// faces connected to north pole
int f = 0;
face[f++] = 0; face[f++] = 1; face[f++] = 2;
face[f++] = 0; face[f++] = 2; face[f++] = 3;
face[f++] = 0; face[f++] = 3; face[f++] = 4;
face[f++] = 0; face[f++] = 4; face[f++] = 1;
// faces on the hemisphere from north pole to equator
for (int row = 0; row < res; row++) {
const int start_curr = 2 * row * (row + 1) + 1;
const int count_curr = 4 * (row + 1);
const int start_next = start_curr + count_curr;
const int count_next = 4 * (row + 2);
for (int side = 0; side < 4; side++) {
for (int i = 0; i < row + 2; i++) {
const int v_curr = i + (row + 1) * side;
const int v_next = i + (row + 2) * side;
face[f++] = start_curr + v_curr % count_curr;
face[f++] = start_next + v_next % count_next;
face[f++] = start_next + (v_next + 1) % count_next;
if (i < row + 1) {
face[f++] = start_curr + v_curr % count_curr;
face[f++] = start_next + (v_next + 1) % count_next;
face[f++] = start_curr + (v_curr + 1) % count_curr;
}
}
}
}
if (make_cap) {
// add faces for the bottom cap
const int start = 2 * res * (res + 1) + 1;
const int count = 4 * (res + 1);
for (int i = 0; i < count; i++) {
face[f++] = start + i;
face[f++] = nvert - 1;
face[f++] = start + (i + 1) % count;
}
}
// save faces
mjs_setInt(spec.userface, face.data(), 3 * nface);
}
}
// make a mesh of a sphere using icosaheral subdivision
void mjCMesh::MakeSphere(int subdiv, bool make_faces) {
// make icosahedron
const float phi = (1.0 + std::sqrt(5.0)) / 2.0;
std::vector<float> vert = {
-1.0, phi, 0.0,
1.0, phi, 0.0,
-1.0, -phi, 0.0,
1.0, -phi, 0.0,
0.0, -1.0, phi,
0.0, 1.0, phi,
0.0, -1.0, -phi,
0.0, 1.0, -phi,
phi, 0.0, -1.0,
phi, 0.0, 1.0,
-phi, 0.0, -1.0,
-phi, 0.0, 1.0,
};
// normalize vertices to be on a unit sphere
const double norm = std::sqrt(1.0 + phi * phi);
for (float& v : vert) {
v /= norm;
}
std::vector<int> face = {
0, 11, 5, 0, 5, 1, 0, 1, 7, 0, 7, 10, 0, 10, 11,
1, 5, 9, 5, 11, 4, 11, 10, 2, 10, 7, 6, 7, 1, 8,
3, 9, 4, 3, 4, 2, 3, 2, 6, 3, 6, 8, 3, 8, 9,
4, 9, 5, 2, 4, 11, 6, 2, 10, 8, 6, 7, 9, 8, 1
};
// subdivision
if (subdiv > 0) {
// helper to get or create a midpoint vertex
auto get_midpoint = [&vert](
int v1_idx, int v2_idx, std::map<std::pair<int, int>, int>& cache) -> int {
// key is the pair of vertex indices, sorted
std::pair<int, int> key = std::minmax(v1_idx, v2_idx);
// if midpoint is already in cache, return its index
auto it = cache.find(key);
if (it != cache.end()) {
return it->second;
}
// otherwise, create it
const float* v1 = &vert[v1_idx * 3];
const float* v2 = &vert[v2_idx * 3];
float mid_x = (v1[0] + v2[0]) / 2.0f;
float mid_y = (v1[1] + v2[1]) / 2.0f;
float mid_z = (v1[2] + v2[2]) / 2.0f;
// normalize the new vertex to put it on the sphere
float mid_norm = std::sqrt(mid_x * mid_x + mid_y * mid_y + mid_z * mid_z);
mid_x /= mid_norm;
mid_y /= mid_norm;
mid_z /= mid_norm;
// add the new vertex to the list
int new_idx = vert.size() / 3;
vert.push_back(mid_x);
vert.push_back(mid_y);
vert.push_back(mid_z);
// add to cache
cache[key] = new_idx;
return new_idx;
};
// subdivision loop
for (int i = 0; i < subdiv; ++i) {
std::map<std::pair<int, int>, int> midpoint_cache;
std::vector<int> new_face;
new_face.reserve(face.size() * 4);
for (size_t j = 0; j < face.size(); j += 3) {
int v1 = face[j];
int v2 = face[j+1];
int v3 = face[j+2];
int m12 = get_midpoint(v1, v2, midpoint_cache);
int m23 = get_midpoint(v2, v3, midpoint_cache);
int m31 = get_midpoint(v3, v1, midpoint_cache);
new_face.insert(new_face.end(), {v1, m12, m31});
new_face.insert(new_face.end(), {v2, m23, m12});
new_face.insert(new_face.end(), {v3, m31, m23});
new_face.insert(new_face.end(), {m12, m23, m31});
}
face = std::move(new_face);
}
}
// save vertices and maybe faces
mjs_setFloat(spec.uservert, vert.data(), vert.size());
if (make_faces) mjs_setInt(spec.userface, face.data(), face.size());
}
// make a mesh of a supersphere
void mjCMesh::MakeSupersphere(int res, double e, double n) {
// allocate vertices and faces
int nvert = (res - 1) * res + 2;
int nface = 2 * res * (res - 1);
std::vector<float> vert;
vert.reserve(3 * nvert);
std::vector<int> face;
face.reserve(3 * nface);
// south pole
vert.insert(vert.end(), {0.0f, 0.0f, -1.0f});
// rings
for (int i = 1; i < res; i++) {
double v = -mjPI/2 + i * mjPI / res;
for (int j = 0; j < res; j++) {
double u = -mjPI + j * 2 * mjPI / res;
vert.push_back(aux_c(v, n) * aux_c(u, e));
vert.push_back(aux_c(v, n) * aux_s(u, e));
vert.push_back(aux_s(v, n));
}
}
// north pole
vert.insert(vert.end(), {0.0f, 0.0f, 1.0f});
// south pole faces
for (int j = 0; j < res; j++) {
int v2 = 1 + j;
int v3 = 1 + (j + 1) % res;
face.insert(face.end(), {0, v3, v2});
}
// ring faces
for (int i = 0; i < res - 2; i++) {
for (int j = 0; j < res; j++) {
int v1 = 1 + i * res + j;
int v2 = 1 + i * res + (j + 1) % res;
int v4 = 1 + (i + 1) * res + j;
int v3 = 1 + (i + 1) * res + (j + 1) % res;
face.insert(face.end(), {v1, v2, v4});
face.insert(face.end(), {v2, v3, v4});
}
}
// north pole faces
int north_pole_idx = nvert - 1;
int last_ring_start_idx = 1 + (res - 2) * res;
for (int j = 0; j < res; j++) {
int v1 = last_ring_start_idx + j;
int v2 = last_ring_start_idx + (j + 1) % res;
face.insert(face.end(), {v1, v2, north_pole_idx});
}
// save vertices and faces
mjs_setFloat(spec.uservert, vert.data(), vert.size());
mjs_setInt(spec.userface, face.data(), face.size());
}
// make a mesh of a torus (subsumed by supertorus, kept for reference only)
void mjCMesh::MakeTorus(int res, double radius) {
// allocate vertices and faces
int nvert = res * res;
int nface = res * res * 2;
std::vector<float> vert(3 * nvert);
std::vector<int> face(3 * nface);
// generate vertices
for (int i = 0; i < res; ++i) {
for (int j = 0; j < res; ++j) {
double u = 2 * mjPI * i / res;
double v = 2 * mjPI * j / res;
int vidx = i * res + j;
vert[3 * vidx + 0] = (1 + radius * cos(v)) * cos(u);
vert[3 * vidx + 1] = (1 + radius * cos(v)) * sin(u);
vert[3 * vidx + 2] = radius * sin(v);
}
}
// generate faces
int fidx = 0;
for (int i = 0; i < res; ++i) {
for (int j = 0; j < res; ++j) {
int i_next = (i + 1) % res;
int j_next = (j + 1) % res;
int v1 = i * res + j;
int v2 = i_next * res + j;
int v3 = i_next * res + j_next;
int v4 = i * res + j_next;
// first triangle
face[3 * fidx + 0] = v1;
face[3 * fidx + 1] = v2;
face[3 * fidx + 2] = v4;
fidx++;
// second triangle
face[3 * fidx + 0] = v2;
face[3 * fidx + 1] = v3;
face[3 * fidx + 2] = v4;
fidx++;
}
}
// save vertices and faces
mjs_setFloat(spec.uservert, vert.data(), vert.size());
mjs_setInt(spec.userface, face.data(), face.size());
}
// make a mesh of a supertoroid, see https://en.wikipedia.org/wiki/Supertoroid
void mjCMesh::MakeSupertorus(int res, double radius, double s, double t) {
// allocate vertices and faces
int nvert = res * res;
int nface = res * res * 2;
std::vector<float> vert(3 * nvert);
std::vector<int> face(3 * nface);
// generate vertices
for (int i = 0; i < res; ++i) {
for (int j = 0; j < res; ++j) {
double u = 2 * mjPI * i / res;
double v = 2 * mjPI * j / res;
int vidx = i * res + j;
vert[3 * vidx + 0] = (1 + radius * aux_c(v, s)) * aux_c(u, t);
vert[3 * vidx + 1] = (1 + radius * aux_c(v, s)) * aux_s(u, t);
vert[3 * vidx + 2] = radius * aux_s(v, s);
}
}
// generate faces
int fidx = 0;
for (int i = 0; i < res; ++i) {
for (int j = 0; j < res; ++j) {
int i_next = (i + 1) % res;
int j_next = (j + 1) % res;
int v1 = i * res + j;
int v2 = i_next * res + j;
int v3 = i_next * res + j_next;
int v4 = i * res + j_next;
// first triangle
face[3 * fidx + 0] = v1;
face[3 * fidx + 1] = v2;
face[3 * fidx + 2] = v4;
fidx++;
// second triangle
face[3 * fidx + 0] = v2;
face[3 * fidx + 1] = v3;
face[3 * fidx + 2] = v4;
fidx++;
}
}
// save vertices and faces
mjs_setFloat(spec.uservert, vert.data(), vert.size());
mjs_setInt(spec.userface, face.data(), face.size());
}
// make a mesh of a spherical wedge
void mjCMesh::MakeWedge(int resolution[2], double fov[2], double gamma) {
std::vector<double> x_edges(resolution[0] + 1, 0);
std::vector<double> y_edges(resolution[1] + 1, 0);
BinEdges(x_edges.data(), y_edges.data(), resolution, fov, gamma);
std::vector<float> uservert(3 * resolution[0] * resolution[1], 0);
std::vector<float> usernormal(9 * resolution[0] * resolution[1], 0);
for (int i = 0; i < resolution[0]; i++) {
for (int j = 0; j < resolution[1]; j++) {
double aer[3];
aer[0] = 0.5 * (x_edges[i + 1] + x_edges[i]);
aer[1] = 0.5 * (y_edges[j + 1] + y_edges[j]);
aer[2] = 1;
SphericalToCartesian(aer, uservert.data() + 3 * (i * resolution[1] + j));
TangentFrame(aer, usernormal.data() + 9 * (i * resolution[1] + j));
}
}
mjs_setFloat(spec.uservert, uservert.data(),
3 * resolution[0] * resolution[1]);
mjs_setFloat(spec.usernormal, usernormal.data(),
9 * resolution[0] * resolution[1]);
}
// make a mesh of a rectangle
void mjCMesh::MakeRect(int resolution[2]) {
std::vector<double> x_edges(resolution[0] + 1, 0);
std::vector<double> y_edges(resolution[1] + 1, 0);
LinSpace(-1, 1, resolution[0] + 1, x_edges.data());
LinSpace(-1, 1, resolution[1] + 1, y_edges.data());
std::vector<float> uservert(3 * resolution[0] * resolution[1], 0);
std::vector<float> usernormal(9 * resolution[0] * resolution[1], 0);
std::vector<int> userface(6 * (resolution[0] - 1) * (resolution[1] - 1), 0);
spec.inertia = mjMESH_INERTIA_SHELL;
for (int i = 0; i < resolution[0]; i++) {
for (int j = 0; j < resolution[1]; j++) {
int vert = i * resolution[1] + j;
mjtNum dx = 2. / resolution[0];
mjtNum dy = 2. / resolution[1];
uservert[3 * vert + 0] = -1 + (i + 0.5) * dx;
uservert[3 * vert + 1] = -1 + (j + 0.5) * dy;
uservert[3 * vert + 2] = -1;
usernormal[9 * vert + 0] = 1;
usernormal[9 * vert + 4] = 1;
usernormal[9 * vert + 8] = 1;
if (i > 0 && j > 0) {
int cell = (i - 1) * (resolution[1] - 1) + j - 1;
userface[6 * cell + 0] = (i - 1) * resolution[1] + j - 1;
userface[6 * cell + 1] = (i - 0) * resolution[1] + j - 1;
userface[6 * cell + 2] = (i - 1) * resolution[1] + j - 0;
userface[6 * cell + 3] = (i - 0) * resolution[1] + j - 0;
userface[6 * cell + 4] = (i - 1) * resolution[1] + j - 0;
userface[6 * cell + 5] = (i - 0) * resolution[1] + j - 1;
}
}
}
mjs_setFloat(spec.uservert, uservert.data(),
3 * resolution[0] * resolution[1]);
mjs_setFloat(spec.usernormal, usernormal.data(),
9 * resolution[0] * resolution[1]);
mjs_setInt(spec.userface, userface.data(),
6 * (resolution[0] - 1) * (resolution[1] - 1));
}
// make a mesh of a generalized discrete cone
void mjCMesh::MakeCone(int nedge, double radius) {
int n = 3 * (nedge + (radius > 0 ? nedge : 1));
std::vector<float> uservert(n, 0);
// bottom face
for (int i = 0; i < nedge; i++) {
uservert[3 * i + 0] = cos(2 * i * mjPI / nedge);
uservert[3 * i + 1] = sin(2 * i * mjPI / nedge);
uservert[3 * i + 2] = -1;
}
// top face or single point
if (radius > 0) {
for (int i = nedge; i < 2 * nedge; i++) {
uservert[3 * i + 0] = radius * cos(2 * i * mjPI / nedge);
uservert[3 * i + 1] = radius * sin(2 * i * mjPI / nedge);
uservert[3 * i + 2] = 1;
}
} else {
uservert[3 * nedge + 2] = 1;
}
mjs_setFloat(spec.uservert, uservert.data(), n);
}
// compute vertex normals
void mjCMesh::MakeNormal(const double* dvert) {
// only if normal data is missing
if (!normal_.empty()) {
return;
}
// allocate and clear normals
normal_.assign(3*nvert(), 0);
if (facenormal_.empty()) {
facenormal_.assign(3*nface(), 0);
}
// loop over faces, accumulate vertex normals
for (int i=0; i < nface(); i++) {
// get vertex ids
int vertid[3];
for (int j=0; j < 3; j++) {
vertid[j] = face_[3*i+j];
}
// get triangle edges
double vec01[3], vec02[3];
for (int j=0; j < 3; j++) {
vec01[j] = dvert[3*vertid[1]+j] - dvert[3*vertid[0]+j];
vec02[j] = dvert[3*vertid[2]+j] - dvert[3*vertid[0]+j];
}
// compute face normal
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++) {
for (int k=0; k < 3; k++) {
normal_[3*vertid[j]+k] += nrm[k]*area;
}
facenormal_[3*i+j] = vertid[j];
}
}
// remove large-angle faces
if (!smoothnormal) {
// allocate removal and clear
float* nremove = (float*) mju_malloc(3*nnormal()*sizeof(float));
memset(nremove, 0, 3*nnormal()*sizeof(float));
// remove contributions from faces at large angles with vertex normal
for (int i=0; i < nface(); i++) {
// get vertex ids
int vertid[3];
for (int j=0; j < 3; j++) {
vertid[j] = face_[3*i+j];
}
// get triangle edges
double vec01[3], vec02[3];
for (int j=0; j < 3; j++) {
vec01[j] = dvert[3*vertid[1]+j] - dvert[3*vertid[0]+j];
vec02[j] = dvert[3*vertid[2]+j] - dvert[3*vertid[0]+j];
}
// compute face normal
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
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 (mjuu_dot3(nrm, vnrm) < 0.8) {
for (int k=0; k < 3; k++) {
nremove[3*vertid[j]+k] += nrm[k]*area;
}
}
}
}
// apply removal, free nremove
for (int i=0; i < 3*nnormal(); i++) {
normal_[i] -= nremove[i];
}
mju_free(nremove);
}
// normalize normals
for (int i=0; i < nnormal(); i++) {
// compute length
float len = sqrtf(normal_[3*i]*normal_[3*i] +
normal_[3*i+1]*normal_[3*i+1] +
normal_[3*i+2]*normal_[3*i+2]);
// divide by length
if (len > mjMINVAL) {
for (int j=0; j < 3; j++) {
normal_[3*i+j] /= len;
}
} else {
normal_[3*i] = normal_[3*i+1] = 0;
normal_[3*i+2] = 1;
}
}
}
// compute face circumradii
void mjCMesh::MakeCenter(const double* dvert) {
if (center_) {
return;
}
// allocate and clear
center_ = (double*) mju_malloc(3*nface()*sizeof(double));
memset(center_, 0, 3*nface()*sizeof(double));
for (int i=0; i < nface(); i++) {
// get vertex ids
int* vertid = face_.data() + 3*i;
// get triangle edges
double a[3], b[3];
for (int j=0; j < 3; j++) {
a[j] = dvert[3*vertid[0]+j] - dvert[3*vertid[2]+j];
b[j] = dvert[3*vertid[1]+j] - dvert[3*vertid[2]+j];
}
// compute face normal
double nrm[3];
mjuu_crossvec(nrm, a, b);
// compute circumradius
double norm_a_2 = mjuu_dot3(a, a);
double norm_b_2 = mjuu_dot3(b, b);
double area = sqrt(mjuu_dot3(nrm, nrm));
// compute circumcenter
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]
};
mjuu_crossvec(res, vec, nrm);
center_[3*i+0] = res[0]/(2*area*area) + dvert[3*vertid[2]+0];
center_[3*i+1] = res[1]/(2*area*area) + dvert[3*vertid[2]+1];
center_[3*i+2] = res[2]/(2*area*area) + dvert[3*vertid[2]+2];
}
}
// compute the normals of the polygons
void mjCMesh::MakePolygonNormals(const double* dvert) {
for (int i = 0; i < polygons_.size(); ++i) {
double n[3];
mjuu_makenormal(n, &dvert[3*polygons_[i][0]], &dvert[3*polygons_[i][1]],
&dvert[3*polygons_[i][2]]);
polygon_normals_[3*i + 0] = n[0];
polygon_normals_[3*i + 1] = n[1];
polygon_normals_[3*i + 2] = n[2];
}
}
// helper class to compute the polygons of a mesh
class MeshPolygon {
public:
// constructors (need starting face)
MeshPolygon(const double v1[3], const double v2[3], const double v3[3],
int v1i, int v2i, int v3i, double theta, double phi);
MeshPolygon() = delete;
MeshPolygon(const MeshPolygon&) = delete;
MeshPolygon& operator=(const MeshPolygon&) = delete;
MeshPolygon(MeshPolygon&&) = default;
MeshPolygon& operator=(MeshPolygon&&) = default;
void InsertFace(int v1, int v2, int v3); // insert a face into the polygon
std::vector<std::vector<int>> Paths() const; // return trace of the polygons
const double* Normal() const { return normal_; } // return the normal of the polygon
double Normal(int i) const { return normal_[i]; } // return the i-th component of the normal
private:
std::vector<std::pair<int, int>> edges_;
// inserted faces do not necessarily share edges with the current polygon, so they're grouped as
// islands until they can be combined with later face insertions
std::vector<int> islands_;
int nisland_ = 0;
double normal_[3] = {0.0, 0.0, 0.0};
void CombineIslands(int& island1, int& island2);
};
bool MeshPolygonKey(std::pair<double, double>& angles, const double v1[3], const double v2[3],
const double v3[3], double angle_tol) {
double diff12[3] = {v2[0] - v1[0], v2[1] - v1[1], v2[2] - v1[2]};
double diff13[3] = {v3[0] - v1[0], v3[1] - v1[1], v3[2] - v1[2]};
double normal[3], norm;
mjuu_crossvec(normal, diff12, diff13);
if ((norm = std::sqrt(mjuu_dot3(normal, normal))) < mjMINVAL) {
return false;
}
// atan2 is sensitive to sign of 0.0, adding 0.0 to enforcing only positive 0.0
normal[0] = (normal[0] / norm) + 0.0;
normal[1] = (normal[1] / norm) + 0.0;
normal[2] = (normal[2] / norm) + 0.0;
double rtheta = 0.0, rphi = 0.0;
// clamp normal to be in valid range for acos
if (std::abs(normal[2]) > 1.0 - 1e-7) {
if (normal[2] < 0) rphi = std::round(mjPI / angle_tol);
angles = std::make_pair(rtheta, rphi);
return true;
}
// rounded azimuthal and polar angles
rtheta = std::round(std::atan2(normal[1], normal[0]) / angle_tol);
rphi = std::round(std::acos(normal[2]) / angle_tol);
angles = std::make_pair(rtheta, rphi);
return true;
}
MeshPolygon::MeshPolygon(const double v1[3], const double v2[3], const double v3[3],
int v1i, int v2i, int v3i, double theta, double phi) {
normal_[0] = std::cos(theta) * std::sin(phi);
normal_[1] = std::sin(theta) * std::sin(phi);
normal_[2] = std::cos(phi);
edges_ = {{v1i, v2i}, {v2i, v3i}, {v3i, v1i}};
nisland_ = 1;
islands_ = {0, 0, 0};
}
// combine two islands when a newly inserted face connects them
void MeshPolygon::CombineIslands(int& island1, int& island2) {
// pick the smaller island
if (island2 < island1) {
int tmp = island1;
island1 = island2;
island2 = tmp;
}
// renumber the islands
for (int k = 0; k < islands_.size(); ++k) {
if (islands_[k] == island2) {
islands_[k] = island1;
} else if (islands_[k] > island2) {
islands_[k]--;
}
}
}
// insert a triangular face into the polygon
void MeshPolygon::InsertFace(int v1, int v2, int v3) {
int add1 = 1, add2 = 1, add3 = 1;
int island = -1;
// check if face can be attached via edge v1v2
for (int i = 0; i < edges_.size(); ++i) {
if (edges_[i].first == v2 && edges_[i].second == v1) {
add1 = 0;
island = islands_[i];
edges_.erase(edges_.begin() + i);
islands_.erase(islands_.begin() + i);
break;
}
}
// check if face can be attached via edge v2v3
for (int i = 0; i < edges_.size(); ++i) {
if (edges_[i].first == v3 && edges_[i].second == v2) {
int island2 = islands_[i];
if (island == -1) {
island = island2;
} else if (island2 != island) {
nisland_--;
CombineIslands(island, island2);
}
add2 = 0;
edges_.erase(edges_.begin() + i);
islands_.erase(islands_.begin() + i);
break;
}
}
// check if face can be attached via edge v3v1
for (int i = 0; i < edges_.size(); ++i) {
if (edges_[i].first == v1 && edges_[i].second == v3) {
int island3 = islands_[i];
if (island == -1) {
island = island3;
} else if (island3 != island) {
nisland_--;
CombineIslands(island, island3);
}
add3 = 0;
edges_.erase(edges_.begin() + i);
islands_.erase(islands_.begin() + i);
break;
}
}
if (island == -1) {
island = nisland_++;
}
// add only new edges to the polygon
if (add1) {
edges_.push_back({v1, v2});
islands_.push_back(island);
}
if (add2) {
edges_.push_back({v2, v3});
islands_.push_back(island);
}
if (add3) {
edges_.push_back({v3, v1});
islands_.push_back(island);
}
}
// return the transverse vertices of the polygon, multiple paths possible if not connected
std::vector<std::vector<int>> MeshPolygon::Paths() const {
std::vector<std::vector<int>> paths;
// shortcut if polygon is just a triangular face
if (edges_.size() == 3) {
return {{edges_[0].first, edges_[1].first, edges_[2].first}};
}
// go through each connected component of the polygon
for (int i = 0; i < nisland_; ++i) {
std::vector<int> path;
// find starting vertex
for (int j = 0; j < edges_.size(); ++j) {
if (islands_[j] == i) {
path.push_back(edges_[j].first);
path.push_back(edges_[j].second);
break;
}
}
// SHOULD NOT OCCUR (See logic in MeshPolygon::CombineIslands)
if (path.empty()) {
continue;
}
// visit the next vertex given the current edge
int next = path.back();
for (int l = 0; l < edges_.size(); ++l) {
int finished = 0;
for (int k = 1; k < edges_.size(); ++k) {
if (islands_[k] == i && edges_[k].first == next) {
next = edges_[k].second;
if (next == path[0]) {
paths.push_back(path);
finished = 1;
break;
}
path.push_back(next);
break;
}
}
// back at start
if (finished) {
break;
}
}
}
return paths;
}
// hash function for std::pair
struct PairHash {
template <class T1, class T2>
std::size_t operator() (const std::pair<T1, T2>& pair) const {
return std::hash<T1>()(pair.first) ^ std::hash<T2>()(pair.second);
}
};
// merge coplanar mesh triangular faces into polygonal sides to represent the geometry of the mesh
void mjCMesh::MakePolygons(const double* dvert) {
constexpr double kAngleTol = 0.01;
std::unordered_map<std::pair<double, double>, MeshPolygon, PairHash> mesh_polygons;
polygons_.clear();
polygon_normals_.clear();
polygon_map_.clear();
polygon_map_.resize(nvert());
// we need a convex mesh, so we use graph faces
int *faces = GraphFaces();
int nfaces = graph_[1];
// process each face
for (int i = 0; i < nfaces; i++) {
int vi1 = faces[3*i + 0];
int vi2 = faces[3*i + 1];
int vi3 = faces[3*i + 2];
const double* v1 = &dvert[3*vi1];
const double* v2 = &dvert[3*vi2];
const double* v3 = &dvert[3*vi3];
std::pair<double, double> key;
if (!MeshPolygonKey(key, v1, v2, v3, kAngleTol)) {
continue;
}
auto it = mesh_polygons.find(key);
if (it == mesh_polygons.end()) {
double theta = kAngleTol * key.first;
double phi = kAngleTol * key.second;
mesh_polygons.emplace(key, MeshPolygon(v1, v2, v3, vi1, vi2, vi3, theta, phi));
} else {
it->second.InsertFace(vi1, vi2, vi3);
}
}
for (const auto& pair : mesh_polygons) {
const MeshPolygon& polygon = pair.second;
std::vector<std::vector<int>> paths = polygon.Paths();
// separate the polygons if they were grouped together
for (const auto& path : paths) {
if (path.size() < 3) continue;
polygons_.push_back(path);
polygon_normals_.push_back(polygon.Normal(0));
polygon_normals_.push_back(polygon.Normal(1));
polygon_normals_.push_back(polygon.Normal(2));
}
}
// populate the polygon map
for (int i = 0; i < polygons_.size(); i++) {
for (int j = 0; j < polygons_[i].size(); ++j) {
polygon_map_[polygons_[i][j]].push_back(i);
}
}
}
//------------------ class mjCSkin implementation --------------------------------------------------
// constructor
mjCSkin::mjCSkin(mjCModel* _model) {
mjs_defaultSkin(&spec);
elemtype = mjOBJ_SKIN;
// set model pointer
model = _model;
if (model) compiler = &model->spec.compiler;
// clear data
spec_file_.clear();
spec_material_.clear();
spec_vert_.clear();
spec_texcoord_.clear();
spec_face_.clear();
spec_bodyname_.clear();
spec_bindpos_.clear();
spec_bindquat_.clear();
spec_vertid_.clear();
spec_vertweight_.clear();
bodyid.clear();
matid = -1;
// point to local
PointToLocal();
// in case this camera is not compiled
CopyFromSpec();
}
mjCSkin::mjCSkin(const mjCSkin& other) {
*this = other;
}
mjCSkin& mjCSkin::operator=(const mjCSkin& other) {
if (this != &other) {
this->spec = other.spec;
*static_cast<mjCSkin_*>(this) = static_cast<const mjCSkin_&>(other);
*static_cast<mjsSkin*>(this) = static_cast<const mjsSkin&>(other);
}
PointToLocal();
return *this;
}
void mjCSkin::PointToLocal() {
spec.element = static_cast<mjsElement*>(this);
spec.file = &spec_file_;
spec.material = &spec_material_;
spec.vert = &spec_vert_;
spec.texcoord = &spec_texcoord_;
spec.face = &spec_face_;
spec.bodyname = &spec_bodyname_;
spec.bindpos = &spec_bindpos_;
spec.bindquat = &spec_bindquat_;
spec.vertid = &spec_vertid_;
spec.vertweight = &spec_vertweight_;
spec.info = &info;
file = nullptr;
material = nullptr;
vert = nullptr;
texcoord = nullptr;
face = nullptr;
bodyname = nullptr;
bindpos = nullptr;
bindquat = nullptr;
vertid = nullptr;
vertweight = nullptr;
}
void mjCSkin::NameSpace(const mjCModel* m) {
// use filename if name is missing
if (name.empty()) {
std::string stripped = mjuu_strippath(spec_file_);
name = mjuu_stripext(stripped);
}
for (auto& name : spec_bodyname_) {
name = m->prefix + name + m->suffix;
}
}
void mjCSkin::CopyFromSpec() {
*static_cast<mjsSkin*>(this) = spec;
file_ = spec_file_;
material_ = spec_material_;
vert_ = spec_vert_;
texcoord_ = spec_texcoord_;
face_ = spec_face_;
bodyname_ = spec_bodyname_;
bindpos_ = spec_bindpos_;
bindquat_ = spec_bindquat_;
vertid_ = spec_vertid_;
vertweight_ = spec_vertweight_;
// use filename if name is missing
if (name.empty()) {
std::string stripped = mjuu_strippath(file_);
name = mjuu_stripext(stripped);
}
}
// destructor
mjCSkin::~mjCSkin() {
spec_file_.clear();
spec_material_.clear();
spec_vert_.clear();
spec_texcoord_.clear();
spec_face_.clear();
spec_bodyname_.clear();
spec_bindpos_.clear();
spec_bindquat_.clear();
spec_vertid_.clear();
spec_vertweight_.clear();
bodyid.clear();
}
void mjCSkin::ResolveReferences(const mjCModel* m) {
size_t nbone = bodyname_.size();
bodyid.resize(nbone);
for (int i=0; i < nbone; i++) {
mjCBase* pbody = m->FindObject(mjOBJ_BODY, bodyname_[i]);
if (!pbody) {
throw mjCError(this, "unknown body '%s' in skin", bodyname_[i].c_str());
}
bodyid[i] = pbody->id;
}
}
// compiler
void mjCSkin::Compile(const mjVFS* vfs) {
CopyFromSpec();
// load file
if (!file_.empty()) {
// make sure data is not present
if (!spec_vert_.empty() ||
!spec_texcoord_.empty() ||
!spec_face_.empty() ||
!spec_bodyname_.empty() ||
!spec_bindpos_.empty() ||
!spec_bindquat_.empty() ||
!spec_vertid_.empty() ||
!spec_vertweight_.empty()) {
throw mjCError(this, "Both skin data and file were specified: %s", file_.c_str());
}
// remove path from file if necessary
if (model->strippath) {
file_ = mjuu_strippath(file_);
}
// load SKN
std::string ext = mjuu_getext(file_);
if (strcasecmp(ext.c_str(), ".skn")) {
throw mjCError(this, "Unknown skin file type: %s", file_.c_str());
}
mujoco::user::FilePath meshdir_;
meshdir_ = FilePath(mjs_getString(compiler->meshdir));
FilePath filename = meshdir_ + FilePath(file_);
mjSpec* owning_spec = model->FindSpec(compiler);
mjResource* resource = LoadResource(owning_spec->modelfiledir->c_str(), filename.Str(), vfs);
try {
LoadSKN(resource);
mju_closeResource(resource);
} catch(mjCError err) {
mju_closeResource(resource);
throw err;
}
}
// make sure all data is present
if (vert_.empty() ||
face_.empty() ||
bodyname_.empty() ||
bindpos_.empty() ||
bindquat_.empty() ||
vertid_.empty() ||
vertweight_.empty()) {
throw mjCError(this, "Missing data in skin");
}
// check mesh sizes
if (vert_.size()%3) {
throw mjCError(this, "Vertex data must be multiple of 3");
}
if (!texcoord_.empty() && texcoord_.size() != 2*vert_.size()/3) {
throw mjCError(this, "Vertex and texcoord data incompatible size");
}
if (face_.size()%3) {
throw mjCError(this, "Face data must be multiple of 3");
}
// check bone sizes
size_t nbone = bodyname_.size();
if (bindpos_.size() != 3*nbone) {
throw mjCError(this, "Unexpected bindpos size in skin");
}
if (bindquat_.size() != 4*nbone) {
throw mjCError(this, "Unexpected bindquat size in skin");
}
if (vertid_.size() != nbone) {
throw mjCError(this, "Unexpected vertid size in skin");
}
if (vertweight_.size() != nbone) {
throw mjCError(this, "Unexpected vertweight size in skin");
}
// resolve body names
ResolveReferences(model);
// resolve material name
mjCBase* pmat = model->FindObject(mjOBJ_MATERIAL, material_);
if (pmat) {
matid = pmat->id;
} else if (!material_.empty()) {
throw mjCError(this, "unknown material '%s' in skin", material_.c_str());
}
// set total vertex weights to 0
std::vector<float> vw;
size_t nvert = vert_.size()/3;
vw.resize(nvert);
fill(vw.begin(), vw.end(), 0.0f);
// accumulate vertex weights from all bones
for (int i=0; i < nbone; i++) {
// make sure bone has vertices and sizes match
size_t nbv = vertid_[i].size();
if (vertweight_[i].size() != nbv || nbv == 0) {
throw mjCError(this, "vertid and vertweight must have same non-zero size in skin");
}
// accumulate weights in global array
for (int j=0; j < nbv; j++) {
// get index and check range
int jj = vertid_[i][j];
if (jj < 0 || jj >= nvert) {
throw mjCError(this, "vertid %d out of range in skin", nullptr, jj);
}
// accumulate
vw[jj] += vertweight_[i][j];
}
}
// check coverage
for (int i=0; i < nvert; i++) {
if (vw[i] <= mjMINVAL) {
throw mjCError(this, "vertex %d must have positive total weight in skin", nullptr, i);
}
}
// normalize vertex weights
for (int i=0; i < nbone; i++) {
for (int j=0; j < vertid_[i].size(); j++) {
vertweight_[i][j] /= vw[vertid_[i][j]];
}
}
// normalize bindquat
for (int i=0; i < nbone; i++) {
double quat[4] = {
(double)bindquat_[4*i],
(double)bindquat_[4*i+1],
(double)bindquat_[4*i+2],
(double)bindquat_[4*i+3]
};
mjuu_normvec(quat, 4);
bindquat_[4*i] = (float) quat[0];
bindquat_[4*i+1] = (float) quat[1];
bindquat_[4*i+2] = (float) quat[2];
bindquat_[4*i+3] = (float) quat[3];
}
}
// load skin in SKN BIN format
void mjCSkin::LoadSKN(mjResource* resource) {
char* buffer = 0;
int buffer_sz = mju_readResource(resource, (const void**)&buffer);
if (buffer_sz < 0) {
throw mjCError(this, "could not read SKN file '%s'", resource->name);
} else if (!buffer_sz) {
throw mjCError(this, "SKN file '%s' is empty", resource->name);
}
// make sure header is present
if (buffer_sz < 16) {
throw mjCError(this, "missing header in SKN file '%s'", resource->name);
}
// get sizes from header
int nvert = ((int*)buffer)[0];
int ntexcoord = ((int*)buffer)[1];
int nface = ((int*)buffer)[2];
int nbone = ((int*)buffer)[3];
// negative sizes not allowed
if (nvert < 0 || ntexcoord < 0 || nface < 0 || nbone < 0) {
throw mjCError(this, "negative size in header of SKN file '%s'", resource->name);
}
// make sure we have data for vert, texcoord, face
if (buffer_sz < 16 + 12*nvert + 8*ntexcoord + 12*nface) {
throw mjCError(this, "insufficient data in SKN file '%s'", resource->name);
}
// data pointer and counter
float* pdata = (float*)(buffer+16);
int cnt = 0;
// copy vert
if (nvert) {
vert_.resize(3*nvert);
memcpy(vert_.data(), pdata+cnt, 3*nvert*sizeof(float));
cnt += 3*nvert;
}
// copy texcoord
if (ntexcoord) {
texcoord_.resize(2*ntexcoord);
memcpy(texcoord_.data(), pdata+cnt, 2*ntexcoord*sizeof(float));
cnt += 2*ntexcoord;
}
// copy face
if (nface) {
face_.resize(3*nface);
memcpy(face_.data(), pdata+cnt, 3*nface*sizeof(int));
cnt += 3*nface;
}
// allocate bone arrays
bodyname_.clear();
bindpos_.resize(3*nbone);
bindquat_.resize(4*nbone);
vertid_.resize(nbone);
vertweight_.resize(nbone);
// read bones
for (int i=0; i < nbone; i++) {
// check size
if (buffer_sz/4-4-cnt < 18) {
throw mjCError(this, "insufficient data in SKN file '%s', bone %d", resource->name, i);
}
// read name
char txt[40];
strncpy(txt, (char*)(pdata+cnt), 39);
txt[39] = '\0';
cnt += 10;
bodyname_.push_back(txt);
// read bindpos
memcpy(bindpos_.data()+3*i, pdata+cnt, 3*sizeof(float));
cnt += 3;
// read bind quat
memcpy(bindquat_.data()+4*i, pdata+cnt, 4*sizeof(float));
cnt += 4;
// read vertex count
int vcount = *(int*)(pdata+cnt);
cnt += 1;
// check for negative
if (vcount < 1) {
throw mjCError(this, "vertex count must be positive in SKN file '%s', bone %d",
resource->name, i);
}
// check size
if (buffer_sz/4-4-cnt < 2*vcount) {
throw mjCError(this, "insufficient vertex data in SKN file '%s', bone %d",
resource->name, i);
}
// read vertid
vertid_[i].resize(vcount);
memcpy(vertid_[i].data(), (int*)(pdata+cnt), vcount*sizeof(int));
cnt += vcount;
// read vertweight
vertweight_[i].resize(vcount);
memcpy(vertweight_[i].data(), (int*)(pdata+cnt), vcount*sizeof(int));
cnt += vcount;
}
// check final size
if (buffer_sz != 16+4*cnt) {
throw mjCError(this, "unexpected buffer size in SKN file '%s'", resource->name);
}
}
//-------------------------- nonlinear elasticity --------------------------------------------------
// simplex connectivity
constexpr int eledge[3][6][2] = {{{ 0, 1}, {-1, -1}, {-1, -1},
{-1, -1}, {-1, -1}, {-1, -1}},
{{ 1, 2}, { 2, 0}, { 0, 1},
{-1, -1}, {-1, -1}, {-1, -1}},
{{ 0, 1}, { 1, 2}, { 2, 0},
{ 2, 3}, { 0, 3}, { 1, 3}}};
struct Stencil2D {
static constexpr int kNumEdges = 3;
static constexpr int kNumVerts = 3;
static constexpr int kNumFaces = 2;
static constexpr int edge[kNumEdges][2] = {{1, 2}, {2, 0}, {0, 1}};
static constexpr int face[kNumVerts][2] = {{1, 2}, {2, 0}, {0, 1}};
static constexpr int edge2face[kNumEdges][2] = {{1, 2}, {2, 0}, {0, 1}};
int vertices[kNumVerts];
int edges[kNumEdges];
};
struct Stencil3D {
static constexpr int kNumEdges = 6;
static constexpr int kNumVerts = 4;
static constexpr int kNumFaces = 3;
static constexpr int edge[kNumEdges][2] = {{0, 1}, {1, 2}, {2, 0},
{2, 3}, {0, 3}, {1, 3}};
static constexpr int face[kNumVerts][3] = {{2, 1, 0}, {0, 1, 3},
{1, 2, 3}, {2, 0, 3}};
static constexpr int edge2face[kNumEdges][2] = {{2, 3}, {1, 3}, {2, 1},
{1, 0}, {0, 2}, {0, 3}};
int vertices[kNumVerts];
int edges[kNumEdges];
};
template <typename T>
inline double ComputeVolume(const double* x, const int v[T::kNumVerts]);
template <>
inline double ComputeVolume<Stencil2D>(const double* x,
const int v[Stencil2D::kNumVerts]) {
double normal[3];
const double* x0 = x + 3*v[0];
const double* x1 = x + 3*v[1];
const double* x2 = x + 3*v[2];
double edge1[3] = {x1[0]-x0[0], x1[1]-x0[1], x1[2]-x0[2]};
double edge2[3] = {x2[0]-x0[0], x2[1]-x0[1], x2[2]-x0[2]};
mjuu_crossvec(normal, edge1, edge2);
return mjuu_normvec(normal, 3) / 2;
}
template<>
inline double ComputeVolume<Stencil3D>(const double* x,
const int v[Stencil3D::kNumVerts]) {
double normal[3];
const double* x0 = x + 3*v[0];
const double* x1 = x + 3*v[1];
const double* x2 = x + 3*v[2];
const double* x3 = x + 3*v[3];
double edge1[3] = {x1[0]-x0[0], x1[1]-x0[1], x1[2]-x0[2]};
double edge2[3] = {x2[0]-x0[0], x2[1]-x0[1], x2[2]-x0[2]};
double edge3[3] = {x3[0]-x0[0], x3[1]-x0[1], x3[2]-x0[2]};
mjuu_crossvec(normal, edge1, edge2);
return mjuu_dot3(normal, edge3) / 6;
}
// compute metric tensor of edge lengths inner product
template <typename T>
void inline MetricTensor(double* metric, int idx, double mu,
double la, const double basis[T::kNumEdges][9]) {
double trE[T::kNumEdges] = {0};
double trEE[T::kNumEdges*T::kNumEdges] = {0};
double k[T::kNumEdges*T::kNumEdges];
// compute first invariant i.e. trace(strain)
for (int e = 0; e < T::kNumEdges; e++) {
for (int i = 0; i < 3; i++) {
trE[e] += basis[e][4*i];
}
}
// compute second invariant i.e. trace(strain^2)
for (int ed1 = 0; ed1 < T::kNumEdges; ed1++) {
for (int ed2 = 0; ed2 < T::kNumEdges; ed2++) {
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
trEE[T::kNumEdges*ed1+ed2] += basis[ed1][3*i+j] * basis[ed2][3*j+i];
}
}
}
}
// assembly of strain metric tensor
for (int ed1 = 0; ed1 < T::kNumEdges; ed1++) {
for (int ed2 = 0; ed2 < T::kNumEdges; ed2++) {
k[T::kNumEdges*ed1 + ed2] = mu * trEE[T::kNumEdges * ed1 + ed2] +
la * trE[ed2] * trE[ed1];
}
}
// copy to triangular representation
int id = 0;
for (int ed1 = 0; ed1 < T::kNumEdges; ed1++) {
for (int ed2 = ed1; ed2 < T::kNumEdges; ed2++) {
metric[21*idx + id++] = k[T::kNumEdges*ed1 + ed2];
}
}
if (id != T::kNumEdges*(T::kNumEdges+1)/2) {
mju_error("incorrect stiffness matrix size");
}
}
// compute local basis
template <typename T>
void inline ComputeBasis(double basis[9], const double* x,
const int v[T::kNumVerts],
const int faceL[T::kNumFaces],
const int faceR[T::kNumFaces], double volume);
template <>
void inline ComputeBasis<Stencil2D>(double basis[9], const double* x,
const int v[Stencil2D::kNumVerts],
const int faceL[Stencil2D::kNumFaces],
const int faceR[Stencil2D::kNumFaces],
double volume) {
double basisL[3], basisR[3];
double normal[3];
const double* xL0 = x + 3*v[faceL[0]];
const double* xL1 = x + 3*v[faceL[1]];
const double* xR0 = x + 3*v[faceR[0]];
const double* xR1 = x + 3*v[faceR[1]];
double edgesL[3] = {xL0[0]-xL1[0], xL0[1]-xL1[1], xL0[2]-xL1[2]};
double edgesR[3] = {xR1[0]-xR0[0], xR1[1]-xR0[1], xR1[2]-xR0[2]};
mjuu_crossvec(normal, edgesR, edgesL);
mjuu_normvec(normal, 3);
mjuu_crossvec(basisL, normal, edgesL);
mjuu_crossvec(basisR, edgesR, normal);
// we use as basis the symmetrized tensor products of the edge normals of the
// other two edges; this is shown in Weischedel "A discrete geometric view on
// shear-deformable shell models" in the remark at the end of section 4.1;
// equivalent to linear finite elements but in a coordinate-free formulation.
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
basis[3*i+j] = (basisL[i]*basisR[j] +
basisR[i]*basisL[j]) / (8*volume*volume);
}
}
}
// compute local basis
template <>
void inline ComputeBasis<Stencil3D>(double basis[9], const double* x,
const int v[Stencil3D::kNumVerts],
const int faceL[Stencil3D::kNumFaces],
const int faceR[Stencil3D::kNumFaces],
double volume) {
const double* xL0 = x + 3*v[faceL[0]];
const double* xL1 = x + 3*v[faceL[1]];
const double* xL2 = x + 3*v[faceL[2]];
const double* xR0 = x + 3*v[faceR[0]];
const double* xR1 = x + 3*v[faceR[1]];
const double* xR2 = x + 3*v[faceR[2]];
double edgesL[6] = {xL1[0] - xL0[0], xL1[1] - xL0[1], xL1[2] - xL0[2],
xL2[0] - xL0[0], xL2[1] - xL0[1], xL2[2] - xL0[2]};
double edgesR[6] = {xR1[0] - xR0[0], xR1[1] - xR0[1], xR1[2] - xR0[2],
xR2[0] - xR0[0], xR2[1] - xR0[1], xR2[2] - xR0[2]};
double normalL[3], normalR[3];
mjuu_crossvec(normalL, edgesL, edgesL+3);
mjuu_crossvec(normalR, edgesR, edgesR+3);
// we use as basis the symmetrized tensor products of the area normals of the
// two faces not adjacent to the edge; this is the 3D equivalent to the basis
// proposed in Weischedel "A discrete geometric view on shear-deformable shell
// models" in the remark at the end of section 4.1. This is also equivalent to
// linear finite elements but in a coordinate-free formulation.
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
basis[3*i+j] = (normalL[i]*normalR[j] +
normalR[i]*normalL[j]) / (36*2*volume*volume);
}
}
}
// compute stiffness for a single element
template <typename T>
void inline ComputeStiffness(std::vector<double>& stiffness,
const std::vector<double>& body_pos,
const int* v, int t, double E,
double nu, double thickness = 4) {
// triangles area
double volume = ComputeVolume<T>(body_pos.data(), v);
// material parameters
double mu = E / (2*(1+nu)) * std::abs(volume) / 4 * thickness;
double la = E*nu / ((1+nu)*(1-2*nu)) * std::abs(volume) / 4 * thickness;
// local geometric quantities
double basis[T::kNumEdges][9] = {{0}};
// compute edge basis
for (int e = 0; e < T::kNumEdges; e++) {
ComputeBasis<T>(basis[e], body_pos.data(), v,
T::face[T::edge2face[e][0]],
T::face[T::edge2face[e][1]], volume);
}
// compute metric tensor
MetricTensor<T>(stiffness.data(), t, mu, la, basis);
}
// local tetrahedron numbering
constexpr int kNumEdges = Stencil2D::kNumEdges;
constexpr int kNumVerts = Stencil2D::kNumVerts;
constexpr int edge[kNumEdges][2] = {{1, 2}, {2, 0}, {0, 1}};
// create map from triangles to vertices and edges and from edges to vertices
static void CreateFlapStencil(std::vector<StencilFlap>& flaps,
const std::vector<int>& simplex,
const std::vector<int>& edgeidx) {
// populate stencil
int ne = 0;
int nt = simplex.size() / kNumVerts;
std::vector<Stencil2D> elements(nt);
for (int t = 0; t < nt; t++) {
for (int v = 0; v < kNumVerts; v++) {
elements[t].vertices[v] = simplex[kNumVerts * t + v];
}
}
// map from edge vertices to their index in `edges` vector
std::unordered_map<std::pair<int, int>, int, PairHash> edge_indices;
// loop over all triangles
for (int t = 0; t < nt; t++) {
int* v = elements[t].vertices;
// compute edges to vertices map for fast computations
for (int e = 0; e < kNumEdges; e++) {
auto pair = std::pair(std::min(v[edge[e][0]], v[edge[e][1]]),
std::max(v[edge[e][0]], v[edge[e][1]]));
// if edge is already present in the vector only store its index
auto [it, inserted] = edge_indices.insert({pair, ne});
if (inserted) {
StencilFlap flap;
flap.vertices[0] = v[edge[e][0]];
flap.vertices[1] = v[edge[e][1]];
flap.vertices[2] = v[(edge[e][1] + 1) % 3];
flap.vertices[3] = -1;
flaps.push_back(flap);
elements[t].edges[e] = ne++;
} else {
elements[t].edges[e] = it->second;
flaps[it->second].vertices[3] = v[(edge[e][1] + 1) % 3];
}
// double check that the edge indices are consistent
if (!edgeidx.empty()) {
if (elements[t].edges[e] != edgeidx[kNumEdges * t + e]) {
mju_error("edge indices do not match in CreateFlapStencil");
}
}
}
}
}
// cotangent between two edges
double inline cot(const double* x, int v0, int v1, int v2) {
double normal[3];
double edge1[3] = {x[3*v1]-x[3*v0], x[3*v1+1]-x[3*v0+1], x[3*v1+2]-x[3*v0+2]};
double edge2[3] = {x[3*v2]-x[3*v0], x[3*v2+1]-x[3*v0+1], x[3*v2+2]-x[3*v0+2]};
mjuu_crossvec(normal, edge1, edge2);
return mjuu_dot3(edge1, edge2) / sqrt(mjuu_dot3(normal, normal));
}
// area of a triangle
double inline ComputeVolume(const double* x, const int v[Stencil2D::kNumVerts]) {
double normal[3];
double edge1[3] = {x[3*v[1]]-x[3*v[0]], x[3*v[1]+1]-x[3*v[0]+1], x[3*v[1]+2]-x[3*v[0]+2]};
double edge2[3] = {x[3*v[2]]-x[3*v[0]], x[3*v[2]+1]-x[3*v[0]+1], x[3*v[2]+2]-x[3*v[0]+2]};
mjuu_crossvec(normal, edge1, edge2);
return sqrt(mjuu_dot3(normal, normal)) / 2;
}
// compute bending stiffness for a single edge
template <typename T>
void inline ComputeBending(double* bending, double* pos, const int v[4], double mu,
double thickness) {
int vadj[3] = {v[1], v[0], v[3]};
if (v[3]== -1) {
// skip boundary edges
return;
}
// cotangent operator from Wardetzky at al., "Discrete Quadratic Curvature
// Energies", https://cims.nyu.edu/gcl/papers/wardetzky2007dqb.pdf
double a01 = cot(pos, v[0], v[1], v[2]);
double a02 = cot(pos, v[0], v[3], v[1]);
double a03 = cot(pos, v[1], v[2], v[0]);
double a04 = cot(pos, v[1], v[0], v[3]);
double c[4] = {a03 + a04, a01 + a02, -(a01 + a03), -(a02 + a04)};
double volume = ComputeVolume(pos, v) + ComputeVolume(pos, vadj);
double stiffness = 3 * mu * pow(thickness, 3) / (24 * volume);
// Garg et al., "Cubic Shells", https://cims.nyu.edu/gcl/papers/garg2007cs.pdf
const double* v0 = pos + 3*v[0];
const double* v1 = pos + 3*v[1];
const double* v2 = pos + 3*v[2];
const double* v3 = pos + 3*v[3];
double e0[3] = {v1[0] - v0[0], v1[1] - v0[1], v1[2] - v0[2]};
double e1[3] = {v2[0] - v0[0], v2[1] - v0[1], v2[2] - v0[2]};
double e2[3] = {v3[0] - v0[0], v3[1] - v0[1], v3[2] - v0[2]};
double e3[3] = {v2[0] - v1[0], v2[1] - v1[1], v2[2] - v1[2]};
double e4[3] = {v3[0] - v1[0], v3[1] - v1[1], v3[2] - v1[2]};
double t0[3] = {-(a03*e1[0] + a01*e3[0]), -(a03*e1[1] + a01*e3[1]), -(a03*e1[2] + a01*e3[2])};
double t1[3] = {-(a04*e2[0] + a02*e4[0]), -(a04*e2[1] + a02*e4[1]), -(a04*e2[2] + a02*e4[2])};
double sqr = mjuu_dot3(e0, e0);
double cos_theta = -mjuu_dot3(t0, t1) / sqr;
for (int v1 = 0; v1 < T::kNumVerts; v1++) {
for (int v2 = 0; v2 < T::kNumVerts; v2++) {
bending[4 * v1 + v2] += c[v1] * c[v2] * cos_theta * stiffness;
}
}
double n[3];
mjuu_crossvec(n, e0, e1);
bending[16] = mjuu_dot3(n, e2) * (a01 - a03) * (a04 - a02) * stiffness / (sqr * sqrt(sqr));
}
//----------------------------- linear elasticity --------------------------------------------------
// Gauss Legendre quadrature points in 1 dimension on the interval [a, b]
void quadratureGaussLegendre(double* points, double* weights,
const int order, const double a, const double b) {
if (order > 3)
mju_error("Integration order > 3 not yet supported.");
// x is on [-1, 1], p on [a, b]
double p0 = (a+b)/2.;
double dpdx = (b-a)/2;
if (order == 2) {
points[0] = -dpdx / sqrt(3) + p0;
points[1] = dpdx / sqrt(3) + p0;
weights[0] = dpdx;
weights[1] = dpdx;
} else {
points[0] = p0;
points[1] = -dpdx / sqrt(3. / 5.) + p0;
points[2] = dpdx / sqrt(3. / 5.) + p0;
weights[0] = 8. / 9. * dpdx;
weights[1] = 5. / 9. * dpdx;
weights[2] = 5. / 9. * dpdx;
}
}
// evaluate 1-dimensional basis function
double phi(const double s, const int i, const int order) {
if (order == 1) {
return i == 0 ? 1 - s : s;
} else if (order == 2) {
switch (i) {
case 0:
return 2 * s * s - 3 * s + 1;
case 1:
return 4 * (s - s * s);
case 2:
return 2 * s * s - s;
default:
mjERROR("invalid index %d", i);
return 0;
}
} else {
mju_error("Order must be 1 or 2.");
return 0;
}
}
// evaluate gradient of 1-dimensional basis function
double dphi(const double s, const int i, const int order) {
if (order == 1) {
return i == 0 ? -1 : 1;
} else if (order == 2) {
switch (i) {
case 0:
return 4 * s - 3;
case 1:
return 4 * (1 - 2 * s);
case 2:
return 4 * s - 1;
default:
mjERROR("invalid index %d, must be 0, 1, or 2", i);
return 0;
}
} else {
mju_error("Order must be 1 or 2.");
return 0;
}
}
typedef std::array<std::array<double, 3>, 3> Matrix;
// symmetrize a tensor
Matrix inline sym(const Matrix& tensor) {
Matrix eps;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
eps[i][j] = (tensor[i][j] + tensor[j][i]) / 2;
}
}
return eps;
}
// compute tensor inner product
Matrix inline inner(const Matrix& tensor1, const Matrix& tensor2) {
Matrix inner;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
inner[i][j] = tensor1[i][0] * tensor2[0][j] +
tensor1[i][1] * tensor2[1][j] +
tensor1[i][2] * tensor2[2][j];
}
}
return inner;
}
// compute trace of a tensor
double inline trace(const Matrix& tensor) {
return tensor[0][0] + tensor[1][1] + tensor[2][2];
}
void inline ComputeLinearStiffness(std::vector<double>& K,
const double* pos,
double E, double nu, int order) {
int nbasis = order + 1;
int n = pow(nbasis, 3);
int ndof = 3*n;
// compute quadrature points
std::vector<double> points(nbasis); // quadrature points
std::vector<double> weight(nbasis); // quadrature weights
quadratureGaussLegendre(points.data(), weight.data(), nbasis, 0, 1);
// compute element transformation
double dx = (pos+3*(n-1))[0] - pos[0];
double dy = (pos+3*(n-1))[1] - pos[1];
double dz = (pos+3*(n-1))[2] - pos[2];
double detJ = dx * dy * dz;
double invJ[3] = {1.0 / dx, 1.0 / dy, 1.0 / dz};
// compute stiffness matrix
std::vector<std::array<double, 3> > F(n);
double la = E * nu / (1 + nu) / (1 - 2 * nu);
double mu = E / (2 * (1 + nu));
// loop over quadrature points
for (int ps=0; ps < nbasis; ps++) {
for (int pt=0; pt < nbasis; pt++) {
for (int pu=0; pu < nbasis; pu++) {
double s = points[ps];
double t = points[pt];
double u = points[pu];
double dvol = weight[ps] * weight[pt] * weight[pu] * detJ;
int dof = 0;
// cartesian product of basis functions
for (int bx=0; bx < nbasis; bx++) {
for (int by=0; by < nbasis; by++) {
for (int bz=0; bz < nbasis; bz++) {
std::array<double, 3> gradient;
gradient[0] = dphi(s, bx, order) * phi(t, by, order) * phi(u, bz, order);
gradient[1] = phi(s, bx, order) * dphi(t, by, order) * phi(u, bz, order);
gradient[2] = phi(s, bx, order) * phi(t, by, order) * dphi(u, bz, order);
F[dof++] = gradient;
}
}
}
if (dof != n) { // SHOULD NOT OCCUR
throw mjCError(nullptr, "incorrect number of basis functions");
}
// tensor contraction of the gradients of elastic strains
// (d(F+F')/dx : d(F+F')/dx)
for (int i=0; i < n; i++) {
for (int j=0; j < n; j++) {
Matrix du;
Matrix dv;
du.fill({0, 0, 0});
dv.fill({0, 0, 0});
for (int k=0; k < 3; k++) {
for (int l=0; l < 3; l++) {
du[k][0] = invJ[0] * F[i][0];
du[k][1] = invJ[1] * F[i][1];
du[k][2] = invJ[2] * F[i][2];
dv[l][0] = invJ[0] * F[j][0];
dv[l][1] = invJ[1] * F[j][1];
dv[l][2] = invJ[2] * F[j][2];
K[ndof*(3*i+k) + 3*j+l] -= la * trace(du) * trace(dv) * dvol;
K[ndof*(3*i+k) + 3*j+l] -= mu * trace(inner(sym(du), sym(dv))) * dvol;
mjuu_zerovec(du[k].data(), 3);
mjuu_zerovec(dv[l].data(), 3);
}
}
}
}
}
}
}
}
// Eigendecompose cell stiffness matrix and store scaled eigenvectors.
// K_cell is n×n stored (negative convention: K_stored = -K_physical).
// Output layout in `out`:
// [0]: neig (as double)
// [1 .. neig*n]: sqrt(λ_phys_i) * v_i, row-major
// Returns number of retained eigenmodes.
static int EigendecomposeStiffness(const double* K_cell_data,
double* out, int ndof) {
// copy K_cell for in-place decomposition
std::vector<double> mat(K_cell_data, K_cell_data + ndof * ndof);
std::vector<double> eigval(ndof);
std::vector<double> eigvec(ndof * ndof);
mjuu_eigendecompose(mat.data(), eigval.data(), eigvec.data(), ndof);
// K_stored = -K_physical, so physical eigenvalue = -eigval[i]
// retain modes where physical eigenvalue > threshold
double max_eigval = 0;
for (int i = 0; i < ndof; i++) {
max_eigval = std::max(max_eigval, std::abs(eigval[i]));
}
double threshold = max_eigval * 1e-8;
int neig = 0;
for (int i = 0; i < ndof; i++) {
double lambda_phys = -eigval[i]; // negate to get physical eigenvalue
if (lambda_phys > threshold) {
// store sqrt(λ) * eigenvector (column i of eigvec matrix)
double scale = std::sqrt(lambda_phys);
for (int j = 0; j < ndof; j++) {
out[1 + neig * ndof + j] = scale * eigvec[j * ndof + i];
}
neig++;
}
}
out[0] = static_cast<double>(neig);
return neig;
}
//------------------ class mjCFlex implementation --------------------------------------------------
// constructor
mjCFlex::mjCFlex(mjCModel* _model) {
mjs_defaultFlex(&spec);
elemtype = mjOBJ_FLEX;
// set model
model = _model;
if (_model) compiler = &_model->spec.compiler;
// clear internal variables
nvert = 0;
nnode = 0;
nedge = 0;
nelem = 0;
matid = -1;
rigid = false;
centered = false;
PointToLocal();
CopyFromSpec();
}
mjCFlex::mjCFlex(const mjCFlex& other) {
*this = other;
}
mjCFlex& mjCFlex::operator=(const mjCFlex& other) {
if (this != &other) {
this->spec = other.spec;
*static_cast<mjCFlex_*>(this) = static_cast<const mjCFlex_&>(other);
*static_cast<mjsFlex*>(this) = static_cast<const mjsFlex&>(other);
}
PointToLocal();
return *this;
}
void mjCFlex::PointToLocal() {
spec.element = static_cast<mjsElement*>(this);
spec.material = &spec_material_;
spec.vertbody = &spec_vertbody_;
spec.nodebody = &spec_nodebody_;
spec.vert = &spec_vert_;
spec.node = &spec_node_;
spec.texcoord = &spec_texcoord_;
spec.elemtexcoord = &spec_elemtexcoord_;
spec.elem = &spec_elem_;
spec.info = &info;
material = nullptr;
vertbody = nullptr;
nodebody = nullptr;
vert = nullptr;
node = nullptr;
texcoord = nullptr;
elemtexcoord = nullptr;
elem = nullptr;
}
void mjCFlex::NameSpace(const mjCModel* m) {
mjCBase::NameSpace(m);
for (auto& name : spec_vertbody_) {
name = m->prefix + name + m->suffix;
}
for (auto& name : spec_nodebody_) {
name = m->prefix + name + m->suffix;
}
if (!spec_material_.empty() && model != m) {
spec_material_ = m->prefix + spec_material_ + m->suffix;
}
}
void mjCFlex::CopyFromSpec() {
*static_cast<mjsFlex*>(this) = spec;
spec.info = &info;
material_ = spec_material_;
vertbody_ = spec_vertbody_;
nodebody_ = spec_nodebody_;
vert_ = spec_vert_;
node_ = spec_node_;
texcoord_ = spec_texcoord_;
elemtexcoord_ = spec_elemtexcoord_;
elem_ = spec_elem_;
// clear precompiled asset. TODO: use asset cache
nedge = 0;
edge.clear();
shell.clear();
evpair.clear();
}
bool mjCFlex::HasTexcoord() const {
return !texcoord_.empty();
}
void mjCFlex::DelTexcoord() {
texcoord_.clear();
}
void mjCFlex::ResolveReferences(const mjCModel* m) {
vertbodyid.clear();
nodebodyid.clear();
for (const auto& vertbody : vertbody_) {
mjCBody* pbody = static_cast<mjCBody*>(m->FindObject(mjOBJ_BODY, vertbody));
if (pbody) {
vertbodyid.push_back(pbody->id);
if (pbody->joints.size() != 3 && dim == 2 && (elastic2d == 1 || elastic2d == 3)) {
// TODO(quaglino): add support for pins
throw mjCError(this, "pins are not supported for bending");
}
} else {
throw mjCError(this, "unknown body '%s' in flex", vertbody.c_str());
}
}
for (const auto& nodebody : nodebody_) {
mjCBase* pbody = m->FindObject(mjOBJ_BODY, nodebody);
if (pbody) {
nodebodyid.push_back(pbody->id);
} else {
throw mjCError(this, "unknown body '%s' in flex", nodebody.c_str());
}
}
}
std::string mjCFlex::ComputeStiffnessCacheKey() const {
std::size_t hash = 0;
auto combine = [&hash](std::size_t v) {
hash ^= v + 0x9e3779b9 + (hash << 6) + (hash >> 2);
};
combine(std::hash<double>{}(young));
combine(std::hash<double>{}(poisson));
combine(std::hash<int>{}(spec.order));
combine(std::hash<int>{}(spec.cellcount[0]));
combine(std::hash<int>{}(spec.cellcount[1]));
combine(std::hash<int>{}(spec.cellcount[2]));
// compute bounding box from vertex positions
if (!vert_.empty()) {
double minx = vert_[0], maxx = vert_[0];
double miny = vert_[1], maxy = vert_[1];
double minz = vert_[2], maxz = vert_[2];
for (std::size_t i = 3; i < vert_.size(); i += 3) {
minx = std::min(minx, vert_[i]);
maxx = std::max(maxx, vert_[i]);
miny = std::min(miny, vert_[i + 1]);
maxy = std::max(maxy, vert_[i + 1]);
minz = std::min(minz, vert_[i + 2]);
maxz = std::max(maxz, vert_[i + 2]);
}
combine(std::hash<double>{}(maxx - minx));
combine(std::hash<double>{}(maxy - miny));
combine(std::hash<double>{}(maxz - minz));
}
for (std::size_t i = 0; i < vert_.size(); i += std::max(1, (int)vert_.size()/100)) {
combine(std::hash<double>{}(vert_[i]));
}
for (std::size_t i = 0; i < shell.size(); i += std::max(1, (int)shell.size()/50)) {
combine(std::hash<int>{}(shell[i]));
}
return "flex_stiffness:" + std::to_string(hash);
}
bool mjCFlex::LoadCachedStiffness() {
mjCCache* cache = reinterpret_cast<mjCCache*>(mj_getCache()->impl_);
if (!cache) return false;
std::string key = ComputeStiffnessCacheKey();
auto load_fn = [this](const void* data) {
const auto* cached = static_cast<const std::vector<double>*>(data);
stiffness = *cached;
return true;
};
mjResource dummy_resource{};
dummy_resource.name = const_cast<char*>(key.c_str());
dummy_resource.timestamp[0] = '\0';
return cache->PopulateData(key, &dummy_resource, load_fn);
}
void mjCFlex::CacheStiffness() {
mjCCache* cache = reinterpret_cast<mjCCache*>(mj_getCache()->impl_);
if (!cache || stiffness.empty()) return;
std::string key = ComputeStiffnessCacheKey();
auto* cached = new std::vector<double>(stiffness);
std::size_t size = sizeof(*cached) + sizeof(double) * stiffness.size();
std::shared_ptr<const void> cached_data(cached, [](const void* data) {
delete static_cast<const std::vector<double>*>(data);
});
mjResource dummy_resource{};
dummy_resource.name = const_cast<char*>(key.c_str());
dummy_resource.timestamp[0] = '\0';
cache->Insert("", key, &dummy_resource, cached_data, size);
}
// compiler
void mjCFlex::Compile(const mjVFS* vfs) {
CopyFromSpec();
interpolated = !nodebody_.empty();
// set nelem; check sizes
if (dim < 1 || dim > 3) {
throw mjCError(this, "dim must be 1, 2 or 3");
}
if (elem_.empty()) {
throw mjCError(this, "elem is empty");
}
if (elem_.size() % (dim+1)) {
throw mjCError(this, "elem size must be multiple of (dim+1)");
}
if (vertbody_.empty() && !interpolated) {
throw mjCError(this, "vertbody and nodebody are both empty");
}
if (vert_.size() % 3) {
throw mjCError(this, "vert size must be a multiple of 3");
}
if (edgestiffness > 0 && dim > 1) {
throw mjCError(this, "edge stiffness only available for dim=1, please use elasticity plugins");
}
if (interpolated && selfcollide != mjFLEXSELF_NONE) {
throw mjCError(this, "trilinear interpolation cannot do self-collision");
}
if (interpolated && internal) {
throw mjCError(this, "trilinear interpolation cannot do internal collisions");
}
nelem = (int)elem_.size()/(dim+1);
// elastic2d checks
if (elastic2d) {
if (thickness <= 0) {
throw mjCError(this, "2d elasticity requires positive thickness");
}
if (interpolated) {
throw mjCError(this, "interpolated flex does not yet support 2d elasticity");
}
if (dim != 2 && !interpolated) {
throw mjCError(this, "2d elasticity requires 2d flex");
}
}
// set nvert, rigid, centered; check size
if (vert_.empty()) {
centered = true;
nvert = (int)vertbody_.size();
}
else {
nvert = (int)vert_.size()/3;
if (vertbody_.size() == 1) {
rigid = true;
} else if (vertbody_.size() != nvert) {
throw mjCError(this, "vertbody size must be 1 or nvert");
}
}
if (nvert < dim+1) {
throw mjCError(this, "not enough vertices");
}
// set nnode
nnode = static_cast<int>(nodebody_.size());
if (nnode && !spec.order) {
throw mjCError(this, "Interpolation order must be explicitly specified (dof is missing)");
}
// check node compatibility with count and dof
if (spec.order > 0) {
if (spec.cellcount[0] == 0 || spec.cellcount[1] == 0 || spec.cellcount[2] == 0) {
throw mjCError(this, "cellcount cannot be 0 in any dimension when interpolation order > 0");
}
int expected_nodes = (spec.cellcount[0] * spec.order + 1) *
(spec.cellcount[1] * spec.order + 1) *
(spec.cellcount[2] * spec.order + 1);
if (nnode != expected_nodes) {
std::string msg = "number of nodes (" + std::to_string(nnode) +
") does not match cellcount and dof expected (" +
std::to_string(expected_nodes) + ")";
throw mjCError(this, msg.c_str());
}
}
// check elem vertex ids
for (const auto& elem : elem_) {
if (elem < 0 || elem >= nvert) {
throw mjCError(this, "elem vertex id out of range");
}
}
// check texcoord
if (!texcoord_.empty() && texcoord_.size() != 2*nvert && elemtexcoord_.empty()) {
throw mjCError(this, "two texture coordinates per vertex expected");
}
// no elemtexcoord: copy from faces
if (elemtexcoord_.empty() && !texcoord_.empty()) {
elemtexcoord_.assign(3*nelem, 0);
memcpy(elemtexcoord_.data(), elem_.data(), 3*nelem*sizeof(int));
}
// resolve material name
mjCBase* pmat = model->FindObject(mjOBJ_MATERIAL, material_);
if (pmat) {
matid = pmat->id;
} else if (!material_.empty()) {
throw mjCError(this, "unknown material '%s' in flex", material_.c_str());
}
// resolve body ids
ResolveReferences(model);
// process elements
for (int e=0; e < (int)elem_.size()/(dim+1); e++) {
// make sorted copy of element
std::vector<int> el;
el.assign(elem_.begin()+e*(dim+1), elem_.begin()+(e+1)*(dim+1));
std::sort(el.begin(), el.end());
// check for repeated vertices
for (int k=0; k < dim; k++) {
if (el[k] == el[k+1]) {
throw mjCError(this, "repeated vertex in element");
}
}
}
// determine rigid if not already set
if (!rigid && !interpolated) {
rigid = true;
for (unsigned i=1; i < vertbodyid.size(); i++) {
if (vertbodyid[i] != vertbodyid[0]) {
rigid = false;
break;
}
}
}
// determine centered if not already set
if (!centered && !interpolated) {
centered = true;
for (const auto& vert : vert_) {
if (vert != 0) {
centered = false;
break;
}
}
}
if (!centered && interpolated) {
centered = true;
for (const auto& node : node_) {
if (node != 0) {
centered = false;
break;
}
}
}
// compute global vertex positions
vertxpos = std::vector<double> (3*nvert);
for (int i=0; i < nvert; i++) {
// get body id, set vertxpos = body.xpos0
int b = rigid ? vertbodyid[0] : vertbodyid[i];
mjuu_copyvec(vertxpos.data()+3*i, model->Bodies()[b]->xpos0, 3);
// add vertex offset within body if not centered
if (!centered || interpolated) {
double offset[3];
mjuu_rotVecQuat(offset, vert_.data()+3*i, model->Bodies()[b]->xquat0);
mjuu_addtovec(vertxpos.data()+3*i, offset, 3);
}
if (interpolated) {
// this should happen in ResolveReferences but we need a body id in this loop to compute
// the global vertex position, this is a hack since it is the id of the parent body
vertbodyid[i] = -1;
}
}
// compute global node positions
std::vector<double> nodexpos = std::vector<double> (3*nnode);
for (int i=0; i < nnode; i++) {
// get body id, set nodexpos = body.xpos0
int b = nodebodyid[i];
mjuu_copyvec(nodexpos.data()+3*i, model->Bodies()[b]->xpos0, 3);
// add node offset within body if not centered
if (!centered) {
double offset[3];
mjuu_rotVecQuat(offset, node_.data()+3*i, model->Bodies()[b]->xquat0);
mjuu_addtovec(nodexpos.data()+3*i, offset, 3);
}
}
// compute unrotated node positions for stiffness computation
std::vector<double> nodexpos_local = ComputeUnrotatedNodePositions(nodexpos);
// reorder tetrahedra so right-handed face orientation is outside
// faces are (0,1,2); (0,2,3); (0,3,1); (1,3,2)
if (dim == 3) {
for (int e=0; e < nelem; e++) {
const int* edata = elem_.data() + e*(dim+1);
double* v0 = vertxpos.data() + 3*edata[0];
double* v1 = vertxpos.data() + 3*edata[1];
double* v2 = vertxpos.data() + 3*edata[2];
double* v3 = vertxpos.data() + 3*edata[3];
double v01[3] = {v1[0]-v0[0], v1[1]-v0[1], v1[2]-v0[2]};
double v02[3] = {v2[0]-v0[0], v2[1]-v0[1], v2[2]-v0[2]};
double v03[3] = {v3[0]-v0[0], v3[1]-v0[1], v3[2]-v0[2]};
// detect wrong orientation
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];
elem_[e*(dim+1)+2] = tmp;
}
}
}
// create edges
edgeidx_.assign(elem_.size()*kNumEdges[dim-1]/(dim+1), 0);
// map from edge vertices to their index in `edges` vector
std::unordered_map<std::pair<int, int>, int, PairHash> edge_indices;
// insert local edges into global vector
for (unsigned f = 0; f < elem_.size()/(dim+1); f++) {
int* v = elem_.data() + f*(dim+1);
for (int e = 0; e < kNumEdges[dim-1]; e++) {
auto pair = std::pair(
min(v[eledge[dim-1][e][0]], v[eledge[dim-1][e][1]]),
max(v[eledge[dim-1][e][0]], v[eledge[dim-1][e][1]]));
// if edge is already present in the vector only store its index
auto [it, inserted] = edge_indices.insert({pair, nedge});
if (inserted) {
edge.push_back(pair);
edgeidx_[f*kNumEdges[dim-1]+e] = nedge++;
} else {
edgeidx_[f*kNumEdges[dim-1]+e] = it->second;
}
}
}
// set size
nedge = (int)edge.size();
// create flap stencil
if (dim == 2) {
CreateFlapStencil(flaps, elem_, edgeidx_);
}
// compute elasticity
if (young > 0) {
if (poisson < 0 || poisson >= 0.5) {
throw mjCError(this, "Poisson ratio must be in [0, 0.5)");
}
// linear elasticity
if (!interpolated) {
stiffness.assign(21 * nelem, 0);
}
// geometrically nonlinear elasticity
for (unsigned int t = 0; t < nelem; t++) {
if (interpolated) {
continue;
}
if (dim == 2 && elastic2d >= 2 && thickness > 0) {
ComputeStiffness<Stencil2D>(stiffness, vertxpos,
elem_.data() + (dim + 1) * t, t, young,
poisson, thickness);
} else if (dim == 3) {
ComputeStiffness<Stencil3D>(stiffness, vertxpos,
elem_.data() + (dim + 1) * t, t, young,
poisson);
}
}
// bending stiffness (2D only)
if (dim == 2 && (elastic2d == 1 || elastic2d == 3)) {
bending.assign(nedge*17, 0);
for (unsigned int e = 0; e < nedge; e++) {
ComputeBending<StencilFlap>(bending.data() + 17 * e, vertxpos.data(), flaps[e].vertices,
young / (2 * (1 + poisson)), thickness);
}
}
}
// placeholder for setting plugins parameters, currently not used
for (const auto& vbodyid : vertbodyid) {
if (vbodyid < 0) {
continue;
}
if (model->Bodies()[vbodyid]->plugin.element) {
mjCPlugin* plugin_instance =
static_cast<mjCPlugin*>(model->Bodies()[vbodyid]->plugin.element);
if (!plugin_instance) {
throw mjCError(this, "plugin instance not found");
}
}
}
// create shell fragments and element-vertex collision pairs
CreateShellPair();
// compute linear stiffness for interpolated elements (cached)
bool stiffness_cached = false;
if (young > 0 && interpolated) {
stiffness_cached = LoadCachedStiffness();
}
if (!stiffness_cached && interpolated && (young > 0 || has_strain_eq)) {
// use young=1 for strain constraints (eigenvectors are geometry-only)
double K_young = has_strain_eq ? 1e1 : young;
double K_poisson = has_strain_eq ? 0.3 : poisson;
int npc = pow(spec.order + 1, 3); // nodes per cell
int ndof_cell = 3 * npc;
int cx = spec.cellcount[0], cy = spec.cellcount[1], cz = spec.cellcount[2];
int ncells = cx * cy * cz;
int ny_global = cy * spec.order + 1;
int nz_global = cz * spec.order + 1;
// total stiffness = ncells * ndof_cell^2
stiffness.resize(ncells * ndof_cell * ndof_cell, 0);
// compute stiffness per cell
for (int ci = 0; ci < cx; ci++) {
for (int cj = 0; cj < cy; cj++) {
for (int ck = 0; ck < cz; ck++) {
int cell_idx = ci * cy * cz + cj * cz + ck;
// skip stiffness computation for empty cells (no mesh content)
if (!cell_empty.empty() && cell_empty[cell_idx]) {
continue;
}
// gather cell's local node positions
std::vector<double> cell_pos(3 * npc);
int local = 0;
for (int li = 0; li <= spec.order; li++) {
for (int lj = 0; lj <= spec.order; lj++) {
for (int lk = 0; lk <= spec.order; lk++) {
int gi = ci * spec.order + li;
int gj = cj * spec.order + lj;
int gk = ck * spec.order + lk;
int global = gi * ny_global * nz_global + gj * nz_global + gk;
mjuu_copyvec(cell_pos.data() + 3*local, nodexpos_local.data() + 3*global, 3);
local++;
}
}
}
// compute per-cell stiffness
std::vector<double> K_cell(ndof_cell * ndof_cell, 0);
ComputeLinearStiffness(K_cell, cell_pos.data(), K_young, K_poisson, spec.order);
double* out = stiffness.data() + cell_idx * ndof_cell * ndof_cell;
if (has_strain_eq) {
// eigendecompose: store [neig, sqrt(λ)*v_1, sqrt(λ)*v_2, ...]
std::fill(out, out + ndof_cell * ndof_cell, 0.0);
EigendecomposeStiffness(K_cell.data(), out, ndof_cell);
} else {
// store raw K for passive forces
std::copy(K_cell.begin(), K_cell.end(), out);
}
}
}
}
}
// create bounding volume hierarchy
CreateBVH();
// compute bounding box coordinates
vert0_.assign(3*nvert, 0);
const mjtNum* bvh = tree.Bvh().data();
size[0] = bvh[3] - radius;
size[1] = bvh[4] - radius;
size[2] = bvh[5] - radius;
for (int j=0; j < nvert; j++) {
for (int k=0; k < 3; k++) {
if (size[k] > mjMINVAL) {
vert0_[3*j+k] = (vertxpos[3*j+k] - bvh[k]) / (2*size[k]) + 0.5;
} else {
vert0_[3*j+k] = 0.5;
}
}
}
// store node positions in unrotated (body-local) frame
// this ensures the runtime displacement refpos - R^{-1}*x is zero at rest
node0_.assign(3*nnode, 0);
for (int i=0; i < nnode; i++) {
mjuu_copyvec(node0_.data()+3*i, nodexpos_local.data()+3*i, 3);
}
}
// compute unrotated node positions for stiffness computation and node0_
//
// the runtime corotational code extracts rotation R from the deformation
// gradient and computes displacement as R^{-1}*x - refpos; at rest R = R0
// (the total grid rotation), so refpos must equal R0^{-1}*nodexpos to get
// zero displacement at rest; additionally, the stiffness eigenvectors must
// be computed from axis-aligned positions to preserve the diagonal Jacobian
// assumption in ComputeLinearStiffness.
std::vector<double> mjCFlex::ComputeUnrotatedNodePositions(
const std::vector<double>& nodexpos) const {
std::vector<double> nodexpos_local(3*nnode);
if (interpolated && nnode > 0) {
int ny_global = spec.cellcount[1] * spec.order + 1;
int nz_global = spec.cellcount[2] * spec.order + 1;
// find first non-empty cell
int cx = spec.cellcount[0], cy = spec.cellcount[1], cz = spec.cellcount[2];
int ref_ci = 0, ref_cj = 0, ref_ck = 0;
bool found = false;
for (int ci = 0; ci < cx && !found; ci++) {
for (int cj = 0; cj < cy && !found; cj++) {
for (int ck = 0; ck < cz && !found; ck++) {
int cell_idx = ci * cy * cz + cj * cz + ck;
if (cell_empty.empty() || !cell_empty[cell_idx]) {
ref_ci = ci; ref_cj = cj; ref_ck = ck;
found = true;
}
}
}
}
// corner indices of the reference cell (order=1 corners at local 0,0,0
// and at offsets along each parametric axis)
int g000 = (ref_ci * spec.order) * ny_global * nz_global +
(ref_cj * spec.order) * nz_global +
(ref_ck * spec.order);
int g100 = ((ref_ci * spec.order) + spec.order) * ny_global * nz_global +
(ref_cj * spec.order) * nz_global +
(ref_ck * spec.order);
int g010 = (ref_ci * spec.order) * ny_global * nz_global +
((ref_cj * spec.order) + spec.order) * nz_global +
(ref_ck * spec.order);
int g001 = (ref_ci * spec.order) * ny_global * nz_global +
(ref_cj * spec.order) * nz_global +
((ref_ck * spec.order) + spec.order);
// edge vectors (columns of the deformation gradient F = R * S)
// we store them as rows in R0 to use mjuu_mulvecmat for applying R0^{-1}
double R0[9];
for (int d = 0; d < 3; d++) {
R0[0+d] = nodexpos[3*g100 + d] - nodexpos[3*g000 + d];
R0[3+d] = nodexpos[3*g010 + d] - nodexpos[3*g000 + d];
R0[6+d] = nodexpos[3*g001 + d] - nodexpos[3*g000 + d];
}
// normalize to get rotation matrix columns (valid for regular grids)
double li = mjuu_normvec(R0+0, 3);
double lj = mjuu_normvec(R0+3, 3);
double lk = mjuu_normvec(R0+6, 3);
(void)li; (void)lj; (void)lk;
// apply inverse rotation to each nodexpos to get local-frame positions
for (int i = 0; i < nnode; i++) {
const double* p = nodexpos.data() + 3*i;
double* q = nodexpos_local.data() + 3*i;
mjuu_mulvecmat(q, p, R0);
}
} else {
nodexpos_local = nodexpos;
}
return nodexpos_local;
}
// create flex BVH
void mjCFlex::CreateBVH() {
int nbvh = 0;
// allocate element bounding boxes
elemaabb_.resize(6*nelem);
tree.AllocateBoundingVolumes(nelem);
// construct element bounding boxes, add to hierarchy
for (int e=0; e < nelem; e++) {
const int* edata = elem_.data() + e*(dim+1);
// skip inactive in 3D
if (dim == 3 && elemlayer[e] >= activelayers) {
continue;
}
// compute min and max along each global axis
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] = std::min(xmin[j], vertxpos[3*edata[i]+j]);
xmax[j] = std::max(xmax[j], vertxpos[3*edata[i]+j]);
}
}
// compute aabb (center, size)
elemaabb_[6*e+0] = 0.5*(xmax[0]+xmin[0]);
elemaabb_[6*e+1] = 0.5*(xmax[1]+xmin[1]);
elemaabb_[6*e+2] = 0.5*(xmax[2]+xmin[2]);
elemaabb_[6*e+3] = 0.5*(xmax[0]-xmin[0]) + radius;
elemaabb_[6*e+4] = 0.5*(xmax[1]-xmin[1]) + radius;
elemaabb_[6*e+5] = 0.5*(xmax[2]-xmin[2]) + radius;
// add bounding volume for this element
// contype and conaffinity are set to nonzero to force bvh generation
const double* aabb = elemaabb_.data() + 6*e;
tree.AddBoundingVolume(e, 1, 1, aabb, nullptr, aabb);
nbvh++;
}
// create hierarchy
tree.RemoveInactiveVolumes(nbvh);
tree.CreateBVH();
}
// create shells and element-vertex collision pairs
void mjCFlex::CreateShellPair(void) {
std::vector<std::vector<int> > fragspec(nelem*(dim+1)); // [sorted frag vertices, elem, original frag vertices]
std::vector<std::vector<int> > connectspec; // [elem1, elem2, common sorted frag vertices]
std::vector<bool> border(nelem, false); // is element on the border
std::vector<bool> borderfrag(nelem*(dim+1), false); // is fragment on the border
// make fragspec
for (int e=0; e < nelem; e++) {
int n = e*(dim+1);
// element vertices in original (unsorted) order
std::vector<int> el;
el.assign(elem_.begin()+n, elem_.begin()+n+dim+1);
// line: 2 vertex fragments
if (dim == 1) {
fragspec[n].push_back(el[0]);
fragspec[n].push_back(e);
fragspec[n].push_back(el[0]);
fragspec[n+1].push_back(el[1]);
fragspec[n+1].push_back(e);
fragspec[n+1].push_back(el[1]);
}
// triangle: 3 edge fragments
else if (dim == 2) {
fragspec[n].push_back(el[0]);
fragspec[n].push_back(el[1]);
fragspec[n].push_back(e);
fragspec[n].push_back(el[0]);
fragspec[n].push_back(el[1]);
fragspec[n+2].push_back(el[1]);
fragspec[n+2].push_back(el[2]);
fragspec[n+2].push_back(e);
fragspec[n+2].push_back(el[1]);
fragspec[n+2].push_back(el[2]);
fragspec[n+1].push_back(el[2]);
fragspec[n+1].push_back(el[0]);
fragspec[n+1].push_back(e);
fragspec[n+1].push_back(el[2]);
fragspec[n+1].push_back(el[0]);
}
// tetrahedron: 4 face fragments
else {
fragspec[n].push_back(el[0]);
fragspec[n].push_back(el[1]);
fragspec[n].push_back(el[2]);
fragspec[n].push_back(e);
fragspec[n].push_back(el[0]);
fragspec[n].push_back(el[1]);
fragspec[n].push_back(el[2]);
fragspec[n+2].push_back(el[0]);
fragspec[n+2].push_back(el[2]);
fragspec[n+2].push_back(el[3]);
fragspec[n+2].push_back(e);
fragspec[n+2].push_back(el[0]);
fragspec[n+2].push_back(el[2]);
fragspec[n+2].push_back(el[3]);
fragspec[n+1].push_back(el[0]);
fragspec[n+1].push_back(el[3]);
fragspec[n+1].push_back(el[1]);
fragspec[n+1].push_back(e);
fragspec[n+1].push_back(el[0]);
fragspec[n+1].push_back(el[3]);
fragspec[n+1].push_back(el[1]);
fragspec[n+3].push_back(el[1]);
fragspec[n+3].push_back(el[3]);
fragspec[n+3].push_back(el[2]);
fragspec[n+3].push_back(e);
fragspec[n+3].push_back(el[1]);
fragspec[n+3].push_back(el[3]);
fragspec[n+3].push_back(el[2]);
}
}
// sort first segment of each fragspec
if (dim > 1) {
for (int n=0; n < nelem*(dim+1); n++) {
std::sort(fragspec[n].begin(), fragspec[n].begin()+dim);
}
}
// sort fragspec
std::sort(fragspec.begin(), fragspec.end());
// make border and connectspec, record borderfrag
int cnt = 1;
for (int n=1; n < nelem*(dim+1); n++) {
// extract frag vertices, without elem
std::vector<int> previous = {fragspec[n-1].begin(), fragspec[n-1].begin()+dim};
std::vector<int> current = {fragspec[n].begin(), fragspec[n].begin()+dim};
// same sequential fragments
if (previous == current) {
// found pair of elements connected by common fragment
std::vector<int> connect;
connect.insert(connect.end(), fragspec[n-1][dim]);
connect.insert(connect.end(), fragspec[n][dim]);
connect.insert(connect.end(), fragspec[n].begin(), fragspec[n].begin()+dim);
connectspec.push_back(connect);
// count same sequential fragments
cnt++;
}
// different sequential fragments
else {
// found border fragment
if (cnt == 1) {
border[fragspec[n-1][dim]] = true;
borderfrag[n-1] = true;
}
// reset count
cnt = 1;
}
}
// last fragment is border
if (cnt == 1) {
int n = nelem*(dim+1);
border[fragspec[n-1][dim]] = true;
borderfrag[n-1] = true;
}
// create shell
for (unsigned i=0; i < borderfrag.size(); i++) {
if (borderfrag[i]) {
// add fragment vertices, in original order
shell.insert(shell.end(), fragspec[i].begin()+dim+1, fragspec[i].end());
}
}
// compute elemlayer (distance from border) via value iteration in 3D
if (dim < 3) {
elemlayer = std::vector<int> (nelem, 0);
}
else {
elemlayer = std::vector<int> (nelem, nelem+1); // init with greater than max value
for (int e=0; e < nelem; e++) {
if (border[e]) {
elemlayer[e] = 0; // set border elements to 0
}
}
bool change = true;
while (change) { // repeat while changes are happening
change = false;
// process edges of element connectivity graph
for (const auto& connect : connectspec) {
int e1 = connect[0]; // get element pair for this edge
int e2 = connect[1];
if (elemlayer[e1] > elemlayer[e2]+1) {
elemlayer[e1] = elemlayer[e2]+1; // better value found for e1: update
change = true;
} else if (elemlayer[e2] > elemlayer[e1]+1) {
elemlayer[e2] = elemlayer[e1]+1; // better value found for e2: update
change = true;
}
}
}
}
// create evpairs in 1D and 2D
if (dim < 3) {
// process connected element pairs containing a border element
for (const auto& connect : connectspec) {
if (border[connect[0]] || border[connect[1]]) {
// extract common fragment
std::vector<int> frag = {connect.begin()+2, connect.end()};
// process both elements
for (int ei=0; ei < 2; ei++) {
const int* edata = elem_.data() + connect[ei]*(dim+1);
// find element vertex that is not in the common fragment
for (int i=0; i <= dim; i++) {
if (frag.end() == std::find(frag.begin(), frag.end(), edata[i])) {
// add ev pair, involving the other element in connectspec
evpair.push_back(connect[1-ei]);
evpair.push_back(edata[i]);
// one such vertex exists
break;
}
}
}
}
}
}
}
Reference in New Issue View Git Blame Copy Permalink