chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Align flex_vert0 to the XY plane during model initialization if dim=2.

Browse Source 
PiperOrigin-RevId: 860435368
Change-Id: Ida616e1e69c224f7c0dbad2a10dd59e64cbadf49
This commit is contained in:
Alessio Quaglino
2026-01-24 00:48:56 -08:00
committed by Copybara-Service
parent 769109630f
commit e9c61694e1
2 changed files with 56 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_smooth.c
+8 -24
View File
@@ -711,39 +711,23 @@ void mj_flex(const mjModel* m, mjData* d) {
// Chen, Kry, and Vouga, "Locking-free Simulation of Isometric Thin Plates", 2019.
if (m->flex_dim[f] == 2 && m->flex_edgeequality[f] == 2) {
int nvert = m->flex_vertnum[f];
mjtNum edge1[3], edge2[3], normal[3];
mjtNum quat[4], mat[9];
int t_adr, t0, t1, t2;
mj_markStack(d);
// compute normal from first element
t_adr = m->flex_elemdataadr[f];
t0 = m->flex_elem[t_adr];
t1 = m->flex_elem[t_adr+1];
t2 = m->flex_elem[t_adr+2];
mju_sub3(edge1, m->flex_vert0 + 3*(vbase+t1), m->flex_vert0 + 3*(vbase+t0));
mju_sub3(edge2, m->flex_vert0 + 3*(vbase+t2), m->flex_vert0 + 3*(vbase+t0));
mji_cross(normal, edge1, edge2);
mju_normalize3(normal);
// compute rotation to Z
mju_quatZ2Vec(quat, normal);
mju_quat2Mat(mat, quat);
// compute edge vectors
mjtNum* edge_dx = mjSTACKALLOC(d, 3*m->flex_edgenum[f], mjtNum);
mjtNum* edge_dy = mjSTACKALLOC(d, 3*m->flex_edgenum[f], mjtNum);
for (int e=0; e < m->flex_edgenum[f]; e++) {
int v1 = m->flex_edge[2*(ebase+e)];
int v2 = m->flex_edge[2*(ebase+e)+1];
mjtNum dx3[3];
mju_sub3(dx3, m->flex_vert0 + 3*(vbase+v2), m->flex_vert0 + 3*(vbase+v1));
dx3[0] *= 2*m->flex_size[3*f+0];
dx3[1] *= 2*m->flex_size[3*f+1];
dx3[2] *= 2*m->flex_size[3*f+2];
mji_mulMatTVec3(edge_dx+3*e, mat, dx3);
mju_sub3(edge_dx + 3 * e, m->flex_vert0 + 3 * (vbase + v2),
m->flex_vert0 + 3 * (vbase + v1));
// apply scaling since they are half sizes
(edge_dx + 3 * e)[0] *= 2 * m->flex_size[3 * f + 0];
(edge_dx + 3 * e)[1] *= 2 * m->flex_size[3 * f + 1];
(edge_dx + 3 * e)[2] *= 2 * m->flex_size[3 * f + 2];
if (mju_abs((edge_dx+3*e)[2]) > mjMINVAL) {
mjERROR("flex vertices are not in the same plane"); // SHOULD NOT OCCUR
}