From 478ce0c8d8954085303278e074c1d4d0da68e2d1 Mon Sep 17 00:00:00 2001
From: Alessio Quaglino <quaglino@google.com>
Date: Tue, 27 Feb 2024 02:54:16 -0800
Subject: [PATCH] Use previous edge length when setting the position of a cable
 body.

Fixes #1440.

PiperOrigin-RevId: 610690343
Change-Id: I0d5288e68069d1117c0c674530e0644960bab06c
---
 src/user/user_composite.cc | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc
index 2aa04775..9d0c480a 100644
--- a/src/user/user_composite.cc
+++ b/src/user/user_composite.cc
@@ -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);