// 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 <mujoco/mjspec.h>
#include "user/user_composite.h"

#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstdio>
#include <cstring>
#include <map>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>

#include <mujoco/mjmacro.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
#include "cc/array_safety.h"
#include "engine/engine_io.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "user/user_model.h"
#include "user/user_objects.h"
#include "user/user_util.h"
#include "xml/xml_util.h"

namespace {
namespace mju = ::mujoco::util;
using std::vector;
using std::string;
}  // 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;
}



// constructor
mjCComposite::mjCComposite(void) {
  // common properties
  prefix.clear();
  type = mjCOMPTYPE_PARTICLE;
  count[0] = count[1] = count[2] = 1;
  spacing = 0;
  mjuu_setvec(offset, 0, 0, 0);
  pin.clear();
  flatinertia = 0;
  mj_defaultSolRefImp(solrefsmooth, solimpsmooth);

  // plugin variables
  mjs_defaultPlugin(&plugin);
  plugin_name = "";
  plugin_instance_name = "";
  plugin.name = (mjString*)&plugin_name;
  plugin.instance_name = (mjString*)&plugin_instance_name;

  // cable
  curve[0] = curve[1] = curve[2] = mjCOMPSHAPE_ZERO;
  mjuu_setvec(size, 1, 0, 0);
  initial = "ball";

  // skin
  skin = false;
  skintexcoord = false;
  skinmaterial.clear();
  mjuu_setvec(skinrgba, 1, 1, 1, 1);
  skininflate = 0;
  skinsubgrid = 0;
  skingroup = 0;

  // clear add flags
  for (int i=0; i<mjNCOMPKINDS; i++) {
    add[i] = false;
  }

  // clear internal
  dim = 0;
}



// adjust constraint softness
void mjCComposite::AdjustSoft(mjtNum* solref, mjtNum* solimp, int level) {
  switch (level) {
  case 0:
    solref[0] = 0.01;
    solimp[0] = solimp[1] = 0.99;
    break;

  case 1:
    solref[0] = 0.02;
    solimp[0] = solimp[1] = 0.9;
    break;
  }
}



// create the array of default joint options, append new elements only for particles type
bool mjCComposite::AddDefaultJoint(char* error, int error_sz) {
  for (int i=0; i<mjNCOMPKINDS; i++) {
    if (!defjoint[(mjtCompKind)i].empty() && type!=mjCOMPTYPE_PARTICLE) {
      comperr(error, "Only particles are allowed to have multiple joints", error_sz);
      return false;
    } else {
      mjCDef jnt;
      jnt.spec.joint->group = 3;
      defjoint[(mjtCompKind)i].push_back(jnt);
    }
  }
  return true;
}



// set defaults, after reading top-level info and skin
void mjCComposite::SetDefault(void) {
  // determine dimensionality
  int tmpdim = 0;
  for (int i=0; i<3; i++) {
    if (count[i]>1) {
      tmpdim++;
    }
  }

  // set all deafult groups to 3
  for (int i=0; i<mjNCOMPKINDS; i++) {
    def[i].spec.geom->group = 3;
    def[i].spec.site->group = 3;
    def[i].spec.tendon->group = 3;
  }

  // set default joint
  AddDefaultJoint();

  // set default geom and tendon group to 0 if needed to be visible
  if (!skin ||
      type==mjCOMPTYPE_PARTICLE   ||
      type==mjCOMPTYPE_ROPE       ||
      type==mjCOMPTYPE_LOOP       ||
      type==mjCOMPTYPE_CABLE      ||
      (type==mjCOMPTYPE_GRID && tmpdim==1)) {
    for (int i=0; i<mjNCOMPKINDS; i++) {
      def[i].spec.geom->group = 0;
      def[i].spec.tendon->group = 0;
    }
  }

  // other type-specific adjustments
  switch (type) {
  case mjCOMPTYPE_PARTICLE:       // particle

    // no friction with anything
    def[0].spec.geom->condim = 1;
    def[0].spec.geom->priority = 1;
    break;

  case mjCOMPTYPE_GRID:           // grid

    // hard main tendon fix
    AdjustSoft(def[mjCOMPKIND_TENDON].spec.equality->solref,
               def[mjCOMPKIND_TENDON].spec.equality->solimp, 0);

    break;

  case mjCOMPTYPE_CABLE:          // cable
  case mjCOMPTYPE_ROPE:           // rope
    break;

  case mjCOMPTYPE_LOOP:           // loop

    // hard smoothing
    AdjustSoft(solrefsmooth, solimpsmooth, 0);
    break;

  case mjCOMPTYPE_CLOTH:          // cloth
    break;

  case mjCOMPTYPE_BOX:            // 3D
  case mjCOMPTYPE_CYLINDER:
  case mjCOMPTYPE_ELLIPSOID:

    // no self-collisions
    def[0].spec.geom->contype = 0;

    // soft smoothing
    AdjustSoft(solrefsmooth, solimpsmooth, 1);

    // soft fix everywhere
    for (int i=0; i<mjNCOMPKINDS; i++) {
      AdjustSoft(def[i].spec.equality->solref, def[i].spec.equality->solimp, 1);
    }

    // hard main tendon fix
    AdjustSoft(def[mjCOMPKIND_TENDON].spec.equality->solref,
               def[mjCOMPKIND_TENDON].spec.equality->solimp, 0);
    break;
  default:
    // SHOULD NOT OCCUR
    mju_error("Invalid composite type: %d", type);
    break;
  }
}



// make composite object
bool mjCComposite::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) {
  mjCModel* model = (mjCModel*)spec->element;

  // check geom type
  if ((def[0].spec.geom->type!=mjGEOM_SPHERE &&
       def[0].spec.geom->type!=mjGEOM_CAPSULE &&
       def[0].spec.geom->type!=mjGEOM_ELLIPSOID) &&
      type!=mjCOMPTYPE_PARTICLE && type!=mjCOMPTYPE_CABLE) {
    return comperr(error, "Composite geom type must be sphere, capsule or ellipsoid", error_sz);
  }

  // check pin coord number
  if (pin.size()%2) {
    return comperr(error, "Pin coordinate number of must be multiple of 2", error_sz);
  }

  // check counts
  for (int i=0; i<3; i++) {
    if (count[i]<1) {
      return comperr(error, "Positive counts expected in composite", error_sz);
    }
  }

  // check spacing
  if (type==mjCOMPTYPE_GRID || (type==mjCOMPTYPE_PARTICLE && uservert.empty())) {
    if (spacing < mju_max(def[0].spec.geom->size[0],
                  mju_max(def[0].spec.geom->size[1], def[0].spec.geom->size[2]))) {
      return comperr(error, "Spacing must be larger than geometry size",
                     error_sz);
    }
  }

  // check cable sizes are nonzero if vertices are not prescribed
  if (mjuu_dot3(size, size)<mjMINVAL && uservert.empty()) {
    return comperr(error, "Positive spacing or length expected in composite", error_sz);
  }

  // check spacing is not used by cable
  if (spacing && type==mjCOMPTYPE_CABLE) {
    return comperr(error, "Spacing is not supported by cable composite", error_sz);
  }

  // check either uservert or count but not both
  if (!uservert.empty()) {
    if (count[0]>1) {
      return comperr(error, "Either vertex or count can be specified, not both", error_sz);
    }
    count[0] = uservert.size()/3;
    count[1] = 1;
  }

  // determine dimensionality, check singleton order
  bool first = false;
  for (int i=0; i<3; i++) {
    if (count[i]==1) {
      first = true;
    } else {
      dim++;
      if (first) {
        return comperr(error, "Singleton counts must come last", error_sz);
      }
    }
  }

  // clear skin vectors
  face.clear();
  vert.clear();
  bindpos.clear();
  bindquat.clear();
  texcoord.clear();
  vertid.clear();
  vertweight.clear();

  // require 3x3 for subgrid
  if (skin && skinsubgrid>0 && type!=mjCOMPTYPE_CABLE) {
    if (count[0]<3 || count[1]<3) {
      return comperr(error, "At least 3x3 required for skin subgrid", error_sz);
    }
  }

  // overwrite plugin name
  if (plugin_instance_name.empty() && plugin.active) {
    plugin_instance_name = "composite" + prefix;
    (static_cast<mjCPlugin*>(plugin.instance))->name = plugin_instance_name;
  }

  // dispatch
  switch (type) {
  case mjCOMPTYPE_PARTICLE:
    return MakeParticle(model, body, error, error_sz);

  case mjCOMPTYPE_GRID:
    return MakeGrid(model, body, error, error_sz);

  case mjCOMPTYPE_ROPE:
    return comperr(error,
                "The \"rope\" composite type is deprecated. Please use "
                "\"cable\" instead.",
                error_sz);

  case mjCOMPTYPE_LOOP:
    mju_warning(
        "The \"loop\" composite type is deprecated. Please use \"cable\" "
        "instead.");
    return MakeRope(model, body, error, error_sz);

  case mjCOMPTYPE_CABLE:
    return MakeCable(model, body, error, error_sz);

  case mjCOMPTYPE_CLOTH:
    return comperr(error,
                   "The \"cloth\" composite type is deprecated. Please use "
                   "\"shell\" instead.",
                   error_sz);

  case mjCOMPTYPE_BOX:
  case mjCOMPTYPE_CYLINDER:
  case mjCOMPTYPE_ELLIPSOID:
    return MakeBox(model, body, error, error_sz);

  default:
    return comperr(error, "Unknown shape in composite", error_sz);
  }
}



