Add 2D membrane elasticity for interpolated flex shell mode
When elastic2d="stretch" is set on an interpolated flexcomp, treat the bounding box boundary as membrane elements rather than volumetric cells. This computes plane-stress stiffness over the boundary faces and updates the runtime force/derivative kernels accordingly. Interior vertex tracking (moving vertices that follow the deforming shell) is not yet implemented so all mesh vertices need to be on the bounding box surface or the background grid should have no interior nodes (i.e. cellcount should be 1 on at least one axis). PiperOrigin-RevId: 907654080 Change-Id: I51b90e2f6a1d1b036f9604e42de20e377dc5d3f9
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
517c113656
commit
9c6a4f76eb
@@ -706,11 +706,12 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
|
||||
break;
|
||||
|
||||
case mjEQ_FLEXSTRAIN: {
|
||||
// each constraint represents a single cell; cell index in eq_data
|
||||
// each constraint represents a single element (3D cell or 2D face)
|
||||
int f = id[0];
|
||||
int nodenum = m->flex_nodenum[f];
|
||||
int order = m->flex_interp[f];
|
||||
order = order < 0 ? -order : order;
|
||||
int interp = m->flex_interp[f];
|
||||
int order = interp < 0 ? -interp : interp;
|
||||
int shell_mode = (interp < 0);
|
||||
|
||||
// skip if not interpolated (order == 0 or no nodes)
|
||||
if (!order || !nodenum) {
|
||||
@@ -722,60 +723,93 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
|
||||
mjERROR("flex strain constraints only support order 1 and 2, got %d", order);
|
||||
}
|
||||
|
||||
int npc = (order+1)*(order+1)*(order+1);
|
||||
int cx = m->flex_cellnum[3*f+0];
|
||||
int cy = m->flex_cellnum[3*f+1];
|
||||
int cz = m->flex_cellnum[3*f+2];
|
||||
int nstart = m->flex_nodeadr[f];
|
||||
int* bodyid = m->flex_nodebodyid + nstart;
|
||||
|
||||
// read cell index from eq_data
|
||||
int ci = (int)data[0];
|
||||
int cj = (int)data[1];
|
||||
int ck = (int)data[2];
|
||||
// nodes per element and element index
|
||||
int npe;
|
||||
int elem_idx;
|
||||
if (shell_mode) {
|
||||
npe = (order+1) * (order+1);
|
||||
elem_idx = (int)data[0]; // face element index
|
||||
} else {
|
||||
npe = (order+1) * (order+1) * (order+1);
|
||||
int ci = (int)data[0];
|
||||
int cj = (int)data[1];
|
||||
int ck = (int)data[2];
|
||||
elem_idx = ci * cy * cz + cj * cz + ck;
|
||||
}
|
||||
|
||||
mj_markStack(d);
|
||||
|
||||
// get cell node indices
|
||||
// get element node indices
|
||||
int gindices[125]; // max npc = 125 for quadratic
|
||||
mju_flexGatherCellState(order, cy, cz, ci, cj, ck,
|
||||
NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
|
||||
if (shell_mode) {
|
||||
mju_flexGatherFaceState(order, cx, cy, cz, elem_idx,
|
||||
NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
|
||||
} else {
|
||||
int ci = (int)data[0], cj = (int)data[1], ck = (int)data[2];
|
||||
mju_flexGatherCellState(order, cy, cz, ci, cj, ck,
|
||||
NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
|
||||
}
|
||||
|
||||
// compute positions only for cell nodes (npc << nodenum)
|
||||
mjtNum* xpos_c = mjSTACKALLOC(d, 3*npc, mjtNum);
|
||||
mjtNum* refpos_c = mjSTACKALLOC(d, 3*npc, mjtNum);
|
||||
for (int n = 0; n < npc; n++) {
|
||||
// compute positions only for element nodes (npe << nodenum)
|
||||
mjtNum* xpos_e = mjSTACKALLOC(d, 3*npe, mjtNum);
|
||||
mjtNum* refpos_e = mjSTACKALLOC(d, 3*npe, mjtNum);
|
||||
for (int n = 0; n < npe; n++) {
|
||||
int gn = gindices[n];
|
||||
if (m->flex_centered[f] ||
|
||||
(m->flex_node[3*(gn + nstart)+0] == 0 &&
|
||||
m->flex_node[3*(gn + nstart)+1] == 0 &&
|
||||
m->flex_node[3*(gn + nstart)+2] == 0)) {
|
||||
mju_copy3(xpos_c + 3*n, d->xpos + 3*bodyid[gn]);
|
||||
mju_copy3(xpos_e + 3*n, d->xpos + 3*bodyid[gn]);
|
||||
} else {
|
||||
mju_mulMatVec3(xpos_c + 3*n, d->xmat + 9*bodyid[gn], m->flex_node + 3*(gn + nstart));
|
||||
mju_addTo3(xpos_c + 3*n, d->xpos + 3*bodyid[gn]);
|
||||
mju_mulMatVec3(xpos_e + 3*n, d->xmat + 9*bodyid[gn], m->flex_node + 3*(gn + nstart));
|
||||
mju_addTo3(xpos_e + 3*n, d->xpos + 3*bodyid[gn]);
|
||||
}
|
||||
mju_copy3(refpos_c + 3*n, m->flex_node0 + 3*(gn + nstart));
|
||||
mju_copy3(refpos_e + 3*n, m->flex_node0 + 3*(gn + nstart));
|
||||
}
|
||||
|
||||
// compute corotational quaternion from cell-local positions
|
||||
mjtNum cell_quat[4] = {1, 0, 0, 0};
|
||||
{
|
||||
// compute corotational quaternion
|
||||
mjtNum elem_quat[4] = {1, 0, 0, 0};
|
||||
if (shell_mode) {
|
||||
// determine face normal axis from elem_idx
|
||||
int face_sizes[6] = {cy*cz, cy*cz, cx*cz, cx*cz, cx*cy, cx*cy};
|
||||
int face_normals[6] = {0, 0, 1, 1, 2, 2};
|
||||
int cumul = 0, normal_axis = 0;
|
||||
for (int ff = 0; ff < 6; ff++) {
|
||||
if (elem_idx < cumul + face_sizes[ff]) {
|
||||
normal_axis = face_normals[ff];
|
||||
break;
|
||||
}
|
||||
cumul += face_sizes[ff];
|
||||
}
|
||||
int na0 = (normal_axis + 1) % 3;
|
||||
int na1 = (normal_axis + 2) % 3;
|
||||
|
||||
// compute corotational rotation from 2D deformation gradient at face center
|
||||
mjtNum p[2] = {.5, .5};
|
||||
mju_flexInterpRotation2D(order, xpos_e, npe, na0, na1, normal_axis, p, elem_quat);
|
||||
} else {
|
||||
mjtNum center[3] = {0.5, 0.5, 0.5};
|
||||
mjtNum mat[9];
|
||||
mju_defGradient(mat, center, xpos_c, order);
|
||||
mju_mat2Rot(cell_quat, mat);
|
||||
mju_negQuat(cell_quat, cell_quat);
|
||||
mju_defGradient(mat, center, xpos_e, order);
|
||||
mju_mat2Rot(elem_quat, mat);
|
||||
mju_negQuat(elem_quat, elem_quat);
|
||||
}
|
||||
|
||||
// build per-cell sparse chain and node Jacobians
|
||||
int* cell_chain = mjSTACKALLOC(d, nv, int);
|
||||
int cell_nnz = 0;
|
||||
mjtNum* cell_node_jac = cell_pos_and_jac(m, d, f, npc, gindices, nv, xpos_c, cell_chain,
|
||||
&cell_nnz);
|
||||
// build per-element sparse chain and node Jacobians
|
||||
int* elem_chain = mjSTACKALLOC(d, nv, int);
|
||||
int elem_nnz = 0;
|
||||
mjtNum* elem_node_jac = cell_pos_and_jac(m, d, f, npe, gindices, nv, xpos_e, elem_chain,
|
||||
&elem_nnz);
|
||||
|
||||
|
||||
mjtNum* strain_jac = mjSTACKALLOC(d, cell_nnz, mjtNum);
|
||||
mjtNum* dSdx_local = mjSTACKALLOC(d, 3*npc, mjtNum);
|
||||
mjtNum* strain_jac = mjSTACKALLOC(d, elem_nnz, mjtNum);
|
||||
mjtNum* dSdx_local = mjSTACKALLOC(d, 3*npe, mjtNum);
|
||||
|
||||
// for dense mode: allocate and zero a dense Jacobian buffer once
|
||||
mjtNum* dense_jac = NULL;
|
||||
@@ -785,58 +819,55 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
|
||||
}
|
||||
|
||||
// read eigenmode data from flex_stiffness
|
||||
int ndof_cell = 3 * npc;
|
||||
int cell_idx = ci * m->flex_cellnum[3*f+1] * m->flex_cellnum[3*f+2]
|
||||
+ cj * m->flex_cellnum[3*f+2] + ck;
|
||||
const mjtNum* k_cell = m->flex_stiffness + m->flex_stiffnessadr[f]
|
||||
+ cell_idx * ndof_cell * ndof_cell;
|
||||
int neig = (int)k_cell[0];
|
||||
int ndof_elem = 3 * npe;
|
||||
const mjtNum* k_elem = m->flex_stiffness + m->flex_stiffnessadr[f]
|
||||
+ elem_idx * ndof_elem * ndof_elem;
|
||||
int neig = (int)k_elem[0];
|
||||
|
||||
// compute displacement in corotational frame
|
||||
mjtNum* displ_c = mjSTACKALLOC(d, ndof_cell, mjtNum);
|
||||
for (int n = 0; n < npc; n++) {
|
||||
// rotate xpos_c to corotational frame
|
||||
mjtNum* displ_e = mjSTACKALLOC(d, ndof_elem, mjtNum);
|
||||
for (int n = 0; n < npe; n++) {
|
||||
// rotate xpos_e to corotational frame
|
||||
mjtNum xrot[3];
|
||||
mju_rotVecQuat(xrot, xpos_c + 3*n, cell_quat);
|
||||
displ_c[3*n + 0] = xrot[0] - refpos_c[3*n + 0];
|
||||
displ_c[3*n + 1] = xrot[1] - refpos_c[3*n + 1];
|
||||
displ_c[3*n + 2] = xrot[2] - refpos_c[3*n + 2];
|
||||
mju_rotVecQuat(xrot, xpos_e + 3*n, elem_quat);
|
||||
displ_e[3*n + 0] = xrot[0] - refpos_e[3*n + 0];
|
||||
displ_e[3*n + 1] = xrot[1] - refpos_e[3*n + 1];
|
||||
displ_e[3*n + 2] = xrot[2] - refpos_e[3*n + 2];
|
||||
}
|
||||
|
||||
// compute inverse quaternion for rotating eigenvectors to world frame
|
||||
mjtNum cell_quat_inv[4];
|
||||
mju_negQuat(cell_quat_inv, cell_quat);
|
||||
mjtNum elem_quat_inv[4];
|
||||
mju_negQuat(elem_quat_inv, elem_quat);
|
||||
|
||||
// loop over eigenmodes
|
||||
for (int eig = 0; eig < neig; eig++) {
|
||||
const mjtNum* eigvec = k_cell + 1 + eig * ndof_cell;
|
||||
const mjtNum* eigvec = k_elem + 1 + eig * ndof_elem;
|
||||
|
||||
// constraint residual: dot product of scaled eigenvector with displacement
|
||||
mjtNum residual = 0;
|
||||
for (int j = 0; j < ndof_cell; j++) {
|
||||
residual += eigvec[j] * displ_c[j];
|
||||
for (int j = 0; j < ndof_elem; j++) {
|
||||
residual += eigvec[j] * displ_e[j];
|
||||
}
|
||||
cpos[0] = residual;
|
||||
|
||||
// rotate eigenvector to world frame for Jacobian
|
||||
// dSdx_local[3*n+c] = Σ_d R_inv[c][d] * eigvec[3*n+d]
|
||||
for (int n = 0; n < npc; n++) {
|
||||
mju_rotVecQuat(dSdx_local + 3*n, eigvec + 3*n, cell_quat_inv);
|
||||
for (int n = 0; n < npe; n++) {
|
||||
mju_rotVecQuat(dSdx_local + 3*n, eigvec + 3*n, elem_quat_inv);
|
||||
}
|
||||
|
||||
// contract with cell_node_jac to get sparse Jacobian
|
||||
cell_strain_jacobian(npc, cell_nnz, dSdx_local, cell_node_jac, strain_jac);
|
||||
// contract with elem_node_jac to get sparse Jacobian
|
||||
cell_strain_jacobian(npe, elem_nnz, dSdx_local, elem_node_jac, strain_jac);
|
||||
|
||||
if (issparse) {
|
||||
mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i,
|
||||
cell_nnz, cell_chain);
|
||||
elem_nnz, elem_chain);
|
||||
} else {
|
||||
for (int k = 0; k < cell_nnz; k++) {
|
||||
dense_jac[cell_chain[k]] = strain_jac[k];
|
||||
for (int k = 0; k < elem_nnz; k++) {
|
||||
dense_jac[elem_chain[k]] = strain_jac[k];
|
||||
}
|
||||
mj_addConstraint(m, d, dense_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL);
|
||||
for (int k = 0; k < cell_nnz; k++) {
|
||||
dense_jac[cell_chain[k]] = 0;
|
||||
for (int k = 0; k < elem_nnz; k++) {
|
||||
dense_jac[elem_chain[k]] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1674,36 +1705,56 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
|
||||
break;
|
||||
|
||||
case mjEQ_FLEXSTRAIN: {
|
||||
// strain constraints: per-cell, use avg inv weight of cell's npc nodes
|
||||
// strain constraints: use avg inv weight of element's nodes
|
||||
int flex_id = m->eq_obj1id[id];
|
||||
int nstart = m->flex_nodeadr[flex_id];
|
||||
int order = m->flex_interp[flex_id];
|
||||
order = order < 0 ? -order : order;
|
||||
int npc = (order+1)*(order+1)*(order+1);
|
||||
int interp = m->flex_interp[flex_id];
|
||||
int order = interp < 0 ? -interp : interp;
|
||||
int is_shell = (interp < 0);
|
||||
|
||||
// per-cell constraint count
|
||||
int nquad = order + 1;
|
||||
int ngauss = nquad * nquad * nquad;
|
||||
int nconstraint = (order == 1) ? (2 + 3 * ngauss) : (6 * ngauss);
|
||||
|
||||
// get cell index from eq_data
|
||||
int eq_id = d->efc_id[i];
|
||||
int ci_cell = (int)m->eq_data[mjNEQDATA*eq_id + 0];
|
||||
int cj_cell = (int)m->eq_data[mjNEQDATA*eq_id + 1];
|
||||
int ck_cell = (int)m->eq_data[mjNEQDATA*eq_id + 2];
|
||||
int cx = m->flex_cellnum[3*flex_id+0];
|
||||
int cy = m->flex_cellnum[3*flex_id+1];
|
||||
int cz = m->flex_cellnum[3*flex_id+2];
|
||||
|
||||
// nodes per element
|
||||
int npe;
|
||||
int elem_idx;
|
||||
if (is_shell) {
|
||||
npe = (order+1) * (order+1);
|
||||
elem_idx = (int)m->eq_data[mjNEQDATA*id + 0];
|
||||
} else {
|
||||
npe = (order+1) * (order+1) * (order+1);
|
||||
int ci_cell = (int)m->eq_data[mjNEQDATA*id + 0];
|
||||
int cj_cell = (int)m->eq_data[mjNEQDATA*id + 1];
|
||||
int ck_cell = (int)m->eq_data[mjNEQDATA*id + 2];
|
||||
elem_idx = ci_cell * cy * cz + cj_cell * cz + ck_cell;
|
||||
}
|
||||
|
||||
// read neig from flex_stiffness
|
||||
int ndof_elem = 3 * npe;
|
||||
const mjtNum* k_elem = m->flex_stiffness + m->flex_stiffnessadr[flex_id]
|
||||
+ elem_idx * ndof_elem * ndof_elem;
|
||||
int nconstraint = (int)k_elem[0];
|
||||
|
||||
// get element node indices
|
||||
int gindices[125];
|
||||
mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell,
|
||||
NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
|
||||
if (is_shell) {
|
||||
mju_flexGatherFaceState(order, cx, cy, cz, elem_idx,
|
||||
NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
|
||||
} else {
|
||||
int ci_cell = (int)m->eq_data[mjNEQDATA*id + 0];
|
||||
int cj_cell = (int)m->eq_data[mjNEQDATA*id + 1];
|
||||
int ck_cell = (int)m->eq_data[mjNEQDATA*id + 2];
|
||||
mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell,
|
||||
NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
|
||||
}
|
||||
|
||||
mjtNum avg_invweight = 0;
|
||||
for (int n = 0; n < npc; n++) {
|
||||
for (int n = 0; n < npe; n++) {
|
||||
int bodyid = m->flex_nodebodyid[nstart + gindices[n]];
|
||||
avg_invweight += m->body_invweight0[2*bodyid];
|
||||
}
|
||||
avg_invweight /= npc;
|
||||
avg_invweight /= npe;
|
||||
for (int c = 0; c < nconstraint; c++) {
|
||||
dA[i++] = avg_invweight;
|
||||
}
|
||||
@@ -2296,37 +2347,56 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
|
||||
break;
|
||||
|
||||
case mjEQ_FLEXSTRAIN: {
|
||||
// per-cell strain constraints: each equality is one cell
|
||||
// per-element strain constraints: each equality is one cell or face
|
||||
int f = id[0];
|
||||
int order = m->flex_interp[f];
|
||||
order = order < 0 ? -order : order;
|
||||
int interp = m->flex_interp[f];
|
||||
int order = interp < 0 ? -interp : interp;
|
||||
int is_shell = (interp < 0);
|
||||
if (!order || !m->flex_nodenum[f]) {
|
||||
break;
|
||||
}
|
||||
int npc = (order+1)*(order+1)*(order+1);
|
||||
|
||||
// read eigenmode count from flex_stiffness
|
||||
int ndof_cell = 3 * npc;
|
||||
int ci_cell = (int)m->eq_data[mjNEQDATA*i + 0];
|
||||
int cj_cell = (int)m->eq_data[mjNEQDATA*i + 1];
|
||||
int ck_cell = (int)m->eq_data[mjNEQDATA*i + 2];
|
||||
int cx = m->flex_cellnum[3*f+0];
|
||||
int cy = m->flex_cellnum[3*f+1];
|
||||
int cz = m->flex_cellnum[3*f+2];
|
||||
int cell_idx = ci_cell * cy * cz + cj_cell * cz + ck_cell;
|
||||
const mjtNum* k_cell = m->flex_stiffness + m->flex_stiffnessadr[f]
|
||||
+ cell_idx * ndof_cell * ndof_cell;
|
||||
size = (int)k_cell[0]; // neig stored as first element
|
||||
|
||||
int npe;
|
||||
int elem_idx;
|
||||
if (is_shell) {
|
||||
npe = (order+1) * (order+1);
|
||||
elem_idx = (int)m->eq_data[mjNEQDATA*i + 0];
|
||||
} else {
|
||||
npe = (order+1) * (order+1) * (order+1);
|
||||
int ci_cell = (int)m->eq_data[mjNEQDATA*i + 0];
|
||||
int cj_cell = (int)m->eq_data[mjNEQDATA*i + 1];
|
||||
int ck_cell = (int)m->eq_data[mjNEQDATA*i + 2];
|
||||
elem_idx = ci_cell * cy * cz + cj_cell * cz + ck_cell;
|
||||
}
|
||||
|
||||
// read eigenmode count from flex_stiffness
|
||||
int ndof_elem = 3 * npe;
|
||||
const mjtNum* k_elem = m->flex_stiffness + m->flex_stiffnessadr[f]
|
||||
+ elem_idx * ndof_elem * ndof_elem;
|
||||
size = (int)k_elem[0]; // neig stored as first element
|
||||
|
||||
if (nnz) {
|
||||
// get the npc node body IDs for this cell
|
||||
// get element node body IDs
|
||||
int gindices[125];
|
||||
mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell,
|
||||
NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
|
||||
if (is_shell) {
|
||||
mju_flexGatherFaceState(order, cx, cy, cz, elem_idx,
|
||||
NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
|
||||
} else {
|
||||
int ci_cell = (int)m->eq_data[mjNEQDATA*i + 0];
|
||||
int cj_cell = (int)m->eq_data[mjNEQDATA*i + 1];
|
||||
int ck_cell = (int)m->eq_data[mjNEQDATA*i + 2];
|
||||
mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell,
|
||||
NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL);
|
||||
}
|
||||
int nstart = m->flex_nodeadr[f];
|
||||
for (int n = 0; n < npc; n++) {
|
||||
for (int n = 0; n < npe; n++) {
|
||||
cell_bodies[n] = m->flex_nodebodyid[nstart + gindices[n]];
|
||||
}
|
||||
NV = mj_jacSumCount(m, d, chain, npc, cell_bodies); // npc nodes only
|
||||
NV = mj_jacSumCount(m, d, chain, npe, cell_bodies);
|
||||
NV = size * NV;
|
||||
}
|
||||
break;
|
||||
|
||||
+119
-103
@@ -882,23 +882,29 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
|
||||
|
||||
// compute upper bounds across all interpolated flexes
|
||||
int max_nodenum = 0;
|
||||
int max_npc = 0;
|
||||
int max_npe = 0; // max nodes per element (3D cell or 2D face)
|
||||
for (int f = 0; f < m->nflex; f++) {
|
||||
if (!m->flex_interp[f]) continue;
|
||||
if (m->flex_rigid[f]) continue;
|
||||
int order = m->flex_interp[f];
|
||||
int shell_mode = order < 0;
|
||||
order = order < 0 ? -order : order;
|
||||
int npc = (order+1)*(order+1)*(order+1);
|
||||
if (npc > max_npc) max_npc = npc;
|
||||
int npe;
|
||||
if (shell_mode) {
|
||||
npe = (order+1)*(order+1);
|
||||
} else {
|
||||
npe = (order+1)*(order+1)*(order+1);
|
||||
}
|
||||
if (npe > max_npe) max_npe = npe;
|
||||
if (m->flex_nodenum[f] > max_nodenum) max_nodenum = m->flex_nodenum[f];
|
||||
}
|
||||
|
||||
// nothing to do
|
||||
if (max_npc == 0) {
|
||||
if (max_npe == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
int max_dim_c = 3 * max_npc;
|
||||
int max_dim_c = 3 * max_npe;
|
||||
|
||||
// single unconditional markStack
|
||||
mj_markStack(d);
|
||||
@@ -915,8 +921,8 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
|
||||
// per-flex node positions (upper bound)
|
||||
mjtNum* xpos = mjSTACKALLOC(d, 3*max_nodenum, mjtNum);
|
||||
|
||||
// per-cell arrays (upper bound)
|
||||
mjtNum* xpos_c = mjSTACKALLOC(d, 3*max_npc, mjtNum);
|
||||
// per-element arrays (upper bound)
|
||||
mjtNum* xpos_c = mjSTACKALLOC(d, 3*max_npe, mjtNum);
|
||||
mjtNum* K_rot_cell = mjSTACKALLOC(d, max_dim_c*max_dim_c, mjtNum);
|
||||
|
||||
// sparse Jacobian for one cell (upper bound)
|
||||
@@ -967,131 +973,141 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op,
|
||||
}
|
||||
|
||||
int order = m->flex_interp[f];
|
||||
int shell_mode = order < 0;
|
||||
order = order < 0 ? -order : order;
|
||||
int npc = (order+1)*(order+1)*(order+1);
|
||||
int cx = m->flex_cellnum[3*f+0];
|
||||
int cy = m->flex_cellnum[3*f+1];
|
||||
int cz = m->flex_cellnum[3*f+2];
|
||||
|
||||
int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
|
||||
|
||||
int dim_c = 3 * npc;
|
||||
// determine element type: 2D boundary quads (shell) or 3D cells (volume)
|
||||
int npe;
|
||||
int nelem_fe;
|
||||
if (shell_mode) {
|
||||
npe = (order+1)*(order+1);
|
||||
nelem_fe = 2*(cy*cz + cx*cz + cx*cy);
|
||||
} else {
|
||||
npe = (order+1)*(order+1)*(order+1);
|
||||
nelem_fe = cx * cy * cz;
|
||||
}
|
||||
int dim_e = 3 * npe;
|
||||
|
||||
// gather raw node positions (unrotated)
|
||||
mju_flexGatherState(m, d, f, xpos, NULL);
|
||||
|
||||
// loop over cells
|
||||
int cell_idx = 0;
|
||||
for (int ci = 0; ci < cx; ci++) {
|
||||
for (int cj = 0; cj < cy; cj++) {
|
||||
for (int ck = 0; ck < cz; ck++) {
|
||||
// get cell stiffness
|
||||
mjtNum* k_cell = K + cell_idx * 3*npc * 3*npc;
|
||||
// loop over finite elements
|
||||
for (int fe = 0; fe < nelem_fe; fe++) {
|
||||
// get element stiffness
|
||||
mjtNum* k_elem = K + fe * 3*npe * 3*npe;
|
||||
|
||||
// skip empty cells: stiffness buffer is zero-initialized at compile time
|
||||
// (user_model.cc), and non-empty cells have strictly positive diagonal
|
||||
if (k_cell[0] == 0) {
|
||||
cell_idx++;
|
||||
continue;
|
||||
}
|
||||
// skip empty elements: stiffness buffer is zero-initialized at compile time
|
||||
// (user_model.cc), and non-empty elements have strictly positive diagonal
|
||||
if (k_elem[0] == 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// gather cell-local node positions
|
||||
int gindices[125]; // max npc = 125 for quadratic
|
||||
mjtNum quat[4];
|
||||
mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos, NULL, NULL,
|
||||
xpos_c, NULL, NULL, gindices, quat);
|
||||
// gather element-local node positions
|
||||
int gindices[125]; // max npe = 125 for quadratic 3D
|
||||
mjtNum quat[4];
|
||||
if (shell_mode) {
|
||||
mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos, NULL, NULL,
|
||||
xpos_c, NULL, NULL, gindices, quat);
|
||||
} else {
|
||||
int ci = fe / (cy * cz);
|
||||
int cj = (fe / cz) % cy;
|
||||
int ck = fe % cz;
|
||||
mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos, NULL, NULL,
|
||||
xpos_c, NULL, NULL, gindices, quat);
|
||||
}
|
||||
|
||||
// R = R_global2local, RT = R_local2global
|
||||
mjtNum R[9], RT[9];
|
||||
mju_quat2Mat(R, quat);
|
||||
mju_transpose(RT, R, 3, 3);
|
||||
// R = R_global2local, RT = R_local2global
|
||||
mjtNum R[9], RT[9];
|
||||
mju_quat2Mat(R, quat);
|
||||
mju_transpose(RT, R, 3, 3);
|
||||
|
||||
// compute K_rot_cell = RT * K_cell * R (block-wise)
|
||||
mju_zero(K_rot_cell, dim_c*dim_c);
|
||||
for (int a = 0; a < npc; a++) {
|
||||
for (int b = 0; b < npc; b++) {
|
||||
mjtNum blk[9], tmp[9];
|
||||
// compute K_rot = RT * K_elem * R (block-wise)
|
||||
mju_zero(K_rot_cell, dim_e*dim_e);
|
||||
for (int a = 0; a < npe; a++) {
|
||||
for (int b = 0; b < npe; b++) {
|
||||
mjtNum blk[9], tmp[9];
|
||||
|
||||
// get K_cell(a,b) 3x3 block
|
||||
int adr_cell = (3*a)*(3*npc) + 3*b;
|
||||
for (int r = 0; r < 3; r++) {
|
||||
for (int c = 0; c < 3; c++) {
|
||||
blk[3*r+c] = k_cell[adr_cell + r*(3*npc) + c];
|
||||
}
|
||||
}
|
||||
|
||||
// tmp = K * R
|
||||
mju_mulMatMat3(tmp, blk, R);
|
||||
// blk = RT * tmp = RT * K * R
|
||||
mju_mulMatMat3(blk, RT, tmp);
|
||||
|
||||
// store in K_rot_cell at (a, b)
|
||||
int adr_out = (3*a)*dim_c + 3*b;
|
||||
for (int r = 0; r < 3; r++) {
|
||||
for (int c = 0; c < 3; c++) {
|
||||
K_rot_cell[adr_out + r*dim_c + c] = scale * blk[3*r+c];
|
||||
}
|
||||
}
|
||||
// get K_elem(a,b) 3x3 block
|
||||
int adr_cell = (3*a)*(3*npe) + 3*b;
|
||||
for (int r = 0; r < 3; r++) {
|
||||
for (int c = 0; c < 3; c++) {
|
||||
blk[3*r+c] = k_elem[adr_cell + r*(3*npe) + c];
|
||||
}
|
||||
}
|
||||
|
||||
// construct sparse Jacobian for this cell's nodes
|
||||
int current_adr = 0;
|
||||
for (int n = 0; n < npc; n++) {
|
||||
int bid = bodyid[gindices[n]];
|
||||
int chain_nnz = mj_bodyChain(m, bid, chain_colind);
|
||||
mj_jacSparse(m, d, blk_jac, NULL, xpos+3*gindices[n], bid,
|
||||
chain_nnz, chain_colind, /*flg_skipcommon=*/0);
|
||||
// tmp = K * R
|
||||
mju_mulMatMat3(tmp, blk, R);
|
||||
// blk = RT * tmp = RT * K * R
|
||||
mju_mulMatMat3(blk, RT, tmp);
|
||||
|
||||
for (int r = 0; r < 3; r++) {
|
||||
int row_idx = 3*n + r;
|
||||
J_rownnz[row_idx] = chain_nnz;
|
||||
J_rowadr[row_idx] = current_adr;
|
||||
|
||||
for (int idx = 0; idx < chain_nnz; idx++) {
|
||||
J_colind[current_adr] = chain_colind[idx];
|
||||
J_val[current_adr] = blk_jac[r*chain_nnz + idx];
|
||||
current_adr++;
|
||||
}
|
||||
// store in K_rot_cell at (a, b)
|
||||
int adr_out = (3*a)*dim_e + 3*b;
|
||||
for (int r = 0; r < 3; r++) {
|
||||
for (int c = 0; c < 3; c++) {
|
||||
K_rot_cell[adr_out + r*dim_e + c] = scale * blk[3*r+c];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// apply operation with cell's K_rot and J
|
||||
if (op == mjFLEXOP_VEC) {
|
||||
addJTBJ_mulSparse(m, d, res, vec, J_rownnz, J_rowadr, J_colind,
|
||||
J_val, K_rot_cell, dim_c);
|
||||
} else if (op == mjFLEXOP_ADDH) {
|
||||
// H -= J_cell^T * K_rot_cell * J_cell (banded format)
|
||||
mju_zero(J_reduced, dim_c*ndof);
|
||||
// construct sparse Jacobian for this element's nodes
|
||||
int current_adr = 0;
|
||||
for (int n = 0; n < npe; n++) {
|
||||
int bid = bodyid[gindices[n]];
|
||||
int chain_nnz = mj_bodyChain(m, bid, chain_colind);
|
||||
mj_jacSparse(m, d, blk_jac, NULL, xpos+3*gindices[n], bid,
|
||||
chain_nnz, chain_colind, /*flg_skipcommon=*/0);
|
||||
|
||||
for (int i = 0; i < dim_c; i++) {
|
||||
int nnz = J_rownnz[i];
|
||||
int adr = J_rowadr[i];
|
||||
for (int idx = 0; idx < nnz; idx++) {
|
||||
int global_col = J_colind[adr + idx];
|
||||
int local_idx = global2local[global_col];
|
||||
if (local_idx >= 0) {
|
||||
J_reduced[i*ndof + local_idx] = J_val[adr + idx];
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int r = 0; r < 3; r++) {
|
||||
int row_idx = 3*n + r;
|
||||
J_rownnz[row_idx] = chain_nnz;
|
||||
J_rowadr[row_idx] = current_adr;
|
||||
|
||||
// KJ = K_rot_cell * J_reduced (dim_c x ndof)
|
||||
mju_mulMatMat(KJ, K_rot_cell, J_reduced, dim_c, dim_c, ndof);
|
||||
for (int idx = 0; idx < chain_nnz; idx++) {
|
||||
J_colind[current_adr] = chain_colind[idx];
|
||||
J_val[current_adr] = blk_jac[r*chain_nnz + idx];
|
||||
current_adr++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// H[i,j] -= J_reduced[k,i] * KJ[k,j], store lower triangle in banded format
|
||||
for (int i = 0; i < ndof; i++) {
|
||||
for (int j = mjMAX(0, i-nband+1); j <= i; j++) {
|
||||
mjtNum val = 0;
|
||||
for (int dim_idx = 0; dim_idx < dim_c; dim_idx++) {
|
||||
val += J_reduced[dim_idx*ndof + i] * KJ[dim_idx*ndof + j];
|
||||
}
|
||||
res[i*nband + nband-1-(i-j)] -= val;
|
||||
}
|
||||
// apply operation with element's K_rot and J
|
||||
if (op == mjFLEXOP_VEC) {
|
||||
addJTBJ_mulSparse(m, d, res, vec, J_rownnz, J_rowadr, J_colind,
|
||||
J_val, K_rot_cell, dim_e);
|
||||
} else if (op == mjFLEXOP_ADDH) {
|
||||
// H -= J_elem^T * K_rot * J_elem (banded format)
|
||||
mju_zero(J_reduced, dim_e*ndof);
|
||||
|
||||
for (int i = 0; i < dim_e; i++) {
|
||||
int nnz = J_rownnz[i];
|
||||
int adr = J_rowadr[i];
|
||||
for (int idx = 0; idx < nnz; idx++) {
|
||||
int global_col = J_colind[adr + idx];
|
||||
int local_idx = global2local[global_col];
|
||||
if (local_idx >= 0) {
|
||||
J_reduced[i*ndof + local_idx] = J_val[adr + idx];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
cell_idx++;
|
||||
// KJ = K_rot * J_reduced (dim_e x ndof)
|
||||
mju_mulMatMat(KJ, K_rot_cell, J_reduced, dim_e, dim_e, ndof);
|
||||
|
||||
// H[i,j] -= J_reduced[k,i] * KJ[k,j], store lower triangle in banded format
|
||||
for (int i = 0; i < ndof; i++) {
|
||||
for (int j = mjMAX(0, i-nband+1); j <= i; j++) {
|
||||
mjtNum val = 0;
|
||||
for (int dim_idx = 0; dim_idx < dim_e; dim_idx++) {
|
||||
val += J_reduced[dim_idx*ndof + i] * KJ[dim_idx*ndof + j];
|
||||
}
|
||||
res[i*nband + nband-1-(i-j)] -= val;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
+70
-67
@@ -236,13 +236,23 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
|
||||
|
||||
if (m->flex_interp[f]) {
|
||||
int order = m->flex_interp[f];
|
||||
int shell_mode = order < 0;
|
||||
order = order < 0 ? -order : order;
|
||||
int npc = (order+1)*(order+1)*(order+1); // nodes per cell
|
||||
int cx = m->flex_cellnum[3*f+0];
|
||||
int cy = m->flex_cellnum[3*f+1];
|
||||
int cz = m->flex_cellnum[3*f+2];
|
||||
int ny_g = cy * order + 1;
|
||||
int nz_g = cz * order + 1;
|
||||
|
||||
// determine element type: 2D boundary quads (shell) or 3D cells (volume)
|
||||
int npe; // nodes per element
|
||||
int nelem_fe; // total finite elements
|
||||
|
||||
if (shell_mode) {
|
||||
npe = (order+1)*(order+1);
|
||||
nelem_fe = 2*(cy*cz + cx*cz + cx*cy);
|
||||
} else {
|
||||
npe = (order+1)*(order+1)*(order+1);
|
||||
nelem_fe = cx * cy * cz;
|
||||
}
|
||||
|
||||
mj_markStack(d);
|
||||
|
||||
@@ -261,77 +271,70 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
|
||||
mju_zero(frc_g, 3*nodenum);
|
||||
mju_zero(dmp_g, 3*nodenum);
|
||||
|
||||
// per-cell arrays
|
||||
mjtNum* xpos_c = mjSTACKALLOC(d, 3*npc, mjtNum);
|
||||
mjtNum* vel_c = mjSTACKALLOC(d, 3*npc, mjtNum);
|
||||
mjtNum* xpos0_c = mjSTACKALLOC(d, 3*npc, mjtNum);
|
||||
mjtNum* displ_c = mjSTACKALLOC(d, 3*npc, mjtNum);
|
||||
mjtNum* frc_c = mjSTACKALLOC(d, 3*npc, mjtNum);
|
||||
mjtNum* dmp_c = mjSTACKALLOC(d, 3*npc, mjtNum);
|
||||
// per-element arrays (sized for npe)
|
||||
mjtNum* xpos_e = mjSTACKALLOC(d, 3*npe, mjtNum);
|
||||
mjtNum* vel_e = mjSTACKALLOC(d, 3*npe, mjtNum);
|
||||
mjtNum* xpos0_e = mjSTACKALLOC(d, 3*npe, mjtNum);
|
||||
mjtNum* displ_e = mjSTACKALLOC(d, 3*npe, mjtNum);
|
||||
mjtNum* frc_e = mjSTACKALLOC(d, 3*npe, mjtNum);
|
||||
mjtNum* dmp_e = mjSTACKALLOC(d, 3*npe, mjtNum);
|
||||
int* gindices = mjSTACKALLOC(d, npe, int);
|
||||
|
||||
// loop over cells
|
||||
int cell_idx = 0;
|
||||
for (int ci = 0; ci < cx; ci++) {
|
||||
for (int cj = 0; cj < cy; cj++) {
|
||||
for (int ck = 0; ck < cz; ck++) {
|
||||
// get cell stiffness matrix
|
||||
mjtNum* k_cell = k + cell_idx * 3*npc * 3*npc;
|
||||
// loop over finite elements
|
||||
for (int fe = 0; fe < nelem_fe; fe++) {
|
||||
// get element stiffness matrix
|
||||
mjtNum* k_elem = k + fe * 3*npe * 3*npe;
|
||||
|
||||
// skip empty cells (zero stiffness)
|
||||
if (k_cell[0] == 0) {
|
||||
cell_idx++;
|
||||
continue;
|
||||
}
|
||||
// skip empty elements (zero stiffness)
|
||||
if (k_elem[0] == 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// gather cell-local node data
|
||||
mjtNum quat[4];
|
||||
mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos_g, vel_g, xpos0,
|
||||
xpos_c, vel_c, xpos0_c, NULL, quat);
|
||||
// gather element-local node data and compute corotational rotation
|
||||
mjtNum quat[4];
|
||||
if (shell_mode) {
|
||||
mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos_g, vel_g, xpos0,
|
||||
xpos_e, vel_e, xpos0_e, gindices, quat);
|
||||
} else {
|
||||
int ci = fe / (cy * cz);
|
||||
int cj = (fe / cz) % cy;
|
||||
int ck = fe % cz;
|
||||
mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos_g, vel_g,
|
||||
xpos0, xpos_e, vel_e, xpos0_e, gindices,
|
||||
quat);
|
||||
}
|
||||
|
||||
// rotate to corotational frame
|
||||
for (int n = 0; n < npc; n++) {
|
||||
mju_rotVecQuat(xpos_c+3*n, xpos_c+3*n, quat);
|
||||
mju_rotVecQuat(vel_c+3*n, vel_c+3*n, quat);
|
||||
}
|
||||
// rotate to corotational frame
|
||||
for (int n = 0; n < npe; n++) {
|
||||
mju_rotVecQuat(xpos_e+3*n, xpos_e+3*n, quat);
|
||||
mju_rotVecQuat(vel_e+3*n, vel_e+3*n, quat);
|
||||
}
|
||||
|
||||
// compute displacement
|
||||
for (int n = 0; n < npc; n++) {
|
||||
mji_addScl3(displ_c+3*n, xpos_c+3*n, xpos0_c+3*n, -1);
|
||||
}
|
||||
// compute displacement
|
||||
for (int n = 0; n < npe; n++) {
|
||||
mji_addScl3(displ_e+3*n, xpos_e+3*n, xpos0_e+3*n, -1);
|
||||
}
|
||||
|
||||
// compute force in corotational frame
|
||||
if (enbl_spring) {
|
||||
mju_mulMatVec(frc_c, k_cell, displ_c, 3*npc, 3*npc);
|
||||
}
|
||||
if (enbl_damper) {
|
||||
mju_mulMatVec(dmp_c, k_cell, vel_c, 3*npc, 3*npc);
|
||||
}
|
||||
// compute force in corotational frame
|
||||
if (enbl_spring) {
|
||||
mju_mulMatVec(frc_e, k_elem, displ_e, 3*npe, 3*npe);
|
||||
}
|
||||
if (enbl_damper) {
|
||||
mju_mulMatVec(dmp_e, k_elem, vel_e, 3*npe, 3*npe);
|
||||
}
|
||||
|
||||
// rotate back to global frame and scatter
|
||||
mju_negQuat(quat, quat);
|
||||
int local = 0;
|
||||
for (int li = 0; li <= order; li++) {
|
||||
for (int lj = 0; lj <= order; lj++) {
|
||||
for (int lk = 0; lk <= order; lk++) {
|
||||
int gi = ci*order + li;
|
||||
int gj = cj*order + lj;
|
||||
int gk = ck*order + lk;
|
||||
int gidx = gi*ny_g*nz_g + gj*nz_g + gk;
|
||||
mjtNum qfrc[3], qdmp[3];
|
||||
mji_rotVecQuat(qfrc, frc_c+3*local, quat);
|
||||
mji_rotVecQuat(qdmp, dmp_c+3*local, quat);
|
||||
if (enbl_spring) {
|
||||
mji_addTo3(frc_g + 3*gidx, qfrc);
|
||||
}
|
||||
if (enbl_damper) {
|
||||
mji_addTo3(dmp_g + 3*gidx, qdmp);
|
||||
}
|
||||
local++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
cell_idx++;
|
||||
// rotate back to global frame and scatter using node indices
|
||||
mju_negQuat(quat, quat);
|
||||
for (int n = 0; n < npe; n++) {
|
||||
mjtNum qfrc[3], qdmp[3];
|
||||
mji_rotVecQuat(qfrc, frc_e+3*n, quat);
|
||||
mji_rotVecQuat(qdmp, dmp_e+3*n, quat);
|
||||
int gidx = gindices[n];
|
||||
if (enbl_spring) {
|
||||
mji_addTo3(frc_g + 3*gidx, qfrc);
|
||||
}
|
||||
if (enbl_damper) {
|
||||
mji_addTo3(dmp_g + 3*gidx, qdmp);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -726,6 +726,133 @@ void mju_flexGatherCellState(int order, int cy, int cz, int ci, int cj, int ck,
|
||||
}
|
||||
|
||||
|
||||
// compute corotational rotation from 2D deformation gradient on a flat face
|
||||
void mju_flexInterpRotation2D(int order, const mjtNum* xpos_f, int npe,
|
||||
int axis0, int axis1, int normal_axis,
|
||||
const mjtNum local[2], mjtNum* quat) {
|
||||
// compute 3x2 deformation gradient F at parametric point local
|
||||
mjtNum t1[3] = {0, 0, 0}; // tangent along axis0
|
||||
mjtNum t2[3] = {0, 0, 0}; // tangent along axis1
|
||||
int idx = 0;
|
||||
for (int l0 = 0; l0 <= order; l0++) {
|
||||
for (int l1 = 0; l1 <= order; l1++) {
|
||||
mjtNum grad0 = dphi(local[0], l0, order) * phi(local[1], l1, order);
|
||||
mjtNum grad1 = phi(local[0], l0, order) * dphi(local[1], l1, order);
|
||||
for (int d = 0; d < 3; d++) {
|
||||
t1[d] += xpos_f[3*idx + d] * grad0;
|
||||
t2[d] += xpos_f[3*idx + d] * grad1;
|
||||
}
|
||||
idx++;
|
||||
}
|
||||
}
|
||||
|
||||
// normal = t1 x t2
|
||||
mjtNum normal[3];
|
||||
mju_cross(normal, t1, t2);
|
||||
|
||||
// build 3x3 matrix with columns assigned to canonical axes (row-major)
|
||||
// axis0 → t1, axis1 → t2, normal_axis → normal
|
||||
// this ensures identity rotation for axis-aligned grids
|
||||
mjtNum mat[9] = {0};
|
||||
mjtNum* vecs[3];
|
||||
vecs[axis0] = t1;
|
||||
vecs[axis1] = t2;
|
||||
vecs[normal_axis] = normal;
|
||||
|
||||
for (int col = 0; col < 3; col++) {
|
||||
mat[0*3 + col] = vecs[col][0];
|
||||
mat[1*3 + col] = vecs[col][1];
|
||||
mat[2*3 + col] = vecs[col][2];
|
||||
}
|
||||
|
||||
// extract rotation via polar decomposition
|
||||
quat[0] = 1;
|
||||
quat[1] = 0;
|
||||
quat[2] = 0;
|
||||
quat[3] = 0;
|
||||
mju_mat2Rot(quat, mat);
|
||||
mju_negQuat(quat, quat);
|
||||
}
|
||||
|
||||
|
||||
// gather face-element-local quantities and optionally compute rotation (shell mode)
|
||||
//
|
||||
// face element enumeration for a grid with cell counts (cx, cy, cz):
|
||||
// face 0: x=0 cy*cz quads (normal=0)
|
||||
// face 1: x=max cy*cz quads (normal=0)
|
||||
// face 2: y=0 cx*cz quads (normal=1)
|
||||
// face 3: y=max cx*cz quads (normal=1)
|
||||
// face 4: z=0 cx*cy quads (normal=2)
|
||||
// face 5: z=max cx*cy quads (normal=2)
|
||||
void mju_flexGatherFaceState(int order, int cx, int cy, int cz,
|
||||
int face_elem_idx,
|
||||
const mjtNum* xpos_g, const mjtNum* vel_g,
|
||||
const mjtNum* xpos0_g,
|
||||
mjtNum* xpos_f, mjtNum* vel_f, mjtNum* xpos0_f,
|
||||
int* nodeindices, mjtNum* quat) {
|
||||
int ny_g = cy * order + 1;
|
||||
int nz_g = cz * order + 1;
|
||||
int npe = (order + 1) * (order + 1);
|
||||
|
||||
// face sizes and properties
|
||||
int face_sizes[6] = {cy*cz, cy*cz, cx*cz, cx*cz, cx*cy, cx*cy};
|
||||
int face_normal[6] = {0, 0, 1, 1, 2, 2};
|
||||
int face_count1[6] = {cz, cz, cx, cx, cy, cy};
|
||||
int face_fixed_vals[6];
|
||||
face_fixed_vals[0] = 0;
|
||||
face_fixed_vals[1] = cx * order;
|
||||
face_fixed_vals[2] = 0;
|
||||
face_fixed_vals[3] = cy * order;
|
||||
face_fixed_vals[4] = 0;
|
||||
face_fixed_vals[5] = cz * order;
|
||||
|
||||
// determine which face and quad within face
|
||||
int face_id = 0;
|
||||
int within_face = face_elem_idx;
|
||||
int cumul = 0;
|
||||
for (int f = 0; f < 6; f++) {
|
||||
if (face_elem_idx < cumul + face_sizes[f]) {
|
||||
face_id = f;
|
||||
within_face = face_elem_idx - cumul;
|
||||
break;
|
||||
}
|
||||
cumul += face_sizes[f];
|
||||
}
|
||||
|
||||
int normal_axis = face_normal[face_id];
|
||||
int na0 = (normal_axis + 1) % 3; // slow in-plane axis
|
||||
int na1 = (normal_axis + 2) % 3; // fast in-plane axis
|
||||
int c1 = face_count1[face_id];
|
||||
int g_fixed = face_fixed_vals[face_id];
|
||||
int q0 = within_face / c1;
|
||||
int q1 = within_face % c1;
|
||||
|
||||
// gather nodes
|
||||
int local = 0;
|
||||
for (int l0 = 0; l0 <= order; l0++) {
|
||||
for (int l1 = 0; l1 <= order; l1++) {
|
||||
int g[3];
|
||||
g[normal_axis] = g_fixed;
|
||||
g[na0] = q0 * order + l0;
|
||||
g[na1] = q1 * order + l1;
|
||||
int gidx = g[0] * ny_g * nz_g + g[1] * nz_g + g[2];
|
||||
|
||||
if (xpos_f && xpos_g) mju_copy3(xpos_f + 3*local, xpos_g + 3*gidx);
|
||||
if (vel_f && vel_g) mju_copy3(vel_f + 3*local, vel_g + 3*gidx);
|
||||
if (xpos0_f && xpos0_g) mju_copy3(xpos0_f + 3*local, xpos0_g + 3*gidx);
|
||||
if (nodeindices) nodeindices[local] = gidx;
|
||||
|
||||
local++;
|
||||
}
|
||||
}
|
||||
|
||||
if (quat && xpos_f) {
|
||||
mjtNum p[2] = {.5, .5};
|
||||
mju_flexInterpRotation2D(order, xpos_f, npe, na0, na1, normal_axis, p, quat);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//------------------------------ actuator models ---------------------------------------------------
|
||||
|
||||
// normalized muscle length-gain curve
|
||||
|
||||
@@ -103,6 +103,19 @@ MJAPI void mju_flexGatherCellState(int order, int cy, int cz, int ci, int cj, in
|
||||
const mjtNum* xpos0_g, mjtNum* xpos_c, mjtNum* vel_c,
|
||||
mjtNum* xpos0_c, int* nodeindices, mjtNum* quat);
|
||||
|
||||
// gather face-element-local quantities and optionally compute rotation (shell mode)
|
||||
MJAPI void mju_flexGatherFaceState(int order, int cx, int cy, int cz,
|
||||
int face_elem_idx,
|
||||
const mjtNum* xpos_g, const mjtNum* vel_g,
|
||||
const mjtNum* xpos0_g,
|
||||
mjtNum* xpos_f, mjtNum* vel_f, mjtNum* xpos0_f,
|
||||
int* nodeindices, mjtNum* quat);
|
||||
|
||||
// compute corotational rotation from 2D deformation gradient on a flat face
|
||||
MJAPI void mju_flexInterpRotation2D(int order, const mjtNum* xpos_f, int npe,
|
||||
int axis0, int axis1, int normal_axis,
|
||||
const mjtNum local[2], mjtNum* quat);
|
||||
|
||||
|
||||
// ----------------------------- Base64 ------------------------------------------------------------
|
||||
|
||||
|
||||
Reference in New Issue
Block a user