From eeda4d0984931036bc0dbde986ea732e10ac2f15 Mon Sep 17 00:00:00 2001
From: Haroon Qureshi <haroonq@google.com>
Date: Thu, 16 Jul 2026 02:39:13 -0700
Subject: [PATCH] Use body center-of-mass (subtree_com) instead of lookat point
 for tracking cameras.

PiperOrigin-RevId: 948850032
Change-Id: I439946ac1ea7f8556c3132540d82f633a7c4a5f5
---
 src/engine/engine_vis_visualize.c | 12 +++++++++++-
 1 file changed, 11 insertions(+), 1 deletion(-)

diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c
index 52cfd781..9109dec7 100644
--- a/src/engine/engine_vis_visualize.c
+++ b/src/engine/engine_vis_visualize.c
@@ -469,13 +469,23 @@ void mjv_cameraFrame(mjtNum headpos[3], mjtNum forward[3], mjtNum up[3], mjtNum
         right[2] = 0;
       }
       if (headpos) {
-        mju_addScl3(headpos, cam->lookat, forward, -cam->distance);
+        if (cam->type == mjCAMERA_TRACKING && cam->trackbodyid >= 0) {
+          if (!d) {
+            mjERROR("data pointer is NULL");
+          }
+          mju_addScl3(headpos, d->subtree_com + 3*cam->trackbodyid, forward, -cam->distance);
+        } else {
+          mju_addScl3(headpos, cam->lookat, forward, -cam->distance);
+        }
       }
       break;
     }
 
     case mjCAMERA_FIXED: {
       const int cid = cam->fixedcamid;
+      if (!d) {
+        mjERROR("data pointer is NULL");
+      }
       const mjtNum* mat = d->cam_xmat + 9*cid;
       if (forward) {
         forward[0] = -mat[2];