bool mjCComposite::MakeParticle(mjCModel* model, mjsBody* body, char* error, int error_sz) {
  char txt[100];
  std::vector<int> face;

  // populate vertices and names
  if (uservert.empty()) {
    if (spacing < mju_max(def[0].spec.geom->size[0],
                  mju_max(def[0].spec.geom->size[1], def[0].spec.geom->size[2])))
      return comperr(error, "Spacing must be larger than geometry size", error_sz);

    for (int ix=0; ix<count[0]; ix++) {
      for (int iy=0; iy<count[1]; iy++) {
        for (int iz=0; iz<count[2]; iz++) {
          uservert.push_back(spacing*(ix - 0.5*count[0]));
          uservert.push_back(spacing*(iy - 0.5*count[1]));
          uservert.push_back(spacing*(iz - 0.5*count[2]));

          mju::sprintf_arr(txt, "%sB%d_%d_%d", prefix.c_str(), ix, iy, iz);
          username.push_back(std::string(txt));
        }
      }
    }
  }

  // create faces
  if (userface.empty()) {
    if (dim == 3) {
      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]-1; ix++) {
        for (int iy = 0; iy < count[1]-1; iy++) {
          for (int iz = 0; iz < count[2]-1; iz++) {
            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++) {
                face.push_back(vert[cube2tets[s][v]]);
              }
            }
          }
        }
      }
    } else if (dim == 2) {
      int quad2tri[2][3] = {{0, 1, 2}, {0, 2, 3}};
      for (int ix = 0; ix < count[0]-1; ix++) {
        for (int iy = 0; iy < count[1]-1; iy++) {
          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++) {
              face.push_back(vert[quad2tri[s][v]]);
            }
          }
        }
      }
    }
    mjXUtil::Vector2String(userface, face);
  } else {
    dim = 2;  // can only load a surface for now
    face = mjXUtil::String2Vector<int>(userface);
    for (int i=0; i<face.size(); face[i++]--) {};
    mjXUtil::Vector2String(userface, face);
  }

  // compute volume
  std::vector<double> volume(uservert.size()/3);
  double thickness = 1;
  if (dim == 2 && plugin.active) {
    try {
      mjCPlugin* pplugin = static_cast<mjCPlugin*>(plugin.instance);
      thickness = std::stod(pplugin->config_attribs["thickness"], nullptr);
    } catch (const std::invalid_argument& e) {
      return comperr(error, "Invalid thickness attribute", error_sz);
    }
  }
  if (!userface.empty()) {
    face = mjXUtil::String2Vector<int>(userface);
    for (int j=0; j<face.size()/3; j++) {
      double area[3];
      double edge1[3];
      double edge2[3];

      for (int i=0; i<3; i++) {
        edge1[i] = uservert[3*face[3*j+1]+i] - uservert[3*face[3*j]+i];
        edge2[i] = uservert[3*face[3*j+2]+i] - uservert[3*face[3*j]+i];
      }

      mjuu_crossvec(area, edge1, edge2);
      for (int i=0; i<3; i++) {
        volume[face[3*j+i]] += sqrt(mjuu_dot3(area, area)) / 2 * thickness;
      }
    }
  } else {
    for (int i=0; i<uservert.size()/3; i++) {
      volume[i] = 6 * spacing * spacing / 2 * thickness;
    }
  }

  // create bodies and geoms
  for (int i=0; i<uservert.size()/3; i++) {
    // create body
    mjsBody* b = mjs_addBody(body, NULL);

    if (!username.empty()) {
      mjs_setString(b->name, username[i].c_str());
    } else {
      mju::sprintf_arr(txt, "%sB%d", prefix.c_str(), i);
      mjs_setString(b->name, txt);
    }

    // set body position
    b->pos[0] = offset[0] + uservert[3*i];
    b->pos[1] = offset[1] + uservert[3*i+1];
    b->pos[2] = offset[2] + uservert[3*i+2];

    // add slider joints if none defined
    if (!add[mjCOMPKIND_PARTICLE]) {
      for (int i=0; i<3; i++) {
        mjsJoint* jnt = mjs_addJoint(b, &defjoint[mjCOMPKIND_JOINT][0].spec);
        mjs_setDefault(jnt->element, mjs_getDefault(body->element));
        jnt->type = mjJNT_SLIDE;
        mjuu_setvec(jnt->pos, 0, 0, 0);
        mjuu_setvec(jnt->axis, 0, 0, 0);
        jnt->axis[i] = 1;
      }
    }

    // add user-specified joints
    else {
      for (auto defjnt : defjoint[mjCOMPKIND_PARTICLE]) {
        mjsJoint* jnt = mjs_addJoint(b, &defjnt.spec);
        mjs_setDefault(jnt->element, mjs_getDefault(body->element));
      }
    }

    // add geom
    mjsGeom* g = mjs_addGeom(b, &def[0].spec);
    mjs_setDefault(g->element, mjs_getDefault(body->element));

    // add site
    mjsSite* s = mjs_addSite(b, &def[0].spec);
    mjs_setDefault(s->element, mjs_getDefault(body->element));
    s->type = mjGEOM_SPHERE;
    mju::sprintf_arr(txt, "%sS%d", prefix.c_str(), i);
    mjs_setString(s->name, txt);

    // add plugin
    if (plugin.active) {
      mjsPlugin* pplugin = &b->plugin;
      mjCPlugin* cplugin = static_cast<mjCPlugin*>(plugin.instance);
      pplugin->active = true;
      pplugin->instance = plugin.instance;
      mjs_setString(pplugin->instance_name, plugin_instance_name.c_str());
      mjs_setString(pplugin->name, mjs_getString(plugin.name));

      if (i==0 && !cplugin->config_attribs["face"].empty()) {
        return comperr(error, "Face attribute already exists in plugin", error_sz);
      }

      cplugin->config_attribs["face"] = userface;
      cplugin->config_attribs["edge"] = "";

      // update density
      if (dim == 2) {
        g->density *= volume[i] / (4./3. * mjPI * pow(g->size[0], 3));
      }
    }
  }

  // add isometry constraints
  if (dim==2) {
    char txt0[100], txt1[100], txt2[100];
    std::vector<std::pair<int, int>> edge;

    // create edges
    for (int i=0; i<face.size()/3; i++) {
      for (int j=0; j<3; j++) {
        int v0 = face[3*i+(j+0)%3];
        int v1 = face[3*i+(j+1)%3];
        edge.push_back(v0 < v1 ? std::pair(v0, v1) : std::pair(v1, v0));
      }
    }

    std::sort(edge.begin(), edge.end());
    auto last = std::unique(edge.begin(), edge.end());
    edge.erase(last, edge.end());

    // create constraints
    for (int i=0; i<edge.size(); i++) {
      int v0 = edge[i].first;
      int v1 = edge[i].second;

      mju::sprintf_arr(txt0, "%sT%d_%d", prefix.c_str(), v0, v1);
      mju::sprintf_arr(txt1, "%sS%d", prefix.c_str(), v0);
      mju::sprintf_arr(txt2, "%sS%d", prefix.c_str(), v1);

      // create tendon
      mjsTendon* ten = mjs_addTendon(&model->spec, &def[mjCOMPKIND_TENDON].spec);
      mjs_setDefault(ten->element, &model->Defaults(0)->spec);
      mjs_setString(ten->name, txt0);
      ten->group = 4;
      mjs_wrapSite(ten, txt1);
      mjs_wrapSite(ten, txt2);

      // add equality constraint
      mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_TENDON].spec);
      mjs_setDefault(eq->element, &model->Defaults(0)->spec);
      eq->type = mjEQ_TENDON;
      mjs_setString(eq->name1, mjs_getString(ten->name));
    }
  }

  if (skin && dim==3) {
    MakeSkin3(model);
  }

  if (skin && dim==2) {
    if (skinsubgrid>0) {
      MakeSkin2Subgrid(model, skininflate);
    } else {
      MakeSkin2(model, skininflate);
    }
  }

  return true;
}



// make grid connected with tendons
bool mjCComposite::MakeGrid(mjCModel* model, mjsBody* body, char* error, int error_sz) {
  char txt[100], txt1[100], txt2[100];

  // check dimensionality
  if (dim>2) {
    return comperr(error, "Grid can only be 1D or 2D", error_sz);
  }

  // check shear dimensionality
  if (add[mjCOMPKIND_SHEAR] && dim!=2) {
    return comperr(error, "Shear requires 2D grid", error_sz);
  }

  // check skin dimensionality
  if (skin && dim!=2) {
    return comperr(error, "Skin requires 2D grid", error_sz);
  }

  // create bodies, joints, geoms, sites
  for (int ix=0; ix<count[0]; ix++) {
    for (int iy=0; iy<count[1]; iy++) {
      // create body
      mjsBody* b = mjs_addBody(body, NULL);
      mju::sprintf_arr(txt, "%sB%d_%d", prefix.c_str(), ix, iy);
      mjs_setString(b->name, txt);

      // set body position
      b->pos[0] = offset[0] + spacing*(ix - 0.5*count[0]);
      b->pos[1] = offset[1] + spacing*(iy - 0.5*count[1]);
      b->pos[2] = offset[2];

      // add geom
      mjsGeom* g = mjs_addGeom(b, &def[0].spec);
      mjs_setDefault(g->element, mjs_getDefault(body->element));
      g->type = mjGEOM_SPHERE;
      mju::sprintf_arr(txt, "%sG%d_%d", prefix.c_str(), ix, iy);
      mjs_setString(g->name, txt);

      // add site
      mjsSite* s = mjs_addSite(b, &def[0].spec);
      mjs_setDefault(s->element, mjs_getDefault(body->element));
      s->type = mjGEOM_SPHERE;
      mju::sprintf_arr(txt, "%sS%d_%d", prefix.c_str(), ix, iy);
      mjs_setString(s->name, txt);

      // skip pinned elements
      bool skip = false;
      for (int ip=0; ip<pin.size(); ip+=2) {
        if (pin[ip]==ix && pin[ip+1]==iy) {
          skip = true;
          break;
        }
      }
      if (skip) {
        continue;
      }

      // add slider joint
      mjsJoint* jnt[3];
      for (int i=0; i<3; i++) {
        jnt[i] = mjs_addJoint(b, &defjoint[mjCOMPKIND_JOINT][0].spec);
        mjs_setDefault(jnt[i]->element, mjs_getDefault(body->element));
        mju::sprintf_arr(txt, "%sJ%d_%d_%d", prefix.c_str(), i, ix, iy);
        mjs_setString(jnt[i]->name, txt);
        jnt[i]->type = mjJNT_SLIDE;
        mjuu_setvec(jnt[i]->pos, 0, 0, 0);
        mjuu_setvec(jnt[i]->axis, 0, 0, 0);
        jnt[i]->axis[i] = 1;
      }
    }
  }

  // create tendons and equality constraints
  for (int i=0; i<2; i++) {
    for (int ix=0; ix<count[0]-(i==0); ix++) {
      for (int iy=0; iy<count[1]-(i==1); iy++) {
        // recover site names
        mju::sprintf_arr(txt1, "%sS%d_%d", prefix.c_str(), ix, iy);
        mju::sprintf_arr(txt2, "%sS%d_%d", prefix.c_str(), ix+(i==0), iy+(i==1));

        // create tendon
        mjCTendon* ten = model->AddTendon(def + mjCOMPKIND_TENDON);
        ten->def = model->Defaults(0);
        mju::sprintf_arr(txt, "%sT%d_%d_%d", prefix.c_str(), i, ix, iy);
        ten->name = txt;
        ten->WrapSite(txt1);
        ten->WrapSite(txt2);

        // add equality constraint
        mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_TENDON].spec);
        mjs_setDefault(eq->element, &model->Defaults(0)->spec);
        eq->type = mjEQ_TENDON;
        mjs_setString(eq->name1, ten->name.c_str());
      }
    }
  }

  // shear for 2D
  if (add[mjCOMPKIND_SHEAR]) {
    MakeShear(model);
  }

  // skin
  if (skin) {
    if (skinsubgrid>0) {
      MakeSkin2Subgrid(model, skininflate);
    } else {
      MakeSkin2(model, skininflate);
    }
  }

  return true;
}



