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

Use previous edge length when setting the position of a cable body.

Browse Source 
Fixes #1440.

PiperOrigin-RevId: 610690343
Change-Id: I0d5288e68069d1117c0c674530e0644960bab06c
This commit is contained in:
Alessio Quaglino
2024-02-27 02:54:16 -08:00
committed by Copybara-Service
parent 1f2d673c7b
commit 478ce0c8d8
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_composite.cc
+3 -3
View File
@@ -790,7 +790,7 @@ mjmBody* mjCComposite::AddCableBody(mjCModel* model, mjmBody* body, int ix, mjtN
bool secondlast = ix==lastidx-1;
// compute edge and tangent vectors
mjtNum edge[3], tprev[3], tnext[3];
mjtNum 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]);
@@ -798,7 +798,7 @@ mjmBody* mjCComposite::AddCableBody(mjCModel* model, mjmBody* body, int ix, mjtN
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]);
mjuu_normvec(tprev, 3);
length_prev = mjuu_normvec(tprev, 3);
}
if (!last) {
mjuu_setvec(tnext, uservert[3*(ix+2)+0]-uservert[3*(ix+1)+0],
@@ -842,7 +842,7 @@ mjmBody* mjCComposite::AddCableBody(mjCModel* model, mjmBody* body, int ix, mjtN
offset[2]+uservert[3*ix+2]);
mjuu_copyvec(body->quat, this_quat, 4);
} else {
mjuu_setvec(body->pos, length, 0, 0);
mjuu_setvec(body->pos, length_prev, 0, 0);
mjtNum 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);