Kyle Bayes
39896f8064
PiperOrigin-RevId: 662496475 Change-Id: I4c3a58a63ef2c3b1196b3e6b30eb556b5528ff7f
1694 lines
48 KiB
C++
1694 lines
48 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 <climits>
|
|
#include <cmath>
|
|
#include <cstddef>
|
|
#include <cstdio>
|
|
#include <cstring>
|
|
#include <iostream>
|
|
#include <sstream>
|
|
#include <stdexcept>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
#include <mujoco/mjmacro.h>
|
|
#include <mujoco/mjmodel.h>
|
|
#include <mujoco/mjtnum.h>
|
|
#include <mujoco/mjplugin.h>
|
|
#include <mujoco/mujoco.h>
|
|
#include "cc/array_safety.h"
|
|
#include "engine/engine_crossplatform.h"
|
|
#include "engine/engine_util_errmem.h"
|
|
#include "user/user_flexcomp.h"
|
|
#include <mujoco/mjspec.h>
|
|
#include "user/user_model.h"
|
|
#include "user/user_objects.h"
|
|
#include "user/user_resource.h"
|
|
#include "user/user_util.h"
|
|
|
|
namespace {
|
|
namespace mju = ::mujoco::util;
|
|
using std::vector;
|
|
using std::stringstream;
|
|
} // namespace
|
|
|
|
// strncpy with 0, return false
|
|
static bool comperr(char* error, const char* msg, int error_sz) {
|
|
mju_strncpy(error, msg, error_sz);
|
|
return false;
|
|
}
|
|
|
|
// 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));
|
|
}
|
|
|
|
|
|
static void ReadStrFromBuffer(char* dest, const char* src, int maxlen) {
|
|
std::strncpy(dest, src, maxlen);
|
|
}
|
|
|
|
bool IsValidElementOrNodeHeader22(const std::string& line) {
|
|
// making sure characters are numbers
|
|
for (char c : line) {
|
|
if (!std::isdigit(c)) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
|
|
// constructor: set defaults outside mjCDef
|
|
mjCFlexcomp::mjCFlexcomp(void) {
|
|
type = mjFCOMPTYPE_GRID;
|
|
count[0] = count[1] = count[2] = 10;
|
|
mjuu_setvec(spacing, 0.02, 0.02, 0.02);
|
|
mjuu_setvec(scale, 1, 1, 1);
|
|
mass = 1;
|
|
inertiabox = 0.005;
|
|
equality = false;
|
|
mjuu_setvec(pos, 0, 0, 0);
|
|
mjuu_setvec(quat, 1, 0, 0, 0);
|
|
rigid = false;
|
|
centered = false;
|
|
|
|
mjs_defaultPlugin(&plugin);
|
|
mjs_defaultOrientation(&alt);
|
|
plugin_name = "";
|
|
plugin_instance_name = "";
|
|
plugin.name = (mjString*)&plugin_name;
|
|
plugin.instance_name = (mjString*)&plugin_instance_name;
|
|
}
|
|
|
|
|
|
|
|
// make flexcomp object
|
|
bool mjCFlexcomp::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) {
|
|
mjCModel* model = (mjCModel*)spec->element;
|
|
mjsFlex* dflex = def.spec.flex;
|
|
int dim = dflex->dim;
|
|
bool radial = (type == mjFCOMPTYPE_BOX ||
|
|
type == mjFCOMPTYPE_CYLINDER ||
|
|
type == mjFCOMPTYPE_ELLIPSOID);
|
|
bool direct = (type == mjFCOMPTYPE_DIRECT ||
|
|
type == mjFCOMPTYPE_MESH ||
|
|
type == mjFCOMPTYPE_GMSH);
|
|
|
|
// check parent body name
|
|
if (std::string(mjs_getString(body->name)).empty()) {
|
|
return comperr(error, "Parent body must have name", error_sz);
|
|
}
|
|
|
|
// check dim
|
|
if (dim < 1 || dim > 3) {
|
|
return comperr(error, "Invalid dim, must be between 1 and 3", error_sz);
|
|
}
|
|
|
|
// check counts
|
|
for (int i=0; i < 3; i++) {
|
|
if (count[i] < 1 || ((radial && count[i] < 2) && dim == 3)) {
|
|
return comperr(error, "Count too small", error_sz);
|
|
}
|
|
}
|
|
|
|
// check spacing
|
|
double minspace = 2*dflex->radius + dflex->margin;
|
|
if (!direct) {
|
|
if (spacing[0] < minspace ||
|
|
spacing[1] < minspace ||
|
|
spacing[2] < minspace) {
|
|
return comperr(error, "Spacing must be larger than geometry size", error_sz);
|
|
}
|
|
}
|
|
|
|
// check scale
|
|
if (scale[0]<mjMINVAL || scale[1]<mjMINVAL || scale[2]<mjMINVAL) {
|
|
return comperr(error, "Scale must be larger than mjMINVAL", error_sz);
|
|
}
|
|
|
|
// check mass and inertia
|
|
if (mass<mjMINVAL || inertiabox<mjMINVAL) {
|
|
return comperr(error, "Mass and inertiabox must be larger than mjMINVAL", error_sz);
|
|
}
|
|
|
|
// compute orientation
|
|
const char* alterr = mjs_resolveOrientation(quat, model->spec.degree, model->spec.eulerseq, &alt);
|
|
if (alterr) {
|
|
return comperr(error, alterr, error_sz);
|
|
}
|
|
|
|
// type-specific constructor: populate point and element, possibly set dim
|
|
bool res;
|
|
switch (type) {
|
|
case mjFCOMPTYPE_GRID:
|
|
res = MakeGrid(error, error_sz);
|
|
break;
|
|
|
|
case mjFCOMPTYPE_BOX:
|
|
case mjFCOMPTYPE_CYLINDER:
|
|
case mjFCOMPTYPE_ELLIPSOID:
|
|
res = MakeBox(error, error_sz);
|
|
break;
|
|
|
|
case mjFCOMPTYPE_SQUARE:
|
|
case mjFCOMPTYPE_DISC:
|
|
res = MakeSquare(error, error_sz);
|
|
break;
|
|
|
|
case mjFCOMPTYPE_MESH:
|
|
res = MakeMesh(model, error, error_sz);
|
|
break;
|
|
|
|
case mjFCOMPTYPE_GMSH:
|
|
res = MakeGMSH(model, error, error_sz);
|
|
break;
|
|
|
|
case mjFCOMPTYPE_DIRECT:
|
|
res = true;
|
|
break;
|
|
|
|
default:
|
|
return comperr(error, "Unknown flexcomp type", error_sz);
|
|
}
|
|
if (!res) {
|
|
return false;
|
|
}
|
|
|
|
// force flatskin shading for box, cylinder and 3D grid
|
|
if (type == mjFCOMPTYPE_BOX || type == mjFCOMPTYPE_CYLINDER ||
|
|
(type == mjFCOMPTYPE_GRID && dim == 3)) {
|
|
dflex->flatskin = true;
|
|
}
|
|
|
|
// check pin sizes
|
|
if (pinrange.size()%2) {
|
|
return comperr(error, "Pin range number must be multiple of 2", error_sz);
|
|
}
|
|
if (pingrid.size()%dim) {
|
|
return comperr(error, "Pin grid number must be multiple of dim", error_sz);
|
|
}
|
|
if (pingridrange.size()%(2*dim)) {
|
|
return comperr(error, "Pin grid range number of must be multiple of 2*dim", error_sz);
|
|
}
|
|
if (type != mjFCOMPTYPE_GRID && !(pingrid.empty() && pingridrange.empty())) {
|
|
return comperr(error, "Pin grid(range) can only be used with grid type", error_sz);
|
|
}
|
|
if (dim == 1 && !(pingrid.empty() && pingridrange.empty())) {
|
|
return comperr(error, "Pin grid(range) cannot be used with dim=1", error_sz);
|
|
}
|
|
|
|
// require element and point
|
|
if (point.empty() || element.empty()) {
|
|
return comperr(error, "Point and element required", error_sz);
|
|
}
|
|
|
|
// check point size
|
|
if (point.size()%3) {
|
|
return comperr(error, "Point size must be a multiple of 3", error_sz);
|
|
}
|
|
|
|
// check element size
|
|
if (element.size()%(dim+1)) {
|
|
return comperr(error, "Element size must be a multiple of dim+1", error_sz);
|
|
}
|
|
|
|
// get number of points
|
|
int npnt = point.size()/3;
|
|
|
|
// check elem vertex ids
|
|
for (int i=0; i < (int)element.size(); i++) {
|
|
if (element[i] < 0 || element[i] >= npnt) {
|
|
char msg[100];
|
|
snprintf(msg, sizeof(msg), "element %d has point id %d, number of points is %d", i,
|
|
element[i], npnt);
|
|
return comperr(error, msg, error_sz);
|
|
}
|
|
}
|
|
|
|
// apply scaling for direct types
|
|
if (direct && (scale[0] != 1 || scale[1] != 1 || scale[2] != 1)) {
|
|
for (int i=0; i < npnt; i++) {
|
|
point[3*i] *= scale[0];
|
|
point[3*i+1] *= scale[1];
|
|
point[3*i+2] *= scale[2];
|
|
}
|
|
}
|
|
|
|
// apply pose transform to points
|
|
for (int i=0; i < npnt; i++) {
|
|
double newp[3], oldp[3] = {point[3*i], point[3*i+1], point[3*i+2]};
|
|
mjuu_trnVecPose(newp, pos, quat, oldp);
|
|
point[3*i] = newp[0];
|
|
point[3*i+1] = newp[1];
|
|
point[3*i+2] = newp[2];
|
|
}
|
|
|
|
// construct pinned array
|
|
pinned = vector<bool>(npnt, rigid);
|
|
|
|
// handle pins if user did not specify rigid
|
|
if (!rigid) {
|
|
// process pinid
|
|
for (int i=0; i < (int)pinid.size(); i++) {
|
|
// check range
|
|
if (pinid[i] < 0 || pinid[i] >= npnt) {
|
|
return comperr(error, "pinid out of range", error_sz);
|
|
}
|
|
|
|
// set
|
|
pinned[pinid[i]] = true;
|
|
}
|
|
|
|
// process pinrange
|
|
for (int i=0; i < (int)pinrange.size(); i+=2) {
|
|
// check range
|
|
if (pinrange[i] < 0 || pinrange[i] >= npnt ||
|
|
pinrange[i+1] < 0 || pinrange[i+1] >= npnt) {
|
|
return comperr(error, "pinrange out of range", error_sz);
|
|
}
|
|
|
|
// set
|
|
for (int k=pinrange[i]; k <= pinrange[i+1]; k++) {
|
|
pinned[k] = true;
|
|
}
|
|
}
|
|
|
|
// process pingrid
|
|
for (int i=0; i < (int)pingrid.size(); i+=dim) {
|
|
// check range
|
|
for (int k=0; k < dim; k++) {
|
|
if (pingrid[i+k] < 0 || pingrid[i+k] >= count[k]) {
|
|
return comperr(error, "pingrid out of range", error_sz);
|
|
}
|
|
}
|
|
|
|
// set
|
|
if (dim == 2) {
|
|
pinned[GridID(pingrid[i], pingrid[i+1])] = true;
|
|
}
|
|
else if (dim == 3) {
|
|
pinned[GridID(pingrid[i], pingrid[i+1], pingrid[i+2])] = true;
|
|
}
|
|
}
|
|
|
|
// process pingridrange
|
|
for (int i=0; i < (int)pingridrange.size(); i+=2*dim) {
|
|
// check range
|
|
for (int k=0; k < 2*dim; k++) {
|
|
if (pingridrange[i+k] < 0 || pingridrange[i+k] >= count[k%dim]) {
|
|
return comperr(error, "pingridrange out of range", error_sz);
|
|
}
|
|
}
|
|
|
|
// set
|
|
if (dim == 2) {
|
|
for (int ix=pingridrange[i]; ix <= pingridrange[i+2]; ix++) {
|
|
for (int iy=pingridrange[i+1]; iy <= pingridrange[i+3]; iy++) {
|
|
pinned[GridID(ix, iy)] = true;
|
|
}
|
|
}
|
|
}
|
|
else if (dim==3) {
|
|
for (int ix=pingridrange[i]; ix <= pingridrange[i+3]; ix++) {
|
|
for (int iy=pingridrange[i+1]; iy <= pingridrange[i+4]; iy++) {
|
|
for (int iz=pingridrange[i+2]; iz <= pingridrange[i+5]; iz++) {
|
|
pinned[GridID(ix, iy, iz)] = true;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// center of radial body is always pinned
|
|
if (radial) {
|
|
pinned[0] = true;
|
|
}
|
|
|
|
// check if all or none are pinned
|
|
bool allpin = true, nopin = true;
|
|
for (int i=0; i < npnt; i++) {
|
|
if (pinned[i]) {
|
|
nopin = false;
|
|
}
|
|
else {
|
|
allpin = false;
|
|
}
|
|
}
|
|
|
|
// adjust rigid and centered
|
|
if (allpin) {
|
|
rigid = true;
|
|
}
|
|
else if (nopin) {
|
|
centered = true;
|
|
}
|
|
}
|
|
|
|
// remove unreferenced for direct, mesh, gmsh
|
|
if (direct) {
|
|
// find used
|
|
used = std::vector<bool> (npnt, false);
|
|
for (int i=0; i < (int)element.size(); i++) {
|
|
used[element[i]] = true;
|
|
}
|
|
|
|
// construct reindex
|
|
bool hasunused = false;
|
|
std::vector<int> reindex (npnt, 0);
|
|
for (int i=0; i < npnt; i++) {
|
|
if (!used[i]) {
|
|
hasunused = true;
|
|
for (int k=i+1; k < npnt; k++) {
|
|
reindex[k]--;
|
|
}
|
|
}
|
|
}
|
|
|
|
// reindex elements if unused present
|
|
if (hasunused) {
|
|
for (int i=0; i < (int)element.size(); i++) {
|
|
element[i] += reindex[element[i]];
|
|
}
|
|
}
|
|
}
|
|
|
|
// nothing to remove for auto-generated types
|
|
else {
|
|
used = std::vector<bool> (npnt, true);
|
|
}
|
|
|
|
// create flex, copy parameters
|
|
mjCFlex* flex = model->AddFlex();
|
|
mjsFlex* pf = &flex->spec;
|
|
int id = flex->id;
|
|
|
|
*flex = def.Flex();
|
|
flex->PointToLocal();
|
|
|
|
flex->model = model;
|
|
flex->id = id;
|
|
mjs_setString(pf->name, name.c_str());
|
|
mjs_setInt(pf->elem, element.data(), element.size());
|
|
mjs_setFloat(pf->texcoord, texcoord.data(), texcoord.size());
|
|
if (!centered) {
|
|
mjs_setDouble(pf->vert, point.data(), point.size());
|
|
}
|
|
|
|
// rigid: set parent name, nothing else to do
|
|
if (rigid) {
|
|
mjs_appendString(pf->vertbody, mjs_getString(body->name));
|
|
return true;
|
|
}
|
|
|
|
// compute body mass and inertia matching specs
|
|
double bodymass = mass/npnt;
|
|
double bodyinertia = bodymass*(2.0*inertiabox*inertiabox)/3.0;
|
|
|
|
// overwrite plugin name
|
|
if (plugin.active && plugin_instance_name.empty()) {
|
|
plugin_instance_name = "flexcomp_" + name;
|
|
static_cast<mjCPlugin*>(plugin.instance)->name = plugin_instance_name;
|
|
}
|
|
|
|
// create bodies, construct flex vert and vertbody
|
|
for (int i=0; i < npnt; i++) {
|
|
// not used: skip
|
|
if (!used[i]) {
|
|
continue;
|
|
}
|
|
|
|
// pinned: parent body
|
|
if (pinned[i]) {
|
|
mjs_appendString(pf->vertbody, mjs_getString(body->name));
|
|
|
|
// add plugin
|
|
if (plugin.active) {
|
|
mjsPlugin* pplugin = &body->plugin;
|
|
pplugin->active = true;
|
|
pplugin->instance = static_cast<mjsElement*>(plugin.instance);
|
|
mjs_setString(pplugin->name, mjs_getString(plugin.name));
|
|
mjs_setString(pplugin->instance_name, plugin_instance_name.c_str());
|
|
}
|
|
}
|
|
|
|
// not pinned: new body
|
|
else {
|
|
// add new body at vertex coordinates
|
|
mjsBody* pb = mjs_addBody(body, 0);
|
|
|
|
// set frame and inertial
|
|
pb->pos[0] = point[3*i];
|
|
pb->pos[1] = point[3*i+1];
|
|
pb->pos[2] = point[3*i+2];
|
|
mjuu_zerovec(pb->ipos, 3);
|
|
pb->mass = bodymass;
|
|
pb->inertia[0] = bodyinertia;
|
|
pb->inertia[1] = bodyinertia;
|
|
pb->inertia[2] = bodyinertia;
|
|
pb->explicitinertial = true;
|
|
|
|
// add radial slider
|
|
if (radial) {
|
|
mjsJoint* jnt = mjs_addJoint(pb, 0);
|
|
|
|
// set properties
|
|
jnt->type = mjJNT_SLIDE;
|
|
mjuu_setvec(jnt->pos, 0, 0, 0);
|
|
mjuu_copyvec(jnt->axis, pb->pos, 3);
|
|
mjuu_normvec(jnt->axis, 3);
|
|
}
|
|
|
|
// add three orthogonal sliders
|
|
else {
|
|
for (int j=0; j < 3; j++) {
|
|
// add joint to body
|
|
mjsJoint* jnt = mjs_addJoint(pb, 0);
|
|
|
|
// set properties
|
|
jnt->type = mjJNT_SLIDE;
|
|
mjuu_setvec(jnt->pos, 0, 0, 0);
|
|
mjuu_setvec(jnt->axis, 0, 0, 0);
|
|
jnt->axis[j] = 1;
|
|
}
|
|
}
|
|
|
|
// construct body name, add to vertbody
|
|
char txt[100];
|
|
mju::sprintf_arr(txt, "%s_%d", name.c_str(), i);
|
|
mjs_setString(pb->name, txt);
|
|
mjs_appendString(pf->vertbody, mjs_getString(pb->name));
|
|
|
|
// clear flex vertex coordinates if allocated
|
|
if (!centered) {
|
|
point[3*i] = 0;
|
|
point[3*i+1] = 0;
|
|
point[3*i+2] = 0;
|
|
}
|
|
|
|
// add plugin
|
|
if (plugin.active) {
|
|
mjsPlugin* pplugin = &pb->plugin;
|
|
pplugin->active = true;
|
|
pplugin->instance = static_cast<mjsElement*>(plugin.instance);
|
|
mjs_setString(pplugin->name, mjs_getString(plugin.name));
|
|
mjs_setString(pplugin->instance_name, plugin_instance_name.c_str());
|
|
}
|
|
}
|
|
}
|
|
|
|
if (!centered) {
|
|
mjs_setDouble(pf->vert, point.data(), point.size());
|
|
}
|
|
|
|
// create edge equality constraint
|
|
if (equality) {
|
|
mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec);
|
|
mjs_setDefault(pe->element, &model->Default()->spec);
|
|
pe->type = mjEQ_FLEX;
|
|
pe->active = true;
|
|
mjs_setString(pe->name1, name.c_str());
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
// get point id from grid coordinates
|
|
int mjCFlexcomp::GridID(int ix, int iy) {
|
|
return ix*count[1] + iy;
|
|
}
|
|
int mjCFlexcomp::GridID(int ix, int iy, int iz) {
|
|
return ix*count[1]*count[2] + iy*count[2] + iz;
|
|
}
|
|
|
|
|
|
|
|
// make grid
|
|
bool mjCFlexcomp::MakeGrid(char* error, int error_sz) {
|
|
int dim = def.Flex().spec.dim;
|
|
bool hastex = texcoord.empty();
|
|
|
|
// 1D
|
|
if (dim == 1) {
|
|
for (int ix=0; ix < count[0]; ix++) {
|
|
// add point
|
|
point.push_back(spacing[0]*(ix - 0.5*(count[0]-1)));
|
|
point.push_back(0);
|
|
point.push_back(0);
|
|
|
|
// add element
|
|
if (ix < count[0]-1) {
|
|
element.push_back(ix);
|
|
element.push_back(ix+1);
|
|
}
|
|
}
|
|
}
|
|
|
|
// 2D
|
|
else if (dim == 2) {
|
|
for (int ix=0; ix < count[0]; ix++) {
|
|
for (int iy=0; iy < count[1]; iy++) {
|
|
int quad2tri[2][3] = {{0, 1, 2}, {0, 2, 3}};
|
|
|
|
// add point
|
|
double pos[2] = {spacing[0]*(ix - 0.5*(count[0]-1)),
|
|
spacing[1]*(iy - 0.5*(count[1]-1))};
|
|
point.push_back(pos[0]);
|
|
point.push_back(pos[1]);
|
|
point.push_back(0);
|
|
|
|
// add texture coordinates, if not specified explicitly
|
|
if (!hastex) {
|
|
texcoord.push_back(ix/(double)std::max(count[0]-1, 1));
|
|
texcoord.push_back(iy/(double)std::max(count[1]-1, 1));
|
|
}
|
|
|
|
// flip triangles if radial projection is requested
|
|
if (((pos[0] < -mjEPS && pos[1] > -mjEPS) ||
|
|
(pos[0] > -mjEPS && pos[1] < -mjEPS)) &&
|
|
type == mjFCOMPTYPE_DISC) {
|
|
quad2tri[0][2] = 3;
|
|
quad2tri[1][0] = 1;
|
|
}
|
|
|
|
// add elements
|
|
if (ix < count[0]-1 && iy < count[1]-1) {
|
|
int vert[4] = {
|
|
count[2]*count[1]*(ix+0) + count[2]*(iy+0),
|
|
count[2]*count[1]*(ix+1) + count[2]*(iy+0),
|
|
count[2]*count[1]*(ix+1) + count[2]*(iy+1),
|
|
count[2]*count[1]*(ix+0) + count[2]*(iy+1),
|
|
};
|
|
for (int s =0; s < 2; s++) {
|
|
for (int v=0; v < 3; v++) {
|
|
element.push_back(vert[quad2tri[s][v]]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// 3D
|
|
else {
|
|
int cube2tets[6][4] = {{0, 3, 1, 7}, {0, 1, 4, 7},
|
|
{1, 3, 2, 7}, {1, 2, 6, 7},
|
|
{1, 5, 4, 7}, {1, 6, 5, 7}};
|
|
for (int ix=0; ix < count[0]; ix++) {
|
|
for (int iy=0; iy < count[1]; iy++) {
|
|
for (int iz=0; iz < count[2]; iz++) {
|
|
// add point
|
|
point.push_back(spacing[0]*(ix - 0.5*(count[0]-1)));
|
|
point.push_back(spacing[1]*(iy - 0.5*(count[1]-1)));
|
|
point.push_back(spacing[2]*(iz - 0.5*(count[2]-1)));
|
|
|
|
// add elements
|
|
if (ix < count[0]-1 && iy < count[1]-1 && iz < count[2]-1) {
|
|
int vert[8] = {
|
|
count[2]*count[1]*(ix+0) + count[2]*(iy+0) + iz+0,
|
|
count[2]*count[1]*(ix+1) + count[2]*(iy+0) + iz+0,
|
|
count[2]*count[1]*(ix+1) + count[2]*(iy+1) + iz+0,
|
|
count[2]*count[1]*(ix+0) + count[2]*(iy+1) + iz+0,
|
|
count[2]*count[1]*(ix+0) + count[2]*(iy+0) + iz+1,
|
|
count[2]*count[1]*(ix+1) + count[2]*(iy+0) + iz+1,
|
|
count[2]*count[1]*(ix+1) + count[2]*(iy+1) + iz+1,
|
|
count[2]*count[1]*(ix+0) + count[2]*(iy+1) + iz+1,
|
|
};
|
|
for (int s=0; s < 6; s++) {
|
|
for (int v=0; v < 4; v++) {
|
|
element.push_back(vert[cube2tets[s][v]]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// check elements
|
|
if (element.empty()) {
|
|
return comperr(error, "No elements were created in grid", error_sz);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
// get point id from box coordinates and side
|
|
int mjCFlexcomp::BoxID(int ix, int iy, int iz) {
|
|
// side iz=0
|
|
if (iz == 0) {
|
|
return ix*count[1] + iy + 1;
|
|
}
|
|
|
|
// side iz=max
|
|
else if (iz == count[2]-1) {
|
|
return count[0]*count[1] + ix*count[1] + iy + 1;
|
|
}
|
|
|
|
// side iy=0
|
|
else if (iy == 0) {
|
|
return 2*count[0]*count[1] + ix*(count[2]-2) + iz - 1 + 1;
|
|
}
|
|
|
|
// side iy=max
|
|
else if (iy == count[1]-1) {
|
|
return 2*count[0]*count[1] + count[0]*(count[2]-2) + ix*(count[2]-2) + iz - 1 + 1;
|
|
}
|
|
|
|
// side ix=0
|
|
else if (ix == 0) {
|
|
return 2*count[0]*count[1] + 2*count[0]*(count[2]-2) + (iy-1)*(count[2]-2) + iz - 1 + 1;
|
|
}
|
|
|
|
// side ix=max
|
|
else {
|
|
return 2*count[0]*count[1] + 2*count[0]*(count[2]-2) + (count[1]-2)*(count[2]-2) +
|
|
(iy-1)*(count[2]-2) + iz - 1 + 1;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// project from box to other shape
|
|
void mjCFlexcomp::BoxProject(double* pos, int ix, int iy, int iz) {
|
|
// init point
|
|
pos[0] = 2.0*ix/(count[0]-1) - 1;
|
|
pos[1] = 2.0*iy/(count[1]-1) - 1;
|
|
pos[2] = 2.0*iz/(count[2]-1) - 1;
|
|
|
|
// determine sizes
|
|
double size[3] = {
|
|
0.5*spacing[0]*(count[0]-1),
|
|
0.5*spacing[1]*(count[1]-1),
|
|
0.5*spacing[2]*(count[2]-1)
|
|
};
|
|
|
|
// box
|
|
if (type==mjFCOMPTYPE_BOX) {
|
|
pos[0] *= size[0];
|
|
pos[1] *= size[1];
|
|
pos[2] *= size[2];
|
|
}
|
|
|
|
// cylinder
|
|
else if (type==mjFCOMPTYPE_CYLINDER) {
|
|
double L0 = std::max(std::abs(pos[0]), std::abs(pos[1]));
|
|
mjuu_normvec(pos, 2);
|
|
pos[0] *= size[0]*L0;
|
|
pos[1] *= size[1]*L0;
|
|
pos[2] *= size[2];
|
|
}
|
|
|
|
// ellipsoid
|
|
else if (type==mjFCOMPTYPE_ELLIPSOID) {
|
|
mjuu_normvec(pos, 3);
|
|
pos[0] *= size[0];
|
|
pos[1] *= size[1];
|
|
pos[2] *= size[2];
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// make 2d square or disc
|
|
bool mjCFlexcomp::MakeSquare(char* error, int error_sz) {
|
|
// set 2D
|
|
def.spec.flex->dim = 2;
|
|
|
|
// create square
|
|
if (!MakeGrid(error, error_sz)) {
|
|
return false;
|
|
}
|
|
|
|
// do projection
|
|
if (type==mjFCOMPTYPE_DISC) {
|
|
double size[2] = {
|
|
0.5*spacing[0]*(count[0]-1),
|
|
0.5*spacing[1]*(count[1]-1),
|
|
};
|
|
|
|
for (int i=0; i < point.size()/3; i++) {
|
|
double* pos = point.data() + i*3;
|
|
double L0 = std::max(std::abs(pos[0]), std::abs(pos[1]));
|
|
mjuu_normvec(pos, 2);
|
|
pos[0] *= size[0]*L0;
|
|
pos[1] *= size[1]*L0;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
// make 3d box, ellipsoid or cylinder
|
|
bool mjCFlexcomp::MakeBox(char* error, int error_sz) {
|
|
double pos[3];
|
|
|
|
// set 3D
|
|
def.spec.flex->dim = 3;
|
|
|
|
// add center point
|
|
point.push_back(0);
|
|
point.push_back(0);
|
|
point.push_back(0);
|
|
|
|
// iz=0/max
|
|
for (int iz=0; iz < count[2]; iz+=count[2]-1) {
|
|
for (int ix=0; ix < count[0]; ix++) {
|
|
for (int iy=0; iy < count[1]; iy++) {
|
|
// add point
|
|
BoxProject(pos, ix, iy, iz);
|
|
point.push_back(pos[0]);
|
|
point.push_back(pos[1]);
|
|
point.push_back(pos[2]);
|
|
|
|
// add elements
|
|
if (ix < count[0]-1 && iy < count[1]-1) {
|
|
element.push_back(0);
|
|
element.push_back(BoxID(ix, iy, iz));
|
|
element.push_back(BoxID(ix+1, iy, iz));
|
|
element.push_back(BoxID(ix+1, iy+1, iz));
|
|
|
|
element.push_back(0);
|
|
element.push_back(BoxID(ix, iy, iz));
|
|
element.push_back(BoxID(ix, iy+1, iz));
|
|
element.push_back(BoxID(ix+1, iy+1, iz));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// iy=0/max
|
|
for (int iy=0; iy < count[1]; iy+=count[1]-1) {
|
|
for (int ix=0; ix < count[0]; ix++) {
|
|
for (int iz=0; iz < count[2]; iz++) {
|
|
// add point
|
|
if (iz>0 && iz < count[2]-1) {
|
|
BoxProject(pos, ix, iy, iz);
|
|
point.push_back(pos[0]);
|
|
point.push_back(pos[1]);
|
|
point.push_back(pos[2]);
|
|
}
|
|
|
|
// add elements
|
|
if (ix < count[0]-1 && iz < count[2]-1) {
|
|
element.push_back(0);
|
|
element.push_back(BoxID(ix, iy, iz));
|
|
element.push_back(BoxID(ix+1, iy, iz));
|
|
element.push_back(BoxID(ix+1, iy, iz+1));
|
|
|
|
element.push_back(0);
|
|
element.push_back(BoxID(ix, iy, iz));
|
|
element.push_back(BoxID(ix, iy, iz+1));
|
|
element.push_back(BoxID(ix+1, iy, iz+1));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// ix=0/max
|
|
for (int ix=0; ix < count[0]; ix+=count[0]-1) {
|
|
for (int iy=0; iy < count[1]; iy++) {
|
|
for (int iz=0; iz < count[2]; iz++) {
|
|
// add point
|
|
if (iz > 0 && iz < count[2]-1 && iy > 0 && iy < count[1]-1) {
|
|
BoxProject(pos, ix, iy, iz);
|
|
point.push_back(pos[0]);
|
|
point.push_back(pos[1]);
|
|
point.push_back(pos[2]);
|
|
}
|
|
|
|
// add elements
|
|
if (iy < count[1]-1 && iz < count[2]-1) {
|
|
element.push_back(0);
|
|
element.push_back(BoxID(ix, iy, iz));
|
|
element.push_back(BoxID(ix, iy+1, iz));
|
|
element.push_back(BoxID(ix, iy+1, iz+1));
|
|
|
|
element.push_back(0);
|
|
element.push_back(BoxID(ix, iy, iz));
|
|
element.push_back(BoxID(ix, iy, iz+1));
|
|
element.push_back(BoxID(ix, iy+1, iz+1));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
// copied from user_mesh.cc
|
|
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;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// make mesh
|
|
bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) {
|
|
// strip path
|
|
if (!file.empty() && model->spec.strippath) {
|
|
file = mjuu_strippath(file);
|
|
}
|
|
|
|
// file is required
|
|
if (file.empty()) {
|
|
return comperr(error, "File is required", error_sz);
|
|
}
|
|
|
|
// get extension and check; must be STL, OBJ or MSH
|
|
std::string ext = mjuu_getext(file);
|
|
if (strcasecmp(ext.c_str(), ".stl") &&
|
|
strcasecmp(ext.c_str(), ".obj") &&
|
|
strcasecmp(ext.c_str(), ".msh")) {
|
|
return comperr(error, "Mesh file extension must be stl, obj or msh", error_sz);
|
|
}
|
|
|
|
// check dim
|
|
if (def.spec.flex->dim != 2) {
|
|
return comperr(error, "Flex dim must be 2 in for mesh", error_sz);
|
|
}
|
|
|
|
// load resource
|
|
std::string filename = mjuu_combinePaths(mjs_getString(model->spec.meshdir), file);
|
|
mjResource* resource = nullptr;
|
|
|
|
try {
|
|
resource = mjCBase::LoadResource(mjs_getString(model->spec.modelfiledir),
|
|
filename, 0);
|
|
} catch (mjCError err) {
|
|
return comperr(error, err.message, error_sz);
|
|
}
|
|
|
|
// load mesh
|
|
mjCMesh mesh;
|
|
bool isobj = false;
|
|
try {
|
|
if (!strcasecmp(ext.c_str(), ".stl")) {
|
|
mesh.LoadSTL(resource);
|
|
} else if (!strcasecmp(ext.c_str(), ".obj")) {
|
|
isobj = true;
|
|
mesh.LoadOBJ(resource);
|
|
} else {
|
|
mesh.LoadMSH(resource);
|
|
}
|
|
mju_closeResource(resource);
|
|
} catch (mjCError err) {
|
|
mju_closeResource(resource);
|
|
return comperr(error, err.message, error_sz);
|
|
}
|
|
|
|
// LoadOBJ uses userXXX, extra processing needed
|
|
if (isobj) {
|
|
// check sizes
|
|
if (mesh.Vert().empty() || mesh.Face().empty()) {
|
|
return comperr(error, "Vertex and face data required", error_sz);
|
|
}
|
|
if (mesh.Vert().size()%3) {
|
|
return comperr(error, "Vertex data must be multiple of 3", error_sz);
|
|
}
|
|
if (mesh.Face().size()%3) {
|
|
return comperr(error, "Face data must be multiple of 3", error_sz);
|
|
}
|
|
|
|
// remove repeated vertices (not called in LoadOBJ)
|
|
mesh.RemoveRepeated();
|
|
}
|
|
|
|
// copy faces
|
|
element = mesh.Face();
|
|
|
|
// copy vertices, convert from float to double
|
|
point = vector<double> (mesh.nvert()*3);
|
|
for (int i=0; i < mesh.nvert()*3; i++) {
|
|
point[i] = (double) mesh.Vert(i);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
// find string in buffer, return position or -1 if not found
|
|
static int findstring(const char* buffer, int buffer_sz, const char* str) {
|
|
int len = (int)strlen(str);
|
|
|
|
// scan buffer
|
|
for (int i=0; i < buffer_sz-len; i++) {
|
|
// check for string at position i
|
|
bool found = true;
|
|
for (int k=0; k < len; k++) {
|
|
if (buffer[i+k] != str[k]) {
|
|
found = false;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// string found
|
|
if (found) {
|
|
return i;
|
|
}
|
|
}
|
|
|
|
// not found
|
|
return -1;
|
|
}
|
|
|
|
|
|
|
|
// load points and elements from GMSH file
|
|
bool mjCFlexcomp::MakeGMSH(mjCModel* model, char* error, int error_sz) {
|
|
// strip path
|
|
if (!file.empty() && model->spec.strippath) {
|
|
file = mjuu_strippath(file);
|
|
}
|
|
|
|
// file is required
|
|
if (file.empty()) {
|
|
return comperr(error, "File is required", error_sz);
|
|
}
|
|
|
|
// open resource
|
|
mjResource* resource = nullptr;
|
|
try {
|
|
std::string filename = mjuu_combinePaths(mjs_getString(model->spec.meshdir), file);
|
|
resource = mjCBase::LoadResource(mjs_getString(model->spec.modelfiledir),
|
|
filename, 0);
|
|
} catch (mjCError err) {
|
|
return comperr(error, err.message, error_sz);
|
|
}
|
|
|
|
// try to load, close resource properly
|
|
try {
|
|
LoadGMSH(model, resource);
|
|
mju_closeResource(resource);
|
|
} catch (mjCError err) {
|
|
mju_closeResource(resource);
|
|
return comperr(error, err.message, error_sz);
|
|
} catch (...) {
|
|
mju_closeResource(resource);
|
|
return comperr(error, "exception while reading GMSH file", error_sz);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
// load GMSH format 4.1
|
|
void mjCFlexcomp::LoadGMSH41(char* buffer, int binary, int nodeend,
|
|
int nodebegin, int elemend, int elembegin){
|
|
// header size
|
|
constexpr int kGmsh41HeaderSize = 52;
|
|
// base for node tags, to be subtracted from element data
|
|
size_t minNodeTag, numEntityBlocks, numNodes, maxNodeTag, numNodesInBlock, tag;
|
|
int entityDim, entityTag, parametric;
|
|
|
|
// ascii nodes
|
|
if (binary == 0) {
|
|
// convert node char buffer to stringstream
|
|
stringstream ss(std::string(buffer + nodebegin, nodeend - nodebegin));
|
|
|
|
// read header
|
|
ss >> numEntityBlocks >> numNodes >> minNodeTag >> maxNodeTag;
|
|
ss >> entityDim >> entityTag >> parametric >> numNodesInBlock;
|
|
if (!ss.good()) {
|
|
throw mjCError(NULL, "Error reading Nodes header");
|
|
}
|
|
|
|
// check number of nodes is a positive number
|
|
if (numNodes < 0) {
|
|
throw mjCError(NULL, "Invalid number of nodes");
|
|
}
|
|
|
|
// require single block
|
|
if (numEntityBlocks != 1 || numNodes != numNodesInBlock) {
|
|
throw mjCError(NULL, "All nodes must be in single block");
|
|
}
|
|
|
|
// check dimensionality and save
|
|
if (entityDim < 1 || entityDim > 3) {
|
|
throw mjCError(NULL, "Entity must be 1D, 2D or 3D");
|
|
}
|
|
def.spec.flex->dim = entityDim;
|
|
|
|
// read and discard node tags; require range from minNodeTag to maxNodeTag
|
|
for (size_t i=0; i < numNodes; i++) {
|
|
size_t tag;
|
|
ss >> tag;
|
|
if (!ss.good()) {
|
|
throw mjCError(NULL, "Error reading node tags");
|
|
}
|
|
if (tag != i+minNodeTag) {
|
|
throw mjCError(NULL, "Node tags must be sequential");
|
|
}
|
|
}
|
|
|
|
// read points
|
|
if (numNodes < 0 || numNodes >= INT_MAX / 3) {
|
|
throw mjCError(NULL, "Invalid number of nodes.");
|
|
}
|
|
point.reserve(3*numNodes);
|
|
for (size_t i=0; i < 3*numNodes; i++) {
|
|
double x;
|
|
ss >> x;
|
|
if (!ss.good()) {
|
|
throw mjCError(NULL, "Error reading node coordinates");
|
|
}
|
|
point.push_back(x);
|
|
}
|
|
}
|
|
// binary nodes
|
|
else {
|
|
// check header size
|
|
if (nodeend-nodebegin < kGmsh41HeaderSize) {
|
|
throw mjCError(NULL, "Invalid nodes header");
|
|
}
|
|
|
|
// read header
|
|
ReadFromBuffer(&numEntityBlocks, buffer + nodebegin);
|
|
ReadFromBuffer(&numNodes, buffer + nodebegin + 8);
|
|
ReadFromBuffer(&minNodeTag, buffer + nodebegin + 16);
|
|
ReadFromBuffer(&maxNodeTag, buffer + nodebegin + 24);
|
|
ReadFromBuffer(&entityDim, buffer + nodebegin + 32);
|
|
ReadFromBuffer(&entityTag, buffer + nodebegin + 36);
|
|
ReadFromBuffer(¶metric, buffer + nodebegin + 40);
|
|
ReadFromBuffer(&numNodesInBlock, buffer + nodebegin + 44);
|
|
|
|
// require single block
|
|
if (numEntityBlocks != 1 || numNodes != numNodesInBlock) {
|
|
throw mjCError(NULL, "All nodes must be in single block");
|
|
}
|
|
|
|
// check number of nodes is a positive number
|
|
if (numNodes < 0) {
|
|
throw mjCError(NULL, "Invalid number of nodes");
|
|
}
|
|
|
|
// check dimensionality and save
|
|
if (entityDim < 1 || entityDim > 3) {
|
|
throw mjCError(NULL, "Entity must be 1D, 2D or 3D");
|
|
}
|
|
def.spec.flex->dim = entityDim;
|
|
|
|
// nodeData: node tag and 3 nodes
|
|
constexpr int numNodeComponents = 4;
|
|
constexpr int componentSize = 8;
|
|
int nodeDataSize = numNodeComponents*componentSize;
|
|
|
|
// check section byte size
|
|
if (nodeend-nodebegin < kGmsh41HeaderSize + numNodes*nodeDataSize) {
|
|
throw mjCError(NULL, "Insufficient byte size of Nodes");
|
|
}
|
|
|
|
// check node tags: must range from minNodeTag to maxNodeTag
|
|
const char* tagbuffer = buffer + nodebegin + kGmsh41HeaderSize;
|
|
for (size_t i=0; i < numNodes; i++) {
|
|
ReadFromBuffer(&tag, tagbuffer + i*componentSize);
|
|
if (tag != i+minNodeTag) {
|
|
throw mjCError(NULL, "Node tags must be sequential");
|
|
}
|
|
}
|
|
|
|
// read points
|
|
if (numNodes < 0 || numNodes >= INT_MAX / 3) {
|
|
throw mjCError(NULL, "Invalid number of nodes.");
|
|
}
|
|
point.reserve(3*numNodes);
|
|
const char* pointbuffer = buffer + nodebegin + kGmsh41HeaderSize + componentSize*numNodes;
|
|
for (size_t i=0; i < 3*numNodes; i++) {
|
|
double x;
|
|
ReadFromBuffer(&x, pointbuffer + i*componentSize);
|
|
point.push_back(x);
|
|
}
|
|
}
|
|
|
|
size_t numElements, minElementTag, maxElementTag, numElementsInBlock;
|
|
int elementType;
|
|
|
|
// ascii elements
|
|
if (binary == 0) {
|
|
// convert element char buffer to stringstream
|
|
buffer[elemend] = 0;
|
|
stringstream ss(std::string(buffer + elembegin, elemend - elembegin));
|
|
|
|
// read header
|
|
ss >> numEntityBlocks >> numElements >> minElementTag >> maxElementTag;
|
|
ss >> entityDim >> entityTag >> elementType >> numElementsInBlock;
|
|
if (!ss.good()) {
|
|
throw mjCError(NULL, "Error reading Elements header");
|
|
}
|
|
|
|
// require single block
|
|
if (numEntityBlocks != 1 || numElements != numElementsInBlock) {
|
|
throw mjCError(NULL, "All elements must be in single block");
|
|
}
|
|
|
|
// check number of elements is a positive number
|
|
if (numElements < 0) {
|
|
throw mjCError(NULL, "Invalid number of elements");
|
|
}
|
|
|
|
// dimensionality must be same as nodes
|
|
if (entityDim != def.spec.flex->dim) {
|
|
throw mjCError(NULL, "Inconsistent dimensionality in Elements");
|
|
}
|
|
|
|
if (numElements < 0 || numElements >= INT_MAX / 4) {
|
|
throw mjCError(NULL, "Invalid numElements.");
|
|
}
|
|
|
|
// type must be consistent with dimensionality
|
|
if ((entityDim == 1 && elementType != 1) ||
|
|
(entityDim == 2 && elementType != 2) ||
|
|
(entityDim == 3 && elementType != 4)) {
|
|
throw mjCError(NULL, "Element type inconsistent with dimensionality");
|
|
}
|
|
|
|
// read elements, discard tags
|
|
element.reserve((entityDim+1)*numElements);
|
|
for (size_t i=0; i < numElements; i++) {
|
|
size_t tag, nodeid;
|
|
ss >> tag;
|
|
for (int k=0; k <= entityDim; k++) {
|
|
ss >> nodeid;
|
|
if (!ss.good()) {
|
|
throw mjCError(NULL, "Error reading Elements");
|
|
}
|
|
element.push_back((int)(nodeid-minNodeTag));
|
|
}
|
|
}
|
|
}
|
|
|
|
// binary elements
|
|
else {
|
|
// check header size
|
|
if (elemend-elembegin < kGmsh41HeaderSize) {
|
|
throw mjCError(NULL, "Invalid elements header");
|
|
}
|
|
|
|
// read header
|
|
ReadFromBuffer(&numEntityBlocks, buffer + elembegin);
|
|
ReadFromBuffer(&numElements, buffer + elembegin + 8);
|
|
ReadFromBuffer(&minElementTag, buffer + elembegin + 16);
|
|
ReadFromBuffer(&maxElementTag, buffer + elembegin + 24);
|
|
ReadFromBuffer(&entityDim, buffer + elembegin + 32);
|
|
ReadFromBuffer(&entityTag, buffer + elembegin + 36);
|
|
ReadFromBuffer(&elementType, buffer + elembegin + 40);
|
|
ReadFromBuffer(&numElementsInBlock, buffer + elembegin + 44);
|
|
|
|
// require single block
|
|
if (numEntityBlocks != 1 || numElements != numElementsInBlock) {
|
|
throw mjCError(NULL, "All elements must be in single block");
|
|
}
|
|
|
|
// check number of elements is a positive number
|
|
if (numElements < 0) {
|
|
throw mjCError(NULL, "Invalid number of elements");
|
|
}
|
|
|
|
// dimensionality must be same as nodes
|
|
if (entityDim != def.spec.flex->dim) {
|
|
throw mjCError(NULL, "Inconsistent dimensionality in Elements");
|
|
}
|
|
|
|
// type must be consistent with dimensionality
|
|
if ((entityDim == 1 && elementType != 1) ||
|
|
(entityDim == 2 && elementType != 2) ||
|
|
(entityDim == 3 && elementType != 4)) {
|
|
throw mjCError(NULL, "Element type inconsistent with dimensionality");
|
|
}
|
|
|
|
if (numElements < 0 || numElements >= INT_MAX / 4) {
|
|
throw mjCError(NULL, "Invalid numElements.");
|
|
}
|
|
|
|
// elementData: element tag and n node tags
|
|
int numElementComponents = (entityDim+2);
|
|
constexpr int componentSize = 8;
|
|
int elementDataSize = numElementComponents*componentSize;
|
|
|
|
// check section byte size
|
|
if (elemend - elembegin < kGmsh41HeaderSize + numElements*elementDataSize) {
|
|
throw mjCError(NULL, "Insufficient byte size of Elements");
|
|
}
|
|
|
|
// read elements, discard tags
|
|
element.reserve((entityDim+1)*numElements);
|
|
const char* elembuffer = buffer + elembegin + kGmsh41HeaderSize;
|
|
for (size_t i=0; i < numElements; i++) {
|
|
// skip element tag
|
|
elembuffer += componentSize;
|
|
|
|
// read vertex ids
|
|
size_t elemid;
|
|
for (int k=0; k <= entityDim; k++) {
|
|
ReadFromBuffer(&elemid, elembuffer);
|
|
int elementid = elemid - minNodeTag;
|
|
element.push_back(elementid);
|
|
elembuffer += componentSize;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// load GMSH format 2.2
|
|
void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend,
|
|
int nodebegin, int elemend, int elembegin) {
|
|
// number of nodes
|
|
size_t numNodes = 0;
|
|
|
|
// ascii nodes
|
|
if (binary == 0) {
|
|
// convert node char buffer to stringstream
|
|
stringstream ss(std::string(buffer + nodebegin, nodeend - nodebegin));
|
|
std::string line;
|
|
|
|
// checking header template
|
|
std::getline(ss, line);
|
|
if (!IsValidElementOrNodeHeader22(line)) {
|
|
throw mjCError(NULL, "Invalid node header");
|
|
}
|
|
ss.seekg(-(line.size()+1), std::ios::cur);
|
|
|
|
// read header
|
|
size_t maxNodeTag = 0;
|
|
ss >> maxNodeTag;
|
|
if (!ss.good()) {
|
|
throw mjCError(NULL, "Error reading Nodes header");
|
|
}
|
|
numNodes = maxNodeTag;
|
|
|
|
if (numNodes < 0 || numNodes >= INT_MAX / 3) {
|
|
throw mjCError(NULL, "Invalid number of nodes.");
|
|
}
|
|
|
|
// read points, discard tag
|
|
point.reserve(3*numNodes);
|
|
for (size_t i=0; i < numNodes; i++) {
|
|
size_t tag;
|
|
double x;
|
|
ss >> tag;
|
|
if (!ss.good()) {
|
|
throw mjCError(NULL, "Error reading node tags");
|
|
}
|
|
// reading nodes
|
|
for (int k=0; k < 3; k++) {
|
|
ss >> x;
|
|
if (!ss.good()) {
|
|
throw mjCError(NULL, "Error reading node coordinates");
|
|
}
|
|
point.push_back(x);
|
|
}
|
|
}
|
|
}
|
|
|
|
// binary nodes
|
|
else {
|
|
// header size for gmshApp
|
|
constexpr int nodeHeaderSizeGmshApp = 5;
|
|
// header size compatible with both gmshApp and Ftetwild
|
|
constexpr int nodeHeaderSize = nodeHeaderSizeGmshApp - 1;
|
|
// check header size
|
|
if (nodeend-nodebegin < nodeHeaderSize) {
|
|
throw mjCError(NULL, "Invalid nodes header");
|
|
}
|
|
|
|
// parse maxNodeTag and then cast it to int
|
|
char maxNodeTagChar[11] = {0};
|
|
ReadStrFromBuffer(maxNodeTagChar, buffer + nodebegin, std::min(10, nodeend - nodebegin));
|
|
size_t measuredHeaderSize = strnlen(maxNodeTagChar, 10) - 1;
|
|
size_t maxNodeTag;
|
|
try {
|
|
maxNodeTag = std::stoi(maxNodeTagChar);
|
|
} catch (const std::out_of_range& e) {
|
|
throw mjCError(NULL, "Invalid number of nodes");
|
|
}
|
|
numNodes = maxNodeTag;
|
|
|
|
// check number of nodes is a positive number
|
|
if (numNodes < 0) {
|
|
throw mjCError(NULL, "Invalid number of nodes");
|
|
}
|
|
|
|
// node data: node tag and 3 nodes
|
|
int nodeSize = sizeof(double);
|
|
int indexSize = sizeof(int);
|
|
int nodeDataSize = indexSize + 3*nodeSize;
|
|
|
|
// check section byte size
|
|
if (nodeend - nodebegin < nodeHeaderSize + numNodes*nodeDataSize) {
|
|
throw mjCError(NULL, "Insufficient byte size of Nodes");
|
|
}
|
|
|
|
// read point, discard tag
|
|
if (numNodes < 0 || numNodes >= INT_MAX / 3) {
|
|
throw mjCError(NULL, "Invalid number of nodes.");
|
|
}
|
|
point.reserve(3*numNodes);
|
|
// beginning of buffer containing node info
|
|
const char* tagBuffer = buffer + nodebegin + measuredHeaderSize;
|
|
for (int i=0; i < numNodes; i++) {
|
|
int tag;
|
|
int offset = i*(sizeof(int) + sizeof(double)*3);
|
|
ReadFromBuffer(&tag, tagBuffer + offset);
|
|
for (int k=0; k < 3; k++) {
|
|
double x;
|
|
const char* nodeBuffer = tagBuffer + sizeof(int) + sizeof(double)*k;
|
|
ReadFromBuffer(&x, nodeBuffer + offset);
|
|
point.push_back(x);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// ascii elements
|
|
if (binary == 0) {
|
|
// convert element char buffer to stringstream
|
|
buffer[elemend] = 0;
|
|
stringstream ss(std::string(buffer + elembegin, elemend - elembegin));
|
|
std::string line;
|
|
|
|
// checking header template
|
|
std::getline(ss, line);
|
|
if (!IsValidElementOrNodeHeader22(line)) {
|
|
throw mjCError(NULL, "Invalid elements header");
|
|
}
|
|
ss.seekg(-(line.size()+1), std::ios::cur);
|
|
// read header
|
|
size_t maxElementTag = 0;
|
|
ss >> maxElementTag;
|
|
if (!ss.good()) {
|
|
throw mjCError(NULL, "Error reading Elements header");
|
|
}
|
|
size_t numElements = maxElementTag;
|
|
|
|
if (numElements < 0 || numElements >= INT_MAX / 4) {
|
|
throw mjCError(NULL, "Invalid number of elements.");
|
|
}
|
|
|
|
|
|
// check number of elements is a positive number
|
|
if (numElements < 0) {
|
|
throw mjCError(NULL, "Invalid number of elements");
|
|
}
|
|
|
|
// reading first element's type
|
|
int tag = 0, elementType = 0, numTags = 0;
|
|
ss >> tag >> elementType >> numTags;
|
|
if (!ss.good()) {
|
|
throw mjCError(NULL, "Error reading Elements");
|
|
}
|
|
|
|
size_t entityDim = 0;
|
|
int numNodeTags = 0;
|
|
// surface
|
|
if (elementType == 2) {
|
|
entityDim = 2;
|
|
numNodeTags = 3;
|
|
}
|
|
// tetrahedral
|
|
else if (elementType == 4) {
|
|
entityDim = 3;
|
|
numNodeTags = 4;
|
|
}
|
|
|
|
if (numNodeTags < 1 || numNodeTags >4) {
|
|
throw mjCError(NULL, "Invalid number of node tags");
|
|
}
|
|
|
|
// setting entityDim
|
|
def.spec.flex->dim = entityDim;
|
|
|
|
// read elements, discard all tags
|
|
element.reserve(numNodeTags*numElements);
|
|
for (size_t i=0; i<numElements; i++) {
|
|
int nodeTag = 0, physicalEntityTag = 0, elmentModelEntityTag = 0;
|
|
if (i != 0) {
|
|
ss >> tag >> elementType >> numTags;
|
|
if (!ss.good()) {
|
|
throw mjCError(NULL, "Error reading Elements");
|
|
}
|
|
}
|
|
if (numTags > 0) {
|
|
ss >> physicalEntityTag >> elmentModelEntityTag;
|
|
if (!ss.good()) {
|
|
throw mjCError(NULL, "Error reading Elements");
|
|
}
|
|
}
|
|
for (int k=0; k < numNodeTags; k++) {
|
|
ss >> nodeTag;
|
|
if (!ss.good()) {
|
|
throw mjCError(NULL, "Error reading Elements");
|
|
}
|
|
if (nodeTag > numNodes || nodeTag < 1) {
|
|
throw mjCError(NULL, "Invalid node tag");
|
|
}
|
|
element.push_back((int)(nodeTag-1));
|
|
}
|
|
}
|
|
}
|
|
// binary elements
|
|
else {
|
|
// header size for gmshApp
|
|
constexpr int elementHeaderSizeGmshApp = 4;
|
|
// header size for Ftetwild
|
|
constexpr int elementHeaderSizeFtetwild = 17;
|
|
// check header size
|
|
if (elemend - elembegin < elementHeaderSizeGmshApp) {
|
|
throw mjCError(NULL, "Invalid elements header");
|
|
}
|
|
|
|
// reading elements
|
|
char maxElementTagChar[11] = {0};
|
|
ReadStrFromBuffer(maxElementTagChar, buffer + elembegin, std::min(10, elemend - elembegin));
|
|
int measuredHeaderSize = strnlen(maxElementTagChar, 10) - 1;
|
|
int maxElementTag;
|
|
try {
|
|
maxElementTag = std::stoi(maxElementTagChar);
|
|
} catch (const std::out_of_range& e) {
|
|
throw mjCError(NULL, "Invalid number of elements");
|
|
}
|
|
int numElements = maxElementTag;
|
|
int tag, numTags;
|
|
int nodeTag;
|
|
int elementType;
|
|
|
|
// check number of elements is a positive number
|
|
if (numElements < 0) {
|
|
throw mjCError(NULL, "Invalid number of elements");
|
|
}
|
|
|
|
// size of single component in element data
|
|
int componentSize = sizeof(int);
|
|
// element buffer
|
|
const char* elementsBuffer = buffer + elembegin + measuredHeaderSize;
|
|
ReadFromBuffer(&elementType, elementsBuffer);
|
|
ReadFromBuffer(&numTags, elementsBuffer + componentSize*2);
|
|
ReadFromBuffer(&tag, elementsBuffer + componentSize*3);
|
|
|
|
// tetrahedral has 4 node tags and surface has 3
|
|
int numNodeTags = 0;
|
|
size_t entityDim = 0;
|
|
// surface
|
|
if (elementType == 2) {
|
|
entityDim = 2;
|
|
numNodeTags = 3;
|
|
}
|
|
// tetrahedral
|
|
else if (elementType == 4) {
|
|
entityDim = 3;
|
|
numNodeTags = 4;
|
|
}
|
|
|
|
if (numNodeTags < 1 || numNodeTags >4) {
|
|
throw mjCError(NULL, "Invalid number of node tags");
|
|
}
|
|
|
|
def.spec.flex->dim = entityDim;
|
|
|
|
// element data(Ftetwild): tag and 4 nodeTag
|
|
constexpr int numComponentsFtetwild = 5;
|
|
// element data(gmshApp): 4 Info components, 2 entity tag and entityDim+1 nodeTags
|
|
constexpr int numInfoComponents = 4;
|
|
constexpr int numEntityTagComponents = 2;
|
|
|
|
int numComponentsGmshApp = numInfoComponents + numEntityTagComponents + numNodeTags;
|
|
|
|
// single element data size
|
|
int elementDataSizeFtetwild = numComponentsFtetwild*componentSize;
|
|
int elementDataSizeGmshApp = numComponentsGmshApp*componentSize;
|
|
|
|
// elements section buffer size
|
|
int elementsBufferSizeFtetwild = elementHeaderSizeFtetwild +
|
|
numElements*elementDataSizeFtetwild;
|
|
int elementsBufferSizeGmshApp = elementHeaderSizeGmshApp +
|
|
numElements*elementDataSizeGmshApp;
|
|
|
|
// check section byte size for ftetwild
|
|
if (elemend - elembegin < elementsBufferSizeFtetwild) {
|
|
throw mjCError(NULL, "Insufficient byte size of Elements");
|
|
}
|
|
|
|
// Handling elements produced by gmsh
|
|
if (numTags > 0) {
|
|
// check section byte size for gmsh
|
|
if (elemend - elembegin < elementsBufferSizeGmshApp) {
|
|
throw mjCError(NULL, "Insufficient byte size of Elements");
|
|
}
|
|
|
|
// read first element
|
|
for (int k =0; k<numNodeTags; k++) {
|
|
ReadFromBuffer(&nodeTag, elementsBuffer + componentSize*(6+k));
|
|
if (nodeTag > numNodes || nodeTag < 1) {
|
|
throw mjCError(NULL, "Invalid node tag");
|
|
}
|
|
element.push_back(nodeTag-1);
|
|
}
|
|
|
|
// read every other element
|
|
for (int i=1; i < numElements; i++) {
|
|
const char* numTagsBuffer = elementsBuffer + componentSize*2;
|
|
const char* tagBuffer = elementsBuffer + componentSize*3;
|
|
int offset = i*elementDataSizeGmshApp;
|
|
ReadFromBuffer(&numTags, numTagsBuffer + offset);
|
|
ReadFromBuffer(&tag, tagBuffer+offset);
|
|
for (int k =0; k < numNodeTags; k++) {
|
|
const char* nodeTagBuffer = elementsBuffer + componentSize*(6+k);
|
|
ReadFromBuffer(&nodeTag, nodeTagBuffer + offset);
|
|
if (nodeTag > numElements || nodeTag < 1) {
|
|
throw mjCError(NULL, "Invalid node tag");
|
|
}
|
|
element.push_back(nodeTag-1);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Handling elements produced by ftetwild
|
|
else {
|
|
// read first element
|
|
for (int k = 0; k < numNodeTags; k++) {
|
|
const char* nodeTagBuffer = elementsBuffer + componentSize*(4+k);
|
|
ReadFromBuffer(&nodeTag, nodeTagBuffer);
|
|
if (nodeTag > numNodes || nodeTag < 1) {
|
|
throw mjCError(NULL, "Invalid node tag");
|
|
}
|
|
element.push_back(nodeTag-1);
|
|
}
|
|
|
|
// read every other element
|
|
for (int i=0; i<numElements-1; i++) {
|
|
int offset = componentSize*(4+2) + i*elementDataSizeFtetwild;
|
|
const char* tagBuffer = elementsBuffer + componentSize*2;
|
|
ReadFromBuffer(&tag, tagBuffer + offset);
|
|
for (int k=0; k < numNodeTags; k++) {
|
|
const char* nodeTagBuffer = elementsBuffer + componentSize*(3+k);
|
|
ReadFromBuffer(&nodeTag, nodeTagBuffer + offset);
|
|
if (nodeTag > numElements || nodeTag < 1) {
|
|
throw mjCError(NULL, "Invalid node tag");
|
|
}
|
|
element.push_back(nodeTag-1);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// load GMSH file from resource
|
|
void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) {
|
|
// get buffer from resource
|
|
char* buffer = 0;
|
|
int buffer_sz = mju_readResource(resource, (const void**) &buffer);
|
|
|
|
// check buffer
|
|
if (buffer_sz < 0) {
|
|
throw mjCError(NULL, "Could not read GMSH file");
|
|
} else if (buffer_sz == 0) {
|
|
throw mjCError(NULL, "Empty GMSH file");
|
|
} else if (buffer_sz < 11 || strncmp(buffer, "$MeshFormat", 11)) {
|
|
throw mjCError(NULL, "GMSH file must begin with $MeshFormat");
|
|
}
|
|
|
|
// check version, determine ascii or binary
|
|
double version;
|
|
int binary;
|
|
if (sscanf(buffer + 11, "%lf %d", &version, &binary) != 2) {
|
|
throw mjCError(NULL, "Could not read GMSH file header");
|
|
}
|
|
if (mju_round(100*version) != 220 && mju_round(100*version) != 410) {
|
|
throw mjCError(NULL, "Only GMSH file format versions 4.1 and 2.2 are supported");
|
|
}
|
|
|
|
// find section begin/end
|
|
int nodebegin = findstring(buffer, buffer_sz, "$Nodes");
|
|
int nodeend = findstring(buffer, buffer_sz, "$EndNodes");
|
|
int elembegin = findstring(buffer, buffer_sz, "$Elements");
|
|
int elemend = findstring(buffer, buffer_sz, "$EndElements");
|
|
|
|
|
|
// correct begin for string size, +1 for LF in binary (CRLF in Win ascii works)
|
|
nodebegin += (int)strlen("$Nodes") + 1;
|
|
elembegin += (int)strlen("$Elements") + 1;
|
|
|
|
// check sections
|
|
if (nodebegin < 0) {
|
|
throw mjCError(NULL, "GMSH file missing $Nodes");
|
|
}
|
|
if (nodeend < nodebegin) {
|
|
throw mjCError(NULL, "GMSH file missing $EndNodes after $Nodes");
|
|
}
|
|
if (elembegin < 0) {
|
|
throw mjCError(NULL, "GMSH file missing $Elements");
|
|
}
|
|
if (elemend < elembegin) {
|
|
throw mjCError(NULL, "GMSH file missing $EndElements after $Elements");
|
|
}
|
|
|
|
// Support for 4.1
|
|
if (mju_round(100*version) == 410) {
|
|
LoadGMSH41(buffer, binary, nodeend, nodebegin, elemend, elembegin);
|
|
}
|
|
|
|
// Support for 2.2
|
|
else if (mju_round(100*version) == 220) {
|
|
LoadGMSH22(buffer, binary, nodeend, nodebegin, elemend, elembegin);
|
|
} else {
|
|
throw mjCError(NULL, "Unsupported GMSH file format version");
|
|
}
|
|
}
|