bool mjCComposite::MakeCable(mjCModel* model, mjsBody* body, char* error, int error_sz) {
  // check dim
  if (dim!=1) {
    return comperr(error, "Cable must be one-dimensional", error_sz);
  }

  // check geom type
  if (def[0].spec.geom->type!=mjGEOM_CYLINDER &&
      def[0].spec.geom->type!=mjGEOM_CAPSULE &&
      def[0].spec.geom->type!=mjGEOM_BOX) {
    return comperr(error, "Cable geom type must be sphere, capsule or box", error_sz);
  }

  // add name to model
  mjsText* pte = mjs_addText(&model->spec);
  mjs_setString(pte->name, ("composite_" + prefix).c_str());
  mjs_setString(pte->data, ("rope_" + prefix).c_str());

  // populate uservert if not specified
  if (uservert.empty()) {
    for (int ix=0; ix<count[0]; ix++) {
      for (int k=0; k<3; k++) {
        switch (curve[k]) {
        case mjCOMPSHAPE_LINE:
          uservert.push_back(ix*size[0]/(count[0]-1));
          break;
        case mjCOMPSHAPE_COS:
          uservert.push_back(size[1]*cos(mjPI*ix*size[2]/(count[0]-1)));
          break;
        case mjCOMPSHAPE_SIN:
          uservert.push_back(size[1]*sin(mjPI*ix*size[2]/(count[0]-1)));
          break;
        case mjCOMPSHAPE_ZERO:
          uservert.push_back(0);
          break;
        default:
          // SHOULD NOT OCCUR
          mju_error("Invalid composite shape: %d", curve[k]);
          break;
        }
      }
    }
  }

  // create frame
  double normal[3], prev_quat[4];
  mjuu_setvec(normal, 0, 1, 0);
  mjuu_setvec(prev_quat, 1, 0, 0, 0);

  // add one body after the other
  for (int ix=0; ix<count[0]-1; ix++) {
    body = AddCableBody(model, body, ix, normal, prev_quat);
  }

  // add skin
  if (def[0].spec.geom->type==mjGEOM_BOX) {
    if (skinsubgrid>0) {
      count[1]+=2;
      MakeSkin2Subgrid(model, 2*def[0].spec.geom->size[2]);
      count[1]-=2;
    } else {
      count[1]++;
      MakeSkin2(model, 2*def[0].spec.geom->size[2]);
      count[1]--;
    }
  }
  return true;
}



mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix,
                                    double normal[3], double prev_quat[4]) {
  char txt_geom[100], txt_site[100], txt_slide[100];
  char this_body[100], next_body[100], this_joint[100];
  double dquat[4], this_quat[4];

  // set flags
  int lastidx = count[0]-2;
  bool first = ix==0;
  bool last = ix==lastidx;
  bool secondlast = ix==lastidx-1;

  // compute edge and tangent vectors
  double edge[3], tprev[3], tnext[3], length_prev = 0;
  mjuu_setvec(edge, uservert[3*(ix+1)+0]-uservert[3*ix+0],
                    uservert[3*(ix+1)+1]-uservert[3*ix+1],
                    uservert[3*(ix+1)+2]-uservert[3*ix+2]);
  if (!first) {
    mjuu_setvec(tprev, uservert[3*ix+0]-uservert[3*(ix-1)+0],
                       uservert[3*ix+1]-uservert[3*(ix-1)+1],
                       uservert[3*ix+2]-uservert[3*(ix-1)+2]);
    length_prev = mjuu_normvec(tprev, 3);
  }
  if (!last) {
    mjuu_setvec(tnext, uservert[3*(ix+2)+0]-uservert[3*(ix+1)+0],
                       uservert[3*(ix+2)+1]-uservert[3*(ix+1)+1],
                       uservert[3*(ix+2)+2]-uservert[3*(ix+1)+2]);
    mjuu_normvec(tnext, 3);
  }

  // update moving frame
  double length = mjuu_updateFrame(this_quat, normal, edge, tprev, tnext, first);

  // create body, joint, and geom names
  if (first) {
    mju::sprintf_arr(this_body, "%sB_first", prefix.c_str());
    mju::sprintf_arr(next_body, "%sB_%d", prefix.c_str(), ix+1);
    mju::sprintf_arr(this_joint, "%sJ_first", prefix.c_str());
    mju::sprintf_arr(txt_site, "%sS_first", prefix.c_str());
  } else if (last) {
    mju::sprintf_arr(this_body, "%sB_last", prefix.c_str());
    mju::sprintf_arr(next_body, "%sB_first", prefix.c_str());
    mju::sprintf_arr(this_joint, "%sJ_last", prefix.c_str());
    mju::sprintf_arr(txt_site, "%sS_last", prefix.c_str());
  } else if (secondlast){
    mju::sprintf_arr(this_body, "%sB_%d", prefix.c_str(), ix);
    mju::sprintf_arr(next_body, "%sB_last", prefix.c_str());
    mju::sprintf_arr(this_joint, "%sJ_%d", prefix.c_str(), ix);
  } else {
    mju::sprintf_arr(this_body, "%sB_%d", prefix.c_str(), ix);
    mju::sprintf_arr(next_body, "%sB_%d", prefix.c_str(), ix+1);
    mju::sprintf_arr(this_joint, "%sJ_%d", prefix.c_str(), ix);
  }
  mju::sprintf_arr(txt_geom, "%sG%d", prefix.c_str(), ix);
  mju::sprintf_arr(txt_slide, "%sJs%d", prefix.c_str(), ix);

  // add body
  body = mjs_addBody(body, 0);
  mjs_setString(body->name, this_body);
  if (first) {
    mjuu_setvec(body->pos, offset[0]+uservert[3*ix],
                           offset[1]+uservert[3*ix+1],
                           offset[2]+uservert[3*ix+2]);
    mjuu_copyvec(body->quat, this_quat, 4);
  } else {
    mjuu_setvec(body->pos, length_prev, 0, 0);
    double negquat[4] = {prev_quat[0], -prev_quat[1], -prev_quat[2], -prev_quat[3]};
    mjuu_mulquat(dquat, negquat, this_quat);
    mjuu_copyvec(body->quat, dquat, 4);
  }

  // add geom
  mjsGeom* geom = mjs_addGeom(body, &def[0].spec);
  mjs_setDefault(geom->element, mjs_getDefault(body->element));
  mjs_setString(geom->name, txt_geom);
  if (def[0].spec.geom->type==mjGEOM_CYLINDER ||
      def[0].spec.geom->type==mjGEOM_CAPSULE) {
    mjuu_zerovec(geom->fromto, 6);
    geom->fromto[3] = length;
  } else if (def[0].spec.geom->type==mjGEOM_BOX) {
    mjuu_zerovec(geom->pos, 3);
    geom->pos[0] = length/2;
    geom->size[0] = length/2;
  }

  // add plugin
  if (plugin.active) {
    mjsPlugin* pplugin = &body->plugin;
    pplugin->active = true;
    pplugin->instance = plugin.instance;
    mjs_setString(pplugin->name, mjs_getString(plugin.name));
    mjs_setString(pplugin->instance_name, plugin_instance_name.c_str());
  }

  // update orientation
  mjuu_copyvec(prev_quat, this_quat, 4);

  // add curvature joint
  if (!first || strcmp(initial.c_str(), "none")) {
    mjsJoint* jnt = mjs_addJoint(body, &defjoint[mjCOMPKIND_JOINT][0].spec);
    mjs_setDefault(jnt->element, mjs_getDefault(body->element));
    jnt->type = (first && strcmp(initial.c_str(), "free")==0) ? mjJNT_FREE : mjJNT_BALL;
    jnt->damping = jnt->type==mjJNT_FREE ? 0 : jnt->damping;
    jnt->armature = jnt->type==mjJNT_FREE ? 0 : jnt->armature;
    jnt->frictionloss = jnt->type==mjJNT_FREE ? 0 : jnt->frictionloss;
    mjs_setString(jnt->name, this_joint);
  }

  // exclude contact pair
  if (!last) {
    mjsExclude* exclude = mjs_addExclude(&model->spec);
    mjs_setString(exclude->bodyname1, std::string(this_body).c_str());
    mjs_setString(exclude->bodyname2, std::string(next_body).c_str());
  }

  // add site at the boundary
  if (last || first) {
    mjsSite* site = mjs_addSite(body, &def[0].spec);
    mjs_setDefault(site->element, mjs_getDefault(body->element));
    mjs_setString(site->name, txt_site);
    mjuu_setvec(site->pos, last ? length : 0, 0, 0);
    mjuu_setvec(site->quat, 1, 0, 0, 0);
  }

  return body;
}


