diff --git a/plugin/elasticity/elasticity.h b/plugin/elasticity/elasticity.h index fa70e356..d12b5a82 100644 --- a/plugin/elasticity/elasticity.h +++ b/plugin/elasticity/elasticity.h @@ -33,28 +33,6 @@ struct PairHash } }; -inline mjtNum SquaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) { - mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]}; - return dif[0]*dif[0] + dif[1]*dif[1] + dif[2]*dif[2]; -} - -inline void UpdateSquaredLengths(std::vector& len, - const std::vector >& edges, - const mjtNum* x) { - for (int e = 0; e < len.size(); e++) { - const mjtNum* p0 = x + 3*edges[e].first; - const mjtNum* p1 = x + 3*edges[e].second; - len[e] = SquaredDist3(p0, p1); - } -} - -inline void UpdateSquaredLengthsFlex(std::vector& len, - const mjtNum* flexedge_length) { - for (int e = 0; e < len.size(); e++) { - len[e] = flexedge_length[e]*flexedge_length[e]; - } -} - struct Stencil2D { static constexpr int kNumEdges = 3; static constexpr int kNumVerts = 3; diff --git a/plugin/elasticity/membrane.cc b/plugin/elasticity/membrane.cc index c380141d..da7c17d5 100644 --- a/plugin/elasticity/membrane.cc +++ b/plugin/elasticity/membrane.cc @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -164,32 +165,30 @@ Membrane::Membrane(const mjModel* m, mjData* d, int instance, mjtNum nu, // allocate array ne = edges.size(); - reference.assign(ne, 0); - deformed.assign(ne, 0); - previous.assign(ne, 0); elongation.assign(ne, 0); force.assign(3*nv, 0); - - // compute edge lengths at equilibrium (m->flexedge_length0 not yet available) - UpdateSquaredLengths(reference, edges, body_pos); - - // save previous lengths - previous = reference; } void Membrane::Compute(const mjModel* m, mjData* d, int instance) { mjtNum kD = damping / m->opt.timestep; - // update edge lengths - UpdateSquaredLengthsFlex(deformed, d->flexedge_length + m->flex_edgeadr[f0]); + // read edge lengths + mjtNum* deformed = d->flexedge_length + m->flex_edgeadr[f0]; + mjtNum* ref = m->flexedge_length0 + m->flex_edgeadr[f0]; + + // m->flexedge_length0 is not initialized when the plugin is constructed + if (prev.empty()) { + prev.assign(ne, 0); + memcpy(prev.data(), ref, sizeof(mjtNum) * ne); + } // we add generalized Rayleigh damping as decribed in Section 5.2 of // Kharevych et al., "Geometric, Variational Integrators for Computer // Animation" http://multires.caltech.edu/pubs/DiscreteLagrangian.pdf for (int idx = 0; idx < ne; idx++) { - elongation[idx] = deformed[idx] - reference[idx] + - ( deformed[idx] - previous[idx] ) * kD; + elongation[idx] = deformed[idx]*deformed[idx] - ref[idx]*ref[idx] + + ( deformed[idx]*deformed[idx] - prev[idx]*prev[idx] ) * kD; } // compute gradient of elastic energy and insert into passive force @@ -204,7 +203,7 @@ void Membrane::Compute(const mjModel* m, mjData* d, int instance) { // update stored lengths if (kD > 0) { - previous = deformed; + memcpy(prev.data(), deformed, sizeof(mjtNum) * ne); } } diff --git a/plugin/elasticity/membrane.h b/plugin/elasticity/membrane.h index 2b9a23da..815c0a5b 100644 --- a/plugin/elasticity/membrane.h +++ b/plugin/elasticity/membrane.h @@ -53,9 +53,7 @@ class Membrane { // precomputed quantities std::vector metric; // geom-induced metric (nt x 9) - std::vector reference; // reference lengths (ne x 1) - std::vector deformed; // deformed lengths (ne x 1) - std::vector previous; // previous-step lengths (ne x 1) + std::vector prev; // previous-step lengths (ne x 1) std::vector elongation; // edge elongation (ne x 1) std::vector force; // force at all vertices (nv x 3) diff --git a/plugin/elasticity/solid.cc b/plugin/elasticity/solid.cc index f624929c..d9fce036 100644 --- a/plugin/elasticity/solid.cc +++ b/plugin/elasticity/solid.cc @@ -16,6 +16,7 @@ #include #include #include +#include #include #include #include @@ -171,32 +172,30 @@ Solid::Solid(const mjModel* m, mjData* d, int instance, mjtNum nu, mjtNum E, // allocate array ne = edges.size(); - reference.assign(ne, 0); - deformed.assign(ne, 0); - previous.assign(ne, 0); elongation.assign(ne, 0); force.assign(3*nv, 0); - - // compute edge lengths at equilibrium (m->flexedge_length0 not yet available) - UpdateSquaredLengths(reference, edges, body_pos); - - // save previous lengths - previous = reference; } void Solid::Compute(const mjModel* m, mjData* d, int instance) { mjtNum kD = damping / m->opt.timestep; - // update edge lengths - UpdateSquaredLengthsFlex(deformed, d->flexedge_length + m->flex_edgeadr[f0]); + // read edge lengths + mjtNum* deformed = d->flexedge_length + m->flex_edgeadr[f0]; + mjtNum* ref = m->flexedge_length0 + m->flex_edgeadr[f0]; + + // m->flexedge_length0 is not initialized when the plugin is constructed + if (prev.empty()) { + prev.assign(ne, 0); + memcpy(prev.data(), ref, sizeof(mjtNum) * ne); + } // we add generalized Rayleigh damping as decribed in Section 5.2 of // Kharevych et al., "Geometric, Variational Integrators for Computer // Animation" http://multires.caltech.edu/pubs/DiscreteLagrangian.pdf for (int idx = 0; idx < ne; idx++) { - elongation[idx] = deformed[idx] - reference[idx] + - ( deformed[idx] - previous[idx] ) * kD; + elongation[idx] = deformed[idx]*deformed[idx] - ref[idx]*ref[idx] + + ( deformed[idx]*deformed[idx] - prev[idx]*prev[idx] ) * kD; } // compute gradient of elastic energy and insert into passive force @@ -211,7 +210,7 @@ void Solid::Compute(const mjModel* m, mjData* d, int instance) { // update stored lengths if (kD > 0) { - previous = deformed; + memcpy(prev.data(), deformed, sizeof(mjtNum) * ne); } } diff --git a/plugin/elasticity/solid.h b/plugin/elasticity/solid.h index acbd99a8..7b282b42 100644 --- a/plugin/elasticity/solid.h +++ b/plugin/elasticity/solid.h @@ -51,9 +51,7 @@ class Solid { // precomputed quantities std::vector metric; // geom-induced metric (nt x 36) - std::vector reference; // reference lengths (ne x 1) - std::vector deformed; // deformed lengths (ne x 1) - std::vector previous; // previous-step lengths (ne x 1) + std::vector prev; // previous-step lengths (ne x 1) std::vector elongation; // edge elongation (ne x 1) std::vector force; // force at all vertices (nv x 3)