From 18f88efe17e3c30ca34892729e3ba86bc3c04f1a Mon Sep 17 00:00:00 2001
From: Yuval Tassa <tassa@google.com>
Date: Wed, 6 Nov 2024 14:45:40 -0800
Subject: [PATCH] Contact visualization: if penetration is deeper than height,
 extend cylinder to show computed depth.

PiperOrigin-RevId: 693869892
Change-Id: I1c873e3803286d6a636336246f498552617e978b
---
 src/engine/engine_vis_visualize.c | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c
index 10396a0b..5f619fdd 100644
--- a/src/engine/engine_vis_visualize.c
+++ b/src/engine/engine_vis_visualize.c
@@ -141,7 +141,9 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
       START
       thisgeom->type = mjGEOM_CYLINDER;
       thisgeom->size[0] = thisgeom->size[1] = m->vis.scale.contactwidth * scl;
-      thisgeom->size[2] = m->vis.scale.contactheight * scl;
+      float halfheight = m->vis.scale.contactheight * scl;
+      float halfdepth = -con->dist / 2;
+      thisgeom->size[2] = mjMAX(halfheight, halfdepth);
       mju_n2f(thisgeom->pos, con->pos, 3);
       mju_n2f(thisgeom->mat, mat, 9);