// make rope
bool mjCComposite::MakeRope(mjCModel* model, mjsBody* body, char* error, int error_sz) {
  // check dim
  if (dim!=1) {
    return comperr(error, "Rope must be one-dimensional", error_sz);
  }

  // check root body name prefix
  char txt[200];
  mju::sprintf_arr(txt, "%sB", prefix.c_str());
  std::string body_name = mjs_getString(body->name);
  if (std::strncmp(txt, body_name.substr(0, strlen(txt)).c_str(), mju::sizeof_arr(txt))) {
    mju::strcat_arr(txt, " must be the beginning of root body name");
    return comperr(error, txt, error_sz);
  }

  // read origin coordinate from root body
  mju::strcpy_arr(txt, body_name.substr(strlen(txt)).c_str());
  int ox = -1;
  if (sscanf(txt, "%d", &ox)!=1) {
    return comperr(error, "Root body name must contain X coordinate", error_sz);
  }
  if (ox<0 || ox>=count[0]) {
    return comperr(error, "Root body coordinate out of range", error_sz);
  }

  // add origin
  AddRopeBody(model, body, ox, ox);

  // add elements: right
  mjsBody* pbody = body;
  for (int ix=ox; ix<count[0]-1; ix++) {
    pbody = AddRopeBody(model, pbody, ix, ix+1);
  }

  // add elements: left
  pbody = body;
  for (int ix=ox; ix>0; ix--) {
    pbody = AddRopeBody(model, pbody, ix, ix-1);
  }

  // close loop
  if (type==mjCOMPTYPE_LOOP) {
    char txt2[200];

    // add equality constraint
    mjsEquality* eq = mjs_addEquality(&model->spec, 0);
    eq->type = mjEQ_CONNECT;
    mju::sprintf_arr(txt, "%sB0", prefix.c_str());
    mju::sprintf_arr(txt2, "%sB%d", prefix.c_str(), count[0]-1);
    mjs_setString(eq->name1, txt);
    mjs_setString(eq->name2, txt2);
    mjuu_setvec(eq->data, -0.5*spacing, 0, 0);
    mju_copy(eq->solref, solrefsmooth, mjNREF);
    mju_copy(eq->solimp, solimpsmooth, mjNIMP);

    // remove contact between connected bodies
    mjsExclude* pair = mjs_addExclude(&model->spec);
    mjs_setString(pair->bodyname1, std::string(txt).c_str());
    mjs_setString(pair->bodyname2, std::string(txt2).c_str());
  }

  return true;
}



// add child body for cloth
mjsBody* mjCComposite::AddRopeBody(mjCModel* model, mjsBody* body, int ix, int ix1) {
  char txt[100];
  bool isroot = (ix==ix1);
  double dx = spacing*(ix1-ix);

  // add child if not root
  if (!isroot) {
    body = mjs_addBody(body, 0);
    mju::sprintf_arr(txt, "%sB%d", prefix.c_str(), ix1);
    mjs_setString(body->name, txt);

    // loop
    if (type==mjCOMPTYPE_LOOP) {
      double alpha = 2*mjPI/count[0];
      double R = 0.5*spacing*sin(mjPI-alpha)/sin(0.5*alpha);

      if (ix1>ix) {
        mjuu_setvec(body->pos, R*cos(0.5*alpha), R*sin(0.5*alpha), 0);
        mjuu_setvec(body->quat, cos(0.5*alpha), 0, 0, sin(0.5*alpha));
      } else {
        mjuu_setvec(body->pos, -R*cos(0.5*alpha), R*sin(0.5*alpha), 0);
        mjuu_setvec(body->quat, cos(-0.5*alpha), 0, 0, sin(-0.5*alpha));
      }
    }

    // no loop
    else {
      mjuu_setvec(body->pos, dx, 0, 0);
    }
  }

  // add geom
  mjsGeom* geom = mjs_addGeom(body, &def[0].spec);
  mjs_setDefault(geom->element, mjs_getDefault(body->element));
  mju::sprintf_arr(txt, "%sG%d", prefix.c_str(), ix1);
  mjs_setString(geom->name, txt);
  mjuu_setvec(geom->pos, 0, 0, 0);
  mjuu_setvec(geom->quat, sqrt(0.5), 0, sqrt(0.5), 0);

  // root: no joints
  if (isroot) {
    return body;
  }

  // add main joint
  for (int i=0; i<2; i++) {
    // add joint
    mjsJoint* jnt = mjs_addJoint(body, &defjoint[mjCOMPKIND_JOINT][0].spec);
    mjs_setDefault(jnt->element, mjs_getDefault(body->element));
    mju::sprintf_arr(txt, "%sJ%d_%d", prefix.c_str(), i, ix1);
    mjs_setString(jnt->name, txt);
    jnt->type = mjJNT_HINGE;
    mjuu_setvec(jnt->pos, -0.5*dx, 0, 0);
    mjuu_setvec(jnt->axis, 0, 0, 0);
    jnt->axis[i+1] = 1;
  }

  // add twist joint
  if (add[mjCOMPKIND_TWIST]) {
    // add joint
    mjsJoint* jnt = mjs_addJoint(body, &defjoint[mjCOMPKIND_TWIST][0].spec);
    mjs_setDefault(jnt->element, mjs_getDefault(body->element));
    mju::sprintf_arr(txt, "%sJT%d", prefix.c_str(), ix1);
    mjs_setString(jnt->name, txt);
    jnt->type = mjJNT_HINGE;
    mjuu_setvec(jnt->pos, -0.5*dx, 0, 0);
    mjuu_setvec(jnt->axis, 1, 0, 0);

    // add constraint
    mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_TWIST].spec);
    mjs_setDefault(eq->element, &model->Defaults(0)->spec);
    eq->type = mjEQ_JOINT;
    mjs_setString(eq->name1, mjs_getString(jnt->name));
  }

  // add stretch joint
  if (add[mjCOMPKIND_STRETCH]) {
    // add joint
    mjsJoint* jnt = mjs_addJoint(body, &defjoint[mjCOMPKIND_STRETCH][0].spec);
    mjs_setDefault(jnt->element, mjs_getDefault(body->element));
    mju::sprintf_arr(txt, "%sJS%d", prefix.c_str(), ix1);
    mjs_setString(jnt->name, txt);
    jnt->type = mjJNT_SLIDE;
    mjuu_setvec(jnt->pos, -0.5*dx, 0, 0);
    mjuu_setvec(jnt->axis, 1, 0, 0);

    // add constraint
    mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_STRETCH].spec);
    mjs_setDefault(eq->element,  &model->Defaults(0)->spec);
    eq->type = mjEQ_JOINT;
    mjs_setString(eq->name1, mjs_getString(jnt->name));
  }

  return body;
}



// project from box to other shape
void mjCComposite::BoxProject(double* pos) {
  // determine sizes
  double size[3] = {
    0.5*spacing*(count[0]-1),
    0.5*spacing*(count[1]-1),
    0.5*spacing*(count[2]-1)
  };

  // box
  if (type==mjCOMPTYPE_BOX) {
    pos[0] *= size[0];
    pos[1] *= size[1];
    pos[2] *= size[2];
  }

  // cylinder
  else if (type==mjCOMPTYPE_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==mjCOMPTYPE_ELLIPSOID) {
    mjuu_normvec(pos, 3);
    pos[0] *= size[0];
    pos[1] *= size[1];
    pos[2] *= size[2];
  }
}



// make 3d box, ellipsoid or cylinder
bool mjCComposite::MakeBox(mjCModel* model, mjsBody* body, char* error, int error_sz) {
  char txt[100];

  // check dim
  if (dim!=3) {
    return comperr(error, "Box and ellipsoid must be three-dimensional", error_sz);
  }

  // center geom: two times bigger
  mjsGeom* geom = mjs_addGeom(body, &def[0].spec);
  mjs_setDefault(geom->element, mjs_getDefault(body->element));
  geom->type = mjGEOM_SPHERE;
  mju::sprintf_arr(txt, "%sGcenter", prefix.c_str());
  mjs_setString(geom->name, txt);
  mjuu_setvec(geom->pos, 0, 0, 0);
  geom->size[0] *= 2;
  geom->size[1] = 0;
  geom->size[2] = 0;

  // fixed tendon for all joints
  mjCTendon* ten = model->AddTendon(def + mjCOMPKIND_TENDON);
  ten->def = model->Defaults(0);
  mju::sprintf_arr(txt, "%sT", prefix.c_str());
  ten->name = txt;

  // create bodies, geoms and joints: outside shell only
  for (int ix=0; ix<count[0]; ix++) {
    for (int iy=0; iy<count[1]; iy++) {
      for (int iz=0; iz<count[2]; iz++) {
        if (ix==0 || ix==count[0]-1 ||
            iy==0 || iy==count[1]-1 ||
            iz==0 || iz==count[2]-1) {
          // create body
          mjsBody* b = mjs_addBody(body, NULL);
          mju::sprintf_arr(txt, "%sB%d_%d_%d", prefix.c_str(), ix, iy, iz);
          mjs_setString(b->name, txt);

          // set body position (+/- 1)
          b->pos[0] = 2.0*ix/(count[0]-1) - 1;
          b->pos[1] = 2.0*iy/(count[1]-1) - 1;
          b->pos[2] = 2.0*iz/(count[2]-1) - 1;

          // reshape
          BoxProject(b->pos);

          // reorient body
          b->alt.type = mjORIENTATION_ZAXIS;
          mjuu_copyvec(b->alt.zaxis, b->pos, 3);
          mjuu_normvec(b->alt.zaxis, 3);

          // add geom
          mjsGeom* g = mjs_addGeom(b, &def[0].spec);
          mjs_setDefault(g->element, mjs_getDefault(body->element));
          mju::sprintf_arr(txt, "%sG%d_%d_%d", prefix.c_str(), ix, iy, iz);
          mjs_setString(g->name, txt);

          // offset inwards, enforce sphere or capsule
          if (g->type==mjGEOM_CAPSULE) {
            g->pos[2] = -(g->size[0] + g->size[1]);
          } else {
            g->type = mjGEOM_SPHERE;
            g->pos[2] = -g->size[0];
          }

          // add slider joint
          mjsJoint* jnt = mjs_addJoint(b, &defjoint[mjCOMPKIND_JOINT][0].spec);
          mjs_setDefault(jnt->element, mjs_getDefault(body->element));
          mju::sprintf_arr(txt, "%sJ%d_%d_%d", prefix.c_str(), ix, iy, iz);
          mjs_setString(jnt->name, txt);
          jnt->type = mjJNT_SLIDE;
          mjuu_setvec(jnt->pos, 0, 0, 0);
          mjuu_setvec(jnt->axis, 0, 0, 1);

          // add fix constraint
          mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_JOINT].spec);
          mjs_setDefault(eq->element,  &model->Defaults(0)->spec);
          eq->type = mjEQ_JOINT;
          mjs_setString(eq->name1, mjs_getString(jnt->name));

          // add joint to tendon
          ten->WrapJoint(std::string(mjs_getString(jnt->name)), 1);

          // add neighbor constraints
          for (int i=0; i<3; i++) {
            int ix1 = mjMIN(ix+(i==0), count[0]-1);
            int iy1 = mjMIN(iy+(i==1), count[1]-1);
            int iz1 = mjMIN(iz+(i==2), count[2]-1);
            if ((ix1==0 || ix1==count[0]-1 ||
                 iy1==0 || iy1==count[1]-1 ||
                 iz1==0 || iz1==count[2]-1) &&
                (ix!=ix1 || iy!=iy1 || iz!=iz1)) {
              char txt2[200];
              mju::sprintf_arr(txt2,
                               "%sJ%d_%d_%d", prefix.c_str(), ix1, iy1, iz1);
              mjsEquality* eqn = mjs_addEquality(&model->spec, 0);
              mju_copy(eqn->solref, solrefsmooth, mjNREF);
              mju_copy(eqn->solimp, solimpsmooth, mjNIMP);
              eqn->type = mjEQ_JOINT;
              mjs_setString(eqn->name1, txt);
              mjs_setString(eqn->name2, txt2);
            }
          }
        }
      }
    }
  }

  // finalize fixed tendon
  mjsEquality* eqt = mjs_addEquality(&model->spec, &def[mjCOMPKIND_TENDON].spec);
  mjs_setDefault(eqt->element, &model->Defaults(0)->spec);
  eqt->type = mjEQ_TENDON;
  mjs_setString(eqt->name1, ten->name.c_str());

  // skin
  if (skin) {
    MakeSkin3(model);
  }

  return true;
}



