Alessio Quaglino
fb8f77df74
PiperOrigin-RevId: 601745538 Change-Id: I590673ca7975e4abc171f98704a6c8f8d08f8327
2715 lines
72 KiB
C++
2715 lines
72 KiB
C++
// 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 <cmath>
|
|
#include <csetjmp>
|
|
#include <cstddef>
|
|
#include <cstdio>
|
|
#include <cstring>
|
|
#include <functional>
|
|
#include <memory>
|
|
#include <optional>
|
|
#include <string>
|
|
#include <unordered_map>
|
|
#include <utility>
|
|
#include <vector>
|
|
|
|
#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/mjtnum.h>
|
|
#include <mujoco/mjplugin.h>
|
|
#include "engine/engine_crossplatform.h"
|
|
#include "engine/engine_io.h"
|
|
#include "engine/engine_plugin.h"
|
|
#include "engine/engine_resource.h"
|
|
#include "engine/engine_util_blas.h"
|
|
#include "engine/engine_util_errmem.h"
|
|
#include "engine/engine_util_misc.h"
|
|
#include "engine/engine_util_solve.h"
|
|
#include "engine/engine_util_spatial.h"
|
|
#include "user/user_model.h"
|
|
#include "user/user_objects.h"
|
|
#include "user/user_util.h"
|
|
#include "xml/xml_util.h"
|
|
#include <tiny_obj_loader.h>
|
|
|
|
extern "C" {
|
|
#include "qhull_ra.h"
|
|
}
|
|
|
|
using std::string;
|
|
using std::vector;
|
|
|
|
// compute triangle area, surface normal, center
|
|
static mjtNum _triangle(mjtNum* normal, mjtNum* center,
|
|
const float* v1, const float* v2, const float* v3) {
|
|
// center
|
|
if (center) {
|
|
for (int i=0; i<3; i++) {
|
|
center[i] = (v1[i] + v2[i] + v3[i])/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] };
|
|
mju_cross(normal, b, c);
|
|
|
|
// get length
|
|
double len = mju_norm3(normal);
|
|
|
|
// ignore small faces
|
|
if (len<mjMINVAL) {
|
|
return 0;
|
|
}
|
|
|
|
// normalize
|
|
normal[0] /= len;
|
|
normal[1] /= len;
|
|
normal[2] /= len;
|
|
|
|
// return area
|
|
return len/2;
|
|
}
|
|
|
|
template <typename T>
|
|
static T* VecToArray(std::vector<T>& vector, bool clear = true){
|
|
if (vector.empty())
|
|
return nullptr;
|
|
else {
|
|
int n = (int)vector.size();
|
|
T* cvec = (T*) mju_malloc(n*sizeof(T));
|
|
memcpy(cvec, vector.data(), n*sizeof(T));
|
|
if (clear) {
|
|
vector.clear();
|
|
}
|
|
return cvec;
|
|
}
|
|
}
|
|
|
|
// 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) {
|
|
// set defaults
|
|
mjuu_setvec(refpos_, 0, 0, 0);
|
|
mjuu_setvec(refquat_, 1, 0, 0, 0);
|
|
mjuu_setvec(scale_, 1, 1, 1);
|
|
smoothnormal_ = false;
|
|
|
|
// clear internal variables
|
|
mjuu_setvec(pos_surface_, 0, 0, 0);
|
|
mjuu_setvec(pos_volume_, 0, 0, 0);
|
|
mjuu_setvec(quat_surface_, 1, 0, 0, 0);
|
|
mjuu_setvec(quat_volume_, 1, 0, 0, 0);
|
|
mjuu_setvec(pos_, 0, 0, 0);
|
|
mjuu_setvec(quat_, 1, 0, 0, 0);
|
|
|
|
mjuu_setvec(boxsz_surface_, 0, 0, 0);
|
|
mjuu_setvec(boxsz_volume_, 0, 0, 0);
|
|
mjuu_setvec(aamm_, 1e10, 1e10, 1e10);
|
|
mjuu_setvec(aamm_+3, -1e10, -1e10, -1e10);
|
|
nvert_ = 0;
|
|
nnormal_ = 0;
|
|
ntexcoord_ = 0;
|
|
nface_ = 0;
|
|
szgraph_ = 0;
|
|
vert_ = NULL;
|
|
normal_ = NULL;
|
|
center_ = NULL;
|
|
texcoord_ = NULL;
|
|
face_ = NULL;
|
|
facenormal_ = NULL;
|
|
facetexcoord_ = NULL;
|
|
graph_ = NULL;
|
|
needhull_ = false;
|
|
invalidorientation_.first = -1;
|
|
invalidorientation_.second = -1;
|
|
validarea_ = true;
|
|
validvolume_ = 1;
|
|
valideigenvalue_ = true;
|
|
validinequality_ = true;
|
|
processed_ = false;
|
|
visual_ = true;
|
|
|
|
// reset to default if given
|
|
if (_def) {
|
|
*this = _def->mesh;
|
|
}
|
|
|
|
// set model, def
|
|
model = _model;
|
|
def = (_def ? _def : (_model ? _model->defaults[0] : 0));
|
|
}
|
|
|
|
|
|
|
|
mjCMesh::~mjCMesh() {
|
|
if (vert_) mju_free(vert_);
|
|
if (normal_) mju_free(normal_);
|
|
if (texcoord_) mju_free(texcoord_);
|
|
if (center_) mju_free(center_);
|
|
if (face_) mju_free(face_);
|
|
if (facenormal_) mju_free(facenormal_);
|
|
if (facetexcoord_) mju_free(facetexcoord_);
|
|
if (graph_) mju_free(graph_);
|
|
}
|
|
|
|
|
|
|
|
void mjCMesh::set_content_type(std::optional<std::string>&& content_type) {
|
|
if (content_type.has_value()) {
|
|
content_type_ = std::move(content_type.value());
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void mjCMesh::set_file(std::optional<std::string>&& file) {
|
|
if (file.has_value()) {
|
|
file_ = std::move(file.value());
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void mjCMesh::set_refpos(std::optional<std::array<double, 3>> refpos) {
|
|
if (refpos.has_value()) {
|
|
std::copy(refpos.value().begin(), refpos.value().end(), refpos_);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void mjCMesh::set_refquat(std::optional<std::array<double, 4>> refquat) {
|
|
if (refquat.has_value()) {
|
|
std::copy(refquat.value().begin(), refquat.value().end(), refquat_);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void mjCMesh::set_scale(std::optional<std::array<double, 3>> scale) {
|
|
if (scale.has_value()) {
|
|
set_scale(scale.value());
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void mjCMesh::set_uservert(std::optional<std::vector<float>>&& uservert) {
|
|
if (uservert.has_value()) {
|
|
uservert_ = std::move(uservert.value());
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void mjCMesh::set_usernormal(std::optional<std::vector<float>>&& usernormal) {
|
|
if (usernormal.has_value()) {
|
|
usernormal_ = std::move(usernormal.value());
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void mjCMesh::set_usertexcoord(std::optional<std::vector<float>>&& usertexcoord) {
|
|
if (usertexcoord.has_value()) {
|
|
usertexcoord_ = std::move(usertexcoord.value());
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void mjCMesh::set_userface(std::optional<std::vector<int>>&& userface) {
|
|
if (userface.has_value()) {
|
|
userface_ = std::move(userface.value());
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void mjCMesh::set_file(const std::string& file) {
|
|
file_ = file;
|
|
}
|
|
|
|
|
|
|
|
void mjCMesh::set_scale(std::array<double, 3> scale) {
|
|
std::copy(scale.begin(), scale.end(), scale_);
|
|
}
|
|
|
|
|
|
|
|
void mjCMesh::set_smoothnormal(bool smoothnormal) {
|
|
smoothnormal_ = smoothnormal;
|
|
}
|
|
|
|
|
|
|
|
void mjCMesh::set_needhull(bool needhull) {
|
|
needhull_ = needhull;
|
|
}
|
|
|
|
|
|
|
|
// 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);
|
|
}
|
|
|
|
model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance);
|
|
const mjpPlugin* plugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot);
|
|
if (!(plugin->capabilityflags & mjPLUGIN_SDF)) {
|
|
throw mjCError(this, "plugin '%s' does not support signed distance fields", plugin->name);
|
|
}
|
|
|
|
std::vector<mjtNum> attributes(plugin->nattribute, 0);
|
|
std::vector<const char*> names(plugin->nattribute, 0);
|
|
std::vector<const char*> values(plugin->nattribute, 0);
|
|
for (int i=0; i < plugin->nattribute; i++) {
|
|
names[i] = plugin->attributes[i];
|
|
values[i] = plugin_instance->config_attribs[names[i]].c_str();
|
|
}
|
|
|
|
if (plugin->sdf_attribute) {
|
|
plugin->sdf_attribute(attributes.data(), names.data(), values.data());
|
|
}
|
|
|
|
mjtNum aabb[6] = {0};
|
|
plugin->sdf_aabb(aabb, attributes.data());
|
|
mjtNum total = aabb[3] + aabb[4] + aabb[5];
|
|
|
|
const mjtNum 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] = plugin->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;
|
|
|
|
for (size_t i = 0; i < mesh.vertices.size(); i++) {
|
|
uservert.push_back(2*aabb[3]*mesh.vertices.at(i).x/(nx-1) + aabb[0]-aabb[3]);
|
|
uservert.push_back(2*aabb[4]*mesh.vertices.at(i).y/(ny-1) + aabb[1]-aabb[4]);
|
|
uservert.push_back(2*aabb[5]*mesh.vertices.at(i).z/(nz-1) + aabb[2]-aabb[5]);
|
|
}
|
|
|
|
for (size_t i = 0; i < mesh.normals.size(); i++) {
|
|
usernormal.push_back(mesh.normals.at(i).x);
|
|
usernormal.push_back(mesh.normals.at(i).y);
|
|
usernormal.push_back(mesh.normals.at(i).z);
|
|
}
|
|
|
|
for (size_t i = 0; i < mesh.indices.size(); i++) {
|
|
userface.push_back(mesh.indices.at(i));
|
|
}
|
|
|
|
set_uservert(uservert);
|
|
set_usernormal(usernormal);
|
|
set_userface(userface);
|
|
delete[] field;
|
|
}
|
|
|
|
|
|
|
|
// compiler
|
|
void mjCMesh::Compile(const mjVFS* vfs) {
|
|
// load file
|
|
if (!file_.empty()) {
|
|
// remove path from file if necessary
|
|
if (model->strippath) {
|
|
file_ = mjuu_strippath(file_);
|
|
}
|
|
|
|
std::string asset_type = GetAssetContentType(file_, content_type_);
|
|
if (asset_type.empty()) {
|
|
throw mjCError(this, "unknown mesh content type for file: '%s'", file_.c_str());
|
|
}
|
|
|
|
if (asset_type != "model/stl" && asset_type != "model/obj"
|
|
&& asset_type != "model/vnd.mujoco.msh") {
|
|
throw mjCError(this, "unsupported content type: '%s'", asset_type.c_str());
|
|
}
|
|
|
|
string filename = mjuu_makefullname(model->modelfiledir, model->meshdir, file_);
|
|
mjResource* resource = LoadResource(filename, vfs);
|
|
|
|
try {
|
|
if (asset_type == "model/stl") {
|
|
LoadSTL(resource);
|
|
} else if (asset_type == "model/obj") {
|
|
LoadOBJ(resource);
|
|
} else {
|
|
LoadMSH(resource);
|
|
}
|
|
mju_closeResource(resource);
|
|
} catch (mjCError err) {
|
|
mju_closeResource(resource);
|
|
throw err;
|
|
}
|
|
}
|
|
|
|
// create using marching cubes
|
|
else if (is_plugin) {
|
|
LoadSDF();
|
|
}
|
|
|
|
// copy user vertex
|
|
if (!uservert_.empty()) {
|
|
// check repeated
|
|
if (vert_) {
|
|
throw mjCError(this, "repeated vertex specification");
|
|
}
|
|
|
|
// check size
|
|
if (uservert_.size()<12) {
|
|
throw mjCError(this, "at least 4 vertices required");
|
|
}
|
|
if (uservert_.size()%3) {
|
|
throw mjCError(this, "vertex data must be a multiple of 3");
|
|
}
|
|
|
|
// copy from user
|
|
nvert_ = (int)uservert_.size()/3;
|
|
vert_ = VecToArray(uservert_, !file_.empty());
|
|
}
|
|
|
|
// copy user normal
|
|
if (!usernormal_.empty()) {
|
|
// check repeated
|
|
if (normal_) {
|
|
throw mjCError(this, "repeated normal specification");
|
|
}
|
|
|
|
// check size
|
|
if (usernormal_.size()%3) {
|
|
throw mjCError(this, "normal data must be a multiple of 3");
|
|
}
|
|
|
|
// copy from user
|
|
nnormal_ = (int)usernormal_.size()/3;
|
|
normal_ = VecToArray(usernormal_, !file_.empty());
|
|
}
|
|
|
|
// copy user texcoord
|
|
if (!usertexcoord_.empty()) {
|
|
// check repeated
|
|
if (texcoord_) {
|
|
throw mjCError(this, "repeated texcoord specification");
|
|
}
|
|
|
|
// check size
|
|
if (usertexcoord_.size()%2) {
|
|
throw mjCError(this, "texcoord must be a multiple of 2");
|
|
}
|
|
|
|
// check size if no face texcoord indices are given
|
|
if (usertexcoord_.size() != 2*nvert_ && userfacetexcoord_.empty()) {
|
|
throw mjCError(this,
|
|
"texcoord must be 2*nv if face texcoord indices are not provided in an OBJ file");
|
|
}
|
|
|
|
// copy from user
|
|
ntexcoord_ = (int)usertexcoord_.size()/2;
|
|
texcoord_ = VecToArray(usertexcoord_, !file_.empty());
|
|
}
|
|
|
|
// copy user face
|
|
if (!userface_.empty()) {
|
|
// check repeated
|
|
if (face_) {
|
|
throw mjCError(this, "repeated face specification");
|
|
}
|
|
|
|
// check size
|
|
if (userface_.size()%3) {
|
|
throw mjCError(this, "face data must be a multiple of 3");
|
|
}
|
|
|
|
// check vertices exist
|
|
for (int i=0; i<userface_.size(); i++) {
|
|
if (userface_[i] >= nvert_ || userface_[i] < 0) {
|
|
throw mjCError(this, "index in face does not exist in vertex array");
|
|
}
|
|
}
|
|
|
|
// create half-edge structure (if mesh was in XML)
|
|
if (useredge_.empty()) {
|
|
for (int i=0; i<userface_.size()/3; i++) {
|
|
int v0 = userface_[3*i+0];
|
|
int v1 = userface_[3*i+1];
|
|
int v2 = userface_[3*i+2];
|
|
mjtNum normal[3];
|
|
if (_triangle(normal, nullptr, vert_+3*v0, vert_+3*v1, vert_+3*v2)>sqrt(mjMINVAL)) {
|
|
useredge_.push_back(std::pair(v0, v1));
|
|
useredge_.push_back(std::pair(v1, v2));
|
|
useredge_.push_back(std::pair(v2, v0));
|
|
} else {
|
|
// TODO(b/255525326)
|
|
}
|
|
}
|
|
}
|
|
|
|
// copy from user
|
|
nface_ = (int)userface_.size()/3;
|
|
face_ = VecToArray(userface_, !file_.empty());
|
|
|
|
// check vertices exist
|
|
for (auto vertex_index : userface_) {
|
|
if (vertex_index>=nvert_ || vertex_index < 0) {
|
|
throw mjCError(this, "found index in userface that exceeds uservert size.");
|
|
}
|
|
}
|
|
}
|
|
|
|
// check for inconsistent face orientations
|
|
if (!useredge_.empty()) {
|
|
std::stable_sort(useredge_.begin(), useredge_.end());
|
|
auto iterator = std::adjacent_find(useredge_.begin(), useredge_.end());
|
|
if (iterator != useredge_.end()) {
|
|
invalidorientation_.first = iterator->first+1;
|
|
invalidorientation_.second = iterator->second+1;
|
|
}
|
|
}
|
|
|
|
// require vertices
|
|
if (!vert_) {
|
|
throw mjCError(this, "no vertices");
|
|
}
|
|
|
|
// make graph describing convex hull
|
|
if ((model->convexhull && needhull_) || !face_) {
|
|
MakeGraph();
|
|
}
|
|
|
|
// no faces: copy from convex hull
|
|
if (!face_) {
|
|
CopyGraph();
|
|
}
|
|
|
|
// no normals: make
|
|
if (!normal_) {
|
|
MakeNormal();
|
|
}
|
|
|
|
// copy user normal indices
|
|
if (!userfacenormal_.empty()) {
|
|
// check repeated
|
|
if (facenormal_) {
|
|
throw mjCError(this, "repeated facenormal specification");
|
|
}
|
|
|
|
if (userfacenormal_.size()!=3*nface_) {
|
|
throw mjCError(this, "face data must have the same size as face normal data");
|
|
}
|
|
|
|
facenormal_ = VecToArray(userfacenormal_, !file_.empty());
|
|
}
|
|
|
|
// copy user texcoord
|
|
if (!userfacetexcoord_.empty()) {
|
|
// check repeated
|
|
if (facetexcoord_) {
|
|
throw mjCError(this, "repeated facetexcoord specification");
|
|
}
|
|
|
|
facetexcoord_ = VecToArray(userfacetexcoord_, !file_.empty());
|
|
}
|
|
|
|
// no facetexcoord: copy from faces
|
|
if (!facetexcoord_ && texcoord_) {
|
|
facetexcoord_ = (int*) mju_malloc(3*nface_*sizeof(int));
|
|
memcpy(facetexcoord_, face_, 3*nface_*sizeof(int));
|
|
}
|
|
|
|
// facenormal might not exist if usernormal was specified
|
|
if (!facenormal_) {
|
|
facenormal_ = (int*) mju_malloc(3*nface_*sizeof(int));
|
|
memcpy(facenormal_, face_, 3*nface_*sizeof(int));
|
|
}
|
|
|
|
// scale, center, orient, compute mass and inertia
|
|
Process();
|
|
processed_ = true;
|
|
|
|
// no radii: make
|
|
if (!center_) {
|
|
MakeCenter();
|
|
}
|
|
|
|
// make bounding volume hierarchy
|
|
if (tree_.bvh.empty()) {
|
|
face_aabb_.assign(6*nface_, 0);
|
|
tree_.AllocateBoundingVolumes(nface_);
|
|
for (int i=0; i<nface_; i++) {
|
|
SetBoundingVolume(i);
|
|
}
|
|
tree_.CreateBVH();
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// get bounding volume
|
|
void mjCMesh::SetBoundingVolume(int faceid) {
|
|
mjCBoundingVolume* node = tree_.GetBoundingVolume(faceid);
|
|
node->SetId(faceid);
|
|
node->conaffinity = 1;
|
|
node->contype = 1;
|
|
node->pos = center_ + 3*faceid;
|
|
node->quat = NULL;
|
|
mjtNum face_aamm[6] = {1E+10, 1E+10, 1E+10, -1E+10, -1E+10, -1E+10};
|
|
for (int j=0; j<3; j++) {
|
|
int vertid = face_[3*faceid+j];
|
|
face_aamm[0] = mjMIN(face_aamm[0], vert_[3*vertid+0]);
|
|
face_aamm[1] = mjMIN(face_aamm[1], vert_[3*vertid+1]);
|
|
face_aamm[2] = mjMIN(face_aamm[2], vert_[3*vertid+2]);
|
|
face_aamm[3] = mjMAX(face_aamm[3], vert_[3*vertid+0]);
|
|
face_aamm[4] = mjMAX(face_aamm[4], vert_[3*vertid+1]);
|
|
face_aamm[5] = mjMAX(face_aamm[5], vert_[3*vertid+2]);
|
|
}
|
|
face_aabb_[6*faceid+0] = .5 * (face_aamm[0] + face_aamm[3]);
|
|
face_aabb_[6*faceid+1] = .5 * (face_aamm[1] + face_aamm[4]);
|
|
face_aabb_[6*faceid+2] = .5 * (face_aamm[2] + face_aamm[5]);
|
|
face_aabb_[6*faceid+3] = .5 * (face_aamm[3] - face_aamm[0]);
|
|
face_aabb_[6*faceid+4] = .5 * (face_aamm[4] - face_aamm[1]);
|
|
face_aabb_[6*faceid+5] = .5 * (face_aamm[5] - face_aamm[2]);
|
|
node->aabb = face_aabb_.data() + 6*faceid;
|
|
}
|
|
|
|
|
|
|
|
// get position
|
|
double* mjCMesh::GetPosPtr(mjtMeshType type) {
|
|
if (type==mjSHELL_MESH) {
|
|
return pos_surface_;
|
|
} else {
|
|
return pos_volume_;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// get orientation
|
|
double* mjCMesh::GetQuatPtr(mjtMeshType type) {
|
|
if (type==mjSHELL_MESH) {
|
|
return quat_surface_;
|
|
} else {
|
|
return quat_volume_;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
double* mjCMesh::GetOffsetPosPtr() {
|
|
return pos_;
|
|
}
|
|
|
|
|
|
|
|
double* mjCMesh::GetOffsetQuatPtr() {
|
|
return quat_;
|
|
}
|
|
|
|
|
|
|
|
bool mjCMesh::HasTexcoord() const {
|
|
return texcoord_ != nullptr;
|
|
}
|
|
|
|
|
|
|
|
void mjCMesh::CopyVert(float* arr) const {
|
|
std::copy(vert_, vert_+3*nvert_, arr);
|
|
}
|
|
|
|
|
|
|
|
void mjCMesh::CopyNormal(float* arr) const {
|
|
std::copy(normal_, normal_+3*nnormal_, arr);
|
|
}
|
|
|
|
|
|
|
|
void mjCMesh::CopyFace(int* arr) const {
|
|
std::copy(face_, face_+3*nface_, arr);
|
|
}
|
|
|
|
|
|
|
|
void mjCMesh::CopyFaceTexcoord(int* arr) const {
|
|
std::copy(facetexcoord_, facetexcoord_+3*nface_, arr);
|
|
}
|
|
|
|
|
|
|
|
void mjCMesh::CopyFaceNormal(int* arr) const {
|
|
std::copy(facenormal_, facenormal_+3*nface_, arr);
|
|
}
|
|
|
|
|
|
|
|
void mjCMesh::CopyTexcoord(float* arr) const {
|
|
std::copy(texcoord_, texcoord_+2*ntexcoord_, arr);
|
|
}
|
|
|
|
|
|
|
|
void mjCMesh::CopyGraph(int* arr) const {
|
|
std::copy(graph_, graph_+szgraph_, arr);
|
|
}
|
|
|
|
|
|
|
|
void mjCMesh::DelTexcoord() {
|
|
if (texcoord_) mju_free(texcoord_);
|
|
ntexcoord_ = 0;
|
|
}
|
|
|
|
|
|
|
|
// set geom size to match mesh
|
|
void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
|
|
// copy mesh pos into meshpos
|
|
mjuu_copyvec(meshpos, GetPosPtr(geom->typeinertia), 3);
|
|
|
|
// use inertial box
|
|
if (!model->fitaabb) {
|
|
// get inertia box type (shell or volume)
|
|
double* boxsz = GetInertiaBoxPtr(geom->typeinertia);
|
|
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] = mju_max(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
|
|
double cen[3] = {(aamm_[0]+aamm_[3])/2, (aamm_[1]+aamm_[4])/2, (aamm_[2]+aamm_[5])/2};
|
|
|
|
// add box center into meshpos
|
|
meshpos[0] += cen[0];
|
|
meshpos[1] += cen[1];
|
|
meshpos[2] += cen[2];
|
|
|
|
// compute depending on type
|
|
switch (geom->type) {
|
|
case mjGEOM_SPHERE:
|
|
// find maximum distance
|
|
geom->size[0] = 0;
|
|
for (int i=0; i<nvert_; i++) {
|
|
double v[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]};
|
|
double dst = mjuu_dist3(v, cen);
|
|
geom->size[0] = mju_max(geom->size[0], dst);
|
|
}
|
|
break;
|
|
|
|
case mjGEOM_CAPSULE:
|
|
case mjGEOM_CYLINDER:
|
|
// find maximum distance in XY, separately in Z
|
|
geom->size[0] = 0;
|
|
geom->size[1] = 0;
|
|
for (int i=0; i<nvert_; i++) {
|
|
double v[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]};
|
|
double dst = sqrt((v[0]-cen[0])*(v[0]-cen[0]) +
|
|
(v[1]-cen[1])*(v[1]-cen[1]));
|
|
geom->size[0] = mju_max(geom->size[0], dst);
|
|
|
|
// proceed with z: valid for cylinder
|
|
double dst2 = fabs(v[2]-cen[2]);
|
|
geom->size[1] = mju_max(geom->size[1], dst2);
|
|
}
|
|
|
|
// special handling of capsule: consider curved cap
|
|
if (geom->type==mjGEOM_CAPSULE) {
|
|
geom->size[1] = 0;
|
|
for (int i=0; i<nvert_; i++) {
|
|
// get distance in XY and Z
|
|
double v[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]};
|
|
double dst = sqrt((v[0]-cen[0])*(v[0]-cen[0]) +
|
|
(v[1]-cen[1])*(v[1]-cen[1]));
|
|
double dst2 = fabs(v[2]-cen[2]);
|
|
|
|
// get spherical elevation at horizontal distance dst
|
|
double h = geom->size[0] * sin(acos(dst/geom->size[0]));
|
|
geom->size[1] = mju_max(geom->size[1], dst2-h);
|
|
}
|
|
}
|
|
break;
|
|
|
|
case mjGEOM_ELLIPSOID:
|
|
case mjGEOM_BOX:
|
|
geom->size[0] = aamm_[3] - cen[0];
|
|
geom->size[1] = aamm_[4] - cen[1];
|
|
geom->size[2] = aamm_[5] - cen[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;
|
|
}
|
|
|
|
|
|
|
|
// comparison function for vertex sorting
|
|
quicksortfunc(vertcompare, context, el1, el2) {
|
|
float* vert = (float*) context;
|
|
float x1 = vert[3*(*(int*)el1)] + 1e-2*vert[1+3*(*(int*)el1)] + 1e-4*vert[2+3*(*(int*)el1)];
|
|
float x2 = vert[3*(*(int*)el2)] + 1e-2*vert[1+3*(*(int*)el2)] + 1e-4*vert[2+3*(*(int*)el2)];
|
|
|
|
if (x1 < x2) {
|
|
return -1;
|
|
} else if (x1 == x2) {
|
|
return 0;
|
|
} else {
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
// remove repeated vertices
|
|
void mjCMesh::RemoveRepeated() {
|
|
int repeated = 0;
|
|
|
|
// allocate sort and redirection indices, set to identity
|
|
auto index = std::unique_ptr<int[]>(new int[nvert_]);
|
|
auto redirect = std::unique_ptr<int[]>(new int[nvert_]);
|
|
for (int i=0; i < nvert_; i++) {
|
|
index[i] = redirect[i] = i;
|
|
}
|
|
|
|
// sort vertices
|
|
mjQUICKSORT(index.get(), nvert_, sizeof(int), vertcompare, vert_);
|
|
|
|
// find repeated vertices, set redirect
|
|
for (int i=1; i < nvert_; i++) {
|
|
if (vert_[3*index[i]] == vert_[3*index[i-1]] &&
|
|
vert_[3*index[i]+1] == vert_[3*index[i-1]+1] &&
|
|
vert_[3*index[i]+2] == vert_[3*index[i-1]+2]) {
|
|
redirect[index[i]] = index[i-1];
|
|
repeated++;
|
|
}
|
|
}
|
|
|
|
// compress vertices, change face data
|
|
if (repeated) {
|
|
// track redirections until non-redirected vertex, set
|
|
for (int i=0; i<nvert_; i++) {
|
|
int j = i;
|
|
while (redirect[j]!=j) {
|
|
j = redirect[j];
|
|
}
|
|
redirect[i] = j;
|
|
}
|
|
|
|
// find good vertices, compress, reuse index to save compressed position
|
|
int j = 0;
|
|
for (int i=0; i<nvert_; i++) {
|
|
if (redirect[i]==i) {
|
|
index[i] = j;
|
|
memcpy(vert_+3*j, vert_+3*i, 3*sizeof(float));
|
|
j++;
|
|
} else {
|
|
index[i] = -1;
|
|
}
|
|
}
|
|
|
|
// recompute face data to reflect compressed vertices
|
|
for (int i=0; i<3*nface_; i++) {
|
|
face_[i] = index[redirect[face_[i]]];
|
|
|
|
// sanity check, SHOULD NOT OCCUR
|
|
if (face_[i]<0 || face_[i]>=nvert_-repeated) {
|
|
throw mjCError(
|
|
this, "error removing vertices from mesh '%s'", name.c_str());
|
|
}
|
|
}
|
|
}
|
|
|
|
// correct vertex count
|
|
nvert_ -= repeated;
|
|
|
|
// resize vert if any vertices were removed
|
|
if (repeated) {
|
|
float* old = vert_;
|
|
vert_ = (float*) mju_malloc(3*nvert_*sizeof(float));
|
|
memcpy(vert_, old, 3*nvert_*sizeof(float));
|
|
mju_free(old);
|
|
}
|
|
}
|
|
|
|
|
|
// load OBJ mesh
|
|
void mjCMesh::LoadOBJ(mjResource* resource) {
|
|
tinyobj::ObjReader objReader;
|
|
const void* bytes = nullptr;
|
|
int buffer_sz = mju_readResource(resource, &bytes);
|
|
if (buffer_sz < 0) {
|
|
throw mjCError(this, "could not read OBJ file '%s'", resource->name);
|
|
}
|
|
|
|
// TODO(etom): support .mtl files?
|
|
const char* buffer = (const char*) bytes;
|
|
objReader.ParseFromString(std::string(buffer, buffer_sz), std::string());
|
|
|
|
if (!objReader.Valid()) {
|
|
throw mjCError(this, "could not parse OBJ file '%s'", resource->name);
|
|
}
|
|
|
|
const auto& attrib = objReader.GetAttrib();
|
|
uservert_ = attrib.vertices; // copy from one std::vector to another
|
|
usernormal_ = attrib.normals;
|
|
usertexcoord_ = attrib.texcoords;
|
|
|
|
if (!objReader.GetShapes().empty()) {
|
|
const auto& mesh = objReader.GetShapes()[0].mesh;
|
|
bool righthand = (scale_[0]*scale_[1]*scale_[2] > 0);
|
|
|
|
// iterate over mesh faces
|
|
std::vector<tinyobj::index_t> face_indices;
|
|
for (int face = 0, idx = 0; idx < mesh.indices.size();) {
|
|
int nfacevert = mesh.num_face_vertices[face];
|
|
if (nfacevert < 3 || nfacevert > 4) {
|
|
throw mjCError(
|
|
this, "only tri or quad meshes are supported for OBJ (file '%s')",
|
|
resource->name);
|
|
}
|
|
|
|
face_indices.push_back(mesh.indices[idx]);
|
|
face_indices.push_back(mesh.indices[idx + (righthand==1 ? 1 : 2)]);
|
|
face_indices.push_back(mesh.indices[idx + (righthand==1 ? 2 : 1)]);
|
|
|
|
if (nfacevert == 4) {
|
|
face_indices.push_back(mesh.indices[idx]);
|
|
face_indices.push_back(mesh.indices[idx + (righthand==1 ? 2 : 3)]);
|
|
face_indices.push_back(mesh.indices[idx + (righthand==1 ? 3 : 2)]);
|
|
}
|
|
idx += nfacevert;
|
|
++face;
|
|
}
|
|
|
|
// for each vertex, store index, normal, and texcoord
|
|
for (const auto& mesh_index : face_indices) {
|
|
userface_.push_back(mesh_index.vertex_index);
|
|
|
|
if (!usernormal_.empty()) {
|
|
userfacenormal_.push_back(mesh_index.normal_index);
|
|
}
|
|
|
|
if (!usertexcoord_.empty()) {
|
|
userfacetexcoord_.push_back(mesh_index.texcoord_index);
|
|
}
|
|
}
|
|
}
|
|
|
|
// flip the second texcoord
|
|
for (int i=0; i<usertexcoord_.size()/2; i++) {
|
|
usertexcoord_[2*i+1] = 1-usertexcoord_[2*i+1];
|
|
}
|
|
}
|
|
|
|
|
|
// load STL binary mesh
|
|
void mjCMesh::LoadSTL(mjResource* resource) {
|
|
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 STL file '%s'", resource->name);
|
|
} else if (!buffer_sz) {
|
|
throw mjCError(this, "STL file '%s' is empty", resource->name);
|
|
}
|
|
|
|
// make sure there is enough data for header
|
|
if (buffer_sz<84) {
|
|
throw mjCError(this, "invalid header in STL file '%s'", resource->name);
|
|
}
|
|
|
|
// get number of triangles, check bounds
|
|
ReadFromBuffer(&nface_, buffer + 80);
|
|
if (nface_<1 || nface_>200000) {
|
|
throw mjCError(this,
|
|
"number of faces should be between 1 and 200000 in STL file '%s';"
|
|
" perhaps this is an ASCII file?", resource->name);
|
|
}
|
|
|
|
// check remaining buffer size
|
|
if (nface_*50 != buffer_sz-84) {
|
|
throw mjCError(this,
|
|
"STL file '%s' has wrong size; perhaps this is an ASCII file?",
|
|
resource->name);
|
|
}
|
|
|
|
// assign stl data pointer
|
|
const char* stl = buffer + 84;
|
|
|
|
// allocate face and vertex data
|
|
face_ = (int*) mju_malloc(3*nface_*sizeof(int));
|
|
vert_ = (float*) mju_malloc(9*nface_*sizeof(float));
|
|
|
|
// add vertices and faces, including repeated for now
|
|
for (int i=0; i<nface_; i++) {
|
|
for (int j=0; j<3; j++) {
|
|
// read vertex coordinates
|
|
float v[3];
|
|
ReadFromBuffer(&v, stl+50*i+12*(j+1));
|
|
|
|
for (int k=0; k < 3; k++) {
|
|
if (std::isnan(v[k]) || std::isinf(v[k])) {
|
|
throw mjCError(this, "STL file '%s' contains invalid vertices.",
|
|
resource->name);
|
|
}
|
|
// check if vertex coordinates can be cast to an int safely
|
|
if (fabs(v[k])>pow(2, 30)) {
|
|
throw mjCError(this,
|
|
"vertex coordinates in STL file '%s' exceed maximum bounds",
|
|
resource->name);
|
|
}
|
|
}
|
|
|
|
// add vertex address in face; change order if scale makes it lefthanded
|
|
if (righthand || j==0) {
|
|
face_[3*i+j] = nvert_;
|
|
} else {
|
|
face_[3*i+3-j] = nvert_;
|
|
}
|
|
|
|
// add vertex data
|
|
memcpy(vert_+3*nvert_, v, 3*sizeof(float));
|
|
nvert_++;
|
|
}
|
|
}
|
|
|
|
RemoveRepeated();
|
|
}
|
|
|
|
|
|
|
|
// load MSH binary mesh
|
|
void mjCMesh::LoadMSH(mjResource* resource) {
|
|
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
|
|
ReadFromBuffer(&nvert_, buffer);
|
|
ReadFromBuffer(&nnormal_, buffer + sizeof(int));
|
|
ReadFromBuffer(&ntexcoord_, buffer + 2*sizeof(int));
|
|
ReadFromBuffer(&nface_, buffer + 3*sizeof(int));
|
|
|
|
// check sizes
|
|
if (nvert_<4 || nface_<0 || nnormal_<0 || ntexcoord_<0 ||
|
|
(nnormal_>0 && nnormal_!=nvert_) ||
|
|
(ntexcoord_>0 && ntexcoord_!=nvert_)) {
|
|
throw mjCError(this, "invalid sizes in MSH file '%s'", resource->name);
|
|
}
|
|
|
|
// check file size
|
|
if (buffer_sz != 4*sizeof(int) + 3*nvert_*sizeof(float) + 3*nnormal_*sizeof(float) +
|
|
2*ntexcoord_*sizeof(float) + 3*nface_*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));
|
|
if (nvert_) {
|
|
vert_ = (float*) mju_malloc(3*nvert_*sizeof(float));
|
|
memcpy(vert_, fdata, 3*nvert_*sizeof(float));
|
|
fdata += 3*nvert_;
|
|
}
|
|
if (nnormal_) {
|
|
normal_ = (float*) mju_malloc(3*nvert_*sizeof(float));
|
|
memcpy(normal_, fdata, 3*nvert_*sizeof(float));
|
|
fdata += 3*nvert_;
|
|
}
|
|
if (ntexcoord_) {
|
|
texcoord_ = (float*) mju_malloc(2*nvert_*sizeof(float));
|
|
memcpy(texcoord_, fdata, 2*nvert_*sizeof(float));
|
|
fdata += 2*nvert_;
|
|
}
|
|
if (nface_) {
|
|
face_ = (int*) mju_malloc(3*nface_*sizeof(int));
|
|
facenormal_ = (int*) mju_malloc(3*nface_*sizeof(int));
|
|
memcpy(face_, fdata, 3*nface_*sizeof(int));
|
|
memcpy(facenormal_, fdata, 3*nface_*sizeof(int));
|
|
}
|
|
if (nface_ && texcoord_) {
|
|
facetexcoord_= (int*) mju_malloc(3*nface_*sizeof(int));
|
|
memcpy(facetexcoord_, fdata, 3*nface_*sizeof(int));
|
|
}
|
|
|
|
// rearrange face data if left-handed scaling
|
|
if (nface_ && !righthand) {
|
|
for (int i=0; i<nface_; i++) {
|
|
int tmp = face_[3*i+1];
|
|
face_[3*i+1] = face_[3*i+2];
|
|
face_[3*i+2] = tmp;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void mjCMesh::ComputeVolume(double CoM[3], mjtMeshType type,
|
|
const double facecen[3], bool exactmeshinertia) {
|
|
double nrm[3];
|
|
double cen[3];
|
|
GetVolumeRef(type) = 0;
|
|
mjuu_zerovec(CoM, 3);
|
|
for (int i=0; i<nface_; i++) {
|
|
// get area, normal and center
|
|
double a = _triangle(nrm, cen, vert_+3*face_[3*i], vert_+3*face_[3*i+1], vert_+3*face_[3*i+2]);
|
|
|
|
// compute and add volume
|
|
const double vec[3] = {cen[0]-facecen[0], cen[1]-facecen[1], cen[2]-facecen[2]};
|
|
double vol = type==mjSHELL_MESH ? a : mjuu_dot3(vec, nrm) * a / 3;
|
|
|
|
// if legacy computation requested, then always positive
|
|
if (!exactmeshinertia && type==mjVOLUME_MESH) {
|
|
vol = fabs(vol);
|
|
}
|
|
|
|
// add pyramid com
|
|
GetVolumeRef(type) += vol;
|
|
for (int j=0; j<3; j++) {
|
|
CoM[j] += vol*(cen[j]*3.0/4.0 + facecen[j]/4.0);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// apply transformations
|
|
void mjCMesh::ApplyTransformations() {
|
|
// translate
|
|
if (refpos_[0]!=0 || refpos_[1]!=0 || refpos_[2]!=0) {
|
|
// prepare translation
|
|
float rp[3] = {(float)refpos_[0], (float)refpos_[1], (float)refpos_[2]};
|
|
|
|
// process vertices
|
|
for (int i=0; i<nvert_; i++) {
|
|
vert_[3*i] -= rp[0];
|
|
vert_[3*i+1] -= rp[1];
|
|
vert_[3*i+2] -= rp[2];
|
|
}
|
|
}
|
|
|
|
// rotate
|
|
if (refquat_[0]!=1 || refquat_[1]!=0 || refquat_[2]!=0 || refquat_[3]!=0) {
|
|
// prepare rotation
|
|
mjtNum quat[4] = {refquat_[0], refquat_[1], refquat_[2], refquat_[3]};
|
|
mjtNum mat[9];
|
|
mju_normalize4(quat);
|
|
mju_quat2Mat(mat, quat);
|
|
|
|
// process vertices
|
|
for (int i=0; i<nvert_; i++) {
|
|
mjtNum p1[3], p0[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]};
|
|
mju_rotVecMatT(p1, p0, mat);
|
|
vert_[3*i] = (float) p1[0];
|
|
vert_[3*i+1] = (float) p1[1];
|
|
vert_[3*i+2] = (float) p1[2];
|
|
}
|
|
|
|
// process normals
|
|
for (int i=0; i<nnormal_; i++) {
|
|
mjtNum n1[3], n0[3] = {normal_[3*i], normal_[3*i+1], normal_[3*i+2]};
|
|
mju_rotVecMatT(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++) {
|
|
vert_[3*i] *= scale_[0];
|
|
vert_[3*i+1] *= scale_[1];
|
|
vert_[3*i+2] *= scale_[2];
|
|
}
|
|
|
|
for (int i=0; i<nnormal_; i++) {
|
|
normal_[3*i] *= 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] *= scl;
|
|
normal_[3*i+1] *= scl;
|
|
normal_[3*i+2] *= scl;
|
|
} else {
|
|
normal_[3*i] = 0;
|
|
normal_[3*i+1] = 0;
|
|
normal_[3*i+2] = 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// find centroid of faces
|
|
void mjCMesh::ComputeFaceCentroid(double facecen[3]) {
|
|
double area = 0;
|
|
double nrm[3];
|
|
double cen[3];
|
|
|
|
for (int i=0; i<nface_; i++) {
|
|
// check vertex indices
|
|
for (int j=0; j<3; j++) {
|
|
if (face_[3*i+j]<0 || face_[3*i+j]>=nvert_) {
|
|
throw mjCError(this, "vertex index out of range in %s (index = %d)", name.c_str(), i);
|
|
}
|
|
}
|
|
|
|
// get area and center
|
|
double a = _triangle(nrm, cen, vert_+3*face_[3*i], vert_+3*face_[3*i+1], vert_+3*face_[3*i+2]);
|
|
|
|
// accumulate
|
|
for (int j=0; j<3; j++) {
|
|
facecen[j] += a*cen[j];
|
|
}
|
|
area += a;
|
|
}
|
|
|
|
// require positive area
|
|
if (area < mjMINVAL) {
|
|
validarea_ = false;
|
|
return;
|
|
}
|
|
|
|
// finalize centroid of faces
|
|
for (int j=0; j<3; j++) {
|
|
facecen[j] /= area;
|
|
}
|
|
}
|
|
|
|
|
|
void mjCMesh::Process() {
|
|
double facecen[3] = {0, 0, 0};
|
|
double nrm[3];
|
|
double cen[3];
|
|
|
|
// user offset, rotation, scaling
|
|
ApplyTransformations();
|
|
|
|
// find centroid of faces
|
|
ComputeFaceCentroid(facecen);
|
|
|
|
// compute inertial properties for both inertia types
|
|
for ( const auto type : { mjtMeshType::mjVOLUME_MESH, mjtMeshType::mjSHELL_MESH } ) {
|
|
double CoM[3] = {0, 0, 0};
|
|
double inert[6] = {0, 0, 0, 0, 0, 0};
|
|
bool exactmeshinertia = model->exactmeshinertia;
|
|
|
|
// compute CoM and volume from pyramid volumes
|
|
ComputeVolume(CoM, type, facecen, model->exactmeshinertia);
|
|
|
|
// perform computation with convex mesh if volume is negative
|
|
if (GetVolumeRef(type) <= 0 && exactmeshinertia) {
|
|
mju_warning("Malformed mesh %s, computing mesh inertia from convex hull", name.c_str());
|
|
exactmeshinertia = false;
|
|
ComputeVolume(CoM, type, facecen, exactmeshinertia);
|
|
}
|
|
|
|
// if volume is still invalid, skip the rest of the computations
|
|
if (GetVolumeRef(type) < mjMINVAL) {
|
|
validvolume_ = GetVolumeRef(type) < 0 ? -1 : 0;
|
|
continue;
|
|
}
|
|
|
|
// finalize CoM, save as mesh center
|
|
for (int j=0; j<3; j++) {
|
|
CoM[j] /= GetVolumeRef(type);
|
|
}
|
|
mjuu_copyvec(GetPosPtr(type), CoM, 3);
|
|
|
|
// re-center mesh at CoM
|
|
if (type==mjVOLUME_MESH || validvolume_<=0) {
|
|
for (int i=0; i<nvert_; i++) {
|
|
for (int j=0; j<3; j++) {
|
|
vert_[3*i+j] -= CoM[j];
|
|
}
|
|
}
|
|
}
|
|
|
|
// 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};
|
|
GetVolumeRef(type) = 0;
|
|
for (int i=0; i<nface_; i++) {
|
|
float* D = vert_+3*face_[3*i];
|
|
float* E = vert_+3*face_[3*i+1];
|
|
float* F = vert_+3*face_[3*i+2];
|
|
|
|
// get area, normal and center; update volume
|
|
double a = _triangle(nrm, cen, D, E, F);
|
|
double vol = type==mjSHELL_MESH ? a : mjuu_dot3(cen, nrm) * a / 3;
|
|
|
|
// if legacy computation requested, then always positive
|
|
if (!exactmeshinertia && type==mjVOLUME_MESH) {
|
|
vol = fabs(vol);
|
|
}
|
|
|
|
// apply formula, accumulate
|
|
GetVolumeRef(type) += vol;
|
|
for (int j=0; j<6; j++) {
|
|
P[j] += def->geom.density*vol /
|
|
(type==mjSHELL_MESH ? 12 : 20) * (
|
|
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];
|
|
|
|
// get quaternion and diagonal inertia
|
|
mjtNum eigval[3], eigvec[9], quattmp[4];
|
|
mjtNum full[9] = {
|
|
inert[0], inert[3], inert[4],
|
|
inert[3], inert[1], inert[5],
|
|
inert[4], inert[5], inert[2]
|
|
};
|
|
mju_eig3(eigval, eigvec, quattmp, full);
|
|
|
|
// check eigval - SHOULD NOT OCCUR
|
|
if (eigval[2]<=0) {
|
|
valideigenvalue_ = false;
|
|
return;
|
|
}
|
|
if (eigval[0] + eigval[1] < eigval[2] ||
|
|
eigval[0] + eigval[2] < eigval[1] ||
|
|
eigval[1] + eigval[2] < eigval[0]) {
|
|
validinequality_ = false;
|
|
return;
|
|
}
|
|
|
|
// compute sizes of equivalent inertia box
|
|
double mass = GetVolumeRef(type) * def->geom.density;
|
|
double* boxsz = GetInertiaBoxPtr(type);
|
|
boxsz[0] = sqrt(6*(eigval[1]+eigval[2]-eigval[0])/mass)/2;
|
|
boxsz[1] = sqrt(6*(eigval[0]+eigval[2]-eigval[1])/mass)/2;
|
|
boxsz[2] = sqrt(6*(eigval[0]+eigval[1]-eigval[2])/mass)/2;
|
|
|
|
// if volume was valid, copy volume quat to shell and stop,
|
|
// otherwise use shell quat for coordinate transformations
|
|
if (type==mjSHELL_MESH && validvolume_>0) {
|
|
mju_copy4(GetQuatPtr(type), GetQuatPtr(mjVOLUME_MESH));
|
|
continue;
|
|
}
|
|
|
|
// rotate vertices and normals into axis-aligned frame
|
|
mju_copy4(GetQuatPtr(type), quattmp);
|
|
double neg[4] = {quattmp[0], -quattmp[1], -quattmp[2], -quattmp[3]};
|
|
double mat[9];
|
|
mjuu_quat2mat(mat, neg);
|
|
for (int i=0; i<nvert_; i++) {
|
|
// vertices
|
|
const double vec[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]};
|
|
double res[3];
|
|
mjuu_mulvecmat(res, vec, mat);
|
|
for (int j=0; j<3; j++) {
|
|
vert_[3*i+j] = (float) res[j];
|
|
|
|
// axis-aligned bounding box
|
|
aamm_[j+0] = mju_min(aamm_[j+0], res[j]);
|
|
aamm_[j+3] = mju_max(aamm_[j+3], res[j]);
|
|
}
|
|
}
|
|
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];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// check that the mesh is valid
|
|
void mjCMesh::CheckMesh(mjtMeshType type) {
|
|
if (!processed_) {
|
|
return;
|
|
}
|
|
if (invalidorientation_.first>=0 || invalidorientation_.second>=0)
|
|
throw mjCError(this,
|
|
"faces of mesh '%s' have inconsistent orientation. Please check the "
|
|
"faces containing the vertices %d and %d.",
|
|
name.c_str(), invalidorientation_.first, invalidorientation_.second);
|
|
if (!validarea_ && type==mjSHELL_MESH)
|
|
throw mjCError(this, "mesh surface area is too small: %s", name.c_str());
|
|
if (validvolume_<0 && type==mjVOLUME_MESH)
|
|
throw mjCError(this, "mesh volume is negative (misoriented triangles): %s", name.c_str());
|
|
if (!validvolume_ && type==mjVOLUME_MESH)
|
|
throw mjCError(this, "mesh volume is too small: %s", name.c_str());
|
|
if (!valideigenvalue_)
|
|
throw mjCError(this, "eigenvalue of mesh inertia must be positive: %s", name.c_str());
|
|
if (!validinequality_)
|
|
throw mjCError(this, "eigenvalues of mesh inertia violate A + B >= C: %s", name.c_str());
|
|
}
|
|
|
|
|
|
// get inertia pointer
|
|
double* mjCMesh::GetInertiaBoxPtr(mjtMeshType type) {
|
|
CheckMesh(type);
|
|
return type==mjSHELL_MESH ? boxsz_surface_ : boxsz_volume_;
|
|
}
|
|
|
|
|
|
double& mjCMesh::GetVolumeRef(mjtMeshType type) {
|
|
CheckMesh(type);
|
|
return type==mjSHELL_MESH ? surface_ : volume_;
|
|
}
|
|
|
|
|
|
// make graph describing convex hull
|
|
void mjCMesh::MakeGraph(void) {
|
|
int adr, ok, curlong, totlong, exitcode;
|
|
double* data;
|
|
facetT* facet, **facetp;
|
|
vertexT* vertex, *vertex1, **vertex1p;
|
|
char qhopt[10] = "qhull Qt";
|
|
|
|
// graph not needed for small meshes
|
|
if (nvert_ < 4) {
|
|
return;
|
|
}
|
|
|
|
// convert mesh data to double
|
|
data = (double*) mju_malloc(3*nvert_*sizeof(double));
|
|
if (!data) {
|
|
throw mjCError(this, "could not allocate data for qhull");
|
|
}
|
|
for (int i=0; i<3*nvert_; i++) {
|
|
if (!std::isfinite(vert_[i])) {
|
|
mju_free(data);
|
|
throw mjCError(this, "vertex coordinate %d is not finite", NULL, i);
|
|
}
|
|
data[i] = (double)vert_[i];
|
|
}
|
|
|
|
qhT qh_qh;
|
|
qhT* qh = &qh_qh;
|
|
qh_zero(qh, stderr);
|
|
|
|
// qhull basic init
|
|
qh_init_A(qh, stdin, stdout, stderr, 0, NULL);
|
|
|
|
// install longjmp error handler
|
|
exitcode = setjmp(qh->errexit);
|
|
qh->NOerrexit = false;
|
|
if (!exitcode) {
|
|
// actual init
|
|
qh_initflags(qh, qhopt);
|
|
qh_init_B(qh, data, nvert_, 3, 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);
|
|
mju_free(data);
|
|
|
|
// 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);
|
|
mju_free(data);
|
|
if (graph_) {
|
|
mju_free(graph_);
|
|
szgraph_ = 0;
|
|
}
|
|
|
|
throw mjCError(this, "qhull error");
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// copy graph into face data
|
|
void mjCMesh::CopyGraph(void) {
|
|
// only if face data is missing
|
|
if (face_) {
|
|
return;
|
|
}
|
|
|
|
// get info from graph, allocate
|
|
int numvert = graph_[0];
|
|
nface_ = graph_[1];
|
|
face_ = (int*) mju_malloc(3*nface_*sizeof(int));
|
|
|
|
// 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] = graph_[j];
|
|
face_[3*i+1] = graph_[j+1];
|
|
face_[3*i+2] = graph_[j+2];
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// compute vertex normals
|
|
void mjCMesh::MakeNormal(void) {
|
|
// only if normal data is missing
|
|
if (normal_) {
|
|
return;
|
|
}
|
|
|
|
// allocate and clear normals
|
|
nnormal_ = nvert_;
|
|
normal_ = (float*) mju_malloc(3*nnormal_*sizeof(float));
|
|
memset(normal_, 0, 3*nnormal_*sizeof(float));
|
|
|
|
if (!facenormal_) {
|
|
facenormal_ = (int*) mju_malloc(3*nface_*sizeof(int));
|
|
memset(facenormal_, 0, 3*nface_*sizeof(int));
|
|
}
|
|
|
|
// 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
|
|
mjtNum vec01[3], vec02[3];
|
|
for (int j=0; j<3; j++) {
|
|
vec01[j] = vert_[3*vertid[1]+j] - vert_[3*vertid[0]+j];
|
|
vec02[j] = vert_[3*vertid[2]+j] - vert_[3*vertid[0]+j];
|
|
}
|
|
|
|
// compute face normal
|
|
mjtNum nrm[3];
|
|
mju_cross(nrm, vec01, vec02);
|
|
mjtNum area = mju_normalize3(nrm);
|
|
|
|
// 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
|
|
mjtNum vec01[3], vec02[3];
|
|
for (int j=0; j<3; j++) {
|
|
vec01[j] = vert_[3*vertid[1]+j] - vert_[3*vertid[0]+j];
|
|
vec02[j] = vert_[3*vertid[2]+j] - vert_[3*vertid[0]+j];
|
|
}
|
|
|
|
// compute face normal
|
|
mjtNum nrm[3];
|
|
mju_cross(nrm, vec01, vec02);
|
|
mjtNum area = mju_normalize3(nrm);
|
|
|
|
// compare to vertex normal, subtract contribution if dot product too small
|
|
for (int j=0; j<3; j++) {
|
|
// normalized vertex normal
|
|
mjtNum vnrm[3] = {normal_[3*vertid[j]], normal_[3*vertid[j]+1], normal_[3*vertid[j]+2]};
|
|
mju_normalize3(vnrm);
|
|
|
|
// dot too small: remove
|
|
if (mju_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(void) {
|
|
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_ + 3*i;
|
|
|
|
// get triangle edges
|
|
mjtNum a[3], b[3];
|
|
for (int j=0; j<3; j++) {
|
|
a[j] = vert_[3*vertid[0]+j] - vert_[3*vertid[2]+j];
|
|
b[j] = vert_[3*vertid[1]+j] - vert_[3*vertid[2]+j];
|
|
}
|
|
|
|
// compute face normal
|
|
mjtNum nrm[3];
|
|
mju_cross(nrm, a, b);
|
|
|
|
// compute circumradius
|
|
mjtNum norm_a_2 = mju_dot3(a, a);
|
|
mjtNum norm_b_2 = mju_dot3(b, b);
|
|
mjtNum area = mju_norm3(nrm);
|
|
|
|
// compute circumcenter
|
|
mjtNum res[3], vec[3] = {
|
|
norm_a_2 * b[0] - norm_b_2 * a[0],
|
|
norm_a_2 * b[1] - norm_b_2 * a[1],
|
|
norm_a_2 * b[2] - norm_b_2 * a[2]
|
|
};
|
|
mju_cross(res, vec, nrm);
|
|
center_[3*i+0] = res[0]/(2*area*area) + vert_[3*vertid[2]+0];
|
|
center_[3*i+1] = res[1]/(2*area*area) + vert_[3*vertid[2]+1];
|
|
center_[3*i+2] = res[2]/(2*area*area) + vert_[3*vertid[2]+2];
|
|
}
|
|
}
|
|
|
|
|
|
|
|
//------------------ class mjCSkin implementation --------------------------------------------------
|
|
|
|
// constructor
|
|
mjCSkin::mjCSkin(mjCModel* _model) {
|
|
// set model pointer
|
|
model = _model;
|
|
|
|
// clear data
|
|
file.clear();
|
|
material.clear();
|
|
rgba[0] = rgba[1] = rgba[2] = 0.5f;
|
|
rgba[3] = 1.0f;
|
|
inflate = 0;
|
|
group = 0;
|
|
|
|
vert.clear();
|
|
texcoord.clear();
|
|
face.clear();
|
|
|
|
bodyname.clear();
|
|
bindpos.clear();
|
|
bindquat.clear();
|
|
vertid.clear();
|
|
vertweight.clear();
|
|
bodyid.clear();
|
|
|
|
matid = -1;
|
|
}
|
|
|
|
|
|
|
|
// destructor
|
|
mjCSkin::~mjCSkin() {
|
|
file.clear();
|
|
material.clear();
|
|
vert.clear();
|
|
texcoord.clear();
|
|
face.clear();
|
|
bodyname.clear();
|
|
bindpos.clear();
|
|
bindquat.clear();
|
|
vertid.clear();
|
|
vertweight.clear();
|
|
bodyid.clear();
|
|
}
|
|
|
|
|
|
|
|
// compiler
|
|
void mjCSkin::Compile(const mjVFS* vfs) {
|
|
// load file
|
|
if (!file.empty()) {
|
|
// make sure data is not present
|
|
if (!vert.empty() ||
|
|
!texcoord.empty() ||
|
|
!face.empty() ||
|
|
!bodyname.empty() ||
|
|
!bindpos.empty() ||
|
|
!bindquat.empty() ||
|
|
!vertid.empty() ||
|
|
!vertweight.empty() ||
|
|
!bodyid.empty()) {
|
|
throw mjCError(this, "Data already exists, trying to load from skin file: %s", file.c_str());
|
|
}
|
|
|
|
// remove path from file if necessary
|
|
if (model->strippath) {
|
|
file = mjuu_strippath(file);
|
|
}
|
|
|
|
// load SKN
|
|
string ext = mjuu_getext(file);
|
|
if (strcasecmp(ext.c_str(), ".skn")) {
|
|
throw mjCError(this, "Unknown skin file type: %s", file.c_str());
|
|
}
|
|
|
|
string filename = mjuu_makefullname(model->modelfiledir, model->meshdir, file);
|
|
mjResource* resource = LoadResource(filename, 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
|
|
bodyid.resize(nbone);
|
|
for (int i=0; i<nbone; i++) {
|
|
mjCBase* pbody = model->FindObject(mjOBJ_BODY, bodyname[i]);
|
|
if (!pbody) {
|
|
throw mjCError(this, "unknown body '%s' in skin", bodyname[i].c_str());
|
|
}
|
|
bodyid[i] = pbody->id;
|
|
}
|
|
|
|
// resolve material name
|
|
mjCBase* pmat = model->FindObject(mjOBJ_MATERIAL, material);
|
|
if (pmat) {
|
|
matid = pmat->id;
|
|
} else if (!material.empty()) {
|
|
throw mjCError(this, "unkown material '%s' in skin", material.c_str());
|
|
}
|
|
|
|
// set total vertex weights to 0
|
|
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", NULL, 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", NULL, 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++) {
|
|
mjtNum quat[4] = {
|
|
(mjtNum)bindquat[4*i],
|
|
(mjtNum)bindquat[4*i+1],
|
|
(mjtNum)bindquat[4*i+2],
|
|
(mjtNum)bindquat[4*i+3]
|
|
};
|
|
mju_normalize4(quat);
|
|
|
|
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);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
//------------------ class mjCFlex implementation --------------------------------------------------
|
|
|
|
// 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);
|
|
}
|
|
};
|
|
|
|
// simplex connectivity
|
|
constexpr int kNumEdges[3] = {1, 3, 6};
|
|
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}}};
|
|
|
|
// constructor
|
|
mjCFlex::mjCFlex(mjCModel* _model) {
|
|
// set model
|
|
model = _model;
|
|
|
|
// set contact defaults
|
|
contype = 1;
|
|
conaffinity = 1;
|
|
condim = 3;
|
|
priority = 0;
|
|
mjuu_setvec(friction, 1, 0.005, 0.0001);
|
|
solmix = 1.0;
|
|
mj_defaultSolRefImp(solref, solimp);
|
|
margin = 0;
|
|
gap = 0;
|
|
|
|
// set other defaults
|
|
dim = 2;
|
|
radius = 0.005;
|
|
internal = true;
|
|
flatskin = false;
|
|
selfcollide = mjFLEXSELF_AUTO;
|
|
activelayers = 1;
|
|
group = 0;
|
|
edgestiffness = 0;
|
|
edgedamping = 0;
|
|
material.clear();
|
|
rgba[0] = rgba[1] = rgba[2] = 0.5f;
|
|
rgba[3] = 1.0f;
|
|
|
|
// clear internal variables
|
|
nvert = 0;
|
|
nedge = 0;
|
|
nelem = 0;
|
|
matid = -1;
|
|
rigid = false;
|
|
centered = false;
|
|
}
|
|
|
|
|
|
bool mjCFlex::HasTexcoord() const {
|
|
return !texcoord.empty();
|
|
}
|
|
|
|
|
|
void mjCFlex::DelTexcoord() {
|
|
texcoord.clear();
|
|
}
|
|
|
|
|
|
// compiler
|
|
void mjCFlex::Compile(const mjVFS* vfs) {
|
|
// 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()) {
|
|
throw mjCError(this, "vertbody is 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");
|
|
}
|
|
nelem = (int)elem.size()/(dim+1);
|
|
|
|
// 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;
|
|
}
|
|
}
|
|
if (nvert<dim+1) {
|
|
throw mjCError(this, "not enough vertices");
|
|
}
|
|
|
|
// check elem vertex ids
|
|
for (int i=0; i<(int)elem.size(); i++) {
|
|
if (elem[i]<0 || elem[i]>=nvert) {
|
|
throw mjCError(this, "elem vertex id out of range");
|
|
}
|
|
}
|
|
|
|
// check texcoord
|
|
if (!texcoord.empty() && texcoord.size()!=2*nvert) {
|
|
throw mjCError(this, "two texture coordinates per vertex expected");
|
|
}
|
|
|
|
// resolve material name
|
|
mjCBase* pmat = model->FindObject(mjOBJ_MATERIAL, material);
|
|
if (pmat) {
|
|
matid = pmat->id;
|
|
} else if (!material.empty()) {
|
|
throw mjCError(this, "unkown material '%s' in flex", material.c_str());
|
|
}
|
|
|
|
// resolve body ids
|
|
for (int i=0; i<(int)vertbody.size(); i++) {
|
|
mjCBase* pbody = model->FindObject(mjOBJ_BODY, vertbody[i]);
|
|
if (pbody) {
|
|
vertbodyid.push_back(pbody->id);
|
|
} else {
|
|
throw mjCError(this, "unkown body '%s' in flex", vertbody[i].c_str());
|
|
}
|
|
}
|
|
|
|
// process elements
|
|
for (int e=0; e<(int)elem.size()/(dim+1); e++) {
|
|
// make sorted copy of element
|
|
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) {
|
|
rigid = true;
|
|
for (int i=1; i<(int)vertbodyid.size(); i++) {
|
|
if (vertbodyid[i]!=vertbodyid[0]) {
|
|
rigid = false;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
// determine centered if not already set
|
|
if (!centered) {
|
|
centered = true;
|
|
for (int i=0; i<(int)vert.size(); i++) {
|
|
if (vert[i]!=0) {
|
|
centered = false;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
// compute global vertex positions
|
|
vertxpos = vector<mjtNum> (3*nvert);
|
|
for (int i=0; i<nvert; i++) {
|
|
// get body id, set vertxpos = body.xpos0
|
|
int b = rigid ? vertbodyid[0] : vertbodyid[i];
|
|
mju_copy3(vertxpos.data()+3*i, model->bodies[b]->xpos0);
|
|
|
|
// add vertex offset within body if not centered
|
|
if (!centered) {
|
|
mjtNum offset[3];
|
|
mju_rotVecQuat(offset, vert.data()+3*i, model->bodies[b]->xquat0);
|
|
mju_addTo3(vertxpos.data()+3*i, offset);
|
|
}
|
|
}
|
|
|
|
// 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);
|
|
mjtNum* v0 = vertxpos.data() + 3*edata[0];
|
|
mjtNum* v1 = vertxpos.data() + 3*edata[1];
|
|
mjtNum* v2 = vertxpos.data() + 3*edata[2];
|
|
mjtNum* v3 = vertxpos.data() + 3*edata[3];
|
|
mjtNum v01[3] = {v1[0]-v0[0], v1[1]-v0[1], v1[2]-v0[2]};
|
|
mjtNum v02[3] = {v2[0]-v0[0], v2[1]-v0[1], v2[2]-v0[2]};
|
|
mjtNum v03[3] = {v3[0]-v0[0], v3[1]-v0[1], v3[2]-v0[2]};
|
|
|
|
// detect wrong orientation
|
|
mjtNum nrm[3];
|
|
mju_cross(nrm, v01, v02);
|
|
if (mju_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
|
|
std::vector<int> edgeidx(elem.size()*kNumEdges[dim-1]);
|
|
|
|
// 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 (int f = 0; f < (int)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(
|
|
std::min(v[eledge[dim-1][e][0]], v[eledge[dim-1][e][1]]),
|
|
std::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();
|
|
|
|
// add plugins
|
|
std::string userface, useredge;
|
|
mjXUtil::Vector2String(userface, elem);
|
|
mjXUtil::Vector2String(useredge, edgeidx);
|
|
|
|
for (int i=0; i<(int)vertbodyid.size(); i++) {
|
|
if (model->bodies[vertbodyid[i]]->plugin_instance) {
|
|
model->bodies[vertbodyid[i]]->plugin_instance->config_attribs["face"] = userface;
|
|
model->bodies[vertbodyid[i]]->plugin_instance->config_attribs["edge"] = useredge;
|
|
}
|
|
}
|
|
|
|
// create shell fragments and element-vertex collision pairs
|
|
CreateShellPair();
|
|
|
|
// create bounding volume hierarchy
|
|
CreateBVH();
|
|
}
|
|
|
|
|
|
|
|
// create flex BVH
|
|
void mjCFlex::CreateBVH(void) {
|
|
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
|
|
mjtNum xmin[3], xmax[3];
|
|
mju_copy3(xmin, vertxpos.data() + 3*edata[0]);
|
|
mju_copy3(xmax, vertxpos.data() + 3*edata[0]);
|
|
for (int i=1; i<=dim; i++) {
|
|
for (int j=0; j<3; j++) {
|
|
xmin[j] = mjMIN(xmin[j], vertxpos[3*edata[i]+j]);
|
|
xmax[j] = mjMAX(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
|
|
mjCBoundingVolume* bv = tree.GetBoundingVolume(nbvh++);
|
|
bv->contype = contype;
|
|
bv->conaffinity = conaffinity;
|
|
bv->quat = NULL;
|
|
bv->SetId(e);
|
|
bv->aabb = elemaabb.data() + 6*e;
|
|
bv->pos = bv->aabb;
|
|
}
|
|
|
|
// create hierarchy
|
|
tree.RemoveInactiveVolumes(nbvh);
|
|
tree.CreateBVH();
|
|
}
|
|
|
|
|
|
|
|
// create shells and element-vertex collision pairs
|
|
void mjCFlex::CreateShellPair(void) {
|
|
vector<vector<int>> fragspec(nelem*(dim+1)); // [sorted frag vertices, elem, original frag vertices]
|
|
vector<vector<int>> connectspec; // [elem1, elem2, common sorted frag vertices]
|
|
vector<bool> border(nelem, false); // is element on the border
|
|
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
|
|
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
|
|
vector<int> previous = {fragspec[n-1].begin(), fragspec[n-1].begin()+dim};
|
|
vector<int> current = {fragspec[n].begin(), fragspec[n].begin()+dim};
|
|
|
|
// same sequential fragments
|
|
if (previous==current) {
|
|
// found pair of elements connected by common fragment
|
|
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 (int i=0; i<(int)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 = vector<int> (nelem, 0);
|
|
}
|
|
else {
|
|
elemlayer = 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 (int i=0; i<(int)connectspec.size(); i++) {
|
|
int e1 = connectspec[i][0]; // get element pair for this edge
|
|
int e2 = connectspec[i][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 (int n=0; n<(int)connectspec.size(); n++) {
|
|
if (border[connectspec[n][0]] || border[connectspec[n][1]]) {
|
|
// extract common fragment
|
|
vector<int> frag = {connectspec[n].begin()+2, connectspec[n].end()};
|
|
|
|
// process both elements
|
|
for (int ei=0; ei<2; ei++) {
|
|
const int* edata = elem.data() + connectspec[n][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(connectspec[n][1-ei]);
|
|
evpair.push_back(edata[i]);
|
|
|
|
// one such vertex exists
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|