// add shear tendons to 2D
void mjCComposite::MakeShear(mjCModel* model) {
  char txt[100], txt1[100], txt2[100];

  for (int ix=0; ix<count[0]-1; ix++) {
    for (int iy=0; iy<count[1]-1; iy++) {
      // recover site names
      mju::sprintf_arr(txt1, "%sS%d_%d", prefix.c_str(), ix, iy);
      mju::sprintf_arr(txt2, "%sS%d_%d", prefix.c_str(), ix+1, iy+1);

      // create tendon
      mjCTendon* ten = model->AddTendon(def + mjCOMPKIND_SHEAR);
      ten->def = model->Defaults(0);
      ten->WrapSite(txt1);
      ten->WrapSite(txt2);

      // name tendon
      mju::sprintf_arr(txt, "%sTS%d_%d", prefix.c_str(), ix, iy);
      ten->name = txt;

      // equality constraint
      mjsEquality* eq = mjs_addEquality(&model->spec, &def[mjCOMPKIND_SHEAR].spec);
      mjs_setDefault(eq->element, &model->Defaults(0)->spec);
      eq->type = mjEQ_TENDON;
      mjs_setString(eq->name1, txt);
    }
  }
}



// copy local vectors to skin
void mjCComposite::CopyIntoSkin(mjsSkin* skin) {
  mjs_setInt(skin->face, face.data(), face.size());
  mjs_setFloat(skin->vert, vert.data(), vert.size());
  mjs_setFloat(skin->bindpos, bindpos.data(), bindpos.size());
  mjs_setFloat(skin->bindquat, bindquat.data(), bindquat.size());
  mjs_setFloat(skin->texcoord, texcoord.data(), texcoord.size());

  for (int i=0; i<vertid.size(); i++) {
    mjs_appendIntVec(skin->vertid, vertid[i].data(), vertid[i].size());
  }
  for (int i=0; i< vertweight.size(); i++) {
    mjs_appendFloatVec(skin->vertweight, vertweight[i].data(), vertweight[i].size());
  }

  face.clear();
  vert.clear();
  bindpos.clear();
  bindquat.clear();
  texcoord.clear();
  vertid.clear();
  vertweight.clear();
}



// add skin to 2D
void mjCComposite::MakeSkin2(mjCModel* model, mjtNum inflate) {
  char txt[100];
  int N = count[0]*count[1];

  // add skin, set name and material
  mjsSkin* skin = mjs_addSkin(&model->spec);
  mju::sprintf_arr(txt, "%sSkin", prefix.c_str());
  mjs_setString(skin->name, txt);
  mjs_setString(skin->material, skinmaterial.c_str());
  mjuu_copyvec(skin->rgba, skinrgba, 4);
  skin->inflate = inflate;
  skin->group = skingroup;

  // copy skin from existing mesh
  if (type==mjCOMPTYPE_PARTICLE && username.empty()) {
    std::vector<int> skinface;
    skinface = mjXUtil::String2Vector<int>(userface);
    int nvert = uservert.size()/3;

    for (int j=0; j<2; j++) {
      for (int i=0; i<nvert; i++) {
        vert.push_back(0);
        vert.push_back(0);
        vert.push_back(0);

        mju::sprintf_arr(txt, "%sB%d", prefix.c_str(), i);
        mjs_appendString(skin->bodyname, txt);
        bindpos.push_back(0);
        bindpos.push_back(0);
        bindpos.push_back(0);
        bindquat.push_back(1);
        bindquat.push_back(0);
        bindquat.push_back(0);
        bindquat.push_back(0);

        vertid.push_back({j*nvert+i});
        vertweight.push_back({1});
      }

      for (int i=0; i<skinface.size()/3; i++) {
        face.push_back(j*nvert+skinface[3*i]);
        face.push_back(j*nvert+skinface[3*i+(j==0 ? 1 : 2)]);
        face.push_back(j*nvert+skinface[3*i+(j==0 ? 2 : 1)]);
      }
    }

    CopyIntoSkin(skin);
    return;
  }

  // populate mesh: two sides
  for (int i=0; i<2; i++) {
    for (int ix=0; ix<count[0]; ix++) {
      for (int iy=0; iy<count[1]; iy++) {
        // vertex
        vert.push_back(0);
        vert.push_back(0);
        vert.push_back(0);

        // texture coordinate
        if (skintexcoord) {
          texcoord.push_back(ix/(float)(count[0]-1));
          texcoord.push_back(iy/(float)(count[1]-1));
        }

        // face
        if (ix<count[0]-1 && iy<count[1]-1) {
          face.push_back(i*N + ix*count[1]+iy);
          face.push_back(i*N + (ix+1)*count[1]+iy+(i==1));
          face.push_back(i*N + (ix+1)*count[1]+iy+(i==0));

          face.push_back(i*N + ix*count[1]+iy);
          face.push_back(i*N + (ix+(i==0))*count[1]+iy+1);
          face.push_back(i*N + (ix+(i==1))*count[1]+iy+1);
        }
      }
    }
  }

  // add thin triangles: X direction, iy = 0
  for (int ix=0; ix<count[0]-1; ix++) {
    face.push_back(ix*count[1]);
    face.push_back(N + (ix+1)*count[1]);
    face.push_back((ix+1)*count[1]);

    face.push_back(ix*count[1]);
    face.push_back(N + ix*count[1]);
    face.push_back(N + (ix+1)*count[1]);
  }

  // add thin triangles: X direction, iy = count[1]-1
  for (int ix=0; ix<count[0]-1; ix++) {
    face.push_back(ix*count[1] + count[1]-1);
    face.push_back((ix+1)*count[1] + count[1]-1);
    face.push_back(N + (ix+1)*count[1] + count[1]-1);

    face.push_back(ix*count[1] + count[1]-1);
    face.push_back(N + (ix+1)*count[1] + count[1]-1);
    face.push_back(N + ix*count[1] + count[1]-1);
  }

  // add thin triangles: Y direction, ix = 0
  for (int iy=0; iy<count[1]-1; iy++) {
    face.push_back(iy);
    face.push_back(iy+1);
    face.push_back(N + iy+1);

    face.push_back(iy);
    face.push_back(N + iy+1);
    face.push_back(N + iy);
  }

  // add thin triangles: Y direction, ix = count[0]-1
  for (int iy=0; iy<count[1]-1; iy++) {
    face.push_back(iy + (count[0]-1)*count[1]);
    face.push_back(N + iy+1 + (count[0]-1)*count[1]);
    face.push_back(iy+1 + (count[0]-1)*count[1]);

    face.push_back(iy + (count[0]-1)*count[1]);
    face.push_back(N + iy + (count[0]-1)*count[1]);
    face.push_back(N + iy+1 + (count[0]-1)*count[1]);
  }

  // couple with bones
  if (type==mjCOMPTYPE_PARTICLE || type==mjCOMPTYPE_GRID) {
    MakeClothBones(model, skin);
  } else if (type==mjCOMPTYPE_CABLE) {
    MakeCableBones(model, skin);
  }

  CopyIntoSkin(skin);
}



// add bones in 2D
void mjCComposite::MakeClothBones(mjCModel* model, mjsSkin* skin) {
  char txt[100];
  int N = count[0]*count[1];

  // populate bones
  for (int ix=0; ix<count[0]; ix++) {
    for (int iy=0; iy<count[1]; iy++) {
      // body name
      if (type==mjCOMPTYPE_GRID) {
        mju::sprintf_arr(txt, "%sB%d_%d", prefix.c_str(), ix, iy);
      } else {
        mju::sprintf_arr(txt, "%sB%d_%d_0", prefix.c_str(), ix, iy);
      }

      // bind pose
      mjs_appendString(skin->bodyname, txt);
      bindpos.push_back(0);
      bindpos.push_back(0);
      bindpos.push_back(0);
      bindquat.push_back(1);
      bindquat.push_back(0);
      bindquat.push_back(0);
      bindquat.push_back(0);

      // create vertid and vertweight
      vertid.push_back({ix*count[1]+iy, N + ix*count[1]+iy});
      vertweight.push_back({1, 1});
    }
  }
}



void mjCComposite::MakeClothBonesSubgrid(mjCModel* model, mjsSkin* skin) {
  char txt[100];

  // populate bones
  for (int ix=0; ix<count[0]; ix++) {
    for (int iy=0; iy<count[1]; iy++) {
      // body name
      if (type==mjCOMPTYPE_GRID) {
        mju::sprintf_arr(txt, "%sB%d_%d", prefix.c_str(), ix, iy);
      } else {
        mju::sprintf_arr(txt, "%sB%d_%d_0", prefix.c_str(), ix, iy);
      }

      // bind pose
      mjs_appendString(skin->bodyname, txt);
      bindpos.push_back(ix*spacing);
      bindpos.push_back(iy*spacing);
      bindpos.push_back(0);
      bindquat.push_back(1);
      bindquat.push_back(0);
      bindquat.push_back(0);
      bindquat.push_back(0);

      // empty vertid and vertweight
      vertid.push_back({});
      vertweight.push_back({});
    }
  }
}



// add bones to 1D
void mjCComposite::MakeCableBones(mjCModel* model, mjsSkin* skin) {
  char this_body[100];
  int N = count[0]*count[1];

  // populate bones
  for (int ix=0; ix<count[0]; ix++) {
    for (int iy=0; iy<count[1]; iy++) {
      // body name
      if (ix==0) {
        mju::sprintf_arr(this_body, "%sB_first", prefix.c_str());
      } else if (ix>=count[0]-2) {
        mju::sprintf_arr(this_body, "%sB_last", prefix.c_str());
      } else {
        mju::sprintf_arr(this_body, "%sB_%d", prefix.c_str(), ix);
      }

      // bind pose
      if (iy==0) {
        mjs_appendString(skin->bodyname, this_body);
        bindpos.push_back((ix==count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
        bindpos.push_back(-def[0].spec.geom->size[1]);
        bindpos.push_back(0);
        bindquat.push_back(1); bindquat.push_back(0);
        bindquat.push_back(0); bindquat.push_back(0);
      } else {
        mjs_appendString(skin->bodyname, this_body);
        bindpos.push_back((ix==count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
        bindpos.push_back(def[0].spec.geom->size[1]);
        bindpos.push_back(0);
        bindquat.push_back(1); bindquat.push_back(0);
        bindquat.push_back(0); bindquat.push_back(0);
      }

      // create vertid and vertweight
      vertid.push_back({ix*count[1]+iy, N + ix*count[1]+iy});
      vertweight.push_back({1, 1});
    }
  }
}



void mjCComposite::MakeCableBonesSubgrid(mjCModel* model, mjsSkin* skin) {
  // populate bones
  for (int ix=0; ix<count[0]; ix++) {
    for (int iy=0; iy<count[1]; iy++) {
      char txt[100];

      // body name
      if (ix==0) {
        mju::sprintf_arr(txt, "%sB_first", prefix.c_str());
      } else if (ix>=count[0]-2) {
        mju::sprintf_arr(txt, "%sB_last", prefix.c_str());
      } else {
        mju::sprintf_arr(txt, "%sB_%d", prefix.c_str(), ix);
      }

      // bind pose
      if (iy==0) {
        bindpos.push_back((ix==count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
        bindpos.push_back(-def[0].Geom().spec.size[1]);
        bindpos.push_back(0);
      } else if (iy==2) {
        bindpos.push_back((ix==count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
        bindpos.push_back(def[0].Geom().spec.size[1]);
        bindpos.push_back(0);
      } else {
        bindpos.push_back((ix==count[0]-1) ? -2*def[0].spec.geom->size[0] : 0);
        bindpos.push_back(0);
        bindpos.push_back(0);
      }
      mjs_appendString(skin->bodyname, txt);
      bindquat.push_back(1);
      bindquat.push_back(0);
      bindquat.push_back(0);
      bindquat.push_back(0);

      // empty vertid and vertweight
      vertid.push_back({});
      vertweight.push_back({});
    }
  }
}



//------------------------------------- subgrid matrices

// C = W * [f; f_x; f_y; f_xy]
static const mjtNum subW[16*16] = {
  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  0,  0,  0,  0,  0,  0,
 -3,  0,  0,  3,  0,  0,  0,  0, -2,  0,  0, -1,  0,  0,  0,  0,
  2,  0,  0, -2,  0,  0,  0,  0,  1,  0,  0,  1,  0,  0,  0,  0,
  0,  0,  0,  0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  0,  0,
  0,  0,  0,  0, -3,  0,  0,  3,  0,  0,  0,  0, -2,  0,  0, -1,
  0,  0,  0,  0,  2,  0,  0, -2,  0,  0,  0,  0,  1,  0,  0,  1,
 -3,  3,  0,  0, -2, -1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
  0,  0,  0,  0,  0,  0,  0,  0, -3,  3,  0,  0, -2, -1,  0,  0,
  9, -9,  9, -9,  6,  3, -3, -6,  6, -6, -3,  3,  4,  2,  1,  2,
 -6,  6, -6,  6, -4, -2,  2,  4, -3,  3,  3, -3, -2, -1, -1, -2,
  2, -2,  0,  0,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
  0,  0,  0,  0,  0,  0,  0,  0,  2, -2,  0,  0,  1,  1,  0,  0,
 -6,  6, -6,  6, -3, -3,  3,  3, -4,  4,  2, -2, -2, -2, -1, -1,
  4, -4,  4, -4,  2,  2, -2, -2,  2, -2, -2,  2,  1,  1,  1,  1
};

// left-bottom
static const mjtNum subD00[] = {
  5,  1, -1,                  // f
  9,  1, -1,
  10, 1, -1,
  6,  1, -1,

  5, -1,   9,  1,   -1,       // f_x
  5, -0.5, 13, 0.5, -1,
  6, -0.5, 14, 0.5, -1,
  6, -1,   10, 1,   -1,

  5, -1,   6,  1,   -1,       // f_y
  9, -1,   10, 1,   -1,
  9, -0.5, 11, 0.5, -1,
  5, -0.5, 7,  0.5, -1,

  9,  -1,    6, -1,    5, 1,    10, 1,    -1,     // f_xy
  13, -0.5,  6, -0.5,  5, 0.5,  14, 0.5,  -1,
  13, -0.25, 7, -0.25, 5, 0.25, 15, 0.25, -1,
  9,  -0.5,  7, -0.5,  5, 0.5,  11, 0.5,  -1
};

// center-bottom
static const mjtNum subD10[] = {
  5,  1, -1,                  // f
  9,  1, -1,
  10, 1, -1,
  6,  1, -1,

  1, -0.5, 9,  0.5, -1,       // f_x
  5, -0.5, 13, 0.5, -1,
  6, -0.5, 14, 0.5, -1,
  2, -0.5, 10, 0.5, -1,

  5, -1,   6,  1,   -1,       // f_y
  9, -1,   10, 1,   -1,
  9, -0.5, 11, 0.5, -1,
  5, -0.5, 7,  0.5, -1,

  9,  -0.5,  2, -0.5,  1, 0.5,  10, 0.5,  -1,     // f_xy
  13, -0.5,  6, -0.5,  5, 0.5,  14, 0.5,  -1,
  13, -0.25, 7, -0.25, 5, 0.25, 15, 0.25, -1,
  9,  -0.25, 3, -0.25, 1, 0.25, 11, 0.25, -1
};

// right-bottom
static const mjtNum subD20[] = {
  5,  1, -1,                  // f
  9,  1, -1,
  10, 1, -1,
  6,  1, -1,

  1, -0.5, 9,  0.5, -1,       // f_x
  5, -1,   9,  1,   -1,
  6, -1,   10, 1,   -1,
  2, -0.5, 10, 0.5, -1,

  5, -1,   6,  1,   -1,       // f_y
  9, -1,   10, 1,   -1,
  9, -0.5, 11, 0.5, -1,
  5, -0.5, 7,  0.5, -1,

  9, -0.5,  2, -0.5,  1, 0.5,  10, 0.5,  -1,      // f_xy
  9, -1,    6, -1,    5, 1,    10, 1,    -1,
  9, -0.5,  7, -0.5,  5, 0.5,  11, 0.5,  -1,
  9, -0.25, 3, -0.25, 1, 0.25, 11, 0.25, -1
};

// left-center
static const mjtNum subD01[] = {
  5,  1, -1,                  // f
  9,  1, -1,
  10, 1, -1,
  6,  1, -1,

  5, -1,   9,  1,   -1,       // f_x
  5, -0.5, 13, 0.5, -1,
  6, -0.5, 14, 0.5, -1,
  6, -1,   10, 1,   -1,

  4, -0.5, 6,  0.5, -1,       // f_y
  8, -0.5, 10, 0.5, -1,
  9, -0.5, 11, 0.5, -1,
  5, -0.5, 7,  0.5, -1,

  8,  -0.5,  6, -0.5,  4, 0.5,  10, 0.5,  -1,     // f_xy
  12, -0.25, 6, -0.25, 4, 0.25, 14, 0.25, -1,
  13, -0.25, 7, -0.25, 5, 0.25, 15, 0.25, -1,
  9,  -0.5,  7, -0.5,  5, 0.5,  11, 0.5,  -1
};

// center-center
static const mjtNum subD11[] = {
  5,  1, -1,                  // f
  9,  1, -1,
  10, 1, -1,
  6,  1, -1,

  1, -0.5, 9,  0.5, -1,       // f_x
  5, -0.5, 13, 0.5, -1,
  6, -0.5, 14, 0.5, -1,
  2, -0.5, 10, 0.5, -1,

  4, -0.5, 6,  0.5, -1,       // f_y
  8, -0.5, 10, 0.5, -1,
  9, -0.5, 11, 0.5, -1,
  5, -0.5, 7,  0.5, -1,

  8,  -0.25, 2, -0.25, 0, 0.25, 10, 0.25, -1,     // f_xy
  12, -0.25, 6, -0.25, 4, 0.25, 14, 0.25, -1,
  13, -0.25, 7, -0.25, 5, 0.25, 15, 0.25, -1,
  9,  -0.25, 3, -0.25, 1, 0.25, 11, 0.25, -1
};

// right-center
static const mjtNum subD21[] = {
  5,  1, -1,                  // f
  9,  1, -1,
  10, 1, -1,
  6,  1, -1,

  1, -0.5, 9,  0.5, -1,       // f_x
  5, -1,   9,  1,   -1,
  6, -1,   10, 1,   -1,
  2, -0.5, 10, 0.5, -1,

  4, -0.5, 6,  0.5, -1,       // f_y
  8, -0.5, 10, 0.5, -1,
  9, -0.5, 11, 0.5, -1,
  5, -0.5, 7,  0.5, -1,

  8, -0.25, 2, -0.25, 0, 0.25, 10, 0.25, -1,      // f_xy
  8, -0.5,  6, -0.5,  4, 0.5,  10, 0.5,  -1,
  9, -0.5,  7, -0.5,  5, 0.5,  11, 0.5,  -1,
  9, -0.25, 3, -0.25, 1, 0.25, 11, 0.25, -1
};

// left-top
static const mjtNum subD02[] = {
  5,  1, -1,                  // f
  9,  1, -1,
  10, 1, -1,
  6,  1, -1,

  5, -1,   9,  1,   -1,       // f_x
  5, -0.5, 13, 0.5, -1,
  6, -0.5, 14, 0.5, -1,
  6, -1,   10, 1,   -1,

  4, -0.5, 6,  0.5, -1,       // f_y
  8, -0.5, 10, 0.5, -1,
  9, -1,   10, 1,   -1,
  5, -1,   6,  1,   -1,

  8,  -0.5,  6, -0.5,  4, 0.5,  10, 0.5,  -1,     // f_xy
  12, -0.25, 6, -0.25, 4, 0.25, 14, 0.25, -1,
  13, -0.5,  6, -0.5,  5, 0.5,  14, 0.5,  -1,
  9,  -1,    6, -1,    5, 1,    10, 1,    -1
};

// center-top
static const mjtNum subD12[] = {
  5,  1, -1,                  // f
  9,  1, -1,
  10, 1, -1,
  6,  1, -1,

  1, -0.5, 9,  0.5, -1,       // f_x
  5, -0.5, 13, 0.5, -1,
  6, -0.5, 14, 0.5, -1,
  2, -0.5, 10, 0.5, -1,

  4, -0.5, 6,  0.5, -1,       // f_y
  8, -0.5, 10, 0.5, -1,
  9, -1,   10, 1,   -1,
  5, -1,   6,  1,   -1,

  8,  -0.25, 2, -0.25, 0, 0.25, 10, 0.25, -1,     // f_xy
  12, -0.25, 6, -0.25, 4, 0.25, 14, 0.25, -1,
  13, -0.5,  6, -0.5,  5, 0.5,  14, 0.5,  -1,
  9,  -0.5,  2, -0.5,  1, 0.5,  10, 0.5,  -1
};

// right-top
static const mjtNum subD22[] = {
  5,  1, -1,                  // f
  9,  1, -1,
  10, 1, -1,
  6,  1, -1,

  1, -0.5, 9,  0.5, -1,       // f_x
  5, -1,   9,  1,   -1,
  6, -1,   10, 1,   -1,
  2, -0.5, 10, 0.5, -1,

  4, -0.5, 6,  0.5, -1,       // f_y
  8, -0.5, 10, 0.5, -1,
  9, -1,   10, 1,   -1,
  5, -1,   6,  1,   -1,

  8, -0.25, 2, -0.25, 0, 0.25, 10, 0.25, -1,      // f_xy
  8, -0.5,  6, -0.5,  4, 0.5,  10, 0.5,  -1,
  9, -1,    6, -1,    5, 1,    10, 1,    -1,
  9, -0.5,  2, -0.5,  1, 0.5,  10, 0.5,  -1
};


// add skin to 2D, with subgrid
void mjCComposite::MakeSkin2Subgrid(mjCModel* model, mjtNum inflate) {
  // assemble pointers to Dxx matrices
  const mjtNum* Dp[3][3] = {
    {subD00, subD01, subD02},
    {subD10, subD11, subD12},
    {subD20, subD21, subD22}
  };

  // allocate
  const int N = (2+skinsubgrid)*(2+skinsubgrid);
  mjtNum* XY = (mjtNum*) mju_malloc(N*16*sizeof(mjtNum));
  mjtNum* XY_W = (mjtNum*) mju_malloc(N*16*sizeof(mjtNum));
  mjtNum* Weight = (mjtNum*) mju_malloc(9*N*16*sizeof(mjtNum));
  mjtNum* D = (mjtNum*) mju_malloc(16*16*sizeof(mjtNum));

  // XY matrix
  const mjtNum step = 1.0/(1+skinsubgrid);
  int rxy = 0;
  for (int sx=0; sx<=1+skinsubgrid; sx++) {
    for (int sy=0; sy<=1+skinsubgrid; sy++) {
      // compute x, y
      mjtNum x = sx*step;
      mjtNum y = sy*step;

      // make XY-row
      XY[16*rxy + 0] =  1;
      XY[16*rxy + 1] =  y;
      XY[16*rxy + 2] =  y*y;
      XY[16*rxy + 3] =  y*y*y;

      XY[16*rxy + 4] =  x*1;
      XY[16*rxy + 5] =  x*y;
      XY[16*rxy + 6] =  x*y*y;
      XY[16*rxy + 7] =  x*y*y*y;

      XY[16*rxy + 8] =  x*x*1;
      XY[16*rxy + 9] =  x*x*y;
      XY[16*rxy + 10] = x*x*y*y;
      XY[16*rxy + 11] = x*x*y*y*y;

      XY[16*rxy + 12] = x*x*x*1;
      XY[16*rxy + 13] = x*x*x*y;
      XY[16*rxy + 14] = x*x*x*y*y;
      XY[16*rxy + 15] = x*x*x*y*y*y;

      // advance row
      rxy++;
    }
  }

  // XY_W = XY * W
  mju_mulMatMat(XY_W, XY, subW, N, 16, 16);

  // Weight matrices
  for (int dx=0; dx<3; dx++) {
    for (int dy=0; dy<3; dy++) {
      // make dense D
      mju_zero(D, 16*16);
      int cnt = 0;
      int r = 0, c;
      while (r<16) {
        // scan row
        while ((c = mju_round(Dp[dx][dy][cnt]))!=-1) {
          D[r*16+c] = Dp[dx][dy][cnt+1];
          cnt +=2;
        }

        // advance
        r++;
        cnt++;
      }

      // Weight(d) = XY * W * D(d)
      mju_mulMatMat(Weight + (dx*3+dy)*N*16, XY_W, D, N, 16, 16);
    }
  }

  // add skin, set name and material
  char txt[100];
  mjsSkin* skin = mjs_addSkin(&model->spec);
  mju::sprintf_arr(txt, "%sSkin", prefix.c_str());
  mjs_setString(skin->name, txt);
  mjs_setString(skin->material, skinmaterial.c_str());
  mjuu_copyvec(skin->rgba, skinrgba, 4);
  skin->inflate = inflate;
  skin->group = skingroup;

  // populate mesh: two sides
  mjtNum S = spacing/(1+skinsubgrid);
  int C0 = count[0] + (count[0]-1)*skinsubgrid;
  int C1 = count[1] + (count[1]-1)*skinsubgrid;
  int NN = C0*C1;
  for (int i=0; i<2; i++) {
    for (int ix=0; ix<C0; ix++) {
      for (int iy=0; iy<C1; iy++) {
        // vertex
        vert.push_back(ix*S);
        vert.push_back(iy*S);
        vert.push_back(0);

        // texture coordinate
        if (skintexcoord) {
          texcoord.push_back(ix/(float)(C0-1));
          texcoord.push_back(iy/(float)(C1-1));
        }

        // face
        if (ix<C0-1 && iy<C1-1) {
          face.push_back(i*NN + ix*C1+iy);
          face.push_back(i*NN + (ix+1)*C1+iy+(i==1));
          face.push_back(i*NN + (ix+1)*C1+iy+(i==0));

          face.push_back(i*NN + ix*C1+iy);
          face.push_back(i*NN + (ix+(i==0))*C1+iy+1);
          face.push_back(i*NN + (ix+(i==1))*C1+iy+1);
        }
      }
    }
  }

  // add thin triangles: X direction, iy = 0
  for (int ix=0; ix<C0-1; ix++) {
    face.push_back(ix*C1);
    face.push_back(NN + (ix+1)*C1);
    face.push_back((ix+1)*C1);

    face.push_back(ix*C1);
    face.push_back(NN + ix*C1);
    face.push_back(NN + (ix+1)*C1);
  }

  // add thin triangles: X direction, iy = C1-1
  for (int ix=0; ix<C0-1; ix++) {
    face.push_back(ix*C1 + C1-1);
    face.push_back((ix+1)*C1 + C1-1);
    face.push_back(NN + (ix+1)*C1 + C1-1);

    face.push_back(ix*C1 + C1-1);
    face.push_back(NN + (ix+1)*C1 + C1-1);
    face.push_back(NN + ix*C1 + C1-1);
  }

  // add thin triangles: Y direction, ix = 0
  for (int iy=0; iy<C1-1; iy++) {
    face.push_back(iy);
    face.push_back(iy+1);
    face.push_back(NN + iy+1);

    face.push_back(iy);
    face.push_back(NN + iy+1);
    face.push_back(NN + iy);
  }

  // add thin triangles: Y direction, ix = C0-1
  for (int iy=0; iy<C1-1; iy++) {
    face.push_back(iy + (C0-1)*C1);
    face.push_back(NN + iy+1 + (C0-1)*C1);
    face.push_back(iy+1 + (C0-1)*C1);

    face.push_back(iy + (C0-1)*C1);
    face.push_back(NN + iy + (C0-1)*C1);
    face.push_back(NN + iy+1 + (C0-1)*C1);
  }

  if (type==mjCOMPTYPE_PARTICLE || type==mjCOMPTYPE_GRID) {
    MakeClothBonesSubgrid(model, skin);
  } else if (type==mjCOMPTYPE_CABLE) {
    MakeCableBonesSubgrid(model, skin);
  }

  // bind vertices to bones: one big square at a time
  for (int ix=0; ix<count[0]-1; ix++) {
    for (int iy=0; iy<count[1]-1; iy++) {
      // determine d for Weight indexing
      int d = 3 * (ix==0 ? 0 : (ix==count[0]-2 ? 2 : 1)) +
              (iy==0 ? 0 : (iy==count[1]-2 ? 2 : 1));

      // precompute 16 bone indices for big square
      int boneid[16];
      int cnt = 0;
      for (int dx=-1; dx<3; dx++) {
        for (int dy=-1; dy<3; dy++) {
          boneid[cnt++] = (ix+dx)*count[1] + (iy+dy);
        }
      }

      // process subgrid, top-rigth owns last index
      for (int dx=0; dx<1+skinsubgrid+(ix==count[0]-2); dx++) {
        for (int dy=0; dy<1+skinsubgrid+(iy==count[1]-2); dy++) {
          // recover vertex id
          int vid = (ix*(1+skinsubgrid)+dx)*C1 + iy*(1+skinsubgrid)+dy;

          // determine row in Weight
          int n = dx*(2+skinsubgrid) + dy;

          // add vertex to 16 bones
          for (int bi=0; bi<16; bi++) {
            mjtNum w = Weight[d*N*16 + n*16 + bi];
            if (w) {
              vertid[boneid[bi]].push_back(vid);
              vertid[boneid[bi]].push_back(vid+NN);
              vertweight[boneid[bi]].push_back((float)w);
              vertweight[boneid[bi]].push_back((float)w);
            }
          }
        }
      }
    }
  }

  CopyIntoSkin(skin);

  // free allocations
  mju_free(XY);
  mju_free(XY_W);
  mju_free(Weight);
  mju_free(D);
}



// add skin to 3D
void mjCComposite::MakeSkin3(mjCModel* model) {
  int vcnt = 0;
  std::map<string, int> vmap;
  char txt[100], cnt0[10], cnt1[10], cnt2[10];
  string fmt;

  // string counts
  mju::sprintf_arr(cnt0, "%d", count[0]-1);
  mju::sprintf_arr(cnt1, "%d", count[1]-1);
  mju::sprintf_arr(cnt2, "%d", count[2]-1);

  // add skin, set name and material
  mjsSkin* skin = mjs_addSkin(&model->spec);
  mju::sprintf_arr(txt, "%sSkin", prefix.c_str());
  mjs_setString(skin->name, txt);
  mjs_setString(skin->material, skinmaterial.c_str());
  mjuu_copyvec(skin->rgba, skinrgba, 4);
  skin->inflate = skininflate;
  skin->group = skingroup;

  // box
  if (type==mjCOMPTYPE_BOX || type==mjCOMPTYPE_PARTICLE) {
    // z-faces
    MakeSkin3Box(skin, count[0], count[1], 1, vcnt, "%sB%d_%d_0");
    fmt = "%sB%d_%d_" + string(cnt2);
    MakeSkin3Box(skin, count[0], count[1], 0, vcnt, fmt.c_str());

    // y-faces
    MakeSkin3Box(skin, count[0], count[2], 0, vcnt, "%sB%d_0_%d");
    fmt = "%sB%d_" + string(cnt1) + "_%d";
    MakeSkin3Box(skin, count[0], count[2], 1, vcnt, fmt.c_str());

    // x-faces
    MakeSkin3Box(skin, count[1], count[2], 1, vcnt, "%sB0_%d_%d");
    fmt = "%sB" + string(cnt0) + "_%d_%d";
    MakeSkin3Box(skin, count[1], count[2], 0, vcnt, fmt.c_str());
  }

  // cylinder
  else if (type==mjCOMPTYPE_CYLINDER) {
    // generate vertices in map
    for (int ix=0; ix<count[0]; ix++) {
      for (int iy=0; iy<count[1]; iy++) {
        for (int iz=0; iz<count[2]; iz++) {
          bool xedge = (ix==0 || ix==count[0]-1);
          bool yedge = (iy==0 || iy==count[1]-1);
          if (xedge || yedge) {
            // body name
            mju::sprintf_arr(txt, "%sB%d_%d_%d", prefix.c_str(), ix, iy, iz);

            // add vertex
            vert.push_back(0);
            vert.push_back(0);
            vert.push_back(0);

            // texture coordinate
            if (skintexcoord) {
              float X = 0, Y = 0;
              if (xedge) {
                X = iy/(float)(count[1]-1);
                Y = iz/(float)(count[2]-1);
              } else {
                X = ix/(float)(count[0]-1);
                Y = iz/(float)(count[2]-1);
              }

              texcoord.push_back(X);
              texcoord.push_back(Y);
            }

            // save vertex id in map
            vmap[txt] = vcnt;
            vcnt++;
          }
        }
      }
    }

    // y-faces
    MakeSkin3Smooth(skin, count[0], count[2], 0, vmap, "%sB%d_0_%d");
    fmt = "%sB%d_" + string(cnt1) + "_%d";
    MakeSkin3Smooth(skin, count[0], count[2], 1, vmap, fmt.c_str());

    // x-faces
    MakeSkin3Smooth(skin, count[1], count[2], 1, vmap, "%sB0_%d_%d");
    fmt = "%sB" + string(cnt0) + "_%d_%d";
    MakeSkin3Smooth(skin, count[1], count[2], 0, vmap, fmt.c_str());

    // z-faces, boxy-type
    MakeSkin3Box(skin, count[0], count[1], 1, vcnt, "%sB%d_%d_0");
    fmt = "%sB%d_%d_" + string(cnt2);
    MakeSkin3Box(skin, count[0], count[1], 0, vcnt, fmt.c_str());
  }

  // smooth
  else {
    // generate vertices in map
    for (int ix=0; ix<count[0]; ix++) {
      for (int iy=0; iy<count[1]; iy++) {
        for (int iz=0; iz<count[2]; iz++) {
          bool xedge = (ix==0 || ix==count[0]-1);
          bool yedge = (iy==0 || iy==count[1]-1);
          bool zedge = (iz==0 || iz==count[2]-1);
          if (xedge || yedge || zedge) {
            // body name
            mju::sprintf_arr(txt, "%sB%d_%d_%d", prefix.c_str(), ix, iy, iz);

            // add vertex
            vert.push_back(0);
            vert.push_back(0);
            vert.push_back(0);

            // texture coordinate
            if (skintexcoord) {
              float X = 0, Y = 0;
              if (xedge) {
                X = iy/(float)(count[1]-1);
                Y = iz/(float)(count[2]-1);
              } else if (yedge) {
                X = ix/(float)(count[0]-1);
                Y = iz/(float)(count[2]-1);
              } else {
                X = ix/(float)(count[0]-1);
                Y = iy/(float)(count[1]-1);
              }

              texcoord.push_back(X);
              texcoord.push_back(Y);
            }

            // save vertex id in map
            vmap[txt] = vcnt;
            vcnt++;
          }
        }
      }
    }

    // z-faces
    MakeSkin3Smooth(skin, count[0], count[1], 1, vmap, "%sB%d_%d_0");
    fmt = "%sB%d_%d_" + string(cnt2);
    MakeSkin3Smooth(skin, count[0], count[1], 0, vmap, fmt.c_str());

    // y-faces
    MakeSkin3Smooth(skin, count[0], count[2], 0, vmap, "%sB%d_0_%d");
    fmt = "%sB%d_" + string(cnt1) + "_%d";
    MakeSkin3Smooth(skin, count[0], count[2], 1, vmap, fmt.c_str());

    // x-faces
    MakeSkin3Smooth(skin, count[1], count[2], 1, vmap, "%sB0_%d_%d");
    fmt = "%sB" + string(cnt0) + "_%d_%d";
    MakeSkin3Smooth(skin, count[1], count[2], 0, vmap, fmt.c_str());
  }

  CopyIntoSkin(skin);
}



// make one face of 3D skin, box
void mjCComposite::MakeSkin3Box(mjsSkin* skin, int c0, int c1, int side,
                                int& vcnt, const char* format) {
  char txt[100];

  // loop over bodies/vertices of specified face
  for (int i0=0; i0<c0; i0++) {
    for (int i1=0; i1<c1; i1++) {
      // vertex
      vert.push_back(0);
      vert.push_back(0);
      vert.push_back(0);

      // texture coordinate
      if (skintexcoord) {
        texcoord.push_back(i0/(float)(c0-1));
        texcoord.push_back(i1/(float)(c1-1));
      }

      // face
      if (i0<c0-1 && i1<c1-1) {
        face.push_back(vcnt + i0*c1+i1);
        face.push_back(vcnt + (i0+1)*c1+i1+(side==1));
        face.push_back(vcnt + (i0+1)*c1+i1+(side==0));

        face.push_back(vcnt + i0*c1+i1);
        face.push_back(vcnt + (i0+(side==0))*c1+i1+1);
        face.push_back(vcnt + (i0+(side==1))*c1+i1+1);
      }

      // body name
      mju::sprintf_arr(txt, format, prefix.c_str(), i0, i1);

      // bind pose: origin
      mjs_appendString(skin->bodyname, txt);
      bindpos.push_back(0);
      bindpos.push_back(0);
      bindpos.push_back(0);
      bindquat.push_back(1);
      bindquat.push_back(0);
      bindquat.push_back(0);
      bindquat.push_back(0);

      // vertid and vertweight
      vertid.push_back({vcnt + i0*c1+i1});
      vertweight.push_back({1});
    }
  }

  // update vertex count
  vcnt += c0*c1;
}



// make one face of 3D skin, smooth
void mjCComposite::MakeSkin3Smooth(mjsSkin* skin, int c0, int c1, int side,
                                   const std::map<string, int>& vmap, const char* format) {
  char txt00[100], txt01[100], txt10[100], txt11[100];

  // loop over bodies/vertices of specified face
  for (int i0=0; i0<c0; i0++) {
    for (int i1=0; i1<c1; i1++) {
      // body names
      mju::sprintf_arr(txt00, format, prefix.c_str(), i0, i1);
      mju::sprintf_arr(txt01, format, prefix.c_str(), i0, i1+1);
      mju::sprintf_arr(txt10, format, prefix.c_str(), i0+1, i1);
      mju::sprintf_arr(txt11, format, prefix.c_str(), i0+1, i1+1);

      // face
      if (i0<c0-1 && i1<c1-1) {
        if (side==0) {
          face.push_back(vmap.find(txt00)->second);
          face.push_back(vmap.find(txt10)->second);
          face.push_back(vmap.find(txt11)->second);

          face.push_back(vmap.find(txt00)->second);
          face.push_back(vmap.find(txt11)->second);
          face.push_back(vmap.find(txt01)->second);
        } else {
          face.push_back(vmap.find(txt00)->second);
          face.push_back(vmap.find(txt01)->second);
          face.push_back(vmap.find(txt11)->second);

          face.push_back(vmap.find(txt00)->second);
          face.push_back(vmap.find(txt11)->second);
          face.push_back(vmap.find(txt10)->second);
        }
      }

      // bind pose: origin
      mjs_appendString(skin->bodyname, txt00);
      bindpos.push_back(0);
      bindpos.push_back(0);
      bindpos.push_back(0);
      bindquat.push_back(1);
      bindquat.push_back(0);
      bindquat.push_back(0);
      bindquat.push_back(0);

      // vertid and vertweight
      vertid.push_back({vmap.find(txt00)->second});
      vertweight.push_back({1});
    }
  }
}